【论文研读】路径规划中的Hybrid A*算法

本文目的

由于在carla-autoware的示例中使用了hybrid A* 算法，所以本文基于以下两篇文章对hybrid A* 算法过程进行整理：（文中挑选了一些个人认为便于理解算法的图片，均来自于以下这两篇文章）

1. 《Practical Search Techniques in Path Planning for Autonomous Driving》
2. 《Path Planning in Unstructured Environments: A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle》
3. 第二篇论文ROS实现的github链接：https://github.com/karlkurzer/path_planner

其实我觉得对这两篇文章还有一些理解不到位的地方，望指正！等开始看autoware的源码之后再来更新这篇。

一、算法简介

解决的问题

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

其他算法中常见问题

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

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

算法的大致过程

1. 在连续坐标系下进行启发式搜索（改进了A*算法）
2. 第二个阶段用数值非线性优化的方法（即共轭梯度下降法）对生成路径的“质量”进行提升，使其至少是局部最优的。（下文对这两个过程进行详细说明）

二、STAGE 1: 启发式搜索+损失函数设计

第一个阶段其实是对传统的A* 算法进行改进，与之不同的是，hybrid A* 是在连续坐标系下进行启发式搜索，并且能够保证生成的轨迹满足车辆非完整性约束（下文对车辆的非完整性约束进行补充），但算法运行过程中该路径并不一定是全局最优的，尽管如此，这条路径是在全局最优解的“附近”（这里的全局最优解指的是由传统A* 算法生成的）。下图（来自文献1）可以很好地解释传统A* 与hyvbrid A* 的异同。两种算法都是基于网格世界的（grid world），A* 是赋予每个网格的中心点相应的损失并且算法只访问这些中心点，而hybrid A* 是先在这些网格中挑选满足车辆3D连续状态的点，并将损失赋值给这些点。（下图中间的图是用其他A* 变体算法实现的）

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

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

1. 第一个启发函数是Constrained heuristics
   ，只考虑车辆的非完整性约束而不考虑障碍物（优点是相比直接用欧几里得距离损失要好一个数量级）。该启发函数忽略了环境中的障碍物等信息，只考虑车辆的运动学特性，从终止点$(x_g,y_g,{\theta }_g)=(0,0,0)$开始，计算从该点到其他点的最短路径。具体的返回值就是：
   $$\text{max}\{\text{non-holonomic-no-obstacles-cost},\text{2D Euclidean Distance}\}$$

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

算法使用的损失函数就是两种启发函数的最大值。下图很好地解释了两种启发函数：图a,c是第一种启发函数下生成的路径，可以看出是连续的；而图b,d则是在第二种启发函数下生成的离散路径（与传统A* 算法得到的结果是类似的）

补充：车辆的微分/运动学约束——非完整性约束
车辆基本的构型空间是：$q=(x,y,\theta )\in {\mathbb{R}}^2\times {\mathbb{S}}^1$，车辆的速度是$(\dot{x},\dot{y})$。但在实际行驶中，车辆不能直接向左向右平移，也就是说垂直于车辆heading方向的速度为0，将下图中的$v_{\perp }$分解到$X,Y$坐标下可以得到：
$$v_{\perp }\sin \theta =\dot{x}$$$$v_{\perp }\cos \theta =\dot{y}$$两者联立可以得到：$$\frac{\dot{x}}{\sin \theta }=\frac{\dot{y}}{\cos \theta }$$可以得到车辆的非完整性约束条件为：$$\dot{x}\cos \theta -\dot{y}\sin \theta =0$$

2. 路径节点扩张的过程

论文中称节点扩张的过程为Analytic Expansions（解析扩张？）。首先状态的表示为：$\mathbf{x}=(x,y,\theta )$，其中$(x,y)$是节点的位置，而$\theta$是节点的朝向（heading），在前文所述的搜索过程中，使用到的是离散的控制动作（control actions, or steering），因此之前每个网格中的连续状态点是不可达的。为了进一步改进搜索速度和提高准确度，这就可以利用Reed-Shepp曲线。A* 的搜索过程都是用直线相连接，而hybrid A* 则是在与网格精度一致的前提下（对应某一小段时间）使用三种控制动作：最大左转，最大右转，不转向来生成路径，因此该路径是一些受车辆转弯半径约束的圆弧和直线。

