python** 如下所示：
#!/usr/bin/python# -*coding: utf-8 -*import mathimport matplotlib.pyplot as pltimport numpy as npfrom celluloid import cameraclass 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 轴距自系数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 = 20 宽度 lf = 38 后桥中心到汽车前部的距离 lb = 08 从后桥中心到汽车后部的距离 最大转向 = 06 最大前轮旋转角度 tr = 05 轮半径 tw = 05 轮宽 wd = w 轴距长度 = lb + lf 车辆长度 def draw 拖车（x， y， yaw， 转向， 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 = 轮子copy（） 左后轮 fr 轮 = 轮copy（） 右前轮 fl 轮 = 轮copy（） 左前轮 rr wheel[1，：]= 车辆信息。wd/2 rl_wheel[1,:]= vehicle_info.WD 2 方向盘旋转 rot1 = NParray([[np.cos(steer), np.sin(steer)],np.sin(steer), np.cos（转向）]]偏航旋转矩阵 rot2 = nparray([[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（） save ** with camera = camera（fig） for i in range（600）： pltcla() 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.s**e('trajectory.gif')结果如下：
文章首发***idoitnow 如果你喜欢，可以关注一下。