这篇《Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments》应该是提出Hybrid A* 方法的论文了，结合相关代码做一些学习笔记。

1 概述

论文中的路径规划方法是一个两阶段方法：
第一阶段搜索，也就是Hybrid A*，第二阶段对A*输出的路径做优化（平滑）；

2 搜索

惩罚倒车行为，与倒车的距离有关；
惩罚方向切换行为，每切换一次就做一次惩罚；
详细参考这里。

2.1 启发值

论文中提到两种启发值，第一种叫non-holonomic-without-obstacles，不考虑障碍物，但是要考虑车辆的运动约束。论文中提到对于城市挑战赛，计算该启发值使用的是160mx160m，1mx1mx5°的网格，每个节点到终点的启发值可以提前计算好。使用这种启发值比欧式距离大幅减少了扩展节点的数量，效果与环境中障碍物的的密集程度有关，随着障碍物密集程度的增加，这种提升效果会变差。第二种叫holonomic-with-obstacles，考虑障碍物，不考虑车辆的运动约束，实际使用时，取两种启发值的最大者。
下图a是障碍物相对稀疏的情况下使用欧式距离作为启发值的搜索结果，图b是障碍物相对稀疏的情况下使用non-holonomic-without-obstacles作为启发值的搜索结果，搜索次数明显减少，图c是障碍物相对稠密的情况下使用non-holonomic-without-obstacles作为启发值的搜索结果，由于没有考虑障碍物，会有很多无效的搜索，图d是结合了holonomic-with-obstacles的搜索结果，搜索次数明显减少：

2.2 可变分辨率

合适的分辨率很重要：
分辨率大的时候可能无法通过狭窄通道；
分辨率大的时候得到的结果在最优解的附近振荡可能会比较大；
分辨小的时候计算量会增大；

论文中网格分辨率保持不变，节点扩展的弧长跟节点周围的障碍物有关：
给定一个2D点$(x,y)$，计算其到最近障碍物的距离$d_o(x,y)$，计算其到最近维诺图（后续有介绍）边的距离$d_v(x,y)$，$d_o(x,y)+d_v(x,y)$的值越大，往后扩展节点时弧长越大。

3 解析扩展

解析扩展使用RS曲线，但不是每一个节点都会做解析扩展，论文中每N个点才会做一次解析扩展，其中N是节点到终点的启发值的函数，越接近终点N越小。下图中粉红色曲线就是RS曲线解析扩展的结果：

4 优化

优化分两步，第一步平滑，第二步插值。

4.1 共轭梯度法平滑轨迹（Conjugate Gradient）

路径点${\mathbf{x}}_{\mathbf{i}}=(x_i,y_i)$；
${\mathbf{o}}_{\mathbf{i}}$表示距离${\mathbf{x}}_{\mathbf{i}}$最近的障碍物；
$$\Delta {\mathbf{x}}_{\mathbf{i}}={\mathbf{x}}_{\mathbf{i}}-{\mathbf{x}}_{\mathbf{i}-1}$$
$$\Delta {\varphi }_i=|{\tan }^{-1}\frac{\Delta y_{i+1}}{\Delta x_{i+1}}-{\tan }^{-1}\frac{\Delta y_i}{\Delta x_i}|$$
平滑的代价函数可写为：
$$\begin{array}{c}{\omega }_o\sum _{i=1}^N{\sigma }_o(|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}|-d_{max})+{\omega }_k\sum _{i=1}^{N-1}{\sigma }_k(\frac{\Delta {\varphi }_i}{|\Delta {\mathbf{x}}_{\mathbf{i}}|}-k_{max})+{\omega }_s\sum _{i=1}^{N-1}(\Delta {\mathbf{x}}_{\mathbf{i}+1}-\Delta {\mathbf{x}}_{\mathbf{i}}{)}^2\end{array}$$
$k_{max}$：路径允许的最大曲率，取决于车辆最小转弯半径；
${\sigma }_o$，${\sigma }_k$表示代价函数，使用平方值就可以；
${\omega }_o$，${\omega }_k$，${\omega }_s$表示权重；
式1第一项对路径点求导：
$$\begin{array}{c}\frac{\partial {\sigma }_o}{\partial {\mathbf{x}}_{\mathbf{i}}}=2(|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}|-d_{max})\frac{{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}}{|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}|}\end{array}$$
对应代码

