版权声明:本文为博主原创文章,未经博主允许不得转载。
源码地址:https://gitlab.com/autowarefoundation/autoware.ai/autoware/wikis/Source-Build
如有错误或建议,欢迎提出。
在Autoware的mpc源码中使用了三种车辆模型,其中车辆动力学误差模型可参考此博文https://blog.csdn.net/u013914471/article/details/83018664
因此本文主要对两种运动学模型进行介绍。
- 1车辆运动学自行车误差模型(no delay)
首先根据自行车运动学模型如下如所示:
其中 δ \delta δ为车辆转向角,R为转弯半径,L为车辆轴距。则他们的关系式如下式所示:
t a n ( δ ) = L / R tan(\delta) = L/R tan(δ)=L/R
之后定义e为横向误差,
e
˙
\dot{e}
e˙为横向误差率,th为航向角误差,
t
h
˙
\dot{th}
th˙为航向角误差率,v为车速,
δ
r
\delta_r
δr为参考转向角。
则:
e
˙
=
v
∗
sin
(
t
h
)
(
1
)
\dot{e} = v*\sin (th) (1)
e˙=v∗sin(th)(1)
t
h
˙
=
v
/
L
∗
tan
(
δ
)
−
v
/
L
∗
tan
(
δ
r
)
(
2
)
\dot{th} = v/L*\tan (\delta) - v/L*\tan (\delta_r)(2)
th˙=v/L∗tan(δ)−v/L∗tan(δr)(2)
其中式(2)可根据小角度理论以及三角函数关系化简成如下形式:
t
h
˙
=
v
/
L
∗
δ
/
cos
2
(
δ
r
)
−
v
/
L
∗
δ
r
/
cos
2
(
δ
r
)
(
3
)
\dot{th} = v/L*\delta/\cos^2(\delta_r) - v/L*\delta_r/\cos^2(\delta_r)(3)
th˙=v/L∗δ/cos2(δr)−v/L∗δr/cos2(δr)(3)
则根据式(1)和式(3)可得到车辆运动学自行车误差模型如下所示:
[
e
˙
t
h
˙
]
=
[
0
v
0
0
]
[
e
t
h
]
+
[
0
v
/
L
/
cos
2
(
δ
r
)
]
δ
+
[
0
−
v
∗
δ
r
/
L
/
cos
2
(
δ
r
)
]
\begin{bmatrix}\dot{e}\\ \dot{th}\end{bmatrix}=\begin{bmatrix}0& v\\ 0& 0\end{bmatrix}\begin{bmatrix}e\\ th\end{bmatrix} +\begin{bmatrix}0\\ v/L/\cos^2(\delta_r)\end{bmatrix}\delta+\begin{bmatrix}0\\ -v*\delta_r/L/\cos^2(\delta_r) \end{bmatrix}
[e˙th˙]=[00v0][eth]+[0v/L/cos2(δr)]δ+[0−v∗δr/L/cos2(δr)]
其中,如果车辆行驶在参考路径上,则
δ
r
=
δ
\delta_r = \delta
δr=δ。
- 2车辆运动学自行车误差模型(考虑转向角变化率)
第二种运动学模型在第一种模型的基础上考虑了转向角的变化率。定义t为循环周期, δ ˙ \dot{\delta} δ˙为转向角变化率,则:
δ ˙ = Δ δ / t \dot{\delta} = \Delta\delta/t δ˙=Δδ/t
结合第一种运动学模型可得:
[ e ˙ t h ˙ δ ˙ ] = [ 0 v 0 0 0 v / L / cos 2 ( δ r ) 0 0 − 1 / t ] [ e t h δ ] + [ 0 0 1 / t ] δ d + [ 0 − v ∗ δ r / L / cos 2 ( δ r ) 0 ] \begin{bmatrix}\dot{e}\\ \dot{th}\\ \dot{\delta}\end{bmatrix}=\begin{bmatrix}0 & v & 0\\ 0 & 0 & v/L/\cos^2(\delta_r)\\ 0 &0 & -1/t\end{bmatrix}\begin{bmatrix}e\\ th\\ \delta\end{bmatrix}+\begin{bmatrix}0\\ 0\\ 1/t\end{bmatrix}\delta_d+\begin{bmatrix}0\\ -v*\delta_r/L/\cos^2(\delta_r)\\ 0 \end{bmatrix} ⎣⎡e˙th˙δ˙⎦⎤=⎣⎡000v000v/L/cos2(δr)−1/t⎦⎤⎣⎡ethδ⎦⎤+⎣⎡001/t⎦⎤δd+⎣⎡0−v∗δr/L/cos2(δr)0⎦⎤
其中, δ d \delta_d δd为模型的输入——目标转向角度。