自动驾驶路径规划算法学习-A Star算法及matlab实现
参考
https://blog.csdn.net/ck_Leo_Libra/article/details/105244795?spm=1001.2014.3001.5506
https://blog.csdn.net/weixin_39199083/article/details/117001590?spm=1001.2014.3001.5501
1. A Star算法原理
A star算法几乎与Dijkstra实现方法一致，不同得是A star是启发式得搜索，相比Dijkstra计算效率更高。
此处A Star原理参见我得另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现 ，甚至连代码得实现也几乎相同，仅仅是代价函数得计算有所不同。
Dijkstra算法是广度优先算法，扩展了很多很远得节点，搜索有些盲目，如下图：
Dijkstra算法搜索效果 A Star算法搜索效果
A Star算法对代价函数加入了启发项：
其他算法流程几乎与Dijkstra算法一致，仅代价函数计算不同
Dijkstra算法流程 A Star算法流程
2. 计算示例
同Dijkstra算法一样，我们这里给出一个计算示例方便理解A Star的工作原理
同样要求s到t的最短路径，相比Dijkstra这里还要求MAP给出每个节点的坐标信息。
同Dijkstra算法，也需要构建两个点集open, close
代价函数F=G（起点到当前节点花费代价）+h(当前节点到目标点代价)
这里启发项的h采用欧氏距离，也可采用曼哈顿或者其他距离。
Step1:
将起始节点加入open表，其cost全为0，其父节点为s
Step2:
s为open表里cost最小点，将其加入close表，并扩展s节点子节点a,b,c，既不在close中又不在open中，分别计算其total cost f,并将a,b,c加入open表
f(a)=G(a)+h(a)=G(s)+G(sa)+h(a)=0+5+((4-1)^2+(2-2)^2)^0.5=0+5+3=8
f(b)=G(b)+h(b)=G(s)+G(sb)+h(b)=0+7+((4-2)^2+(2-2)^2)^0.5=0+7+2=9
f(c)=G(c)+h(c)=G(s)+G(sc)+h(c)=0+2+((4-1)^2+(2-0)^2)^0.5=0+2+3.6=5.6
Step3:
找到open表中cost f 最小的是c, 将c加入close表中，并扩展c子节点e,e不在open中，又不在close中，将e加入open表，计算e 的cost f
f(e)=G(e)+h(e)=G(c)+G(ce)+h(e)=2+8+((4-3)^2+(2-3)^2)^0.5=10+1.4=11.4
...........
除了代价函数的计算与Dijkstra算法不同，其他完全相同，这里只列出3步作为参考，不再赘述，重复下去，直到目标点出现在Open中，就找到了最优路径。
3.Matlab代码实现
要实现A star算法，这里需要11个m函数,只要在Dijkstra代码的基础上稍加修改即可，Dijkstra算法代码见我另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现
相比Dijkstra多加入一个节点启发值计算函数h.m
main.m略加修改
AStar.m代替Dijkstra.m，其实就是修改了里面的代价函数计算方法而已。
剩下8个m函数 CreateMap.m, FindList.m， GetPath.m， GetObstacle.m, isopen.m, isObstacle.m , MotionModel.m , plot_map.m 与Dijkstra代码一模一样，见我另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现
这里仅贴出与Dijkstra算法不同的3个m函数：AStar.m, main.m, h.m
3.1 AStar.m
这是A Star算法实现的主体程序，参照算法流程
function path=AStar(obstacle,map)
% 该程序为A*算法
% 用于存储路径
path = [];
%OpenList
open = [];
%CloseList
close = [];
% findFlag用于判断While循环是否结束
findFlag=false;%目标标志
%===================1.将起始点放在Openlist中======================
%open变量每一行 [节点坐标，代价值F=G+H,代价值G,父节点坐标]
open =[map.start(1), map.start(2) , 0+h(map.start,map.goal) , 0 , map.start(1) , map.start(2)];
%更新状态--下一步的八个点
next = MotionModel();
%=======================2.重复以下过程==============================
while ~findFlag
%--------------------首先判断是否达到目标点，或无路径-----
if isempty(open(:,1))
disp('No path to goal!!');
return;
end
%------------------判断目标点是否出现在open列表中
[isopenFlag,Id]=isopen(map.goal,open);
if isopenFlag
disp('Find Goal!!');
close = [open(Id,:);close]
findFlag=true;
break;
end
%------------------a.按照Openlist中的第三列（代价函数F）进行排序，
%--------------------查找F值最小的节点
[Y,I] = sort(open(:,3)); % 对OpenList中第三列排序
open=open(I,:);%open中第一行节点是F值最小的
%------------------b.将F值最小的节点(即open中第一行节点)，放到close
%--------------------第一行(close是不断积压的)，作为当前节点
close = [open(1,:);close];
current = open(1,:);
open(1,:)=[];% 因为已经从open中移除了，所以第一列需要为空
%--------------------c.对当前节点周围的4个相邻节点，算法的主体：------------------------
for in=1:length(next(:,1))
% 获得相邻节点的坐标,代价值F先等于0,代价值G先等于0 ,后面两个值是
% 其父节点的坐标值，暂定为零(因为暂时还无法判断其父节点坐标是多少)
m = [current(1,1)+next(in,1) , current(1,2)+next(in,2) , 0 , 0 , 0 ,0];
m(4) = current(1,4) + next(in,3); % m(4) 相邻节点G值
m(3) = m(4) + h(m(1:2),map.goal);% m(3) 相邻节点F值
%>>如果它不可达，忽略它，处理下一个相邻节点 (注意，obstacle这个数
% 组中是包括边界的)
if isObstacle(m,obstacle)
continue;
end
%flag == 1：相邻节点 在Closelist中 targetInd = close中行号
%flag == 2：相邻节点不在Openlist中 targetInd = []
%flag == 3：相邻节点 在Openlist中 targetInd = open中行号
[flag,targetInd] = FindList(m,open,close);
%>>如果它在Closelist中，忽略此相邻节点
if flag==1
continue;
%>>如果它不在Openlist中，加入Openlist,并把当前节点设置为它的父节点
elseif flag==2
m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
open = [open;m];%将此相邻节点加放openlist中
%>>剩下的情况就是它在Openlist中，检查由当前节点到相邻节点是否更好，
% 如果更好则将当前节点设置为其父节点，并更新F,G值；否则不操作
else
%由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数F值)
if m(3) < open(targetInd,3)
%更好，则将此相邻节点的父节点设置为当前节点，否则不作处理
m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新
end
end
end
plot_map(map,obstacle,open,close);
end
%追溯路径
path=GetPath(close,map.start);
end
3.2 h.m
这里采用的是曼哈顿距离再乘以10，如下
h(当前节点)=abs（当前节点x坐标-目标节点x坐标）+abs（当前节点y坐标-目标节点y坐标）
function hcost = h( m,goal )
%计算启发函数代价值 ，这里采用曼哈顿算法
hcost =10* abs( m(1)-goal(1) )+10*abs( m(2)-goal(2) );
end3.3 main.m
其实main.m与Dijkstra算法的主程序也是几乎一样，除了调用AStar算法命令不同，以及提示程序开始打印的文字不同外，其他的都相同
% 该文件为以map.mat为地图文件，point.mat为起止位置文件，
% 进行A*算法路径规划的主程序
clc
clear all
close all;
disp('A Star Path Planing start!!')
load map.mat % 加载地图
load point.mat % 加载起止位置点
[map.XMAX,map.YMAX] = size(MAP); %%代表我们要画一个地图的长和宽
map.start = node(1:2); %起始点 注意必须在地图范围内
map.goal = node(3:4); %目标点 注意必须在地图范围内
obstacle = GetObstacle(map,MAP);% 获取边界数据和障碍物坐标
clear MAP node % 后续程序不再使用这两个变量
%obstacle = [obstacle;4,1; 4,2; 4,3; 4,4; 3,4 ;2,4;];%全封死的情况，是没有路的
% 画出地图和起止点
figure(1)
if length(obstacle)>=1
plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
% plot(obstacle(:,1),obstacle(:,2),'om');hold on;
end
pause(1);
h=msgbox('Please confirm the map information and click the buttion "confirm".');
uiwait(h,20);% 5s后关闭消息框
if ishandle(h) == 1
delete(h);
end
close 1
figure(1)
axis([1 map.XMAX+1 1 map.YMAX+1])
set(gca,'YTick',0:1:map.YMAX);
set(gca,'XTick',0:1:map.XMAX);
grid on;hold on;
% 绘制边界和障碍点
plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
% 绘制起始点
plot(map.start(1)+.5,map.start(2)+.5,'bo');hold on;
% 绘制终止点
plot(map.goal(1)+.5,map.goal(2)+.5,'gd');hold on;
text(map.goal(1)+1,map.goal(2)+.5,'Target');
% plot(map.start(1),map.start(2),'*r');hold on;
% plot(map.goal(1),map.goal(2),'*b');hold on;
% 采用A*算法进行路径规划
path = AStar(obstacle,map)% A*算法
%画出路径
%
if length(path)>=1
plot(path(:,1)+0.5,path(:,2)+0.5,'-m','LineWidth',5);hold on;
end
%}
grid on;4.运行结果
可以看视频的运行结果，与Dijkstra算法不同，AStar算法很高效，几乎是直接奔着目标路径去的，只扩展很少的子节点。
运行视频：https://www.bilibili.com/video/BV1i5411u75w
完整代码：https://download.csdn.net/download/weixin_39199083/18886562?spm=1001.2014.3001.5501
扫一扫
1121
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
