无人驾驶全局路径规划之A星算法

根据查阅网上关于A星算法的原理，结合自己的理解，使用MATLAB实现A星算法生成路径。原理部分我不再赘述，网上介绍的文章还是很多的。
下面主要展示自己编的A星算法，有疑问欢迎交流。

注：我这里虽然用了栅格地图，但生成新节点时，是按“上下左右”四个方向按一定生长步长生成新节点，这和常见的按栅格生成新栅格的方法有些不同，请注意区分。

clear;
close all;
%注意障碍物的权值按行列生成，而路径曲线是以左下角为原点生成，注意两者的区别
load parkingLotCostVal.mat % costVal
%生成带权值的网格地图
costmap = vehicleCostmap(costVal,'CellSize',1);%每个网格边长1米
%设置障碍物的膨胀范围
ccConfig =inflationCollisionChecker('CenterPlacements',[0.2 0.5 0.8],'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
plot(costmap);hold on;
set(gca,"XTick",0:1:150);
set(gca,"YTick",0:1:100);
grid on;%hold on;
legend("off");
%检测有障碍物的区域
occMat = checkOccupied(costmap);

x_start=[6,10];%起点坐标
goal=[90,43];%终点坐标
r=1;%生成步长
likai_length=0.01;%两点距离小于0.01，则认为两点为同一点
goal_radius=1;%搜索停止阈值

%画出起点
plot(x_start(1),x_start(2),'k>','MarkerFaceColor','g','MarkerSize',9);hold on;
%画出终点
plot(goal(1),goal(2),'ko','MarkerFaceColor','r','MarkerSize',9);hold on;

%创建各个中间量记录表
closed=[];%记录探索过的点
closed_parent_index=[];%记录探索过的点的父节点在cLosed表中的位置
open=[];%记录即将探索的点
open_parent_index=[];%记录即将探索的点的父节点在cLosed表中的位置
F_value=[];%记录F值
open_G_value=[];%记录open表中每个点的G值

%初始化各个表
closed=[closed;x_start];
closed_parent_index=[closed_parent_index;1];
closed_G_value=0;


flag=1;%终止循环的标志
search_p=x_start;%起始点为第一个探索点

while flag
    
    %自定义函数around_points生成search_p周围4个方向的点
    all_p=around_points_4([search_p,r]);
    for i=1:size(all_p,1)%对每个点进行判断
        tem1=all_p(i,:);
        rule0=(tem1(1)>=0)&&(tem1(1)<=150)&&(tem1(2)>=0)&&(tem1(2)<=100);%判断有无超出地图边界
        if rule0
            tem2=tem1-closed;
            rule2=sum(sqrt(tem2(:,1).^2+tem2(:,2).^2)>likai_length)==size(closed,1);%生成的点是否在closed列表中
            if isempty(open)
                rule3=true;%open表空，表明正在探索起始点，则新生成的点肯定不在open表中
            else
                tem3=tem1-open;
                %生成的点是否在open列表中
                rule3=sum(sqrt(tem3(:,1).^2+tem3(:,2).^2)>likai_length)==size(open,1);
            end
            %判断生成节点与父节点连线是否在障碍物上
            cha=cha_points_4(search_p,i,r);
            RULE1_TEM=[];
            for t=1:size(cha,2)
                tem6=cha(:,t);
                rule1_tem=occMat(101-(fix(tem6(2))+1),(fix(tem6(1))+1))==false;%生成的点是否在障碍物上
                RULE1_TEM=[RULE1_TEM;rule1_tem];
            end
            rule1=sum(RULE1_TEM)==t;
            if rule1 && rule2 && rule3 %三个条件都满足才能将该点放入open表中
                %画出探索点与生成点的连线
                plot([search_p(1),tem1(1)],[search_p(2),tem1(2)],'b-');hold on;
                plot(tem1(1),tem1(2),'b.','MarkerSize',6);hold on;
                open=[open;tem1];%将该点加入到open表中
                open_parent_index=[open_parent_index;size(closed,1)];%更新open_parent_index
                open_G_value=[open_G_value;1+closed_G_value(end)];%更新open_parent_indexopen_G_value
                G_value=open_G_value(end);%G值
                H_value=abs(tem1(1)-goal(1))+abs(tem1(2)-goal(2));%H值，使用曼哈顿距离
                F_value_tem=G_value+H_value;%F值
                F_value=[F_value;F_value_tem];
            end
        end
    end
    
    
    [F_value,I] = sort(F_value);%F值排序
    open=open(I,:);%open表排序
    open_parent_index=open_parent_index(I);%open_parent_index排序
    
    search_p=open(1,:);%更新下一个探索点，并从open表中删掉该点
    closed=[closed;search_p];%更新closed
    closed_parent_index=[closed_parent_index;open_parent_index(1)];%更新closed_parent_index
    closed_G_value=[closed_G_value;open_G_value(1)];%closed_G_value
    
    open(1,:)=[];
    F_value(1,:)=[];
    open_parent_index(1,:)=[];
    
    %判断是否到达终点
    tem4=goal-closed;
    find_goal=sum(sqrt(tem4(:,1).^2+tem4(:,2).^2)<=goal_radius)>0;
    if (find_goal)
        flag=0;
    end
end

%画出找到的路径
tem1=length(closed_parent_index);%从终点所在的行开始搜索
zuiyou_lujing=[goal(1),goal(2)];%首先将终点加入进去
while tem1>1 %如果没有到起点的行号（也就是第一行），则一直找下去
    zuiyou_lujing=[zuiyou_lujing;closed(tem1,:)];
    tem1=closed_parent_index(tem1);%使用当前行的closed_parent_index更新tem1
end
zuiyou_lujing=[zuiyou_lujing;x_start];%将起点加入进去
%画出所生成的路径
plot(zuiyou_lujing(:,1),zuiyou_lujing(:,2),'-r',"LineWidth",2);

%使用直线平滑对生成的路径进行平滑
s_index=1;
i=2;
while i<=size(zuiyou_lujing,1)
    cha_p=s_cha(zuiyou_lujing(s_index,:),zuiyou_lujing(i,:));
    TEM7=[];
    for t=1:size(cha_p,2)
        tem6=cha_p(:,t);
        tem7=occMat(101-(fix(tem6(2))+1),(fix(tem6(1))+1))==false;%生成的点是否在障碍物上
        TEM7=[TEM7;tem7];          
    end
    if sum(TEM7)==t
        i=i+1;
    else
        tem8=zuiyou_lujing(i-1,:);
        tem9=zuiyou_lujing(s_index,:);
        plot([tem9(1),tem8(1)],[tem9(2),tem8(2)],'g-',"LineWidth",2);hold on;
        s_index=i-1;   
    end  
end
tem9=zuiyou_lujing(s_index,:);
tem8=zuiyou_lujing(end,:);
plot([tem9(1),tem8(1)],[tem9(2),tem8(2)],'g-',"LineWidth",2);
hold off;

其中使用到的自定义函数如下，

function y=around_points_4(x)
r=x(3);
y(1,:)=[x(1)-r,x(2)];
y(2,:)=[x(1),x(2)+r];
y(3,:)=[x(1)+r,x(2)];
y(4,:)=[x(1),x(2)-r];
end

function y=cha_points_4(x,i,r)
xx=x(1);
yy=x(2);
switch i
    case 1
        a=linspace(xx,(xx-r),100);
        b=ones(1,100)*yy;
    case 2
         a=ones(1,100)*xx;
        b=linspace(yy,(yy+r),100);
    case 3
        a=linspace(xx,(xx+r),100);
        b=ones(1,100)*yy;
    case 4
         a=ones(1,100)*xx;
        b=linspace(yy,(yy-r),100);
end
y=[a;b];
end

function out=s_cha(p1,p2)
x1=p1(1);
y1=p1(2);
x2=p2(1);
y2=p2(2);
if x1==x2
    a=ones(1,100)*x1;
    b=linspace(y1,y2,100);
else
    k=(y2-y1)/(x2-x1);
    angle=atan(k);
    L=sqrt((x2-x1)^2+(y2-y1)^2);
    Lslice=linspace(0,L,100);
    a=(double((x2-x1)>0)*2-1)*Lslice*abs(cos(angle))+x1;
    b=(double((y2-y1)>0)*2-1)*Lslice*abs(sin(angle))+y1;
end
out=[a;b];
end

当目标点坐标为goal=[90,43]时，输出结果如下（其中红线为生成的路径，绿线为直线平滑后的路径），

当目标点坐标为goal=[90,54]时，输出结果如下，