1. 运动学方程

自行车模型（Bicycle Model）是车辆数字化模型中最常见的一种运动学模型。其除了可以反映车辆的一些基础特性外，更重要的是简单易用。通常情况下我们会把车辆模型简化为二自由度的自行车模型。

自行车模型主要基于以下假设：

• 车辆是在一个二维平面上运动，不考虑车辆垂直平面的（Z轴）方向上的移动。
• 车辆左右两个轮胎的运动可以合并为一个轮胎来描述，即假设车辆左右轮胎在任意时刻都拥有相同（或者近乎相同）的转向角度和速度。
• 车辆处于低速运动，可以忽略前后轴载荷的偏移。
• 车辆整体是刚体结构。

一般情况下，我们可以将车辆运动学模型简化为如下形式：

• ${\delta }_f$：前轮转角
• ${\delta }_r$：后轮转角
• $\beta$：质心侧偏角，即质心速度与车身坐标系$X$轴的夹角
• $\phi$：横摆角，即车的轴线与$X$轴的夹角
• $\beta +\phi$：航向角
• $v$：质心速度
• $O$：速度瞬心
• $C$：质心
• $L_f$：质心$C$到前轴距离
• $L_r$：质心$C$到后轴距离
• $L$：轴距，$L=L_f+L_r$

我们对质心速度$v$进行矢量分解，如上图中的$\dot{X}$和$\dot{Y}$所示，可以得到下式子$(1)$和$(2)$，根据理论力学刚体角速度公式可得公式$(3)$。由此得到单车模型下的车辆运动学微分模型为
$$\dot{X}=vcos(\beta +\phi )\text{\hspace{1em}(1)}$$

$$\dot{Y}=vsin(\beta +\phi )\text{\hspace{1em}(2)}$$

$$\dot{\phi }=\frac{v}{R}\text{\hspace{1em}(3)}$$

注：一个刚体的角速度 = 线速度/线速度到速度瞬心的距离

根据图中几何关系和正弦定理可知：
$$\frac{L_f}{sin({\delta }_f-\beta )}=\frac{R}{sin(\frac{\pi }{2}-{\delta }_f)}\text{\hspace{1em}(4)}$$

$$\frac{L_r}{sin({\delta }_r+\beta )}=\frac{R}{sin(\frac{\pi }{2}-{\delta }_r)}\text{\hspace{1em}(5)}$$

由上式展开可得
$$\frac{L_f}{R}=\frac{sin{\delta }_{f}cos\beta -cos{\delta }_{f}sin\beta }{cos{\delta }_f}=tan{\delta }_{f}cos\beta -sin\beta \text{\hspace{1em}(6)}$$

$$\frac{L_r}{R}=\frac{sin{\delta }_{r}cos\beta +cos{\delta }_{r}sin\beta }{cos{\delta }_r}=tan{\delta }_{r}cos\beta +sin\beta \text{\hspace{1em}(7)}$$

由载荷的影响，质心$C$位置会发生变化，导致$L_f$和$L_r$的长度发生变化，但是由于$L=l_f+L_r$是不变的，因此由式子$(6)(7)$可得
$$\frac{L_f+L_r}{R}=\frac{L}{R}=(tan{\delta }_f+tan{\delta }_r)cos\beta \text{\hspace{1em}(8)}$$
由$(3)$和$(8)$可得
$$\dot{\phi }=\frac{v}{R}=\frac{v(tan{\delta }_f+tan{\delta }_r)cos\beta }{L}\text{\hspace{1em}(9)}$$
由于低速条件下，我们可以假设车辆不会发生侧向滑动（漂移），此时我们可以将$v_y\approx 0$，因此$\beta \approx 0$，则横摆角$\phi$约等于航向角$\phi +\beta$ 。又由于大部分车辆不具备后轮转向的功能，因此我们可以假设后轮转角${\delta }_r\approx 0$，因此基于我们假设的前提下的运动学微分方程化简为
$$\begin{gathered} \dot{X}=vcos\phi \\ \dot{Y}=vsin\phi \\ \dot{\phi }=\frac{vtan{\delta }_f}{L}\text{\hspace{1em}(10)} \end{gathered}$$

