• 1. 运动学模型线性化
• 2. 线性模型离散化
• 3. python实现

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.

χ ˙ = 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 ( χ 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 ( χ , u ) ∂ χ \frac{\partial f(\chi, \mathbf{u})}{\partial \chi} ∂ f ( χ , u ) ∂ u \frac{\partial f(\chi, \mathbf{u}) }{\partial \mathbf{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 ) ∂ 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}

χ ~ ˙ = [ 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}

2. 线性模型离散化

χ ~ ˙ = χ ~ ( 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)

χ ~ ( 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 \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}

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