机器人纯跟踪算法 Base Pure Pursuit

前言

纯跟踪算法是一种路径跟踪算法，他通过改变角速度指令将机器人当前位置移动到前面的某个路径点，并不断的跟随路径前进。适用于很多机器人运动学模型，包括阿克曼和全向轮。
本文简单分析求解运动学之后的纯跟踪算法。

原理介绍

如果把机器人当做驴，上图形象的解释了纯跟踪算法的原理，纯跟踪算法需要一直更新“胡萝卜”的位置，让机器人朝着路径的方向前进。

算法

纯跟踪算法的步骤是

1. 定位当前机器人的位置。
2. 找到路径中匹配的目标点。
3. 改变机器人移动的角度。

如何找到匹配点

为了解决这个问题，我们需要先定义路径，我们是在什么样的路径里找到匹配点？

路径

描述一个路径一般是用一些离散的点，如:`(x,y) 。一般而言机器人的移动轨迹是连续的，我们也应该使在路径中找到的匹配点连续， 在算法里我们将这些离散的路径点相连接，变成连续的路径。

前瞻距离

和上图中人拿着杆子吸引机器人前进一样，我们的算法中也需要一个一定长度的杆子，在此我们定义“杆子”为前瞻距离。
前瞻距离的选取应该与机器人移动的速度相关，太大或太小都不行。如下图就是太长和太短的效果。

匹配点

当我们定义完路径和前瞻距离，接下来就应该解决匹配点。这个问题本质就是直线与圆的交点。
具体而言为路径中连续的两点连接成一直线，而前瞻距离绕机器人当前位置一圈为圆。求这两几何的交点。

圆与直线交点求解

这里可以用公式法或者向量法求解交点，公式法可能会遇到斜率为0的情况，需要分类讨论，在这里我介绍向量法，只需要考虑一种情况。
已知，路径点分别为(x1,y1),(x2,y2) 当前机器人位置为(currentX,currentY),前瞻距离为l。
为了简化计算，我们将当前所有的路径点减去机器人的坐标，算出来的交点再加回来即可。
则路径两点变为(x1-currentX,y1-currentY)和(x2-currentXmy2-currentY)
圆的方程为: $x^2+y^2=l^2$
直线的方程为: $(y_1-y_2)x-(x_1-x_2)y+x_1{y}_2-x_2{y}_1=0$ （化为一般式不用考虑斜率为0的情况）
直线到点的距离公式为:

我们可求得此路径两点所成的直线到圆心(0,0)的距离d,
若d>l则没有交点
若d=l则有且仅有一个交点
若d>l则有两个交点
下面用向量法求当有交点的时候如何得到交点。

记AB为当前两路径点，其方向向量为$\vec{a}=(x_a,y_a)$
则与AB垂直的OC 方向向量为$\vec{b}=(-y_a,x_a)$
可求得$\overrightarrow{OC}=d\ast \vec{b}$

$\left|\overrightarrow{CE}\right|=\sqrt[]{l^2-d^2}$
CE方向向量为:$\frac{\overrightarrow{AB}}{|\overrightarrow{AB}|}$
则:$\overrightarrow{CE}=\frac{\sqrt[]{l^2-d^2}}{|\overrightarrow{AB}|}\overrightarrow{AB}$
$\overrightarrow{OE}=\overrightarrow{OC}+\overrightarrow{CE}$
同理可得$\overrightarrow{OF}$
得到的向量加上机器人的位置坐标即可得交点。

只取路径间的解

这种情况虽然直线与圆有两个交点，即有两个匹配点，但在算法中，我们只取在路径间线段的点。

• 情况一：无交点
  

• 情况二：无交点
  

• 情况三：一交点
  

• 情况四：两交点
  

匹配点的判断

这种情况下，两条线段我们都能寻得解，那哪个点跟合适？
很明显，我们需要参考机器人当前的位置，如果 机器人到路径点1的距离小于匹配点到路径点1的距离，则我们跟应该去寻找下一个点。
所有在我们的遍历循环中，找到匹配点之后，应该再加入当前位置和匹配点、路径点的判断，通过这个判断才能知道是否是真的匹配点。

机器人控制

不同的机器人运动学在这一步可能会有所不同，我在此举的是舵轮，需要输入速度方向和底盘角度，在此我输入的是速度方向和底盘角度
$x_{currrent}-x_{match}$，大小为定值1。

效果:

黄色为前瞻距离，红色小球为匹配点

舵轮纯跟踪 ROS2 Control Gazebo 仿真

引用

[1]: base pure pursuit
[2]: mathworks pure puresuit controller
[3]: autoware
[4]: pure pursuit papaer