【路径规划】基于matlab HybridA_Star算法机器人路径规划【含Matlab源码 1390期】

一、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

图3 A Star算法流程

二、部分源代码

% Main entry:ObstList = [-25:25;15*ones(1,51)]';            % Obstacle point list
ObstList = [ObstList; [-10: 10; 0*ones(1,21)]'];
ObstList = [ObstList; [-25:-10; 5*ones(1,16)]'];
ObstList = [ObstList; [ 10: 25; 5*ones(1,16)]'];
ObstList = [ObstList; [ 10*ones(1,6);0:  5;]'];
ObstList = [ObstList; [-10*ones(1,6);0:  5;]'];
% Park lot line for collision check
ObstLine = [-25, 15 , 25, 15;-25,  5, -10,  5;-10,  5, -10,  0;-10,  0,  10,  0;10,  0,  10,  5;10,  5,  25,  5;-25,  5, -25, 15;25,  5,  25, 15];
% ObstList and ObstLine
ObstInfo.ObstList = ObstList;
ObstInfo.ObstLine = ObstLine;
% ObstInfo.ObstMap = GridAStar(ObstList,End,XY_GRID_RESOLUTION);Vehicle.WB = 3.7;         % [m] wheel base: rear to front steer
Vehicle.W  = 2.6;         % [m] width of vehicle
Vehicle.LF = 4.5;         % [m] distance from rear to vehicle front end of vehicle
Vehicle.LB = 1.0;         % [m] distance from rear to vehicle back end of vehicle
Vehicle.MAX_STEER = 0.6;  % [rad] maximum steering angle
Vehicle.MIN_CIRCLE = Vehicle.WB/tan(Vehicle.MAX_STEER); % [m] mininum steering circle radius% Motion resolution define
Configure.MOTION_RESOLUTION = 0.1;             % [m] path interporate resolution
Configure.N_STEER = 20.0;                      % number of steer command
Configure.EXTEND_AREA = 0;                     % [m] map extend length
Configure.XY_GRID_RESOLUTION = 2.0;            % [m]
Configure.YAW_GRID_RESOLUTION = deg2rad(15.0); % [rad]
% Grid bound
Configure.MINX = min(ObstList(:,1))-Configure.EXTEND_AREA;
Configure.MAXX = max(ObstList(:,1))+Configure.EXTEND_AREA;
Configure.MINY = min(ObstList(:,2))-Configure.EXTEND_AREA;
Configure.MAXY = max(ObstList(:,2))+Configure.EXTEND_AREA;
Configure.MINYAW = -pi-0.01;
Configure.MAXYAW = pi;
% Cost related define
Configure.SB_COST = 0;             % switch back penalty cost
Configure.BACK_COST = 1.5;         % backward penalty cost
Configure.STEER_CHANGE_COST = 1.5; % steer angle change penalty cost
Configure.STEER_COST = 1.5;        % steer angle change penalty cost
Configure.H_COST = 10;             % Heuristic costStartState = [22, 13, pi  ];
EndState =   [7,   2, pi/2];
[x,y,th,~,~] = HybridAStar(StartState,EndState,Vehicle,Configure,ObstInfo);
if isempty(x)disp('Failed to find path!')
elsehold on;VehicleAnimation(x,y,th,Configure,Vehicle,ObstInfo)
end
% path = FindRSPath(5,1,pi);function path = FindRSPath(x,y,phi,veh)rmin = veh.MIN_CIRCLE; %minimum turning radiusx = x/rmin;y = y/rmin;[isok1,path1] = CSC(x,y,phi);[isok2,path2] = CCC(x,y,phi);[isok3,path3] = CCCC(x,y,phi);[isok4,path4] = CCSC(x,y,phi);[isok5,path5] = CCSCC(x,y,phi);isoks = [isok1, isok2, isok3, isok4, isok5];paths = {path1, path2, path3, path4, path5};Lmin = inf;for i = 1:5if isoks(i) == trueelem = paths{i};if Lmin > elem.totalLengthLmin = elem.totalLength;path = elem;endendend
%     PlotPath(path,veh);
endfunction v = mod2pi(x)v = rem(x,2*pi);if v < -piv = v+2*pi;elseif v > piv = v-2*pi;end
endfunction [tau,omega] = tauOmega(u,v,xi,eta,phi)delta = mod2pi(u-v);A = sin(u)-sin(delta);B = cos(u)-cos(delta)-1;t1 = atan2(eta*A-xi*B,xi*A+eta*B);t2 = 2*(cos(delta)-2*cos(v)-2*cos(u))+3;if t2 < 0tau = mod2pi(t1+pi);elsetau = mod2pi(t1);endomega = mod2pi(tau-u+v-phi);
end% formula 8.1
function [isok,t,u,v] = LpSpLp(x,y,phi)[t,u] = cart2pol(x-sin(phi),y-1+cos(phi));if t >= 0v = mod2pi(phi-t);if v >= 0isok = true;returnendendisok = false;t = 0;u = 0;v = 0;
end% formula 8.2
function [isok,t,u,v] = LpSpRp(x,y,phi)[t1,u1] = cart2pol(x+sin(phi),y-1-cos(phi));if u1^2 >= 4u = sqrt(u1^2-4);theta = atan2(2,u);t = mod2pi(t1+theta);v = mod2pi(t-phi);if t >= 0 && v >= 0isok = true;returnendendisok = false;t = 0;u = 0;v = 0;
endfunction [isok,path] = CSC(x,y,phi)Lmin = inf;type = repmat('N',[1,5]);path = RSPath(type,0,0,0,0,0);[isok,t,u,v] = LpSpLp(x,y,phi);if isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(15,:),t,u,v,0,0);endend[isok,t,u,v] = LpSpLp(-x,y,-phi); % timeflipif isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(15,:),-t,-u,-v,0,0);endend[isok,t,u,v] = LpSpLp(x,-y,-phi); % reflectif isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(16,:),t,u,v,0,0);endend[isok,t,u,v] = LpSpLp(-x,-y,phi); % timeflp + reflectif isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(16,:),-t,-u,-v,0,0);endend[isok,t,u,v] = LpSpRp(x,y,phi);if isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(13,:),t,u,v,0,0);endend[isok,t,u,v] = LpSpRp(-x,y,-phi); % timeflipif isokL = abs(t)+abs(u)+abs(v);if Lmin > LLmin = L;path = RSPath(RSPath.Types(13,:),-t,-u,-v,0,0);endend

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)