- 1. 运动学模型线性化
- 2. 线性模型离散化
- 3. python实现
之前讲解了车辆的运动学模型，这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。
以后轴中心为车辆中心的单车运动学模型如下：
{
x
˙
=
v
cos
(
ψ
)
=
f
1
y
˙
=
v
sin
(
ψ
)
=
f
2
ψ
˙
=
v
L
tan
δ
f
=
f
3
(1)
tag{1} left{begin{array}{l} dot{x}=v cos (psi) =f_1\ dot{y}=v sin (psi) =f_2\ dot{psi}=frac{v}{L}tan{delta_f}=f_3 end{array}right.
⎩⎨⎧x˙=vcos(ψ)=f1y˙=vsin(ψ)=f2ψ˙=Lvtanδf=f3(1)
选取状态量为
χ
=
[
x
,
y
,
ψ
]
T
boldsymbol{chi}=[x, y, psi]^{T}
χ=[x,y,ψ]T ，控制量为
u
=
[
v
,
δ
]
T
mathbf{u}=[v, delta]^{T}
u=[v,δ]T ，则对于参考轨迹的任意一个参考点，用
r
r
r 表示，上式可以改写为:
χ
˙
=
f
(
χ
,
u
)
⇒
χ
˙
r
=
f
(
χ
r
,
u
r
)
(2)
tag{2} dot{boldsymbol{chi}}=f(boldsymbol{chi}, mathbf{u}) Rightarrow dot{boldsymbol{chi}}_{r}=fleft(boldsymbol{chi}_{r}, mathbf{u}_{r}right)
χ˙=f(χ,u)⇒χ˙r=f(χr,ur)(2)
其中
χ
r
=
[
x
r
,
y
r
,
ψ
r
]
T
,
u
r
=
[
v
r
,
δ
r
]
T
chi_{r}=left[x_{r}, y_{r}, psi_{r}right]^{T}, mathbf{u}_{r}=left[v_{r}, delta_{r}right]^{T}
χr=[xr,yr,ψr]T,ur=[vr,δr]T 。对上式在参考点采用泰勒级数展开，并忽略高阶项：
χ
˙
=
f
(
χ
r
,
u
r
)
+
∂
f
(
χ
,
u
)
∂
χ
(
χ
−
χ
r
)
+
∂
f
(
χ
,
u
)
∂
u
(
u
−
u
r
)
(3)
tag{3} dot{boldsymbol{chi}}=fleft(boldsymbol{chi}_{r}, mathbf{u}_{r}right)+frac{partial f(boldsymbol{chi}, mathbf{u})}{partial boldsymbol{chi}}left(boldsymbol{chi}-boldsymbol{chi}_{r}right)+frac{partial f(boldsymbol{chi}, mathbf{u})}{partial mathbf{u}}left(mathbf{u}-mathbf{u}_{r}right)
χ˙=f(χr,ur)+∂χ∂f(χ,u)(χ−χr)+∂u∂f(χ,u)(u−ur)(3)
对
∂
f
(
χ
,
u
)
∂
χ
frac{partial f(chi, mathbf{u})}{partial chi}
∂χ∂f(χ,u) 和
∂
f
(
χ
,
u
)
∂
u
frac{partial f(chi, mathbf{u}) }{partial mathbf{u}}
∂u∂f(χ,u) 求雅克比矩阵，有：
∂
f
(
χ
,
u
)
∂
χ
=
[
∂
f
1
∂
x
∂
f
1
∂
y
∂
f
1
∂
ψ
∂
f
2
∂
x
∂
f
2
∂
y
∂
f
2
∂
ψ
∂
f
3
∂
x
∂
f
3
∂
y
∂
f
3
∂
ψ
]
=
[
0
0
−
v
r
sin
φ
r
0
0
v
r
cos
φ
r
0
0
0
]
(4)
tag{4} begin{aligned} frac{partial f(boldsymbol{chi}, mathbf{u})}{partial boldsymbol{chi}}=& {left[begin{array}{lll} frac{partial f_{1}}{partial x} & frac{partial f_{1}}{partial y} & frac{partial f_{1}}{partial psi} \ frac{partial f_{2}}{partial x} & frac{partial f_{2}}{partial y} & frac{partial f_{2}}{partial psi} \ frac{partial f_{3}}{partial x} & frac{partial f_{3}}{partial y} & frac{partial f_{3}}{partial psi} end{array}right]=left[begin{array}{ccc} 0 & 0 & -v_{r} sin varphi_{r} \ 0 & 0 & v_{r} cos varphi_{r} \ 0 & 0 & 0 end{array}right] } \ end{aligned}
∂χ∂f(χ,u)=⎣⎢⎡∂x∂f1∂x∂f2∂x∂f3∂y∂f1∂y∂f2∂y∂f3∂ψ∂f1∂ψ∂f2∂ψ∂f3⎦⎥⎤=⎣⎡000000−vrsinφrvrcosφr0⎦⎤(4)
∂
f
(
χ
,
u
)
∂
u
=
[
∂
f
1
∂
v
∂
f
1
∂
δ
∂
f
2
∂
v
∂
f
2
∂
δ
∂
f
3
∂
v
∂
f
3
∂
δ
]
=
[
cos
ψ
r
0
sin
ψ
r
0
tan
ψ
r
l
v
r
l
cos
2
δ
r
]
(5)
tag{5} begin{aligned} frac{partial f(boldsymbol{chi}, mathbf{u})}{partial mathbf{u}}=left[begin{array}{ll} frac{partial f_{1}}{partial v} & frac{partial f_{1}}{partial delta} \ frac{partial f_{2}}{partial v} & frac{partial f_{2}}{partial delta} \ frac{partial f_{3}}{partial v} & frac{partial f_{3}}{partial delta} end{array}right]=left[begin{array}{lc} cos psi_{r} & 0 \ sin psi_{r} & 0 \ frac{tan psi_{r}}{l} & frac{v_{r}}{l cos ^{2} delta_{r}} end{array}right] end{aligned}
∂u∂f(χ,u)=⎣⎡∂v∂f1∂v∂f2∂v∂f3∂δ∂f1∂δ∂f2∂δ∂f3⎦⎤=⎣⎡cosψrsinψrltanψr00lcos2δrvr⎦⎤(5)
则状态量误差的变化量
χ
~
˙
dot{tilde{chi}}
χ~˙ 表示为 :
χ
~
˙
=
[
x
˙
−
x
˙
r
y
˙
−
y
˙
r
ψ
˙
−
ψ
˙
r
]
=
[
0
0
−
v
r
sin
ψ
r
0
0
v
r
cos
ψ
r
0
0
0
]
[
x
−
x
r
y
−
y
r
ψ
−
ψ
r
]
+
[
cos
ψ
r
0
sin
ψ
r
0
tan
ψ
r
l
v
r
l
cos
2
δ
r
]
[
v
−
v
r
δ
−
δ
r
]
=
A
χ
~
+
B
u
~
(6)
tag{6} begin{aligned} dot{tilde{chi}}&=left[begin{array}{c} dot{x}-dot{x}_{r} \ dot{y}-dot{y}_{r} \ dot{psi}-dot{psi}_{r} end{array}right]\&=left[begin{array}{ccc} 0 & 0 & -v_{r} sin psi_{r} \ 0 & 0 & v_{r} cos psi_{r} \ 0 & 0 & 0 end{array}right]left[begin{array}{c} x-x_{r} \ y-y_{r} \ psi-psi_{r} end{array}right]+left[begin{array}{cc} cos psi_{r} & 0 \ sin psi_{r} & 0 \ frac{tan psi_{r}}{l} & frac{v_{r}}{l cos ^{2} delta_{r}} end{array}right]left[begin{array}{l} v-v_{r} \ delta-delta_{r} end{array}right] \ &=A tilde{chi}+B tilde{mathbf{u}} end{aligned}
χ~˙=⎣⎡x˙−x˙ry˙−y˙rψ˙−ψ˙r⎦⎤=⎣⎡000000−vrsinψrvrcosψr0⎦⎤⎣⎡x−xry−yrψ−ψr⎦⎤+⎣⎡cosψrsinψrltanψr00lcos2δrvr⎦⎤[v−vrδ−δr]=Aχ~+Bu~(6)
上式表明状态误差量可以构成线性状态空间。
2. 线性模型离散化对等式(6)进行前向欧拉离散化，得到
χ
~
˙
=
χ
~
(
k
+
1
)
−
χ
~
(
k
)
T
=
A
χ
~
(
k
)
+
B
u
~
(
k
)
(7)
tag{7} dot{tilde{chi}}=frac{tilde{chi}(k+1)-tilde{chi}(k)}{T}=A tilde{chi}(k)+B tilde{mathbf{u}}(k)
χ~˙=Tχ~(k+1)−χ~(k)=Aχ~(k)+Bu~(k)(7)
整理后，得到：
χ
~
(
k
+
1
)
=
(
T
A
+
I
)
χ
~
(
k
)
+
T
B
u
~
(
k
)
=
[
1
0
−
T
v
r
sin
ψ
r
0
1
T
v
r
cos
ψ
r
0
0
1
]
χ
~
(
k
)
+
[
T
cos
ψ
r
0
T
sin
ψ
r
0
T
tan
ψ
r
L
v
r
T
L
cos
2
δ
r
]
u
~
(
k
)
=
A
~
χ
~
(
k
)
+
B
~
u
~
(
k
)
(8)
tag{8} begin{aligned} tilde{chi}(k+1)&=(T A+I) tilde{chi}(k)+T B tilde{mathbf{u}}(k)\ &=left[begin{array}{ccc} 1 & 0 & -T v_{r} sin psi_{r} \ 0 & 1 & Tv_{r} cos psi_{r} \ 0 & 0 & 1 end{array}right] tilde{chi}(k)+left[begin{array}{ccc} T cos psi_{r} & 0 \ T sin psi_{r} & 0 \ frac{Ttan psi_{r}}{L} & frac{v_{r}T}{L cos ^{2} delta_{r}} end{array}right] tilde{mathbf{u}}(k)\ &=tilde{A} tilde{chi}(k)+tilde{B} tilde{mathbf{u}}(k) end{aligned}
χ~(k+1)=(TA+I)χ~(k)+TBu~(k)=⎣⎡100010−TvrsinψrTvrcosψr1⎦⎤χ~(k)+⎣⎡TcosψrTsinψrLTtanψr00Lcos2δrvrT⎦⎤u~(k)=A~χ~(k)+B~u~(k)(8)
式中， T T T为采样步长， I I I为单位矩阵，维度与矩阵 A A A一致。
3. python实现import math
import numpy as np
class KinematicModel_3:
"""假设控制量为转向角delta_f和加速度a
"""
def __init__(self, x, y, psi, v, L, dt):
self.x = x
self.y = y
self.psi = psi
self.v = v
self.L = L
# 实现是离散的模型
self.dt = dt
def update_state(self, a, delta_f):
self.x = self.x+self.v*math.cos(self.psi)*self.dt
self.y = self.y+self.v*math.sin(self.psi)*self.dt
self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt
self.v = self.v+a*self.dt
def get_state(self):
return self.x, self.y, self.psi, self.v
def state_space(self, ref_delta, ref_yaw):
"""将模型离散化后的状态空间表达
Args:
delta (_type_): 参考输入
Returns:
_type_: _description_
"""
A = np.matrix([
[1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)],
[0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],
[0.0,0.0,1.0]])
B = np.matrix([
[self.dt*math.cos(ref_yaw), 0],
[self.dt*math.sin(ref_yaw), 0],
[self.dt*math.tan(ref_delta), self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
])
return A,B
