车辆运动学方程推导和代码实现

文章目录

- 1. 运动学方程
- 2. 模型实现

1. 运动学方程

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

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

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

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

在这里插入图片描述

$\delta_f$ ：前轮转角
$\delta_r$ ：后轮转角
$\beta$ ：质心侧偏角，即质心速度与车身坐标系 $X$ 轴的夹角
$\varphi$ ：横摆角，即车的轴线与 $X$ 轴的夹角
$\beta+\varphi$ ：航向角
$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+\varphi) \tag{1}$

$\dot{Y} = vsin(\beta+\varphi) \tag{2}$

$\dot{\varphi} = \frac{v}{R} \tag{3}$

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

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

$\frac{L_r}{sin(\delta_r + \beta)} = \frac{R}{sin(\frac{\pi}{2} - \delta_r)} \tag{5}$

由上式展开可得
$\frac{L_f}{R} = \frac{sin\delta_f cos\beta - cos\delta_f sin\beta}{cos\delta_f} = tan\delta_fcos\beta - sin\beta\tag{6}$

$\frac{L_r}{R} = \frac{sin\delta_r cos\beta + cos\delta_r sin\beta}{cos\delta_r} = tan\delta_rcos\beta+sin\beta \tag{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 \tag{8}$
由 $(3)$ 和 $(8)$ 可得
$\dot{\varphi} = \frac{v}{R} =\frac{v(tan\delta_f + tan\delta_r)cos\beta}{L} \tag{9}$
由于低速条件下，我们可以假设车辆不会发生侧向滑动（漂移），此时我们可以将 $v_y \approx 0$ ，因此 $\beta \approx 0$ ，则横摆角 $\varphi$ 约等于航向角 $\varphi + \beta$ 。又由于大部分车辆不具备后轮转向的功能，因此我们可以假设后轮转角 $\delta_r\approx0$ ，因此基于我们假设的前提下的运动学微分方程化简为
$\dot{X} = vcos\varphi \\ \dot{Y} = vsin\varphi \tag{10} \\ \dot{\varphi} = \frac{vtan\delta_f}{L}$

2. 模型实现

python代码如下：

#!/usr/bin/python
# -*- coding: UTF-8 -*-

import math
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera


class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.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 = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 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/2
    rl_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, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.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 = 1
    trajectory_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')