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(ψ)=f1​y˙​=vsin(ψ)=f2​ψ˙​=Lv​tanδ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​​​⎦⎥⎤​=⎣⎡​000​000​−vr​sinφr​vr​cosφr​0​⎦⎤​​(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ψr​sinψr​ltanψr​​​00lcos2δr​vr​​​⎦⎤​​(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˙r​y˙​−y˙​r​ψ˙​−ψ˙​r​​⎦⎤​=⎣⎡​000​000​−vr​sinψr​vr​cosψr​0​⎦⎤​⎣⎡​x−xr​y−yr​ψ−ψr​​⎦⎤​+⎣⎡​cosψr​sinψr​ltanψr​​​00lcos2δr​vr​​​⎦⎤​[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{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} χ~​(k+1)​=(TA+I)χ~​(k)+TBu~(k)=⎣⎡​100​010​−Tvr​sinψr​Tvr​cosψr​1​⎦⎤​χ~​(k)+⎣⎡​Tcosψr​Tsinψr​LTtanψr​​​00Lcos2δr​vr​T​​⎦⎤​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