Hybrid A-star 路径规划

2010年，斯坦福首次提出一种满足车辆运动学的算法(Hybrid A*), 并在(DARPA)的城市挑战赛中得以运用，本文记录相关内容。

简介

Hybrid A* 算法解决的问题是：假设无人车有充分的感知和定位能力，则其能够在线重新规划，生成障碍物地图，并且能够在未知环境中行驶。

算法在连续坐标系下进行启发式搜索（改进了A* 算法）

• Hybrid A*算法的启发式包括两种: non-holonomic without-obstacles和 holonomic with obstacles。

  前者考虑了车辆的运动约束，但不考虑障碍物，一般使用RS曲线，Dubins曲线；后者将车辆当成网格地图上面的点，但考虑了障碍物。

第二阶段用数值非线性优化的方法（即共轭梯度下降法）对生成路径的“质量”进行提升，使其至少是局部最优的。

其他算法中常见问题

开发实际无人驾驶的路径规划器存在的问题：

1. 机器人的控制空间、轨迹空间都是连续的，而现存算法基本都是适用于离散情况的，所以生成的路径是不光滑的。
2. 不满足机器人的非完整性约束

和A*的区别

	Hybrid A*	A*
维数	$(x,y,\theta,R)$	$(x,y)$
H(n)	Max(Rends_Shepp, A*)	Manhattan/Euclidean
节点	车辆的运动学模型为节点（2）	二维平面坐标点（1）
节点与节点连接处	交点可以不是栅格中心（4）	交点是栅格中心（3）
约束	考虑物体的实际运动方向	所有的相邻节点都可以顺利转移
路径	连续	线段连接
缺点	不具有完备性	不满足车辆运动学特性

驾驶行为规划系统

更高层的规划系统可以用有限状态机表示，这是一个非常简单的抽象反应系统，它之所以非常简单是因为它只对特定的外界输入产生数量有限的响应，在有限状态机中，我们只能构造有限数量的状态，外界的输入只能让状态机在有限的状态中从一个状态跳到另一个状态。

分析一下这个状态机：

状态	描述
LOCATE_VEHICLE	这是Junior的初始状态，即在无人车出发之前确定其在地图中的位置。
FORWARD_DRIVE	这个超级状态实际上包含了直行，车道保持和障碍物规避，当不是在停车场（即无道路开放区域）时，这是状态机首选的状态。
STOP_SIGN_WAIT	当无人车在停车标志处等待时，进入此状态。（停车标志是美国十字路口的常见标志）
CROSS_INTERSECTION	在这个状态下无人车处理十字路口通过这一场景，无人车会等待直到确认能够安全通过。
UTURN_DRIVE	在U型掉头时调用的状态
UTURN_STOP	在U型掉头前的停车状态
CROSS_DIVIDER	跨过黄线行驶
PARKING_NAVIGATE	停车场内的普通驾驶模式
TRAFFIC_JAM和ESCAPE	处理交通阻塞时的两个状态
BAD_RNDF	如果当前道路和预先做的路网图不同的时候，即进入该状态，在这个状态下，无人车会采用混合A*算法完成车辆的路径规划。
MISSION_COMPLETE	当挑战赛（DARPA）结束，无人车进入该状态，即整个状态机的结束状态。

在无人车正常行驶中这个状态机几乎处在普通驾驶模式（即FORWARD_DRIVE和PARKING_NAVIGATE这两个状态），系统通过stuckness detectors（胶着检测器）来确定是否从普通驾驶状态转移至底层的其他状态，在完成了相应的动作以后行为模块又会回到原来的普通驾驶模式。

这样的状态机能够让无人车处理一下复杂情况：

• 当当前车道阻塞的时候，车辆会考虑驶入对面车道（即CROSS_DIVIDER）, 如果对面车道也被阻塞了，则会启动U形转弯（UTURN_STOP/UTURN_DRIVE），此时内部RNDF（Route Network Definition File，即全局路网图）也会相应修改，并执行dynamic programming以重新生成RNDF值函数。

