Introduction to self-driving cars-W4_Kinematic_Bicycle_Model 车辆动力学模型

车辆动力学模型)
Coursera上一门自动驾驶相关的专项课程《Self-Driving Cars Specialization》，也是目前整个平台上唯一的概览性介绍自动驾驶的课程。
本文是对Course1《Introduction to Self-Driving Cars》中Week4里车辆动力学模型编程的作业进行了记录。
链接地址：https://www.coursera.org/learn/intro-self-driving-cars/ungradedLab/RL3rX/module-4-programming-exercise-kinematic-bicycle-model/labpath=%2Fnotebooks%2FCourse_1_Module_4%2FKinematic_Bicycle_Model.ipynb
注意：如果之前没有登录和注册课程，有可能无法进入，想免费的话选旁听课程即可。

这个训练的核心目的是学习Bicycle model，为了能够脱离链接仍能正常运行，只能对其中程序做了部分修改，主要是将Solution model 即参考模型做了注释，其他改动不大，作业里每个需要编程的部分都写成了test函数。
Bicycle模型：

核心公式

程序示例：

# from notebook_grader import BicycleSolution, grade_bicycle
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg


class Bicycle():
    def __init__(self):
        self.xc = 0
        self.yc = 0
        self.theta = 0
        self.delta = 0
        self.beta = 0

        self.L = 2
        self.lr = 1.2
        self.w_max = 1.22

        self.sample_time = 0.01

    def reset(self):
        self.xc = 0
        self.yc = 0
        self.theta = 0
        self.delta = 0
        self.beta = 0

    def step(self, v, w):
        # ==================================
        #  Implement kinematic model here
        # ==================================
        if w > 0:
            w = min(w, self.w_max)
        elif w < 0:
            w = max(w, -self.w_max)

        xc_dot = v * np.cos(self.theta + self.beta)
        yc_dot = v * np.sin(self.theta + self.beta)
        theta_dot = v * np.cos(self.beta) * np.tan(self.delta) / self.L
        delta_dot = w

        #update the state
        self.xc += xc_dot * self.sample_time
        self.yc += yc_dot * self.sample_time
        self.theta += theta_dot * self.sample_time
        self.delta += delta_dot * self.sample_time


def test_In26():
    sample_time = 0.01
    time_end = 20
    model = Bicycle()
    # solution_model = BicycleSolution()

    # set delta directly
    model.delta = 0.992 * np.arctan(2 / 10)
    # solution_model.delta = np.arctan(2/10)

    t_data = np.arange(0, time_end, sample_time)
    x_data = np.zeros_like(t_data)
    y_data = np.zeros_like(t_data)
    x_solution = np.zeros_like(t_data)
    y_solution = np.zeros_like(t_data)

    for i in range(t_data.shape[0]):
        x_data[i] = model.xc
        y_data[i] = model.yc
        model.step(np.pi, 0)

        # x_solution[i] = solution_model.xc
        # y_solution[i] = solution_model.yc
        # solution_model.step(np.pi, 0)

        #model.beta = 0
        #solution_model.beta=0

    plt.axis('equal')
    plt.plot(x_data, y_data, label='Learner Model')
    # plt.plot(x_solution, y_solution,label='Solution Model')
    plt.legend()
    plt.show()


def test_In4():
    sample_time = 0.01
    time_end = 20
    model = Bicycle()
    # model.reset()
    # solution_model.reset()

    t_data = np.arange(0, time_end, sample_time)
    x_data = np.zeros_like(t_data)
    y_data = np.zeros_like(t_data)
    x_solution = np.zeros_like(t_data)
    y_solution = np.zeros_like(t_data)

    for i in range(t_data.shape[0]):
        x_data[i] = model.xc
        y_data[i] = model.yc

        if model.delta < np.arctan(2 / 10):
            model.step(np.pi, model.w_max)
        else:
            model.step(np.pi, 0)

        # x_solution[i] = solution_model.xc
        # y_solution[i] = solution_model.yc

        # if solution_model.delta < np.arctan(2/10):
        #     solution_model.step(np.pi, model.w_max)
        # else:
        #     solution_model.step(np.pi, 0)

    plt.axis('equal')
    plt.plot(x_data, y_data, label='Learner Model')
    # plt.plot(x_solution, y_solution,label='Solution Model')
    plt.legend()
    plt.show()


