Cpp五条

这个屌丝很懒，什么也没留下！

热门标签

深度科普：机器人都在用的Hybrid A*算法，你知道多少？

作者：Cpp五条 | 2024-05-17 04:27:44

踩

hybrid a*

全局路径规划，是指在已知的环境中为机器人规划一条最优行驶路径。本文将对比经典的A*算法，深度探讨机器人常用全局路径规划算法——Hybrid A*算法的原理，包括Hybrid A*算法特性、RS曲线、代价函数与启发式、节点拓展、碰撞检测，以及局部优化与平滑等内容。

01 Hybrid A*算法特性

Hybrid A*算法是一种图搜索算法，是基于A*算法的一种「变形」。

A*算法采用贪心策略，结合启发式的引导，在静态网路中求解最短路径有着非常不错的效果。但它在规划过程中，将机器人看做一个质点，只考虑（x，y）两个维度，未考虑机器人的实际运动约束。

而Hybrid A*算法增加了θ维度，优化了节点的扩展方式，可以更好地满足车辆的运动特性，在有障碍物、低速等场景的阿克曼底盘、无人车等存在运动约束的机器人中，具有更好的使用表现。至于差速、全向机器人等不存在运动约束的机器人场景中，则不需要用到Hybrid A*算法。

应用中，A*算法规划的路径点，都在各个栅格的中心；Hybrid A*算法的路径点可以在栅格的任意位置，并且通常使用Reeds-Shepp曲线连接，生成平滑路径。

相较于Dubins曲线只允许车辆向前运动，Reeds-Shepp曲线运动模型既允许车辆向前运动，也允许车辆向后运动。

据J Reeds和L.A. Shepp证明，Reeds-Shepp Car从起点qI到终点qG的最短路径，一定在下面的word之中：

且所有word组合不超过48种：

注：word中的"|"表示车辆运动朝向，由正向转为反向或者由反向转为正向。每个word都由L+，L-，R+，R-，S+，S-这六种primitives组成。其中，L+表示车辆左转前进；L-表示车辆左转后退；R+表示车辆右转前进；R-表示车辆右转后退，S+表示车辆直行前进；S-表示车辆直行后退。 $C_{\beta}$ 表示两个曲线长度一样的曲线部分， $C_{\pi/2}$ 表示曲线部分有90度。

02 代价函数与启发式

在A*算法中，代价函数通常是从起点到当前节点的距离之和。

Hybrid A*算法则结合实际情况，对车辆的角度、挡位、前后向变化等因素添加惩罚，使得规划的轨迹更加符合实际运动场景。

另外，启发式也是非常重要的一环，主要采用了两种启发式算法。

第一种启发式——无障碍物的非完整约束启发式。主要考虑车辆的运动特性，但是忽略障碍物。

即将车辆的最小半径作为输入，并且获取从当前点到目标点的最优曲线路径（如RS曲线、Dubins曲线等），然后计算最优曲线的长度作为启发函数的代价值。

第二种启发式是第一种启发式的对偶——有障碍物的完整性启发式。主要考虑环境中的障碍物信息，忽略车辆的运动特性。

通过在每个节点使用Djikstra算法，获得该节点到达目标点的最近距离作为其代价函数的代价值。Djikstra算法能够在类似迷宫环境中，获得扩展节点到目标点的最近路径，因此可以避免Hybrid A*算法在扩展过程中朝向不正确的问题。

之后比较两种启发式的代价值大小，选择其中较大的代价作为Hybrid A*的代价值。实践证明，这样可以极大提升规划效率和效果。

注：（a）二维欧氏距离扩展21515个节点。（b）无障碍物的非完整约束启发式是一个显著的改进，因为它扩展了1465个节点，但是如（c）所示，它会导致在更复杂的环境中（68730个节点）对死胡同的浪费性探索。结合使用两种启发式之后，有比较明显的改善（10588节点）如（d）所示。

由于这些启发式不依赖Hybrid A*的任何特性，也同样适用于A*算法等其他搜索方法。

03 节点扩展

A*算法的扩展节点，是在四个或者八个方向相邻离散栅格的中心点，因此无法表达在连续空间中车辆的朝向和位置。

Hybrid A*算法则是通过考虑车辆运动特性生成的轨迹替代A*算法中的节点。同时在扩展节点时，会充分考虑车辆的运动特性，所以可以生成更加符合车辆运动特性的轨迹。

Hybrid A*算法节点扩展的通常做法是：从当前节点出发，根据预先设定的运动步长，同时考虑最大转弯半径，从前向和后向采样车辆运动轨迹。对采样的运动轨迹做碰撞检测，满足车辆运动条件的运动轨迹即可作为待选节点。

另外考虑到运算量，普通节点间的扩展通常不使用RS曲线，只有在接近目标节点时才会使用RS曲线生成目标轨迹。

04 碰撞检测

在全局路径规划过程中，为了避免机器人与障碍物发生碰撞，需要将规划的路径与障碍物保持安全距离。

A*算法的处理方式是，通过检查路径经过的栅格占据状态来判断碰撞。

而Hybrid A*算法是在机器人的路径采样过程中，在路径的每个采样点上结合机器人的外形和位姿，来判断机器人各时刻的轮廓范围内是否存在障碍物。如果存在障碍物，就放弃这条采样路径。

这种碰撞检测方法，可以很好地结合机器人实际尺寸，和各采样时刻的姿态准确判断碰撞关系，但同时也大大增加了算法的复杂度。

05 局部优化和平滑

由于Hybrid A*算法产生的路径往往不是最优解，通常存在不自然转向以及不必要转向，需要进一步改进优化。

第一步，先对路径点进行非线性优化。

我们可以通过局部平滑的目标函数，得到更加平滑的路径：

$\begin{aligned} & 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{aligned}$

注： $\rho_V$ 为Voronoi场，可以有效地引导机器人远离窄通道和宽通道中的障碍物； $\kappa_{max}$ 是路径的最大允许曲率，即与障碍物的碰撞惩罚； $\sigma_o$ 和 $\sigma_\kappa$ 是惩罚函数，即每个节点上轨迹的瞬时曲率，并加强车辆的非完整约束； $w_\rho$ , $w_o$ , $w_\kappa$ , $w_s$ 是权重，对路径平滑性的度量。

其中，Voronoi场用来获取与障碍物之间的关系，定义如下：

$\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}}$ 是节点到达维诺地图边的最近距离； $\alpha$ 表示势能下降速率，为常数，当 $\alpha$ 值越大，(x,y)处的势能下降得越慢； $d_{\mathcal{O}}^{\max }$ 表示势能的控制范围，为常数。其中，节点(x,y)处的势能，取值从0到1，当 $d_{\mathcal{O}} >d_{\mathcal{O}}^{\max }$ 时，(x,y)势能为0，势能值到达最大的时候是(x,y)在障碍物上或者里面，势能值到达最小的时候是(x,y)在广义泰森多边形的边上。