2. 模型实现

python代码如下：

#!/usr/bin/python
# -*- coding: UTF-8 -*-import math
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Cameraclass Vehicle:def __init__(self,x=0.0,y=0.0,yaw=0.0,v=0.0,dt=0.1,l=3.0):self.steer = 0self.x = xself.y = yself.yaw = yawself.v = vself.dt = dtself.L = l  # 轴距self.x_front = x + l * math.cos(yaw)self.y_front = y + l * math.sin(yaw)def update(self, a, delta, max_steer=np.pi):delta = np.clip(delta, -max_steer, max_steer)self.steer = deltaself.x = self.x + self.v * math.cos(self.yaw) * self.dtself.y = self.y + self.v * math.sin(self.yaw) * self.dtself.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dtself.v = self.v + a * self.dtself.x_front = self.x + self.L * math.cos(self.yaw)self.y_front = self.y + self.L * math.sin(self.yaw)class VehicleInfo:# Vehicle parameterL = 3.0  #轴距W = 2.0  #宽度LF = 3.8  #后轴中心到车头距离LB = 0.8  #后轴中心到车尾距离MAX_STEER = 0.6  # 最大前轮转角TR = 0.5  # 轮子半径TW = 0.5  # 轮子宽度WD = W  #轮距LENGTH = LB + LF  # 车辆长度def draw_trailer(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):vehicle_outline = np.array([[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],[vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],[vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2, vehicle_info.TW / 2]])rr_wheel = wheel.copy() #右后轮rl_wheel = wheel.copy() #左后轮fr_wheel = wheel.copy() #右前轮fl_wheel = wheel.copy() #左前轮rr_wheel[1,:] += vehicle_info.WD/2rl_wheel[1,:] -= vehicle_info.WD/2#方向盘旋转rot1 = np.array([[np.cos(steer), -np.sin(steer)],[np.sin(steer), np.cos(steer)]])#yaw旋转矩阵rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],[np.sin(yaw), np.cos(yaw)]])fr_wheel = np.dot(rot1, fr_wheel)fl_wheel = np.dot(rot1, fl_wheel)fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])fr_wheel = np.dot(rot2, fr_wheel)fr_wheel[0, :] += xfr_wheel[1, :] += yfl_wheel = np.dot(rot2, fl_wheel)fl_wheel[0, :] += xfl_wheel[1, :] += yrr_wheel = np.dot(rot2, rr_wheel)rr_wheel[0, :] += xrr_wheel[1, :] += yrl_wheel = np.dot(rot2, rl_wheel)rl_wheel[0, :] += xrl_wheel[1, :] += yvehicle_outline = np.dot(rot2, vehicle_outline)vehicle_outline[0, :] += xvehicle_outline[1, :] += yax.plot(fr_wheel[0, :], fr_wheel[1, :], color)ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)# ax.axis('equal')if __name__ == "__main__":vehicle = Vehicle(x=0.0,y=0.0,yaw=0,v=0.0,dt=0.1,l=VehicleInfo.L)vehicle.v = 1trajectory_x = []trajectory_y = []fig = plt.figure()# 保存动图用# camera = Camera(fig)for i in range(600):plt.cla()plt.gca().set_aspect('equal', adjustable='box')vehicle.update(0, np.pi / 10)draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)trajectory_x.append(vehicle.x)trajectory_y.append(vehicle.y)plt.plot(trajectory_x, trajectory_y, 'blue')plt.xlim(-12, 12)plt.ylim(-2.5, 21)plt.pause(0.001)#     camera.snap()# animation = camera.animate(interval=5)# animation.save('trajectory.gif')

运行结果如下：

文章首发公众号：iDoitnow如果喜欢话，可以关注一下