【自动驾驶】动力学横向控制误差模型

参考资料

• 车辆模型-跟踪误差模型
• Vehicle Dynamics and Control

1. 基本概述

车辆横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建车辆动力学模型时，可以建立以相对于道路的方向和距离误差为状态变量的动力学模型。

假设——

• $e_y$​:车辆重心距车道中心线的距离；
• $e_{\psi }$​:偏航角误差；
• $V_x$:车辆纵向速度；
• ​$R$：车辆转弯半径，其中转弯半径 $R$足够大，以满足上述章节的小角度近似假设。

定义——

• 车身转过期望角度所需要的偏航角速度为
  $${\dot{\psi }}_{des}=\frac{V_x}{R}\text{\hspace{1em}(1)}$$

• 所需的横向加速度（即期望的向心加速度）为
  $$a_{des}=\frac{V_x^2}{R}=V_x{\dot{\psi }}_{des}\text{\hspace{1em}(2)}$$

• 车辆偏航角误差为

$$e_{\psi }=\psi -{\psi }_{des}\text{\hspace{1em}(3)}$$

• 车辆偏航角速度误差为
  $${\dot{e}}_{\psi }=\dot{\psi }-{\dot{\psi }}_{\text{des }}\text{\hspace{1em}(4)}$$

• 车辆偏航角加速度误差为
  $${\ddot{e}}_{\psi }=\ddot{\psi }-{\ddot{\psi }}_{\text{des }}\text{\hspace{1em}(5)}$$

• 车辆 横向加速度误差为
  $$\begin{array}{rl}{\ddot{e}}_y& =a_y-a_{des}\\ & =\left(\ddot{y}+V_x\dot{\psi }\right)-V_x{\dot{\psi }}_{\text{des }}\\ & =\ddot{y}+V_x\left(\dot{\psi }-{\dot{\psi }}_{des}\right)\end{array}\text{\hspace{1em}(6)}$$
  其中，$a_y$由两 种力共同作用产生 : 车辆延 $y$ 轴产生的惯性加速度 $\ddot{y}$ 和车辆绕旋转中心 $O$ 旋转产生的向心加速度 $a_c=\frac{{V_x}^2}{R}=V_x{\dot{\psi }}_{\circ }$

• 车辆 横向速度误差为

  当车辆纵向速度$V_x$恒定时， $y$ 轴方向的速度误差可以表示为
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\dot{y}+V_x\left(\psi -{\psi }_{des}\right)\text{\hspace{1em}(7)}$$
  当纵向速度$V_x$随着时间变化时，对等式(6)积分得
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\int \ddot{y}+V_x\left(\dot{\psi }-{\dot{\psi }}_{\text{des }}\right)\mathrm{d}t=\dot{y}+\int V_x\left(\dot{\psi }-{\dot{\psi }}_{\text{des }}\right)\mathrm{d}t\text{\hspace{1em}(8)}$$

  参考资料中的等式(8)为
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\dot{y}+\int V_x\left(\psi -{\psi }_{\text{des }}\right)\mathrm{d}t$$
  根据一步一步推导看的话书中的这个等式应该是有问题的，这里先贴在这，有懂的朋友也可以帮忙解惑一下~~

  这就使得模型非线性且时变，不利于控制系统的设计。因此这里假设纵向速度$V_x$恒定， 即当作一个线性时不变(LTI)模型。如果速度变化，就需要使用线性参变模型(LPV) 替代， 其纵向速度是一个随着时间变化的参数。

2. 误差动力学模型

将等式(6)(7)变换如下：

$$\begin{array}{rl}& \ddot{y}={\ddot{e}}_y+V_x{\dot{\psi }}_{des}-V_x\dot{\psi }\\ & \dot{y}={\dot{e}}_y-V_x{e}_{\psi }\end{array}\text{\hspace{1em}(9)}$$
根据车辆动力学模型中的等式(14)
$$\ddot{y}=-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\dot{y}-\left(V_x+\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\right)\dot{\psi }+\frac{2C_{\alpha f}}{m}\delta \text{\hspace{1em}(10)}$$

将等式(4)(9)代入等式(10)得