• 对于十字路口的交通阻塞问题，在等待时间结束以后，会调用混合A*算法找出最近出口制动车辆离开阻塞区域。

• 在单向通道阻塞的时候，如果规划失败也会调用混合A*算法规划至下一个GPS路点。

• 某些路点在循环多次以后仍然无法到达，那么跳过这个路点（这是为了防止车辆为了抵达规划中某个路点而进入死循环）

• 如果无人车长时间没有取得任何的进展（指比赛的进展），车辆将会调用混合A*规划出通往附近的GPS航点的路径，这个规划是无视交通规则的。

虽然Junior的策略有些是针对DRAPA挑战赛设计的（即为了尽可能赢得比赛），但是其设计的理念仍然具有参考价值。在实际的无人驾驶应用中，需要实现的状态机将更为复杂。

算法流程

注：在实现 Hybrid A*的过程中，要维持好 A* 搜索树(和RRT类似)。当我们成功使用 Reeds-Shepp 曲线时，我们需要从节点反向从树枝到树根保留路径(在反向保留路径时，不仅仅是路径点，而是一个运动模式——一段一段保存)，然后在加上Reeds-Shepp曲线。一共两个部分，共同构成了 Hybrid A*的路径。

1. 启发式搜索+损失函数设计

第一个阶段其实是对传统的 A* 算法进行改进，与之不同的是，hybrid A* 是在连续坐标系下进行启发式搜索，并且能够保证生成的轨迹满足车辆非完整性约束，但算法运行过程中该路径并不一定是全局最优的，尽管如此，这条路径是在全局最优解的“附近”（这里的全局最优解指的是由传统 A* 算法生成的）。

搜索中所用的两种启发函数

在传统的A* 算法中，由于启发函数的选取不同，运行算法后节点扩张（expansion）的效率也就不同（目的是希望算法在遍历最少的节点的情况下找到最优路径）。传统的 A* 算法的启发函数一般是2D欧几里得距离，而 hybrid A* 算法构造了两个启发函数。

1. 第一个启发函数是 Constrained heuristics ，只考虑车辆的非完整性约束而不考虑障碍物（优点是相比直接用欧几里得距离损失要好一个数量级）。该启发函数忽略了环境中的障碍物等信息，只考虑车辆的运动学特性，从终止点 $(𝑥_𝑔,𝑦_𝑔,𝜃_𝑔)=(0,0,0)$开始，计算从该点到其他点的最短路径。具体的返回值就是：

max{non-holonomic-no-obstacles-cost, 2D Euclidean Distance}

2. 第二个启发函数是第一个的对偶，称为Unconstrained heuristics，只考虑障碍物信息而不考虑车辆的非完整性约束条件（优点是引入该启发函数后能够发现2D空间中所有的U形障碍物和死胡同dead end）。随后使用2D动态规划的方法（其实就是传统的2D A* 算法）计算每个节点到终点的最短路径。

算法使用的损失函数就是两种启发函数的最大值。

下图很好地解释了两种启发函数：

• 图 a,c 是第一种启发函数下生成的路径，可以看出是连续的；
• 图 b,d 则是在第二种启发函数下生成的离散路径（与传统A* 算法得到的结果是类似的）。

车辆的微分/运动学约束

非完整性约束

车辆基本的构型空间：$𝑞=(𝑥,𝑦,𝜃)∈𝑅^2×𝑆^1$

车辆的速度： $(\dot{𝑥},\dot{𝑦})$

但在实际行驶中，车辆不能直接向左向右平移，也就是说垂直于车辆 heading 方向的速度为0.

将下图中的$𝑣_⊥$分解到 $𝑋,𝑌$ 坐标下可以得到：

$$ \begin{aligned}v_\perp\sin\theta&=\dot x\\v_\perp\cos\theta&=\dot y\end{aligned} $$

