全局路径由一系列路径点构成，这些路径点只要包含 空间位置信息即可，也可以包含 姿态信息，但是 不需要与时间相关，这些路径点被称为 全局路径点。
路径（Path）和轨迹（Trajectory）的区别就在于， 轨迹还包含了时间信息，轨迹点也是一种路径点，它在路径点的基础上加入了 时间约束。
目前的主流轨迹跟踪方法分为两类： 基于几何追踪的方法和基于模型预测的方法。
本节介绍一种广泛使用的 基于几何追踪的方法——纯追踪法（Pure Pursuit）。
1.自行车模型（汽车二自由度模型）
详见这篇文章无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型
注意点
Point1
自行车模型基于如下几个假设：
忽略车辆在垂直方向的运动，即假定车辆是一个二维平面上的运动物体。
假设车辆的结构就像自行车一样，即车辆的前面两个轮胎拥有一致的角度和转速，后面的两个轮胎也是如此，那么前后轮两个轮胎就可以用一个轮胎来描述。
假设车辆运动也与自行车一样，意味着前面的轮胎控制车辆的转角。
Point2
几何自行车模型
自行车模型实际上是对阿克曼转向几何的一个简化。它简化了前轮转向角与后轴将遵循的曲率之间的几何关系：
tan
(
δ
)
=
L
R
\tan (\delta ) = \frac{L}{R}
tan(δ)=RL δ表示前轮的转角，L为轴距（Wheelbase），R则为在给定的转向角下后轴遵循着的圆的半径。
这个公式能够在较低速度的场景下对车辆运动做估计。
2.纯追踪控制
详见这篇文章 无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪
注意点
Point1
从自行车模型出发纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角,使车辆可以沿着一条经过目标路点（goal point）的圆弧行驶： 
通过正弦定理求出曲率、偏转角与车轴之间的关系。
k
=
2
sin
α
l
d
k = \frac{{2\sin \alpha }}{{{l_d}}}
k=ld2sinα ld表示车辆当前位置到目标路点的距离， α表示目前车身姿态和目标路点的夹角
前轮的转向角δ的表达式为：
δ
=
arctan
(
k
L
)
\delta = \arctan (kL)
δ=arctan(kL) 纯追踪算法控制量的的最终表达式：
δ
(
t
)
=
arctan
(
2
L
sin
(
α
(
t
)
)
l
d
)
\delta (t) = \arctan (\frac{{2L\sin (\alpha (t))}}{{{l_d}}})
δ(t)=arctan(ld2Lsin(α(t)))
Point2
为了更好的理解纯追踪控制器的原理，我们定义一个新的量：el—— 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：
sin
(
α
)
=
e
l
l
d
\sin (\alpha ) = \frac{{{e_l}}}{{{l_d}}}
sin(α)=ldel 圆弧的弧度就可重写为：
k
=
2
l
d
2
e
l
k = \frac{2}{{l_d^2}}{e_l}
k=ld22el 考虑到本质是横向上的CTE，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为 2/(ld)2；
这个P控制器受到参数 ld（即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键；
通常来说，ld 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。
一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即
l
d
=
k
v
x
{l_d} = k{v_x}
ld=kvx，那么前轮的转角公式就变成了：
δ
(
t
)
=
arctan
(
2
L
sin
(
α
(
t
)
)
k
v
x
(
t
)
)
\delta (t) = \arctan (\frac{{2L\sin (\alpha (t))}}{{k{v_x}(t)}})
δ(t)=arctan(kvx(t)2Lsin(α(t))) 那么纯追踪控制器的调整就变成了调整系数k。
Point3
通常来说，会使用最大、最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡）。
驾驶员在实际驾驶过程中会观察车辆前方一定距离范围内的道路信息,以获得一定的预见性,提前对车辆进行调整,从而取得良好的控制效果。
在路径跟踪过程中,前视距离与车速的大小有直接的关系。当车速较高时,需要较大的前视距离;当车速较低时,较小的前视距离可以达到很好的跟踪精度.
3.相关代码
Pure Pursuit纯跟踪算法Python/Matlab算法实现
参考文献
[1] 纯追踪算法论文
[2] 孙怀江,杨静宇.关于纯追踪模型的讨论[J].计算机学报,2002(12):1445-1449.
[3] 孙木楠,孙怀江.纯追踪模型研究[J].机器人,2001(S1):612-615.DOI:10.13973/j.cnki.robot.2001.s1.010.
[4] 无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型
[5] 吕文杰,马戎,李岁劳,付维平.基于纯追踪模型的路径跟踪改进算法[J].测控技术,2011,30(07):93-96+100.DOI:10.19708/j.ckjs.2011.07.024.
[6] 无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪
声明
本人所有文章仅作为自己的学习记录，若有侵权，联系立删。
扫一扫
2470
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
