├── .idea ├── .gitignore ├── MotionPlanning.iml ├── inspectionProfiles │ └── profiles_settings.xml ├── misc.xml ├── modules.xml └── vcs.xml ├── Control ├── .gitattributes ├── LQR_Dynamics_Model.py ├── LQR_Kinematic_Model.py ├── MPC_Frenet_Frame.py ├── MPC_XY_Frame.py ├── Pure_Pursuit.py ├── Rear_Wheel_Feedback.py ├── Stanley.py ├── __pycache__ │ ├── config_control.cpython-37.pyc │ ├── draw.cpython-37.pyc │ ├── draw.cpython-38.pyc │ └── draw_lqr.cpython-37.pyc ├── config_control.py ├── draw.py ├── draw_lqr.py ├── fig │ ├── carmodel.png │ ├── frontwheel.png │ ├── ps.png │ ├── purepursuit.jpg │ ├── rearwheel.png │ └── simple car.jpg ├── gif │ ├── FWF.gif │ ├── LQR_Dynamics.gif │ ├── LQR_Kinematic.gif │ ├── MPC.gif │ ├── RWF1.gif │ ├── RWF2.gif │ ├── demo.gif │ ├── purepursuit1.gif │ ├── purepursuit2.gif │ └── stanley.gif └── utils.py ├── CurvesGenerator ├── __pycache__ │ ├── cubic_spline.cpython-37.pyc │ ├── cubic_spline.cpython-38.pyc │ ├── draw.cpython-37.pyc │ ├── quartic_polynomial.cpython-37.pyc │ ├── quintic_polynomial.cpython-37.pyc │ ├── reeds_shepp.cpython-37.pyc │ └── reeds_shepp.cpython-38.pyc ├── cubic_spline.py ├── draw.py ├── dubins_path.py ├── quartic_polynomial.py ├── quintic_polynomial.py └── reeds_shepp.py ├── HybridAstarPlanner ├── __pycache__ │ ├── astar.cpython-37.pyc │ ├── astar.cpython-38.pyc │ ├── draw.cpython-37.pyc │ ├── draw.cpython-38.pyc │ ├── hybrid_astar.cpython-37.pyc │ └── hybrid_astar.cpython-38.pyc ├── astar.py ├── draw.py ├── gif │ ├── hybrid Astar-1.gif │ ├── hybrid Astar-2.gif │ ├── hybrid Astar-t1.gif │ ├── hybrid Astar-t2.gif │ ├── hybrid Astar-t3.gif │ └── hybrid Astar-t5.gif ├── hybrid_astar.py └── hybrid_astar_with_trailer.py ├── LatticePlanner ├── __pycache__ │ ├── draw.cpython-37.pyc │ └── env.cpython-37.pyc ├── draw.py ├── env.py ├── gif │ ├── Crusing.gif │ └── StoppingMode.gif └── lattice_planner.py └── README.md /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /workspace.xml -------------------------------------------------------------------------------- /.idea/MotionPlanning.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /Control/.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Control/LQR_Dynamics_Model.py: -------------------------------------------------------------------------------- 1 | """ 2 | LQR and PID Controller 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | from enum import Enum 10 | import matplotlib.pyplot as plt 11 | 12 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 13 | "/../../MotionPlanning/") 14 | 15 | import Control.draw_lqr as draw 16 | from Control.config_control import * 17 | import CurvesGenerator.reeds_shepp as rs 18 | 19 | 20 | class Gear(Enum): 21 | GEAR_DRIVE = 1 22 | GEAR_REVERSE = 2 23 | 24 | 25 | class VehicleState: 26 | def __init__(self, x=0.0, y=0.0, yaw=0.0, 27 | v=0.0, gear=Gear.GEAR_DRIVE): 28 | self.x = x 29 | self.y = y 30 | self.yaw = yaw 31 | self.v = v 32 | self.e_cg = 0.0 33 | self.theta_e = 0.0 34 | 35 | self.gear = gear 36 | self.steer = 0.0 37 | 38 | def UpdateVehicleState(self, delta, a, e_cg, theta_e, 39 | gear=Gear.GEAR_DRIVE): 40 | """ 41 | update states of vehicle 42 | :param theta_e: yaw error to ref trajectory 43 | :param e_cg: lateral error to ref trajectory 44 | :param delta: steering angle [rad] 45 | :param a: acceleration [m / s^2] 46 | :param gear: gear mode [GEAR_DRIVE / GEAR/REVERSE] 47 | """ 48 | 49 | wheelbase_ = wheelbase 50 | delta, a = self.RegulateInput(delta, a) 51 | 52 | self.steer = delta 53 | self.gear = gear 54 | self.x += self.v * math.cos(self.yaw) * ts 55 | self.y += self.v * math.sin(self.yaw) * ts 56 | self.yaw += self.v / wheelbase_ * math.tan(delta) * ts 57 | self.e_cg = e_cg 58 | self.theta_e = theta_e 59 | 60 | if gear == Gear.GEAR_DRIVE: 61 | self.v += a * ts 62 | else: 63 | self.v += -1.0 * a * ts 64 | 65 | self.v = self.RegulateOutput(self.v) 66 | 67 | @staticmethod 68 | def RegulateInput(delta, a): 69 | """ 70 | regulate delta to : - max_steer_angle ~ max_steer_angle 71 | regulate a to : - max_acceleration ~ max_acceleration 72 | :param delta: steering angle [rad] 73 | :param a: acceleration [m / s^2] 74 | :return: regulated delta and acceleration 75 | """ 76 | 77 | if delta < -1.0 * max_steer_angle: 78 | delta = -1.0 * max_steer_angle 79 | 80 | if delta > 1.0 * max_steer_angle: 81 | delta = 1.0 * max_steer_angle 82 | 83 | if a < -1.0 * max_acceleration: 84 | a = -1.0 * max_acceleration 85 | 86 | if a > 1.0 * max_acceleration: 87 | a = 1.0 * max_acceleration 88 | 89 | return delta, a 90 | 91 | @staticmethod 92 | def RegulateOutput(v): 93 | """ 94 | regulate v to : -max_speed ~ max_speed 95 | :param v: calculated speed [m / s] 96 | :return: regulated speed 97 | """ 98 | 99 | max_speed_ = max_speed 100 | 101 | if v < -1.0 * max_speed_: 102 | v = -1.0 * max_speed_ 103 | 104 | if v > 1.0 * max_speed_: 105 | v = 1.0 * max_speed_ 106 | 107 | return v 108 | 109 | 110 | class TrajectoryAnalyzer: 111 | def __init__(self, x, y, yaw, k): 112 | self.x_ = x 113 | self.y_ = y 114 | self.yaw_ = yaw 115 | self.k_ = k 116 | 117 | self.ind_old = 0 118 | self.ind_end = len(x) 119 | 120 | def ToTrajectoryFrame(self, vehicle_state): 121 | """ 122 | errors to trajectory frame 123 | theta_e = yaw_vehicle - yaw_ref_path 124 | e_cg = lateral distance of center of gravity (cg) in frenet frame 125 | :param vehicle_state: vehicle state (class VehicleState) 126 | :return: theta_e, e_cg, yaw_ref, k_ref 127 | """ 128 | 129 | x_cg = vehicle_state.x 130 | y_cg = vehicle_state.y 131 | yaw = vehicle_state.yaw 132 | 133 | # calc nearest point in ref path 134 | dx = [x_cg - ix for ix in self.x_[self.ind_old: self.ind_end]] 135 | dy = [y_cg - iy for iy in self.y_[self.ind_old: self.ind_end]] 136 | 137 | ind_add = int(np.argmin(np.hypot(dx, dy))) 138 | dist = math.hypot(dx[ind_add], dy[ind_add]) 139 | 140 | # calc lateral relative position of vehicle to ref path 141 | vec_axle_rot_90 = np.array([[math.cos(yaw + math.pi / 2.0)], 142 | [math.sin(yaw + math.pi / 2.0)]]) 143 | 144 | vec_path_2_cg = np.array([[dx[ind_add]], 145 | [dy[ind_add]]]) 146 | 147 | if np.dot(vec_axle_rot_90.T, vec_path_2_cg) > 0.0: 148 | e_cg = 1.0 * dist # vehicle on the right of ref path 149 | else: 150 | e_cg = -1.0 * dist # vehicle on the left of ref path 151 | 152 | # calc yaw error: theta_e = yaw_vehicle - yaw_ref 153 | self.ind_old += ind_add 154 | yaw_ref = self.yaw_[self.ind_old] 155 | theta_e = pi_2_pi(yaw - yaw_ref) 156 | 157 | # calc ref curvature 158 | k_ref = self.k_[self.ind_old] 159 | 160 | return theta_e, e_cg, yaw_ref, k_ref 161 | 162 | 163 | class LatController: 164 | """ 165 | Lateral Controller using LQR 166 | """ 167 | 168 | def ComputeControlCommand(self, vehicle_state, ref_trajectory): 169 | """ 170 | calc lateral control command. 171 | :param vehicle_state: vehicle state 172 | :param ref_trajectory: reference trajectory (analyzer) 173 | :return: steering angle (optimal u), theta_e, e_cg 174 | """ 175 | 176 | ts_ = ts 177 | e_cg_old = vehicle_state.e_cg 178 | theta_e_old = vehicle_state.theta_e 179 | 180 | theta_e, e_cg, yaw_ref, k_ref = \ 181 | ref_trajectory.ToTrajectoryFrame(vehicle_state) 182 | 183 | # Calc linearized time-discrete system model 184 | matrix_ad_, matrix_bd_ = self.UpdateMatrix(vehicle_state) 185 | 186 | matrix_state_ = np.zeros((state_size, 1)) 187 | matrix_r_ = np.diag(matrix_r) 188 | matrix_q_ = np.diag(matrix_q) 189 | 190 | # Solve Ricatti equations using value iteration 191 | matrix_k_ = self.SolveLQRProblem(matrix_ad_, matrix_bd_, matrix_q_, 192 | matrix_r_, eps, max_iteration) 193 | 194 | # state vector: 4x1 195 | matrix_state_[0][0] = e_cg 196 | matrix_state_[1][0] = (e_cg - e_cg_old) / ts_ 197 | matrix_state_[2][0] = theta_e 198 | matrix_state_[3][0] = (theta_e - theta_e_old) / ts_ 199 | 200 | # feedback steering angle 201 | steer_angle_feedback = -(matrix_k_ @ matrix_state_)[0][0] 202 | 203 | # calc feedforward term to decrease steady error 204 | steer_angle_feedforward = self.ComputeFeedForward(vehicle_state, k_ref, matrix_k_) 205 | 206 | steer_angle = steer_angle_feedback + steer_angle_feedforward 207 | 208 | return steer_angle, theta_e, e_cg 209 | 210 | @staticmethod 211 | def ComputeFeedForward(vehicle_state, ref_curvature, matrix_k_): 212 | """ 213 | calc feedforward control term to decrease the steady error. 214 | :param vehicle_state: vehicle state 215 | :param ref_curvature: curvature of the target point in ref trajectory 216 | :param matrix_k_: feedback matrix K 217 | :return: feedforward term 218 | """ 219 | 220 | mass_ = m_f + m_r 221 | wheelbase_ = l_f + l_r 222 | 223 | kv = l_r * mass_ / 2.0 / c_f / wheelbase_ - \ 224 | l_f * mass_ / 2.0 / c_r / wheelbase_ 225 | 226 | v = vehicle_state.v 227 | 228 | if vehicle_state.gear == Gear.GEAR_REVERSE: 229 | steer_angle_feedforward = wheelbase_ * ref_curvature 230 | else: 231 | steer_angle_feedforward = wheelbase_ * ref_curvature + kv * v * v * ref_curvature - \ 232 | matrix_k_[0][2] * \ 233 | (l_r * ref_curvature - 234 | l_f * mass_ * v * v * ref_curvature / 2.0 / c_r / wheelbase_) 235 | 236 | return steer_angle_feedforward 237 | 238 | @staticmethod 239 | def SolveLQRProblem(A, B, Q, R, tolerance, max_num_iteration): 240 | """ 241 | iteratively calculating feedback matrix K 242 | :param A: matrix_a_ 243 | :param B: matrix_b_ 244 | :param Q: matrix_q_ 245 | :param R: matrix_r_ 246 | :param tolerance: lqr_eps 247 | :param max_num_iteration: max_iteration 248 | :return: feedback matrix K 249 | """ 250 | 251 | assert np.size(A, 0) == np.size(A, 1) and \ 252 | np.size(B, 0) == np.size(A, 0) and \ 253 | np.size(Q, 0) == np.size(Q, 1) and \ 254 | np.size(Q, 0) == np.size(A, 1) and \ 255 | np.size(R, 0) == np.size(R, 1) and \ 256 | np.size(R, 0) == np.size(B, 1), \ 257 | "LQR solver: one or more matrices have incompatible dimensions." 258 | 259 | M = np.zeros((np.size(Q, 0), np.size(R, 1))) 260 | 261 | AT = A.T 262 | BT = B.T 263 | MT = M.T 264 | 265 | P = Q 266 | num_iteration = 0 267 | diff = math.inf 268 | 269 | while num_iteration < max_num_iteration and diff > tolerance: 270 | num_iteration += 1 271 | P_next = AT @ P @ A - (AT @ P @ B + M) @ \ 272 | np.linalg.pinv(R + BT @ P @ B) @ (BT @ P @ A + MT) + Q 273 | 274 | # check the difference between P and P_next 275 | diff = (abs(P_next - P)).max() 276 | P = P_next 277 | 278 | if num_iteration >= max_num_iteration: 279 | print("LQR solver cannot converge to a solution", 280 | "last consecutive result diff is: ", diff) 281 | 282 | K = np.linalg.inv(BT @ P @ B + R) @ (BT @ P @ A + MT) 283 | 284 | return K 285 | 286 | @staticmethod 287 | def UpdateMatrix(vehicle_state): 288 | """ 289 | calc A and b matrices of linearized, discrete system. 290 | :return: A, b 291 | """ 292 | 293 | ts_ = ts 294 | mass_ = m_f + m_r 295 | 296 | v = vehicle_state.v 297 | 298 | matrix_a_ = np.zeros((state_size, state_size)) # continuous A matrix 299 | 300 | if vehicle_state.gear == Gear.GEAR_REVERSE: 301 | """ 302 | A matrix (Gear Reverse) 303 | [0.0, 0.0, 1.0 * v 0.0; 304 | 0.0, -(c_f + c_r) / m / v, (c_f + c_r) / m, 305 | (l_r * c_r - l_f * c_f) / m / v; 306 | 0.0, 0.0, 0.0, 1.0; 307 | 0.0, (lr * cr - lf * cf) / i_z / v, (l_f * c_f - l_r * c_r) / i_z, 308 | -1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z / v;] 309 | """ 310 | 311 | matrix_a_[0][1] = 0.0 312 | matrix_a_[0][2] = 1.0 * v 313 | else: 314 | """ 315 | A matrix (Gear Drive) 316 | [0.0, 1.0, 0.0, 0.0; 317 | 0.0, -(c_f + c_r) / m / v, (c_f + c_r) / m, 318 | (l_r * c_r - l_f * c_f) / m / v; 319 | 0.0, 0.0, 0.0, 1.0; 320 | 0.0, (lr * cr - lf * cf) / i_z / v, (l_f * c_f - l_r * c_r) / i_z, 321 | -1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z / v;] 322 | """ 323 | 324 | matrix_a_[0][1] = 1.0 325 | matrix_a_[0][2] = 0.0 326 | 327 | matrix_a_[1][1] = -1.0 * ( + c_r) / mass_ / v 328 | matrix_a_[1][2] = (c_f + c_r) / mass_ 329 | matrix_a_[1][3] = (l_r * c_r - l_f * c_f) / mass_ / v 330 | matrix_a_[2][3] = 1.0 331 | matrix_a_[3][1] = (l_r * c_r - l_f * c_f) / Iz / v 332 | matrix_a_[3][2] = (l_f * c_f - l_r * c_r) / Iz 333 | matrix_a_[3][3] = -1.0 * (l_f ** 2 * c_f + l_r ** 2 * c_r) / Iz / v 334 | 335 | # Tustin's method (bilinear transform) 336 | matrix_i = np.eye(state_size) # identical matrix 337 | matrix_ad_ = np.linalg.pinv(matrix_i - ts_ * 0.5 * matrix_a_) @ \ 338 | (matrix_i + ts_ * 0.5 * matrix_a_) # discrete A matrix 339 | 340 | # b = [0.0, c_f / m, 0.0, l_f * c_f / I_z].T 341 | matrix_b_ = np.zeros((state_size, 1)) # continuous b matrix 342 | matrix_b_[1][0] = c_f / mass_ 343 | matrix_b_[3][0] = l_f * c_f / Iz 344 | matrix_bd_ = matrix_b_ * ts_ # discrete b matrix 345 | 346 | return matrix_ad_, matrix_bd_ 347 | 348 | 349 | class LonController: 350 | """ 351 | Longitudinal Controller using PID. 352 | """ 353 | 354 | @staticmethod 355 | def ComputeControlCommand(target_speed, vehicle_state, dist): 356 | """ 357 | calc acceleration command using PID. 358 | :param target_speed: target speed [m / s] 359 | :param vehicle_state: vehicle state 360 | :param dist: distance to goal [m] 361 | :return: control command (acceleration) [m / s^2] 362 | """ 363 | 364 | if vehicle_state.gear == Gear.GEAR_DRIVE: 365 | direct = 1.0 366 | else: 367 | direct = -1.0 368 | 369 | a = 0.3 * (target_speed - direct * vehicle_state.v) 370 | 371 | if dist < 10.0: 372 | if vehicle_state.v > 2.0: 373 | a = -3.0 374 | elif vehicle_state.v < -2: 375 | a = -1.0 376 | 377 | return a 378 | 379 | 380 | def pi_2_pi(angle): 381 | """ 382 | regulate theta to -pi ~ pi. 383 | :param angle: input angle 384 | :return: regulated angle 385 | """ 386 | 387 | M_PI = math.pi 388 | 389 | if angle > M_PI: 390 | return angle - 2.0 * M_PI 391 | 392 | if angle < -M_PI: 393 | return angle + 2.0 * M_PI 394 | 395 | return angle 396 | 397 | 398 | def generate_path(s): 399 | """ 400 | design path using reeds-shepp path generator. 401 | divide paths into sections, in each section the direction is the same. 402 | :param s: objective positions and directions. 403 | :return: paths 404 | """ 405 | wheelbase_ = wheelbase 406 | 407 | max_c = math.tan(0.5 * max_steer_angle) / wheelbase_ 408 | path_x, path_y, yaw, direct, rc = [], [], [], [], [] 409 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = [], [], [], [], [] 410 | direct_flag = 1.0 411 | 412 | for i in range(len(s) - 1): 413 | s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2]) 414 | g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2]) 415 | 416 | path_i = rs.calc_optimal_path(s_x, s_y, s_yaw, 417 | g_x, g_y, g_yaw, max_c) 418 | 419 | irc, rds = rs.calc_curvature(path_i.x, path_i.y, path_i.yaw, path_i.directions) 420 | 421 | ix = path_i.x 422 | iy = path_i.y 423 | iyaw = path_i.yaw 424 | idirect = path_i.directions 425 | 426 | for j in range(len(ix)): 427 | if idirect[j] == direct_flag: 428 | x_rec.append(ix[j]) 429 | y_rec.append(iy[j]) 430 | yaw_rec.append(iyaw[j]) 431 | direct_rec.append(idirect[j]) 432 | rc_rec.append(irc[j]) 433 | else: 434 | if len(x_rec) == 0 or direct_rec[0] != direct_flag: 435 | direct_flag = idirect[j] 436 | continue 437 | 438 | path_x.append(x_rec) 439 | path_y.append(y_rec) 440 | yaw.append(yaw_rec) 441 | direct.append(direct_rec) 442 | rc.append(rc_rec) 443 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = \ 444 | [x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]], [rc_rec[-1]] 445 | 446 | path_x.append(x_rec) 447 | path_y.append(y_rec) 448 | yaw.append(yaw_rec) 449 | direct.append(direct_rec) 450 | rc.append(rc_rec) 451 | 452 | x_all, y_all = [], [] 453 | for ix, iy in zip(path_x, path_y): 454 | x_all += ix 455 | y_all += iy 456 | 457 | return path_x, path_y, yaw, direct, rc, x_all, y_all 458 | 459 | 460 | def main(): 461 | # generate path 462 | states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), 463 | (20, 0, 120), (5, -10, 180), (15, 5, 30)] 464 | # 465 | # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), 466 | # (35, 10, 180), (30, -10, 160), (5, -12, 90)] 467 | 468 | x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states) 469 | 470 | maxTime = 100.0 471 | yaw_old = 0.0 472 | x0, y0, yaw0, direct0 = \ 473 | x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0] 474 | 475 | x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] 476 | 477 | lat_controller = LatController() 478 | lon_controller = LonController() 479 | 480 | for x, y, yaw, gear, k in zip(x_ref, y_ref, yaw_ref, direct, curv): 481 | t = 0.0 482 | 483 | if gear[0] == 1.0: 484 | direct = Gear.GEAR_DRIVE 485 | else: 486 | direct = Gear.GEAR_REVERSE 487 | 488 | ref_trajectory = TrajectoryAnalyzer(x, y, yaw, k) 489 | 490 | vehicle_state = VehicleState(x=x0, y=y0, yaw=yaw0, v=0.1, gear=direct) 491 | 492 | while t < maxTime: 493 | 494 | dist = math.hypot(vehicle_state.x - x[-1], vehicle_state.y - y[-1]) 495 | 496 | if gear[0] > 0: 497 | target_speed = 25.0 / 3.6 498 | else: 499 | target_speed = 15.0 / 3.6 500 | 501 | delta_opt, theta_e, e_cg = \ 502 | lat_controller.ComputeControlCommand(vehicle_state, ref_trajectory) 503 | 504 | a_opt = lon_controller.ComputeControlCommand(target_speed, vehicle_state, dist) 505 | 506 | vehicle_state.UpdateVehicleState(pi_2_pi(delta_opt), a_opt, e_cg, theta_e, direct) 507 | 508 | t += ts 509 | 510 | if dist <= 0.5: 511 | break 512 | 513 | x_rec.append(vehicle_state.x) 514 | y_rec.append(vehicle_state.y) 515 | yaw_rec.append(vehicle_state.yaw) 516 | 517 | x0 = x_rec[-1] 518 | y0 = y_rec[-1] 519 | yaw0 = yaw_rec[-1] 520 | 521 | plt.cla() 522 | plt.plot(x_all, y_all, color='gray', linewidth=2.0) 523 | plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet') 524 | draw.draw_car(x0, y0, yaw0, -vehicle_state.steer) 525 | plt.axis("equal") 526 | plt.title("LQR (Dynamics): v=" + str(vehicle_state.v * 3.6)[:4] + "km/h") 527 | plt.gcf().canvas.mpl_connect('key_release_event', 528 | lambda event: 529 | [exit(0) if event.key == 'escape' else None]) 530 | plt.pause(0.001) 531 | 532 | plt.show() 533 | 534 | 535 | if __name__ == '__main__': 536 | main() 537 | -------------------------------------------------------------------------------- /Control/LQR_Kinematic_Model.py: -------------------------------------------------------------------------------- 1 | """ 2 | LQR and PID Controller 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | from enum import Enum 11 | import matplotlib.pyplot as plt 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 14 | "/../../MotionPlanning/") 15 | 16 | import Control.draw_lqr as draw 17 | from Control.config_control import * 18 | import CurvesGenerator.reeds_shepp as rs 19 | 20 | 21 | # class C: 22 | # # PID config 23 | # Kp = 1.0 24 | # 25 | # # System config 26 | # dt = 0.1 27 | # dist_stop = 0.5 28 | # Q = np.eye(4) 29 | # R = np.eye(1) 30 | # 31 | # # vehicle config 32 | # RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 33 | # RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 34 | # W = 2.4 # [m] width of vehicle 35 | # WD = 0.7 * W # [m] distance between left-right wheels 36 | # WB = 2.5 # [m] Wheel base 37 | # TR = 0.44 # [m] Tyre radius 38 | # TW = 0.7 # [m] Tyre width 39 | # MAX_STEER = 0.30 40 | 41 | 42 | # Controller Config 43 | ts = 0.1 # [s] 44 | l_f = 1.165 # [m] 45 | l_r = 1.165 # [m] 46 | max_iteration = 150 47 | eps = 0.01 48 | 49 | matrix_q = [0.5, 0.0, 1.0, 0.0] 50 | matrix_r = [1.0] 51 | 52 | state_size = 4 53 | 54 | max_acceleration = 5.0 # [m / s^2] 55 | max_steer_angle = np.deg2rad(40) # [rad] 56 | max_speed = 35 / 3.6 # [m / s] 57 | 58 | 59 | class Gear(Enum): 60 | GEAR_DRIVE = 1 61 | GEAR_REVERSE = 2 62 | 63 | 64 | class VehicleState: 65 | def __init__(self, x=0.0, y=0.0, yaw=0.0, 66 | v=0.0, gear=Gear.GEAR_DRIVE): 67 | self.x = x 68 | self.y = y 69 | self.yaw = yaw 70 | self.v = v 71 | self.e_cg = 0.0 72 | self.theta_e = 0.0 73 | 74 | self.gear = gear 75 | self.steer = 0.0 76 | 77 | def UpdateVehicleState(self, delta, a, e_cg, theta_e, 78 | gear=Gear.GEAR_DRIVE): 79 | """ 80 | update states of vehicle 81 | :param theta_e: yaw error to ref trajectory 82 | :param e_cg: lateral error to ref trajectory 83 | :param delta: steering angle [rad] 84 | :param a: acceleration [m / s^2] 85 | :param gear: gear mode [GEAR_DRIVE / GEAR/REVERSE] 86 | """ 87 | 88 | wheelbase_ = l_r + l_f 89 | delta, a = self.RegulateInput(delta, a) 90 | 91 | self.gear = gear 92 | self.steer = delta 93 | self.x += self.v * math.cos(self.yaw) * ts 94 | self.y += self.v * math.sin(self.yaw) * ts 95 | self.yaw += self.v / wheelbase_ * math.tan(delta) * ts 96 | self.e_cg = e_cg 97 | self.theta_e = theta_e 98 | 99 | if gear == Gear.GEAR_DRIVE: 100 | self.v += a * ts 101 | else: 102 | self.v += -1.0 * a * ts 103 | 104 | self.v = self.RegulateOutput(self.v) 105 | 106 | @staticmethod 107 | def RegulateInput(delta, a): 108 | """ 109 | regulate delta to : - max_steer_angle ~ max_steer_angle 110 | regulate a to : - max_acceleration ~ max_acceleration 111 | :param delta: steering angle [rad] 112 | :param a: acceleration [m / s^2] 113 | :return: regulated delta and acceleration 114 | """ 115 | 116 | if delta < -1.0 * max_steer_angle: 117 | delta = -1.0 * max_steer_angle 118 | 119 | if delta > 1.0 * max_steer_angle: 120 | delta = 1.0 * max_steer_angle 121 | 122 | if a < -1.0 * max_acceleration: 123 | a = -1.0 * max_acceleration 124 | 125 | if a > 1.0 * max_acceleration: 126 | a = 1.0 * max_acceleration 127 | 128 | return delta, a 129 | 130 | @staticmethod 131 | def RegulateOutput(v): 132 | """ 133 | regulate v to : -max_speed ~ max_speed 134 | :param v: calculated speed [m / s] 135 | :return: regulated speed 136 | """ 137 | 138 | max_speed_ = max_speed 139 | 140 | if v < -1.0 * max_speed_: 141 | v = -1.0 * max_speed_ 142 | 143 | if v > 1.0 * max_speed_: 144 | v = 1.0 * max_speed_ 145 | 146 | return v 147 | 148 | 149 | class TrajectoryAnalyzer: 150 | def __init__(self, x, y, yaw, k): 151 | self.x_ = x 152 | self.y_ = y 153 | self.yaw_ = yaw 154 | self.k_ = k 155 | 156 | self.ind_old = 0 157 | self.ind_end = len(x) 158 | 159 | def ToTrajectoryFrame(self, vehicle_state): 160 | """ 161 | errors to trajectory frame 162 | theta_e = yaw_vehicle - yaw_ref_path 163 | e_cg = lateral distance of center of gravity (cg) in frenet frame 164 | :param vehicle_state: vehicle state (class VehicleState) 165 | :return: theta_e, e_cg, yaw_ref, k_ref 166 | """ 167 | 168 | x_cg = vehicle_state.x 169 | y_cg = vehicle_state.y 170 | yaw = vehicle_state.yaw 171 | 172 | # calc nearest point in ref path 173 | dx = [x_cg - ix for ix in self.x_[self.ind_old: self.ind_end]] 174 | dy = [y_cg - iy for iy in self.y_[self.ind_old: self.ind_end]] 175 | 176 | ind_add = int(np.argmin(np.hypot(dx, dy))) 177 | dist = math.hypot(dx[ind_add], dy[ind_add]) 178 | 179 | # calc lateral relative position of vehicle to ref path 180 | vec_axle_rot_90 = np.array([[math.cos(yaw + math.pi / 2.0)], 181 | [math.sin(yaw + math.pi / 2.0)]]) 182 | 183 | vec_path_2_cg = np.array([[dx[ind_add]], 184 | [dy[ind_add]]]) 185 | 186 | if np.dot(vec_axle_rot_90.T, vec_path_2_cg) > 0.0: 187 | e_cg = 1.0 * dist # vehicle on the right of ref path 188 | else: 189 | e_cg = -1.0 * dist # vehicle on the left of ref path 190 | 191 | # calc yaw error: theta_e = yaw_vehicle - yaw_ref 192 | self.ind_old += ind_add 193 | yaw_ref = self.yaw_[self.ind_old] 194 | theta_e = pi_2_pi(yaw - yaw_ref) 195 | 196 | # calc ref curvature 197 | k_ref = self.k_[self.ind_old] 198 | 199 | return theta_e, e_cg, yaw_ref, k_ref 200 | 201 | 202 | class LatController: 203 | """ 204 | Lateral Controller using LQR 205 | """ 206 | 207 | def ComputeControlCommand(self, vehicle_state, ref_trajectory): 208 | """ 209 | calc lateral control command. 210 | :param vehicle_state: vehicle state 211 | :param ref_trajectory: reference trajectory (analyzer) 212 | :return: steering angle (optimal u), theta_e, e_cg 213 | """ 214 | 215 | ts_ = ts 216 | e_cg_old = vehicle_state.e_cg 217 | theta_e_old = vehicle_state.theta_e 218 | 219 | theta_e, e_cg, yaw_ref, k_ref = \ 220 | ref_trajectory.ToTrajectoryFrame(vehicle_state) 221 | 222 | matrix_ad_, matrix_bd_ = self.UpdateMatrix(vehicle_state) 223 | 224 | matrix_state_ = np.zeros((state_size, 1)) 225 | matrix_r_ = np.diag(matrix_r) 226 | matrix_q_ = np.diag(matrix_q) 227 | 228 | matrix_k_ = self.SolveLQRProblem(matrix_ad_, matrix_bd_, matrix_q_, 229 | matrix_r_, eps, max_iteration) 230 | 231 | matrix_state_[0][0] = e_cg 232 | matrix_state_[1][0] = (e_cg - e_cg_old) / ts_ 233 | matrix_state_[2][0] = theta_e 234 | matrix_state_[3][0] = (theta_e - theta_e_old) / ts_ 235 | 236 | steer_angle_feedback = -(matrix_k_ @ matrix_state_)[0][0] 237 | 238 | steer_angle_feedforward = self.ComputeFeedForward(k_ref) 239 | 240 | steer_angle = steer_angle_feedback + steer_angle_feedforward 241 | 242 | return steer_angle, theta_e, e_cg 243 | 244 | @staticmethod 245 | def ComputeFeedForward(ref_curvature): 246 | """ 247 | calc feedforward control term to decrease the steady error. 248 | :param ref_curvature: curvature of the target point in ref trajectory 249 | :return: feedforward term 250 | """ 251 | 252 | wheelbase_ = l_f + l_r 253 | 254 | steer_angle_feedforward = wheelbase_ * ref_curvature 255 | 256 | return steer_angle_feedforward 257 | 258 | @staticmethod 259 | def SolveLQRProblem(A, B, Q, R, tolerance, max_num_iteration): 260 | """ 261 | iteratively calculating feedback matrix K 262 | :param A: matrix_a_ 263 | :param B: matrix_b_ 264 | :param Q: matrix_q_ 265 | :param R: matrix_r_ 266 | :param tolerance: lqr_eps 267 | :param max_num_iteration: max_iteration 268 | :return: feedback matrix K 269 | """ 270 | 271 | assert np.size(A, 0) == np.size(A, 1) and \ 272 | np.size(B, 0) == np.size(A, 0) and \ 273 | np.size(Q, 0) == np.size(Q, 1) and \ 274 | np.size(Q, 0) == np.size(A, 1) and \ 275 | np.size(R, 0) == np.size(R, 1) and \ 276 | np.size(R, 0) == np.size(B, 1), \ 277 | "LQR solver: one or more matrices have incompatible dimensions." 278 | 279 | M = np.zeros((np.size(Q, 0), np.size(R, 1))) 280 | 281 | AT = A.T 282 | BT = B.T 283 | MT = M.T 284 | 285 | P = Q 286 | num_iteration = 0 287 | diff = math.inf 288 | 289 | while num_iteration < max_num_iteration and diff > tolerance: 290 | num_iteration += 1 291 | P_next = AT @ P @ A - (AT @ P @ B + M) @ \ 292 | np.linalg.pinv(R + BT @ P @ B) @ (BT @ P @ A + MT) + Q 293 | 294 | # check the difference between P and P_next 295 | diff = (abs(P_next - P)).max() 296 | P = P_next 297 | 298 | if num_iteration >= max_num_iteration: 299 | print("LQR solver cannot converge to a solution", 300 | "last consecutive result diff is: ", diff) 301 | 302 | K = np.linalg.inv(BT @ P @ B + R) @ (BT @ P @ A + MT) 303 | 304 | return K 305 | 306 | @staticmethod 307 | def UpdateMatrix(vehicle_state): 308 | """ 309 | calc A and b matrices of linearized, discrete system. 310 | :return: A, b 311 | """ 312 | 313 | ts_ = ts 314 | wheelbase_ = l_f + l_r 315 | 316 | v = vehicle_state.v 317 | 318 | matrix_ad_ = np.zeros((state_size, state_size)) # time discrete A matrix 319 | 320 | matrix_ad_[0][0] = 1.0 321 | matrix_ad_[0][1] = ts_ 322 | matrix_ad_[1][2] = v 323 | matrix_ad_[2][2] = 1.0 324 | matrix_ad_[2][3] = ts_ 325 | 326 | # b = [0.0, 0.0, 0.0, v / L].T 327 | matrix_bd_ = np.zeros((state_size, 1)) # time discrete b matrix 328 | matrix_bd_[3][0] = v / wheelbase_ 329 | 330 | return matrix_ad_, matrix_bd_ 331 | 332 | 333 | class LonController: 334 | """ 335 | Longitudinal Controller using PID. 336 | """ 337 | 338 | @staticmethod 339 | def ComputeControlCommand(target_speed, vehicle_state, dist): 340 | """ 341 | calc acceleration command using PID. 342 | :param target_speed: target speed [m / s] 343 | :param vehicle_state: vehicle state 344 | :param dist: distance to goal [m] 345 | :return: control command (acceleration) [m / s^2] 346 | """ 347 | 348 | if vehicle_state.gear == Gear.GEAR_DRIVE: 349 | direct = 1.0 350 | else: 351 | direct = -1.0 352 | 353 | a = 0.3 * (target_speed - direct * vehicle_state.v) 354 | 355 | if dist < 10.0: 356 | if vehicle_state.v > 2.0: 357 | a = -3.0 358 | elif vehicle_state.v < -2: 359 | a = -1.0 360 | 361 | return a 362 | 363 | 364 | def pi_2_pi(angle): 365 | """ 366 | regulate theta to -pi ~ pi. 367 | :param angle: input angle 368 | :return: regulated angle 369 | """ 370 | 371 | M_PI = math.pi 372 | 373 | if angle > M_PI: 374 | return angle - 2.0 * M_PI 375 | 376 | if angle < -M_PI: 377 | return angle + 2.0 * M_PI 378 | 379 | return angle 380 | 381 | 382 | def generate_path(s): 383 | """ 384 | design path using reeds-shepp path generator. 385 | divide paths into sections, in each section the direction is the same. 386 | :param s: objective positions and directions. 387 | :return: paths 388 | """ 389 | wheelbase_ = l_f + l_r 390 | 391 | max_c = math.tan(0.5 * max_steer_angle) / wheelbase_ 392 | path_x, path_y, yaw, direct, rc = [], [], [], [], [] 393 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = [], [], [], [], [] 394 | direct_flag = 1.0 395 | 396 | for i in range(len(s) - 1): 397 | s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2]) 398 | g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2]) 399 | 400 | path_i = rs.calc_optimal_path(s_x, s_y, s_yaw, 401 | g_x, g_y, g_yaw, max_c) 402 | 403 | irc, rds = rs.calc_curvature(path_i.x, path_i.y, path_i.yaw, path_i.directions) 404 | 405 | ix = path_i.x 406 | iy = path_i.y 407 | iyaw = path_i.yaw 408 | idirect = path_i.directions 409 | 410 | for j in range(len(ix)): 411 | if idirect[j] == direct_flag: 412 | x_rec.append(ix[j]) 413 | y_rec.append(iy[j]) 414 | yaw_rec.append(iyaw[j]) 415 | direct_rec.append(idirect[j]) 416 | rc_rec.append(irc[j]) 417 | else: 418 | if len(x_rec) == 0 or direct_rec[0] != direct_flag: 419 | direct_flag = idirect[j] 420 | continue 421 | 422 | path_x.append(x_rec) 423 | path_y.append(y_rec) 424 | yaw.append(yaw_rec) 425 | direct.append(direct_rec) 426 | rc.append(rc_rec) 427 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = \ 428 | [x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]], [rc_rec[-1]] 429 | 430 | path_x.append(x_rec) 431 | path_y.append(y_rec) 432 | yaw.append(yaw_rec) 433 | direct.append(direct_rec) 434 | rc.append(rc_rec) 435 | 436 | x_all, y_all = [], [] 437 | for ix, iy in zip(path_x, path_y): 438 | x_all += ix 439 | y_all += iy 440 | 441 | return path_x, path_y, yaw, direct, rc, x_all, y_all 442 | 443 | 444 | def main(): 445 | # generate path 446 | states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), 447 | (20, 0, 120), (5, -10, 180), (15, 5, 30)] 448 | # 449 | # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), 450 | # (35, 10, 180), (30, -10, 160), (5, -12, 90)] 451 | 452 | x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states) 453 | 454 | wheelbase_ = l_f + l_r 455 | 456 | maxTime = 100.0 457 | yaw_old = 0.0 458 | x0, y0, yaw0, direct0 = \ 459 | x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0] 460 | 461 | x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] 462 | 463 | lat_controller = LatController() 464 | lon_controller = LonController() 465 | 466 | for x, y, yaw, gear, k in zip(x_ref, y_ref, yaw_ref, direct, curv): 467 | t = 0.0 468 | 469 | if gear[0] == 1.0: 470 | direct = Gear.GEAR_DRIVE 471 | else: 472 | direct = Gear.GEAR_REVERSE 473 | 474 | ref_trajectory = TrajectoryAnalyzer(x, y, yaw, k) 475 | 476 | vehicle_state = VehicleState(x=x0, y=y0, yaw=yaw0, v=0.1, gear=direct) 477 | 478 | while t < maxTime: 479 | 480 | dist = math.hypot(vehicle_state.x - x[-1], vehicle_state.y - y[-1]) 481 | 482 | if gear[0] > 0: 483 | target_speed = 25.0 / 3.6 484 | else: 485 | target_speed = 15.0 / 3.6 486 | 487 | delta_opt, theta_e, e_cg = \ 488 | lat_controller.ComputeControlCommand(vehicle_state, ref_trajectory) 489 | 490 | a_opt = lon_controller.ComputeControlCommand(target_speed, vehicle_state, dist) 491 | 492 | vehicle_state.UpdateVehicleState(delta_opt, a_opt, e_cg, theta_e, direct) 493 | 494 | t += ts 495 | 496 | if dist <= 0.5: 497 | break 498 | 499 | x_rec.append(vehicle_state.x) 500 | y_rec.append(vehicle_state.y) 501 | yaw_rec.append(vehicle_state.yaw) 502 | 503 | dy = (vehicle_state.yaw - yaw_old) / (vehicle_state.v * ts) 504 | # steer = rs.pi_2_pi(-math.atan(wheelbase_ * dy)) 505 | 506 | yaw_old = vehicle_state.yaw 507 | x0 = x_rec[-1] 508 | y0 = y_rec[-1] 509 | yaw0 = yaw_rec[-1] 510 | 511 | plt.cla() 512 | plt.plot(x_all, y_all, color='gray', linewidth=2.0) 513 | plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet') 514 | # plt.plot(x[ind], y[ind], '.r') 515 | draw.draw_car(x0, y0, yaw0, -vehicle_state.steer) 516 | plt.axis("equal") 517 | plt.title("LQR (Kinematic): v=" + str(vehicle_state.v * 3.6)[:4] + "km/h") 518 | plt.gcf().canvas.mpl_connect('key_release_event', 519 | lambda event: 520 | [exit(0) if event.key == 'escape' else None]) 521 | plt.pause(0.001) 522 | 523 | plt.show() 524 | 525 | 526 | if __name__ == '__main__': 527 | main() 528 | -------------------------------------------------------------------------------- /Control/MPC_Frenet_Frame.py: -------------------------------------------------------------------------------- 1 | """ 2 | Linear MPC controller (Frenet frame) 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import cvxpy 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 14 | "/../../MotionPlanning/") 15 | 16 | import Control.draw as draw 17 | import CurvesGenerator.reeds_shepp as rs 18 | import CurvesGenerator.cubic_spline as cs 19 | 20 | 21 | class P: 22 | # System config 23 | NX = 5 # state vector: z = [e, e_dot, theta_e, theta_e_dot, v] 24 | NU = 2 # input vector: u = [acceleration, steer] 25 | T = 6 # finite time horizon length 26 | 27 | # MPC config 28 | Q = np.diag([1.0, 1.0, 1.0, 1.0, 1.0]) # penalty for states 29 | Qf = np.diag([1.0, 1.0, 1.0, 1.0, 1.0]) # penalty for end state 30 | R = np.diag([0.01, 0.1]) # penalty for inputs 31 | Rd = np.diag([0.01, 0.1]) # penalty for change of inputs 32 | 33 | dist_stop = 1.5 # stop permitted when dist to goal < dist_stop 34 | speed_stop = 0.5 / 3.6 # stop permitted when speed < speed_stop 35 | time_max = 500.0 # max simulation time 36 | iter_max = 5 # max iteration 37 | target_speed = 10.0 / 3.6 # target speed 38 | N_IND = 10 # search index number 39 | dt = 0.2 # time step 40 | d_dist = 1.0 # dist step 41 | du_res = 0.1 # threshold for stopping iteration 42 | 43 | # vehicle config 44 | RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 45 | RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 46 | W = 2.4 # [m] width of vehicle 47 | WD = 0.7 * W # [m] distance between left-right wheels 48 | WB = 2.5 # [m] Wheel base 49 | TR = 0.44 # [m] Tyre radius 50 | TW = 0.7 # [m] Tyre width 51 | 52 | steer_max = np.deg2rad(45.0) # max steering angle [rad] 53 | steer_change_max = np.deg2rad(30.0) # maximum steering speed [rad/s] 54 | speed_max = 55.0 / 3.6 # maximum speed [m/s] 55 | speed_min = -20.0 / 3.6 # minimum speed [m/s] 56 | acceleration_max = 1.0 # maximum acceleration [m/s2] 57 | 58 | 59 | class Node: 60 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direct=1.0): 61 | self.x = x 62 | self.y = y 63 | self.yaw = yaw 64 | self.v = v 65 | self.direct = direct 66 | 67 | def update(self, a, delta, direct): 68 | delta = self.limit_input_delta(delta) 69 | self.x += self.v * math.cos(self.yaw) * P.dt 70 | self.y += self.v * math.sin(self.yaw) * P.dt 71 | self.yaw += self.v / P.WB * math.tan(delta) * P.dt 72 | self.direct = direct 73 | self.v += self.direct * a * P.dt 74 | self.v = self.limit_speed(self.v) 75 | 76 | @staticmethod 77 | def limit_input_delta(delta): 78 | if delta >= P.steer_max: 79 | return P.steer_max 80 | 81 | if delta <= -P.steer_max: 82 | return -P.steer_max 83 | 84 | return delta 85 | 86 | @staticmethod 87 | def limit_speed(v): 88 | if v >= P.speed_max: 89 | return P.speed_max 90 | 91 | if v <= P.speed_min: 92 | return P.speed_min 93 | 94 | return v 95 | 96 | 97 | class PATH: 98 | def __init__(self, cx, cy, cyaw, ck): 99 | self.cx = cx 100 | self.cy = cy 101 | self.cyaw = cyaw 102 | self.ck = ck 103 | self.length = len(cx) 104 | self.ind_old = 0 105 | 106 | def calc_theta_e_and_er(self, node): 107 | dx = [node.x - x for x in self.cx[self.ind_old: (self.ind_old + P.N_IND)]] 108 | dy = [node.y - y for y in self.cy[self.ind_old: (self.ind_old + P.N_IND)]] 109 | dist = np.hypot(dx, dy) 110 | 111 | ind_in_N = int(np.argmin(dist)) 112 | ind = self.ind_old + ind_in_N 113 | self.ind_old = ind 114 | 115 | rear_axle_vec_rot_90 = np.array([[math.cos(node.yaw + math.pi / 2.0)], 116 | [math.sin(node.yaw + math.pi / 2.0)]]) 117 | 118 | vec_target_2_rear = np.array([[dx[ind_in_N]], 119 | [dy[ind_in_N]]]) 120 | 121 | er = np.dot(vec_target_2_rear.T, rear_axle_vec_rot_90) 122 | er = er[0][0] 123 | 124 | theta = node.yaw 125 | theta_p = self.cyaw[ind] 126 | theta_e = pi_2_pi(theta - theta_p) 127 | 128 | return theta_e, er, ind 129 | 130 | 131 | def calc_ref_trajectory_in_T_step(node, ref_path, sp): 132 | z_ref = np.zeros((P.NX, P.T + 1)) 133 | length = ref_path.length 134 | 135 | theta_e, er, ind = ref_path.calc_theta_e_and_er(node) 136 | 137 | z_ref[4, 0] = sp[ind] 138 | dist_move = 0.0 139 | 140 | for i in range(1, P.T + 1): 141 | dist_move += abs(node.v) * P.dt 142 | ind_move = int(round(dist_move / P.d_dist)) 143 | index = min(ind + ind_move, length - 1) 144 | 145 | z_ref[4, i] = sp[index] 146 | 147 | return z_ref, ind, theta_e, er 148 | 149 | 150 | def linear_mpc_control(z_ref, node0, z0, a_old, delta_old): 151 | if a_old is None or delta_old is None: 152 | a_old = [0.0] * P.T 153 | delta_old = [0.0] * P.T 154 | 155 | for k in range(P.iter_max): 156 | v_bar = predict_states_in_T_step(node0, a_old, delta_old) 157 | a_rec, delta_rec = a_old[:], delta_old[:] 158 | a_old, delta_old = solve_linear_mpc(z_ref, v_bar, z0) 159 | 160 | du_a_max = max([abs(ia - iao) for ia, iao in zip(a_old, a_rec)]) 161 | du_d_max = max([abs(ide - ido) for ide, ido in zip(delta_old, delta_rec)]) 162 | 163 | if max(du_a_max, du_d_max) < P.du_res: 164 | break 165 | 166 | return a_old, delta_old 167 | 168 | 169 | def solve_linear_mpc(z_ref, v_bar, z0): 170 | z = cvxpy.Variable((P.NX, P.T + 1)) 171 | u = cvxpy.Variable((P.NU, P.T)) 172 | 173 | cost = 0.0 174 | constrains = [] 175 | 176 | for t in range(P.T): 177 | cost += cvxpy.quad_form(u[:, t], P.R) 178 | cost += cvxpy.quad_form(z[:, t] - z_ref[:, t], P.Q) 179 | 180 | A, B = calc_linear_discrete_model(v_bar[t]) 181 | 182 | constrains += [z[:, t + 1] == A @ z[:, t] + B @ u[:, t]] 183 | 184 | if t < P.T - 1: 185 | cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], P.Rd) 186 | constrains += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= P.steer_change_max * P.dt] 187 | 188 | cost += cvxpy.quad_form(z_ref[:, P.T] - z[:, P.T], P.Qf) 189 | 190 | constrains += [z[:, 0] == z0] 191 | constrains += [z[4, :] <= P.speed_max] 192 | constrains += [z[4, :] >= P.speed_min] 193 | constrains += [cvxpy.abs(u[0, :]) <= P.acceleration_max] 194 | constrains += [cvxpy.abs(u[1, :]) <= P.steer_max] 195 | 196 | prob = cvxpy.Problem(cvxpy.Minimize(cost), constrains) 197 | prob.solve(solver=cvxpy.OSQP) 198 | 199 | a, delta = None, None 200 | 201 | if prob.status == cvxpy.OPTIMAL or \ 202 | prob.status == cvxpy.OPTIMAL_INACCURATE: 203 | a = u.value[0, :] 204 | delta = u.value[1, :] 205 | else: 206 | print("Cannot solve linear mpc!") 207 | 208 | print(delta) 209 | 210 | return a, delta 211 | 212 | 213 | def predict_states_in_T_step(node0, a, delta): 214 | v_bar = [0.0] * (P.T + 1) 215 | v_bar[0] = node0.v 216 | 217 | for ai, di, i in zip(a, delta, range(1, P.T + 1)): 218 | node0.update(ai, di, 1.0) 219 | v_bar[i] = node0.v 220 | 221 | return v_bar 222 | 223 | 224 | def calc_linear_discrete_model(v): 225 | A = np.array([[1.0, P.dt, 0.0, 0.0, 0.0], 226 | [0.0, 0.0, v, 0.0, 0.0], 227 | [0.0, 0.0, 1.0, P.dt, 0.0], 228 | [0.0, 0.0, 0.0, 0.0, 0.0], 229 | [0.0, 0.0, 0.0, 0.0, 1.0]]) 230 | 231 | B = np.array([[0.0, 0.0], 232 | [0.0, 0.0], 233 | [0.0, 0.0], 234 | [v / P.WB, 0.0], 235 | [0.0, P.dt]]) 236 | 237 | return A, B 238 | 239 | 240 | def calc_speed_profile(cx, cy, cyaw, target_speed): 241 | speed_profile = [target_speed] * len(cx) 242 | direction = 1.0 # forward 243 | 244 | # Set stop point 245 | for i in range(len(cx) - 1): 246 | dx = cx[i + 1] - cx[i] 247 | dy = cy[i + 1] - cy[i] 248 | 249 | move_direction = math.atan2(dy, dx) 250 | 251 | if dx != 0.0 and dy != 0.0: 252 | dangle = abs(pi_2_pi(move_direction - cyaw[i])) 253 | if dangle >= math.pi / 4.0: 254 | direction = -1.0 255 | else: 256 | direction = 1.0 257 | 258 | if direction != 1.0: 259 | speed_profile[i] = - target_speed 260 | else: 261 | speed_profile[i] = target_speed 262 | 263 | speed_profile[-1] = 0.0 264 | 265 | return speed_profile 266 | 267 | 268 | def pi_2_pi(angle): 269 | if angle > math.pi: 270 | return angle - 2.0 * math.pi 271 | 272 | if angle < -math.pi: 273 | return angle + 2.0 * math.pi 274 | 275 | return angle 276 | 277 | 278 | def main(): 279 | ax = [0.0, 20.0, 40.0, 55.0, 70.0, 85.0] 280 | ay = [0.0, 50.0, 20.0, 35.0, 0.0, 10.0] 281 | 282 | cx, cy, cyaw, ck, s = \ 283 | cs.calc_spline_course(ax, ay, ds=P.d_dist) 284 | 285 | sp = calc_speed_profile(cx, cy, cyaw, P.target_speed) 286 | 287 | ref_path = PATH(cx, cy, cyaw, ck) 288 | node = Node(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) 289 | 290 | time = 0.0 291 | x = [node.x] 292 | y = [node.y] 293 | yaw = [node.yaw] 294 | v = [node.v] 295 | t = [0.0] 296 | d = [0.0] 297 | a = [0.0] 298 | 299 | delta_opt, a_opt = None, None 300 | a_exc, delta_exc = 0.0, 0.0 301 | 302 | while time < P.time_max: 303 | z_ref, target_ind, theta_e, er = \ 304 | calc_ref_trajectory_in_T_step(node, ref_path, sp) 305 | 306 | node0 = Node(x=node.x, y=node.y, yaw=node.yaw, v=node.v) 307 | z0 = [er, 0.0, theta_e, 0.0, node.v] 308 | 309 | a_opt, delta_opt = \ 310 | linear_mpc_control(z_ref, node0, z0, a_opt, delta_opt) 311 | 312 | # node_opt = Node(x=node.x, y=node.y, yaw=node.yaw, v=node.v) 313 | # x_opt, y_opt = [node_opt.x], [node_opt.y] 314 | 315 | if delta_opt is not None: 316 | delta_exc, a_exc = delta_opt[0], a_opt[0] 317 | 318 | # for ao, do in zip(a_opt, delta_opt): 319 | # node_opt.update(ao, do, 1.0) 320 | # x_opt.append(node_opt.x) 321 | # y_opt.append(node_opt.y) 322 | 323 | node.update(a_exc, delta_exc, 1.0) 324 | time += P.dt 325 | 326 | x.append(node.x) 327 | y.append(node.y) 328 | yaw.append(node.yaw) 329 | v.append(node.v) 330 | t.append(time) 331 | d.append(delta_exc) 332 | a.append(a_exc) 333 | 334 | dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) 335 | 336 | if dist < P.dist_stop and \ 337 | abs(node.v) < P.speed_stop: 338 | break 339 | 340 | plt.cla() 341 | plt.gcf().canvas.mpl_connect('key_release_event', 342 | lambda event: 343 | [exit(0) if event.key == 'escape' else None]) 344 | 345 | # if x_opt is not None: 346 | # plt.plot(x_opt, y_opt, 'xr') 347 | 348 | plt.plot(cx, cy, '-r') 349 | plt.plot(x, y, '-b') 350 | plt.plot(z_ref[0, :], z_ref[1, :], 'xk') 351 | plt.plot(cx[target_ind], cy[target_ind], 'xg') 352 | plt.axis("equal") 353 | plt.title("Linear MPC, " + "v = " + str(round(node.v * 3.6, 2))) 354 | plt.pause(0.001) 355 | 356 | plt.show() 357 | 358 | 359 | if __name__ == '__main__': 360 | main() -------------------------------------------------------------------------------- /Control/MPC_XY_Frame.py: -------------------------------------------------------------------------------- 1 | """ 2 | Linear MPC controller (X-Y frame) 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import cvxpy 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 14 | "/../../MotionPlanning/") 15 | 16 | import Control.draw as draw 17 | import CurvesGenerator.reeds_shepp as rs 18 | import CurvesGenerator.cubic_spline as cs 19 | 20 | 21 | class P: 22 | # System config 23 | NX = 4 # state vector: z = [x, y, v, phi] 24 | NU = 2 # input vector: u = [acceleration, steer] 25 | T = 6 # finite time horizon length 26 | 27 | # MPC config 28 | Q = np.diag([1.0, 1.0, 1.0, 1.0]) # penalty for states 29 | Qf = np.diag([1.0, 1.0, 1.0, 1.0]) # penalty for end state 30 | R = np.diag([0.01, 0.1]) # penalty for inputs 31 | Rd = np.diag([0.01, 0.1]) # penalty for change of inputs 32 | 33 | dist_stop = 1.5 # stop permitted when dist to goal < dist_stop 34 | speed_stop = 0.5 / 3.6 # stop permitted when speed < speed_stop 35 | time_max = 500.0 # max simulation time 36 | iter_max = 5 # max iteration 37 | target_speed = 10.0 / 3.6 # target speed 38 | N_IND = 10 # search index number 39 | dt = 0.2 # time step 40 | d_dist = 1.0 # dist step 41 | du_res = 0.1 # threshold for stopping iteration 42 | 43 | # vehicle config 44 | RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 45 | RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 46 | W = 2.4 # [m] width of vehicle 47 | WD = 0.7 * W # [m] distance between left-right wheels 48 | WB = 2.5 # [m] Wheel base 49 | TR = 0.44 # [m] Tyre radius 50 | TW = 0.7 # [m] Tyre width 51 | 52 | steer_max = np.deg2rad(45.0) # max steering angle [rad] 53 | steer_change_max = np.deg2rad(30.0) # maximum steering speed [rad/s] 54 | speed_max = 55.0 / 3.6 # maximum speed [m/s] 55 | speed_min = -20.0 / 3.6 # minimum speed [m/s] 56 | acceleration_max = 1.0 # maximum acceleration [m/s2] 57 | 58 | 59 | class Node: 60 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direct=1.0): 61 | self.x = x 62 | self.y = y 63 | self.yaw = yaw 64 | self.v = v 65 | self.direct = direct 66 | 67 | def update(self, a, delta, direct): 68 | delta = self.limit_input_delta(delta) 69 | self.x += self.v * math.cos(self.yaw) * P.dt 70 | self.y += self.v * math.sin(self.yaw) * P.dt 71 | self.yaw += self.v / P.WB * math.tan(delta) * P.dt 72 | self.direct = direct 73 | self.v += self.direct * a * P.dt 74 | self.v = self.limit_speed(self.v) 75 | 76 | @staticmethod 77 | def limit_input_delta(delta): 78 | if delta >= P.steer_max: 79 | return P.steer_max 80 | 81 | if delta <= -P.steer_max: 82 | return -P.steer_max 83 | 84 | return delta 85 | 86 | @staticmethod 87 | def limit_speed(v): 88 | if v >= P.speed_max: 89 | return P.speed_max 90 | 91 | if v <= P.speed_min: 92 | return P.speed_min 93 | 94 | return v 95 | 96 | 97 | class PATH: 98 | def __init__(self, cx, cy, cyaw, ck): 99 | self.cx = cx 100 | self.cy = cy 101 | self.cyaw = cyaw 102 | self.ck = ck 103 | self.length = len(cx) 104 | self.ind_old = 0 105 | 106 | def nearest_index(self, node): 107 | """ 108 | calc index of the nearest node in N steps 109 | :param node: current information 110 | :return: nearest index, lateral distance to ref point 111 | """ 112 | 113 | dx = [node.x - x for x in self.cx[self.ind_old: (self.ind_old + P.N_IND)]] 114 | dy = [node.y - y for y in self.cy[self.ind_old: (self.ind_old + P.N_IND)]] 115 | dist = np.hypot(dx, dy) 116 | 117 | ind_in_N = int(np.argmin(dist)) 118 | ind = self.ind_old + ind_in_N 119 | self.ind_old = ind 120 | 121 | rear_axle_vec_rot_90 = np.array([[math.cos(node.yaw + math.pi / 2.0)], 122 | [math.sin(node.yaw + math.pi / 2.0)]]) 123 | 124 | vec_target_2_rear = np.array([[dx[ind_in_N]], 125 | [dy[ind_in_N]]]) 126 | 127 | er = np.dot(vec_target_2_rear.T, rear_axle_vec_rot_90) 128 | er = er[0][0] 129 | 130 | return ind, er 131 | 132 | 133 | def calc_ref_trajectory_in_T_step(node, ref_path, sp): 134 | """ 135 | calc referent trajectory in T steps: [x, y, v, yaw] 136 | using the current velocity, calc the T points along the reference path 137 | :param node: current information 138 | :param ref_path: reference path: [x, y, yaw] 139 | :param sp: speed profile (designed speed strategy) 140 | :return: reference trajectory 141 | """ 142 | 143 | z_ref = np.zeros((P.NX, P.T + 1)) 144 | length = ref_path.length 145 | 146 | ind, _ = ref_path.nearest_index(node) 147 | 148 | z_ref[0, 0] = ref_path.cx[ind] 149 | z_ref[1, 0] = ref_path.cy[ind] 150 | z_ref[2, 0] = sp[ind] 151 | z_ref[3, 0] = ref_path.cyaw[ind] 152 | 153 | dist_move = 0.0 154 | 155 | for i in range(1, P.T + 1): 156 | dist_move += abs(node.v) * P.dt 157 | ind_move = int(round(dist_move / P.d_dist)) 158 | index = min(ind + ind_move, length - 1) 159 | 160 | z_ref[0, i] = ref_path.cx[index] 161 | z_ref[1, i] = ref_path.cy[index] 162 | z_ref[2, i] = sp[index] 163 | z_ref[3, i] = ref_path.cyaw[index] 164 | 165 | return z_ref, ind 166 | 167 | 168 | def linear_mpc_control(z_ref, z0, a_old, delta_old): 169 | """ 170 | linear mpc controller 171 | :param z_ref: reference trajectory in T steps 172 | :param z0: initial state vector 173 | :param a_old: acceleration of T steps of last time 174 | :param delta_old: delta of T steps of last time 175 | :return: acceleration and delta strategy based on current information 176 | """ 177 | 178 | if a_old is None or delta_old is None: 179 | a_old = [0.0] * P.T 180 | delta_old = [0.0] * P.T 181 | 182 | x, y, yaw, v = None, None, None, None 183 | 184 | for k in range(P.iter_max): 185 | z_bar = predict_states_in_T_step(z0, a_old, delta_old, z_ref) 186 | a_rec, delta_rec = a_old[:], delta_old[:] 187 | a_old, delta_old, x, y, yaw, v = solve_linear_mpc(z_ref, z_bar, z0, delta_old) 188 | 189 | du_a_max = max([abs(ia - iao) for ia, iao in zip(a_old, a_rec)]) 190 | du_d_max = max([abs(ide - ido) for ide, ido in zip(delta_old, delta_rec)]) 191 | 192 | if max(du_a_max, du_d_max) < P.du_res: 193 | break 194 | 195 | return a_old, delta_old, x, y, yaw, v 196 | 197 | 198 | def predict_states_in_T_step(z0, a, delta, z_ref): 199 | """ 200 | given the current state, using the acceleration and delta strategy of last time, 201 | predict the states of vehicle in T steps. 202 | :param z0: initial state 203 | :param a: acceleration strategy of last time 204 | :param delta: delta strategy of last time 205 | :param z_ref: reference trajectory 206 | :return: predict states in T steps (z_bar, used for calc linear motion model) 207 | """ 208 | 209 | z_bar = z_ref * 0.0 210 | 211 | for i in range(P.NX): 212 | z_bar[i, 0] = z0[i] 213 | 214 | node = Node(x=z0[0], y=z0[1], v=z0[2], yaw=z0[3]) 215 | 216 | for ai, di, i in zip(a, delta, range(1, P.T + 1)): 217 | node.update(ai, di, 1.0) 218 | z_bar[0, i] = node.x 219 | z_bar[1, i] = node.y 220 | z_bar[2, i] = node.v 221 | z_bar[3, i] = node.yaw 222 | 223 | return z_bar 224 | 225 | 226 | def calc_linear_discrete_model(v, phi, delta): 227 | """ 228 | calc linear and discrete time dynamic model. 229 | :param v: speed: v_bar 230 | :param phi: angle of vehicle: phi_bar 231 | :param delta: steering angle: delta_bar 232 | :return: A, B, C 233 | """ 234 | 235 | A = np.array([[1.0, 0.0, P.dt * math.cos(phi), - P.dt * v * math.sin(phi)], 236 | [0.0, 1.0, P.dt * math.sin(phi), P.dt * v * math.cos(phi)], 237 | [0.0, 0.0, 1.0, 0.0], 238 | [0.0, 0.0, P.dt * math.tan(delta) / P.WB, 1.0]]) 239 | 240 | B = np.array([[0.0, 0.0], 241 | [0.0, 0.0], 242 | [P.dt, 0.0], 243 | [0.0, P.dt * v / (P.WB * math.cos(delta) ** 2)]]) 244 | 245 | C = np.array([P.dt * v * math.sin(phi) * phi, 246 | -P.dt * v * math.cos(phi) * phi, 247 | 0.0, 248 | -P.dt * v * delta / (P.WB * math.cos(delta) ** 2)]) 249 | 250 | return A, B, C 251 | 252 | 253 | def solve_linear_mpc(z_ref, z_bar, z0, d_bar): 254 | """ 255 | solve the quadratic optimization problem using cvxpy, solver: OSQP 256 | :param z_ref: reference trajectory (desired trajectory: [x, y, v, yaw]) 257 | :param z_bar: predicted states in T steps 258 | :param z0: initial state 259 | :param d_bar: delta_bar 260 | :return: optimal acceleration and steering strategy 261 | """ 262 | 263 | z = cvxpy.Variable((P.NX, P.T + 1)) 264 | u = cvxpy.Variable((P.NU, P.T)) 265 | 266 | cost = 0.0 267 | constrains = [] 268 | 269 | for t in range(P.T): 270 | cost += cvxpy.quad_form(u[:, t], P.R) 271 | cost += cvxpy.quad_form(z_ref[:, t] - z[:, t], P.Q) 272 | 273 | A, B, C = calc_linear_discrete_model(z_bar[2, t], z_bar[3, t], d_bar[t]) 274 | 275 | constrains += [z[:, t + 1] == A @ z[:, t] + B @ u[:, t] + C] 276 | 277 | if t < P.T - 1: 278 | cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], P.Rd) 279 | constrains += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= P.steer_change_max * P.dt] 280 | 281 | cost += cvxpy.quad_form(z_ref[:, P.T] - z[:, P.T], P.Qf) 282 | 283 | constrains += [z[:, 0] == z0] 284 | constrains += [z[2, :] <= P.speed_max] 285 | constrains += [z[2, :] >= P.speed_min] 286 | constrains += [cvxpy.abs(u[0, :]) <= P.acceleration_max] 287 | constrains += [cvxpy.abs(u[1, :]) <= P.steer_max] 288 | 289 | prob = cvxpy.Problem(cvxpy.Minimize(cost), constrains) 290 | prob.solve(solver=cvxpy.OSQP) 291 | 292 | a, delta, x, y, yaw, v = None, None, None, None, None, None 293 | 294 | if prob.status == cvxpy.OPTIMAL or \ 295 | prob.status == cvxpy.OPTIMAL_INACCURATE: 296 | x = z.value[0, :] 297 | y = z.value[1, :] 298 | v = z.value[2, :] 299 | yaw = z.value[3, :] 300 | a = u.value[0, :] 301 | delta = u.value[1, :] 302 | else: 303 | print("Cannot solve linear mpc!") 304 | 305 | return a, delta, x, y, yaw, v 306 | 307 | 308 | def calc_speed_profile(cx, cy, cyaw, target_speed): 309 | """ 310 | design appropriate speed strategy 311 | :param cx: x of reference path [m] 312 | :param cy: y of reference path [m] 313 | :param cyaw: yaw of reference path [m] 314 | :param target_speed: target speed [m/s] 315 | :return: speed profile 316 | """ 317 | 318 | speed_profile = [target_speed] * len(cx) 319 | direction = 1.0 # forward 320 | 321 | # Set stop point 322 | for i in range(len(cx) - 1): 323 | dx = cx[i + 1] - cx[i] 324 | dy = cy[i + 1] - cy[i] 325 | 326 | move_direction = math.atan2(dy, dx) 327 | 328 | if dx != 0.0 and dy != 0.0: 329 | dangle = abs(pi_2_pi(move_direction - cyaw[i])) 330 | if dangle >= math.pi / 4.0: 331 | direction = -1.0 332 | else: 333 | direction = 1.0 334 | 335 | if direction != 1.0: 336 | speed_profile[i] = - target_speed 337 | else: 338 | speed_profile[i] = target_speed 339 | 340 | speed_profile[-1] = 0.0 341 | 342 | return speed_profile 343 | 344 | 345 | def pi_2_pi(angle): 346 | if angle > math.pi: 347 | return angle - 2.0 * math.pi 348 | 349 | if angle < -math.pi: 350 | return angle + 2.0 * math.pi 351 | 352 | return angle 353 | 354 | 355 | def main(): 356 | ax = [0.0, 15.0, 30.0, 50.0, 60.0] 357 | ay = [0.0, 40.0, 15.0, 30.0, 0.0] 358 | cx, cy, cyaw, ck, s = cs.calc_spline_course( 359 | ax, ay, ds=P.d_dist) 360 | 361 | sp = calc_speed_profile(cx, cy, cyaw, P.target_speed) 362 | 363 | ref_path = PATH(cx, cy, cyaw, ck) 364 | node = Node(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) 365 | 366 | time = 0.0 367 | x = [node.x] 368 | y = [node.y] 369 | yaw = [node.yaw] 370 | v = [node.v] 371 | t = [0.0] 372 | d = [0.0] 373 | a = [0.0] 374 | 375 | delta_opt, a_opt = None, None 376 | a_exc, delta_exc = 0.0, 0.0 377 | 378 | while time < P.time_max: 379 | z_ref, target_ind = \ 380 | calc_ref_trajectory_in_T_step(node, ref_path, sp) 381 | 382 | z0 = [node.x, node.y, node.v, node.yaw] 383 | 384 | a_opt, delta_opt, x_opt, y_opt, yaw_opt, v_opt = \ 385 | linear_mpc_control(z_ref, z0, a_opt, delta_opt) 386 | 387 | if delta_opt is not None: 388 | delta_exc, a_exc = delta_opt[0], a_opt[0] 389 | 390 | node.update(a_exc, delta_exc, 1.0) 391 | time += P.dt 392 | 393 | x.append(node.x) 394 | y.append(node.y) 395 | yaw.append(node.yaw) 396 | v.append(node.v) 397 | t.append(time) 398 | d.append(delta_exc) 399 | a.append(a_exc) 400 | 401 | dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) 402 | 403 | if dist < P.dist_stop and \ 404 | abs(node.v) < P.speed_stop: 405 | break 406 | 407 | dy = (node.yaw - yaw[-2]) / (node.v * P.dt) 408 | steer = rs.pi_2_pi(-math.atan(P.WB * dy)) 409 | 410 | plt.cla() 411 | draw.draw_car(node.x, node.y, node.yaw, steer, P) 412 | plt.gcf().canvas.mpl_connect('key_release_event', 413 | lambda event: 414 | [exit(0) if event.key == 'escape' else None]) 415 | 416 | if x_opt is not None: 417 | plt.plot(x_opt, y_opt, color='darkviolet', marker='*') 418 | 419 | plt.plot(cx, cy, color='gray') 420 | plt.plot(x, y, '-b') 421 | plt.plot(cx[target_ind], cy[target_ind]) 422 | plt.axis("equal") 423 | plt.title("Linear MPC, " + "v = " + str(round(node.v * 3.6, 2))) 424 | plt.pause(0.001) 425 | 426 | plt.show() 427 | 428 | 429 | if __name__ == '__main__': 430 | main() 431 | -------------------------------------------------------------------------------- /Control/Pure_Pursuit.py: -------------------------------------------------------------------------------- 1 | """ 2 | Pure Pursuit 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 13 | "/../../MotionPlanning/") 14 | 15 | import Control.draw as draw 16 | import CurvesGenerator.reeds_shepp as rs 17 | 18 | 19 | class C: 20 | # PID config 21 | Kp = 0.3 # proportional gain 22 | 23 | # system config 24 | Ld = 2.6 # look ahead distance 25 | kf = 0.1 # look forward gain 26 | dt = 0.1 # T step 27 | dist_stop = 0.7 # stop distance 28 | dc = 0.0 29 | 30 | # vehicle config 31 | RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 32 | RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 33 | W = 2.4 # [m] width of vehicle 34 | WD = 0.7 * W # [m] distance between left-right wheels 35 | WB = 2.5 # [m] Wheel base 36 | TR = 0.44 # [m] Tyre radius 37 | TW = 0.7 # [m] Tyre width 38 | MAX_STEER = 0.30 39 | MAX_ACCELERATION = 5.0 40 | 41 | 42 | class Node: 43 | def __init__(self, x, y, yaw, v, direct): 44 | self.x = x 45 | self.y = y 46 | self.yaw = yaw 47 | self.v = v 48 | self.direct = direct 49 | 50 | def update(self, a, delta, direct): 51 | # delta = self.limit_input(delta) 52 | self.x += self.v * math.cos(self.yaw) * C.dt 53 | self.y += self.v * math.sin(self.yaw) * C.dt 54 | self.yaw += self.v / C.WB * math.tan(delta) * C.dt 55 | self.direct = direct 56 | self.v += self.direct * a * C.dt 57 | 58 | @staticmethod 59 | def limit_input(delta): 60 | if delta > 1.2 * C.MAX_STEER: 61 | return 1.2 * C.MAX_STEER 62 | 63 | if delta < -1.2 * C.MAX_STEER: 64 | return -1.2 * C.MAX_STEER 65 | 66 | return delta 67 | 68 | 69 | class Nodes: 70 | def __init__(self): 71 | self.x = [] 72 | self.y = [] 73 | self.yaw = [] 74 | self.v = [] 75 | self.t = [] 76 | self.direct = [] 77 | 78 | def add(self, t, node): 79 | self.x.append(node.x) 80 | self.y.append(node.y) 81 | self.yaw.append(node.yaw) 82 | self.v.append(node.v) 83 | self.t.append(t) 84 | self.direct.append(node.direct) 85 | 86 | 87 | class PATH: 88 | def __init__(self, cx, cy): 89 | self.cx = cx 90 | self.cy = cy 91 | self.ind_end = len(self.cx) - 1 92 | self.index_old = None 93 | 94 | def target_index(self, node): 95 | """ 96 | search index of target point in the reference path. 97 | the distance between target point and current position is ld 98 | :param node: current information 99 | :return: index of target point 100 | """ 101 | 102 | if self.index_old is None: 103 | self.calc_nearest_ind(node) 104 | 105 | Lf = C.kf * node.v + C.Ld 106 | 107 | for ind in range(self.index_old, self.ind_end + 1): 108 | if self.calc_distance(node, ind) > Lf: 109 | self.index_old = ind 110 | return ind, Lf 111 | 112 | self.index_old = self.ind_end 113 | 114 | return self.ind_end, Lf 115 | 116 | def calc_nearest_ind(self, node): 117 | """ 118 | calc index of the nearest point to current position 119 | :param node: current information 120 | :return: index of nearest point 121 | """ 122 | 123 | dx = [node.x - x for x in self.cx] 124 | dy = [node.y - y for y in self.cy] 125 | ind = np.argmin(np.hypot(dx, dy)) 126 | self.index_old = ind 127 | 128 | def calc_distance(self, node, ind): 129 | return math.hypot(node.x - self.cx[ind], node.y - self.cy[ind]) 130 | 131 | 132 | def pure_pursuit(node, ref_path, index_old): 133 | """ 134 | pure pursuit controller 135 | :param node: current information 136 | :param ref_path: reference path: x, y, yaw, curvature 137 | :param index_old: target index of last time 138 | :return: optimal steering angle 139 | """ 140 | 141 | ind, Lf = ref_path.target_index(node) # target point and pursuit distance 142 | ind = max(ind, index_old) 143 | 144 | tx = ref_path.cx[ind] 145 | ty = ref_path.cy[ind] 146 | 147 | alpha = math.atan2(ty - node.y, tx - node.x) - node.yaw 148 | delta = math.atan2(2.0 * C.WB * math.sin(alpha), Lf) 149 | 150 | return delta, ind 151 | 152 | 153 | def pid_control(target_v, v, dist, direct): 154 | """ 155 | PID controller and design speed profile. 156 | :param target_v: target speed (forward and backward are different) 157 | :param v: current speed 158 | :param dist: distance from current position to end position 159 | :param direct: current direction 160 | :return: desired acceleration 161 | """ 162 | 163 | a = 0.3 * (target_v - direct * v) 164 | 165 | if dist < 10.0: 166 | if v > 3.0: 167 | a = -2.5 168 | elif v < -2.0: 169 | a = -1.0 170 | 171 | return a 172 | 173 | 174 | def generate_path(s): 175 | """ 176 | divide paths into some sections, in each section, the direction is the same. 177 | :param s: target position and yaw 178 | :return: sections 179 | """ 180 | 181 | max_c = math.tan(C.MAX_STEER) / C.WB # max curvature 182 | 183 | path_x, path_y, yaw, direct = [], [], [], [] 184 | x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] 185 | direct_flag = 1.0 186 | 187 | for i in range(len(s) - 1): 188 | s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2]) 189 | g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2]) 190 | 191 | path_i = rs.calc_optimal_path(s_x, s_y, s_yaw, 192 | g_x, g_y, g_yaw, max_c) 193 | 194 | ix = path_i.x 195 | iy = path_i.y 196 | iyaw = path_i.yaw 197 | idirect = path_i.directions 198 | 199 | for j in range(len(ix)): 200 | if idirect[j] == direct_flag: 201 | x_rec.append(ix[j]) 202 | y_rec.append(iy[j]) 203 | yaw_rec.append(iyaw[j]) 204 | direct_rec.append(idirect[j]) 205 | else: 206 | if len(x_rec) == 0 or direct_rec[0] != direct_flag: 207 | direct_flag = idirect[j] 208 | continue 209 | 210 | path_x.append(x_rec) 211 | path_y.append(y_rec) 212 | yaw.append(yaw_rec) 213 | direct.append(direct_rec) 214 | x_rec, y_rec, yaw_rec, direct_rec = \ 215 | [x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]] 216 | 217 | path_x.append(x_rec) 218 | path_y.append(y_rec) 219 | yaw.append(yaw_rec) 220 | direct.append(direct_rec) 221 | 222 | x_all, y_all = [], [] 223 | 224 | for ix, iy in zip(path_x, path_y): 225 | x_all += ix 226 | y_all += iy 227 | 228 | return path_x, path_y, yaw, direct, x_all, y_all 229 | 230 | 231 | def main(): 232 | # generate path: [x, y, yaw] 233 | states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), 234 | (20, 0, 120), (5, -10, 180), (15, 5, 30)] 235 | 236 | # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), 237 | # (35, 10, 180), (30, -10, 160), (5, -12, 90)] 238 | 239 | x, y, yaw, direct, path_x, path_y = generate_path(states) 240 | 241 | # simulation 242 | maxTime = 100.0 243 | yaw_old = 0.0 244 | x0, y0, yaw0, direct0 = x[0][0], y[0][0], yaw[0][0], direct[0][0] 245 | x_rec, y_rec = [], [] 246 | 247 | for cx, cy, cyaw, cdirect in zip(x, y, yaw, direct): 248 | t = 0.0 249 | node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=direct0) 250 | nodes = Nodes() 251 | nodes.add(t, node) 252 | ref_trajectory = PATH(cx, cy) 253 | target_ind, _ = ref_trajectory.target_index(node) 254 | 255 | while t <= maxTime: 256 | if cdirect[0] > 0: 257 | target_speed = 30.0 / 3.6 258 | C.Ld = 4.0 259 | C.dist_stop = 1.5 260 | C.dc = -1.1 261 | else: 262 | target_speed = 20.0 / 3.6 263 | C.Ld = 2.5 264 | C.dist_stop = 0.2 265 | C.dc = 0.2 266 | 267 | xt = node.x + C.dc * math.cos(node.yaw) 268 | yt = node.y + C.dc * math.sin(node.yaw) 269 | dist = math.hypot(xt - cx[-1], yt - cy[-1]) 270 | 271 | if dist < C.dist_stop: 272 | break 273 | 274 | acceleration = pid_control(target_speed, node.v, dist, cdirect[0]) 275 | delta, target_ind = pure_pursuit(node, ref_trajectory, target_ind) 276 | 277 | t += C.dt 278 | 279 | node.update(acceleration, delta, cdirect[0]) 280 | nodes.add(t, node) 281 | x_rec.append(node.x) 282 | y_rec.append(node.y) 283 | 284 | dy = (node.yaw - yaw_old) / (node.v * C.dt) 285 | steer = rs.pi_2_pi(-math.atan(C.WB * dy)) 286 | 287 | yaw_old = node.yaw 288 | x0 = nodes.x[-1] 289 | y0 = nodes.y[-1] 290 | yaw0 = nodes.yaw[-1] 291 | direct0 = nodes.direct[-1] 292 | 293 | # animation 294 | plt.cla() 295 | plt.plot(node.x, node.y, marker='.', color='k') 296 | plt.plot(path_x, path_y, color='gray', linewidth=2) 297 | plt.plot(x_rec, y_rec, color='darkviolet', linewidth=2) 298 | plt.plot(cx[target_ind], cy[target_ind], ".r") 299 | draw.draw_car(node.x, node.y, yaw_old, steer, C) 300 | 301 | # for m in range(len(states)): 302 | # draw.Arrow(states[m][0], states[m][1], np.deg2rad(states[m][2]), 2, 'blue') 303 | 304 | plt.axis("equal") 305 | plt.title("PurePursuit: v=" + str(node.v * 3.6)[:4] + "km/h") 306 | plt.gcf().canvas.mpl_connect('key_release_event', 307 | lambda event: 308 | [exit(0) if event.key == 'escape' else None]) 309 | plt.pause(0.001) 310 | 311 | plt.show() 312 | 313 | 314 | if __name__ == '__main__': 315 | main() 316 | -------------------------------------------------------------------------------- /Control/Rear_Wheel_Feedback.py: -------------------------------------------------------------------------------- 1 | """ 2 | Rear-Wheel Feedback Controller 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 13 | "/../../MotionPlanning/") 14 | 15 | import Control.draw as draw 16 | import CurvesGenerator.reeds_shepp as rs 17 | 18 | 19 | class C: 20 | # PID config 21 | Kp = 1.0 22 | 23 | # System config 24 | K_theta = 1.0 25 | K_e = 0.5 26 | dt = 0.1 27 | dist_stop = 0.2 28 | 29 | # vehicle config 30 | RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 31 | RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 32 | W = 2.4 # [m] width of vehicle 33 | WD = 0.7 * W # [m] distance between left-right wheels 34 | WB = 2.5 # [m] Wheel base 35 | TR = 0.44 # [m] Tyre radius 36 | TW = 0.7 # [m] Tyre width 37 | MAX_STEER = 0.25 38 | 39 | 40 | class Node: 41 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, direct=1.0): 42 | self.x = x 43 | self.y = y 44 | self.yaw = yaw 45 | self.v = v 46 | self.direct = direct 47 | 48 | def update(self, a, delta, direct): 49 | self.x += self.v * math.cos(self.yaw) * C.dt 50 | self.y += self.v * math.sin(self.yaw) * C.dt 51 | self.yaw += self.v / C.WB * math.tan(delta) * C.dt 52 | self.direct = direct 53 | self.v += self.direct * a * C.dt 54 | 55 | 56 | class PATH: 57 | def __init__(self, cx, cy, cyaw, ccurv): 58 | self.cx = cx 59 | self.cy = cy 60 | self.cyaw = cyaw 61 | self.ccurv = ccurv 62 | self.len = len(self.cx) 63 | self.s0 = 1 64 | 65 | def calc_theta_e_and_er(self, node): 66 | """ 67 | calc theta_e and er. 68 | theta_e = theta_car - theta_path 69 | er = lateral distance in frenet frame 70 | 71 | :param node: current information of vehicle 72 | :return: theta_e and er 73 | """ 74 | 75 | ind = self.nearest_index(node) 76 | 77 | k = self.ccurv[ind] 78 | yaw = self.cyaw[ind] 79 | 80 | rear_axle_vec_rot_90 = np.array([[math.cos(node.yaw + math.pi / 2.0)], 81 | [math.sin(node.yaw + math.pi / 2.0)]]) 82 | 83 | vec_target_2_rear = np.array([[node.x - self.cx[ind]], 84 | [node.y - self.cy[ind]]]) 85 | 86 | er = np.dot(vec_target_2_rear.T, rear_axle_vec_rot_90) 87 | theta_e = pi_2_pi(node.yaw - self.cyaw[ind]) 88 | 89 | return theta_e, er, k, yaw, ind 90 | 91 | def nearest_index(self, node): 92 | """ 93 | find the index of the nearest point to current position. 94 | :param node: current information 95 | :return: nearest index 96 | """ 97 | 98 | dx = [node.x - x for x in self.cx] 99 | dy = [node.y - y for y in self.cy] 100 | dist = np.hypot(dx, dy) 101 | self.s0 += np.argmin(dist[self.s0:self.len]) 102 | 103 | return self.s0 104 | 105 | 106 | def rear_wheel_feedback_control(node, ref_path): 107 | """ 108 | rear wheel feedback controller 109 | :param node: current information 110 | :param ref_path: reference path: x, y, yaw, curvature 111 | :return: optimal steering angle 112 | """ 113 | 114 | theta_e, er, k, yaw, ind = ref_path.calc_theta_e_and_er(node) 115 | vr = node.v 116 | 117 | omega = vr * k * math.cos(theta_e) / (1.0 - k * er) - \ 118 | C.K_theta * abs(vr) * theta_e - C.K_e * vr * math.sin(theta_e) * er / theta_e 119 | 120 | delta = math.atan2(C.WB * omega, vr) 121 | 122 | return delta, ind 123 | 124 | 125 | def pi_2_pi(angle): 126 | if angle > math.pi: 127 | return angle - 2.0 * math.pi 128 | if angle < -math.pi: 129 | return angle + 2.0 * math.pi 130 | 131 | return angle 132 | 133 | 134 | def pid_control(target_v, v, dist, direct): 135 | """ 136 | using LQR as lateral controller, PID as longitudinal controller (speed control) 137 | :param target_v: target speed 138 | :param v: current speed 139 | :param dist: distance to end point 140 | :param direct: current direction of vehicle, 1.0: forward, -1.0: backward 141 | :return: acceleration 142 | """ 143 | 144 | a = 0.3 * (target_v - direct * v) 145 | 146 | if dist < 10.0: 147 | if v > 2: 148 | a = -3.0 149 | elif v < -2: 150 | a = -1.0 151 | 152 | return a 153 | 154 | 155 | def generate_path(s): 156 | """ 157 | design path using reeds-shepp path generator. 158 | divide paths into sections, in each section the direction is the same. 159 | :param s: objective positions and directions. 160 | :return: paths 161 | """ 162 | 163 | max_c = math.tan(C.MAX_STEER) / C.WB 164 | path_x, path_y, yaw, direct, rc = [], [], [], [], [] 165 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = [], [], [], [], [] 166 | direct_flag = 1.0 167 | 168 | for i in range(len(s) - 1): 169 | s_x, s_y, s_yaw = s[i][0], s[i][1], np.deg2rad(s[i][2]) 170 | g_x, g_y, g_yaw = s[i + 1][0], s[i + 1][1], np.deg2rad(s[i + 1][2]) 171 | 172 | path_i = rs.calc_optimal_path(s_x, s_y, s_yaw, 173 | g_x, g_y, g_yaw, max_c) 174 | 175 | irc, rds = rs.calc_curvature(path_i.x, path_i.y, path_i.yaw, path_i.directions) 176 | 177 | ix = path_i.x 178 | iy = path_i.y 179 | iyaw = path_i.yaw 180 | idirect = path_i.directions 181 | 182 | for j in range(len(ix)): 183 | if idirect[j] == direct_flag: 184 | x_rec.append(ix[j]) 185 | y_rec.append(iy[j]) 186 | yaw_rec.append(iyaw[j]) 187 | direct_rec.append(idirect[j]) 188 | rc_rec.append(irc[j]) 189 | else: 190 | if len(x_rec) == 0 or direct_rec[0] != direct_flag: 191 | direct_flag = idirect[j] 192 | continue 193 | 194 | path_x.append(x_rec) 195 | path_y.append(y_rec) 196 | yaw.append(yaw_rec) 197 | direct.append(direct_rec) 198 | rc.append(rc_rec) 199 | x_rec, y_rec, yaw_rec, direct_rec, rc_rec = \ 200 | [x_rec[-1]], [y_rec[-1]], [yaw_rec[-1]], [-direct_rec[-1]], [rc_rec[-1]] 201 | 202 | path_x.append(x_rec) 203 | path_y.append(y_rec) 204 | yaw.append(yaw_rec) 205 | direct.append(direct_rec) 206 | rc.append(rc_rec) 207 | 208 | x_all, y_all = [], [] 209 | for ix, iy in zip(path_x, path_y): 210 | x_all += ix 211 | y_all += iy 212 | 213 | return path_x, path_y, yaw, direct, rc, x_all, y_all 214 | 215 | 216 | def main(): 217 | # generate path 218 | states = [(0, 0, 0), (20, 15, 0), (35, 20, 90), (40, 0, 180), 219 | (20, 0, 120), (5, -10, 180), (15, 5, 30)] 220 | # 221 | # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), 222 | # (35, 10, 180), (30, -10, 160), (5, -12, 90)] 223 | 224 | x_ref, y_ref, yaw_ref, direct, curv, x_all, y_all = generate_path(states) 225 | 226 | maxTime = 100.0 227 | yaw_old = 0.0 228 | x0, y0, yaw0, direct0 = \ 229 | x_ref[0][0], y_ref[0][0], yaw_ref[0][0], direct[0][0] 230 | 231 | x_rec, y_rec, yaw_rec, direct_rec = [], [], [], [] 232 | 233 | for cx, cy, cyaw, cdirect, ccurv in zip(x_ref, y_ref, yaw_ref, direct, curv): 234 | t = 0.0 235 | node = Node(x=x0, y=y0, yaw=yaw0, v=0.0, direct=cdirect[0]) 236 | ref_trajectory = PATH(cx, cy, cyaw, ccurv) 237 | 238 | while t < maxTime: 239 | if cdirect[0] > 0: 240 | speed_ref = 30.0 / 3.6 241 | C.Ld = 3.5 242 | else: 243 | speed_ref = 15.0 / 3.6 244 | C.Ld = 2.5 245 | 246 | delta, ind = rear_wheel_feedback_control(node, ref_trajectory) 247 | 248 | dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) 249 | 250 | acceleration = pid_control(speed_ref, node.v, dist, node.direct) 251 | node.update(acceleration, delta, node.direct) 252 | t += C.dt 253 | 254 | if dist <= C.dist_stop: 255 | break 256 | 257 | x_rec.append(node.x) 258 | y_rec.append(node.y) 259 | yaw_rec.append(node.yaw) 260 | direct_rec.append(node.direct) 261 | 262 | dy = (node.yaw - yaw_old) / (node.v * C.dt) 263 | steer = rs.pi_2_pi(-math.atan(C.WB * dy)) 264 | 265 | yaw_old = node.yaw 266 | x0 = x_rec[-1] 267 | y0 = y_rec[-1] 268 | yaw0 = yaw_rec[-1] 269 | 270 | plt.cla() 271 | plt.plot(x_all, y_all, color='gray', linewidth=2.0) 272 | plt.plot(x_rec, y_rec, linewidth=2.0, color='darkviolet') 273 | plt.plot(cx[ind], cy[ind], '.r') 274 | draw.draw_car(node.x, node.y, node.yaw, steer, C) 275 | plt.axis("equal") 276 | plt.title("RearWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h") 277 | plt.gcf().canvas.mpl_connect('key_release_event', 278 | lambda event: 279 | [exit(0) if event.key == 'escape' else None]) 280 | plt.pause(0.001) 281 | 282 | plt.show() 283 | 284 | 285 | if __name__ == '__main__': 286 | main() 287 | -------------------------------------------------------------------------------- /Control/Stanley.py: -------------------------------------------------------------------------------- 1 | """ 2 | Front-Wheel Feedback Controller (Stanley) 3 | author: huiming zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 13 | "/../../MotionPlanning/") 14 | 15 | import Control.draw as draw 16 | import CurvesGenerator.reeds_shepp as rs 17 | import CurvesGenerator.cubic_spline as cs 18 | 19 | 20 | class C: 21 | # PID config 22 | Kp = 1.0 23 | 24 | # System config 25 | k = 0.5 26 | dt = 0.1 27 | dref = 0.5 28 | 29 | # vehicle config 30 | RF = 3.3 # [m] distance from rear to vehicle front end of vehicle 31 | RB = 0.8 # [m] distance from rear to vehicle back end of vehicle 32 | W = 2.4 # [m] width of vehicle 33 | WD = 0.7 * W # [m] distance between left-right wheels 34 | WB = 2.5 # [m] Wheel base 35 | TR = 0.44 # [m] Tyre radius 36 | TW = 0.7 # [m] Tyre width 37 | MAX_STEER = 0.65 38 | 39 | 40 | class Node: 41 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 42 | self.x = x 43 | self.y = y 44 | self.yaw = yaw 45 | self.v = v 46 | 47 | def update(self, a, delta): 48 | delta = self.limit_input(delta) 49 | self.x += self.v * math.cos(self.yaw) * C.dt 50 | self.y += self.v * math.sin(self.yaw) * C.dt 51 | self.yaw += self.v / C.WB * math.tan(delta) * C.dt 52 | self.v += a * C.dt 53 | 54 | @staticmethod 55 | def limit_input(delta): 56 | if delta > C.MAX_STEER: 57 | return C.MAX_STEER 58 | 59 | if delta < -C.MAX_STEER: 60 | return -C.MAX_STEER 61 | 62 | return delta 63 | 64 | 65 | class Trajectory: 66 | def __init__(self, cx, cy, cyaw): 67 | self.cx = cx 68 | self.cy = cy 69 | self.cyaw = cyaw 70 | self.ind_old = 0 71 | 72 | def calc_theta_e_and_ef(self, node): 73 | """ 74 | calc theta_e and ef. 75 | theta_e = theta_car - theta_path 76 | ef = lateral distance in frenet frame (front wheel) 77 | 78 | :param node: current information of vehicle 79 | :return: theta_e and ef 80 | """ 81 | 82 | fx = node.x + C.WB * math.cos(node.yaw) 83 | fy = node.y + C.WB * math.sin(node.yaw) 84 | 85 | dx = [fx - x for x in self.cx] 86 | dy = [fy - y for y in self.cy] 87 | 88 | target_index = int(np.argmin(np.hypot(dx, dy))) 89 | target_index = max(self.ind_old, target_index) 90 | self.ind_old = max(self.ind_old, target_index) 91 | 92 | front_axle_vec_rot_90 = np.array([[math.cos(node.yaw - math.pi / 2.0)], 93 | [math.sin(node.yaw - math.pi / 2.0)]]) 94 | 95 | vec_target_2_front = np.array([[dx[target_index]], 96 | [dy[target_index]]]) 97 | 98 | ef = np.dot(vec_target_2_front.T, front_axle_vec_rot_90) 99 | 100 | theta = node.yaw 101 | theta_p = self.cyaw[target_index] 102 | theta_e = pi_2_pi(theta_p - theta) 103 | 104 | return theta_e, ef, target_index 105 | 106 | 107 | def front_wheel_feedback_control(node, ref_path): 108 | """ 109 | front wheel feedback controller 110 | :param node: current information 111 | :param ref_path: reference path: x, y, yaw, curvature 112 | :return: optimal steering angle 113 | """ 114 | 115 | theta_e, ef, target_index = ref_path.calc_theta_e_and_ef(node) 116 | delta = theta_e + math.atan2(C.k * ef, node.v) 117 | 118 | return delta, target_index 119 | 120 | 121 | def pi_2_pi(angle): 122 | if angle > math.pi: 123 | return angle - 2.0 * math.pi 124 | if angle < -math.pi: 125 | return angle + 2.0 * math.pi 126 | 127 | return angle 128 | 129 | 130 | def pid_control(target_v, v, dist): 131 | """ 132 | PID controller and design speed profile. 133 | :param target_v: target speed 134 | :param v: current speed 135 | :param dist: distance to end point 136 | :return: acceleration 137 | """ 138 | 139 | a = 0.3 * (target_v - v) 140 | 141 | if dist < 10.0: 142 | if v > 3.0: 143 | a = -2.5 144 | elif v < -2.0: 145 | a = -1.0 146 | 147 | return a 148 | 149 | 150 | def main(): 151 | # generate path 152 | ax = np.arange(0, 50, 0.5) 153 | ay = [math.sin(ix / 5.0) * ix / 2.0 for ix in ax] 154 | 155 | cx, cy, cyaw, _, _ = cs.calc_spline_course(ax, ay, ds=C.dt) 156 | 157 | t = 0.0 158 | maxTime = 100.0 159 | yaw_old = 0.0 160 | x0, y0, yaw0 = cx[0], cy[0], cyaw[0] 161 | xrec, yrec, yawrec = [], [], [] 162 | 163 | node = Node(x=x0, y=y0, yaw=yaw0, v=0.0) 164 | ref_path = Trajectory(cx, cy, cyaw) 165 | 166 | while t < maxTime: 167 | speed_ref = 25.0 / 3.6 168 | C.Ld = 3.5 169 | 170 | di, target_index = front_wheel_feedback_control(node, ref_path) 171 | 172 | dist = math.hypot(node.x - cx[-1], node.y - cy[-1]) 173 | ai = pid_control(speed_ref, node.v, dist) 174 | node.update(ai, di) 175 | t += C.dt 176 | 177 | if dist <= C.dref: 178 | break 179 | 180 | dy = (node.yaw - yaw_old) / (node.v * C.dt) 181 | steer = rs.pi_2_pi(-math.atan(C.WB * dy)) 182 | yaw_old = node.yaw 183 | 184 | xrec.append(node.x) 185 | yrec.append(node.y) 186 | yawrec.append(node.yaw) 187 | 188 | plt.cla() 189 | plt.plot(cx, cy, color='gray', linewidth=2.0) 190 | plt.plot(xrec, yrec, linewidth=2.0, color='darkviolet') 191 | plt.plot(cx[target_index], cy[target_index], '.r') 192 | draw.draw_car(node.x, node.y, node.yaw, steer, C) 193 | plt.axis("equal") 194 | plt.title("FrontWheelFeedback: v=" + str(node.v * 3.6)[:4] + "km/h") 195 | plt.gcf().canvas.mpl_connect('key_release_event', 196 | lambda event: 197 | [exit(0) if event.key == 'escape' else None]) 198 | plt.pause(0.001) 199 | 200 | plt.show() 201 | 202 | 203 | if __name__ == '__main__': 204 | main() 205 | -------------------------------------------------------------------------------- /Control/__pycache__/config_control.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/__pycache__/config_control.cpython-37.pyc -------------------------------------------------------------------------------- /Control/__pycache__/draw.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/__pycache__/draw.cpython-37.pyc -------------------------------------------------------------------------------- /Control/__pycache__/draw.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/__pycache__/draw.cpython-38.pyc -------------------------------------------------------------------------------- /Control/__pycache__/draw_lqr.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/__pycache__/draw_lqr.cpython-37.pyc -------------------------------------------------------------------------------- /Control/config_control.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # Vehicle config 4 | wheelbase = 2.33 # wheel base: front to rear axle [m] 5 | wheeldist = 1.85 # wheel dist: left to right wheel [m] 6 | v_w = 2.33 # vehicle width [m] 7 | r_b = 0.80 # rear to back [m] 8 | r_f = 3.15 # rear to front [m] 9 | t_r = 0.40 # tire radius [m] 10 | t_w = 0.30 # tire width [m] 11 | 12 | c_f = 155494.663 # [N / rad] 13 | c_r = 155494.663 # [N / rad] 14 | m_f = 570 # [kg] 15 | m_r = 570 # [kg] 16 | l_f = 1.165 # [m] 17 | l_r = 1.165 # [m] 18 | Iz = 1436.24 # [kg m2] 19 | 20 | # Controller Config 21 | ts = 0.10 # [s] 22 | max_iteration = 150 23 | eps = 0.01 24 | 25 | matrix_q = [1.0, 0.0, 1.0, 0.0] 26 | matrix_r = [1.0] 27 | 28 | state_size = 4 29 | 30 | max_acceleration = 5.0 # [m / s^2] 31 | max_steer_angle = np.deg2rad(40) # [rad] 32 | max_speed = 30 / 3.6 # [km/h] -------------------------------------------------------------------------------- /Control/draw.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | 6 | class Arrow: 7 | def __init__(self, x, y, theta, L, c): 8 | angle = np.deg2rad(30) 9 | d = 0.4 * L 10 | w = 2 11 | 12 | x_start = x 13 | y_start = y 14 | x_end = x + L * np.cos(theta) 15 | y_end = y + L * np.sin(theta) 16 | 17 | theta_hat_L = theta + math.pi - angle 18 | theta_hat_R = theta + math.pi + angle 19 | 20 | x_hat_start = x_end 21 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 22 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 23 | 24 | y_hat_start = y_end 25 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 26 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 27 | 28 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 29 | plt.plot([x_hat_start, x_hat_end_L], 30 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 31 | plt.plot([x_hat_start, x_hat_end_R], 32 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 33 | 34 | 35 | def draw_car(x, y, yaw, steer, C, color='black'): 36 | car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], 37 | [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]]) 38 | 39 | wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR], 40 | [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]]) 41 | 42 | rlWheel = wheel.copy() 43 | rrWheel = wheel.copy() 44 | frWheel = wheel.copy() 45 | flWheel = wheel.copy() 46 | 47 | Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], 48 | [math.sin(yaw), math.cos(yaw)]]) 49 | 50 | Rot2 = np.array([[math.cos(steer), math.sin(steer)], 51 | [-math.sin(steer), math.cos(steer)]]) 52 | 53 | frWheel = np.dot(Rot2, frWheel) 54 | flWheel = np.dot(Rot2, flWheel) 55 | 56 | frWheel += np.array([[C.WB], [-C.WD / 2]]) 57 | flWheel += np.array([[C.WB], [C.WD / 2]]) 58 | rrWheel[1, :] -= C.WD / 2 59 | rlWheel[1, :] += C.WD / 2 60 | 61 | frWheel = np.dot(Rot1, frWheel) 62 | flWheel = np.dot(Rot1, flWheel) 63 | 64 | rrWheel = np.dot(Rot1, rrWheel) 65 | rlWheel = np.dot(Rot1, rlWheel) 66 | car = np.dot(Rot1, car) 67 | 68 | frWheel += np.array([[x], [y]]) 69 | flWheel += np.array([[x], [y]]) 70 | rrWheel += np.array([[x], [y]]) 71 | rlWheel += np.array([[x], [y]]) 72 | car += np.array([[x], [y]]) 73 | 74 | plt.plot(car[0, :], car[1, :], color) 75 | plt.plot(frWheel[0, :], frWheel[1, :], color) 76 | plt.plot(rrWheel[0, :], rrWheel[1, :], color) 77 | plt.plot(flWheel[0, :], flWheel[1, :], color) 78 | plt.plot(rlWheel[0, :], rlWheel[1, :], color) 79 | Arrow(x, y, yaw, C.WB * 0.6, color) 80 | -------------------------------------------------------------------------------- /Control/draw_lqr.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from Control.config_control import * 3 | 4 | PI = np.pi 5 | 6 | 7 | class Arrow: 8 | def __init__(self, x, y, theta, L, c): 9 | angle = np.deg2rad(30) 10 | d = 0.3 * L 11 | w = 2 12 | 13 | x_start = x 14 | y_start = y 15 | x_end = x + L * np.cos(theta) 16 | y_end = y + L * np.sin(theta) 17 | 18 | theta_hat_L = theta + PI - angle 19 | theta_hat_R = theta + PI + angle 20 | 21 | x_hat_start = x_end 22 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 23 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 24 | 25 | y_hat_start = y_end 26 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 27 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 28 | 29 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 30 | plt.plot([x_hat_start, x_hat_end_L], 31 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 32 | plt.plot([x_hat_start, x_hat_end_R], 33 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 34 | 35 | 36 | def draw_car(x, y, yaw, steer, color='black'): 37 | car = np.array([[-r_b, -r_b, r_f, r_f, -r_b], 38 | [v_w / 2, -v_w / 2, -v_w / 2, v_w / 2, v_w / 2]]) 39 | 40 | wheel = np.array([[-t_r, -t_r, t_r, t_r, -t_r], 41 | [t_w / 2, -t_w / 2, -t_w / 2, t_w / 2, t_w / 2]]) 42 | 43 | rlWheel = wheel.copy() 44 | rrWheel = wheel.copy() 45 | frWheel = wheel.copy() 46 | flWheel = wheel.copy() 47 | 48 | Rot1 = np.array([[np.cos(yaw), -np.sin(yaw)], 49 | [np.sin(yaw), np.cos(yaw)]]) 50 | 51 | Rot2 = np.array([[np.cos(steer), np.sin(steer)], 52 | [-np.sin(steer), np.cos(steer)]]) 53 | 54 | frWheel = np.dot(Rot2, frWheel) 55 | flWheel = np.dot(Rot2, flWheel) 56 | 57 | frWheel += np.array([[wheelbase], [-wheeldist / 2]]) 58 | flWheel += np.array([[wheelbase], [wheeldist / 2]]) 59 | rrWheel[1, :] -= wheeldist / 2 60 | rlWheel[1, :] += wheeldist / 2 61 | 62 | frWheel = np.dot(Rot1, frWheel) 63 | flWheel = np.dot(Rot1, flWheel) 64 | 65 | rrWheel = np.dot(Rot1, rrWheel) 66 | rlWheel = np.dot(Rot1, rlWheel) 67 | car = np.dot(Rot1, car) 68 | 69 | frWheel += np.array([[x], [y]]) 70 | flWheel += np.array([[x], [y]]) 71 | rrWheel += np.array([[x], [y]]) 72 | rlWheel += np.array([[x], [y]]) 73 | car += np.array([[x], [y]]) 74 | 75 | plt.plot(car[0, :], car[1, :], color) 76 | plt.plot(frWheel[0, :], frWheel[1, :], color) 77 | plt.plot(rrWheel[0, :], rrWheel[1, :], color) 78 | plt.plot(flWheel[0, :], flWheel[1, :], color) 79 | plt.plot(rlWheel[0, :], rlWheel[1, :], color) 80 | Arrow(x, y, yaw, 0.8 * wheelbase, color) 81 | plt.axis("equal") 82 | # plt.show() 83 | 84 | 85 | if __name__ == '__main__': 86 | draw_car(0, 0, 0, 0.2) -------------------------------------------------------------------------------- /Control/fig/carmodel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/carmodel.png -------------------------------------------------------------------------------- /Control/fig/frontwheel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/frontwheel.png -------------------------------------------------------------------------------- /Control/fig/ps.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/ps.png -------------------------------------------------------------------------------- /Control/fig/purepursuit.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/purepursuit.jpg -------------------------------------------------------------------------------- /Control/fig/rearwheel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/rearwheel.png -------------------------------------------------------------------------------- /Control/fig/simple car.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/fig/simple car.jpg -------------------------------------------------------------------------------- /Control/gif/FWF.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/FWF.gif -------------------------------------------------------------------------------- /Control/gif/LQR_Dynamics.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/LQR_Dynamics.gif -------------------------------------------------------------------------------- /Control/gif/LQR_Kinematic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/LQR_Kinematic.gif -------------------------------------------------------------------------------- /Control/gif/MPC.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/MPC.gif -------------------------------------------------------------------------------- /Control/gif/RWF1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/RWF1.gif -------------------------------------------------------------------------------- /Control/gif/RWF2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/RWF2.gif -------------------------------------------------------------------------------- /Control/gif/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/demo.gif -------------------------------------------------------------------------------- /Control/gif/purepursuit1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/purepursuit1.gif -------------------------------------------------------------------------------- /Control/gif/purepursuit2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/purepursuit2.gif -------------------------------------------------------------------------------- /Control/gif/stanley.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/Control/gif/stanley.gif -------------------------------------------------------------------------------- /Control/utils.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | def calc_curvature_point(s1, s2, s3): 6 | ta = math.hypot(s2[0] - s1[0], s2[1] - s1[1]) 7 | tb = math.hypot(s3[0] - s2[0], s3[1] - s2[1]) 8 | M = np.array([[1, -ta, ta ** 2], 9 | [1, 0, 0], 10 | [1, tb, tb ** 2]]) 11 | X = np.array([[s1[0]], [s2[0]], [s3[0]]]) 12 | Y = np.array([[s1[1]], [s2[1]], [s3[1]]]) 13 | A = np.linalg.solve(M, X) 14 | B = np.linalg.solve(M, Y) 15 | k = 2 * (A[2][0] * B[1][0] - A[1][0] * B[2][0]) / (A[1][0] ** 2 + B[1][0] ** 2) ** (3 / 2) 16 | 17 | return k 18 | 19 | 20 | def calc_curvature(x, y): 21 | K = [0.0] 22 | x, y = map(np.asarray, (x, y)) 23 | ta = (np.diff(x[0:-1]) ** 2 + np.diff(y[0:-1]) ** 2) ** 0.5 24 | tb = (np.diff(x[1:len(x)]) ** 2 + np.diff(y[1:len(y)]) ** 2) ** 0.5 25 | for i in range(len(ta) - 2): 26 | M = np.array([[1, -ta[i], ta[i] ** 2], 27 | [1, 0, 0], 28 | [1, tb[i], tb[i] ** 2]]) 29 | X = np.array([[x[i]], [x[i + 1]], [x[i + 2]]]) 30 | Y = np.array([[y[i]], [y[i + 1]], [y[i + 2]]]) 31 | A = np.linalg.solve(M, X) 32 | B = np.linalg.solve(M, Y) 33 | k = 2 * (A[2][0] * B[1][0] - A[1][0] * B[2][0]) / \ 34 | (A[1][0] ** 2 + B[1][0] ** 2) ** (3 / 2) 35 | K.append(k) 36 | K.append(0.0) 37 | K.append(0.0) 38 | K.append(0.0) 39 | 40 | return K 41 | 42 | 43 | def main(): 44 | R = 2 45 | theta = np.arange(0, 2 * math.pi, 0.1 * math.pi) 46 | x = R * np.cos(theta) 47 | y = R * np.sin(theta) 48 | 49 | K = calc_curvature(x, y) 50 | 51 | 52 | if __name__ == '__main__': 53 | main() 54 | -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/cubic_spline.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/cubic_spline.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/cubic_spline.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/cubic_spline.cpython-38.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/draw.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/draw.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/quartic_polynomial.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/quartic_polynomial.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/quintic_polynomial.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/quintic_polynomial.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/reeds_shepp.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/reeds_shepp.cpython-37.pyc -------------------------------------------------------------------------------- /CurvesGenerator/__pycache__/reeds_shepp.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/CurvesGenerator/__pycache__/reeds_shepp.cpython-38.pyc -------------------------------------------------------------------------------- /CurvesGenerator/cubic_spline.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | u""" 4 | Cubic Spline library on python 5 | 6 | author Atsushi Sakai 7 | 8 | usage: see test codes as below 9 | 10 | license: MIT 11 | """ 12 | import math 13 | import numpy as np 14 | import bisect 15 | 16 | 17 | class Spline: 18 | u""" 19 | Cubic Spline class 20 | """ 21 | 22 | def __init__(self, x, y): 23 | self.b, self.c, self.d, self.w = [], [], [], [] 24 | 25 | self.x = x 26 | self.y = y 27 | 28 | self.nx = len(x) # dimension of x 29 | h = np.diff(x) 30 | 31 | # calc coefficient c 32 | self.a = [iy for iy in y] 33 | 34 | # calc coefficient c 35 | A = self.__calc_A(h) 36 | B = self.__calc_B(h) 37 | self.c = np.linalg.solve(A, B) 38 | # print(self.c1) 39 | 40 | # calc spline coefficient b and d 41 | for i in range(self.nx - 1): 42 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 43 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 44 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 45 | self.b.append(tb) 46 | 47 | def calc(self, t): 48 | u""" 49 | Calc position 50 | 51 | if t is outside of the input x, return None 52 | 53 | """ 54 | 55 | if t < self.x[0]: 56 | return None 57 | elif t > self.x[-1]: 58 | return None 59 | 60 | i = self.__search_index(t) 61 | dx = t - self.x[i] 62 | result = self.a[i] + self.b[i] * dx + \ 63 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 64 | 65 | return result 66 | 67 | def calcd(self, t): 68 | u""" 69 | Calc first derivative 70 | 71 | if t is outside of the input x, return None 72 | """ 73 | 74 | if t < self.x[0]: 75 | return None 76 | elif t > self.x[-1]: 77 | return None 78 | 79 | i = self.__search_index(t) 80 | dx = t - self.x[i] 81 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 82 | return result 83 | 84 | def calcdd(self, t): 85 | u""" 86 | Calc second derivative 87 | """ 88 | 89 | if t < self.x[0]: 90 | return None 91 | elif t > self.x[-1]: 92 | return None 93 | 94 | i = self.__search_index(t) 95 | dx = t - self.x[i] 96 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 97 | return result 98 | 99 | def __search_index(self, x): 100 | u""" 101 | search data segment index 102 | """ 103 | return bisect.bisect(self.x, x) - 1 104 | 105 | def __calc_A(self, h): 106 | u""" 107 | calc matrix A for spline coefficient c 108 | """ 109 | A = np.zeros((self.nx, self.nx)) 110 | A[0, 0] = 1.0 111 | for i in range(self.nx - 1): 112 | if i != (self.nx - 2): 113 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 114 | A[i + 1, i] = h[i] 115 | A[i, i + 1] = h[i] 116 | 117 | A[0, 1] = 0.0 118 | A[self.nx - 1, self.nx - 2] = 0.0 119 | A[self.nx - 1, self.nx - 1] = 1.0 120 | # print(A) 121 | return A 122 | 123 | def __calc_B(self, h): 124 | u""" 125 | calc matrix B for spline coefficient c 126 | """ 127 | B = np.zeros(self.nx) 128 | for i in range(self.nx - 2): 129 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 130 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 131 | # print(B) 132 | return B 133 | 134 | 135 | class Spline2D: 136 | u""" 137 | 2D Cubic Spline class 138 | 139 | """ 140 | 141 | def __init__(self, x, y): 142 | self.s = self.__calc_s(x, y) 143 | self.sx = Spline(self.s, x) 144 | self.sy = Spline(self.s, y) 145 | 146 | def __calc_s(self, x, y): 147 | dx = np.diff(x) 148 | dy = np.diff(y) 149 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 150 | for (idx, idy) in zip(dx, dy)] 151 | s = [0] 152 | s.extend(np.cumsum(self.ds)) 153 | return s 154 | 155 | def calc_position(self, s): 156 | u""" 157 | calc position 158 | """ 159 | x = self.sx.calc(s) 160 | y = self.sy.calc(s) 161 | 162 | return x, y 163 | 164 | def calc_curvature(self, s): 165 | u""" 166 | calc curvature 167 | """ 168 | dx = self.sx.calcd(s) 169 | ddx = self.sx.calcdd(s) 170 | dy = self.sy.calcd(s) 171 | ddy = self.sy.calcdd(s) 172 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 173 | return k 174 | 175 | def calc_yaw(self, s): 176 | u""" 177 | calc yaw 178 | """ 179 | dx = self.sx.calcd(s) 180 | dy = self.sy.calcd(s) 181 | yaw = math.atan2(dy, dx) 182 | return yaw 183 | 184 | 185 | def calc_spline_course(x, y, ds=0.1): 186 | sp = Spline2D(x, y) 187 | s = np.arange(0, sp.s[-1], ds) 188 | 189 | rx, ry, ryaw, rk = [], [], [], [] 190 | for i_s in s: 191 | ix, iy = sp.calc_position(i_s) 192 | rx.append(ix) 193 | ry.append(iy) 194 | ryaw.append(sp.calc_yaw(i_s)) 195 | rk.append(sp.calc_curvature(i_s)) 196 | 197 | return rx, ry, ryaw, rk, s 198 | 199 | 200 | def test_spline2d(): 201 | print("Spline 2D test") 202 | import matplotlib.pyplot as plt 203 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 204 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 205 | 206 | sp = Spline2D(x, y) 207 | s = np.arange(0, sp.s[-1], 0.1) 208 | 209 | rx, ry, ryaw, rk = [], [], [], [] 210 | for i_s in s: 211 | ix, iy = sp.calc_position(i_s) 212 | rx.append(ix) 213 | ry.append(iy) 214 | ryaw.append(sp.calc_yaw(i_s)) 215 | rk.append(sp.calc_curvature(i_s)) 216 | 217 | flg, ax = plt.subplots(1) 218 | plt.plot(x, y, "xb", label="input") 219 | plt.plot(rx, ry, "-r", label="spline") 220 | plt.grid(True) 221 | plt.axis("equal") 222 | plt.xlabel("x[m]") 223 | plt.ylabel("y[m]") 224 | plt.legend() 225 | 226 | flg, ax = plt.subplots(1) 227 | plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") 228 | plt.grid(True) 229 | plt.legend() 230 | plt.xlabel("line length[m]") 231 | plt.ylabel("yaw angle[deg]") 232 | 233 | flg, ax = plt.subplots(1) 234 | plt.plot(s, rk, "-r", label="curvature") 235 | plt.grid(True) 236 | plt.legend() 237 | plt.xlabel("line length[m]") 238 | plt.ylabel("curvature [1/m]") 239 | 240 | plt.show() 241 | 242 | 243 | def test_spline(): 244 | print("Spline test") 245 | import matplotlib.pyplot as plt 246 | x = [-0.5, 0.0, 0.5, 1.0, 1.5] 247 | y = [3.2, 2.7, 6, 5, 6.5] 248 | 249 | spline = Spline(x, y) 250 | rx = np.arange(-2.0, 4, 0.01) 251 | ry = [spline.calc(i) for i in rx] 252 | 253 | plt.plot(x, y, "xb") 254 | plt.plot(rx, ry, "-r") 255 | plt.grid(True) 256 | plt.axis("equal") 257 | plt.show() 258 | 259 | 260 | if __name__ == '__main__': 261 | test_spline() 262 | test_spline2d() 263 | -------------------------------------------------------------------------------- /CurvesGenerator/draw.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | PI = np.pi 4 | 5 | 6 | class Arrow: 7 | def __init__(self, x, y, theta, L, c): 8 | angle = np.deg2rad(30) 9 | d = 0.5 * L 10 | w = 2 11 | 12 | x_start = x 13 | y_start = y 14 | x_end = x + L * np.cos(theta) 15 | y_end = y + L * np.sin(theta) 16 | 17 | theta_hat_L = theta + PI - angle 18 | theta_hat_R = theta + PI + angle 19 | 20 | x_hat_start = x_end 21 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 22 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 23 | 24 | y_hat_start = y_end 25 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 26 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 27 | 28 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 29 | plt.plot([x_hat_start, x_hat_end_L], 30 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 31 | plt.plot([x_hat_start, x_hat_end_R], 32 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 33 | 34 | 35 | class Car: 36 | def __init__(self, x, y, yaw, w, L): 37 | theta_B = PI + yaw 38 | 39 | xB = x + L / 4 * np.cos(theta_B) 40 | yB = y + L / 4 * np.sin(theta_B) 41 | 42 | theta_BL = theta_B + PI / 2 43 | theta_BR = theta_B - PI / 2 44 | 45 | x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex 46 | y_BL = yB + w / 2 * np.sin(theta_BL) 47 | x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex 48 | y_BR = yB + w / 2 * np.sin(theta_BR) 49 | 50 | x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex 51 | y_FL = y_BL + L * np.sin(yaw) 52 | x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex 53 | y_FR = y_BR + L * np.sin(yaw) 54 | 55 | plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL], 56 | [y_BL, y_BR, y_FR, y_FL, y_BL], 57 | linewidth=1, color='black') 58 | 59 | Arrow(x, y, yaw, L / 2, 'black') 60 | # plt.axis("equal") 61 | # plt.show() 62 | 63 | 64 | if __name__ == '__main__': 65 | # Arrow(-1, 2, 60) 66 | Car(0, 0, 1, 2, 60) -------------------------------------------------------------------------------- /CurvesGenerator/dubins_path.py: -------------------------------------------------------------------------------- 1 | """ 2 | Dubins Path 3 | """ 4 | 5 | import math 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from scipy.spatial.transform import Rotation as Rot 9 | 10 | from CurvesGenerator import draw 11 | 12 | 13 | # class for PATH element 14 | class PATH: 15 | def __init__(self, L, mode, x, y, yaw): 16 | self.L = L # total path length [float] 17 | self.mode = mode # type of each part of the path [string] 18 | self.x = x # final x positions [m] 19 | self.y = y # final y positions [m] 20 | self.yaw = yaw # final yaw angles [rad] 21 | 22 | 23 | # utils 24 | def pi_2_pi(theta): 25 | while theta > math.pi: 26 | theta -= 2.0 * math.pi 27 | 28 | while theta < -math.pi: 29 | theta += 2.0 * math.pi 30 | 31 | return theta 32 | 33 | 34 | def mod2pi(theta): 35 | return theta - 2.0 * math.pi * math.floor(theta / math.pi / 2.0) 36 | 37 | 38 | def LSL(alpha, beta, dist): 39 | sin_a = math.sin(alpha) 40 | sin_b = math.sin(beta) 41 | cos_a = math.cos(alpha) 42 | cos_b = math.cos(beta) 43 | cos_a_b = math.cos(alpha - beta) 44 | 45 | p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b) 46 | 47 | if p_lsl < 0: 48 | return None, None, None, ["WB", "S", "WB"] 49 | else: 50 | p_lsl = math.sqrt(p_lsl) 51 | 52 | denominate = dist + sin_a - sin_b 53 | t_lsl = mod2pi(-alpha + math.atan2(cos_b - cos_a, denominate)) 54 | q_lsl = mod2pi(beta - math.atan2(cos_b - cos_a, denominate)) 55 | 56 | return t_lsl, p_lsl, q_lsl, ["WB", "S", "WB"] 57 | 58 | 59 | def RSR(alpha, beta, dist): 60 | sin_a = math.sin(alpha) 61 | sin_b = math.sin(beta) 62 | cos_a = math.cos(alpha) 63 | cos_b = math.cos(beta) 64 | cos_a_b = math.cos(alpha - beta) 65 | 66 | p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a) 67 | 68 | if p_rsr < 0: 69 | return None, None, None, ["R", "S", "R"] 70 | else: 71 | p_rsr = math.sqrt(p_rsr) 72 | 73 | denominate = dist - sin_a + sin_b 74 | t_rsr = mod2pi(alpha - math.atan2(cos_a - cos_b, denominate)) 75 | q_rsr = mod2pi(-beta + math.atan2(cos_a - cos_b, denominate)) 76 | 77 | return t_rsr, p_rsr, q_rsr, ["R", "S", "R"] 78 | 79 | 80 | def LSR(alpha, beta, dist): 81 | sin_a = math.sin(alpha) 82 | sin_b = math.sin(beta) 83 | cos_a = math.cos(alpha) 84 | cos_b = math.cos(beta) 85 | cos_a_b = math.cos(alpha - beta) 86 | 87 | p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b) 88 | 89 | if p_lsr < 0: 90 | return None, None, None, ["WB", "S", "R"] 91 | else: 92 | p_lsr = math.sqrt(p_lsr) 93 | 94 | rec = math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr) 95 | t_lsr = mod2pi(-alpha + rec) 96 | q_lsr = mod2pi(-mod2pi(beta) + rec) 97 | 98 | return t_lsr, p_lsr, q_lsr, ["WB", "S", "R"] 99 | 100 | 101 | def RSL(alpha, beta, dist): 102 | sin_a = math.sin(alpha) 103 | sin_b = math.sin(beta) 104 | cos_a = math.cos(alpha) 105 | cos_b = math.cos(beta) 106 | cos_a_b = math.cos(alpha - beta) 107 | 108 | p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b) 109 | 110 | if p_rsl < 0: 111 | return None, None, None, ["R", "S", "WB"] 112 | else: 113 | p_rsl = math.sqrt(p_rsl) 114 | 115 | rec = math.atan2(cos_a + cos_b, dist - sin_a - sin_b) - math.atan2(2.0, p_rsl) 116 | t_rsl = mod2pi(alpha - rec) 117 | q_rsl = mod2pi(beta - rec) 118 | 119 | return t_rsl, p_rsl, q_rsl, ["R", "S", "WB"] 120 | 121 | 122 | def RLR(alpha, beta, dist): 123 | sin_a = math.sin(alpha) 124 | sin_b = math.sin(beta) 125 | cos_a = math.cos(alpha) 126 | cos_b = math.cos(beta) 127 | cos_a_b = math.cos(alpha - beta) 128 | 129 | rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0 130 | 131 | if abs(rec) > 1.0: 132 | return None, None, None, ["R", "WB", "R"] 133 | 134 | p_rlr = mod2pi(2 * math.pi - math.acos(rec)) 135 | t_rlr = mod2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + mod2pi(p_rlr / 2.0)) 136 | q_rlr = mod2pi(alpha - beta - t_rlr + mod2pi(p_rlr)) 137 | 138 | return t_rlr, p_rlr, q_rlr, ["R", "WB", "R"] 139 | 140 | 141 | def LRL(alpha, beta, dist): 142 | sin_a = math.sin(alpha) 143 | sin_b = math.sin(beta) 144 | cos_a = math.cos(alpha) 145 | cos_b = math.cos(beta) 146 | cos_a_b = math.cos(alpha - beta) 147 | 148 | rec = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0 149 | 150 | if abs(rec) > 1.0: 151 | return None, None, None, ["WB", "R", "WB"] 152 | 153 | p_lrl = mod2pi(2 * math.pi - math.acos(rec)) 154 | t_lrl = mod2pi(-alpha - math.atan2(cos_a - cos_b, dist + sin_a - sin_b) + p_lrl / 2.0) 155 | q_lrl = mod2pi(mod2pi(beta) - alpha - t_lrl + mod2pi(p_lrl)) 156 | 157 | return t_lrl, p_lrl, q_lrl, ["WB", "R", "WB"] 158 | 159 | 160 | def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions): 161 | if m == "S": 162 | px[ind] = ox + l / maxc * math.cos(oyaw) 163 | py[ind] = oy + l / maxc * math.sin(oyaw) 164 | pyaw[ind] = oyaw 165 | else: 166 | ldx = math.sin(l) / maxc 167 | if m == "WB": 168 | ldy = (1.0 - math.cos(l)) / maxc 169 | elif m == "R": 170 | ldy = (1.0 - math.cos(l)) / (-maxc) 171 | 172 | gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy 173 | gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy 174 | px[ind] = ox + gdx 175 | py[ind] = oy + gdy 176 | 177 | if m == "WB": 178 | pyaw[ind] = oyaw + l 179 | elif m == "R": 180 | pyaw[ind] = oyaw - l 181 | 182 | if l > 0.0: 183 | directions[ind] = 1 184 | else: 185 | directions[ind] = -1 186 | 187 | return px, py, pyaw, directions 188 | 189 | 190 | def generate_local_course(L, lengths, mode, maxc, step_size): 191 | point_num = int(L / step_size) + len(lengths) + 3 192 | 193 | px = [0.0 for _ in range(point_num)] 194 | py = [0.0 for _ in range(point_num)] 195 | pyaw = [0.0 for _ in range(point_num)] 196 | directions = [0 for _ in range(point_num)] 197 | ind = 1 198 | 199 | if lengths[0] > 0.0: 200 | directions[0] = 1 201 | else: 202 | directions[0] = -1 203 | 204 | if lengths[0] > 0.0: 205 | d = step_size 206 | else: 207 | d = -step_size 208 | 209 | ll = 0.0 210 | 211 | for m, l, i in zip(mode, lengths, range(len(mode))): 212 | if l > 0.0: 213 | d = step_size 214 | else: 215 | d = -step_size 216 | 217 | ox, oy, oyaw = px[ind], py[ind], pyaw[ind] 218 | 219 | ind -= 1 220 | if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: 221 | pd = -d - ll 222 | else: 223 | pd = d - ll 224 | 225 | while abs(pd) <= abs(l): 226 | ind += 1 227 | px, py, pyaw, directions = \ 228 | interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) 229 | pd += d 230 | 231 | ll = l - pd - d # calc remain length 232 | 233 | ind += 1 234 | px, py, pyaw, directions = \ 235 | interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) 236 | 237 | if len(px) <= 1: 238 | return [], [], [], [] 239 | 240 | # remove unused data 241 | while len(px) >= 1 and px[-1] == 0.0: 242 | px.pop() 243 | py.pop() 244 | pyaw.pop() 245 | directions.pop() 246 | 247 | return px, py, pyaw, directions 248 | 249 | 250 | def planning_from_origin(gx, gy, gyaw, curv, step_size): 251 | D = math.hypot(gx, gy) 252 | d = D * curv 253 | 254 | theta = mod2pi(math.atan2(gy, gx)) 255 | alpha = mod2pi(-theta) 256 | beta = mod2pi(gyaw - theta) 257 | 258 | planners = [LSL, RSR, LSR, RSL, RLR, LRL] 259 | 260 | best_cost = float("inf") 261 | bt, bp, bq, best_mode = None, None, None, None 262 | 263 | for planner in planners: 264 | t, p, q, mode = planner(alpha, beta, d) 265 | 266 | if t is None: 267 | continue 268 | 269 | cost = (abs(t) + abs(p) + abs(q)) 270 | if best_cost > cost: 271 | bt, bp, bq, best_mode = t, p, q, mode 272 | best_cost = cost 273 | lengths = [bt, bp, bq] 274 | 275 | x_list, y_list, yaw_list, directions = generate_local_course( 276 | sum(lengths), lengths, best_mode, curv, step_size) 277 | 278 | return x_list, y_list, yaw_list, best_mode, best_cost 279 | 280 | 281 | def calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, curv, step_size=0.1): 282 | gx = gx - sx 283 | gy = gy - sy 284 | 285 | l_rot = Rot.from_euler('z', syaw).as_dcm()[0:2, 0:2] 286 | le_xy = np.stack([gx, gy]).T @ l_rot 287 | le_yaw = gyaw - syaw 288 | 289 | lp_x, lp_y, lp_yaw, mode, lengths = planning_from_origin( 290 | le_xy[0], le_xy[1], le_yaw, curv, step_size) 291 | 292 | rot = Rot.from_euler('z', -syaw).as_dcm()[0:2, 0:2] 293 | converted_xy = np.stack([lp_x, lp_y]).T @ rot 294 | x_list = converted_xy[:, 0] + sx 295 | y_list = converted_xy[:, 1] + sy 296 | yaw_list = [pi_2_pi(i_yaw + syaw) for i_yaw in lp_yaw] 297 | 298 | return PATH(lengths, mode, x_list, y_list, yaw_list) 299 | 300 | 301 | def main(): 302 | # choose states pairs: (x, y, yaw) 303 | # simulation-1 304 | states = [(0, 0, 0), (10, 10, -90), (20, 5, 60), (30, 10, 120), 305 | (35, -5, 30), (25, -10, -120), (15, -15, 100), (0, -10, -90)] 306 | 307 | # simulation-2 308 | # states = [(-3, 3, 120), (10, -7, 30), (10, 13, 30), (20, 5, -25), 309 | # (35, 10, 180), (32, -10, 180), (5, -12, 90)] 310 | 311 | max_c = 0.25 # max curvature 312 | path_x, path_y, yaw = [], [], [] 313 | 314 | for i in range(len(states) - 1): 315 | s_x = states[i][0] 316 | s_y = states[i][1] 317 | s_yaw = np.deg2rad(states[i][2]) 318 | g_x = states[i + 1][0] 319 | g_y = states[i + 1][1] 320 | g_yaw = np.deg2rad(states[i + 1][2]) 321 | 322 | path_i = calc_dubins_path(s_x, s_y, s_yaw, g_x, g_y, g_yaw, max_c) 323 | 324 | for x, y, iyaw in zip(path_i.x, path_i.y, path_i.yaw): 325 | path_x.append(x) 326 | path_y.append(y) 327 | yaw.append(iyaw) 328 | 329 | # animation 330 | plt.ion() 331 | plt.figure(1) 332 | 333 | for i in range(len(path_x)): 334 | plt.clf() 335 | plt.plot(path_x, path_y, linewidth=1, color='gray') 336 | 337 | for x, y, theta in states: 338 | draw.Arrow(x, y, np.deg2rad(theta), 2, 'blueviolet') 339 | 340 | draw.Car(path_x[i], path_y[i], yaw[i], 1.5, 3) 341 | 342 | plt.axis("equal") 343 | plt.title("Simulation of Dubins Path") 344 | plt.axis([-10, 42, -20, 20]) 345 | plt.draw() 346 | plt.pause(0.001) 347 | 348 | plt.pause(1) 349 | 350 | 351 | if __name__ == '__main__': 352 | main() 353 | -------------------------------------------------------------------------------- /CurvesGenerator/quartic_polynomial.py: -------------------------------------------------------------------------------- 1 | """ 2 | Quartic Polynomial 3 | """ 4 | 5 | import numpy as np 6 | 7 | 8 | class QuarticPolynomial: 9 | def __init__(self, x0, v0, a0, v1, a1, T): 10 | A = np.array([[3 * T ** 2, 4 * T ** 3], 11 | [6 * T, 12 * T ** 2]]) 12 | b = np.array([v1 - v0 - a0 * T, 13 | a1 - a0]) 14 | X = np.linalg.solve(A, b) 15 | 16 | self.a0 = x0 17 | self.a1 = v0 18 | self.a2 = a0 / 2.0 19 | self.a3 = X[0] 20 | self.a4 = X[1] 21 | 22 | def calc_xt(self, t): 23 | xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ 24 | self.a3 * t ** 3 + self.a4 * t ** 4 25 | 26 | return xt 27 | 28 | def calc_dxt(self, t): 29 | xt = self.a1 + 2 * self.a2 * t + \ 30 | 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 31 | 32 | return xt 33 | 34 | def calc_ddxt(self, t): 35 | xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 36 | 37 | return xt 38 | 39 | def calc_dddxt(self, t): 40 | xt = 6 * self.a3 + 24 * self.a4 * t 41 | 42 | return xt 43 | 44 | -------------------------------------------------------------------------------- /CurvesGenerator/quintic_polynomial.py: -------------------------------------------------------------------------------- 1 | """ 2 | Quintic Polynomial 3 | """ 4 | 5 | import math 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | 9 | from CurvesGenerator import draw 10 | 11 | 12 | class QuinticPolynomial: 13 | def __init__(self, x0, v0, a0, x1, v1, a1, T): 14 | A = np.array([[T ** 3, T ** 4, T ** 5], 15 | [3 * T ** 2, 4 * T ** 3, 5 * T ** 4], 16 | [6 * T, 12 * T ** 2, 20 * T ** 3]]) 17 | b = np.array([x1 - x0 - v0 * T - a0 * T ** 2 / 2, 18 | v1 - v0 - a0 * T, 19 | a1 - a0]) 20 | X = np.linalg.solve(A, b) 21 | 22 | self.a0 = x0 23 | self.a1 = v0 24 | self.a2 = a0 / 2.0 25 | self.a3 = X[0] 26 | self.a4 = X[1] 27 | self.a5 = X[2] 28 | 29 | def calc_xt(self, t): 30 | xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ 31 | self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 32 | 33 | return xt 34 | 35 | def calc_dxt(self, t): 36 | dxt = self.a1 + 2 * self.a2 * t + \ 37 | 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 38 | 39 | return dxt 40 | 41 | def calc_ddxt(self, t): 42 | ddxt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 43 | 44 | return ddxt 45 | 46 | def calc_dddxt(self, t): 47 | dddxt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 48 | 49 | return dddxt 50 | 51 | 52 | class Trajectory: 53 | def __init__(self): 54 | self.t = [] 55 | self.x = [] 56 | self.y = [] 57 | self.yaw = [] 58 | self.v = [] 59 | self.a = [] 60 | self.jerk = [] 61 | 62 | 63 | def simulation(): 64 | sx, sy, syaw, sv, sa = 10.0, 10.0, np.deg2rad(10.0), 1.0, 0.1 65 | gx, gy, gyaw, gv, ga = 30.0, -10.0, np.deg2rad(180.0), 1.0, 0.1 66 | 67 | MAX_ACCEL = 1.0 # max accel [m/s2] 68 | MAX_JERK = 0.5 # max jerk [m/s3] 69 | dt = 0.1 # T tick [s] 70 | 71 | MIN_T = 5 72 | MAX_T = 100 73 | T_STEP = 5 74 | 75 | sv_x = sv * math.cos(syaw) 76 | sv_y = sv * math.sin(syaw) 77 | gv_x = gv * math.cos(gyaw) 78 | gv_y = gv * math.sin(gyaw) 79 | 80 | sa_x = sa * math.cos(syaw) 81 | sa_y = sa * math.sin(syaw) 82 | ga_x = ga * math.cos(gyaw) 83 | ga_y = ga * math.sin(gyaw) 84 | 85 | path = Trajectory() 86 | 87 | for T in np.arange(MIN_T, MAX_T, T_STEP): 88 | path = Trajectory() 89 | xqp = QuinticPolynomial(sx, sv_x, sa_x, gx, gv_x, ga_x, T) 90 | yqp = QuinticPolynomial(sy, sv_y, sa_y, gy, gv_y, ga_y, T) 91 | 92 | for t in np.arange(0.0, T + dt, dt): 93 | path.t.append(t) 94 | path.x.append(xqp.calc_xt(t)) 95 | path.y.append(yqp.calc_xt(t)) 96 | 97 | vx = xqp.calc_dxt(t) 98 | vy = yqp.calc_dxt(t) 99 | path.v.append(np.hypot(vx, vy)) 100 | path.yaw.append(math.atan2(vy, vx)) 101 | 102 | ax = xqp.calc_ddxt(t) 103 | ay = yqp.calc_ddxt(t) 104 | a = np.hypot(ax, ay) 105 | 106 | if len(path.v) >= 2 and path.v[-1] - path.v[-2] < 0.0: 107 | a *= -1 108 | path.a.append(a) 109 | 110 | jx = xqp.calc_dddxt(t) 111 | jy = yqp.calc_dddxt(t) 112 | j = np.hypot(jx, jy) 113 | 114 | if len(path.a) >= 2 and path.a[-1] - path.a[-2] < 0.0: 115 | j *= -1 116 | path.jerk.append(j) 117 | 118 | if max(np.abs(path.a)) <= MAX_ACCEL and max(np.abs(path.jerk)) <= MAX_JERK: 119 | break 120 | 121 | print("t_len: ", path.t, "s") 122 | print("max_v: ", max(path.v), "m/s") 123 | print("max_a: ", max(np.abs(path.a)), "m/s2") 124 | print("max_jerk: ", max(np.abs(path.jerk)), "m/s3") 125 | 126 | for i in range(len(path.t)): 127 | plt.cla() 128 | plt.gcf().canvas.mpl_connect('key_release_event', 129 | lambda event: [exit(0) if event.key == 'escape' else None]) 130 | plt.axis("equal") 131 | plt.plot(path.x, path.y, linewidth=2, color='gray') 132 | draw.Car(sx, sy, syaw, 1.5, 3) 133 | draw.Car(gx, gy, gyaw, 1.5, 3) 134 | draw.Car(path.x[i], path.y[i], path.yaw[i], 1.5, 3) 135 | plt.title("Quintic Polynomial Curves") 136 | plt.pause(0.001) 137 | 138 | plt.show() 139 | 140 | 141 | if __name__ == '__main__': 142 | simulation() 143 | -------------------------------------------------------------------------------- /CurvesGenerator/reeds_shepp.py: -------------------------------------------------------------------------------- 1 | import time 2 | import math 3 | import numpy as np 4 | 5 | 6 | # parameters initiation 7 | STEP_SIZE = 0.2 8 | MAX_LENGTH = 1000.0 9 | PI = math.pi 10 | 11 | 12 | # class for PATH element 13 | class PATH: 14 | def __init__(self, lengths, ctypes, L, x, y, yaw, directions): 15 | self.lengths = lengths # lengths of each part of path (+: forward, -: backward) [float] 16 | self.ctypes = ctypes # type of each part of the path [string] 17 | self.L = L # total path length [float] 18 | self.x = x # final x positions [m] 19 | self.y = y # final y positions [m] 20 | self.yaw = yaw # final yaw angles [rad] 21 | self.directions = directions # forward: 1, backward:-1 22 | 23 | 24 | def calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): 25 | paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=step_size) 26 | 27 | minL = paths[0].L 28 | mini = 0 29 | 30 | for i in range(len(paths)): 31 | if paths[i].L <= minL: 32 | minL, mini = paths[i].L, i 33 | 34 | return paths[mini] 35 | 36 | 37 | def calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=STEP_SIZE): 38 | q0 = [sx, sy, syaw] 39 | q1 = [gx, gy, gyaw] 40 | 41 | paths = generate_path(q0, q1, maxc) 42 | 43 | for path in paths: 44 | x, y, yaw, directions = \ 45 | generate_local_course(path.L, path.lengths, 46 | path.ctypes, maxc, step_size * maxc) 47 | 48 | # convert global coordinate 49 | path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for (ix, iy) in zip(x, y)] 50 | path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for (ix, iy) in zip(x, y)] 51 | path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw] 52 | path.directions = directions 53 | path.lengths = [l / maxc for l in path.lengths] 54 | path.L = path.L / maxc 55 | 56 | return paths 57 | 58 | 59 | def set_path(paths, lengths, ctypes): 60 | path = PATH([], [], 0.0, [], [], [], []) 61 | path.ctypes = ctypes 62 | path.lengths = lengths 63 | 64 | # check same path exist 65 | for path_e in paths: 66 | if path_e.ctypes == path.ctypes: 67 | if sum([x - y for x, y in zip(path_e.lengths, path.lengths)]) <= 0.01: 68 | return paths # not insert path 69 | 70 | path.L = sum([abs(i) for i in lengths]) 71 | 72 | if path.L >= MAX_LENGTH: 73 | return paths 74 | 75 | assert path.L >= 0.01 76 | paths.append(path) 77 | 78 | return paths 79 | 80 | 81 | def LSL(x, y, phi): 82 | u, t = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) 83 | 84 | if t >= 0.0: 85 | v = M(phi - t) 86 | if v >= 0.0: 87 | return True, t, u, v 88 | 89 | return False, 0.0, 0.0, 0.0 90 | 91 | 92 | def LSR(x, y, phi): 93 | u1, t1 = R(x + math.sin(phi), y - 1.0 - math.cos(phi)) 94 | u1 = u1 ** 2 95 | 96 | if u1 >= 4.0: 97 | u = math.sqrt(u1 - 4.0) 98 | theta = math.atan2(2.0, u) 99 | t = M(t1 + theta) 100 | v = M(t - phi) 101 | 102 | if t >= 0.0 and v >= 0.0: 103 | return True, t, u, v 104 | 105 | return False, 0.0, 0.0, 0.0 106 | 107 | 108 | def LRL(x, y, phi): 109 | u1, t1 = R(x - math.sin(phi), y - 1.0 + math.cos(phi)) 110 | 111 | if u1 <= 4.0: 112 | u = -2.0 * math.asin(0.25 * u1) 113 | t = M(t1 + 0.5 * u + PI) 114 | v = M(phi - t + u) 115 | 116 | if t >= 0.0 and u <= 0.0: 117 | return True, t, u, v 118 | 119 | return False, 0.0, 0.0, 0.0 120 | 121 | 122 | def SCS(x, y, phi, paths): 123 | flag, t, u, v = SLS(x, y, phi) 124 | 125 | if flag: 126 | paths = set_path(paths, [t, u, v], ["S", "WB", "S"]) 127 | 128 | flag, t, u, v = SLS(x, -y, -phi) 129 | if flag: 130 | paths = set_path(paths, [t, u, v], ["S", "R", "S"]) 131 | 132 | return paths 133 | 134 | 135 | def SLS(x, y, phi): 136 | phi = M(phi) 137 | 138 | if y > 0.0 and 0.0 < phi < PI * 0.99: 139 | xd = -y / math.tan(phi) + x 140 | t = xd - math.tan(phi / 2.0) 141 | u = phi 142 | v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) 143 | return True, t, u, v 144 | elif y < 0.0 and 0.0 < phi < PI * 0.99: 145 | xd = -y / math.tan(phi) + x 146 | t = xd - math.tan(phi / 2.0) 147 | u = phi 148 | v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) 149 | return True, t, u, v 150 | 151 | return False, 0.0, 0.0, 0.0 152 | 153 | 154 | def CSC(x, y, phi, paths): 155 | flag, t, u, v = LSL(x, y, phi) 156 | if flag: 157 | paths = set_path(paths, [t, u, v], ["WB", "S", "WB"]) 158 | 159 | flag, t, u, v = LSL(-x, y, -phi) 160 | if flag: 161 | paths = set_path(paths, [-t, -u, -v], ["WB", "S", "WB"]) 162 | 163 | flag, t, u, v = LSL(x, -y, -phi) 164 | if flag: 165 | paths = set_path(paths, [t, u, v], ["R", "S", "R"]) 166 | 167 | flag, t, u, v = LSL(-x, -y, phi) 168 | if flag: 169 | paths = set_path(paths, [-t, -u, -v], ["R", "S", "R"]) 170 | 171 | flag, t, u, v = LSR(x, y, phi) 172 | if flag: 173 | paths = set_path(paths, [t, u, v], ["WB", "S", "R"]) 174 | 175 | flag, t, u, v = LSR(-x, y, -phi) 176 | if flag: 177 | paths = set_path(paths, [-t, -u, -v], ["WB", "S", "R"]) 178 | 179 | flag, t, u, v = LSR(x, -y, -phi) 180 | if flag: 181 | paths = set_path(paths, [t, u, v], ["R", "S", "WB"]) 182 | 183 | flag, t, u, v = LSR(-x, -y, phi) 184 | if flag: 185 | paths = set_path(paths, [-t, -u, -v], ["R", "S", "WB"]) 186 | 187 | return paths 188 | 189 | 190 | def CCC(x, y, phi, paths): 191 | flag, t, u, v = LRL(x, y, phi) 192 | if flag: 193 | paths = set_path(paths, [t, u, v], ["WB", "R", "WB"]) 194 | 195 | flag, t, u, v = LRL(-x, y, -phi) 196 | if flag: 197 | paths = set_path(paths, [-t, -u, -v], ["WB", "R", "WB"]) 198 | 199 | flag, t, u, v = LRL(x, -y, -phi) 200 | if flag: 201 | paths = set_path(paths, [t, u, v], ["R", "WB", "R"]) 202 | 203 | flag, t, u, v = LRL(-x, -y, phi) 204 | if flag: 205 | paths = set_path(paths, [-t, -u, -v], ["R", "WB", "R"]) 206 | 207 | # backwards 208 | xb = x * math.cos(phi) + y * math.sin(phi) 209 | yb = x * math.sin(phi) - y * math.cos(phi) 210 | 211 | flag, t, u, v = LRL(xb, yb, phi) 212 | if flag: 213 | paths = set_path(paths, [v, u, t], ["WB", "R", "WB"]) 214 | 215 | flag, t, u, v = LRL(-xb, yb, -phi) 216 | if flag: 217 | paths = set_path(paths, [-v, -u, -t], ["WB", "R", "WB"]) 218 | 219 | flag, t, u, v = LRL(xb, -yb, -phi) 220 | if flag: 221 | paths = set_path(paths, [v, u, t], ["R", "WB", "R"]) 222 | 223 | flag, t, u, v = LRL(-xb, -yb, phi) 224 | if flag: 225 | paths = set_path(paths, [-v, -u, -t], ["R", "WB", "R"]) 226 | 227 | return paths 228 | 229 | 230 | def calc_tauOmega(u, v, xi, eta, phi): 231 | delta = M(u - v) 232 | A = math.sin(u) - math.sin(delta) 233 | B = math.cos(u) - math.cos(delta) - 1.0 234 | 235 | t1 = math.atan2(eta * A - xi * B, xi * A + eta * B) 236 | t2 = 2.0 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0 237 | 238 | if t2 < 0: 239 | tau = M(t1 + PI) 240 | else: 241 | tau = M(t1) 242 | 243 | omega = M(tau - u + v - phi) 244 | 245 | return tau, omega 246 | 247 | 248 | def LRLRn(x, y, phi): 249 | xi = x + math.sin(phi) 250 | eta = y - 1.0 - math.cos(phi) 251 | rho = 0.25 * (2.0 + math.sqrt(xi * xi + eta * eta)) 252 | 253 | if rho <= 1.0: 254 | u = math.acos(rho) 255 | t, v = calc_tauOmega(u, -u, xi, eta, phi) 256 | if t >= 0.0 and v <= 0.0: 257 | return True, t, u, v 258 | 259 | return False, 0.0, 0.0, 0.0 260 | 261 | 262 | def LRLRp(x, y, phi): 263 | xi = x + math.sin(phi) 264 | eta = y - 1.0 - math.cos(phi) 265 | rho = (20.0 - xi * xi - eta * eta) / 16.0 266 | 267 | if 0.0 <= rho <= 1.0: 268 | u = -math.acos(rho) 269 | if u >= -0.5 * PI: 270 | t, v = calc_tauOmega(u, u, xi, eta, phi) 271 | if t >= 0.0 and v >= 0.0: 272 | return True, t, u, v 273 | 274 | return False, 0.0, 0.0, 0.0 275 | 276 | 277 | def CCCC(x, y, phi, paths): 278 | flag, t, u, v = LRLRn(x, y, phi) 279 | if flag: 280 | paths = set_path(paths, [t, u, -u, v], ["WB", "R", "WB", "R"]) 281 | 282 | flag, t, u, v = LRLRn(-x, y, -phi) 283 | if flag: 284 | paths = set_path(paths, [-t, -u, u, -v], ["WB", "R", "WB", "R"]) 285 | 286 | flag, t, u, v = LRLRn(x, -y, -phi) 287 | if flag: 288 | paths = set_path(paths, [t, u, -u, v], ["R", "WB", "R", "WB"]) 289 | 290 | flag, t, u, v = LRLRn(-x, -y, phi) 291 | if flag: 292 | paths = set_path(paths, [-t, -u, u, -v], ["R", "WB", "R", "WB"]) 293 | 294 | flag, t, u, v = LRLRp(x, y, phi) 295 | if flag: 296 | paths = set_path(paths, [t, u, u, v], ["WB", "R", "WB", "R"]) 297 | 298 | flag, t, u, v = LRLRp(-x, y, -phi) 299 | if flag: 300 | paths = set_path(paths, [-t, -u, -u, -v], ["WB", "R", "WB", "R"]) 301 | 302 | flag, t, u, v = LRLRp(x, -y, -phi) 303 | if flag: 304 | paths = set_path(paths, [t, u, u, v], ["R", "WB", "R", "WB"]) 305 | 306 | flag, t, u, v = LRLRp(-x, -y, phi) 307 | if flag: 308 | paths = set_path(paths, [-t, -u, -u, -v], ["R", "WB", "R", "WB"]) 309 | 310 | return paths 311 | 312 | 313 | def LRSR(x, y, phi): 314 | xi = x + math.sin(phi) 315 | eta = y - 1.0 - math.cos(phi) 316 | rho, theta = R(-eta, xi) 317 | 318 | if rho >= 2.0: 319 | t = theta 320 | u = 2.0 - rho 321 | v = M(t + 0.5 * PI - phi) 322 | if t >= 0.0 and u <= 0.0 and v <= 0.0: 323 | return True, t, u, v 324 | 325 | return False, 0.0, 0.0, 0.0 326 | 327 | 328 | def LRSL(x, y, phi): 329 | xi = x - math.sin(phi) 330 | eta = y - 1.0 + math.cos(phi) 331 | rho, theta = R(xi, eta) 332 | 333 | if rho >= 2.0: 334 | r = math.sqrt(rho * rho - 4.0) 335 | u = 2.0 - r 336 | t = M(theta + math.atan2(r, -2.0)) 337 | v = M(phi - 0.5 * PI - t) 338 | if t >= 0.0 and u <= 0.0 and v <= 0.0: 339 | return True, t, u, v 340 | 341 | return False, 0.0, 0.0, 0.0 342 | 343 | 344 | def CCSC(x, y, phi, paths): 345 | flag, t, u, v = LRSL(x, y, phi) 346 | if flag: 347 | paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "WB"]) 348 | 349 | flag, t, u, v = LRSL(-x, y, -phi) 350 | if flag: 351 | paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "WB"]) 352 | 353 | flag, t, u, v = LRSL(x, -y, -phi) 354 | if flag: 355 | paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "R"]) 356 | 357 | flag, t, u, v = LRSL(-x, -y, phi) 358 | if flag: 359 | paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "R"]) 360 | 361 | flag, t, u, v = LRSR(x, y, phi) 362 | if flag: 363 | paths = set_path(paths, [t, -0.5 * PI, u, v], ["WB", "R", "S", "R"]) 364 | 365 | flag, t, u, v = LRSR(-x, y, -phi) 366 | if flag: 367 | paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["WB", "R", "S", "R"]) 368 | 369 | flag, t, u, v = LRSR(x, -y, -phi) 370 | if flag: 371 | paths = set_path(paths, [t, -0.5 * PI, u, v], ["R", "WB", "S", "WB"]) 372 | 373 | flag, t, u, v = LRSR(-x, -y, phi) 374 | if flag: 375 | paths = set_path(paths, [-t, 0.5 * PI, -u, -v], ["R", "WB", "S", "WB"]) 376 | 377 | # backwards 378 | xb = x * math.cos(phi) + y * math.sin(phi) 379 | yb = x * math.sin(phi) - y * math.cos(phi) 380 | 381 | flag, t, u, v = LRSL(xb, yb, phi) 382 | if flag: 383 | paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "R", "WB"]) 384 | 385 | flag, t, u, v = LRSL(-xb, yb, -phi) 386 | if flag: 387 | paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "R", "WB"]) 388 | 389 | flag, t, u, v = LRSL(xb, -yb, -phi) 390 | if flag: 391 | paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "WB", "R"]) 392 | 393 | flag, t, u, v = LRSL(-xb, -yb, phi) 394 | if flag: 395 | paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "WB", "R"]) 396 | 397 | flag, t, u, v = LRSR(xb, yb, phi) 398 | if flag: 399 | paths = set_path(paths, [v, u, -0.5 * PI, t], ["R", "S", "R", "WB"]) 400 | 401 | flag, t, u, v = LRSR(-xb, yb, -phi) 402 | if flag: 403 | paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["R", "S", "R", "WB"]) 404 | 405 | flag, t, u, v = LRSR(xb, -yb, -phi) 406 | if flag: 407 | paths = set_path(paths, [v, u, -0.5 * PI, t], ["WB", "S", "WB", "R"]) 408 | 409 | flag, t, u, v = LRSR(-xb, -yb, phi) 410 | if flag: 411 | paths = set_path(paths, [-v, -u, 0.5 * PI, -t], ["WB", "S", "WB", "R"]) 412 | 413 | return paths 414 | 415 | 416 | def LRSLR(x, y, phi): 417 | # formula 8.11 *** TYPO IN PAPER *** 418 | xi = x + math.sin(phi) 419 | eta = y - 1.0 - math.cos(phi) 420 | rho, theta = R(xi, eta) 421 | 422 | if rho >= 2.0: 423 | u = 4.0 - math.sqrt(rho * rho - 4.0) 424 | if u <= 0.0: 425 | t = M(math.atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta)) 426 | v = M(t - phi) 427 | 428 | if t >= 0.0 and v >= 0.0: 429 | return True, t, u, v 430 | 431 | return False, 0.0, 0.0, 0.0 432 | 433 | 434 | def CCSCC(x, y, phi, paths): 435 | flag, t, u, v = LRSLR(x, y, phi) 436 | if flag: 437 | paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["WB", "R", "S", "WB", "R"]) 438 | 439 | flag, t, u, v = LRSLR(-x, y, -phi) 440 | if flag: 441 | paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["WB", "R", "S", "WB", "R"]) 442 | 443 | flag, t, u, v = LRSLR(x, -y, -phi) 444 | if flag: 445 | paths = set_path(paths, [t, -0.5 * PI, u, -0.5 * PI, v], ["R", "WB", "S", "R", "WB"]) 446 | 447 | flag, t, u, v = LRSLR(-x, -y, phi) 448 | if flag: 449 | paths = set_path(paths, [-t, 0.5 * PI, -u, 0.5 * PI, -v], ["R", "WB", "S", "R", "WB"]) 450 | 451 | return paths 452 | 453 | 454 | def generate_local_course(L, lengths, mode, maxc, step_size): 455 | point_num = int(L / step_size) + len(lengths) + 3 456 | 457 | px = [0.0 for _ in range(point_num)] 458 | py = [0.0 for _ in range(point_num)] 459 | pyaw = [0.0 for _ in range(point_num)] 460 | directions = [0 for _ in range(point_num)] 461 | ind = 1 462 | 463 | if lengths[0] > 0.0: 464 | directions[0] = 1 465 | else: 466 | directions[0] = -1 467 | 468 | if lengths[0] > 0.0: 469 | d = step_size 470 | else: 471 | d = -step_size 472 | 473 | ll = 0.0 474 | 475 | for m, l, i in zip(mode, lengths, range(len(mode))): 476 | if l > 0.0: 477 | d = step_size 478 | else: 479 | d = -step_size 480 | 481 | ox, oy, oyaw = px[ind], py[ind], pyaw[ind] 482 | 483 | ind -= 1 484 | if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: 485 | pd = -d - ll 486 | else: 487 | pd = d - ll 488 | 489 | while abs(pd) <= abs(l): 490 | ind += 1 491 | px, py, pyaw, directions = \ 492 | interpolate(ind, pd, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) 493 | pd += d 494 | 495 | ll = l - pd - d # calc remain length 496 | 497 | ind += 1 498 | px, py, pyaw, directions = \ 499 | interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions) 500 | 501 | if len(px) <= 1: 502 | return [], [], [], [] 503 | 504 | # remove unused data 505 | while len(px) >= 1 and px[-1] == 0.0: 506 | px.pop() 507 | py.pop() 508 | pyaw.pop() 509 | directions.pop() 510 | 511 | return px, py, pyaw, directions 512 | 513 | 514 | def interpolate(ind, l, m, maxc, ox, oy, oyaw, px, py, pyaw, directions): 515 | if m == "S": 516 | px[ind] = ox + l / maxc * math.cos(oyaw) 517 | py[ind] = oy + l / maxc * math.sin(oyaw) 518 | pyaw[ind] = oyaw 519 | else: 520 | ldx = math.sin(l) / maxc 521 | if m == "WB": 522 | ldy = (1.0 - math.cos(l)) / maxc 523 | elif m == "R": 524 | ldy = (1.0 - math.cos(l)) / (-maxc) 525 | 526 | gdx = math.cos(-oyaw) * ldx + math.sin(-oyaw) * ldy 527 | gdy = -math.sin(-oyaw) * ldx + math.cos(-oyaw) * ldy 528 | px[ind] = ox + gdx 529 | py[ind] = oy + gdy 530 | 531 | if m == "WB": 532 | pyaw[ind] = oyaw + l 533 | elif m == "R": 534 | pyaw[ind] = oyaw - l 535 | 536 | if l > 0.0: 537 | directions[ind] = 1 538 | else: 539 | directions[ind] = -1 540 | 541 | return px, py, pyaw, directions 542 | 543 | 544 | def generate_path(q0, q1, maxc): 545 | dx = q1[0] - q0[0] 546 | dy = q1[1] - q0[1] 547 | dth = q1[2] - q0[2] 548 | c = math.cos(q0[2]) 549 | s = math.sin(q0[2]) 550 | x = (c * dx + s * dy) * maxc 551 | y = (-s * dx + c * dy) * maxc 552 | 553 | paths = [] 554 | paths = SCS(x, y, dth, paths) 555 | paths = CSC(x, y, dth, paths) 556 | paths = CCC(x, y, dth, paths) 557 | paths = CCCC(x, y, dth, paths) 558 | paths = CCSC(x, y, dth, paths) 559 | paths = CCSCC(x, y, dth, paths) 560 | 561 | return paths 562 | 563 | 564 | # utils 565 | def pi_2_pi(theta): 566 | while theta > PI: 567 | theta -= 2.0 * PI 568 | 569 | while theta < -PI: 570 | theta += 2.0 * PI 571 | 572 | return theta 573 | 574 | 575 | def R(x, y): 576 | """ 577 | Return the polar coordinates (r, theta) of the point (x, y) 578 | """ 579 | r = math.hypot(x, y) 580 | theta = math.atan2(y, x) 581 | 582 | return r, theta 583 | 584 | 585 | def M(theta): 586 | """ 587 | Regulate theta to -pi <= theta < pi 588 | """ 589 | phi = theta % (2.0 * PI) 590 | 591 | if phi < -PI: 592 | phi += 2.0 * PI 593 | if phi > PI: 594 | phi -= 2.0 * PI 595 | 596 | return phi 597 | 598 | 599 | def get_label(path): 600 | label = "" 601 | 602 | for m, l in zip(path.ctypes, path.lengths): 603 | label = label + m 604 | if l > 0.0: 605 | label = label + "+" 606 | else: 607 | label = label + "-" 608 | 609 | return label 610 | 611 | 612 | def calc_curvature(x, y, yaw, directions): 613 | c, ds = [], [] 614 | 615 | for i in range(1, len(x) - 1): 616 | dxn = x[i] - x[i - 1] 617 | dxp = x[i + 1] - x[i] 618 | dyn = y[i] - y[i - 1] 619 | dyp = y[i + 1] - y[i] 620 | dn = math.hypot(dxn, dyn) 621 | dp = math.hypot(dxp, dyp) 622 | dx = 1.0 / (dn + dp) * (dp / dn * dxn + dn / dp * dxp) 623 | ddx = 2.0 / (dn + dp) * (dxp / dp - dxn / dn) 624 | dy = 1.0 / (dn + dp) * (dp / dn * dyn + dn / dp * dyp) 625 | ddy = 2.0 / (dn + dp) * (dyp / dp - dyn / dn) 626 | curvature = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 627 | d = (dn + dp) / 2.0 628 | 629 | if np.isnan(curvature): 630 | curvature = 0.0 631 | 632 | if directions[i] <= 0.0: 633 | curvature = -curvature 634 | 635 | if len(c) == 0: 636 | ds.append(d) 637 | c.append(curvature) 638 | 639 | ds.append(d) 640 | c.append(curvature) 641 | 642 | ds.append(ds[-1]) 643 | c.append(c[-1]) 644 | 645 | return c, ds 646 | 647 | 648 | def check_path(sx, sy, syaw, gx, gy, gyaw, maxc): 649 | paths = calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc) 650 | 651 | assert len(paths) >= 1 652 | 653 | for path in paths: 654 | assert abs(path.x[0] - sx) <= 0.01 655 | assert abs(path.y[0] - sy) <= 0.01 656 | assert abs(path.yaw[0] - syaw) <= 0.01 657 | assert abs(path.x[-1] - gx) <= 0.01 658 | assert abs(path.y[-1] - gy) <= 0.01 659 | assert abs(path.yaw[-1] - gyaw) <= 0.01 660 | 661 | # course distance check 662 | d = [math.hypot(dx, dy) 663 | for dx, dy in zip(np.diff(path.x[0:len(path.x) - 1]), 664 | np.diff(path.y[0:len(path.y) - 1]))] 665 | 666 | for i in range(len(d)): 667 | assert abs(d[i] - STEP_SIZE) <= 0.001 668 | 669 | 670 | def main(): 671 | start_x = 3.0 # [m] 672 | start_y = 10.0 # [m] 673 | start_yaw = np.deg2rad(40.0) # [rad] 674 | end_x = 0.0 # [m] 675 | end_y = 1.0 # [m] 676 | end_yaw = np.deg2rad(0.0) # [rad] 677 | max_curvature = 0.1 678 | 679 | t0 = time.time() 680 | 681 | for i in range(1000): 682 | _ = calc_optimal_path(start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature) 683 | 684 | t1 = time.time() 685 | print(t1 - t0) 686 | 687 | 688 | if __name__ == '__main__': 689 | main() 690 | -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/astar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/astar.cpython-37.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/astar.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/astar.cpython-38.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/draw.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/draw.cpython-37.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/draw.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/draw.cpython-38.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/hybrid_astar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/hybrid_astar.cpython-37.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/__pycache__/hybrid_astar.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/__pycache__/hybrid_astar.cpython-38.pyc -------------------------------------------------------------------------------- /HybridAstarPlanner/astar.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | import math 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | class Node: 8 | def __init__(self, x, y, cost, pind): 9 | self.x = x # x position of node 10 | self.y = y # y position of node 11 | self.cost = cost # g cost of node 12 | self.pind = pind # parent index of node 13 | 14 | 15 | class Para: 16 | def __init__(self, minx, miny, maxx, maxy, xw, yw, reso, motion): 17 | self.minx = minx 18 | self.miny = miny 19 | self.maxx = maxx 20 | self.maxy = maxy 21 | self.xw = xw 22 | self.yw = yw 23 | self.reso = reso # resolution of grid world 24 | self.motion = motion # motion set 25 | 26 | 27 | def astar_planning(sx, sy, gx, gy, ox, oy, reso, rr): 28 | """ 29 | return path of A*. 30 | :param sx: starting node x [m] 31 | :param sy: starting node y [m] 32 | :param gx: goal node x [m] 33 | :param gy: goal node y [m] 34 | :param ox: obstacles x positions [m] 35 | :param oy: obstacles y positions [m] 36 | :param reso: xy grid resolution 37 | :param rr: robot radius 38 | :return: path 39 | """ 40 | 41 | n_start = Node(round(sx / reso), round(sy / reso), 0.0, -1) 42 | n_goal = Node(round(gx / reso), round(gy / reso), 0.0, -1) 43 | 44 | ox = [x / reso for x in ox] 45 | oy = [y / reso for y in oy] 46 | 47 | P, obsmap = calc_parameters(ox, oy, rr, reso) 48 | 49 | open_set, closed_set = dict(), dict() 50 | open_set[calc_index(n_start, P)] = n_start 51 | 52 | q_priority = [] 53 | heapq.heappush(q_priority, 54 | (fvalue(n_start, n_goal), calc_index(n_start, P))) 55 | 56 | while True: 57 | if not open_set: 58 | break 59 | 60 | _, ind = heapq.heappop(q_priority) 61 | n_curr = open_set[ind] 62 | closed_set[ind] = n_curr 63 | open_set.pop(ind) 64 | 65 | for i in range(len(P.motion)): 66 | node = Node(n_curr.x + P.motion[i][0], 67 | n_curr.y + P.motion[i][1], 68 | n_curr.cost + u_cost(P.motion[i]), ind) 69 | 70 | if not check_node(node, P, obsmap): 71 | continue 72 | 73 | n_ind = calc_index(node, P) 74 | if n_ind not in closed_set: 75 | if n_ind in open_set: 76 | if open_set[n_ind].cost > node.cost: 77 | open_set[n_ind].cost = node.cost 78 | open_set[n_ind].pind = ind 79 | else: 80 | open_set[n_ind] = node 81 | heapq.heappush(q_priority, 82 | (fvalue(node, n_goal), calc_index(node, P))) 83 | 84 | pathx, pathy = extract_path(closed_set, n_start, n_goal, P) 85 | 86 | return pathx, pathy 87 | 88 | 89 | def calc_holonomic_heuristic_with_obstacle(node, ox, oy, reso, rr): 90 | n_goal = Node(round(node.x[-1] / reso), round(node.y[-1] / reso), 0.0, -1) 91 | 92 | ox = [x / reso for x in ox] 93 | oy = [y / reso for y in oy] 94 | 95 | P, obsmap = calc_parameters(ox, oy, reso, rr) 96 | 97 | open_set, closed_set = dict(), dict() 98 | open_set[calc_index(n_goal, P)] = n_goal 99 | 100 | q_priority = [] 101 | heapq.heappush(q_priority, (n_goal.cost, calc_index(n_goal, P))) 102 | 103 | while True: 104 | if not open_set: 105 | break 106 | 107 | _, ind = heapq.heappop(q_priority) 108 | n_curr = open_set[ind] 109 | closed_set[ind] = n_curr 110 | open_set.pop(ind) 111 | 112 | for i in range(len(P.motion)): 113 | node = Node(n_curr.x + P.motion[i][0], 114 | n_curr.y + P.motion[i][1], 115 | n_curr.cost + u_cost(P.motion[i]), ind) 116 | 117 | if not check_node(node, P, obsmap): 118 | continue 119 | 120 | n_ind = calc_index(node, P) 121 | if n_ind not in closed_set: 122 | if n_ind in open_set: 123 | if open_set[n_ind].cost > node.cost: 124 | open_set[n_ind].cost = node.cost 125 | open_set[n_ind].pind = ind 126 | else: 127 | open_set[n_ind] = node 128 | heapq.heappush(q_priority, (node.cost, calc_index(node, P))) 129 | 130 | hmap = [[np.inf for _ in range(P.yw)] for _ in range(P.xw)] 131 | 132 | for n in closed_set.values(): 133 | hmap[n.x - P.minx][n.y - P.miny] = n.cost 134 | 135 | return hmap 136 | 137 | 138 | def check_node(node, P, obsmap): 139 | if node.x <= P.minx or node.x >= P.maxx or \ 140 | node.y <= P.miny or node.y >= P.maxy: 141 | return False 142 | 143 | if obsmap[node.x - P.minx][node.y - P.miny]: 144 | return False 145 | 146 | return True 147 | 148 | 149 | def u_cost(u): 150 | return math.hypot(u[0], u[1]) 151 | 152 | 153 | def fvalue(node, n_goal): 154 | return node.cost + h(node, n_goal) 155 | 156 | 157 | def h(node, n_goal): 158 | return math.hypot(node.x - n_goal.x, node.y - n_goal.y) 159 | 160 | 161 | def calc_index(node, P): 162 | return (node.y - P.miny) * P.xw + (node.x - P.minx) 163 | 164 | 165 | def calc_parameters(ox, oy, rr, reso): 166 | minx, miny = round(min(ox)), round(min(oy)) 167 | maxx, maxy = round(max(ox)), round(max(oy)) 168 | xw, yw = maxx - minx, maxy - miny 169 | 170 | motion = get_motion() 171 | P = Para(minx, miny, maxx, maxy, xw, yw, reso, motion) 172 | obsmap = calc_obsmap(ox, oy, rr, P) 173 | 174 | return P, obsmap 175 | 176 | 177 | def calc_obsmap(ox, oy, rr, P): 178 | obsmap = [[False for _ in range(P.yw)] for _ in range(P.xw)] 179 | 180 | for x in range(P.xw): 181 | xx = x + P.minx 182 | for y in range(P.yw): 183 | yy = y + P.miny 184 | for oxx, oyy in zip(ox, oy): 185 | if math.hypot(oxx - xx, oyy - yy) <= rr / P.reso: 186 | obsmap[x][y] = True 187 | break 188 | 189 | return obsmap 190 | 191 | 192 | def extract_path(closed_set, n_start, n_goal, P): 193 | pathx, pathy = [n_goal.x], [n_goal.y] 194 | n_ind = calc_index(n_goal, P) 195 | 196 | while True: 197 | node = closed_set[n_ind] 198 | pathx.append(node.x) 199 | pathy.append(node.y) 200 | n_ind = node.pind 201 | 202 | if node == n_start: 203 | break 204 | 205 | pathx = [x * P.reso for x in reversed(pathx)] 206 | pathy = [y * P.reso for y in reversed(pathy)] 207 | 208 | return pathx, pathy 209 | 210 | 211 | def get_motion(): 212 | motion = [[-1, 0], [-1, 1], [0, 1], [1, 1], 213 | [1, 0], [1, -1], [0, -1], [-1, -1]] 214 | 215 | return motion 216 | 217 | 218 | def get_env(): 219 | ox, oy = [], [] 220 | 221 | for i in range(60): 222 | ox.append(i) 223 | oy.append(0.0) 224 | for i in range(60): 225 | ox.append(60.0) 226 | oy.append(i) 227 | for i in range(61): 228 | ox.append(i) 229 | oy.append(60.0) 230 | for i in range(61): 231 | ox.append(0.0) 232 | oy.append(i) 233 | for i in range(40): 234 | ox.append(20.0) 235 | oy.append(i) 236 | for i in range(40): 237 | ox.append(40.0) 238 | oy.append(60.0 - i) 239 | 240 | return ox, oy 241 | 242 | 243 | def main(): 244 | sx = 10.0 # [m] 245 | sy = 10.0 # [m] 246 | gx = 50.0 # [m] 247 | gy = 50.0 # [m] 248 | 249 | robot_radius = 2.0 250 | grid_resolution = 1.0 251 | ox, oy = get_env() 252 | 253 | pathx, pathy = astar_planning(sx, sy, gx, gy, ox, oy, grid_resolution, robot_radius) 254 | 255 | plt.plot(ox, oy, 'sk') 256 | plt.plot(pathx, pathy, '-r') 257 | plt.plot(sx, sy, 'sg') 258 | plt.plot(gx, gy, 'sb') 259 | plt.axis("equal") 260 | plt.show() 261 | 262 | 263 | if __name__ == '__main__': 264 | main() 265 | -------------------------------------------------------------------------------- /HybridAstarPlanner/draw.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import math 4 | PI = np.pi 5 | 6 | 7 | class Arrow: 8 | def __init__(self, x, y, theta, L, c): 9 | angle = np.deg2rad(30) 10 | d = 0.3 * L 11 | w = 2 12 | 13 | x_start = x 14 | y_start = y 15 | x_end = x + L * np.cos(theta) 16 | y_end = y + L * np.sin(theta) 17 | 18 | theta_hat_L = theta + PI - angle 19 | theta_hat_R = theta + PI + angle 20 | 21 | x_hat_start = x_end 22 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 23 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 24 | 25 | y_hat_start = y_end 26 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 27 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 28 | 29 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 30 | plt.plot([x_hat_start, x_hat_end_L], 31 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 32 | plt.plot([x_hat_start, x_hat_end_R], 33 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 34 | 35 | 36 | class Car: 37 | def __init__(self, x, y, yaw, w, L): 38 | theta_B = PI + yaw 39 | 40 | xB = x + L / 4 * np.cos(theta_B) 41 | yB = y + L / 4 * np.sin(theta_B) 42 | 43 | theta_BL = theta_B + PI / 2 44 | theta_BR = theta_B - PI / 2 45 | 46 | x_BL = xB + w / 2 * np.cos(theta_BL) # Bottom-Left vertex 47 | y_BL = yB + w / 2 * np.sin(theta_BL) 48 | x_BR = xB + w / 2 * np.cos(theta_BR) # Bottom-Right vertex 49 | y_BR = yB + w / 2 * np.sin(theta_BR) 50 | 51 | x_FL = x_BL + L * np.cos(yaw) # Front-Left vertex 52 | y_FL = y_BL + L * np.sin(yaw) 53 | x_FR = x_BR + L * np.cos(yaw) # Front-Right vertex 54 | y_FR = y_BR + L * np.sin(yaw) 55 | 56 | plt.plot([x_BL, x_BR, x_FR, x_FL, x_BL], 57 | [y_BL, y_BR, y_FR, y_FL, y_BL], 58 | linewidth=1, color='black') 59 | 60 | Arrow(x, y, yaw, L / 2, 'black') 61 | # plt.axis("equal") 62 | # plt.show() 63 | 64 | 65 | def draw_car(x, y, yaw, steer, C, color='black'): 66 | car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], 67 | [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]]) 68 | 69 | wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR], 70 | [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]]) 71 | 72 | rlWheel = wheel.copy() 73 | rrWheel = wheel.copy() 74 | frWheel = wheel.copy() 75 | flWheel = wheel.copy() 76 | 77 | Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], 78 | [math.sin(yaw), math.cos(yaw)]]) 79 | 80 | Rot2 = np.array([[math.cos(steer), math.sin(steer)], 81 | [-math.sin(steer), math.cos(steer)]]) 82 | 83 | frWheel = np.dot(Rot2, frWheel) 84 | flWheel = np.dot(Rot2, flWheel) 85 | 86 | frWheel += np.array([[C.WB], [-C.WD / 2]]) 87 | flWheel += np.array([[C.WB], [C.WD / 2]]) 88 | rrWheel[1, :] -= C.WD / 2 89 | rlWheel[1, :] += C.WD / 2 90 | 91 | frWheel = np.dot(Rot1, frWheel) 92 | flWheel = np.dot(Rot1, flWheel) 93 | 94 | rrWheel = np.dot(Rot1, rrWheel) 95 | rlWheel = np.dot(Rot1, rlWheel) 96 | car = np.dot(Rot1, car) 97 | 98 | frWheel += np.array([[x], [y]]) 99 | flWheel += np.array([[x], [y]]) 100 | rrWheel += np.array([[x], [y]]) 101 | rlWheel += np.array([[x], [y]]) 102 | car += np.array([[x], [y]]) 103 | 104 | plt.plot(car[0, :], car[1, :], color) 105 | plt.plot(frWheel[0, :], frWheel[1, :], color) 106 | plt.plot(rrWheel[0, :], rrWheel[1, :], color) 107 | plt.plot(flWheel[0, :], flWheel[1, :], color) 108 | plt.plot(rlWheel[0, :], rlWheel[1, :], color) 109 | Arrow(x, y, yaw, C.WB * 0.8, color) 110 | 111 | 112 | if __name__ == '__main__': 113 | # Arrow(-1, 2, 60) 114 | Car(0, 0, 1, 2, 60) 115 | -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-1.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-2.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-t1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-t1.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-t2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-t2.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-t3.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-t3.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/gif/hybrid Astar-t5.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/HybridAstarPlanner/gif/hybrid Astar-t5.gif -------------------------------------------------------------------------------- /HybridAstarPlanner/hybrid_astar.py: -------------------------------------------------------------------------------- 1 | """ 2 | Hybrid A* 3 | @author: Huiming Zhou 4 | """ 5 | 6 | import os 7 | import sys 8 | import math 9 | import heapq 10 | from heapdict import heapdict 11 | import time 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import scipy.spatial.kdtree as kd 15 | 16 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 17 | "/../../MotionPlanning/") 18 | 19 | import HybridAstarPlanner.astar as astar 20 | import HybridAstarPlanner.draw as draw 21 | import CurvesGenerator.reeds_shepp as rs 22 | 23 | 24 | class C: # Parameter config 25 | PI = math.pi 26 | 27 | XY_RESO = 2.0 # [m] 28 | YAW_RESO = np.deg2rad(15.0) # [rad] 29 | MOVE_STEP = 0.4 # [m] path interporate resolution 30 | N_STEER = 20.0 # steer command number 31 | COLLISION_CHECK_STEP = 5 # skip number for collision check 32 | EXTEND_BOUND = 1 # collision check range extended 33 | 34 | GEAR_COST = 100.0 # switch back penalty cost 35 | BACKWARD_COST = 5.0 # backward penalty cost 36 | STEER_CHANGE_COST = 5.0 # steer angle change penalty cost 37 | STEER_ANGLE_COST = 1.0 # steer angle penalty cost 38 | H_COST = 15.0 # Heuristic cost penalty cost 39 | 40 | RF = 4.5 # [m] distance from rear to vehicle front end of vehicle 41 | RB = 1.0 # [m] distance from rear to vehicle back end of vehicle 42 | W = 3.0 # [m] width of vehicle 43 | WD = 0.7 * W # [m] distance between left-right wheels 44 | WB = 3.5 # [m] Wheel base 45 | TR = 0.5 # [m] Tyre radius 46 | TW = 1 # [m] Tyre width 47 | MAX_STEER = 0.6 # [rad] maximum steering angle 48 | 49 | 50 | class Node: 51 | def __init__(self, xind, yind, yawind, direction, x, y, 52 | yaw, directions, steer, cost, pind): 53 | self.xind = xind 54 | self.yind = yind 55 | self.yawind = yawind 56 | self.direction = direction 57 | self.x = x 58 | self.y = y 59 | self.yaw = yaw 60 | self.directions = directions 61 | self.steer = steer 62 | self.cost = cost 63 | self.pind = pind 64 | 65 | 66 | class Para: 67 | def __init__(self, minx, miny, minyaw, maxx, maxy, maxyaw, 68 | xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree): 69 | self.minx = minx 70 | self.miny = miny 71 | self.minyaw = minyaw 72 | self.maxx = maxx 73 | self.maxy = maxy 74 | self.maxyaw = maxyaw 75 | self.xw = xw 76 | self.yw = yw 77 | self.yaww = yaww 78 | self.xyreso = xyreso 79 | self.yawreso = yawreso 80 | self.ox = ox 81 | self.oy = oy 82 | self.kdtree = kdtree 83 | 84 | 85 | class Path: 86 | def __init__(self, x, y, yaw, direction, cost): 87 | self.x = x 88 | self.y = y 89 | self.yaw = yaw 90 | self.direction = direction 91 | self.cost = cost 92 | 93 | 94 | class QueuePrior: 95 | def __init__(self): 96 | self.queue = heapdict() 97 | 98 | def empty(self): 99 | return len(self.queue) == 0 # if Q is empty 100 | 101 | def put(self, item, priority): 102 | self.queue[item] = priority # push 103 | 104 | def get(self): 105 | return self.queue.popitem()[0] # pop out element with smallest priority 106 | 107 | 108 | def hybrid_astar_planning(sx, sy, syaw, gx, gy, gyaw, ox, oy, xyreso, yawreso): 109 | sxr, syr = round(sx / xyreso), round(sy / xyreso) 110 | gxr, gyr = round(gx / xyreso), round(gy / xyreso) 111 | syawr = round(rs.pi_2_pi(syaw) / yawreso) 112 | gyawr = round(rs.pi_2_pi(gyaw) / yawreso) 113 | 114 | nstart = Node(sxr, syr, syawr, 1, [sx], [sy], [syaw], [1], 0.0, 0.0, -1) 115 | ngoal = Node(gxr, gyr, gyawr, 1, [gx], [gy], [gyaw], [1], 0.0, 0.0, -1) 116 | 117 | kdtree = kd.KDTree([[x, y] for x, y in zip(ox, oy)]) 118 | P = calc_parameters(ox, oy, xyreso, yawreso, kdtree) 119 | 120 | hmap = astar.calc_holonomic_heuristic_with_obstacle(ngoal, P.ox, P.oy, P.xyreso, 1.0) 121 | steer_set, direc_set = calc_motion_set() 122 | open_set, closed_set = {calc_index(nstart, P): nstart}, {} 123 | 124 | qp = QueuePrior() 125 | qp.put(calc_index(nstart, P), calc_hybrid_cost(nstart, hmap, P)) 126 | 127 | while True: 128 | if not open_set: 129 | return None 130 | 131 | ind = qp.get() 132 | n_curr = open_set[ind] 133 | closed_set[ind] = n_curr 134 | open_set.pop(ind) 135 | 136 | update, fpath = update_node_with_analystic_expantion(n_curr, ngoal, P) 137 | 138 | if update: 139 | fnode = fpath 140 | break 141 | 142 | for i in range(len(steer_set)): 143 | node = calc_next_node(n_curr, ind, steer_set[i], direc_set[i], P) 144 | 145 | if not node: 146 | continue 147 | 148 | node_ind = calc_index(node, P) 149 | 150 | if node_ind in closed_set: 151 | continue 152 | 153 | if node_ind not in open_set: 154 | open_set[node_ind] = node 155 | qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) 156 | else: 157 | if open_set[node_ind].cost > node.cost: 158 | open_set[node_ind] = node 159 | qp.put(node_ind, calc_hybrid_cost(node, hmap, P)) 160 | 161 | return extract_path(closed_set, fnode, nstart) 162 | 163 | 164 | def extract_path(closed, ngoal, nstart): 165 | rx, ry, ryaw, direc = [], [], [], [] 166 | cost = 0.0 167 | node = ngoal 168 | 169 | while True: 170 | rx += node.x[::-1] 171 | ry += node.y[::-1] 172 | ryaw += node.yaw[::-1] 173 | direc += node.directions[::-1] 174 | cost += node.cost 175 | 176 | if is_same_grid(node, nstart): 177 | break 178 | 179 | node = closed[node.pind] 180 | 181 | rx = rx[::-1] 182 | ry = ry[::-1] 183 | ryaw = ryaw[::-1] 184 | direc = direc[::-1] 185 | 186 | direc[0] = direc[1] 187 | path = Path(rx, ry, ryaw, direc, cost) 188 | 189 | return path 190 | 191 | 192 | def calc_next_node(n_curr, c_id, u, d, P): 193 | step = C.XY_RESO * 2 194 | 195 | nlist = math.ceil(step / C.MOVE_STEP) 196 | xlist = [n_curr.x[-1] + d * C.MOVE_STEP * math.cos(n_curr.yaw[-1])] 197 | ylist = [n_curr.y[-1] + d * C.MOVE_STEP * math.sin(n_curr.yaw[-1])] 198 | yawlist = [rs.pi_2_pi(n_curr.yaw[-1] + d * C.MOVE_STEP / C.WB * math.tan(u))] 199 | 200 | for i in range(nlist - 1): 201 | xlist.append(xlist[i] + d * C.MOVE_STEP * math.cos(yawlist[i])) 202 | ylist.append(ylist[i] + d * C.MOVE_STEP * math.sin(yawlist[i])) 203 | yawlist.append(rs.pi_2_pi(yawlist[i] + d * C.MOVE_STEP / C.WB * math.tan(u))) 204 | 205 | xind = round(xlist[-1] / P.xyreso) 206 | yind = round(ylist[-1] / P.xyreso) 207 | yawind = round(yawlist[-1] / P.yawreso) 208 | 209 | if not is_index_ok(xind, yind, xlist, ylist, yawlist, P): 210 | return None 211 | 212 | cost = 0.0 213 | 214 | if d > 0: 215 | direction = 1 216 | cost += abs(step) 217 | else: 218 | direction = -1 219 | cost += abs(step) * C.BACKWARD_COST 220 | 221 | if direction != n_curr.direction: # switch back penalty 222 | cost += C.GEAR_COST 223 | 224 | cost += C.STEER_ANGLE_COST * abs(u) # steer angle penalyty 225 | cost += C.STEER_CHANGE_COST * abs(n_curr.steer - u) # steer change penalty 226 | cost = n_curr.cost + cost 227 | 228 | directions = [direction for _ in range(len(xlist))] 229 | 230 | node = Node(xind, yind, yawind, direction, xlist, ylist, 231 | yawlist, directions, u, cost, c_id) 232 | 233 | return node 234 | 235 | 236 | def is_index_ok(xind, yind, xlist, ylist, yawlist, P): 237 | if xind <= P.minx or \ 238 | xind >= P.maxx or \ 239 | yind <= P.miny or \ 240 | yind >= P.maxy: 241 | return False 242 | 243 | ind = range(0, len(xlist), C.COLLISION_CHECK_STEP) 244 | 245 | nodex = [xlist[k] for k in ind] 246 | nodey = [ylist[k] for k in ind] 247 | nodeyaw = [yawlist[k] for k in ind] 248 | 249 | if is_collision(nodex, nodey, nodeyaw, P): 250 | return False 251 | 252 | return True 253 | 254 | 255 | def update_node_with_analystic_expantion(n_curr, ngoal, P): 256 | path = analystic_expantion(n_curr, ngoal, P) # rs path: n -> ngoal 257 | 258 | if not path: 259 | return False, None 260 | 261 | fx = path.x[1:-1] 262 | fy = path.y[1:-1] 263 | fyaw = path.yaw[1:-1] 264 | fd = path.directions[1:-1] 265 | 266 | fcost = n_curr.cost + calc_rs_path_cost(path) 267 | fpind = calc_index(n_curr, P) 268 | fsteer = 0.0 269 | 270 | fpath = Node(n_curr.xind, n_curr.yind, n_curr.yawind, n_curr.direction, 271 | fx, fy, fyaw, fd, fsteer, fcost, fpind) 272 | 273 | return True, fpath 274 | 275 | 276 | def analystic_expantion(node, ngoal, P): 277 | sx, sy, syaw = node.x[-1], node.y[-1], node.yaw[-1] 278 | gx, gy, gyaw = ngoal.x[-1], ngoal.y[-1], ngoal.yaw[-1] 279 | 280 | maxc = math.tan(C.MAX_STEER) / C.WB 281 | paths = rs.calc_all_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=C.MOVE_STEP) 282 | 283 | if not paths: 284 | return None 285 | 286 | pq = QueuePrior() 287 | for path in paths: 288 | pq.put(path, calc_rs_path_cost(path)) 289 | 290 | while not pq.empty(): 291 | path = pq.get() 292 | ind = range(0, len(path.x), C.COLLISION_CHECK_STEP) 293 | 294 | pathx = [path.x[k] for k in ind] 295 | pathy = [path.y[k] for k in ind] 296 | pathyaw = [path.yaw[k] for k in ind] 297 | 298 | if not is_collision(pathx, pathy, pathyaw, P): 299 | return path 300 | 301 | return None 302 | 303 | 304 | def is_collision(x, y, yaw, P): 305 | for ix, iy, iyaw in zip(x, y, yaw): 306 | d = 1 307 | dl = (C.RF - C.RB) / 2.0 308 | r = (C.RF + C.RB) / 2.0 + d 309 | 310 | cx = ix + dl * math.cos(iyaw) 311 | cy = iy + dl * math.sin(iyaw) 312 | 313 | ids = P.kdtree.query_ball_point([cx, cy], r) 314 | 315 | if not ids: 316 | continue 317 | 318 | for i in ids: 319 | xo = P.ox[i] - cx 320 | yo = P.oy[i] - cy 321 | dx = xo * math.cos(iyaw) + yo * math.sin(iyaw) 322 | dy = -xo * math.sin(iyaw) + yo * math.cos(iyaw) 323 | 324 | if abs(dx) < r and abs(dy) < C.W / 2 + d: 325 | return True 326 | 327 | return False 328 | 329 | 330 | def calc_rs_path_cost(rspath): 331 | cost = 0.0 332 | 333 | for lr in rspath.lengths: 334 | if lr >= 0: 335 | cost += 1 336 | else: 337 | cost += abs(lr) * C.BACKWARD_COST 338 | 339 | for i in range(len(rspath.lengths) - 1): 340 | if rspath.lengths[i] * rspath.lengths[i + 1] < 0.0: 341 | cost += C.GEAR_COST 342 | 343 | for ctype in rspath.ctypes: 344 | if ctype != "S": 345 | cost += C.STEER_ANGLE_COST * abs(C.MAX_STEER) 346 | 347 | nctypes = len(rspath.ctypes) 348 | ulist = [0.0 for _ in range(nctypes)] 349 | 350 | for i in range(nctypes): 351 | if rspath.ctypes[i] == "R": 352 | ulist[i] = -C.MAX_STEER 353 | elif rspath.ctypes[i] == "WB": 354 | ulist[i] = C.MAX_STEER 355 | 356 | for i in range(nctypes - 1): 357 | cost += C.STEER_CHANGE_COST * abs(ulist[i + 1] - ulist[i]) 358 | 359 | return cost 360 | 361 | 362 | def calc_hybrid_cost(node, hmap, P): 363 | cost = node.cost + \ 364 | C.H_COST * hmap[node.xind - P.minx][node.yind - P.miny] 365 | 366 | return cost 367 | 368 | 369 | def calc_motion_set(): 370 | s = np.arange(C.MAX_STEER / C.N_STEER, 371 | C.MAX_STEER, C.MAX_STEER / C.N_STEER) 372 | 373 | steer = list(s) + [0.0] + list(-s) 374 | direc = [1.0 for _ in range(len(steer))] + [-1.0 for _ in range(len(steer))] 375 | steer = steer + steer 376 | 377 | return steer, direc 378 | 379 | 380 | def is_same_grid(node1, node2): 381 | if node1.xind != node2.xind or \ 382 | node1.yind != node2.yind or \ 383 | node1.yawind != node2.yawind: 384 | return False 385 | 386 | return True 387 | 388 | 389 | def calc_index(node, P): 390 | ind = (node.yawind - P.minyaw) * P.xw * P.yw + \ 391 | (node.yind - P.miny) * P.xw + \ 392 | (node.xind - P.minx) 393 | 394 | return ind 395 | 396 | 397 | def calc_parameters(ox, oy, xyreso, yawreso, kdtree): 398 | minx = round(min(ox) / xyreso) 399 | miny = round(min(oy) / xyreso) 400 | maxx = round(max(ox) / xyreso) 401 | maxy = round(max(oy) / xyreso) 402 | 403 | xw, yw = maxx - minx, maxy - miny 404 | 405 | minyaw = round(-C.PI / yawreso) - 1 406 | maxyaw = round(C.PI / yawreso) 407 | yaww = maxyaw - minyaw 408 | 409 | return Para(minx, miny, minyaw, maxx, maxy, maxyaw, 410 | xw, yw, yaww, xyreso, yawreso, ox, oy, kdtree) 411 | 412 | 413 | def draw_car(x, y, yaw, steer, color='black'): 414 | car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], 415 | [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]]) 416 | 417 | wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR], 418 | [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]]) 419 | 420 | rlWheel = wheel.copy() 421 | rrWheel = wheel.copy() 422 | frWheel = wheel.copy() 423 | flWheel = wheel.copy() 424 | 425 | Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], 426 | [math.sin(yaw), math.cos(yaw)]]) 427 | 428 | Rot2 = np.array([[math.cos(steer), math.sin(steer)], 429 | [-math.sin(steer), math.cos(steer)]]) 430 | 431 | frWheel = np.dot(Rot2, frWheel) 432 | flWheel = np.dot(Rot2, flWheel) 433 | 434 | frWheel += np.array([[C.WB], [-C.WD / 2]]) 435 | flWheel += np.array([[C.WB], [C.WD / 2]]) 436 | rrWheel[1, :] -= C.WD / 2 437 | rlWheel[1, :] += C.WD / 2 438 | 439 | frWheel = np.dot(Rot1, frWheel) 440 | flWheel = np.dot(Rot1, flWheel) 441 | 442 | rrWheel = np.dot(Rot1, rrWheel) 443 | rlWheel = np.dot(Rot1, rlWheel) 444 | car = np.dot(Rot1, car) 445 | 446 | frWheel += np.array([[x], [y]]) 447 | flWheel += np.array([[x], [y]]) 448 | rrWheel += np.array([[x], [y]]) 449 | rlWheel += np.array([[x], [y]]) 450 | car += np.array([[x], [y]]) 451 | 452 | plt.plot(car[0, :], car[1, :], color) 453 | plt.plot(frWheel[0, :], frWheel[1, :], color) 454 | plt.plot(rrWheel[0, :], rrWheel[1, :], color) 455 | plt.plot(flWheel[0, :], flWheel[1, :], color) 456 | plt.plot(rlWheel[0, :], rlWheel[1, :], color) 457 | draw.Arrow(x, y, yaw, C.WB * 0.8, color) 458 | 459 | 460 | def design_obstacles(x, y): 461 | ox, oy = [], [] 462 | 463 | for i in range(x): 464 | ox.append(i) 465 | oy.append(0) 466 | for i in range(x): 467 | ox.append(i) 468 | oy.append(y - 1) 469 | for i in range(y): 470 | ox.append(0) 471 | oy.append(i) 472 | for i in range(y): 473 | ox.append(x - 1) 474 | oy.append(i) 475 | for i in range(10, 21): 476 | ox.append(i) 477 | oy.append(15) 478 | for i in range(15): 479 | ox.append(20) 480 | oy.append(i) 481 | for i in range(15, 30): 482 | ox.append(30) 483 | oy.append(i) 484 | for i in range(16): 485 | ox.append(40) 486 | oy.append(i) 487 | 488 | return ox, oy 489 | 490 | 491 | def main(): 492 | print("start!") 493 | x, y = 51, 31 494 | sx, sy, syaw0 = 10.0, 7.0, np.deg2rad(120.0) 495 | gx, gy, gyaw0 = 45.0, 20.0, np.deg2rad(90.0) 496 | 497 | ox, oy = design_obstacles(x, y) 498 | 499 | t0 = time.time() 500 | path = hybrid_astar_planning(sx, sy, syaw0, gx, gy, gyaw0, 501 | ox, oy, C.XY_RESO, C.YAW_RESO) 502 | t1 = time.time() 503 | print("running T: ", t1 - t0) 504 | 505 | if not path: 506 | print("Searching failed!") 507 | return 508 | 509 | x = path.x 510 | y = path.y 511 | yaw = path.yaw 512 | direction = path.direction 513 | 514 | for k in range(len(x)): 515 | plt.cla() 516 | plt.plot(ox, oy, "sk") 517 | plt.plot(x, y, linewidth=1.5, color='r') 518 | 519 | if k < len(x) - 2: 520 | dy = (yaw[k + 1] - yaw[k]) / C.MOVE_STEP 521 | steer = rs.pi_2_pi(math.atan(-C.WB * dy / direction[k])) 522 | else: 523 | steer = 0.0 524 | 525 | draw_car(gx, gy, gyaw0, 0.0, 'dimgray') 526 | draw_car(x[k], y[k], yaw[k], steer) 527 | plt.title("Hybrid A*") 528 | plt.axis("equal") 529 | plt.pause(0.0001) 530 | 531 | plt.show() 532 | print("Done!") 533 | 534 | 535 | if __name__ == '__main__': 536 | main() 537 | -------------------------------------------------------------------------------- /LatticePlanner/__pycache__/draw.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/LatticePlanner/__pycache__/draw.cpython-37.pyc -------------------------------------------------------------------------------- /LatticePlanner/__pycache__/env.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/LatticePlanner/__pycache__/env.cpython-37.pyc -------------------------------------------------------------------------------- /LatticePlanner/draw.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import math 4 | 5 | PI = np.pi 6 | 7 | 8 | class Arrow: 9 | def __init__(self, x, y, theta, L, c): 10 | angle = np.deg2rad(30) 11 | d = 0.3 * L 12 | w = 2 13 | 14 | x_start = x 15 | y_start = y 16 | x_end = x + L * np.cos(theta) 17 | y_end = y + L * np.sin(theta) 18 | 19 | theta_hat_L = theta + PI - angle 20 | theta_hat_R = theta + PI + angle 21 | 22 | x_hat_start = x_end 23 | x_hat_end_L = x_hat_start + d * np.cos(theta_hat_L) 24 | x_hat_end_R = x_hat_start + d * np.cos(theta_hat_R) 25 | 26 | y_hat_start = y_end 27 | y_hat_end_L = y_hat_start + d * np.sin(theta_hat_L) 28 | y_hat_end_R = y_hat_start + d * np.sin(theta_hat_R) 29 | 30 | plt.plot([x_start, x_end], [y_start, y_end], color=c, linewidth=w) 31 | plt.plot([x_hat_start, x_hat_end_L], 32 | [y_hat_start, y_hat_end_L], color=c, linewidth=w) 33 | plt.plot([x_hat_start, x_hat_end_R], 34 | [y_hat_start, y_hat_end_R], color=c, linewidth=w) 35 | 36 | 37 | def draw_car(x, y, yaw, steer, C, color='black'): 38 | car = np.array([[-C.RB, -C.RB, C.RF, C.RF, -C.RB], 39 | [C.W / 2, -C.W / 2, -C.W / 2, C.W / 2, C.W / 2]]) 40 | 41 | wheel = np.array([[-C.TR, -C.TR, C.TR, C.TR, -C.TR], 42 | [C.TW / 4, -C.TW / 4, -C.TW / 4, C.TW / 4, C.TW / 4]]) 43 | 44 | rlWheel = wheel.copy() 45 | rrWheel = wheel.copy() 46 | frWheel = wheel.copy() 47 | flWheel = wheel.copy() 48 | 49 | Rot1 = np.array([[math.cos(yaw), -math.sin(yaw)], 50 | [math.sin(yaw), math.cos(yaw)]]) 51 | 52 | Rot2 = np.array([[math.cos(steer), math.sin(steer)], 53 | [-math.sin(steer), math.cos(steer)]]) 54 | 55 | frWheel = np.dot(Rot2, frWheel) 56 | flWheel = np.dot(Rot2, flWheel) 57 | 58 | frWheel += np.array([[C.WB], [-C.WD / 2]]) 59 | flWheel += np.array([[C.WB], [C.WD / 2]]) 60 | rrWheel[1, :] -= C.WD / 2 61 | rlWheel[1, :] += C.WD / 2 62 | 63 | frWheel = np.dot(Rot1, frWheel) 64 | flWheel = np.dot(Rot1, flWheel) 65 | 66 | rrWheel = np.dot(Rot1, rrWheel) 67 | rlWheel = np.dot(Rot1, rlWheel) 68 | car = np.dot(Rot1, car) 69 | 70 | frWheel += np.array([[x], [y]]) 71 | flWheel += np.array([[x], [y]]) 72 | rrWheel += np.array([[x], [y]]) 73 | rlWheel += np.array([[x], [y]]) 74 | car += np.array([[x], [y]]) 75 | 76 | plt.plot(car[0, :], car[1, :], color) 77 | plt.plot(frWheel[0, :], frWheel[1, :], color) 78 | plt.plot(rrWheel[0, :], rrWheel[1, :], color) 79 | plt.plot(flWheel[0, :], flWheel[1, :], color) 80 | plt.plot(rlWheel[0, :], rlWheel[1, :], color) 81 | Arrow(x, y, yaw, C.WB * 0.8, color) -------------------------------------------------------------------------------- /LatticePlanner/env.py: -------------------------------------------------------------------------------- 1 | """ 2 | Environment for Lattice Planner Simulation 3 | """ 4 | 5 | import math 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | 9 | 10 | class ENVCrusing: 11 | def __init__(self): 12 | self.max_c = 0.15 13 | self.road_width = 8.0 14 | self.ref_line = self.design_reference_line() 15 | self.bound_in = self.design_boundary_in() 16 | self.bound_out = self.design_boundary_out() 17 | 18 | @staticmethod 19 | def design_reference_line(): 20 | rx, ry, ryaw, rc = [], [], [], [] 21 | step_curve = 0.1 * math.pi 22 | step_line = 4 23 | 24 | cx, cy, cr = 30, 30, 20 25 | theta = np.arange(math.pi, math.pi * 1.5, step_curve) 26 | for itheta in theta: 27 | rx.append(cx + cr * math.cos(itheta)) 28 | ry.append(cy + cr * math.sin(itheta)) 29 | 30 | for ix in np.arange(30, 80, step_line): 31 | rx.append(ix) 32 | ry.append(10) 33 | 34 | cx, cy, cr = 80, 25, 15 35 | theta = np.arange(-math.pi / 2.0, math.pi / 2.0, step_curve) 36 | for itheta in theta: 37 | rx.append(cx + cr * math.cos(itheta)) 38 | ry.append(cy + cr * math.sin(itheta)) 39 | 40 | for ix in np.arange(80, 60, -step_line): 41 | rx.append(ix) 42 | ry.append(40) 43 | 44 | cx, cy, cr = 60, 60, 20 45 | theta = np.arange(-math.pi / 2.0, -math.pi, -step_curve) 46 | for itheta in theta: 47 | rx.append(cx + cr * math.cos(itheta)) 48 | ry.append(cy + cr * math.sin(itheta)) 49 | 50 | cx, cy, cr = 25, 60, 15 51 | theta = np.arange(0.0, math.pi, step_curve) 52 | for itheta in theta: 53 | rx.append(cx + cr * math.cos(itheta)) 54 | ry.append(cy + cr * math.sin(itheta)) 55 | 56 | for iy in np.arange(60, 30, -step_line): 57 | rx.append(10) 58 | ry.append(iy) 59 | 60 | return rx, ry 61 | 62 | def design_boundary_in(self): 63 | bx, by = [], [] 64 | step_curve = 0.1 65 | step_line = 2 66 | road_width = self.road_width 67 | 68 | cx, cy, cr = 30, 30, 20 - road_width 69 | theta = np.arange(math.pi, math.pi * 1.5, step_curve) 70 | for itheta in theta: 71 | bx.append(cx + cr * math.cos(itheta)) 72 | by.append(cy + cr * math.sin(itheta)) 73 | 74 | for ix in np.arange(30, 80, step_line): 75 | bx.append(ix) 76 | by.append(10 + road_width) 77 | 78 | cx, cy, cr = 80, 25, 15 - road_width 79 | theta = np.arange(-math.pi / 2.0, math.pi / 2.0, step_curve) 80 | for itheta in theta: 81 | bx.append(cx + cr * math.cos(itheta)) 82 | by.append(cy + cr * math.sin(itheta)) 83 | 84 | for ix in np.arange(80, 60, -step_line): 85 | bx.append(ix) 86 | by.append(40 - road_width) 87 | 88 | cx, cy, cr = 60, 60, 20 + road_width 89 | theta = np.arange(-math.pi / 2.0, -math.pi, -step_curve) 90 | for itheta in theta: 91 | bx.append(cx + cr * math.cos(itheta)) 92 | by.append(cy + cr * math.sin(itheta)) 93 | 94 | cx, cy, cr = 25, 60, 15 - road_width 95 | theta = np.arange(0.0, math.pi, step_curve) 96 | for itheta in theta: 97 | bx.append(cx + cr * math.cos(itheta)) 98 | by.append(cy + cr * math.sin(itheta)) 99 | 100 | for iy in np.arange(60, 30, -step_line): 101 | bx.append(10 + road_width) 102 | by.append(iy) 103 | 104 | bx.append(bx[0]) 105 | by.append(by[0]) 106 | 107 | return bx, by 108 | 109 | def design_boundary_out(self): 110 | bx, by = [], [] 111 | step_curve = 0.1 112 | step_line = 2 113 | road_width = self.road_width 114 | 115 | cx, cy, cr = 30, 30, 20 + road_width 116 | theta = np.arange(math.pi, math.pi * 1.5, step_curve) 117 | for itheta in theta: 118 | bx.append(cx + cr * math.cos(itheta)) 119 | by.append(cy + cr * math.sin(itheta)) 120 | 121 | for ix in np.arange(30, 80, step_line): 122 | bx.append(ix) 123 | by.append(10 - road_width) 124 | 125 | cx, cy, cr = 80, 25, 15 + road_width 126 | theta = np.arange(-math.pi / 2.0, math.pi / 2.0, step_curve) 127 | for itheta in theta: 128 | bx.append(cx + cr * math.cos(itheta)) 129 | by.append(cy + cr * math.sin(itheta)) 130 | 131 | for ix in np.arange(80, 60, -step_line): 132 | bx.append(ix) 133 | by.append(40 + road_width) 134 | 135 | cx, cy, cr = 60, 60, 20 - road_width 136 | theta = np.arange(-math.pi / 2.0, -math.pi, -step_curve) 137 | for itheta in theta: 138 | bx.append(cx + cr * math.cos(itheta)) 139 | by.append(cy + cr * math.sin(itheta)) 140 | 141 | cx, cy, cr = 25, 60, 15 + road_width 142 | theta = np.arange(0.0, math.pi, step_curve) 143 | for itheta in theta: 144 | bx.append(cx + cr * math.cos(itheta)) 145 | by.append(cy + cr * math.sin(itheta)) 146 | 147 | for iy in np.arange(60, 30, -step_line): 148 | bx.append(10 - road_width) 149 | by.append(iy) 150 | 151 | bx.append(bx[0]) 152 | by.append(by[0]) 153 | 154 | return bx, by 155 | 156 | 157 | class ENVStopping: 158 | def __init__(self): 159 | self.road_width = 6.0 160 | self.ref_line = self.design_reference_line() 161 | self.bound_up = self.design_bound_up() 162 | self.bound_down = self.design_bound_down() 163 | 164 | @staticmethod 165 | def design_reference_line(): 166 | rx, ry = [], [] 167 | 168 | for i in np.arange(0.0, 60.0, 1.0): 169 | rx.append(i) 170 | ry.append(0.0) 171 | 172 | return rx, ry 173 | 174 | def design_bound_up(self): 175 | bx_up, by_up = [], [] 176 | 177 | for i in np.arange(0.0, 60.0, 0.1): 178 | bx_up.append(i) 179 | by_up.append(self.road_width) 180 | 181 | return bx_up, by_up 182 | 183 | def design_bound_down(self): 184 | bx_down, by_down = [], [] 185 | 186 | for i in np.arange(0.0, 60.0, 0.1): 187 | bx_down.append(i) 188 | by_down.append(-self.road_width) 189 | 190 | return bx_down, by_down 191 | 192 | 193 | def main(): 194 | env = ENVCrusing() 195 | rx, ry = env.design_reference_line() 196 | bx1, by1 = env.design_boundary_in() 197 | bx2, by2 = env.design_boundary_out() 198 | 199 | plt.plot(rx, ry, marker='.') 200 | plt.plot(bx1, by1, linewidth=1.5, color='k') 201 | plt.plot(bx2, by2, linewidth=1.5, color='k') 202 | plt.axis("equal") 203 | plt.show() 204 | 205 | 206 | if __name__ == '__main__': 207 | main() 208 | -------------------------------------------------------------------------------- /LatticePlanner/gif/Crusing.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/LatticePlanner/gif/Crusing.gif -------------------------------------------------------------------------------- /LatticePlanner/gif/StoppingMode.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhm-real/MotionPlanning/7a6b43d9c1eecf97ae6bf1aa23bc4d5aaf0e1817/LatticePlanner/gif/StoppingMode.gif -------------------------------------------------------------------------------- /LatticePlanner/lattice_planner.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import math 4 | import copy 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | 8 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 9 | "/../../MotionPlanning/") 10 | 11 | from CurvesGenerator import cubic_spline, quintic_polynomial, quartic_polynomial 12 | import LatticePlanner.env as env 13 | import LatticePlanner.draw as draw 14 | 15 | 16 | class C: 17 | # Parameter 18 | MAX_SPEED = 50.0 / 3.6 19 | MAX_ACCEL = 8.0 20 | MAX_CURVATURE = 2.5 21 | 22 | ROAD_WIDTH = 8.0 23 | ROAD_SAMPLE_STEP = 1.0 24 | 25 | T_STEP = 0.15 26 | MAX_T = 5.0 27 | MIN_T = 4.0 28 | 29 | TARGET_SPEED = 30.0 / 3.6 30 | SPEED_SAMPLE_STEP = 5.0 / 3.6 31 | 32 | # cost weights for Cruising 33 | K_JERK = 0.1 34 | K_TIME = 0.1 35 | K_V_DIFF = 1.0 36 | K_OFFSET = 1.5 37 | K_COLLISION = 500 38 | 39 | # cost weights for Stopping 40 | # K_JERK = 0.1 41 | # K_TIME = 0.1 42 | # K_V_DIFF = 200 43 | # K_OFFSET = 1.0 44 | # K_COLLISION = 500 45 | 46 | # parameters for vehicle 47 | K_SIZE = 0.9 48 | RF = 4.5 * K_SIZE # [m] distance from rear to vehicle front end of vehicle 49 | RB = 1.0 * K_SIZE # [m] distance from rear to vehicle back end of vehicle 50 | W = 3.0 * K_SIZE # [m] width of vehicle 51 | WD = 0.7 * W # [m] distance between left-right wheels 52 | WB = 3.5 * K_SIZE # [m] Wheel base 53 | TR = 0.5 * K_SIZE # [m] Tyre radius 54 | TW = 1 * K_SIZE # [m] Tyre width 55 | MAX_STEER = 0.6 # [rad] maximum steering angle 56 | 57 | 58 | class Path: 59 | def __init__(self): 60 | self.t = [] 61 | 62 | self.l = [] 63 | self.l_v = [] 64 | self.l_a = [] 65 | self.l_jerk = [] 66 | 67 | self.s = [] 68 | self.s_v = [] 69 | self.s_a = [] 70 | self.s_jerk = [] 71 | 72 | self.x = [] 73 | self.y = [] 74 | self.yaw = [] 75 | self.ds = [] 76 | self.curv = [] 77 | 78 | self.cost = 0.0 79 | 80 | 81 | def sampling_paths_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path): 82 | PATHS = dict() 83 | 84 | for s1_v in np.arange(C.TARGET_SPEED * 0.6, C.TARGET_SPEED * 1.4, C.TARGET_SPEED * 0.2): 85 | 86 | for t1 in np.arange(4.5, 5.5, 0.2): 87 | path_pre = Path() 88 | path_lon = quartic_polynomial.QuarticPolynomial(s0, s0_v, s0_a, s1_v, 0.0, t1) 89 | 90 | path_pre.t = list(np.arange(0.0, t1, C.T_STEP)) 91 | path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t] 92 | path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t] 93 | path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t] 94 | path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t] 95 | 96 | for l1 in np.arange(-C.ROAD_WIDTH, C.ROAD_WIDTH, C.ROAD_SAMPLE_STEP): 97 | path = copy.deepcopy(path_pre) 98 | path_lat = quintic_polynomial.QuinticPolynomial(l0, l0_v, l0_a, l1, 0.0, 0.0, t1) 99 | 100 | path.l = [path_lat.calc_xt(t) for t in path_pre.t] 101 | path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t] 102 | path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t] 103 | path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t] 104 | 105 | path.x, path.y = SL_2_XY(path.s, path.l, ref_path) 106 | path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y) 107 | 108 | l_jerk_sum = sum(np.abs(path.l_jerk)) 109 | s_jerk_sum = sum(np.abs(path.s_jerk)) 110 | v_diff = abs(C.TARGET_SPEED - path.s_v[-1]) 111 | 112 | path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \ 113 | C.K_V_DIFF * v_diff + \ 114 | C.K_TIME * t1 * 2 + \ 115 | C.K_OFFSET * abs(path.l[-1]) + \ 116 | C.K_COLLISION * is_path_collision(path) 117 | 118 | PATHS[path] = path.cost 119 | 120 | return PATHS 121 | 122 | 123 | def sampling_paths_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path): 124 | PATHS = dict() 125 | 126 | for s1_v in [-1.0, 0.0, 1.0, 2.0]: 127 | 128 | for t1 in np.arange(1.0, 16.0, 1.0): 129 | path_pre = Path() 130 | path_lon = quintic_polynomial.QuinticPolynomial(s0, s0_v, s0_a, 55.0, s1_v, 0.0, t1) 131 | 132 | path_pre.t = list(np.arange(0.0, t1, C.T_STEP)) 133 | path_pre.s = [path_lon.calc_xt(t) for t in path_pre.t] 134 | path_pre.s_v = [path_lon.calc_dxt(t) for t in path_pre.t] 135 | path_pre.s_a = [path_lon.calc_ddxt(t) for t in path_pre.t] 136 | path_pre.s_jerk = [path_lon.calc_dddxt(t) for t in path_pre.t] 137 | 138 | for l1 in [0.0]: 139 | path = copy.deepcopy(path_pre) 140 | path_lat = quintic_polynomial.QuinticPolynomial(l0, l0_v, l0_a, l1, 0.0, 0.0, t1) 141 | 142 | path.l = [path_lat.calc_xt(t) for t in path_pre.t] 143 | path.l_v = [path_lat.calc_dxt(t) for t in path_pre.t] 144 | path.l_a = [path_lat.calc_ddxt(t) for t in path_pre.t] 145 | path.l_jerk = [path_lat.calc_dddxt(t) for t in path_pre.t] 146 | 147 | path.x, path.y = SL_2_XY(path.s, path.l, ref_path) 148 | path.yaw, path.curv, path.ds = calc_yaw_curv(path.x, path.y) 149 | 150 | if path.yaw is None: 151 | continue 152 | 153 | l_jerk_sum = sum(np.abs(path.l_jerk)) 154 | s_jerk_sum = sum(np.abs(path.s_jerk)) 155 | v_diff = (path.s_v[-1]) ** 2 156 | 157 | path.cost = C.K_JERK * (l_jerk_sum + s_jerk_sum) + \ 158 | C.K_V_DIFF * v_diff + \ 159 | C.K_TIME * t1 * 2 + \ 160 | C.K_OFFSET * abs(path.l[-1]) + \ 161 | 50.0 * sum(np.abs(path.s_v)) 162 | 163 | PATHS[path] = path.cost 164 | 165 | return PATHS 166 | 167 | 168 | def SL_2_XY(s_set, l_set, ref_path): 169 | pathx, pathy = [], [] 170 | 171 | for i in range(len(s_set)): 172 | x_ref, y_ref = ref_path.calc_position(s_set[i]) 173 | 174 | if x_ref is None: 175 | break 176 | 177 | yaw = ref_path.calc_yaw(s_set[i]) 178 | x = x_ref + l_set[i] * math.cos(yaw + math.pi / 2.0) 179 | y = y_ref + l_set[i] * math.sin(yaw + math.pi / 2.0) 180 | 181 | pathx.append(x) 182 | pathy.append(y) 183 | 184 | return pathx, pathy 185 | 186 | 187 | def calc_yaw_curv(x, y): 188 | yaw, curv, ds = [], [], [] 189 | 190 | for i in range(len(x) - 1): 191 | dx = x[i + 1] - x[i] 192 | dy = y[i + 1] - y[i] 193 | ds.append(math.hypot(dx, dy)) 194 | yaw.append(math.atan2(dy, dx)) 195 | 196 | if len(yaw) == 0: 197 | return None, None, None 198 | 199 | yaw.append(yaw[-1]) 200 | ds.append(ds[-1]) 201 | 202 | for i in range(len(yaw) - 1): 203 | curv.append((yaw[i + 1] - yaw[i]) / ds[i]) 204 | 205 | return yaw, curv, ds 206 | 207 | 208 | def is_path_collision(path): 209 | index = range(0, len(path.x), 5) 210 | x = [path.x[i] for i in index] 211 | y = [path.y[i] for i in index] 212 | yaw = [path.yaw[i] for i in index] 213 | 214 | for ix, iy, iyaw in zip(x, y, yaw): 215 | d = 1.8 216 | dl = (C.RF - C.RB) / 2.0 217 | r = math.hypot((C.RF + C.RB) / 2.0, C.W / 2.0) + d 218 | 219 | cx = ix + dl * math.cos(iyaw) 220 | cy = iy + dl * math.sin(iyaw) 221 | 222 | for i in range(len(C.obs)): 223 | xo = C.obs[i][0] - cx 224 | yo = C.obs[i][1] - cy 225 | dx = xo * math.cos(iyaw) + yo * math.sin(iyaw) 226 | dy = -xo * math.sin(iyaw) + yo * math.cos(iyaw) 227 | 228 | if abs(dx) < r and abs(dy) < C.W / 2 + d: 229 | return 1.0 230 | 231 | return 0.0 232 | 233 | 234 | def verify_path(path): 235 | # if any([v > C.speed_max for v in path.s_v]) or \ 236 | # any([abs(a) > C.acceleration_max for a in path.s_a]): 237 | # return False 238 | 239 | if any([v > C.MAX_SPEED for v in path.s_v]) or \ 240 | any([abs(a) > C.MAX_ACCEL for a in path.s_a]) or \ 241 | any([abs(curv) > C.MAX_CURVATURE for curv in path.curv]): 242 | return False 243 | 244 | return True 245 | 246 | 247 | def extract_optimal_path(paths): 248 | if len(paths) == 0: 249 | return None 250 | 251 | while len(paths) > 1: 252 | path = min(paths, key=paths.get) 253 | paths.pop(path) 254 | if verify_path(path) is False: 255 | continue 256 | else: 257 | return path 258 | 259 | return paths[-1] 260 | 261 | 262 | def lattice_planner_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path): 263 | paths = sampling_paths_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path) 264 | path = extract_optimal_path(paths) 265 | 266 | return path 267 | 268 | 269 | def lattice_planner_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path): 270 | paths = sampling_paths_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path) 271 | path = extract_optimal_path(paths) 272 | 273 | return path 274 | 275 | 276 | def get_reference_line(x, y): 277 | index = range(0, len(x), 3) 278 | x = [x[i] for i in index] 279 | y = [y[i] for i in index] 280 | 281 | cubicspline = cubic_spline.Spline2D(x, y) 282 | s = np.arange(0, cubicspline.s[-1], 0.1) 283 | 284 | rx, ry, ryaw, rk = [], [], [], [] 285 | for i_s in s: 286 | ix, iy = cubicspline.calc_position(i_s) 287 | rx.append(ix) 288 | ry.append(iy) 289 | ryaw.append(cubicspline.calc_yaw(i_s)) 290 | rk.append(cubicspline.calc_curvature(i_s)) 291 | 292 | return rx, ry, ryaw, rk, cubicspline 293 | 294 | 295 | def pi_2_pi(theta): 296 | if theta > math.pi: 297 | return theta - 2.0 * math.pi 298 | 299 | if theta < -math.pi: 300 | return theta + 2.0 * math.pi 301 | 302 | return theta 303 | 304 | 305 | def main_Crusing(): 306 | ENV = env.ENVCrusing() 307 | wx, wy = ENV.ref_line 308 | bx1, by1 = ENV.bound_in 309 | bx2, by2 = ENV.bound_out 310 | 311 | C.obs = np.array([[50, 10], [96, 25], [70, 40], 312 | [40, 50], [25, 75]]) 313 | 314 | obs_x = [x for x, y in C.obs] 315 | obs_y = [y for x, y in C.obs] 316 | 317 | rx, ry, ryaw, rk, ref_path = get_reference_line(wx, wy) 318 | 319 | l0 = 2.0 # current lateral position [m] 320 | l0_v = 0.0 # current lateral speed [m/s] 321 | l0_a = 0.0 # current lateral acceleration [m/s] 322 | s0 = 0.0 # current course position 323 | s0_v = 20.0 / 3.6 # current speed [m/s] 324 | s0_a = 0.0 325 | 326 | while True: 327 | path = lattice_planner_for_Cruising(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path) 328 | 329 | if path is None: 330 | print("No feasible path found!!") 331 | break 332 | 333 | l0 = path.l[1] 334 | l0_v = path.l_v[1] 335 | l0_a = path.l_a[1] 336 | s0 = path.s[1] 337 | s0_v = path.s_v[1] 338 | s0_a = path.s_a[1] 339 | 340 | if np.hypot(path.x[1] - rx[-1], path.y[1] - ry[-1]) <= 2.0: 341 | print("Goal") 342 | break 343 | 344 | dy = (path.yaw[2] - path.yaw[1]) / path.ds[1] 345 | steer = pi_2_pi(math.atan(-C.WB * dy)) 346 | 347 | plt.cla() 348 | # for stopping simulation with the esc key. 349 | plt.gcf().canvas.mpl_connect( 350 | 'key_release_event', 351 | lambda event: [exit(0) if event.key == 'escape' else None]) 352 | plt.plot(rx, ry, linestyle='--', color='gray') 353 | plt.plot(bx1, by1, linewidth=1.5, color='k') 354 | plt.plot(bx2, by2, linewidth=1.5, color='k') 355 | plt.plot(path.x[1:], path.y[1:], linewidth='2', color='royalblue') 356 | plt.plot(obs_x, obs_y, 'ok') 357 | draw.draw_car(path.x[1], path.y[1], path.yaw[1], steer, C) 358 | plt.title("[Crusing Mode] v :" + str(s0_v * 3.6)[0:4] + " km/h") 359 | plt.axis("equal") 360 | plt.pause(0.0001) 361 | 362 | plt.pause(0.0001) 363 | plt.show() 364 | 365 | 366 | def main_Stopping(): 367 | ENV = env.ENVStopping() 368 | wx, wy = ENV.ref_line 369 | bx1, by1 = ENV.bound_up 370 | bx2, by2 = ENV.bound_down 371 | 372 | C.ROAD_WIDTH = ENV.road_width 373 | rx, ry, ryaw, rk, ref_path = get_reference_line(wx, wy) 374 | 375 | l0 = 0.0 # current lateral position [m] 376 | l0_v = 0.0 # current lateral speed [m/s] 377 | l0_a = 0.0 # current lateral acceleration [m/s] 378 | s0 = 0.0 # current course position 379 | s0_v = 30.0 / 3.6 # current speed [m/s] 380 | s0_a = 0.0 381 | 382 | while True: 383 | path = lattice_planner_for_Stopping(l0, l0_v, l0_a, s0, s0_v, s0_a, ref_path) 384 | 385 | if path is None: 386 | print("No feasible path found!!") 387 | break 388 | 389 | l0 = path.l[1] 390 | l0_v = path.l_v[1] 391 | l0_a = path.l_a[1] 392 | s0 = path.s[1] 393 | s0_v = path.s_v[1] 394 | s0_a = path.s_a[1] 395 | 396 | if np.hypot(path.x[1] - 56.0, path.y[1] - 0) <= 1.5: 397 | print("Goal") 398 | break 399 | 400 | plt.cla() 401 | # for stopping simulation with the esc key. 402 | plt.gcf().canvas.mpl_connect( 403 | 'key_release_event', 404 | lambda event: [exit(0) if event.key == 'escape' else None]) 405 | plt.plot(rx, ry, linestyle='--', color='gray') 406 | plt.plot(bx1, by1, linewidth=1.5, color='k') 407 | plt.plot(bx2, by2, linewidth=1.5, color='k') 408 | plt.plot(path.x[1:], path.y[1:], linewidth='2', color='royalblue') 409 | draw.draw_car(path.x[1], path.y[1], path.yaw[1], 0.0, C) 410 | plt.title("[Stopping Mode] v :" + str(s0_v * 3.6)[0:4] + " km/h") 411 | plt.axis("equal") 412 | plt.pause(0.0001) 413 | 414 | plt.pause(0.0001) 415 | plt.show() 416 | 417 | plt.plot(rx, ry, linestyle='--', color='gray') 418 | plt.plot(bx1, by1, linewidth=1.5, color='k') 419 | plt.plot(bx2, by2, linewidth=1.5, color='k') 420 | plt.axis("equal") 421 | plt.show() 422 | 423 | 424 | if __name__ == '__main__': 425 | main_Crusing() 426 | # main_Stopping() 427 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Overview 2 | This repository implemented some common motion planners used on autonomous vehicles, including 3 | * [Hybrid A* Planner](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) 4 | * [Frenet Optimal Trajectory](https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame) 5 | * [Hierarchical Optimization-Based Collision Avoidance (H-OBCA)](https://ieeexplore.ieee.org/document/9062306) (Incomplete) 6 | 7 | Also, this repository provides some controllers for path tracking, including 8 | * [Pure Pursuit + PID](https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf) 9 | * [Rear-Wheel Feedback + PID](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf) 10 | * [Front-Wheel Feedback / Stanley + PID](http://robots.stanford.edu/papers/thrun.stanley05.pdf) 11 | * [LQR + PID](https://github.com/ApolloAuto/apollo/tree/master/modules/control/controller) 12 | * [Linear MPC](https://borrelli.me.berkeley.edu/pdfpub/pub-6.pdf) 13 | 14 | ## Requirement 15 | * Python 3.6 or above 16 | * [SciPy](https://www.scipy.org/) 17 | * [cvxpy](https://github.com/cvxgrp/cvxpy) 18 | * [Reeds-Shepp Curves](https://github.com/zhm-real/ReedsSheppCurves) 19 | * [pycubicspline](https://github.com/AtsushiSakai/pycubicspline) 20 | 21 | ## Vehicle models 22 | This repository uses two models: simple car model and [car pulling trailers model](http://planning.cs.uiuc.edu/node661.html#77556). 23 | 24 | ## Hybrid A* Planner 25 |

