δ ( t ) = θ e ( t ) + a r c t a n ( k ∗ e f a ( t ) / v ( t ) ) \delta(t) = \theta_{e}(t)+arctan(k*e_{fa}(t)/v(t)) δ(t)=θe(t)+arctan(k∗efa(t)/v(t))
e
f
a
e_{fa}
efa为前轴中心到最近路径点
(
c
x
,
c
y
)
(c_x,c_y)
(cx,cy)的距离。
θ
e
\theta_e
θe表示车辆航向与最近路径点切线方向之间的夹角,即前轮期望转角。但由于前轮横向误差的存在,导致需要多转一个角度才能走到参考线上,因此这里定义多转的这个角度
δ
e
=
δ
−
θ
e
\delta_e = \delta - \theta_e
δe=δ−θe为前轮偏转误差的补偿部分。
δ
e
\delta_e
δe是为了补偿横向误差,因此应该和横向误差正相关,同时由于速度越快时,横向位移和前轮转角越大,因此也应该和速度成负相关,一种方法是:
δ
e
=
arctan
k
e
f
a
v
\delta_e=\arctan\frac{ke_{fa}}{v}
δe=arctanvkefa