单车模型及其线性化

1 单车模型

  这里讨论的是以后轴为中心的单车运动学模型，由下式表达：

$$\dot{S}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}vcos(\psi )\\ vsin(\psi )\\ v\frac{tan(\psi )}{L}\end{array}\right]$$

其中$S$代表系统状态，$\delta$为单车模型的前轮转角，$x,y$是车辆在世界坐标系下的位置，$L$为轴距，$v$为车辆速度，$\psi$代表航向角，单车模型中应该是假设了质心侧偏角为0，所以$\psi$应该也等于横摆角，即车头朝向。

  如果把加速度和前轮转角的变化加上，则可以写成：
$$\dot{S}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\\ \dot{v}\\ \dot{\delta }\end{array}\right]=\left[\begin{array}{c}vcos(\psi )\\ vsin(\psi )\\ v\frac{tan(\psi )}{L}\\ a\\ \gamma \end{array}\right]=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\\ f_4\\ f_5\end{array}\right]=f(S)$$
这里$\gamma$是我自己取的名字，看别的博客把速度和前轮转角用额外的矩阵表示了，这里我放在一起推一推。

2 线性化

  将上述系统$\dot{S}=f(S)$在$S=S_r=\left[\begin{array}{c}{x}_r\\ y_r\\ {\psi }_r\\ v_r\\ {\delta }_r\end{array}\right]$处泰勒展开，忽略高次项：

$$\dot{S}\approx f(S_r)+\frac{df}{dS}(S-S_r)$$

进而有：
$$\dot{S}-{\dot{S}}_r=\left(\begin{array}{ccccc}\frac{d{f}_1}{dx}& \frac{d{f}_1}{dy}& \frac{d{f}_1}{d\psi }& \frac{d{f}_1}{dv}& \frac{d{f}_1}{d\delta }\\ \frac{d{f}_2}{dx}& \frac{d{f}_2}{dy}& \frac{d{f}_2}{d\psi }& \frac{d{f}_2}{dv}& \frac{d{f}_2}{d\delta }\\ \frac{d{f}_3}{dx}& \frac{d{f}_3}{dy}& \frac{d{f}_3}{d\psi }& \frac{d{f}_3}{dv}& \frac{d{f}_3}{d\delta }\\ \frac{d{f}_4}{dx}& \frac{d{f}_4}{dy}& \frac{d{f}_4}{d\psi }& \frac{d{f}_4}{dv}& \frac{d{f}_4}{d\delta }\\ \frac{d{f}_5}{dx}& \frac{d{f}_5}{dy}& \frac{d{f}_5}{d\psi }& \frac{d{f}_5}{dv}& \frac{d{f}_5}{d\delta }\end{array}\right)\left[\begin{array}{c}x-x_r\\ y-y_r\\ \psi -{\psi }_r\\ v-v_r\\ \delta -{\delta }_r\end{array}\right]$$

求导可得：
$$\dot{S}-{\dot{S}}_r=\left(\begin{array}{ccccc}0& 0& -vsin(\psi )& cos(\psi )& 0\\ 0& 0& vcos(\psi )& sin(\psi )& 0\\ 0& 0& 0& \frac{tan(\delta )}{L}& \frac{v}{Lco{s}^2(\delta )}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right)\left[\begin{array}{c}x-x_r\\ y-y_r\\ \psi -{\psi }_r\\ v-v_r\\ \delta -{\delta }_r\end{array}\right]=J\Delta S$$

最后可得线性化表示

$$\Delta \dot{S}=\dot{S}-{\dot{S}}_r=J\Delta S$$

在实际应用中需要把它写成差分的形式，假设采样时间间隔为$\Delta t$，则有：

$$\frac{\Delta S_{k+1}-\Delta S_k}{\Delta t}=J\Delta S_k$$
最后得到车辆状态变化的差分方程形式
$$\Delta S_{k+1}=(J\Delta t+I)\Delta S_k$$

3 实现效果

  现在假设采样时间为0.1秒，车辆初始状态在原点，车头朝向是45度，运行1.5秒，分别用单车模型和线性化方法实现两个单车模型，看看跑出来的误差是怎样的。这里给的加速度是$4m/s^2$，前轮转角不变。效果如下红色是单车模型，绿色是线性化后的单车模型，这里的动画运行时间间隔是0.2s，方便看效果。可以看到在前1秒内这个线性化算出来的车辆状态和单车模型本身的误差还是可以的。

4 相关代码

  以车辆后轴为中心的单车模型

class KinematicModelBackCenter:
    """
    车辆后轴中心，单车模型
    x, y: 代表车辆中心的坐标
    psi: 代表车辆的航向角，也就是车辆的heading
    v: 代表车辆的速度
    L: 轴距，前轮到后轮中心的距离
    dt: 仿真的时间间隔
    """
    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

  线性化的差分形式的伪代码，线性化在应用中不应该是模拟用，这里只是想看1秒内误差是怎样的

# 上面推出来的(J*dt + I)
J = np.matrix([[1, 0, -v * math.sin(psi) * dt, math.cos(psi) * dt, 0],
               [0, 1, v * math.cos(psi) * dt, math.sin(psi) * dt, 0],
               [0, 0, 1, math.tan(delta_f) * dt / L, v * dt / (L * math.cos(delta_f) ** 2)],
               [0, 0, 0, 1, 0],
               [0, 0, 0, 0, 1]])
# 加速度为4，delta_f的变化率为0，相邻时间步的变化量
ds = np.matrix([[v * math.cos(psi) * dt],
                  [v * math.sin(psi) * dt],
                  [v * math.tan(delta_f) / L * dt],
                  [a * dt],
                  [0.0]])
# 每一步更新ds
ds = J * ds

5 参考资料

• 无人车系统（一）：运动学模型及其线性化
• 汽车运动学模型的线性化推导过程
• 上面的视频制作软件是manim，3B1B的视频制作软件哦，用来自己做做简单的动画还是ok的