Vec2d Smoother::obstacleTerm(const Vec2d& xi) {
  Vec2d gradient(0, 0);
  // the vector determining where the obstacle is
  int x = (int)(xi.x()) / voronoi.resolution;
  int y = (int)(xi.y()) / voronoi.resolution;

  // the distance to the closest obstacle from the current node
  float obsDst = voronoi.getDistance(x, y);

  // if the node is within the map
  if (x < width && x >= 0 && y < height && y >= 0) {
    //从当前点xi到最近障碍点的向量 x_i-o_i
    Vec2d obsVct((xi.x()) / voronoi.resolution - voronoi.data[x][y].obstX,
                 (xi.y()) / voronoi.resolution - voronoi.data[x][y].obstY);

    // the closest obstacle is closer than desired correct the path for that
    if (obsDst < obsDMax) {
      int debug = 0;
      // 对应式2
      return gradient = wObstacle * 2 * (obsDst - obsDMax) * obsVct / obsDst;
      
    }
  }
  return gradient;
}

对于第二项，
$$\begin{array}{c}\Delta {\varphi }_i={\cos }^{-1}\frac{\Delta {{\mathbf{x}}_{\mathbf{i}}}^T\Delta {\mathbf{x}}_{\mathbf{i}+1}}{|\Delta {\mathbf{x}}_{\mathbf{i}}||\Delta {\mathbf{x}}_{\mathbf{i}+1}|}\end{array}$$
$$\begin{array}{c}{k}_i=\frac{\Delta {\varphi }_i}{|\Delta {\mathbf{x}}_{\mathbf{i}}|}\end{array}$$

$$\begin{array}{c}\frac{\partial k_i}{\partial {\mathbf{x}}_{\mathbf{i}}}=-\frac{1}{|\Delta {\mathbf{x}}_{\mathbf{i}}|}\frac{\partial \Delta {\varphi }_i}{\partial \cos (\Delta {\varphi }_i)}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}}}-\frac{\Delta {\varphi }_i}{(\Delta {\mathbf{x}}_{\mathbf{i}}{)}^2}\frac{\partial \Delta {\mathbf{x}}_{\mathbf{i}}}{\partial {\mathbf{x}}_{\mathbf{i}}}\end{array}$$
$$\begin{array}{c}\frac{\partial k_i}{\partial {\mathbf{x}}_{\mathbf{i}-1}}=-\frac{1}{|\Delta {\mathbf{x}}_{\mathbf{i}}|}\frac{\partial \Delta {\varphi }_i}{\partial \cos (\Delta {\varphi }_i)}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}-1}}-\frac{\Delta {\varphi }_i}{(\Delta {\mathbf{x}}_{\mathbf{i}}{)}^2}\frac{\partial \Delta {\mathbf{x}}_{\mathbf{i}}}{\partial {\mathbf{x}}_{\mathbf{i}-1}}\end{array}$$
$$\begin{array}{c}\frac{\partial k_i}{\partial {\mathbf{x}}_{\mathbf{i}+1}}=-\frac{1}{|\Delta {\mathbf{x}}_{\mathbf{i}}|}\frac{\partial \Delta {\varphi }_i}{\partial \cos (\Delta {\varphi }_i)}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}+1}}\end{array}$$
$$\begin{array}{c}\frac{\partial \Delta {\varphi }_i}{\partial \cos (\Delta {\varphi }_i)}=\frac{\partial {\cos }^{-1}(\cos \Delta {\varphi }_i)}{\partial \cos (\Delta {\varphi }_i)}=\frac{-1}{(1-{\cos }^2(\Delta {\varphi }_i){)}^{1/2}}\end{array}$$
$$\begin{array}{c}\mathbf{a}\perp \mathbf{b}=\mathbf{a}-\frac{{\mathbf{a}}^T\mathbf{b}}{|\mathbf{b}|}\frac{\mathbf{b}}{|\mathbf{b}|}\end{array}$$
$$\begin{array}{c}{\mathbf{p}}_1=\frac{{\mathbf{x}}_{\mathbf{i}}\perp (-{\mathbf{x}}_{\mathbf{i}+1})}{|{\mathbf{x}}_{\mathbf{i}}||{\mathbf{x}}_{\mathbf{i}+1}|}\end{array}$$
$$\begin{array}{c}{\mathbf{p}}_2=\frac{(-{\mathbf{x}}_{\mathbf{i}+1})\perp {\mathbf{x}}_{\mathbf{i}}}{|{\mathbf{x}}_{\mathbf{i}}||{\mathbf{x}}_{\mathbf{i}+1}|}\end{array}$$
$$\begin{array}{c}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}}}=-{\mathbf{p}}_1-{\mathbf{p}}_2\end{array}$$
$$\begin{array}{c}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}-1}}={\mathbf{p}}_2\end{array}$$
$$\begin{array}{c}\frac{\partial \cos (\Delta {\varphi }_i)}{\partial {\mathbf{x}}_{\mathbf{i}+1}}={\mathbf{p}}_1\end{array}$$
对应代码：