两者联立可以得到：
$$
\frac{\dot{x}}{\sin\theta}=\frac{\dot{y}}{\cos\theta}
$$
得到车辆的非完整性约束条件为：
$$
\dot{x}\cos\theta-\dot{y}\sin\theta=0
$$
根据 车辆模型 还有角度约束：
$$
\begin{array}{c}
\frac {d\theta}{dt}=\frac{tan\phi}{L}\frac{d\omega}{dt}\
\dot{\theta}= \frac {s}{L} \tan \phi
\end{array} \tag{11}
$$

路径节点扩张

论文中称节点扩张的过程为 Analytic Expansions.

状态的表示为：$x=(𝑥,𝑦,𝜃)$，其中 $(𝑥,𝑦)$ 是节点的位置，而 $𝜃$ 是节点的朝向（heading），在前文所述的搜索过程中，使用到的是离散的控制动作（control actions, or steering），因此之前每个网格中的连续状态点是不可达的。

为了进一步改进搜索速度和提高准确度，这就可以利用Reed-Shepp曲线。A* 的搜索过程都是用直线相连接，而 hybrid A* 则是在与网格精度一致的前提下（对应某一小段时间）使用三种控制动作：最大左转，最大右转，不转向 ，来生成路径，因此该路径是一些受车辆转弯半径约束的圆弧和直线。

以上过程会生成一些子节点，在此基础上，假设不考虑环境（对应第一个启发函数），算法会通过计算从起点到终点的最优Reed-Shepp曲线的方式，再生成一个额外的子节点；之后算法基于现有的障碍物地图对该路径进行碰撞检测，无碰撞路径对应的点会加到扩张树中。

可以看出与直线相比，Reed-Shepp曲线的计算量是很大的。所以论文中作者使用简单的selection rule，在每N个节点中选取一个计算Reed-Shepp曲线（这里的N随启发函数递减而减少，即越发靠近终点时，N越小）。下面这两张图也很好的解释了扩张过程。

路径损失函数的设计

损失函数的设计困难在于所生成路径的要求太多：首先路径长度或损失应该是接近最优的；路径必须是光滑的；生成的路径必须与障碍物保持一定的距离（传统的人工势场法的缺点是在狭窄路段构造了高势场，使得机器人或车辆无法通过）

解决方法：构造 Voronoi 势场函数（具体公式是下列三项连乘），voronoi 场的值随着导航中所有可行空间的大小成比例缩放。

$$ P_{vor}=w_{vor}\sum_{i=1}^N\left(\frac{\alpha}{\alpha+d_{obs}(x,y)}\right)\left(\frac{d_{vor}(x,y)}{d_{obs}+d_{vor}(x,y)}\right)\left(\frac{(d_{obs}(x,y)-d_{vor}^\vee)^2}{(d_{vor}^\vee)^2}\right) $$

其中：

• $d_{obs}$ 表示路径节点到最近的障碍物的距离

• $d_{vor}^\vee$ 表示到最近 Voronoi 图边缘的距离

• $\alpha > 0$ 控制衰减率

• $\omega _{vor}$ 表示 Voronoi 权重，表示对路径的影响

voronoi 场有如下的一些特点：

1. $𝑑_𝑂≥𝑑_𝑂^{𝑚𝑎𝑥}$时，场的值为0
2. 该场函数 $𝜌_𝑉(𝑥,𝑦)$ 的区间时 [0, 1]，且连续，$𝑑_𝑂,𝑑_𝑉$不能同时为0。
3. 在障碍物附近 Voronoi 场的值达到最大值
4. Voronoi 场的值在 GVD 的边上达到最小值

下图很好地阐释了voronoi场的效果，使用voronoi场之后车辆也能很好地通过狭窄路段。

2. 对路径进行后处理

目的

在上一步子节点扩张的过程中，路径会有一些额外的不必要控制动作（即steering），所以算法的第二个部分就是对生成点曲线进行平滑处理。

目标函数的组成

目标函数由以下四部分组成。

