Hybrid A*论文，Practical Search Techniques in Path Planning for Autonomous Driving笔记

Practical Search Techniques in Path Planning for Autonomous Driving

Code reference here:

• KTH GitHub repository based on ROS and OMPL

1. Introduction and Related Work

The first step uses a heuristic search in continuous coordinates that guarantees kinematic feasibility of computed trajectories.

The second step uses conjugate gradient (CG) descent to locally improve the quality of the solution, producing a path that is at least locally optimal, but usually attains the global optimum as well.

2. Hybrid-State A* Search

与传统A star只能经过cell的center不同，本方法是可以取到cell的内部或边界点的。但是并不是一个cell中任意一点都能取到（在比较小的分辨率下），由于动力学约束的限制。

搜索空间为 ( x , y , θ ) (x,y,\theta) (x,y,θ)比传统的网格A star多了角度，均为离散量。

虽然这样得到的路径path不一定是最优（minimum cost）的，但是drivable的，而不是传统A star生成的piecewise-linear的形式。

注意（我的想法）：A star类算法不同于RRT类采样算法，都不可避免地要面对网格划分这一问题，网格划分地过大，则路径最优性较差，网格划分地过小，则对于一定体积的robot没有意义（无法达到那样的精度，可能是动力学约束或分辨率太低，远小于模型大小）；所以分辨率的选择也是问题。

整个hybrid A star过程分为两步，一是Heuristics，二是Analytic Expansion。

Heuristics：

Our search algorithm is guided by two heuristics.

These heuristics do not rely on any properties of hybrid-state A* and are also applicable to other search methods (e.g., discrete A*).

单纯从heuristic searching algorithm而言，不只是用于hybrid a star，也用于其他搜索方法。

启发式搜索的第一阶段使用non-holonomic-without-obstacles，不考虑障碍物的非完整性约束搜索方法（考虑车辆的非完整性约束），从 ( x , y , θ ) (x,y,\theta) (x,y,θ)到 ( x g , y g . θ g ) (x_g,y_g.\theta_g) (xg​,yg​.θg​)的邻域（因为完全的精确，在离散空间内是无法达到的），完全忽略障碍物，得到最短路径，启发项；

这里有两个问题：

• 首先是原文并不是这么翻译的，原文为To compute it, we assume a goal state of ( x g , y g , θ g ) = ( 0 , 0 , 0 ) (x_g,y_g,\theta_g)=(0,0,0) (xg​,yg​,θg​)=(0,0,0) and compute the shortest path to the goal from every point ( x , y , θ ) (x,y,\theta) (x,y,θ) in some discretized neighborhood of the goal, assuming complete absence of obstacles. 这样看来的翻译是将goal设为全0，在其离散领域内的所有点寻找到goal的最短路径，尽管翻译是这样，但我暂时看不出这样做有什么意义，留一个TODO
• 第二个问题是，原文说到We then use a max of the non-holonomic-without-obstacles cost and 2D Euclidean distance as our heuristic；这个cost和2D距离如何在and的情况下，作启发项h？

作者的解释是，这样的做法可以cut off search branches that approach the goal with wrong headings；我觉得使用欧氏距离做启发项的一项，或是comparative是有道理的，尤其是当NHWO项过大的时候，具体还需要看代码，或者继续读，留一个TODO

第二阶段忽略车辆的非完整性约束，采用2-D动态规划，仅使用障碍物地图计算到达目标点的最短距离。

解决U-shape的问题。

Analytic Expansion：

可以理解为，越趋近于goal，在每一个node的下一个child中，会有更大的频率插入无碰撞的Reed-Sheep曲线的check，若为真，停止搜索直接使用RS曲线。

3. Path-Cost Function Using the Voronoi Field

the author define Voronoi field as a field, whose concept comes from potential field and generalized Voronoi field.

文章中定义的Voronoi场来自potential field和Voronoi Diagram两个概念，相当于在GVD上应用了potential的方法。
ρ V ( x , y ) = ( α α + d o ( x , y ) ) ( d V ( x , y ) d O ( x , y ) + d V ( x , y ) ) ( d O − d O m a x ) 2 ( d O m a x ) 2 \rho_V(x,y)=(\frac{\alpha}{\alpha+do(x,y)})(\frac{d_V(x,y)}{d_O(x,y)+d_V(x,y)})\frac{(d_O-d_O^{max})^2}{(d_O^{max})^2} ρV​(x,y)=(α+do(x,y)α​)(dO​(x,y)+dV​(x,y)dV​(x,y)​)(dOmax​)2(dO​−dOmax​)2​
其中：

• d O d_O dO​为到最近obstacle的距离
• d V d_V dV​为到GVD的edge的距离，应该也是最近
• α \alpha α为一个大于0的常数
• d O m a x d_O^{max} dOmax​作者虽然没有给出解释，但其是一个“安全值”，距离障碍物的距离超出该安全值后，该障碍物在场中的权重 ρ \rho ρ为0

