1. 汽车运动学模型
1. 运动学模型
在后轴处,即
(
x
r
,
y
r
)
(x_r,y_r)
(xr,yr)处,速度为
v
r
=
x
˙
r
c
o
s
(
φ
)
+
y
˙
r
s
i
n
(
φ
)
v_r = \dot{x}_r cos(\varphi)+\dot{y}_rsin(\varphi)
vr=x˙rcos(φ)+y˙rsin(φ)
在沿车横向的约束条件为:
{
x
˙
f
s
i
n
(
φ
+
δ
f
)
−
y
˙
f
c
o
s
(
φ
+
δ
f
)
x
˙
r
s
i
n
(
φ
)
−
y
˙
r
c
o
s
(
φ
)
\left\{ \begin{aligned} &\dot{x}_fsin(\varphi+\delta_f)-\dot{y}_f cos(\varphi+\delta_f) \\ &\dot{x}_rsin(\varphi)-\dot{y}_rcos(\varphi) \end{aligned} \right.
{x˙fsin(φ+δf)−y˙fcos(φ+δf)x˙rsin(φ)−y˙rcos(φ)
并且x方向和y方向速度与车纵向速度的关系为:
{
x
˙
r
=
v
r
c
o
s
(
φ
)
y
˙
r
=
v
r
s
i
n
(
φ
)
\left\{ \begin{aligned} &\dot{x}_r=v_rcos(\varphi)\\ &\dot{y}_r=v_rsin(\varphi) \end{aligned} \right.
{x˙r=vrcos(φ)y˙r=vrsin(φ)
前后轮的关系为:
{
x
˙
f
=
x
r
+
l
c
o
s
(
φ
)
y
˙
f
=
y
r
+
l
s
i
n
(
φ
)
\left\{ \begin{aligned} &\dot{x}_f=x_r+lcos(\varphi)\\ &\dot{y}_f=y_r+lsin(\varphi) \end{aligned} \right.
{x˙f=xr+lcos(φ)y˙f=yr+lsin(φ)
对上述关系求导得:
{
x
˙
f
=
v
r
c
o
s
(
φ
)
−
ω
l
s
i
n
(
φ
)
y
˙
f
=
v
r
s
i
n
(
φ
)
+
ω
l
c
o
s
(
φ
)
\left\{ \begin{aligned} &\dot{x}_f=v_rcos(\varphi)-\omega lsin(\varphi)\\ &\dot{y}_f=v_rsin(\varphi)+\omega lcos(\varphi) \end{aligned} \right.
{x˙f=vrcos(φ)−ωlsin(φ)y˙f=vrsin(φ)+ωlcos(φ)
联立上面速度关系可得到:
s
i
n
(
φ
+
δ
f
)
c
o
s
(
φ
+
δ
f
)
=
v
r
s
i
n
(
φ
+
ω
l
c
o
s
(
φ
)
)
v
r
c
o
s
(
φ
)
−
ω
l
s
i
n
(
φ
)
\frac{sin(\varphi+\delta_f)}{cos(\varphi+\delta_f)}=\frac{v_rsin(\varphi+\omega lcos(\varphi))}{v_rcos(\varphi)-\omega lsin(\varphi)}
cos(φ+δf)sin(φ+δf)=vrcos(φ)−ωlsin(φ)vrsin(φ+ωlcos(φ))
对上式展开,即得到:
ω
=
v
r
l
t
a
n
(
δ
f
)
\omega = \frac{v_r}{l}tan(\delta_f)
ω=lvrtan(δf)
即,我们得到如下非线性运动学模型:
[
x
˙
r
y
˙
r
φ
˙
]
=
[
c
o
s
(
φ
)
s
i
n
(
φ
)
t
a
n
(
δ
f
)
l
]
v
r
{ \left[ \begin{array}{ccc} \dot{x}_r\\ \dot{y}_r\\ \dot{\varphi} \end{array} \right ]}={ \left[ \begin{array}{ccc} cos(\varphi)\\ sin(\varphi)\\ \frac{tan(\delta_f)}{l} \end{array} \right ]}v_r
⎣⎡x˙ry˙rφ˙⎦⎤=⎣⎡cos(φ)sin(φ)ltan(δf)⎦⎤vr
其中,状态变量为 :
[
x
r
y
r
φ
]
T
{ \left[\begin{array}{ccc} x_r & y_r &\varphi \end{array} \right ]}^{T}
[xryrφ]T
控制量为
v
r
和
δ
f
v_r和\delta_f
vr和δf
2. 非线性模型线性化
利用泰勒展开:
设参考系统为:
ξ
˙
r
=
f
(
ξ
r
,
u
r
)
\dot{\xi}_r=f(\xi_r,u_r)
ξ˙r=f(ξr,ur)
当前系统在参考系统点处展开得:
ξ
˙
=
f
(
ξ
r
,
u
r
)
+
∂
f
∂
ξ
∣
ξ
=
ξ
r
,
u
=
u
r
(
ξ
−
ξ
r
)
+
∂
f
∂
u
∣
ξ
=
ξ
r
,
u
=
u
r
(
u
−
u
r
)
\dot{\xi} = f(\xi_r,u_r)+\frac{\partial f}{\partial \xi}|_{\xi=\xi_r,u=u_r}(\xi-\xi_r)+\frac{\partial f}{\partial u}|_{\xi=\xi_r,u=u_r}(u-u_r)
ξ˙=f(ξr,ur)+∂ξ∂f∣ξ=ξr,u=ur(ξ−ξr)+∂u∂f∣ξ=ξr,u=ur(u−ur)
即我们对非线性模型进行一阶泰勒展开并离散化得到:
ξ
~
k
i
n
(
k
+
1
)
=
A
k
i
n
(
k
)
ξ
~
k
i
n
(
k
)
+
B
k
i
n
(
k
)
u
~
k
i
n
(
k
)
\widetilde{\xi}_{kin}(k+1) = A_{kin}(k)\widetilde{\xi}_{kin}(k)+B_{kin}(k)\widetilde{u}_{kin}(k)
ξ
kin(k+1)=Akin(k)ξ
kin(k)+Bkin(k)u
kin(k)
其中
ξ ~ k i n ( k ) = [ x − x r y − y r φ − φ r ] A k i n = [ 1 0 − v r s i n ( φ r ) T 0 1 v r c o s ( φ r ) T 0 0 1 ] B k i n = [ c o s ( φ r ) T 0 s i n ( φ r ) T 0 t a n ( δ f ) T l v r T l c o s 2 ( δ f ) ] \widetilde{\xi}_{kin}(k)= { \left[ \begin{array}{ccc} x-x_r\\ y-y_r\\ \varphi-\varphi_r \end{array} \right ]} A_{kin}= { \left[ \begin{array}{ccc} 1 & 0 & -v_rsin(\varphi_r)T\\ 0 & 1 & v_rcos(\varphi_r)T\\ 0 & 0 & 1 \end{array} \right ] } B_{kin}= { \left[ \begin{array}{ccc} cos(\varphi_r)T & 0 \\ sin(\varphi_r)T & 0\\ \frac{tan(\delta_f)T}{l} & \frac{v_rT}{lcos^{2}(\delta_f)} \end{array} \right ] } ξ kin(k)=⎣⎡x−xry−yrφ−φr⎦⎤Akin=⎣⎡100010−vrsin(φr)Tvrcos(φr)T1⎦⎤Bkin=⎣⎡cos(φr)Tsin(φr)Tltan(δf)T00lcos2(δf)vrT⎦⎤