以上过程会生成一些子节点，在此基础上，假设不考虑环境（对应第一个启发函数），算法会通过计算从起点到终点的最优Reed-Shepp曲线的方式，再生成一个额外的子节点；之后算法基于现有的障碍物地图对该路径进行碰撞检测，无碰撞路径对应的点会加到扩张树中。可以看出与直线相比，Reed-Shepp曲线的计算量是很大的。所以论文中作者使用简单的selection rule，在每N个节点中选取一个计算Reed-Shepp曲线（这里的N随启发函数递减而减少，即越发靠近终点时，N越小）。下面这两张图也很好的解释了扩张过程。

3. 路径损失函数的设计

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

The key advantage of the Voronoi field over a conventional potential fields is the fact that the field value is scaled in proportion to the total available clearance for navigation. As a result, even narrow openings remain navigable, which is not always the case for standard potential fields.

解决方法：构造Voronoi势场函数（具体公式是三项连乘，物理意义我还没有搞清楚，之后查下相关资料），voronoi场的值随着导航中所有可行空间的大小成比例缩放。公式中$d_O,d_V$分别代表路径节点到最近的障碍物和最近的GVD（广义voronoi图）的长度，$\alpha >0$ 控制场的衰减率（Falloff Rate）

下图很好地阐释了voronoi场的效果，从中可以看出voronoi场的一些特点：

1. $d_O\ge d_O^{max}$时，场的值为0
2. 该场函数${\rho }_V(x,y)$的区间时[0, 1]，且连续，$d_O,d_V$不能同时为0。
3. 在障碍物附近Voronoi场的值达到最大值
4. Voronoi场的值在GVD的边上达到最小值
   

三、STAGE2: 对路径进行后处理

1. 进行后处理的目的

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

2. 后处理所用目标函数的组成

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

1. 障碍物项$P_{obs}$：惩罚与障碍物的碰撞。公式中${\mathbf{x}}_i$是路径节点的坐标，${\mathbf{o}}_i$是最近障碍物的坐标，$d_{obs}^V$是最大障碍物的距离（类似阈值）。选择二次函数作为惩罚项的目的是放大障碍物与节点越来越靠近的效果。从公式可以看出，节点靠近障碍物的时候，第一项$|{\mathbf{x}}_i-{\mathbf{o}}_i|-d_{obs}^V$ 的值会减小，因此该差值的二次方变大。
   

2. 曲率项$P_{cur}$
   该项相当于对路径的每个节点的瞬时曲率设置一个上限，这里$\Delta x_i=x_i-x_{i-1}$，$\Delta {\varphi }_i={\text{cos}}^{-1}\frac{{\mathbf{x}}_i\cdot {\mathbf{x}}_{i+1}}{|{\mathbf{x}}_{i+1}||{\mathbf{x}}_{i+1}|}$，并且$\frac{\Delta {\varphi }_i}{|\Delta x_i|}>{\kappa }_{max}$，惩罚函数是二次函数，即${\sigma }_{cur}$。
   

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

4. Voronoi Term——$P_{vor}$
   $$P_{vor}={\omega }_{\rho }\sum _{i=1}^N{\rho }_V(x_i,y_i)$$
   这一项基于之前定义的voronoi场函数，使路径远离障碍物，
   （向量x就是路径点的坐标x, y）

优化阶段分为两个阶段：
a. 对路径的顶点坐标进行非线性优化规划问题的建模（也就是上文的损失函数定义）
b. 采用共轭梯度方法进行非参数化插值（non-parametrc interpolation）
（这里具体的共轭梯度方法，学习了之后再更新）

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

1. hybrid A* 算法伪代码：
   （这一部分之后看完autoware源码之后再详细地对比、解释）
   与A*算法类似，算法也是维护两个列表，一个open list, 一个是closed list。算法的结束条件是：open list为空或者已经搜索到终点。Line 21: 算法不一定会准确搜索到终点，因此引入RoundState函数，在判断当前节点是否到达终点之前对此进行估算。如果没有达到终点，算法会通过执行动作空间中的所有动作对路径节点进行扩张。如果生成的节点不在C中（也就是没有被算法遍历过），则计算cost-so-far；如果生成的节点不在O中（已被遍历过）或者所得到的cost-so-far小于当前节点已有的cost，这时用当前得到的较小的cost更新cost-so-far
   
2. 算法实现的流程：
   这里是第二篇文献中的实现流程，算法的输入为事先定义好的障碍物地图，经过以上的两步：hybrid A* 搜索和路径平滑之后在rviz中显示路径。