羊村懒王

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

热门标签

【自动驾驶轨迹规划之hybrid A*算法】_自动驾驶轨迹规划算法

作者：羊村懒王 | 2024-04-22 07:56:38

踩

自动驾驶轨迹规划算法

欢迎大家关注我的B站：

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

本文参考论文：https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf

这篇文章是基于经典A*算法与reeds-shepp曲线的，不清楚的读者请参考

【自动驾驶轨迹规划5之A*算法】_无意2121的博客-CSDN博客

【自动驾驶轨迹规划之dubins曲线与reeds-shepp曲线】_无意2121的博客-CSDN博客

1 hybrid A* 算法创新点

1.1 搜索方式

A*算法：在二维网格中进行搜索，本质上就是把车辆简化为质点，并且移动方向是固定的八个方向（或四个方向），移动距离也是确定的。但这不符合实际的车辆运动学模型。

hybrid A*算法：引入航向角，将搜索变成在 $(x,y,\theta )$ 三个维度的空间中进行。符合车辆运动学模型。

第一幅图是 A*的搜索方式，第二幅图是hybrid A*的搜索方式。

1.2 车辆运动学模型

为了便于计算，hybrid A*采用车辆二自由度运动学模型（见上图），但是忽略了车辆加速度与前轮转角速度，于是经过简化的运动学模型如下

$\frac{\mathrm{d} }{\mathrm{d} t}\begin{bmatrix} x(t)\\ y(t)\\ \theta (t) \end{bmatrix}=\begin{bmatrix} v(t)*cos(\theta (t))\\ v(t)*sin(\theta (t)) \\ v(t)*tan(\phi (t))/L_{w} \end{bmatrix}$

所以这里的控制变量是速度 $v$ 和前轮转向角 $\phi$ ，根据状态变量 $x$ 横坐标， $y$ 纵坐标， $\theta$ 航向角的目前所处状态， $L_{w}$ 就是上图中的 b ，代表前后轴距离，通过控制变量的输入，在单位时间内，就能得到下一个时刻的状态。

1.3 reeds-shepp曲线的引入

reeds-shepp曲线证明，任何始末位姿的最短路径假如包含圆弧，一定在最小转弯半径时取到，也就是 $\phi =\begin{Bmatrix} -\phi _{max},0,\phi _{max} \end{Bmatrix}$ ,前轮转向角在这样的取值时路径最短，同时 $v=\begin{Bmatrix} -v_{max},v_{max} \end{Bmatrix}$ 时，车辆的耗时也更短，因此相当于每个状态能够通向另外六个状态，类似于经典A*，从一个状态从八个方向移动到另外八个状态，但是经典A*是状态采样，从一个状态跳到另一个状态，忽略了其中的运动学过程。但是hybrid A*是控制采样，更加符合运动学实际。当然真正运动时不一定一直按照最小转弯半径，这里为了简单，就先这样处理。下图是hybrid A*的六个子节点。

但是reeds-shepp曲线完全未考虑避障因素，但是由于reeds-shepp曲线比较简单易计算，所以构造速度非常快，使得先构造再检验是否碰撞成为可能，这里的碰撞指的是整条轨迹是否会与障碍物有交集，因此不像传统A*不考虑中间过程，所以需要均匀采样整段时间，进行碰撞检验。假如该段轨迹无碰撞则加入搜索树中，作为候选轨迹，如下图

图a：假如每次搜索都用reeds-shepp，由下图a可见这个搜索轨迹构成的树规模很庞大，节点很多将会造成极大的计算量。

图b：但间歇性得用reeds-shepp和传统A*去搜索，在每N个节点中选取一个计算Reed-Shepp曲线（这里的N随启发函数递减而减少，即越发靠近终点时，N越小）。由下图b可见搜索树规模小，节点少。但是在利用reeds-shepp曲线搜索时，若出现48种reeds-shepp曲线都会碰撞，也需要重新进行经典的A*搜索，这种混合的搜索使得搜索速度提升。

1.4 G的定义更丰富

在传统A*算法中，G是从起点到当前节点的路径消耗，由于一段直线前进的轨迹肯定优于反复前进倒退或扭曲的轨迹，因此我们对频繁切换速度 $v$ 和前轮转向角 $\phi$ 两个控制量的值这种行为进行惩罚，这样就能使得最后的轨迹更加合理。还有很多为了轨迹合理可以惩罚的地方，这其实就是一个评价函数的设计，具体可以参考【自动驾驶轨迹规划之最优控制】_无意2121的博客-CSDN博客但需要说明的是，如果是极端狭窄的泊车场景中，我们不得不采用复杂扭曲的轨迹。如下图

