Introduction to self-driving cars-W4_Longitudinal_Vehicle_Model 车辆纵向动力学模型

车辆纵向动力学模型具体内容可以参考教材《汽车理论》或者《汽车系统动力学》中动力性章节。
纵向上汽车驱动力应该等于加速阻力+坡道阻力+滚动阻力+空气阻力，这些阻力是可以见名知意的。

核心公式：

程序只是将这些公式做了实现，重要的还是掌握公式背后的理论。
题目中主要实现了两种工况，一是在没有坡度的路上固定节气门行驶，看速度变化过程；二是有坡度路面同时节气门也变化看速度变化过程，看位移变化。
第二个工况的图：


为了脱离Coursera能够直接使用，进行了简单修改。

实现：

#Longitudinal_Vehicle_Model.py

import sys
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg

class Vehicle():
    def __init__(self):
 
        # ==================================
        #  Parameters
        # ==================================
    
        #Throttle to engine torque
        self.a_0 = 400
        self.a_1 = 0.1
        self.a_2 = -0.0002
        
        # Gear ratio, effective radius, mass + inertia
        self.GR = 0.35
        self.r_e = 0.3
        self.J_e = 10
        self.m = 2000
        self.g = 9.81
        
        # Aerodynamic and friction coefficients
        self.c_a = 1.36
        self.c_r1 = 0.01
        
        # Tire force 
        self.c = 10000
        self.F_max = 10000
        
        # State variables
        self.x = 0
        self.v = 5
        self.a = 0
        self.w_e = 100
        self.w_e_dot = 0
        
        self.sample_time = 0.01
        
    def reset(self):
        # reset state variables
        self.x = 0
        self.v = 5
        self.a = 0
        self.w_e = 100
        self.w_e_dot = 0

    def step(self, throttle, alpha):
        w_w = self.GR * self.w_e

        s = (w_w * self.r_e - self.v) / self.v
        if abs(s) < 1:
            F_x = self.c * s
        else:
            F_x = self.F_max

        F_load = self.c_a * self.v**2 + self.c_r1 * self.v + self.m * self.g * np.sin(
            alpha)

        self.a = (F_x - F_load) / self.m
        self.v += self.a * self.sample_time
        self.x += self.v * self.sample_time

        T_e = throttle * (self.a_0 + self.a_1 * self.w_e +
                          self.a_2 * self.w_e**2)
        self.w_e_dot = (T_e - self.GR * (self.r_e * F_load)) / self.J_e
        self.w_e += self.w_e_dot * self.sample_time


def test1():
    sample_time = 0.01
    time_end = 100
    model = Vehicle()

    t_data = np.arange(0,time_end,sample_time)
    v_data = np.zeros_like(t_data)
    # throttle percentage between 0 and 1
    throttle = 0.2

    # incline angle (in radians)
    alpha = 0

    for i in range(t_data.shape[0]):
        v_data[i] = model.v
        model.step(throttle, alpha)

    print(t_data)
    plt.plot(t_data, v_data)
    plt.show()

def test2(): 
    sample_time = 0.01
    time_end = 20
    t_data = np.arange(0, time_end, sample_time)
    throttle_data = np.zeros_like(t_data)
    alpha_data = np.zeros_like(t_data)
    x_data = np.zeros_like(t_data)

    model = Vehicle()

    for i, t in enumerate(t_data):
        if t < 5:
            throttle_data[i] = 0.2 + (0.5 - 0.2) / 5 * t
        elif t < 15:
            throttle_data[i] = 0.5
        else:
            throttle_data[i] = 0.5 - (0.5) / 5 * (t - 15)

    for i in range(t_data.shape[0]):
        x = model.x
        if x < 60:
            model.step(throttle_data[i], np.arctan(3 / 60))
        elif x < 150:
            model.step(throttle_data[i], np.arctan(9 / 90))
        else:
            model.step(throttle_data[i], 0)
        x_data[i] = model.x

    plt.plot(t_data, x_data)
    plt.show()
    
    data = np.vstack([t_data, x_data]).T
    np.savetxt('xdata.txt', data, delimiter=', ')

def main():
    test1()
    test2()
    
if __name__=="__main__":
    main()

工况1结果（速度）：

工况2结果（位移）：

以上。