26 | 27 | 28 | 29 | 30 | 31 |

32 | 33 | 34 | 35 | 36 | 37 |

38 | 39 | 40 | 41 | 42 | 43 |

44 |

45 | 46 | ## State Lattice Planner 47 |

48 | 49 | 50 | 51 | 52 | 53 |

54 |

55 | 56 | ## Controllers 57 |

58 | 59 | 60 | 61 | 62 | 63 |

64 | 65 | 66 | 67 | 68 | 69 |

70 | 71 | 72 | 73 | 74 | 75 |

76 |

77 | 78 | ## Paper 79 | ### Planning 80 | * [Basic Path Planning Algorithms: ](https://github.com/zhm-real/PathPlanning) PathPlanning 81 | * [Baidu Apollo Planning module: ](https://github.com/ApolloAuto/apollo/tree/master/modules/planning) Recommended Materials 82 | * [Survey of Planning and Control algos: ](https://arxiv.org/pdf/1604.07446.pdf) A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles 83 | * [Hybrid A* Planner: ](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf) Practical Search Techniques in Path Planning for Autonomous Driving 84 | * [Frenet Optimal Trajectory: ](https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame) Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame 85 | 86 | ### Control 87 | * [Baidu Apollo Control module: ](https://github.com/ApolloAuto/apollo/tree/master/modules/control) Recommended Materials 88 | * [Pure Pursuit: ](https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf) Implementation of the Pure Pursuit Path Tracking Algorithm 89 | * [Rear-Wheel Feedback: ](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf) Automatic Steering Methods for Autonomous Automobile Path Tracking 90 | * [Front-Wheel Feedback / Stanley: ](http://robots.stanford.edu/papers/thrun.stanley05.pdf) Stanley: The Robot that Won the DARPA Grand Challenge 91 | * [LQR: ](https://github.com/ApolloAuto/apollo/tree/master/modules/control/controller) ApolloAuto/apollo: An open autonomous driving platform 92 | * [Linear MPC: ](https://borrelli.me.berkeley.edu/pdfpub/pub-6.pdf) MPC-Based Approach to Active Steering for Autonomous Vehicle Systems 93 | 94 | ## Useful Material 95 | * [HybridAStarTrailer (Julia): ](https://github.com/AtsushiSakai/HybridAStarTrailer) by AtsushiSakai 96 | * [Hybrid Path Planner (C++): ](https://github.com/karlkurzer/path_planner) by KTH Research Concept Vehicle 97 | --------------------------------------------------------------------------------