Introduction to self-driving cars-W4_Kinematic_Bicycle_Model 车辆动力学模型
作者：互联网
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()
结果
标签：Bicycle,driving,Kinematic,self,solution,time,np,model,data 来源： https://blog.csdn.net/weixin_42093587/article/details/122129500