float absDxi1Inv = 1 / absDxi;  // 1/|delta_xi|，式5等式右侧第一项的第一部分
float PDphi_PcosDphi = -1 / std::sqrt(1 - std::pow(std::cos(Dphi), 2)); // 式8
float u = -absDxi1Inv * PDphi_PcosDphi;
// calculate the p1 and p2 terms
p1 = ort(xi, -xip1) / (absDxi * absDxip1); //式10
p2 = ort(-xip1, xi) / (absDxi * absDxip1); //式11
// calculate the last terms
float s = Dphi / (absDxi * absDxi); // 式5等式右侧第二项的第一部分
Vec2d ones(1, 1);
Vec2d ki = u * (-p1 - p2) - (s * ones);  // 式5
Vec2d kim1 = u * p2 - (s * ones);        // 式6
Vec2d kip1 = u * p1;                     // 式7

// calculate the gradient
gradient = wCurvature * (0.25 * kim1 + 0.5 * ki + 0.25 * kip1);

下图是路径平滑的效果示意，红色是A*输出的轨迹，蓝色是平滑后的轨迹：

4.2 平滑安全保障

式1中第一项作为障碍物距离的惩罚项，但其不能保证最终生成的路径就不会发生碰撞，原因是该项会最大化每个路径点与其最近障碍物的距离，并没有考虑到车身的形状，所以这种结果不一定是对的，为解决该问题，并且为节省计算消耗，论文没有去构建更复杂的代价函数，而是在CG每次迭代后做碰撞检查，如果有不安全的点，就把这些点锚回A*搜索到的解，如下图中的红色圆圈点，这样可以保证平滑后的解是无碰撞的。

5 维诺图

前面所述的路径规划只考虑了路径长度和与障碍物的距离，算法倾向于输出无碰撞的最短路径，往往得到的是在满足不碰撞的情况下尽可能贴着障碍物走的路径，可以使用势场来惩罚靠近障碍物，势场计算效率要高、要对节点$(x,y)$可微，论文使用维诺图。
对于一个静态的障碍物地图，生成维诺图的过程可以简单的理解为从障碍物节点开始逐渐向外扩散，扩散相交的地方就是维诺图的边，如图所示：

在维诺图的代码中，生成的维诺图是一个dataCell的二位数组，其中dataCell的定义如下：

struct dataCell {
    float dist;
    char voronoi;  // free表示维诺图的边上的点
    char queueing;
    int obstX;    // 最近障碍物的节点坐标 x
    int obstY;    // 最近障碍物的节点坐标 y
    bool needsRaise;
    int sqdist;
  };
typedef enum {voronoiKeep=-4, freeQueued = -3, voronoiRetry=-2, voronoiPrune=-1, free=0, occupied=1} State;

5.1 维诺图项的计算