1.5 H的定义更丰富

按理来说，H函数应该是从当前节点位姿到终点节点位姿，同时满足避障以及车辆运动学约束的最短路径长度，这是真实路径，但这很难在还没采样搜索剩下的位置环境时就知道真实路径，所以hybrid A* 设计两个H的子函数，H1代表符合车辆运动学约束但忽略碰撞因素的最短路径，H2代表满足避障约束但是忽略车辆运动学约束的最短路径，H定义为H1与H2的最大值。

H1：当前节点离终点较近时，更应该关注车辆运动学约束，忽略障碍物的情况下，路径不依赖于任何在线场景信息，可以通过离线的方式采样枚举所有reeds-shepp曲线可能，提前将路径长度记录出来，在线调用该函数时只需索引、插值即可返回函数值。同时，这项启发函数的主要目的是为修剪传统A*搜索树的分支，保证最后能精准衔接终止位姿。。如下图

H2：当前节点离终点较远时，更应该关注避障行驶，防止陷入死胡同，利用传统A*的H进行计算。如下图

可见这样对启发函数的设计有利于提高搜索速度。

1.6 Voronoi势场函数

生成的路径必须与障碍物保持一定的距离，这也是最优轨迹的要求，由于传统的人工势场法的缺点是在狭窄路段构造了高势场，使得机器人或车辆无法通过，因此，构造Voronoi势场函数。首先，介绍一下Voronoi图。

Voronoi图，它是由一组由连接两邻点直线的垂直平分线组成的连续多边形组成。每个点有一个它的最近邻区域。

每个Cell中包含的都是距离当前Cell距离最近的所有点，因此Cell的边界就是距离种子点最远的点的集合。利用这个特性，将采样障碍物的边界当做种子点，那么Cell的边界就是远离所有障碍物的可行驶路径。效果见下图

当然不可能就照着cell的边界去运动，因为设计的出发点是为了通过较窄的地方。在路较宽时，没有必要，所以我们需要构建一个势场，能够让车辆或机器人趋于cell的边界去运动。势场函数如下

公式中 $d_{o}$ 、 $d_{v}$ 分别代表路径节点到最近的障碍物和最近的GVD（广义voronoi图）的长度， $\alpha$ 控制势场的衰减率， $d_{o}^{ max}$ 控制势场的影响范围。同时voronoi势场有如下的一些特点：

（1） $d_{o}\geq d_{o}^{ max}$ 时，场的值为0

（2） $p_{v}(x,y)$ 的区间在[0, 1] 时，且连续， $d_{o},d_{v}$ 不能同时为0

（3）在障碍物附近Voronoi势场的值达到最大值

（4）Voronoi势场的值在GVD的边上达到最小值

构建的Voronoi势场效果图如下（越黑，势场越大）

1.7 损失函数的设计

由于搜索树有许多候选轨迹，我们需要设计一个损失函数，来评价这些候选轨迹

按着原始论文损失函数各成分的顺序 $function=P_{vor}+P_{obs}+P_{cur}+P_{smo}$

$P_{vor}$ 基于之前定义的voronoi场函数，使路径远离障碍物

$P_{obs}$ 惩罚与障碍物的碰撞， $X_{i}$ 是当前节点的坐标， $O_{i}$ 是最近障碍物的坐标， $d_{max}$ 是离障碍物的最大距离，选择二次函数 $\sigma _{o}$ 作为惩罚项的目的是放大障碍物与节点越来越靠近的效果

$P_{cur}$ 相当于对路径的每个节点的瞬时曲率设置一个上限 $\kappa _{max}$ ，二次函数 $\sigma _{k }$ 作为惩罚项 $\Delta x_{i}=x_{i}-x_{i-1}$ ， $\Delta \phi _{i}=cos^{-1}\frac{x_{i}*x_{i+1}}{\left |x_{i+1} \right |*\left |x_{i+1} \right |}$

$P_{smo}$ 作为光滑度项，计算每个节点之间位移向量的差值的平方。这一项将损失值赋给非均匀分布和方向变化的节点，以保证路径的平滑性

优化分为两个阶段： a. 对路径的顶点坐标进行非线性优化规划问题的建模（也就是上文的损失函数定义） b. 采用共轭梯度方法进行非参数化插值(具体请参考原始论文)

2 hybrid A* 算法实现

2.1 伪代码

2.2 hybrid A* 算法效果图

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/羊村懒王/article/detail/467569