$$\begin{array}{rl}{\ddot{e}}_y+V_x{\dot{\psi }}_{des}-V_x\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)& =-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\left({\dot{e}}_y-V_x{e}_{\psi }\right)-\\ & \left(V_x+\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\right)\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)+\frac{2C_{\alpha f}}{m}\delta \end{array}\text{\hspace{1em}(11)}$$
对等式(11)提取 ${\ddot{e}}_y$​、${\dot{e}}_y$​、$e_y$​、$e_{\psi }$、${\dot{e}}_{\psi }$、${\dot{\psi }}_{des}$​和$\delta$项得

$$\begin{array}{c}{\ddot{e}}_y=\frac{-2C_{\alpha f}-2C_{\alpha r}}{m{V}_x}{\dot{e}}_y+\frac{2C_{\alpha f}+2C_{\alpha r}}{m}{e}_{\psi }+\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}{\dot{e}}_{\psi }\\ +\left(\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\right){\dot{\psi }}_{\text{des }}+\frac{2C_{\alpha f}}{m}\delta \end{array}\text{\hspace{1em}(12)}$$
整理成矩阵形式为

$$\begin{gathered} \frac{\mathrm{d}}{\mathrm{d}t}{\dot{e}}_y=\left[\begin{array}{llll}0& -\frac{2C_{\alpha f}+2C_{\alpha r}r}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\end{array}\right]\left[\begin{array}{c}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]+ \\ \left(\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\right){\dot{\psi }}_{des}+\frac{2C_{\alpha f}}{m}\delta \text{\hspace{1em}(13)} \end{gathered}$$

同理可得，根据车辆动力学模型中的等式(17)
$$\ddot{\psi }=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{v}_x}\dot{y}-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{v}_x}\dot{\psi }+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \text{\hspace{1em}(14)}$$

将等式(4)(5)(9)代入等式(14)，得
$$\begin{array}{r}{\ddot{e}}_{\psi }+{\ddot{\psi }}_{\text{des }}=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}\left({\dot{e}}_y-V_x{e}_{\psi }\right)-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \end{array}\text{\hspace{1em}(15)}$$

对等式(15)提取 ${\dot{e}}_y$​、$e_y$​、$e_{\psi }$、${\ddot{e}}_{\psi }$​、${\dot{e}}_{\psi }$、${\dot{\psi }}_{des}$​和$\delta$项得
$$\begin{array}{c}{\ddot{e}}_{\psi }=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}{\dot{e}}_y+\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}{e}_{\psi }\\ -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{e}}_{\psi }-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{\psi }}_{des}+\frac{2l_f{C}_{\alpha f}}{I_z}\delta -{\ddot{\psi }}_{des}\end{array}\text{\hspace{1em}(16)}$$
由于上述假设为线性时不变系统 $(LT1)\left({\dot{V}}_x=0\right)$ ，故 ${\ddot{\psi }}_{des}=\frac{{\dot{V}}_x}{R}=0$ ，将上述等式整理成矩阵形式得
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}{\dot{e}}_{\psi }=& \left[\begin{array}{llll}0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{c}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{\psi }}_{des}+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \end{array}\text{\hspace{1em}(17)}$$
根据等式(13)和(17)，基于跟踪误差变量的状态空间模型表示为
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]=& \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & +\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]{\dot{\psi }}_{des}\end{array}\text{\hspace{1em}(18)}$$

写成一般形式，状态空间表达式如下:
$$\dot{X}=AX+B_1\delta +B_2{\dot{\psi }}_{\text{des }}\text{\hspace{1em}(19)}$$

在使用时，我们一般会忽略$B_2{\dot{\psi }}_{\text{des }}$， 然后将状态方程离散化，便于实现。即：
$$X_t=X_{t-1}+A{X}_{t-1}\Delta t+B_1\delta \Delta t=(I+A\Delta t)X_{t-1}+B_1\delta \Delta t=\bar{A}{X}_{t-1}+\bar{B}\delta \text{\hspace{1em}(20)}$$
其中
$$\begin{array}{rl}\bar{A}& =I+A\Delta t(I为单位矩阵)\\ \bar{B}& =B_1\Delta t\end{array}$$

另外，如果考虑路面坡度角(road bank angle)$\varphi$（定义可参考前文动力学模型），则等式(18)变为
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]=& \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & +\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]{\dot{\psi }}_{des}+\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]\sin \varphi \end{array}\text{\hspace{1em}(21)}$$

3. python 实现

根据公式(18)，我们可以使用numpy实现如下代码：

import numpy as np

