???????????????????????????????????????????????????????????????????????????????????????????
Hybrid A*
2021 年 8 月 25 日
Hybrid A* 是在 A* 算法的基础上考虑移动机器人实际运动约束的算法，最早在 2010
年斯坦福大学提出，并在 DARPA 的城市挑战赛得以应用。传统的 A* 算法有以下缺点：
-
- A*?算法适用于离散的情况，机器人的控制空间、轨迹空间都是连续的，所以生成的路径是不光滑的。
- A*?算法规划出的路径不满足机器人的非完整性约束，例如非全向移动机器人不能横向移动。
而 Hybrid A* 算法作为 A* 算法的一个变种，适用于车辆三维状态空间 (x, y, Θ)，通过修改状态更新规则，可以在 A* 离散节点中捕获车辆连续状态，从而保证路径的运动可行性。
以阿克曼小车运动模型为例，车辆模型简化为二维平面上的刚体结构，任意时刻车辆的状态 q= (x, y, Θ)。车辆坐标的原点位于后轴中心位置，坐标轴与车身平行。v 代表车辆的速度，Φ?表示车轮转角（向左为正，向右为负），L?表示前轮和后轮的距离，如果车轮转角保持不变，车辆就会在原地转圈，半径为 ρ, 如图片1所示:
?
?
考虑车辆的前进和倒退，令 dir 代表小车前进方向，则速度表示为 s=dir*v, 即设置小车速度大小一定，而方向与小车朝向有关。将小车朝向角约束在 ?π/4 到 π/4 之间，因此小车可以实现前后左右一定范围移动。阿克曼小车运动学模型可表达为公式1
在 A*?路径搜索中，将空间划分为小网格，使用网格中心作为 A*?路径规划的节点，并在节点中寻找一条避开障碍物的路径，求解路径时只保证了连通性，不保证车辆实际可行。Hybrid?A*?分别考虑空间连通性和车辆的朝向转角，通过考虑车辆的运动学约束，Hybrid A*?搜索出的路径节点可以是二维网格中的任意点。节点的扩展过程中，首先父亲节点根据当前车辆状态以及地图障碍物信息，以给定的 steer_list?和 direction_list（转角序列和方向序列）在一定的 move_step?内，求解出一段无碰撞的路径，将此段路径的最后点作为下一个子节点的位置。因此子节点在地图栅格中的位置取决于在运动约束下一段路径最终点落在栅格中的位置。
图?2:?A*?搜索路径 图?3:?Hybrid?A*?搜索路径
节点的总代价 f_cost=g_cost+H*h_cost，其中 g_cost 是由实际运动产生，其代价由四部分组成，朝向（1?和-1）、转向角大小、转向角变化、行驶方向变化。H?是启发函数代价的比例系数，用来调整启发函数代价的影响因子。h_cost 为启发函数代价。采用了两种启发函数，并取两者的最大值作为最终的启发函数代价 [1]。第一个启发函数为”non-holonomic- without-obstacles”，非全向无障碍启发函数，只考虑车辆的非完整性约束而不考虑障碍物， 一般采用当前节点状态 (xs, ys, Θs), 到目标状态 (xt, yt, Θt) 的最短 Reeds shepp 曲线的代价值。第二个启发函数是”holonomic-with-obstacles heuristic”，忽略汽车的非完整约束，使用障碍物地图计算到目标的最短距离，可以避免运动过程中陷入死胡同或者 U 型障碍物，一般采用 A* 搜索出的最短路径代价。
- Reeds shepp?曲 线
由于 Hybrid A* 算法中对运动空间 (x, y, Θ) 和车辆的 steer_list、direction_list 都是进行了离散化处理。因此不可以精确到达连续变化的目标姿态。运动轨迹也会不平滑，因此使用 Reeds shepp 曲线。即从扩展的子节点中找到能从此节点到目标节点的无碰撞 Reeds shepp 曲线，并选择代价最小的曲线作为作后路径。如下图所示，其中黄绿色的是由扩展子节点产生的路径，紫色路径是由最终 Reeds shepp 产生的路径。
图 4: Reeds shepp 曲线扩展
整个算法的流程如下：首先定义起始节点和终止节点 (每个节点包括机器人状态、总代价以及父节点)，定义障碍物地图。建立 open_dic 和 close_dic 空字典，字典的键代表节点在障碍物地图中唯一的位置，键值为节点。将起始节点存入 open_dic 中。
进入循环，设置开始进行 Reeds shepp 曲线拓展的循环数 N_rs（即每循环 N_rs 次后进行一次查询当前节点状态到目标状态是否存在 Reeds shepp 曲线，将 N_rs 的大少设置为当前节点位置到目标位置欧式距离的四分之一）。从 open_dic 中选出 f_cost 最小的节点作为父节点，如果找到相应的 Redds shepp 曲线则不再扩展节点，并把当前节点作为最终节点，直接选取路径代价最小的 reeds_shepp 曲线，并将曲线的路经点加入到最终的路径序列中。否则继续扩展子节点，与 A 的扩展相邻是栅格不同，Hybrid A* 扩展子节点的过程是根据转角和航向序列产生一系列考虑障碍物和车体形状的无碰撞子节点，如果子节点在close_dic 中，则舍弃，如果不在 close_dic 中，而在 open_dic 中，若子节点的 g_cost 比在 open_dic 中相应节点的 g_cost 小，则更新为子节点。如果既不在 close_dic 中，也不在open_dic 中，则直接将子节点加入到 open_dic 中。
经过循环之后 close_dic 包括从初始节点到最终节点的所父节点，每个父节点都包含一段路径点，最后将所有路径拼接作为 Hybrid A* 规划出的路径。
让小车分别向四个停车位置进行泊车, 起始点标记为蓝色圆圈，目标点标记为黄色五角星。开始进行 Reeds shepp 曲线拓展的最后节点标记为绿色与圆点。在实际的移动机器运动规划过程中，Hybrid A* 规划出的全局路径往往可以作为前端，经过后端的局部轨迹优化处理后可以产生更加平滑的运动轨迹。
程序参考1
 |
?