【路径规划】基于matlab Hybrid A_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;15ones(1,51)]'; % Obstacle point list
ObstList = [ObstList; [-10: 10; 0ones(1,21)]‘];
ObstList = [ObstList; [-25:-10; 5ones(1,16)]'];
ObstList = [ObstList; [ 10: 25; 5ones(1,16)]’];
ObstList = [ObstList; [ 10ones(1,6);0: 5;]'];
ObstList = [ObstList; [-10ones(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 cost

StartState = [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!’)
else
hold 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 radius
x = 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:5
if isoks(i) == true
elem = paths{i};
if Lmin > elem.totalLength
Lmin = elem.totalLength;
path = elem;
end
end
end
% PlotPath(path,veh);
end

function v = mod2pi(x)
v = rem(x,2pi);
if v < -pi
v = v+2pi;
elseif v > pi
v = v-2*pi;
end
end

function [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(etaA-xiB,xiA+etaB);
t2 = 2*(cos(delta)-2cos(v)-2cos(u))+3;
if t2 < 0
tau = mod2pi(t1+pi);
else
tau = mod2pi(t1);
end
omega = 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 >= 0
v = mod2pi(phi-t);
if v >= 0
isok = true;
return
end
end
isok = 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 >= 4
u = sqrt(u1^2-4);
theta = atan2(2,u);
t = mod2pi(t1+theta);
v = mod2pi(t-phi);
if t >= 0 && v >= 0
isok = true;
return
end
end
isok = false;
t = 0;
u = 0;
v = 0;
end

function [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 isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(15,:),t,u,v,0,0);
end
end
[isok,t,u,v] = LpSpLp(-x,y,-phi); % timeflip
if isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(15,:),-t,-u,-v,0,0);
end
end
[isok,t,u,v] = LpSpLp(x,-y,-phi); % reflect
if isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(16,:),t,u,v,0,0);
end
end
[isok,t,u,v] = LpSpLp(-x,-y,phi); % timeflp + reflect
if isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(16,:),-t,-u,-v,0,0);
end
end
[isok,t,u,v] = LpSpRp(x,y,phi);
if isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(13,:),t,u,v,0,0);
end
end
[isok,t,u,v] = LpSpRp(-x,y,-phi); % timeflip
if isok
L = abs(t)+abs(u)+abs(v);
if Lmin > L
Lmin = L;
path = RSPath(RSPath.Types(13,:),-t,-u,-v,0,0);
end
end