【自动驾驶】运动学模型的线性离散化

文章目录

  • 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. 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)+uf(χ,u)(uur)(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}} uf(χ,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)=xf1xf2xf3yf1yf2yf3ψf1ψf2ψf3=000000vrsinφ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} uf(χ,u)=vf1vf2vf3δ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=000000vrsinψrvrcosψr0xxryyrψψr+cosψrsinψrltanψr00lcos2δrvr[vvrδδ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)=100010TvrsinψrTvrcosψr1χ~(k)+TcosψrTsinψrLTtanψr00Lcos2δrvrTu~(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


你可能感兴趣的