def test_In29():
    sample_time = 0.01
    time_end = 60
    model = Bicycle()
    # model.reset()
    # solution_model.reset()

    t_data = np.arange(0, time_end, sample_time)
    x_data = np.zeros_like(t_data)
    y_data = np.zeros_like(t_data)
    x_solution = np.zeros_like(t_data)
    y_solution = np.zeros_like(t_data)

    # maintain velocity at 4 m/s
    v_data = np.zeros_like(t_data)
    v_data[:] = 4

    w_data = np.zeros_like(t_data)

    # ==================================
    #  Square Path: set w at corners only
    # ==================================
    w_data[670:670 + 100] = 0.753
    w_data[670 + 100:670 + 100 * 2] = -0.753
    w_data[2210:2210 + 100] = 0.753
    w_data[2210 + 100:2210 + 100 * 2] = -0.753
    w_data[3670:3670 + 100] = 0.753
    w_data[3670 + 100:3670 + 100 * 2] = -0.753
    w_data[5220:5220 + 100] = 0.753
    w_data[5220 + 100:5220 + 100 * 2] = -0.753

    # ==================================
    #  Spiral Path: high positive w, then small negative w
    # ==================================
    # w_data[:] = -1/100
    # w_data[0:100] = 1

    # ==================================
    #  Wave Path: square wave w input
    # ==================================
    #w_data[:] = 0
    #w_data[0:100] = 1
    #w_data[100:300] = -1
    #w_data[300:500] = 1
    #w_data[500:5700] = np.tile(w_data[100:500], 13)
    #w_data[5700:] = -1

    # ==================================
    #  Step through bicycle model
    # ==================================
    for i in range(t_data.shape[0]):
        x_data[i] = model.xc
        y_data[i] = model.yc
        model.step(v_data[i], w_data[i])

        # x_solution[i] = solution_model.xc
        # y_solution[i] = solution_model.yc
        # solution_model.step(v_data[i], w_data[i])

    plt.axis('equal')
    plt.plot(x_data, y_data, label='Learner Model')
    # plt.plot(x_solution, y_solution,label='Solution Model')
    plt.legend()
    plt.show()


def test_In59():
    sample_time = 0.01
    time_end = 30
    model = Bicycle()
    # model.reset()

    t_data = np.arange(0, time_end, sample_time)
    x_data = np.zeros_like(t_data)
    y_data = np.zeros_like(t_data)
    v_data = np.zeros_like(t_data)
    w_data = np.zeros_like(t_data)

    # calculate the velocity to keep
    radius = 8
    delta = 0.993 * np.arctan(model.L / radius)
    v_data[:] = np.pi * 4 * radius / time_end

    # ==================================
    #  Learner solution begins here
    # ==================================
    for i in range(t_data.shape[0]):
        x_data[i] = model.xc
        y_data[i] = model.yc
        if i < t_data.shape[0] / 8 or (i > t_data.shape[0] / 8 * 5.015
                                       and i < t_data.shape[0]):
            if model.delta < delta:
                model.step(v_data[i], model.w_max)
            else:
                model.step(v_data[i], 0)
        else:
            if model.delta > -delta:
                model.step(v_data[i], -model.w_max)
            else:
                model.step(v_data[i], 0)

    # ==================================
    #  Learner solution ends here
    # ==================================
    plt.axis('equal')
    plt.plot(x_data, y_data, label='double_Circle')
    plt.legend()
    plt.show()

def main():
    test_In4()
    test_In26()
    test_In29()
    test_In59()
if __name__ == "__main__":
    main()

结果