def main(): # generate path ax = np.arange(0, 50, 0.5) ay = [math.sin(ix / 5.0) * ix / 2.0 for ix in ax] cx, cy, cyaw, ck, _ = cs.calc_spline_course(ax, ay, ds=C.dt) t = 0.0 maxTime = 100.0 yaw_old = 0.0 x0, y0, yaw0 = cx[0], cy[0], cyaw[0] xrec, yrec, yawrec = [], [], [] node = Node(x=x0, y=y0, yaw=yaw0, v=0.0) ref_trajectory = Trajectory(cx, cy, cyaw, ck) e, e_th = 0.0, 0.0 while t < maxTime: speed_ref = 25.0 / 3.6 sp = calc_speed_profile(cyaw, speed_ref) dl, target_ind, e, e_th, ai = ref_trajectory.lqr_control( node, e, e_th, sp, C.lqr_Q, C.lqr_R) dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) node.update(ai, dl, 1.0) t += C.dt if dist <= C.dref: break dy = (node.yaw - yaw_old) / (node.v * C.dt) steer = rs.pi_2_pi(-math.atan(C.WB * dy)) yaw_old = node.yaw xrec.append(node.x) yrec.append(node.y) yawrec.append(node.yaw) plt.cla() plt.plot(cx, cy, color='gray', linewidth=2.0) plt.plot(xrec, yrec, linewidth=2.0, color='darkviolet') plt.plot(cx[target_ind], cy[target_ind], '.r') draw.draw_car(node.x, node.y, node.yaw, steer, C) plt.axis("equal") plt.title("FrontWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h") plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) plt.show()
def main(): ax = [0.0, 15.0, 30.0, 50.0, 60.0] ay = [0.0, 40.0, 15.0, 30.0, 0.0] cx, cy, cyaw, ck, s = cs.calc_spline_course(ax, ay, ds=P.d_dist) sp = calc_speed_profile(cx, cy, cyaw, P.target_speed) ref_path = PATH(cx, cy, cyaw, ck) node = Node(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) time = 0.0 x = [node.x] y = [node.y] yaw = [node.yaw] v = [node.v] t = [0.0] d = [0.0] a = [0.0] delta_opt, a_opt = None, None a_exc, delta_exc = 0.0, 0.0 while time < P.time_max: z_ref, target_ind = \ calc_ref_trajectory_in_T_step(node, ref_path, sp) z0 = [node.x, node.y, node.v, node.yaw] a_opt, delta_opt, x_opt, y_opt, yaw_opt, v_opt = \ linear_mpc_control(z_ref, z0, a_opt, delta_opt) if delta_opt is not None: delta_exc, a_exc = delta_opt[0], a_opt[0] node.update(a_exc, delta_exc, 1.0) time += P.dt x.append(node.x) y.append(node.y) yaw.append(node.yaw) v.append(node.v) t.append(time) d.append(delta_exc) a.append(a_exc) dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) if dist < P.dist_stop and \ abs(node.v) < P.speed_stop: break dy = (node.yaw - yaw[-2]) / (node.v * P.dt) steer = rs.pi_2_pi(-math.atan(P.WB * dy)) plt.cla() draw.draw_car(node.x, node.y, node.yaw, steer, P) plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if x_opt is not None: plt.plot(x_opt, y_opt, color='darkviolet', marker='*') plt.plot(cx, cy, color='gray') plt.plot(x, y, '-b') plt.plot(cx[target_ind], cy[target_ind]) plt.axis("equal") plt.title("Linear MPC, " + "v = " + str(round(node.v * 3.6, 2))) plt.pause(0.001) plt.show()
def main(): # generate path states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120), (5, -10, 180), (15, 5, 30)] # # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), # (35, 10, 180), (30, -10, 160), (5, -12, 90)] x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states) maxTime = 100.0 yaw_old = 0.0 x0, y0, yaw0, direct0 = \ x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0] x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct, curv): t = 0.0 er, theta_e = 0.0, 0.0 node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0]) ref_path = PATH(cx, cy, cyaw, ccurv) while t < maxTime: if cdirect[0] > 0: speed_ref = 30.0 / 3.6 else: speed_ref = 20.0 / 3.6 delta, er, theta_e, ind = lqr_lateral_control( node, er, theta_e, ref_path) dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) acceleration = pid_longitudinal_control(speed_ref, node.v, dist, node.direct) node.update(acceleration, delta, node.direct) t += C.dt if dist <= C.dist_stop: break x_rec.append(node.x) y_rec.append(node.y) yaw_rec.append(node.yaw) direct_rec.append(node.direct) dy = (node.yaw - yaw_old) / (node.v * C.dt) steer = rs.pi_2_pi(-math.atan(C.WB * dy)) yaw_old = node.yaw x0 = x_rec[-1] y0 = y_rec[-1] yaw0 = yaw_rec[-1] plt.cla() plt.plot(x_all, y_all, color='gray', linewidth=2.0) plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet') plt.plot(cx[ind], cy[ind], '.r') draw.draw_car(node.x, node.y, node.yaw, steer, C) plt.axis("equal") plt.title("LQR & PID: v=" + str(node.v * 3.6)[:4] + "km/h") plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) plt.show()
def main(): # generate path: [x, y, yaw] states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120), (5, -10, 180), (15, 5, 30)] # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), # (35, 10, 180), (30, -10, 160), (5, -12, 90)] x, y, yaw, direct, path_x, path_y = generate_path(states) # simulation maxTime = 100.0 yaw_old = 0.0 x0, y0, yaw0, direct0 = x[0][0], y[0][0], yaw[0][0], direct[0][0] x_rec, y_rec = [], [] for cx, cy, cyaw, cdirect in zip(x, y, yaw, direct): t = 0.0 node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=direct0) nodes = Nodes() nodes.add(t, node) ref_trajectory = PATH(cx, cy) target_ind, _ = ref_trajectory.target_index(node) while t <= maxTime: if cdirect[0] > 0: target_speed = 30.0 / 3.6 C.Ld = 4.0 C.dist_stop = 1.5 C.dc = -1.1 else: target_speed = 20.0 / 3.6 C.Ld = 2.5 C.dist_stop = 0.2 C.dc = 0.2 xt = node.x + C.dc * math.cos(node.yaw) yt = node.y + C.dc * math.sin(node.yaw) dist = math.hypot(xt - cx[-1], yt - cy[-1]) if dist < C.dist_stop: break acceleration = pid_control(target_speed, node.v, dist, cdirect[0]) delta, target_ind = pure_pursuit(node, ref_trajectory, target_ind) t += C.dt node.update(acceleration, delta, cdirect[0]) nodes.add(t, node) x_rec.append(node.x) y_rec.append(node.y) dy = (node.yaw - yaw_old) / (node.v * C.dt) steer = rs.pi_2_pi(-math.atan(C.WB * dy)) yaw_old = node.yaw x0 = nodes.x[-1] y0 = nodes.y[-1] yaw0 = nodes.yaw[-1] direct0 = nodes.direct[-1] # animation plt.cla() plt.plot(node.x, node.y, marker='.', color='k') plt.plot(path_x, path_y, color='gray', linewidth=2) plt.plot(x_rec, y_rec, color='darkviolet', linewidth=2) plt.plot(cx[target_ind], cy[target_ind], ".r") draw.draw_car(node.x, node.y, yaw_old, steer, C) # for m in range(len(states)): # draw.Arrow(states[m][0], states[m][1], np.deg2rad(states[m][2]), 2, 'blue') plt.axis("equal") plt.title("PurePursuit: v=" + str(node.v * 3.6)[:4] + "km/h") plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) plt.show()
def main(): # generate path states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), (20, 0, 120), (5, -10, 180), (15, 5, 30)] # # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), # (35, 10, 180), (30, -10, 160), (5, -12, 90)] x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states) maxTime = 100.0 yaw_old = 0.0 x0, y0, yaw0, direct0 = \ x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0] x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct, curv): t = 0.0 node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0]) ref_trajectory = Trajectory(cx, cy, cyaw, ccurv) speed_ref = 30 / 3.6 while t < maxTime: if cdirect[0] > 0: speed_ref = 30.0 / 3.6 C.Ld = 3.5 else: speed_ref = 20.0 / 3.6 C.Ld = 2.5 e, k, yawref, s0 = ref_trajectory.calc_track_error(node) di = rear_wheel_feedback_control(node, e, k, yawref) dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) ai = pid_control(speed_ref, node.v, dist, node.direct) node.update(ai, di, node.direct) t += C.dt if dist <= C.dref: break x_rec.append(node.x) y_rec.append(node.y) yaw_rec.append(node.yaw) direct_rec.append(node.direct) dy = (node.yaw - yaw_old) / (node.v * C.dt) steer = rs.pi_2_pi(-math.atan(C.WB * dy)) yaw_old = node.yaw x0 = x_rec[-1] y0 = y_rec[-1] yaw0 = yaw_rec[-1] plt.cla() plt.plot(x_all, y_all, color='gray', linewidth=2.0) plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet') plt.plot(cx[s0], cy[s0], '.r') draw.draw_car(node.x, node.y, node.yaw, steer, C) plt.axis("equal") plt.title("RearWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h") plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) plt.pause(0.001) plt.show()