• 障碍物项 $𝑃_{𝑜𝑏𝑠}$：惩罚与障碍物的碰撞。公式中 $x_𝑖$是路径节点的坐标，$o_𝑖$是最近障碍物的坐标，$𝑑_{𝑜𝑏𝑠}^𝑉$是最大障碍物的距离（类似阈值）。选择二次函数作为惩罚项的目的是放大障碍物与节点越来越靠近的效果。从公式可以看出，节点靠近障碍物的时候，第一项 $|x_𝑖−o_𝑖|−𝑑_{𝑜𝑏𝑠}^𝑉 $的值会减小，因此该差值的二次方变大。

$$ P_{obs}=w_{obs}\sum_{i=1}^{N}\sigma_{obs}(|\mathbf{x}_{i}-\mathbf{o}_{i}|-d_{obs}^{\vee}) $$

• 曲率项 $𝑃_{𝑐𝑢𝑟}$： 该项相当于对路径的每个节点的瞬时曲率设置一个上限，这里 $\Delta x_i=x_i-x_{i-1},:\Delta\phi_i=\cos^{-1}\frac{\mathbf{x}i\cdot\mathbf{x}{i+1}}{|\mathbf{x}{i+1}||\mathbf{x}{i+1}|}$ ，并且 $\frac{\Delta\phi_i}{|\Delta x_i|}>\kappa_{max}$ ，惩罚函数是二次函数，即 $𝜎_{𝑐𝑢𝑟}$。

$$ P_{cur}=w_{cur}\sum_{i=1}^{N-1}\sigma_{cur}\left(\frac{\Delta\phi_i}{|\Delta\text{x}_i|}-\kappa_{max}\right) $$

• 光滑度项smoothness： $𝑃_{𝑠𝑚𝑜}$ 光滑度项计算每个节点之间位移向量的差值的平方。这一项将损失值赋给非均匀分布和方向变化的节点，以保证路径的平滑性。

$$ P_{smo}=w_{smo}\sum_{i=1}^{N-1}(\Delta\mathbf{x}_{i+1}-\Delta\mathbf{x}_i)^2 $$

• Voronoi Term $𝑃_{𝑣𝑜𝑟}$

$$
P_{vor}=\omega_\rho\sum_{i=1}^N\rho_V(x_i,y_i)
$$

这一项基于之前定义的voronoi场函数，使路径远离障碍物

优化阶段分为两个阶段：

a. 对路径的顶点坐标进行非线性优化规划问题的建模（也就是上文的损失函数定义）

b. 采用 共轭梯度方法 进行非参数化插值（non-parametrc interpolation）

hybrid A* 算法伪代码及实现流程

算法伪代码：

与A*算法类似，算法也是维护两个列表，一个open list, 一个是closed list。

算法的结束条件是：open list为空或者已经搜索到终点。

Line 21: 算法不一定会准确搜索到终点，因此引入RoundState函数，在判断当前节点是否到达终点之前对此进行合理量化。

如果没有达到终点，算法会通过执行动作空间中的所有动作对路径节点进行扩张。如果生成的节点不在C中（也就是没有被算法遍历过），则计算 cost-so-far；如果生成的节点不在O中（已被遍历过）或者所得到的 cost-so-far 小于当前节点已有的 cost，这时用当前得到的较小的 cost 更新 cost-so-far

源码实现

• matlab

  

原始论文

• 《Practical Search Techniques in Path Planning for Autonomous Driving》

• 《Path Planning in Unstructured Environments: A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle》

参考资料

• https://zhkmxx9302013.github.io/post/c0e17f81.html

• https://blog.csdn.net/qq_31815513/article/details/88709640

• https://zhuanlan.zhihu.com/p/161660932

• https://blog.csdn.net/Forwarding3/article/details/125804796

• https://zhuanlan.zhihu.com/p/40776683

• https://github.com/karlkurzer/path_planner

• https://blog.csdn.net/AdamShan/article/details/80633099

• https://github.com/teddyluo/hybrid-a-star-annotation

• https://kth.diva-portal.org/smash/get/diva2:1057261/FULLTEXT01.pdf

文章链接：
https://www.zywvvd.com/notes/study/algorithm/graph/hybrid-a-start/hybrid-a-start/