前言
纯跟踪算法是一种路径跟踪算法，他通过改变角速度指令将机器人当前位置移动到前面的某个路径点，并不断的跟随路径前进。适用于很多机器人运动学模型，包括阿克曼和全向轮。
本文简单分析求解运动学之后的纯跟踪算法。
原理介绍
如果把机器人当做驴，上图形象的解释了纯跟踪算法的原理，纯跟踪算法需要一直更新“胡萝卜”的位置，让机器人朝着路径的方向前进。
算法
纯跟踪算法的步骤是
- 定位当前机器人的位置。
- 找到路径中匹配的目标点。
- 改变机器人移动的角度。
如何找到匹配点
为了解决这个问题，我们需要先定义路径，我们是在什么样的路径里找到匹配点？
路径
描述一个路径一般是用一些离散的点，如:`(x,y) 。一般而言机器人的移动轨迹是连续的，我们也应该使在路径中找到的匹配点连续， 在算法里我们将这些离散的路径点相连接，变成连续的路径。
前瞻距离
和上图中人拿着杆子吸引机器人前进一样，我们的算法中也需要一个一定长度的杆子，在此我们定义“杆子”为前瞻距离。
前瞻距离的选取应该与机器人移动的速度相关，太大或太小都不行。如下图就是太长和太短的效果。
匹配点
当我们定义完路径和前瞻距离，接下来就应该解决匹配点。这个问题本质就是直线与圆的交点。
具体而言为路径中连续的两点连接成一直线，而前瞻距离绕机器人当前位置一圈为圆。求这两几何的交点。
圆与直线交点求解
这里可以用公式法或者向量法求解交点，公式法可能会遇到斜率为0的情况，需要分类讨论，在这里我介绍向量法，只需要考虑一种情况。
已知，路径点分别为(x1,y1),(x2,y2) 当前机器人位置为(currentX,currentY),前瞻距离为l。
为了简化计算，我们将当前所有的路径点减去机器人的坐标，算出来的交点再加回来即可。
则路径两点变为(x1-currentX,y1-currentY)和(x2-currentXmy2-currentY)
圆的方程为:
x
2
+
y
2
=
l
2
x^2+y^2 = l^2
x2+y2=l2
直线的方程为:
(
y
1
−
y
2
)
x
−
(
x
1
−
x
2
)
y
+
x
1
y
2
−
x
2
y
1
=
0
(y_1-y_2)x-(x_1-x_2)y+x_1y_2-x_2y_1=0
(y1−y2)x−(x1−x2)y+x1y2−x2y1=0 （化为一般式不用考虑斜率为0的情况）
直线到点的距离公式为:
我们可求得此路径两点所成的直线到圆心(0,0)的距离d,
若d>l则没有交点
若d=l则有且仅有一个交点
若d>l则有两个交点
下面用向量法求当有交点的时候如何得到交点。
记AB为当前两路径点，其方向向量为
a
⃗
=
(
x
a
,
y
a
)
\vec{a} = (x_a,y_a)
a=(xa,ya)
则与AB垂直的OC 方向向量为
b
⃗
=
(
−
y
a
,
x
a
)
\vec{b}=(-y_a,x_a)
b=(−ya,xa)
可求得
O
C
⃗
=
d
∗
b
⃗
\vec{OC} = d* \vec{b}
OC=d∗b
∣
C
E
⃗
∣
=
l
2
−
d
2
\left| \vec{CE} \right| = \sqrt[ ]{l^2 - d^2}
CE
=l2−d2
CE方向向量为:
A
B
⃗
∣
A
B
⃗
∣
\frac{\vec{AB}}{ \left| \vec{AB} \right| }
∣AB∣AB
则:
C
E
⃗
=
l
2
−
d
2
∣
A
B
⃗
∣
A
B
⃗
\vec{CE} = \frac{\sqrt[ ]{l^2 - d^2} }{ \left| \vec{AB} \right| } \vec{AB}
CE=∣AB∣l2−d2AB
O
E
⃗
=
O
C
⃗
+
C
E
⃗
\vec{OE} = \vec{OC} + \vec{CE}
OE=OC+CE
同理可得
O
F
⃗
\vec{OF}
OF
得到的向量加上机器人的位置坐标即可得交点。
只取路径间的解
这种情况虽然直线与圆有两个交点，即有两个匹配点，但在算法中，我们只取在路径间线段的点。
-
情况一：无交点
-
情况二：无交点
-
情况三：一交点
-
情况四：两交点
匹配点的判断
这种情况下，两条线段我们都能寻得解，那哪个点跟合适？
很明显，我们需要参考机器人当前的位置，如果 机器人到路径点1的距离小于匹配点到路径点1的距离，则我们跟应该去寻找下一个点。
所有在我们的遍历循环中，找到匹配点之后，应该再加入当前位置和匹配点、路径点的判断，通过这个判断才能知道是否是真的匹配点。
机器人控制
不同的机器人运动学在这一步可能会有所不同，我在此举的是舵轮，需要输入速度方向和底盘角度，在此我输入的是速度方向和底盘角度
x
c
u
r
r
r
e
n
t
−
x
m
a
t
c
h
{x_{currrent}} -x_{match}
xcurrrent−xmatch，大小为定值1。
效果:
黄色为前瞻距离，红色小球为匹配点
舵轮纯跟踪 ROS2 Control Gazebo 仿真文章来源：https://www.toymoban.com/news/detail-773561.html
引用
[1]: base pure pursuit
[2]: mathworks pure puresuit controller
[3]: autoware
[4]: pure pursuit papaer文章来源地址https://www.toymoban.com/news/detail-773561.html
到了这里，关于机器人纯跟踪算法 Base Pure Pursuit的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
