目前手头有一辆小拖车,需要用到pure pursuit算法,故拜读了大神的博客后,整理了此篇文章,首先奉上大神链接
Pure Pursuit算法最早是用于差速轮底盘小车,现在经过适配用于阿克曼底盘运动模型车辆(阿克曼运动模型这里就不多说了)。首先上个图,
PP算法以车辆后轴中心为切点,车辆纵向车身为切线,通过控制前轮转角,使车辆按照一段圆弧经过目标路点,
l
d
l_d
ld表示车辆当前后轴中心到目标点的距离,
α
\alpha
α表示当前车辆和目标路点的heading角度偏差。
根据正弦定理有,
l
d
s
i
n
(
2
α
)
=
R
s
i
n
(
π
2
−
α
)
\frac{l_d}{sin(2\alpha)} = \frac{R}{sin(\frac{\pi}{2}-\alpha)}
sin(2α)ld=sin(2π−α)R
化简后,有
l
d
s
i
n
α
=
2
R
\frac{l_d}{sin\alpha}=2R
sinαld=2R
也可以表示为
k
=
2
s
i
n
α
l
d
k=\frac{2sin\alpha}{l_d}
k=ld2sinα
式中,
k
=
1
/
R
k=1/R
k=1/R为阿克曼运动模型车辆行驶的圆弧,那么前轮转角
δ
\delta
δ的表达式为:
δ
=
arctan
(
k
L
)
\delta = \arctan(kL)
δ=arctan(kL)
L
L
L为车辆轴距,综上所述,有
δ
(
t
)
=
arctan
2
L
s
i
n
(
α
(
t
)
)
l
d
\delta(t)=\arctan\frac{2Lsin(\alpha(t))}{l_d}
δ(t)=arctanld2Lsin(α(t))
在
t
t
t时刻,知道车身和目标点之间的夹角
α
(
t
)
\alpha(t)
α(t)和目标路点之间的前视距离
l
d
l_d
ld的情况下,车辆轴距是固定值
L
L
L,可以用上式计算出当前的车辆前轮需要偏转的控制量
α
\alpha
α。
为了便于后面的计算,定义一个新的量
e
l
e_l
el,表示车辆当前位置和车辆目标点之间的横向误差,由此可得:
s
i
n
(
α
)
=
e
l
l
d
sin(\alpha)=\frac{e_l}{l_d}
sin(α)=ldel
圆弧的弧度计算可以表示为:
k
=
2
l
d
2
e
l
k=\frac{2}{l^2_d}e_l
k=ld22el
可以看出PP算法的本质就是类似一个横向的P控制器,系数
P
P
P为
2
l
d
2
\frac{2}{l^2_d}
ld22,这个
P
P
P系数受到参数
l
d
l_d
ld的影响很大,因此,引入了一个新的问题,如何选取前视距离是PP算法的关键,一般来说,Autoware中的PP算法就是使前视距离为当前车速乘以一个比例系数,在不同的时刻选取不同的前视距离。
PP算法其实比较适合用于差速轮小车的运动跟踪,在阿克曼模型中,用起来并不是很合适,尤其是在车速变化较大的情况,前视距离如何选取是个很大的问题,如果选取距离过短,会导致轨迹无法收敛,车辆横向控制发散,选取距离过长,又会导致车辆转角不足的问题。