给定一个2D点$(x,y)$，计算其到最近障碍物的距离$d_o(x,y)$，计算其到最近维诺图的边的距离$d_v(x,y)$，定义：
$$\begin{array}{c}{\rho }_V(x,y)=(\frac{\alpha }{\alpha +d_o(x,y)})(\frac{d_v(x,y)}{d_v(x,y)+d_o(x,y)})\times \frac{(d_o-d_o^{max}{)}^2}{(d_o^{max}{)}^2}\end{array}$$
$\alpha >0$，$d_o^{max}>0$且都为常数，分别表示falloff rate和图的最大有效范围。
当$d_o>d_o^{max}$时，${\rho }_V(x,y)=0$，${\rho }_V(x,y)$取值范围为$[0,1]$，在障碍物点处取最大值，在维诺图边上取最小值。
对点求导：
$$\begin{array}{c}\frac{\partial {\rho }_V}{\partial {\mathbf{x}}_{\mathbf{i}}}=\frac{\partial {\rho }_V}{\partial d_o}\frac{\partial d_o}{\partial {\mathbf{x}}_{\mathbf{i}}}+\frac{\partial {\rho }_V}{\partial d_v}\frac{\partial d_v}{\partial {\mathbf{x}}_{\mathbf{i}}}\end{array}$$
$$\begin{array}{c}\frac{\partial d_o}{\partial {\mathbf{x}}_{\mathbf{i}}}=\frac{{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}}{|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}|}\end{array}$$
$$\begin{array}{c}\frac{\partial d_v}{\partial {\mathbf{x}}_{\mathbf{i}}}=\frac{{\mathbf{x}}_{\mathbf{i}}-{\mathbf{v}}_{\mathbf{i}}}{|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{v}}_{\mathbf{i}}|}\end{array}$$
$$\begin{array}{c}\frac{\partial {\rho }_V}{\partial d_v}=\frac{\alpha }{\alpha +d_o}\frac{(d_o-d_o^{max}{)}^2}{(d_o^{max}{)}^2}\frac{d_o}{(d_o+d_v{)}^2}\end{array}$$
$$\begin{array}{c}\frac{\partial {\rho }_V}{\partial d_o}=\frac{\alpha }{\alpha +d_o}\frac{d_v}{d_o+d_v}\frac{(d_o-d_o^{max})}{(d_o^{max}{)}^2}\times (\frac{-(d_o-d_o^{max})}{\alpha +d_o}-\frac{d_o-d_o^{max}}{d_o+d_v}+2)\end{array}$$
对应代码：

 // 维诺图中对应的节点的最近的障碍物距离
  float obsDst = voronoi_.getDistance(xi.x(), xi.y());
  // x_i-o_i
  Vec2d obsVct(xi.x() - voronoi_.GetClosetObstacleCoor(xi).x(),
               xi.y() - voronoi_.GetClosetObstacleCoor(xi).y());

  double edgDst = 0.0;
  // 最近的维诺图中的边上的点，通过遍历维诺图的边上的点来查找
  Vec2i closest_edge_pt = voronoi_.GetClosestVoronoiEdgePoint(xi, edgDst);
  // x_i-v_i
  Vec2d edgVct(xi.x() - closest_edge_pt.x(), xi.y() - closest_edge_pt.y());

  if (obsDst < vorObsDMax_ && obsDst > 1e-6) {
    if (edgDst > 0) {
      Vec2d PobsDst_Pxi = obsVct / obsDst;
      Vec2d PedgDst_Pxi = edgVct / edgDst;
      // 式19
      float PvorPtn_PedgDst = (alpha_ / alpha_ + obsDst) *
       (pow(obsDst - vorObsDMax_, 2) / pow(vorObsDMax_, 2)) * (obsDst / pow(obsDst + edgDst, 2));
      // 式20
      float PvorPtn_PobsDst = (alpha_ / (alpha_ + obsDst)) *
                              (edgDst / (edgDst + obsDst)) * ((obsDst - vorObsDMax_) / pow(vorObsDMax_, 2))
                              * (-(obsDst - vorObsDMax_) / (alpha_ + obsDst) - (obsDst - vorObsDMax_) / (obsDst + edgDst) + 2);
      gradient = wVoronoi_ * (PvorPtn_PobsDst * PobsDst_Pxi + PvorPtn_PedgDst * PedgDst_Pxi) * 100;
     }

如图所示，图a为障碍物地图，图c是按式2计算得到势场图，图d是另一种势场图。作为对比图c中即便是狭窄的通道之间也有一条$\rho =0$的路径，而图d中狭窄通道之间的$\rho$值就比较高。

把式15加到式1里
$$\begin{array}{c}{\omega }_o\sum _{i=1}^N{\sigma }_o(|{\mathbf{x}}_{\mathbf{i}}-{\mathbf{o}}_{\mathbf{i}}|-d_{max})+{\omega }_k\sum _{i=1}^{N-1}{\sigma }_k(\frac{\Delta {\varphi }_i}{|\Delta x_i|}-k_{max})+{\omega }_s\sum _{i=1}^{N-1}(\Delta {\mathbf{x}}_{\mathbf{i}+1}-\Delta {\mathbf{x}}_{\mathbf{i}}{)}^2+{\omega }_{\rho }\sum _{i=1}^N{\rho }_V(x_i,y_i)\end{array}$$
这就是轨迹平滑时的优化项。

6 轨迹插值

论文中平滑后的轨迹点之间的距离在0.5m到1m之间，通过插值可以使路径更平滑，控制更连续。论文提到参数化插值比如样条曲线易受噪声影响，如下图三次样条曲线，红点的轻微移动曲线就有较大的变化：

论文中使用非参数化插值方法：先对路径进行超采样，随后在固定原始点的约束下，使用共轭梯度法（CG）最小化路径曲率。