## 一、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移动到目标点的估算移动代价.

## 二、部分源代码

``````% 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]
% 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,2*pi);
if v < -pi
v = v+2*pi;
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(eta*A-xi*B,xi*A+eta*B);
t2 = 2*(cos(delta)-2*cos(v)-2*cos(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

## 四、matlab版本及参考文献

1 matlab版本 2014a

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