【自动驾驶】运动学模型的线性离散化(含python实现)
文章目录1. 运动学模型线性化2. 线性模型离散化3. python实现1. 运动学模型线性化之前讲解了车辆的运动学模型,这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。以后轴中心为车辆中心的单车运动学模型如下:{x˙=vcos(ψ)=f1y˙=vsin(ψ)=f2ψ˙=vLtanδf=f3(1)\tag{1}\left\{\begin{array}{l}\dot{x}=v \co
1. 运动学模型线性化
之前讲解了车辆的运动学模型,这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。
以后轴中心为车辆中心的单车运动学模型如下:
{
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}=f\left(\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}}=f\left(\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 \delta_{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 \delta_{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. 线性模型离散化
常用的线性模型离散化的方法包括(1)欧拉法;(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{T\tan\delta_{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.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
])
return A,B
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)