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

1. 运动学模型线性化

之前讲解了车辆的运动学模型，这边以以后轴中心为车辆中心的单车运动学模型为例进行线性化。

以后轴中心为车辆中心的单车运动学模型如下：

$$\{\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}\text{\hspace{1em}(1)}$$

选取状态量为 $\mathbf{\chi }=[x,y,\psi {]}^T$ ，控制量为 $\mathbf{u}=[v,\delta {]}^T$ ，则对于参考轨迹的任意一个参考点，用 $r$ 表示，上式可以改写为:
$$\dot{\mathbf{\chi }}=f(\mathbf{\chi },\mathbf{u})\Rightarrow {\dot{\mathbf{\chi }}}_r=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)\text{\hspace{1em}(2)}$$
其中 ${\chi }_r={\left[x_r,y_r,{\psi }_r\right]}^T,{\mathbf{u}}_r={\left[v_r,{\delta }_r\right]}^T$ 。对上式在参考点采用泰勒级数展开，并忽略高阶项：
$$\dot{\mathbf{\chi }}=f\left({\mathbf{\chi }}_r,{\mathbf{u}}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{\chi }}\left(\mathbf{\chi }-{\mathbf{\chi }}_r\right)+\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{u}}\left(\mathbf{u}-{\mathbf{u}}_r\right)\text{\hspace{1em}(3)}$$

对 $\frac{\partial f(\chi ,\mathbf{u})}{\partial \chi }$ 和$\frac{\partial f(\chi ,\mathbf{u})}{\partial \mathbf{u}}$ 求雅克比矩阵，有：
$$\begin{array}{rl}\frac{\partial f(\mathbf{\chi },\mathbf{u})}{\partial \mathbf{\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 {\phi }_r\\ 0& 0& v_r\cos {\phi }_r\\ 0& 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(4)}$$
$$\begin{array}{r}\frac{\partial f(\mathbf{\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{array}\text{\hspace{1em}(5)}$$

则状态量误差的变化量 $\dot{\tilde{\chi }}$ 表示为 :
$$\begin{array}{rl}\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{array}\text{\hspace{1em}(6)}$$

上式表明状态误差量可以构成线性状态空间。

2. 线性模型离散化

对等式(6)进行前向欧拉离散化，得到
$$\dot{\tilde{\chi }}=\frac{\tilde{\chi }(k+1)-\tilde{\chi }(k)}{T}=A\tilde{\chi }(k)+B\tilde{\mathbf{u}}(k)\text{\hspace{1em}(7)}$$
整理后，得到：
$$\begin{array}{rl}\tilde{\chi }(k+1)& =(TA+I)\tilde{\chi }(k)+TB\tilde{\mathbf{u}}(k)\\ & =\left[\begin{array}{ccc}1& 0& -T{v}_r\sin {\psi }_r\\ 0& 1& T{v}_r\cos {\psi }_r\\ 0& 0& 1\end{array}\right]\tilde{\chi }(k)+\left[\begin{array}{cc}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{array}\text{\hspace{1em}(8)}$$

式中，$T$为采样步长，$I$为单位矩阵，维度与矩阵$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