自动驾驶运动规划-Hybird A*算法(续)

YoungTimes

发布于 2022-04-28 15:30:55

1.1K0

发布于 2022-04-28 15:30:55

文章被收录于专栏：半杯茶的小酒杯半杯茶的小酒杯

Hybird A*算法保证生成的路径是车辆可实际行驶的，但它仍然包含很多不必要的车辆转向操作，我们可以对其进行进一步的平滑和优化。

Objective Function

对于Hybird A*生成的车辆轨迹序列：

{(x_1, y_1),(x_2, y_2),...,(x_N, y_N)}

,论文【1】中提出如下的目标优化函数(Objective Function)：

\begin{array}{l} w_{\rho} \sum_{i=1}^{N} \rho_{V}\left(x_{i}, y_{i}\right)+ \\ w_{o} \sum_{i=1}^{N} \sigma_{o}\left(\left|\mathbf{x}_{i}-\mathbf{o}_{i}\right|-d_{\max }\right)+ \\ w_{\kappa} \sum_{i=1}^{N-1} \sigma_{\kappa}\left(\frac{\Delta \phi_{i}}{\left|\Delta \mathbf{x}_{i}\right|}-\kappa_{\max }\right)+ \\ w_{s} \sum_{i=1}^{N-1}\left(\Delta \mathbf{x}_{i+1}-\Delta \mathbf{x}_{i}\right)^{2} \end{array}

该优化函数是Voronoi Term、Obstacle Term、Curvature Term和Smoothness Term四个部分的加权平均：第一个部分引导车辆尽可能的避开障碍物区域；第二个部分惩罚车辆与障碍物的碰撞行为；第三部分约束规划的每个点的最大曲率，并提供车辆非完整约束的保证；第四个部分是轨迹的平滑性约束。w_{\rho} 、w_{o} 、w_{\kappa} 、w_{s}

分别是这四个部分的权重因子。

1、Voronoi Term

Voronoi Term中引入了Voronoi Field的概念，Voronoi Field是机器人Motion Planning领域两种经典算法Voronoi Diagram和Potential Field的结合。

自动驾驶路径规划-Voronoi Planner

路径规划-人工势场法(Artificial Potential Field)

此处采用Voronoi Field的定义如下：

\begin{aligned} \rho_{V}(x, y)=&\left(\frac{\alpha}{\alpha+d_{\mathcal{O}}(x, y)}\right)\left(\frac{d_{\mathcal{V}}(x, y)}{d_{\mathcal{O}}(x, y)+d_{\mathcal{V}}(x, y)}\right) \\ & \frac{\left(d_{\mathcal{O}}-d_{\mathcal{O}}^{\max }\right)^{2}}{\left(d_{\mathcal{O}}^{\max }\right)^{2}} \end{aligned}

其中d_{\mathcal{O}} 和d_{\mathcal{V}}

分别是路径点(x,y)到最近障碍物的距离和到最近Voronoi Diagram的边的距离。越靠近障碍物，\rho_{V}(x, y)

的值越大，越接近1；越靠近Voronoi Edge，\rho_{V}(x, y)

的值越接近0。

上图图左一为Voronoi Field的实际效果，上图右一是标准Potential Field的实际效果。可以看到，Voronoi Field对狭窄通道的效果要明显优于Potential Field。

实际停车场的Voronoi Field和Junior的规划路径

2、Obstacle Term

Obstacle Term中x_i

是路径点坐标位置，o_i

是附近障碍物的位置，d_{max}

是决定Obstacle Term是否影响路径Cost的阈值。当路径点距离障碍物的距离小于d_{max}

时，Obstacle Term才会对轨迹的Cost进行惩罚。距离障碍物越近，|x_i - o_i|

的值越小，Obstacle Term的值就越大，整个轨迹的Cost也就越大。这样就达到了使得平滑后的路径远离障碍物的效果。

这里\sigma_{o}

一般使用二次函数。即:

\sigma_{o} \left(\left| {\mathbf{x}}_{i}-{\mathbf{o}}_{i}\right|-d_{\max }\right) = \left(\left| {\mathbf{x}}_{i}-{\mathbf{o}}_{i}\right|-d_{\max }\right)^2

3、Curvature Term

对于一系列的点X_i=\{x_i, y_i\}, i \in [1,N] ，\Delta X_i = X_i - X_{i-1} ，即为规划路径的方向向量；\Delta \phi_i 为路径点的方向角变化。\kappa = \Delta \phi / |\Delta X_i| 为X_i 处的曲率。与Obstacle Term类似，Curvature Term也设置了一个最大允许的路径曲率\kappa_{max} ，当曲率大于\kappa_{max} 时，Curvature Term才会对路径的Cost施加惩罚。

4、Smoothness Term

平滑项利用当前点前后两个方向向量的差值来衡量，方向向量既可以衡量方向的改变，也可以体现轨迹点的分布变换。

5、非线性优化

确定Objective Function函数之后，就可以利用Conjugate Gradient(CG，共轭梯度法)或者Gradient Descent求解最优路径。

Path Planning in Unstructured Environments【2】中采用了如下的梯度下降算法：

代码参见:

https://github.com/teddyluo/hybrid-a-star-annotation/blob/master/src/smoother.cpp

平滑后的路径如下：

Hybrid-A*(红色路径) VS CG path(蓝色路径)

Non-Parametric Interpolation

对路径进行非线性优化后，我们得到一条比Hybird A*算法路线更加平滑的路径，但是这条路径仍然由一段段的折线组成。在论文【1】中提到在它们的实现中组成路径的折线大约在0.5m-1m，这些折线仍然会导致车辆会出现非常生硬的转向，所以需要使用插值算法进一步平滑路径。

参数化的插值算法对噪声非常敏感，比如当路径中两个顶点非常接近时，三次样条曲线(Cubic Spline)算法的输出就会产生非常大的震荡。

【1】中提出通过固定原始路径顶点，然后在固定顶点之间插入新的顶点，最后使用Conjugate Gradient(CG，共轭梯度法)最小化曲率的非参数插值(Non-Parametric Interpolation)方法对曲线进一步平滑，平滑效果如下：

参考资料

1、Practical Search Techniques in Path Planning for Autonomous Driving。Dmitri Dolgov，Sebastian Thrun，Michael Montemerlo，James Diebel.

2、Path Planning in Unstructured Environments, A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle.

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2020-04-30，如有侵权请联系 cloudcommunity@tencent.com 删除

android

编程算法

本文分享自半杯茶的小酒杯微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体分享计划，欢迎热爱写作的你一起参与！

android

编程算法

登录后参与评论

0 条评论

热度