自动驾驶控制小仿真pure

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_vehicle from pure_pursuit_controller import pure_pursuit_steer_control from Path_generator import Path import numpy as np import matplotlib.pyplot as plt import imageio.v2 as imageio MAX_SIMULATION_TIME = 200.0# 程序最大运行时间200*dt def main(): # 设置跟踪轨迹 rx, ry, rv, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info() ref_path = np.column_stack((rx, ry, rv, ref_yaw, ref_s, ref_kappa)) # 假设车辆初始位置为（5，60），航向角yaw=0.0，速度为2m/s，时间周期dt为0.1秒 vehicle1 = Vehicle(x=5.0, y=60.0, yaw=0.0, v=2.0, dt=0.1, l=VehicleInfo.L) vehicle2 = Vehicle(x=5.0, y=60.0, yaw=0.0, v=2.0, dt=0.1, l=VehicleInfo.L) time = 0.0# 初始时间 target_ind1 = 0 target_ind2 = 0 # 记录车辆轨迹 trajectory_x1 = [] trajectory_y1 = [] trajectory_x2 = [] trajectory_y2 = [] lat_err1 = [] # 记录横向误差 lat_err2 = [] # 记录横向误差 i = 0 image_list = [] # 存储图片 plt.figure(1, dpi=100) old_index1 = None old_index2 = None last_idx = ref_path.shape[0] - 1# 跟踪轨迹的最后一个点的索引 while MAX_SIMULATION_TIME >= time and (last_idx > target_ind1 or last_idx > target_ind2): time += vehicle1.dt # 累加一次时间周期 if last_idx > target_ind1: delta_f1, target_ind1, e_y1 = pure_pursuit_steer_control(vehicle1, ref_path, old_index1, False) old_index1 = target_ind1 # 横向误差 lat_err1.append(e_y1) # 更新车辆状态 vehicle1.update(0.0, delta_f1, np.pi/10) # 由于假设纵向匀速运动，所以加速度a=0.0 trajectory_x1.append(vehicle1.x) trajectory_y1.append(vehicle1.y) if last_idx > target_ind2: delta_f2, target_ind2, e_y2 = pure_pursuit_steer_control(vehicle2, ref_path, old_index2, True) old_index2 = target_ind2 lat_err2.append(e_y2) vehicle2.update(0.0, delta_f2, np.pi / 10) # 由于假设纵向匀速运动，所以加速度a=0.0 trajectory_x2.append(vehicle2.x) trajectory_y2.append(vehicle2.y) # 显示动图 plt.subplots_adjust(hspace=0.5, wspace=0.5) # 调整垂直和水平间距 plt.subplot(3, 1, 1) plt.cla() plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0) draw_vehicle(vehicle1.x, vehicle1.y, vehicle1.yaw, vehicle1.steer, plt, color='blue') draw_vehicle(vehicle2.x, vehicle2.y, vehicle2.yaw, vehicle2.steer, plt, color='green') plt.plot(trajectory_x1, trajectory_y1, "-b", label="RWPP-trajectory") plt.plot(trajectory_x2, trajectory_y2, "-g", label="FWPP-trajectory") plt.plot(ref_path[target_ind1, 0], ref_path[target_ind1, 1], "b-o", label="RWPP-target") plt.plot(ref_path[target_ind2, 0], ref_path[target_ind2, 1], "g-o", label="FWPP-target") plt.xlim(min(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0]) - 3, max(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0]) + 3) plt.ylim(min(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1]) - 3, max(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1]) + 3) plt.legend() plt.grid(True) plt.subplot(3, 1, 2) plt.cla() plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0) plt.plot(trajectory_x1, trajectory_y1, 'b', label="RWPP-trajectory") plt.plot(trajectory_x2, trajectory_y2, 'g', label="FWPP-trajectory") plt.title("actual tracking effect") plt.xlim(min(trajectory_x1[-1], trajectory_x2[-1]) - 1, max(trajectory_x1[-1], trajectory_x2[-1]) + 1) plt.ylim(min(trajectory_y1[-1], trajectory_y2[-1]) - 0.5, max(trajectory_y1[-1], trajectory_y2[-1]) + 0.5) plt.legend() plt.grid(True) plt.subplot(3, 1, 3) plt.cla() plt.plot(lat_err1, 'b', label="RWPP") plt.plot(lat_err2, 'g', label="FWPP") plt.title("lateral error") plt.legend() plt.xlim((len(trajectory_x1) + len(trajectory_x2)) / 2 - 35, (len(trajectory_x1) + len(trajectory_x2)) / 2 + 35) plt.ylim(min(lat_err1[-1], lat_err2[-1]) - 0.1, max(lat_err1[-1], lat_err2[-1]) + 0.1) plt.grid(True) plt.pause(0.001) plt.savefig("temp.png") i += 1 if (i % 5) == 0: image_list.append(imageio.imread("temp.png")) imageio.mimsave("display.gif", image_list, duration=0.1) plt.figure(2) plt.subplots_adjust(hspace=0.5, wspace=0.5) # 调整垂直和水平间距 plt.subplot(2, 1, 1) plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0) plt.plot(trajectory_x1, trajectory_y1, 'b', label="RWPP-trajectory") plt.plot(trajectory_x2, trajectory_y2, 'g', label="FWPP-trajectory") plt.title("actual tracking effect") plt.legend() plt.grid(True) plt.subplot(2, 1, 2) plt.plot(lat_err1, 'b', label="Rear Wheel Pure Pursuit") plt.plot(lat_err2, 'g', label="Front Wheel Pure Pursuit") plt.title("lateral error") plt.legend() plt.grid(True) plt.show() if __name__ == '__main__': main()