α \alpha α与 d O d_O dO​为大于0的常数，控制falloff rate和Voronoi场的最大影响范围；上式针对的是 d O < d O m a x d_O<d_O^{max} dO​<dOmax​的情况，如果不符合该情况，则 ρ V ( x , y ) = 0 \rho_V(x,y)=0 ρV​(x,y)=0。

上式具有如下的性质：

• 如果 d O > d O m a x d_O>d_O^{max} dO​>dOmax​，则 ρ V ( x , y ) = 0 \rho_V(x,y)=0 ρV​(x,y)=0
• ρ V ( x , y ) ∈ [ 0 , 1 ] \rho_V(x,y)\in[0,1] ρV​(x,y)∈[0,1]，且关于 ( x , y ) (x,y) (x,y)连续
• ρ V ( x , y ) \rho_V(x,y) ρV​(x,y)在obstacle内部达到最大值
• ρ V ( x , y ) \rho_V(x,y) ρV​(x,y)在GVD的edge上达到最小值

使用作者提出的Voronoi field的好处是，没有传统的potential field那么保守，使得一些狭窄的，在原potential field中不能navigate的passage变得navigable，原文如下：

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.

稀疏点云似乎不能被用作规划，但稠密点云如何构建GVD，又如何从GVD构建VF，这是第三部分的关键问题。

这个问题可以从Google和论文代码中找到答案。

所以VD及其衍生的diagram，如果robot沿着或趋向于沿着他们的edge移动，这样的path是clear to obstacles的，但这并不一定是最优（比如时间，距离上），又或者根据GVD的edge扩张方法，或是本文中提到的VF场，来增加可行域的面积，为最优path提供先决条件。

本章节作者完成了VF的定义，从程序上理解为，输入一张map（可能是mat或什么数据格式），输出一张基于此map的VF图，之后的工作就交给planner去做了。

4. Local Optimization and Smoothing

分为两个优化阶段：

• non-linear optimization on coordinates of the vertices, improves the length and smoothness of the solution
• non-parametric interpolation, with higher resolution path discretization

given a sequence of vertices x i = ( x i , y i ) , i ∈ [ 1 , N ] x_i=(x_i,y_i),\quad i\in [1,N] xi​=(xi​,yi​),i∈[1,N]

• o i \textbf{o}_i oi​，距离vertex最近的obstacle的位置
• Δ x i = x i − x i − 1 \Delta \textbf{x}_i=\textbf{x}_i -\textbf{x}_{i-1} Δxi​=xi​−xi−1​, the displacement vector at the vertex
• Δ ϕ i \Delta \phi_i Δϕi​, the change in the tangential angle at the vertex

and Δ ϕ i \Delta \phi_i Δϕi​
Δ ϕ i = ∣ tan ⁡ − 1 Δ y i + 1 Δ x i + 1 − tan ⁡ − 1 Δ y i Δ x i ∣ \Delta \phi_i = | \tan^{-1}\frac{\Delta y_{i+1}}{\Delta x_{i+1}} - \tan^{-1}\frac{\Delta y_i}{\Delta x_i} | Δϕi​=∣tan−1Δxi+1​Δyi+1​​−tan−1Δxi​Δyi​​∣
The objective function is
ω ρ ∑ i = 1 N ρ V ( x i , y i ) + ω o ∑ i = 1 N σ o ( ∣ x i − o i ∣ − d m a x ) + ω k ∑ i = 1 N − 1 σ k ( Δ ϕ i ∣ Δ x i ∣ − k m a x ) + ω s ∑ i = 1 N − 1 ( Δ x i + 1 − Δ x i ) 2 \omega_{\rho}\sum_{i=1}^{N}\rho_V(x_i,y_i)+\omega_o\sum_{i=1}^{N}\sigma_o(|\textbf{x}_i-\textbf{o}_i|-d_{max})+\omega_k\sum_{i=1}^{N-1}\sigma_k(\frac{\Delta \phi_i}{|\Delta \textbf{x}_i|}-k_{max})+\omega_s\sum_{i=1}^{N-1}(\Delta \textbf{x}_{i+1}-\Delta \textbf{x}_i)^2 ωρ​i=1∑N​ρV​(xi​,yi​)+ωo​i=1∑N​σo​(∣xi​−oi​∣−dmax​)+ωk​i=1∑N−1​σk​(∣Δxi​∣Δϕi​​−kmax​)+ωs​i=1∑N−1​(Δxi+1​−Δxi​)2

• ρ V \rho_V ρV​ is Voronoi field, defined in part3
• k m a x k_{max} kmax​ is the maximum allowable curvature of the path (defined by turning radius of the car)
• σ o \sigma_o σo​ and σ k \sigma_k σk​ are penalty functions (empirically, simple quadratic penalties can work well)
• ω \omega ω are weights

and for each items

• The first term of the cost function effectively guides the robot away from obstacles in both narrow and wide passages.
• The second term penalizes collisions with obstacles.
• The third term upper-bounds the instantaneous curvature of the trajectory at every node and enforces the non-holonomic constraints of the vehicle.
• The fourth term is a measure of the smoothness of the path.