Pure Pursuit（纯追踪算法）ROS实践_ros pure puresuit

作者：菜鸟追梦旅行 | 2024-05-24 18:38:34

踩

ros pure puresuit

Pure Pursuit（纯追踪算法）ROS实践

自主移动机器人，或者说无人驾驶车辆，一般有三大子系统

首先感性的去认识一下纯追踪算法，简单理解就是，无人车关注轨迹中某个点目标goal，然后去追踪它。

在无人地面载具（UGV）场景下，这里的goal是Planning子系统所规划轨迹上的一些点，称为路点（Way Points）。如果轨迹是连续的，就在轨迹上采样多个点。如果轨迹是离散的，那么就可以直接使用。

自行车模型

上图是一个自行车模型，接下来简单说明是怎么绘制的。车辆坐标原点在后轴中心，如果转向角δ确定以后，车辆会以某个圆心做一个转圈动作，这个圆的半径就是R。那么pure pursuit算法就是一个横向控制器，通过计算转向角δ来控制车辆，使得车辆后轴中心能沿着轨迹移动。

各变量含义如下：

\begin{aligned} δ ： 前 轮 的 转 向 角 （ 度 ） \\ L ： 轴 距 （ 米 ） \\ R ： 给 定 转 向 角 后 ， 车 后 轴 中 心 遵 循 的 圆 的 半 径 \end{aligned}

$\begin{aligned} & \delta：前轮的转向角（度）\\ & L：轴距（米）\\ & R：给定转向角后，车后轴中心遵循的圆的半径 \end{aligned}$

δ ： 前 轮 的 转 向 角 （ 度 ） L ： 轴 距 （ 米 ） R ： 给 定 转 向 角 后 ， 车 后 轴 中 心 遵 循 的 圆 的 半 径

根据图中关系可得

tan(\delta)= \frac{L}{R} \tag{1}

（1）式的δ是我们要求的转向角，而未知量式R。由于曲率k与半径R呈倒数关系可将（1）式改写为

\delta = tan^{-1}(kL) \tag{2}

这时求解δ就成为了，解等式右边式子，其中L已知，是车辆的轴距，未知量是曲率k。接下来我们就来求解这个k。

pure pursuit算法几何模型

上图便是pure pursuit算法的几何图形，简单说明一下怎么绘制的。其中点g是我们的目标路点，我们要确定转向角δ（注意，不是图中的α），使后轴中心遵循的圆弧能经过g。再次强调，我们要求是圆弧的半径R，或者说曲率k，上图中的未知量只有R

各变量含义如下

\begin{aligned} (g_{x}, g_{y}) ： 目 标 路 点 g 在 车 辆 坐 标 系 中 的 位 置 （ 即 G P S 路 径 中 采 样 的 轨 迹 点 ） 。 \\ α ： 车 后 轴 与 目 标 路 点 g 连 成 向 量 与 车 偏 航 角 夹 角 。 \\ l_{d} ： 前 视 距 离 。 \end{aligned}

$\begin{aligned} & (g_x,g_y)：目标路点g在车辆坐标系中的位置（即GPS路径中采样的轨迹点）。\\ & α：车后轴与目标路点g连成向量与车偏航角夹角。\\ & l_d：前视距离。 \end{aligned}$

(g_{x}, g_{y}) ： 目 标 路 点 g 在 车 辆 坐 标 系 中 的 位 置 （ 即 G P S 路 径 中 采 样 的 轨 迹 点 ） 。 α ： 车 后 轴 与 目 标 路 点 g 连 成 向 量 与 车 偏 航 角 夹 角 。 l_{d} ： 前 视 距 离 。

接下来推导怎么求曲率k
在这里插入图片描述

考虑时间t，可以将（2）式表示为
$\delta(t) = tan^{-1}(k(t)L) = tan^{-1}(\frac{2Lsin(\alpha(t))}{l_d}) \tag{6}$
至此，pure pursuit算法就变成了每一个时刻求解α。

实验平台使用的GPS+惯导套件能提供8位小数精度的定位和偏航角。实验流程是先记录下人工控制移动的GPS轨迹，然后让无人车利用pure pursuit算法去跟随该轨迹移动。

在实现过程中有几个问题需要注意：

根据两个GPS坐标计算距离

如何计算两个GPS点的距离，因为原始算法是以车辆坐标系（笛卡尔坐标系，单位是米）定位每个点的，所以可以直接用欧几里得距离公式求距离，而GPS坐标不是以车辆坐标系定位点，要用到专门的算法求距离。

我们要找到目标路点g，算法过程如下：

（1）找到轨迹上离车后轴中心最近的路点o。

（2）以o为起始位置，遍历之后的路点，将相邻两个路点比如第i，i+1两个点的距离累加到L，直到累加距离L大于前视距离Ld，即找到目标路点g。

刚开始我尝试用近似的方法求解，但是因为距离较小，近似求解误差很大，需要做到很精确的算法，代码网上能查到就不放了。
根据两个GPS坐标计算航向角

计算α角时需要计算从车当前后轴中心坐标到目标路点g的偏航角（相对真北的角度）。同上面的问题一样也是因为原始代码是用的车辆坐标系（笛卡尔坐标系），可以用三角函数求得偏航角。而GPS坐标求解不能用三角函数求解，这时也需要求精确的值，代码网上能查到就不放了。
弧度与角度的转换

数学库里面三角函数sin，cos，tan，atan等函数都是使用的弧度计算，在GPS计算和车辆控制都是用角度计算，在实现pure pursuit算法步骤中要注意弧度和角度的转换。

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