【HybridAtar论文阅读笔记】【Practical Search Techniques in Path Planning for Autonomous Driving】

Practical Search Techniques in Path Planning for Autonomous Driving

Metadata

• CiteKey: PracticalSearchTechniques
• Type: journalArticle
• Title: Practical Search Techniques in Path Planning for Autonomous Driving,
• Author: Dolgov, Dmitri; Thrun, Sebastian; Montemerlo, Michael; Diebel, James,

Abstract

🔤我们描述了一种实用的路径规划算法，该算法为在未知环境中运行的自动驾驶汽车生成了平稳的路径，在该环境中，机器人的传感器在线检测到障碍物。这项工作是受到2007年DARPA城市挑战赛的激励和实验验证的，机器人车必须自动驾驶停车场。我们的方法有两个主要步骤。第一步使用了众所周知的A搜索算法的变体，该变体应用于车辆的3D运动状态空间，但具有修改后的状态更高规则，可在A的离散节点中捕获车辆的连续状态。 （从而保证路径的运动可行性）。然后，第二步通过数字非线性优化提高了解决方案的质量，从而导致局部（且经常全局）最佳。斯坦福赛车队Junior在Urban Challenge中使用了本文所述的路径规划算法。初中表现出在复杂的一般路径规划任务中表现出完美的表现，例如在停车场导航和在被阻塞的道路上执行掉头，典型的饱满的重型时间为50-300ms。
We describe a practical path-planning algorithm that generates smooth paths for an autonomous vehicle operating in an unknown environment, where obstacles are detected online by the robot’s sensors. This work was motivated by and experimentally validated in the 2007 DARPA Urban Challenge, where robotic vehicles had to autonomously navigate parking lots. Our approach has two main steps. The first step uses a variant of the well-known A* search algorithm, applied to the 3D kinematic state space of the vehicle, but with a modified state-update rule that captures the continuous state of the vehicle in the discrete nodes of A* (thus guaranteeing kinematic feasibility of the path). The second step then improves the quality of the solution via numeric non-linear optimization, leading to a local (and frequently global) optimum. The path-planning algorithm described in this paper was used by the Stanford Racing Teams robot, Junior, in the Urban Challenge. Junior demonstrated flawless performance in complex general path-planning tasks such as navigating parking lots and executing U-turns on blocked roads, with typical fullcycle replaning times of 50–300ms.

Notes

1. 启发式值

算法使用的启发式算法并不是独有的，而是通用的。文章共使用了3种启发式进行测试：
（1）欧氏距离启发式——图a
（2）取欧氏距离和非完整无障碍代价的最大值的启发式——图b，c
（3）忽略非完整只考虑障碍的启发式与（2）的结合的启发式——图d

由图可知，单纯用欧几里得距离启发式，扩展了21515个节点（图a）。如果用非完整无障碍启发式，则拓展了1465个节点（图b）。但是非完整无障碍启发式在断头路会耗费很多及节点（图c）。通过非完整无障碍启发式和纯障碍启发式结合，可以将该问题解决（图d）

2. 节点拓展规则

为了解决节点拓展时精度不够的问题，选择了reed-sheep曲线（下面简称为R-S曲线或R-S拓展）作为节点拓展的工具。

具体而言，在拓展一些节点时，通过计算从当前节点到目标节点的reed-shepp曲线来生成一个额外的节点。然后检查该路径是不是存在障碍物冲突，如果没有冲突，就将其增加到搜索树(closed表)中。

至于为什么是部分节点，而不是每个节点，作者认为，在远离目标的区域，大部分的路径都是有障碍物的，没有必要使用R-S拓展。作者采用的方法时：只从N个节点中选择1个节点拓展，而N随着距离目标的代价启发式函数值得增加而减小。

3. 路径代价的计算

3.1 定义维诺场

本文提出了维诺场（The Voronoi Field）的概念，用来计算障碍物代价。

• 维诺场值（The Voronoi Field）的计算公式——对应图Fig5（a）
  

公式中的定义：
$d_O(x,y)$代表(x,y)距离最近的障碍物（Fig.5(a)）的距离
$d_V(x,y)$代笔(x,y)距离一般维诺图（GVD）（Fig.5(b)）中最近的边的距离。
$d_O$ 此处应当是$d_O(x,y)$，应该是作者笔误。
$\alpha$ 是常数，用来控制图中最大影响范围的下降率。
$d_O^{max}$ 也是常数，用来控制场的最大影响范围。

需要注意的是，式（1）需要满足$d_O(x,y)<=d_O^{max}$，否则的话，${\rho }_V(x,y)=0$

举个例子理解这个公式：如果(x,y)取图中蓝色的框标明的位置，那么$d_V$值接近0，代价值很小。最极端的情况，(x,y)刚好位于一般维诺图的边缘上，此时$d_V=0$，那么整个代价${\rho }_V(x,y)$也是0.

• 简单常规势场值计算公式——对应图Fig5（c）
  $$\rho (x,y)=\alpha (\alpha +d_O(x,y){)}^{-1}$$
  Fig5：（a）维诺场（2）维诺场对应的维诺图（3）常规势场
  

3.2 维诺场比常规势场的优势：

• 优势在于：
  维诺场的势场值按照导航中全部可通行的间隙进行了比例缩放。即使狭窄的开口仍然可以导航。而常规的势场则不行。

例如在FIg.5中，（a）是维诺场的2维投影，（b）是相对广义的维诺图。注意到在障碍物之间，总是有一个连续的路径使得场值${\rho }_V(x,y)=0$。与（c）相比，在障碍物之间的狭窄的通道中，总是存在高场值的区域（黄色圆圈区域），会导致规划的时候无法通行。

3.3 维诺场与其他类似的场函数的比较

关于维诺图和势场，在机器人运动规划领域已经提出了很长时间了，例如( Sensor-based exploration: The hierarchical generalized voronoi graph)[[@doi10.117702783640022066770]]中使用分层广义维诺图（HGVG）来对机器人搜索路径。但是对于非完整性约束的汽车，直接使用维诺图规划不可行。本文并不是直接使用维诺场进行全局路径导航，但是注意到对于凸障碍空间，维诺场可以由全局吸引域增强，产生一个没有局部最小值的域，因此也可以适合于全局路径导航。

4. 路径的优化

• 可以尝试展示路径优化的过程
• 共轭梯度下降的概念了解：conjugate gradient (CG) descent

采用HybridAStar规划的路径是次优的，而且可能会存在不必要的突然转向，所以作者引入了后处理对路径进行优化。后处理分为两个阶段。

• 第一个阶段：非线性优化
  在路径顶点坐标上制定了一个非线性优化程序，以改善路径长度和平滑度。

• 第二个阶段：非线性插值
  第一个阶段得到的路径顶点之间仍然存在间隙，会导致突然转向，所以需要进行非参数的差值，使得路径顶点之间的曲率变化更加平缓。

4.1 第一个阶段：非线性优化

在路径顶点坐标上制定了一个非线性优化程序，以改善路径长度和平滑度。

4.1.1变量的定义：

顶点坐标序列：${\mathbf{x}}_{\mathbf{i}}=(x_i,y_i),i\subset [1,N]$
障碍物接近顶点的位置：${\mathbf{o}}_{\mathbf{i}}$
顶点移动向量：$\mathbf{\Delta }{\mathbf{x}}_{\mathbf{i}}$