def hybrid_astar_planning(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso, yawreso): sxr, syr = round(sx / xyreso), round(sy / xyreso) gxr, gyr = round(gx / xyreso), round(gy / xyreso) syawr = round(rs.pi_2_pi(syaw) / yawreso) gyawr = round(rs.pi_2_pi(gyaw) / yawreso) nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [1], 0.0, 0.0, -1) ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [1], 0.0, 0.0, -1) kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)]) P = calc_parameters(ox, oy, xyreso, yawreso, kdtree) hmap = astar.calc_holonomic_heuristic_with_obstacle( ngoal, P.ox, P.oy, P.xyreso, 1.0) steer_set, direc_set = calc_motion_set() open_set, closed_set = {calc_index(nstart, P): nstart}, {} qp = QueuePrior() qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P)) while True: if not open_set: return None ind = qp.get() n_curr = open_set[ind] closed_set[ind] = n_curr open_set.pop(ind) update, fpath = update_node_with_analystic_expantion(n_curr, ngoal, P) if update: fnode = fpath break for i in range(len(steer_set)): node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P) if not node: continue node_ind = calc_index(node, P) if node_ind in closed_set: continue if node_ind not in open_set: open_set[node_ind] = node qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) else: if open_set[node_ind].cost > node.cost: open_set[node_ind] = node qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) return extract_path(closed_set, fnode, nstart)
def calc_next_node(n, ind, u, d, P): step = C.XY_RESO * 2.0 nlist = math.ceil(step / C.MOVE_STEP) xlist = [n.x[-1] + d * C.MOVE_STEP * math.cos(n.yaw[-1])] ylist = [n.y[-1] + d * C.MOVE_STEP * math.sin(n.yaw[-1])] yawlist = [rs.pi_2_pi(n.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))] yawtlist = [ rs.pi_2_pi(n.yawt[-1] + d * C.MOVE_STEP / C.RTR * math.sin(n.yaw[-1] - n.yawt[-1])) ] for i in range(nlist - 1): xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i])) ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i])) yawlist.append( rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u))) yawtlist.append( rs.pi_2_pi(yawtlist[i] + d * C.MOVE_STEP / C.RTR * math.sin(yawlist[i] - yawtlist[i]))) xind = round(xlist[-1] / P.xyreso) yind = round(ylist[-1] / P.xyreso) yawind = round(yawlist[-1] / P.yawreso) cost = 0.0 if d > 0: direction = 1.0 cost += abs(step) else: direction = -1.0 cost += abs(step) * C.BACKWARD_COST if direction != n.direction: # switch back penalty cost += C.GEAR_COST cost += C.STEER_ANGLE_COST * abs(u) # steer penalyty cost += C.STEER_CHANGE_COST * abs(n.steer - u) # steer change penalty cost += C.SCISSORS_COST * sum( [abs(rs.pi_2_pi(x - y)) for x, y in zip(yawlist, yawtlist)]) # jacknif cost cost = n.cost + cost directions = [direction for _ in range(len(xlist))] node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, yawtlist, directions, u, cost, ind) return node
def calc_rs_path_cost(rspath, yawt): cost = 0.0 for lr in rspath.lengths: if lr >= 0: cost += 1 else: cost += abs(lr) * C.BACKWARD_COST for i in range(len(rspath.lengths) - 1): if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: cost += C.GEAR_COST for ctype in rspath.ctypes: if ctype != "S": cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER) nctypes = len(rspath.ctypes) ulist = [0.0 for _ in range(nctypes)] for i in range(nctypes): if rspath.ctypes[i] == "R": ulist[i] = -C.MAX_STEER elif rspath.ctypes[i] == "L": ulist[i] = C.MAX_STEER for i in range(nctypes - 1): cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) cost += C.SCISSORS_COST * sum( [abs(rs.pi_2_pi(x - y)) for x, y in zip(rspath.yaw, yawt)]) return cost
def update_node_with_analystic_expantion(n_curr, ngoal, gyawt, P): path = analystic_expantion(n_curr, ngoal, P) # rs path: n -> ngoal if not path: return False, None steps = [C.MOVE_STEP * d for d in path.directions] yawt = calc_trailer_yaw(path.yaw, n_curr.yawt[-1], steps) if abs(rs.pi_2_pi(yawt[-1] - gyawt)) >= C.GOAL_YAW_ERROR: return False, None fx = path.x[1:-1] fy = path.y[1:-1] fyaw = path.yaw[1:-1] fd = [] for d in path.directions[1:-1]: if d >= 0: fd.append(1.0) else: fd.append(-1.0) # fd = path.directions[1:-1] fcost = n_curr.cost + calc_rs_path_cost(path, yawt) fpind = calc_index(n_curr, P) fyawt = yawt[1:-1] fsteer = 0.0 fpath = Node(n_curr.xind, n_curr.yind, n_curr.yawind, n_curr.direction, fx, fy, fyaw, fyawt, fd, fsteer, fcost, fpind) return True, fpath
def calc_next_node(n_curr, c_id, u, d, P): step = C.XY_RESO * 2 nlist = math.ceil(step / C.MOVE_STEP) xlist = [n_curr.x[-1] + d * C.MOVE_STEP * math.cos(n_curr.yaw[-1])] ylist = [n_curr.y[-1] + d * C.MOVE_STEP * math.sin(n_curr.yaw[-1])] yawlist = [ rs.pi_2_pi(n_curr.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u)) ] for i in range(nlist - 1): xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i])) ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i])) yawlist.append( rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u))) xind = round(xlist[-1] / P.xyreso) yind = round(ylist[-1] / P.xyreso) yawind = round(yawlist[-1] / P.yawreso) if not is_index_ok(xind, yind, xlist, ylist, yawlist, P): return None cost = 0.0 if d > 0: direction = 1 cost += abs(step) else: direction = -1 cost += abs(step) * C.BACKWARD_COST if direction != n_curr.direction: # switch back penalty cost += C.GEAR_COST cost += C.STEER_ANGLE_COST * abs(u) # steer angle penalyty cost += C.STEER_CHANGE_COST * abs(n_curr.steer - u) # steer change penalty cost = n_curr.cost + cost directions = [direction for _ in range(len(xlist))] node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, directions, u, cost, c_id) return node
def main(): print("start!") sx, sy = 18.0, 34.0 # [m] syaw0 = np.deg2rad(180.0) syawt = np.deg2rad(180.0) gx, gy = 0.0, 12.0 # [m] gyaw0 = np.deg2rad(90.0) gyawt = np.deg2rad(90.0) ox, oy = design_obstacles() plt.plot(ox, oy, 'sk') draw_model(sx, sy, syaw0, syawt, 0.0) draw_model(gx, gy, gyaw0, gyawt, 0.0) # test(sx, sy, syaw0, syawt, 3.5, 32) # plt.axis("equal") # plt.show() oox, ooy = ox[:], oy[:] t0 = time.time() path = hybrid_astar_planning(sx, sy, syaw0, syawt, gx, gy, gyaw0, gyawt, oox, ooy, C.XY_RESO, C.YAW_RESO) t1 = time.time() print("running T: ", t1 - t0) x = path.x y = path.y yaw = path.yaw yawt = path.yawt direction = path.direction plt.pause(10) for k in range(len(x)): plt.cla() plt.plot(ox, oy, "sk") plt.plot(x, y, linewidth=1.5, color='r') if k < len(x) - 2: dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP steer = rs.pi_2_pi(math.atan(C.WB * dy / direction[k])) else: steer = 0.0 draw_model(gx, gy, gyaw0, gyawt, 0.0, 'gray') draw_model(x[k], y[k], yaw[k], yawt[k], steer) plt.axis("equal") plt.pause(0.0001) plt.show() print("Done")
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(): print("start!") x, y = 51, 31 sx, sy, syaw0 = 10.0, 7.0, np.deg2rad(120.0) gx, gy, gyaw0 = 45.0, 20.0, np.deg2rad(90.0) ox, oy = design_obstacles(x, y) t0 = time.time() path = hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, ox, oy, C.XY_RESO, C.YAW_RESO) t1 = time.time() print("running T: ", t1 - t0) if not path: print("Searching failed!") return x = path.x y = path.y yaw = path.yaw direction = path.direction for k in range(len(x)): plt.cla() plt.plot(ox, oy, "sk") plt.plot(x, y, linewidth=1.5, color='r') if k < len(x) - 2: dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP steer = rs.pi_2_pi(math.atan(-C.WB * dy / direction[k])) else: steer = 0.0 draw_car(gx, gy, gyaw0, 0.0, 'dimgray') draw_car(x[k], y[k], yaw[k], steer) plt.title("Hybrid A*") plt.axis("equal") plt.pause(0.0001) plt.show() print("Done!")
def hybrid_astar_planning(sx, sy, syaw, syawt, gx, gy, gyaw, gyawt, ox, oy, xyreso, yawreso): """ planning hybrid A* path. :param sx: starting node x position [m] :param sy: starting node y position [m] :param syaw: starting node yaw angle [rad] :param syawt: starting node trailer yaw angle [rad] :param gx: goal node x position [m] :param gy: goal node y position [m] :param gyaw: goal node yaw angle [rad] :param gyawt: goal node trailer yaw angle [rad] :param ox: obstacle x positions [m] :param oy: obstacle y positions [m] :param xyreso: grid resolution [m] :param yawreso: yaw resolution [m] :return: hybrid A* path """ sxr, syr = round(sx / xyreso), round(sy / xyreso) gxr, gyr = round(gx / xyreso), round(gy / xyreso) syawr = round(rs.pi_2_pi(syaw) / yawreso) gyawr = round(rs.pi_2_pi(gyaw) / yawreso) nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [syawt], [1], 0.0, 0.0, -1) ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [gyawt], [1], 0.0, 0.0, -1) kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)]) P = calc_parameters(ox, oy, xyreso, yawreso, kdtree) hmap = astar.calc_holonomic_heuristic_with_obstacle( ngoal, P.ox, P.oy, P.xyreso, 1.0) steer_set, direc_set = calc_motion_set() open_set, closed_set = {calc_index(nstart, P): nstart}, {} qp = QueuePrior() qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P)) while True: if not open_set: return None ind = qp.get() n_curr = open_set[ind] closed_set[ind] = n_curr open_set.pop(ind) update, fpath = update_node_with_analystic_expantion( n_curr, ngoal, gyawt, P) if update: fnode = fpath break yawt0 = n_curr.yawt[0] for i in range(len(steer_set)): node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P) if not is_index_ok(node, yawt0, P): continue node_ind = calc_index(node, P) if node_ind in closed_set: continue if node_ind not in open_set: open_set[node_ind] = node qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) else: if open_set[node_ind].cost > node.cost: open_set[node_ind] = node print("final expand node: ", len(open_set) + len(closed_set)) return extract_path(closed_set, fnode, nstart)
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()