一、A*与HybridA*路径区别

        基于A*算法的原理，可以总结出以下信息：

        路径规划就是在栅格地图下，已知起点、终点的栅格坐标，通过计算每个栅格（已行走的距离H+预估距离G）的和F，来确定每个栅格周围八个栅格的F值，然后选取最小F的栅格来扩展，直到扩展至目标终点，然后回溯走过的栅格，进而生成路径。

        然而A*算法对于车辆来说，有个重要缺陷。如下图所示，红圈所示部分的转角对于车辆而言是不可能实现的，车辆行走的路径在拐角处必然是圆滑的，不可能出现尖角，因此A*不完全适用于车辆，需要对A*加上符合车辆运动学的限制，于是HybridA*算法应运而生。

        HybridA*算法原理与A*算法一致，区别在于A*扩展时是从当前栅格的中心点，扩展到周围八个栅格的中心点，路径是两两中心点的连线。HybridA*需要考虑运动物体的方向和运动约束，比如车辆有最小转弯半径，车辆转弯时的最大转角永远不会超过最小转弯半径的限制，因此HybridA*的路径就是直线前进后退，以及左右转弯的圆。另一个重要不同是，A*到达新的栅格，位置必定是栅格中心，而HybridA*到达的位置可以是路径上的任一点。

        对于车辆，一般而言，HybridA*一共六个方向，左圆前进、倒车，直线前进、倒车，右圆前进、倒车，当然车辆转弯不一定按最小转弯半径转弯，因此可以在直线和左圆中间再细分出许多航向，为了简单，只固定这六个方向。

二、泊车算法代码详解——建图，配置车辆参数

        代码采用上篇文章末尾的代码（https://github.com/wanghuohuo0716/hybrid_A_star），其中代码结构如下图，核心部分是HybridA*部分：

        首先看主m文件，此文件的结构很简单，主要分为6个步骤，具体如下：

1、设置世界坐标系下障碍物点，此点用于Astar算法，需要注意，泊车算法分三个坐标系，分别是车辆坐标系，世界坐标系，栅格坐标，注意区分

栅格坐标用于HybridA*和A*算法，车辆坐标系用于RS曲线，世界坐标系就是地图坐标。

2、根据障碍物点，设置障碍物线，障碍线用于碰撞检测，不设置障碍物线的话，仅靠点会有缝隙，所以必须设置障碍物线，蓝色线包围的区域就是泊车区域，左右两个侧边没有障碍线。

3、设置车辆参数，以及栅格地图的参数，车辆起点，泊车终点

4、利用Astar算法，将此地图转为栅格地图，然后计算每个栅格到目标栅格的总代价F，得到costmap，costmap是一个二维数组，用于指引HybridA*的扩展方向

5、利用HybridA*算法，找到泊车路径，泊车路径分两段，一段是扩展路径，通过costmap的指引搜出，一部分是RS曲线的路径。

6、对路径进行判断，如果路径数组都是0，则报规划失败，否则绘制泊车动画。

        具体matlab代码注释如下：

%% 设置泊车区域，ObstList为构成障碍线的点的坐标，用于Astar算法，来计算障碍物栅格坐标，单位是m，属于世界坐标
ObstList = []; % Obstacle point list
for i = -10:10
    ObstList(end+1,:) = [i,20];
end
for i = -3:3
    ObstList(end+1,:) = [i, 0];
end
for i = -10:-3
    ObstList(end+1,:) = [i, 3.5];
end
for i = 3:10
    ObstList(end+1,:) = [i, 3.5];
end
for i = 0:3.5
    ObstList(end+1,:) = [3, i];
end
for i = 0:3.5
    ObstList(end+1,:) = [-3, i];
end

%% 根据障碍点，设置障碍线，用于后续的碰撞检测
ObstLine = []; % Park lot line for collision check
tLine = [-10, 20 , 10, 20]; %start_x start_y end_x end_y
ObstLine(end+1,:) = tLine;
tLine = [-10, 3.5, -3, 3.5];
ObstLine(end+1,:) = tLine;
tLine = [-3, 3.5, -3, 0];
ObstLine(end+1,:) = tLine;
tLine = [-3, 0, 3, 0];
ObstLine(end+1,:) = tLine;
tLine = [3, 0, 3, 3.5];
ObstLine(end+1,:) = tLine;
tLine = [3, 3.5, 10, 3.5];
ObstLine(end+1,:) = tLine;
tLine = [-10, 3.5, -10, 20];
ObstLine(end+1,:) = tLine;
tLine = [10, 3.5, 10, 20];
ObstLine(end+1,:) = tLine;

%% 设置车辆、地图的参数
Vehicle.WB = 1.7;  % [m] wheel base: rear to front steer轴距
Vehicle.W = 2.4; % [m] width of vehicle车宽
Vehicle.LF = 4; % [m] distance from rear to vehicle front end of vehicle后轴中心到车辆最前端的距离
Vehicle.LB = 1; % [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车辆最小转弯半径
 
% ObstList and ObstLine将地图的信息放于结构体Configure中，方便后续函数调用
Configure.ObstList = ObstList;
Configure.ObstLine = ObstLine;

% 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; % [m]世界坐标生成栅格坐标的分辨率（2m一个栅格）
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;
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

%设置起点，终点的坐标及航向
Start = [5, 5.5, 0];
End = [-1.5,1.8,0];

%% 使用完整约束有障碍情况下用A*搜索的最短路径做为hybrid A*的启发值
ObstMap = GridAStar(Configure.ObstList,End,Configure.XY_GRID_RESOLUTION);
Configure.ObstMap = ObstMap;%将costmap加入结构体中
cla %  从当前坐标区删除包含可见句柄的所有图形对象。

%% 利用HybridAStar找泊车路径
[x,y,th,D,delta] = HybridAStar(Start,End,Vehicle,Configure);
% GridAStar(ObstList,End,2);

%% 根据路径结果进行判断
if isempty(x)
    disp("Failed to find path!")%没有路，报告
else
    VehicleAnimation(x,y,th,Configure,Vehicle)%生成泊车动画
end