class LateralErrorModel:
    """
    This model describes the lateral motion of a vehicle and its response to various inputs.

    Parameters:
        self.m (float): Mass of the vehicle (kg).
        self.Vx (float): Longitudinal velocity (self.m/s).
        self.C_alpha_f (float): Front tire cornering stiffness (N/rad).
        self.C_alpha_r (float): Rear tire cornering stiffness (N/rad).
        self.l_f (float): Distance from the vehicle's center of mass to the front axle (self.m).
        self.l_r (float): Distance from the vehicle's center of mass to the rear axle (self.m).
        self.I_z (float): Moment of inertia about the vertical axis (kg self.m^2).
        self.g (float): Acceleration due to gravity (self.m/s^2).
    """
    def __init__(self, m, Vx, C_alpha_f, C_alpha_r, l_f, l_r, I_z, g):
        self.m = m
        self.Vx = Vx
        self.C_alpha_f = C_alpha_f
        self.C_alpha_r = C_alpha_r
        self.l_f = l_f
        self.l_r = l_r
        self.I_z = I_z
        self.g = g
        
    def GenerateStateSpace(self):
        A = np.array([[0, 1, 0, 0],
                           [0, -(2*self.C_alpha_f + 2*self.C_alpha_r) / (self.m * self.Vx), (2*self.C_alpha_f + 2*self.C_alpha_r) / self.m,
                            (-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx)],
                           [0, 0, 0, 1],
                           [0, -(2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / (self.I_z * self.Vx),
                            (2*self.l_f*self.C_alpha_f - 2*self.l_r*self.C_alpha_r) / self.I_z,
                            (-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])
        
        B = np.array([[0],
                           [2 * self.C_alpha_f / self.m],
                           [0],
                           [2 * self.l_f * self.C_alpha_f / self.I_z]])
        
        C = np.array([[0],
                           [(-2*self.C_alpha_f*self.l_f + 2*self.C_alpha_r*self.l_r) / (self.m * self.Vx) - self.Vx],
                           [0],
                           [(-2*self.l_f**2*self.C_alpha_f + 2*self.l_r**2*self.C_alpha_r) / (self.I_z * self.Vx)]])
        
        D = np.array([[0],
                           [self.g],
                           [0],
                           [0]])
        return A,B,C,D
    def ComputeStateDerivative(self, state, delta, psi_des, phi):
        """
        Computes the derivative of the state vector using the lateral error dynamics model.

        Parameters:
            state (numpy.ndarray): The current state vector [e_y, e_y_dot, e_psi, e_psi_dot].
            delta (float): The steering input.
            psi_des (float): The desired yaw rate input.
            phi (float): The roll angle input.

        Returns:
            numpy.ndarray: The derivative of the state vector [e_y_dot, e_y_ddot, e_psi_dot, e_psi_ddot].
        """
        A,B,C,D=self.GenerateStateSpace()
        state_dot = np.dot(A, state) + np.dot(B, delta) + np.dot(C, psi_des) + np.dot(D, np.sin(phi))
        return state_dot[:,0]
    def DiscreteStateSpace(self,dt):
        """
        Returns:
            discrete state space
        """
        A,B,C,D = self.GenerateStateSpace()
        I = np.eye(4)
        A_bar = I + A * dt
        B_bar = B*dt
        return A_bar,B_bar

例如我们使用如下参数：

model = LateralErrorModel(m=1000.0, Vx=20.0, C_alpha_f=10000.0, C_alpha_r=15000.0, l_f=1.5, l_r=1.0, I_z=3000.0, g=9.8)

# Initial conditions and inputs
initial_state = np.array([0.0, 0.0, 0.0, 0.0])
delta_input = 0.1
psi_des_input = 0.0
phi_input = 0.1
dt = 0.1
# Compute the derivative of the state using the model instance
state_derivative = model.ComputeStateDerivative(initial_state, delta_input, psi_des_input, phi_input)
A_bar, B_bar = model.DiscreteStateSpace(dt)
print("State derivative:", state_derivative)
print("A_bar: ", A_bar)
print("B_bar: ", B_bar)

最终得到状态转移：

State derivative: [0.         2.97836748    0.         1.        ]
A_bar:  [[1.    0.1   0.    0.   ]
 [0.    0.75  5.    0.   ]
 [0.    0.    1.    0.1  ]
 [0.    0.    0.    0.975]]
B_bar:  [[0.]
 [2.]
 [0.]
 [1.]]

完整代码文件可见github仓库.