├── README.md ├── addition ├── apf.py ├── assistant.py └── zong.py ├── hybrid (new) ├── __pycache__ │ ├── apf.cpython-37.pyc │ ├── assistant.cpython-37.pyc │ ├── astar.cpython-37.pyc │ ├── car.cpython-37.pyc │ └── supplement.cpython-37.pyc ├── apf.py ├── assistant.py ├── astar.py ├── car.py ├── supplement.py └── zong.py ├── hybrid ├── apf.py ├── assistant.py ├── astar.py ├── car.py ├── supplement.py └── zong.py ├── result ├── Figure_1.png ├── Figure_2.png ├── Figure_3.png ├── Figure_4.png ├── Figure_5.png ├── Figure_6.png ├── Uatt.jpg ├── Ucar.jpg ├── Ulane.jpg ├── Uobs.jpg ├── Utotal.jpg ├── a1.png ├── a2.png ├── a3.png ├── a4.png ├── b1.png ├── b2.png ├── b3.png ├── b4.png ├── dynamic.png ├── dynamic_2.png ├── static.png └── static_2.png ├── singleAPF (new) ├── __pycache__ │ ├── apf.cpython-37.pyc │ ├── assistant.cpython-37.pyc │ ├── car.cpython-37.pyc │ └── supplement.cpython-37.pyc ├── apf.py ├── assistant.py ├── car.py ├── supplement.py └── zong.py ├── singleAPF ├── apf.py ├── assistant.py ├── car.py ├── supplement.py └── zong.py ├── singleAstar (new) ├── __pycache__ │ ├── assistant.cpython-37.pyc │ ├── astar.cpython-37.pyc │ └── car.cpython-37.pyc ├── assistant.py ├── astar.py ├── car.py └── zong.py └── singleAstar ├── assistant.py ├── astar.py ├── car.py └── zong.py /README.md: -------------------------------------------------------------------------------- 1 | # A-hybrid-path-planning-algorithm-based-on-APF-and-Astar 2 | An improved hybrid approach based on A* and artificial potential field Algorithms for path planning of autonomous vehicles in complex environments 3 | -------------------------------------------------------------------------------- /addition/apf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | class APF: 6 | 7 | def __init__(self, k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, L, B): 8 | self.k_att = k_att 9 | self.k_rep = k_rep 10 | self.r_apf = r_apf 11 | self.k_lane = k_lane 12 | self.k_replane = k_replane 13 | self.k_car = k_car 14 | self.lw = lane_width 15 | self.L = L 16 | self.B = B 17 | self.car_Lmin = 2 * L 18 | self.car_Bmin = B 19 | 20 | def attraction(self, node, target): 21 | distance = math.sqrt((target[0] - node[0]) ** 2 + (target[1] - node[1]) ** 2) 22 | Uatt = self.k_att * distance 23 | 24 | return Uatt 25 | 26 | def keeplane(self, node, solid_lines): 27 | yd = solid_lines[0][2] 28 | yu = solid_lines[1][2] 29 | 30 | rd = abs(node[1] - yd) - 1 / 4 * self.lw 31 | if rd <= 0: 32 | U1_d = float("inf") 33 | else: 34 | U1_d = 1 / 2 * abs(np.sign(node[1] - yd - 1 / 2 * self.lw) + 35 | np.sign(node[1] - yu + 1 / 2 * self.lw)) * self.k_replane * (1 / rd - 2 / self.lw) ** 2 36 | ru = abs(node[1] - yu) - 1 / 4 * self.lw 37 | if ru <= 0: 38 | U1_u = float("inf") 39 | else: 40 | U1_u = 1 / 2 * abs(np.sign(node[1] - yd - 1 / 2 * self.lw) + 41 | np.sign(node[1] - yu + 1 / 2 * self.lw)) * self.k_replane * (1 / ru - 2 / self.lw) ** 2 42 | U1 = U1_d + U1_u 43 | 44 | U2 = self.k_lane * self.lw * math.cos(math.pi / self.lw * ((node[1] - yd) % self.lw) + math.pi / 2) 45 | 46 | U3 = U1 + U2 47 | 48 | if U3 > 600: 49 | Ulane = 600 50 | else: 51 | Ulane = U3 52 | 53 | return Ulane 54 | 55 | def repulsion(self, node, obstacles): 56 | Uobs = 0 57 | 58 | for obstacle in obstacles: 59 | r_obs = math.sqrt((node[0] - obstacle[0]) ** 2 + (node[1] - obstacle[1]) ** 2) - 1.5 * obstacle[2] 60 | r_min = obstacle[3] * self.r_apf 61 | 62 | if r_obs > r_min: 63 | continue 64 | elif r_obs <= 0: 65 | if obstacle[3] == 2: 66 | Uobs += 450 67 | else: 68 | Uobs += 550 69 | else: 70 | Uobs += self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) ** 2 71 | if obstacle[3] == 2: 72 | if Uobs >= 450: 73 | Uobs = 450 74 | else: 75 | if Uobs >= 550: 76 | Uobs = 550 77 | 78 | return Uobs 79 | 80 | def leavecar(self, node, vehicles): 81 | Uveh = 0 82 | 83 | for vehicle in vehicles: 84 | car_L = abs(node[0] - vehicle[0]) 85 | car_B = abs(node[1] - vehicle[1]) 86 | 87 | if car_L > self.car_Lmin or car_B > self.car_Bmin: 88 | continue 89 | else: 90 | Uveh += self.k_car * math.exp(-(node[0] - vehicle[0]) ** 2 / 6 / self.L - 91 | (node[1] - vehicle[1]) ** 2 / 2 / self.B) 92 | 93 | return Uveh 94 | -------------------------------------------------------------------------------- /addition/assistant.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.pyplot import MultipleLocator 4 | from matplotlib.patches import Circle, Rectangle, Arc 5 | from matplotlib import cm 6 | from mpl_toolkits.mplot3d import Axes3D 7 | from apf import * 8 | 9 | 10 | class MAP: 11 | 12 | def __init__(self, length, width, del_block): 13 | self.length = length 14 | self.width = width 15 | self.del_block = del_block 16 | 17 | def create_map(self): 18 | subplot = plt.figure(figsize=(5, 3)) 19 | fig = subplot.add_subplot(111) 20 | fig.set_xlabel('X-distance', size=7) 21 | fig.set_ylabel('Y-distance', size=7) 22 | fig.xaxis.set_ticks_position('bottom') 23 | fig.yaxis.set_ticks_position('left') 24 | plt.tick_params(labelsize=7) 25 | x_major = MultipleLocator(5) 26 | y_major = MultipleLocator(5) 27 | fig.xaxis.set_major_locator(x_major) 28 | fig.yaxis.set_major_locator(y_major) 29 | plt.xlim(0, self.length) 30 | plt.ylim(0, self.width) 31 | 32 | return fig 33 | 34 | def draw_lines(self, solid_lines, dotted_lines): 35 | for solid_line in solid_lines: 36 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 37 | 38 | for dotted_line in dotted_lines: 39 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 40 | linewidth=2) # xx, yy 41 | 42 | return 43 | 44 | def create_lanes(self): 45 | solid_lines = [[0, 50, 8], [0, 50, 22]] 46 | dotted_lines = [[0, 50, 11.5], [0, 50, 15], [0, 50, 18.5]] 47 | self.draw_lines(solid_lines, dotted_lines) 48 | 49 | return solid_lines, dotted_lines 50 | 51 | def draw_cycle(self, fig, obstacles): 52 | for obstacle in obstacles: 53 | if obstacle[3] == 2: 54 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 55 | else: 56 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 57 | fig.add_patch(cir) 58 | 59 | return 60 | 61 | def get_obstacles(self, fig): 62 | obstacles = [[25, 14, 2, 2], [35, 16, 2, 3]] 63 | self.draw_cycle(fig, obstacles) 64 | 65 | return obstacles 66 | 67 | def draw_vehicle(self, fig, vehicles, L, B): 68 | for vehicle in vehicles: 69 | x1 = vehicle[0] - L / 2 70 | y1 = vehicle[1] - B / 2 71 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='blue') 72 | fig.add_patch(rec) 73 | 74 | return 75 | 76 | def get_vehicles(self, fig, L, B): 77 | vehicles = [[13, 17]] 78 | self.draw_vehicle(fig, vehicles, L, B) 79 | 80 | return vehicles 81 | 82 | def create_areas(self, solid_lines): 83 | x1 = solid_lines[0][0] 84 | y1 = solid_lines[0][2] 85 | x2 = solid_lines[1][1] 86 | y2 = solid_lines[1][2] 87 | effective_area = [x1, y1, x2, y2] 88 | 89 | x3 = solid_lines[0][0] 90 | y3 = 0 91 | x4 = solid_lines[0][1] 92 | y4 = solid_lines[0][2] 93 | x5 = solid_lines[1][0] 94 | y5 = solid_lines[1][2] 95 | x6 = solid_lines[1][1] 96 | y6 = self.width 97 | infeasible_areas = [[x3, y3, x4, y4], [x5, y5, x6, y6]] 98 | 99 | height = solid_lines[1][2] - solid_lines[0][2] 100 | 101 | return effective_area, infeasible_areas, height 102 | 103 | def get_nodes(self, effective_area, infeasible_areas, height): 104 | block1 = int(self.length / self.del_block) 105 | block2 = int(height / self.del_block) 106 | 107 | nodes, node_x, node_y = [], [], [] 108 | for i in range(block1 + 1): 109 | node_x.append(round(i * self.del_block, 2)) 110 | for j in range(block2 + 1): 111 | node_y.append(round(effective_area[1] + j * self.del_block, 2)) 112 | for i in range(block1 + 1): 113 | for j in range(block2 + 1): 114 | nodes.append([node_x[i], node_y[j]]) 115 | 116 | x1 = np.linspace(infeasible_areas[0][0], infeasible_areas[0][2], 2) 117 | y1 = np.linspace(infeasible_areas[0][1], infeasible_areas[0][3], 2) 118 | X1, Y1 = np.meshgrid(x1, y1) 119 | Z1 = X1 * 0 + Y1 * 0 120 | x2 = np.linspace(infeasible_areas[1][0], infeasible_areas[1][2], 2) 121 | y2 = np.linspace(infeasible_areas[1][1], infeasible_areas[1][3], 2) 122 | X2, Y2 = np.meshgrid(x2, y2) 123 | Z2 = X2 * 0 + Y2 * 0 124 | useless_nodes = [[X1, Y1, Z1], [X2, Y2, Z2]] 125 | 126 | return nodes, useless_nodes 127 | 128 | def get_startandtarget(self): 129 | start = [5, 10] 130 | target = [45, 20] 131 | plt.plot(start[0], start[1], '*', color='purple') 132 | plt.plot(target[0], target[1], 'o', color='purple') 133 | 134 | return start, target 135 | 136 | def appear_attraction(self, nodes, useless_nodes, target, apf): 137 | U_att, U_x, U_y, U_z = [], [], [], [] 138 | 139 | for node in nodes: 140 | Uatt = apf.attraction(node, target) 141 | U_att.append([node[0], node[1], Uatt]) 142 | 143 | for an in U_att: 144 | U_x.append(an[0]) 145 | U_y.append(an[1]) 146 | U_z.append(round(an[2], 1)) 147 | 148 | fig1 = plt.figure() 149 | ax = fig1.add_subplot(111, projection='3d') 150 | ax.plot_trisurf(U_x, U_y, U_z, cmap=cm.jet, linewidth=0.1) 151 | ax.plot_surface(useless_nodes[0][0], useless_nodes[0][1], useless_nodes[0][2], color='g', alpha=0.3) 152 | ax.plot_surface(useless_nodes[1][0], useless_nodes[1][1], useless_nodes[1][2], color='g', alpha=0.3) 153 | 154 | print("U_attx:", U_x) 155 | print("U_atty:", U_y) 156 | print("U_attz:", U_z) 157 | 158 | return U_att 159 | 160 | def appear_lanerepulsion(self, nodes, useless_nodes, solid_lines, apf): 161 | U_lane, U_x, U_y, U_z = [], [], [], [] 162 | 163 | for node in nodes: 164 | Ulane = apf.keeplane(node, solid_lines) 165 | U_lane.append([node[0], node[1], Ulane]) 166 | 167 | for bn in U_lane: 168 | if bn[2] != float("inf"): 169 | U_x.append(bn[0]) 170 | U_y.append(bn[1]) 171 | U_z.append(round(bn[2], 1)) 172 | 173 | fig2 = plt.figure() 174 | bx = fig2.add_subplot(111, projection='3d') 175 | bx.plot_trisurf(U_x, U_y, U_z, cmap=cm.jet, linewidth=0.1) 176 | bx.plot_surface(useless_nodes[0][0], useless_nodes[0][1], useless_nodes[0][2], color='g', alpha=0.3) 177 | bx.plot_surface(useless_nodes[1][0], useless_nodes[1][1], useless_nodes[1][2], color='g', alpha=0.3) 178 | 179 | print("U_lanex:", U_x) 180 | print("U_laney:", U_y) 181 | print("U_lanez:", U_z) 182 | 183 | return U_lane 184 | 185 | def appear_obsrepulsion(self, nodes, useless_nodes, obstacles, apf): 186 | U_obs, U_x, U_y, U_z, no_nodes2 = [], [], [], [], [] 187 | 188 | for node in nodes: 189 | Uobs = apf.repulsion(node, obstacles) 190 | U_obs.append([node[0], node[1], Uobs]) 191 | 192 | for cn in U_obs: 193 | U_x.append(cn[0]) 194 | U_y.append(cn[1]) 195 | U_z.append(round(cn[2], 1)) 196 | 197 | fig3 = plt.figure() 198 | cx = fig3.add_subplot(111, projection='3d') 199 | cx.plot_trisurf(U_x, U_y, U_z, cmap=cm.jet, linewidth=0.1) 200 | cx.plot_surface(useless_nodes[0][0], useless_nodes[0][1], useless_nodes[0][2], color='g', alpha=0.3) 201 | cx.plot_surface(useless_nodes[1][0], useless_nodes[1][1], useless_nodes[1][2], color='g', alpha=0.3) 202 | 203 | print("U_obsx:", U_x) 204 | print("U_obsy:", U_y) 205 | print("U_obsz:", U_z) 206 | 207 | return U_obs 208 | 209 | def appear_vehirepulsion(self, nodes, useless_nodes, vehicles, apf): 210 | U_veh, U_x, U_y, U_z = [], [], [], [] 211 | 212 | for node in nodes: 213 | Uveh = apf.leavecar(node, vehicles) 214 | U_veh.append([node[0], node[1], Uveh]) 215 | 216 | for dn in U_veh: 217 | U_x.append(dn[0]) 218 | U_y.append(dn[1]) 219 | U_z.append(round(dn[2], 1)) 220 | 221 | fig4 = plt.figure() 222 | dx = fig4.add_subplot(111, projection='3d') 223 | dx.plot_trisurf(U_x, U_y, U_z, cmap=cm.jet, linewidth=0.1) 224 | dx.plot_surface(useless_nodes[0][0], useless_nodes[0][1], useless_nodes[0][2], color='g', alpha=0.3) 225 | dx.plot_surface(useless_nodes[1][0], useless_nodes[1][1], useless_nodes[1][2], color='g', alpha=0.3) 226 | 227 | print("U_veh U_x:", U_x) 228 | print("U_veh U_y:", U_y) 229 | print("U_veh U_z:", U_z) 230 | 231 | return U_veh 232 | 233 | def total_potential(self, nodes, useless_nodes, U_att, U_lane, U_obs, U_veh): 234 | U_total, U_x, U_y, U_z = [], [], [], [] 235 | 236 | for i in range(len(nodes)): 237 | U_potential = U_att[i][2] + U_lane[i][2] + U_obs[i][2] + U_veh[i][2] 238 | U_total.append([nodes[i][0], nodes[i][1], U_potential]) 239 | 240 | for en in U_total: 241 | U_x.append(en[0]) 242 | U_y.append(en[1]) 243 | U_z.append(round(en[2], 1)) 244 | 245 | fig5 = plt.figure() 246 | ex = fig5.add_subplot(111, projection='3d') 247 | ex.plot_trisurf(U_x, U_y, U_z, cmap=cm.jet, linewidth=0.1) 248 | ex.plot_surface(useless_nodes[0][0], useless_nodes[0][1], useless_nodes[0][2], color='g', alpha=0.3) 249 | ex.plot_surface(useless_nodes[1][0], useless_nodes[1][1], useless_nodes[1][2], color='g', alpha=0.3) 250 | 251 | print("U_totalx:", U_x) 252 | print("U_totaly:", U_y) 253 | print("U_totalz:", U_z) 254 | 255 | return U_total 256 | -------------------------------------------------------------------------------- /addition/zong.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from assistant import * 3 | from apf import * 4 | 5 | 6 | # main 7 | if __name__ == "__main__": 8 | print("Creating a map:\n----------") 9 | length = 50 10 | width = 30 11 | del_block = 0.25 # 0.5 12 | 13 | map = MAP(length, width, del_block) 14 | fig = map.create_map() 15 | 16 | print("\nCreating lanes:\n----------") 17 | solid_lines, dotted_lines = map.create_lanes() 18 | print("solid lines:", solid_lines) 19 | print("dotted lines:", dotted_lines) 20 | 21 | print("\nCreating obstacles:\n----------") 22 | obstacles = map.get_obstacles(fig) 23 | print("obstacles:", obstacles) 24 | 25 | print("\nCreating vehicles:\n----------") 26 | L = 4.8 27 | B = 1.8 28 | vehicles = map.get_vehicles(fig, L, B) 29 | print("vehicles:", vehicles) 30 | 31 | print("\nCreating effective area:\n----------") 32 | effective_area, infeasible_areas, height = map.create_areas(solid_lines) 33 | print("the effective area:", effective_area) 34 | print("the infeasible areas:", infeasible_areas) 35 | 36 | print("\nCreating nodes:\n----------") 37 | nodes, useless_nodes = map.get_nodes(effective_area, infeasible_areas, height) 38 | print("nodes:", nodes) 39 | print("useless nodes:", useless_nodes) 40 | 41 | print("\nSetting start and target:\n----------") 42 | start, target = map.get_startandtarget() 43 | print("start:", start, ", target:", target) 44 | 45 | print("\n----------") 46 | 47 | k_att = 40 48 | k_rep = 10 49 | r_apf = 5 50 | k_replane = 5 51 | k_lane = 10 52 | k_car = 500 53 | lane_width = 3.5 54 | L = 4.8 55 | B = 1.8 56 | 57 | apf = APF(k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, L, B) 58 | 59 | U_att = map.appear_attraction(nodes, useless_nodes, target, apf) 60 | U_lane = map.appear_lanerepulsion(nodes, useless_nodes, solid_lines, apf) 61 | U_obs = map.appear_obsrepulsion(nodes, useless_nodes, obstacles, apf) 62 | U_veh = map.appear_vehirepulsion(nodes, useless_nodes, vehicles, apf) 63 | U_total = map.total_potential(nodes, useless_nodes, U_att, U_lane, U_obs, U_veh) 64 | 65 | plt.show() 66 | -------------------------------------------------------------------------------- /hybrid (new)/__pycache__/apf.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/hybrid (new)/__pycache__/apf.cpython-37.pyc -------------------------------------------------------------------------------- /hybrid (new)/__pycache__/assistant.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/hybrid (new)/__pycache__/assistant.cpython-37.pyc -------------------------------------------------------------------------------- /hybrid (new)/__pycache__/astar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/hybrid (new)/__pycache__/astar.cpython-37.pyc -------------------------------------------------------------------------------- /hybrid (new)/__pycache__/car.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/hybrid (new)/__pycache__/car.cpython-37.pyc -------------------------------------------------------------------------------- /hybrid (new)/__pycache__/supplement.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/hybrid (new)/__pycache__/supplement.cpython-37.pyc -------------------------------------------------------------------------------- /hybrid (new)/apf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | from supplement import * 5 | 6 | 7 | class APF: 8 | 9 | def __init__(self, k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 10 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, 11 | direction, front_velocities, front_points, steering): 12 | self.k_att = k_att 13 | self.k_rep = k_rep 14 | self.r_apf = r_apf 15 | self.k_replane = k_replane 16 | self.k_lane = k_lane 17 | self.k_car = k_car 18 | self.lw = lane_width 19 | self.target_area = target_area 20 | self.F_e = F_e 21 | self.del_t = del_t 22 | self.max_iters = max_iters 23 | self.h_sollines, self.v_sollines = self.solid(solid_lines) 24 | self.t_lines = transition_lines 25 | self.obstacles = obstacles 26 | self.vehicles = vehicles 27 | self.L = L 28 | self.B = B 29 | self.car_Lmin = 2 * L 30 | self.car_Bmin = B 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | self.F_atts = [] 39 | self.F_lanes = [] 40 | self.F_obss = [] 41 | self.F_vehs = [] 42 | self.F_totals = [] 43 | 44 | def solid(self, solid_lines): 45 | h_sollines, v_sollines = [], [] 46 | 47 | for solid_line in solid_lines: 48 | if solid_line[3] == 'h': 49 | h_sollines.append(solid_line) 50 | else: 51 | v_sollines.append(solid_line) 52 | 53 | return h_sollines, v_sollines 54 | 55 | def attraction(self, front_p, target): 56 | force_att = self.k_att 57 | theta_att = math.atan2(target[1] - front_p[1], target[0] - front_p[0]) 58 | F_attx = force_att * math.cos(theta_att) 59 | F_atty = force_att * math.sin(theta_att) 60 | F_att = [F_attx, F_atty] 61 | 62 | return F_att 63 | 64 | def keeplane(self, front_p, supply, iters): 65 | use_sollinesx, use_sollinesy = supply.useful_lines(front_p) 66 | 67 | if use_sollinesx != [] and use_sollinesy != []: # 直角路口 68 | F_lane = supply.lane_xy(front_p, use_sollinesx, use_sollinesy, iters) 69 | elif use_sollinesx != [] and use_sollinesy == []: # 水平路或者水平丁字路口 70 | F_lane = supply.lane_x(front_p, use_sollinesx, iters) 71 | elif use_sollinesy != [] and use_sollinesx == []: # 竖直路或者竖直丁字路口 72 | F_lane = supply.lane_y(front_p, use_sollinesy, iters) 73 | else: # 十字路口 74 | F_lane = supply.lane(front_p, iters) 75 | 76 | return F_lane, use_sollinesx, use_sollinesy 77 | 78 | def repulsion(self, front_p, emergency, use_sollinesx, use_sollinesy, iters): 79 | F_obsx, F_obsy = 0, 0 80 | 81 | for obstacle in self.obstacles: 82 | r_obs = math.sqrt((front_p[0] - obstacle[0]) ** 2 + (front_p[1] - obstacle[1]) ** 2) - 1.5 * obstacle[2] 83 | r_min = obstacle[3] * self.r_apf 84 | obs_Le = obstacle[3] * obstacle[2] 85 | obs_Be = 0.5 * obstacle[3] * self.B 86 | 87 | if obstacle[4] == 'h': 88 | obs_L = abs(front_p[0] - obstacle[0]) 89 | obs_B = abs(front_p[1] - obstacle[1]) 90 | else: 91 | obs_L = abs(front_p[1] - obstacle[1]) 92 | obs_B = abs(front_p[0] - obstacle[0]) 93 | 94 | if r_obs > r_min: 95 | continue 96 | elif r_obs <= 0: 97 | print("APF is failed, because the vehicle moves into the safe area.") 98 | plt.savefig("E:/pythonpro/modifyandtry/9_dongtai/results/failed" + str(iters) + ".png") 99 | # exit() 100 | break 101 | elif obs_L <= obs_Le and obs_B <= obs_Be: 102 | print("The vehicle meets the local minimum.") 103 | Fe_obsx, Fe_obsy = emergency.obstacle_force(obstacle, use_sollinesx, use_sollinesy) 104 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 105 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 106 | F_obsx += force_rep * math.cos(theta_rep) + Fe_obsx 107 | F_obsy += force_rep * math.sin(theta_rep) + Fe_obsy 108 | else: 109 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 110 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 111 | F_obsx += force_rep * math.cos(theta_rep) 112 | F_obsy += force_rep * math.sin(theta_rep) 113 | 114 | F_obs = [F_obsx, F_obsy] 115 | 116 | return F_obs 117 | 118 | def leavecar(self, front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy, iters): 119 | F_vehx, F_vehy = 0, 0 120 | car_Le = self.L 121 | car_Be = 0.5 * self.B 122 | all_vehicles = self.vehicles + moving_vehicles 123 | 124 | for vehicle in all_vehicles: 125 | if vehicle[2] == 'h': 126 | car_L = abs(front_p[0] - vehicle[0]) 127 | car_B = abs(front_p[1] - vehicle[1]) 128 | else: 129 | car_L = abs(front_p[1] - vehicle[1]) 130 | car_B = abs(front_p[0] - vehicle[0]) 131 | 132 | if car_L > self.car_Lmin or car_B > self.car_Bmin: 133 | continue 134 | elif car_L == 0 and car_B == 0: 135 | print("APF is failed, because the vehicle vehicle crashes a car.") 136 | plt.savefig("E:/pythonpro/modifyandtry/9_dongtai/results/failed" + str(iters) + ".png") 137 | # exit() 138 | break 139 | elif car_L <= car_Le and car_B <= car_Be: 140 | print("The vehicle meets the local minimum.") 141 | Fe_vehx, Fe_vehy = emergency.vehicle_force(vehicle, use_sollinesx, use_sollinesy) 142 | if vehicle[2] == 'h': 143 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 144 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 145 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehx 146 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 147 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 148 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehy 149 | else: 150 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 151 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 152 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehx 153 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 154 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 155 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehy 156 | else: 157 | if vehicle[2] == 'h': 158 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 159 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 160 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 161 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 162 | else: 163 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 164 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 165 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 166 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 167 | 168 | F_veh = [F_vehx, F_vehy] 169 | 170 | return F_veh 171 | 172 | def pathplanning(self, rear_p, front_p, fi, target, iters, moving_vehicles): 173 | supply = SUPPLY(self.k_replane, self.k_lane, self.lw, self.h_sollines, self.v_sollines, self.t_lines) 174 | emergency = EMERGENCY(self.F_e, self.h_sollines, self.v_sollines, self.lw) 175 | 176 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 177 | 178 | while distance > self.target_area and iters <= self.max_iters: 179 | iters += 1 180 | print("\n----------\nNo.", iters) 181 | 182 | F_att = self.attraction(front_p, target) 183 | self.F_atts.append(F_att) 184 | F_lane, use_sollinesx, use_sollinesy = self.keeplane(front_p, supply, iters) 185 | self.F_lanes.append(F_lane) 186 | F_obs = self.repulsion(front_p, emergency, use_sollinesx, use_sollinesy, iters) 187 | self.F_obss.append(F_obs) 188 | F_veh = self.leavecar(front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy, iters) 189 | self.F_vehs.append(F_veh) 190 | 191 | F_total = [F_att[0] + F_lane[0] + F_obs[0] + F_veh[0], F_att[1] + F_lane[1] + F_obs[1] + F_veh[1]] 192 | self.F_totals.append(F_total) 193 | F_direction = math.atan2(F_total[1], F_total[0]) 194 | # print("total force:", F_total) 195 | # print("force direction:", int(F_direction / math.pi * 180)) 196 | 197 | if F_direction - fi > 40 / 180 * math.pi: 198 | si = 40 / 180 * math.pi 199 | elif F_direction - fi < -40 / 180 * math.pi: 200 | si = -40 / 180 * math.pi 201 | else: 202 | si = F_direction - fi 203 | # print("steering angle:", int(si / math.pi * 180)) 204 | 205 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 206 | print("rear velocity:", rear_v, ", rear position:", rear_p) 207 | # print("front velocity:", front_v, ", front position:", front_p) 208 | # print("vehicle direction:", int(fi / math.pi * 180)) 209 | 210 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 211 | # print("difference between formula and subtract:", dif_distance - dif_distance2) 212 | # print("distance difference:", dif_distance) 213 | 214 | self.car.draw_car(rear_p, front_p) 215 | moving_vehicles, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 216 | # plt.pause(self.del_t) 217 | p1.remove() 218 | p2.remove() 219 | p3.remove() 220 | 221 | self.rear_velocities.append(self.car.v) 222 | self.path.append([rear_p[0], rear_p[1]]) 223 | self.direction.append(fi) 224 | self.front_velocities.append(vf) 225 | self.front_points.append([front_p[0], front_p[1]]) 226 | self.steering.append(si) 227 | 228 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 229 | 230 | if distance <= self.target_area and iters <= self.max_iters: 231 | print("\n----------\nAPF is successful!\n----------") 232 | else: 233 | print("\n----------\nAPF is failed!\n----------") 234 | 235 | return rear_p, front_p, fi, iters, moving_vehicles 236 | -------------------------------------------------------------------------------- /hybrid (new)/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, del_block): 9 | self.length = length 10 | self.width = width 11 | self.del_block = del_block 12 | self.no_block_x = int(length / del_block) 13 | self.no_block_y = int(width / del_block) 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | # xx, yy 37 | else: 38 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 39 | 40 | for dotted_line in dotted_lines: 41 | if dotted_line[3] == 'h': 42 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 43 | linewidth=2) 44 | else: 45 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 46 | linewidth=2) 47 | 48 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 49 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 50 | fig.add_patch(arc1) 51 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 52 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 53 | fig.add_patch(arc2) 54 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 55 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 56 | fig.add_patch(arc3) 57 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 58 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 59 | fig.add_patch(arc4) 60 | 61 | return 62 | 63 | def create_lanes(self, fig): 64 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 65 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 66 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 67 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 68 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 69 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 70 | 71 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 72 | 73 | return solid_lines, dotted_lines, transition_lines 74 | 75 | def draw_cycle(self, fig, obstacles): 76 | for obstacle in obstacles: 77 | if obstacle[3] == 2: 78 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 79 | else: 80 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 81 | fig.add_patch(cir) 82 | 83 | return 84 | 85 | def get_obstacles(self, fig): 86 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 87 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 88 | self.draw_cycle(fig, obstacles) 89 | 90 | return obstacles 91 | 92 | def draw_vehicle(self, fig, vehicles, L, B): 93 | for vehicle in vehicles: 94 | if vehicle[2] == 'h': 95 | x1 = vehicle[0] - L / 2 96 | y1 = vehicle[1] - B / 2 97 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='blue') 98 | fig.add_patch(rec) 99 | else: 100 | x1 = vehicle[0] - B / 2 101 | y1 = vehicle[1] - L / 2 102 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='blue') 103 | fig.add_patch(rec) 104 | 105 | return 106 | 107 | def draw_initmovingvehi(self, fig, init_movingvehis, L, B): 108 | for moving_vehicle in init_movingvehis: 109 | if moving_vehicle[2] == 'h': 110 | x1 = moving_vehicle[0] - L / 2 111 | y1 = moving_vehicle[1] - B / 2 112 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 113 | fig.add_patch(rec) 114 | else: 115 | x1 = moving_vehicle[0] - B / 2 116 | y1 = moving_vehicle[1] - L / 2 117 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 118 | fig.add_patch(rec) 119 | 120 | return 121 | 122 | def get_vehicles(self, fig, L, B): 123 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 124 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 125 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 126 | 127 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 128 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 129 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 130 | 131 | self.draw_vehicle(fig, vehicles, L, B) 132 | # self.draw_initmovingvehi(fig, init_movingvehis, L, B) 133 | 134 | return vehicles, init_movingvehis, moving_vehicles 135 | 136 | def create_areas(self): 137 | infeasible_areas = [[0, 14, 50, 73], [64, 14, 80, 73]] 138 | 139 | return infeasible_areas 140 | 141 | def get_nodes(self, infeasible_areas, solid_lines): 142 | nodes, free_nodes, new_x, new_y = [], [], [], [] 143 | 144 | for i in range(self.no_block_x + 1): 145 | new_x.append(int(i * self.del_block)) 146 | # plt.axvline(x=new_x[i], color='lightgray', lw=0.5) 147 | 148 | for j in range(self.no_block_y + 1): 149 | new_y.append(int(j * self.del_block)) 150 | # plt.axhline(y=new_y[j], color='lightgray', lw=0.5) 151 | 152 | for i in range(self.no_block_x + 1): 153 | for j in range(self.no_block_y + 1): 154 | nodes.append([new_x[i], new_y[j]]) 155 | free_nodes.append([new_x[i], new_y[j]]) 156 | 157 | for area in infeasible_areas: 158 | for node in nodes: 159 | if area[0] <= node[0] <= area[2] and area[1] <= node[1] <= area[3]: 160 | if node in free_nodes: 161 | free_nodes.remove([node[0], node[1]]) 162 | 163 | for solid_line in solid_lines: 164 | for node in nodes: 165 | if solid_line[3] == "h": 166 | if solid_line[0] <= node[0] <= solid_line[1] and node[1] == solid_line[2]: 167 | if node in free_nodes: 168 | free_nodes.remove([node[0], node[1]]) 169 | else: 170 | if node[0] == solid_line[0] and solid_line[1] <= node[1] <= solid_line[2]: 171 | if node in free_nodes: 172 | free_nodes.remove([node[0], node[1]]) 173 | 174 | ''' 175 | for free_node in free_nodes: 176 | plt.plot(free_node[0], free_node[1], 'x', color='lightblue') 177 | ''' 178 | 179 | return nodes, free_nodes 180 | 181 | def get_startandtarget(self): 182 | # start = [5, 2] 183 | # target = [63, 60] 184 | start = [5, 2] 185 | target = [78, 78] 186 | 187 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 188 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 189 | 190 | return start, target 191 | 192 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 193 | front_x, front_y, path_x, path_y = [], [], [], [] 194 | for i in front_points: 195 | front_x.append(i[0]) 196 | front_y.append(i[1]) 197 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 198 | for j in path: 199 | path_x.append(j[0]) 200 | path_y.append(j[1]) 201 | # plt.plot(front_x, front_y, c='green', linewidth=2) 202 | plt.plot(path_x, path_y, c='r', linewidth=2) 203 | 204 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 205 | if v1[2] == 'h': 206 | x1 = v1[0] - L / 2 207 | y1 = v1[1] - B / 2 208 | h = v2[0] + L / 2 - x1 209 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 210 | fig.add_patch(rec) 211 | else: 212 | x1 = v1[0] - B / 2 213 | y1 = v1[1] - L / 2 214 | h = v2[1] + L / 2 - y1 215 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 216 | fig.add_patch(rec) 217 | 218 | for vehicle in moving_vehicles: 219 | if vehicle[2] == 'h': 220 | x1 = vehicle[0] - L / 2 221 | y1 = vehicle[1] - B / 2 222 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 223 | fig.add_patch(rec) 224 | else: 225 | x1 = vehicle[0] - B / 2 226 | y1 = vehicle[1] - L / 2 227 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 228 | fig.add_patch(rec) 229 | 230 | # plt.pause(0.1) 231 | 232 | return 233 | -------------------------------------------------------------------------------- /hybrid (new)/astar.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class ASTAR: 6 | 7 | class NODE: 8 | 9 | def __init__(self, current_node, father_node, target, cost=0): 10 | self.current_node = current_node 11 | self.father_node = father_node 12 | self.target = target 13 | self.direction = math.atan2(self.current_node[1] - self.father_node[1], 14 | self.current_node[0] - self.father_node[0]) 15 | self.g = cost 16 | self.h = abs(self.target[0] - self.current_node[0]) + abs(self.target[1] - self.current_node[1]) 17 | self.f = self.g + self.h 18 | 19 | def __init__(self, del_block, astar_start, astar_target, free_nodes): 20 | self.del_block = del_block 21 | self.astar_start = astar_start 22 | self.astar_target = astar_target 23 | self.open_set = free_nodes 24 | self.close_set = [] 25 | 26 | def possible_moves(self, current_node): 27 | next_nodes, next_costs = [], [] 28 | 29 | possible_nodes = [[current_node[0] + self.del_block, current_node[1]], 30 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 31 | [current_node[0], current_node[1] + self.del_block], 32 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 33 | [current_node[0] - self.del_block, current_node[1]], 34 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 35 | [current_node[0], current_node[1] - self.del_block], 36 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 37 | 38 | possible_costs = [self.del_block, math.hypot(self.del_block, self.del_block), 39 | self.del_block, math.hypot(self.del_block, self.del_block), 40 | self.del_block, math.hypot(self.del_block, self.del_block), 41 | self.del_block, math.hypot(self.del_block, self.del_block)] 42 | 43 | for i in range(8): 44 | if possible_nodes[i] in self.open_set: 45 | next_nodes.append(possible_nodes[i]) 46 | next_costs.append(possible_costs[i]) 47 | 48 | return next_nodes, next_costs 49 | 50 | def get_minf(self, best, next): 51 | if next.f < best.f: 52 | best = next 53 | 54 | return best 55 | 56 | def findway(self): 57 | path, direction = [], [] 58 | success = 0 59 | 60 | father_node = self.astar_start 61 | current_node = self.astar_start 62 | current = ASTAR.NODE(current_node, father_node, self.astar_target) 63 | 64 | while current.current_node != self.astar_target: 65 | 66 | if current.current_node in self.open_set: 67 | self.open_set.remove(current.current_node) 68 | self.close_set.append(current.current_node) 69 | 70 | path.append(current.current_node) 71 | print("current node:", current.current_node, ", previous angle:", current.direction, ", current g:", 72 | current.g, ", h:", current.h, ", f:", current.f) 73 | # plt.plot(current.current_node[0], current.current_node[1], '.k', markersize=5) 74 | # plt.pause(0.1) 75 | 76 | next_nodes, next_costs = self.possible_moves(current.current_node) 77 | next_set = [] 78 | for next_node, next_cost in zip(next_nodes, next_costs): 79 | next = ASTAR.NODE(next_node, current.current_node, self.astar_target, next_cost + current.g) 80 | next_set.append(next) 81 | 82 | current.f = float("inf") 83 | best = current 84 | for next in next_set: 85 | best = self.get_minf(best, next) 86 | 87 | current_node = best.current_node 88 | father_node = best.father_node 89 | current_cost = best.g 90 | current = ASTAR.NODE(current_node, father_node, self.astar_target, current_cost) 91 | direction.append(current.direction) 92 | 93 | else: 94 | print("A* pre path planning is failed!\n----------") 95 | 96 | return path, direction, success 97 | 98 | print("A* pre path planning is successful!\n----------") 99 | success = 1 100 | path.append(current.current_node) 101 | # plt.plot(current.current_node[0], current.current_node[1], '.k', markersize=5) 102 | # plt.pause(0.1) 103 | 104 | return path, direction, success 105 | 106 | def find_local(self, path, direction): 107 | local_targets = [] 108 | 109 | path.remove(path[0]) 110 | path.remove(path[-1]) 111 | 112 | angle = [] 113 | for i in range(len(direction) - 1): 114 | angle.append(direction[i + 1] - direction[i]) 115 | 116 | i = 1 117 | for pa, ang in zip(path, angle): 118 | if ang == 0: 119 | continue 120 | else: 121 | if pa[0] == self.astar_start[0] or pa[1] == self.astar_start[1]: 122 | local_targets.append(pa) 123 | plt.plot(pa[0], pa[1], 'o', color='orangered', markersize=10) 124 | # plt.pause(0.1) 125 | elif pa[0] == self.astar_target[0] or pa[1] == self.astar_target[1]: 126 | continue 127 | else: 128 | if i % 2 == 0: 129 | local_targets.append(pa) 130 | plt.plot(pa[0], pa[1], 'o', color='orangered', markersize=10) 131 | # plt.pause(0.1) 132 | i += 1 133 | else: 134 | i += 1 135 | 136 | return local_targets 137 | -------------------------------------------------------------------------------- /hybrid (new)/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, p1, p2, p3, = [], [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | ''' 89 | new_moving_vehicles, p, = [], [] 90 | 91 | for moving_vehicle in moving_vehicles: 92 | if moving_vehicle[2] == 'h': 93 | x1 = moving_vehicle[0] - self.l / 2 94 | y1 = moving_vehicle[1] 95 | x2 = moving_vehicle[0] + self.l / 2 96 | y2 = moving_vehicle[1] 97 | draw_x0 = [x1, x2] 98 | draw_y0 = [y1, y2] 99 | p, = plt.plot(draw_x0, draw_y0, c='green', linewidth=10) 100 | 101 | coop_x = moving_vehicle[0] + moving_vehicle[3] * del_t 102 | coop_y = moving_vehicle[1] 103 | new_moving_vehicles.append([coop_x, coop_y, 'h', moving_vehicle[3]]) 104 | else: 105 | x1 = moving_vehicle[0] 106 | y1 = moving_vehicle[1] - self.l / 2 107 | x2 = moving_vehicle[0] 108 | y2 = moving_vehicle[1] + self.l / 2 109 | draw_x0 = [x1, x2] 110 | draw_y0 = [y1, y2] 111 | p, = plt.plot(draw_x0, draw_y0, c='green', linewidth=10) 112 | 113 | coop_x = moving_vehicle[0] 114 | coop_y = moving_vehicle[1] + moving_vehicle[3] * del_t 115 | new_moving_vehicles.append([coop_x, coop_y, 'v', moving_vehicle[3]]) 116 | 117 | moving_vehicles.clear() 118 | moving_vehicles = new_moving_vehicles 119 | ''' 120 | 121 | return moving_vehicles, p1, p2, p3, 122 | -------------------------------------------------------------------------------- /hybrid (new)/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from astar import * 6 | from car import * 7 | from apf import * 8 | 9 | 10 | # main 11 | # Note: If there is an output image during the path planning process, it means failure. 12 | if __name__ == "__main__": 13 | print("Creating a map:\n----------") 14 | length = 80 15 | width = 80 16 | del_block = 2 17 | 18 | map = MAP(length, width, del_block) 19 | fig = map.create_map() 20 | 21 | print("\nCreating lanes:\n----------") 22 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 23 | print("solid lines:", solid_lines) 24 | print("dotted lines:", dotted_lines) 25 | print("transition lines:", transition_lines) 26 | 27 | print("\nCreating obstacles:\n----------") 28 | obstacles = map.get_obstacles(fig) 29 | print("obstacles:", obstacles) 30 | 31 | print("\nCreating vehicles:\n----------") 32 | L = 4.8 33 | B = 1.8 34 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig, L, B) 35 | print("vehicles:", vehicles) 36 | print("moving vehicles:", moving_vehicles) 37 | 38 | print("\nCreating infeasible areas:\n----------") 39 | infeasible_areas = map.create_areas() 40 | print("infeasible areas:", infeasible_areas) 41 | 42 | print("\nCreating nodes:\n----------") 43 | nodes, free_nodes = map.get_nodes(infeasible_areas, solid_lines) 44 | print("nodes:", nodes) 45 | print("free nodes:", free_nodes) 46 | 47 | print("\nSetting start and target:\n----------") 48 | start, target = map.get_startandtarget() 49 | print("start:", start, ", target:", target) 50 | 51 | # A* algorithm for finding local targets 52 | t1 = time.time() 53 | 54 | print("\nEstablishing local targets by A* algorithm:\n----------") 55 | astar_start = [int(start[0] / del_block) * del_block, int(start[1] / del_block) * del_block] 56 | astar_target = [int(target[0] / del_block) * del_block, int(target[1] / del_block) * del_block] 57 | print("A* start:", astar_start, ", A* target:", astar_target) 58 | 59 | astar = ASTAR(del_block, astar_start, astar_target, free_nodes) 60 | astar_path, astar_direction, success = astar.findway() 61 | print("A* pre-path:", astar_path) 62 | print("A* pre-direction:", astar_direction) 63 | 64 | if success == 0: 65 | local_targets = [] 66 | exit() 67 | else: 68 | local_targets = astar.find_local(astar_path, astar_direction) 69 | print("local targets:", local_targets) 70 | 71 | t2 = time.time() 72 | astar_time = t2 - t1 73 | 74 | # vehicle model 75 | print("\nEstablishing the vehicle model:\n----------") 76 | l = 3 77 | # b = 1.6 78 | velocity = 20 # changeable 79 | init_force_direction = 0 80 | init_fi = 0 # changeable 81 | init_si = init_force_direction - init_fi # -40°~40° 82 | init_position = [start[0], start[1], init_fi] 83 | 84 | car = CAR(L, B, l, velocity) 85 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 86 | print("initial steering angle:", int(init_si / math.pi * 180)) 87 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 88 | print("initial front velocity:", front_v, ", initial front position:", front_p) 89 | print("initial vehicle direction:", int(fi / math.pi * 180)) 90 | 91 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 92 | print("difference between formula and subtract:", dif_distance - dif_distance2) 93 | print("distance difference:", dif_distance) 94 | 95 | car.draw_car(rear_p, front_p) 96 | moving_vehicles, p1, p2, p3, = car.draw_movingcar(moving_vehicles) # keeping static 97 | # plt.pause(0.1) 98 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 99 | p1.remove() 100 | p2.remove() 101 | p3.remove() 102 | 103 | # APF path planning algorithm 104 | t3 = time.time() 105 | 106 | print("\nAPF algorithm path planning:\n----------") 107 | k_att = 40 108 | k_rep = 10 109 | r_apf = 5 110 | k_replane = 5 111 | k_lane = 10 112 | k_car = 500 113 | lane_width = 3.5 114 | target_area = 3.5 115 | F_e = 50 116 | del_t = 0.05 # changeable 117 | max_iters = 1000 118 | 119 | rear_velocities = [car.v] 120 | path = [[rear_p[0], rear_p[1]]] 121 | direction = [fi] 122 | front_velocities = [vf] 123 | front_points = [[front_p[0], front_p[1]]] 124 | steering = [init_si] 125 | 126 | apf = APF(k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 127 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, direction, 128 | front_velocities, front_points, steering) 129 | 130 | targets = local_targets + [target] 131 | iters = 0 132 | 133 | for target in targets: 134 | rear_p, front_p, fi, iters, moving_vehicles = apf.pathplanning(rear_p, front_p, fi, target, iters, 135 | moving_vehicles) 136 | 137 | t4 = time.time() 138 | apf_time = t4 - t3 139 | 140 | print("total rear velocity:", apf.rear_velocities) 141 | print("total path:", apf.path) 142 | print("total direction:", apf.direction) 143 | print("total front velocity:", apf.front_velocities) 144 | print("total front position:", apf.front_points) 145 | print("total steering:", apf.steering) 146 | print("total iters:", iters) 147 | print("pre time:", astar_time) 148 | print("apf time:", apf_time) 149 | print("total distance difference:", car.dif_distances) 150 | 151 | print("attractive forces:", apf.F_atts) 152 | print("lane forces:", apf.F_lanes) 153 | print("obstacle forces:", apf.F_obss) 154 | print("vehicles forces:", apf.F_vehs) 155 | print("total forces:", apf.F_totals) 156 | 157 | map.final_draw(fig, apf.front_points, apf.path, init_movingvehis, moving_vehicles, L, B) 158 | plt.pause(0.1) 159 | 160 | plt.show() 161 | -------------------------------------------------------------------------------- /hybrid/apf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | from supplement import * 5 | 6 | 7 | class APF: 8 | 9 | def __init__(self, k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 10 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, 11 | direction, front_velocities, front_points, steering): 12 | self.k_att = k_att 13 | self.k_rep = k_rep 14 | self.r_apf = r_apf 15 | self.k_replane = k_replane 16 | self.k_lane = k_lane 17 | self.k_car = k_car 18 | self.lw = lane_width 19 | self.target_area = target_area 20 | self.F_e = F_e 21 | self.del_t = del_t 22 | self.max_iters = max_iters 23 | self.h_sollines, self.v_sollines = self.solid(solid_lines) 24 | self.t_lines = transition_lines 25 | self.obstacles = obstacles 26 | self.vehicles = vehicles 27 | self.L = L 28 | self.B = B 29 | self.car_Lmin = 2 * L 30 | self.car_Bmin = B 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | self.F_atts = [] 39 | self.F_lanes = [] 40 | self.F_obss = [] 41 | self.F_vehs = [] 42 | self.F_totals = [] 43 | 44 | def solid(self, solid_lines): 45 | h_sollines, v_sollines = [], [] 46 | 47 | for solid_line in solid_lines: 48 | if solid_line[3] == 'h': 49 | h_sollines.append(solid_line) 50 | else: 51 | v_sollines.append(solid_line) 52 | 53 | return h_sollines, v_sollines 54 | 55 | def attraction(self, front_p, target): 56 | force_att = self.k_att 57 | theta_att = math.atan2(target[1] - front_p[1], target[0] - front_p[0]) 58 | F_attx = force_att * math.cos(theta_att) 59 | F_atty = force_att * math.sin(theta_att) 60 | F_att = [F_attx, F_atty] 61 | 62 | return F_att 63 | 64 | def keeplane(self, front_p, supply, iters): 65 | use_sollinesx, use_sollinesy = supply.useful_lines(front_p) 66 | 67 | if use_sollinesx != [] and use_sollinesy != []: # 直角路口 68 | F_lane = supply.lane_xy(front_p, use_sollinesx, use_sollinesy, iters) 69 | elif use_sollinesx != [] and use_sollinesy == []: # 水平路或者水平丁字路口 70 | F_lane = supply.lane_x(front_p, use_sollinesx, iters) 71 | elif use_sollinesy != [] and use_sollinesx == []: # 竖直路或者竖直丁字路口 72 | F_lane = supply.lane_y(front_p, use_sollinesy, iters) 73 | else: # 十字路口 74 | F_lane = supply.lane(front_p, iters) 75 | 76 | return F_lane, use_sollinesx, use_sollinesy 77 | 78 | def repulsion(self, front_p, emergency, use_sollinesx, use_sollinesy, iters): 79 | F_obsx, F_obsy = 0, 0 80 | 81 | for obstacle in self.obstacles: 82 | r_obs = math.sqrt((front_p[0] - obstacle[0]) ** 2 + (front_p[1] - obstacle[1]) ** 2) - 1.5 * obstacle[2] 83 | r_min = obstacle[3] * self.r_apf 84 | obs_Le = obstacle[3] * obstacle[2] 85 | obs_Be = 0.5 * obstacle[3] * self.B 86 | 87 | if obstacle[4] == 'h': 88 | obs_L = abs(front_p[0] - obstacle[0]) 89 | obs_B = abs(front_p[1] - obstacle[1]) 90 | else: 91 | obs_L = abs(front_p[1] - obstacle[1]) 92 | obs_B = abs(front_p[0] - obstacle[0]) 93 | 94 | if r_obs > r_min: 95 | continue 96 | elif r_obs <= 0: 97 | print("APF is failed, because the vehicle moves into the safe area.") 98 | plt.savefig("E:/pythonpro/modifyandtry/9_dongtai/results/failed" + str(iters) + ".png") 99 | # exit() 100 | break 101 | elif obs_L <= obs_Le and obs_B <= obs_Be: 102 | print("The vehicle meets the local minimum.") 103 | Fe_obsx, Fe_obsy = emergency.obstacle_force(obstacle, use_sollinesx, use_sollinesy) 104 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 105 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 106 | F_obsx += force_rep * math.cos(theta_rep) + Fe_obsx 107 | F_obsy += force_rep * math.sin(theta_rep) + Fe_obsy 108 | else: 109 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 110 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 111 | F_obsx += force_rep * math.cos(theta_rep) 112 | F_obsy += force_rep * math.sin(theta_rep) 113 | 114 | F_obs = [F_obsx, F_obsy] 115 | 116 | return F_obs 117 | 118 | def leavecar(self, front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy, iters): 119 | F_vehx, F_vehy = 0, 0 120 | car_Le = self.L 121 | car_Be = 0.5 * self.B 122 | all_vehicles = self.vehicles + moving_vehicles 123 | 124 | for vehicle in all_vehicles: 125 | if vehicle[2] == 'h': 126 | car_L = abs(front_p[0] - vehicle[0]) 127 | car_B = abs(front_p[1] - vehicle[1]) 128 | else: 129 | car_L = abs(front_p[1] - vehicle[1]) 130 | car_B = abs(front_p[0] - vehicle[0]) 131 | 132 | if car_L > self.car_Lmin or car_B > self.car_Bmin: 133 | continue 134 | elif car_L == 0 and car_B == 0: 135 | print("APF is failed, because the vehicle vehicle crashes a car.") 136 | plt.savefig("E:/pythonpro/modifyandtry/9_dongtai/results/failed" + str(iters) + ".png") 137 | # exit() 138 | break 139 | elif car_L <= car_Le and car_B <= car_Be: 140 | print("The vehicle meets the local minimum.") 141 | Fe_vehx, Fe_vehy = emergency.vehicle_force(vehicle, use_sollinesx, use_sollinesy) 142 | if vehicle[2] == 'h': 143 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 144 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 145 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehx 146 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 147 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 148 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehy 149 | else: 150 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 151 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 152 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehx 153 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 154 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 155 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehy 156 | else: 157 | if vehicle[2] == 'h': 158 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 159 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 160 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 161 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 162 | else: 163 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 164 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 165 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 166 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 167 | 168 | F_veh = [F_vehx, F_vehy] 169 | 170 | return F_veh 171 | 172 | def pathplanning(self, rear_p, front_p, fi, target, iters, moving_vehicles): 173 | supply = SUPPLY(self.k_replane, self.k_lane, self.lw, self.h_sollines, self.v_sollines, self.t_lines) 174 | emergency = EMERGENCY(self.F_e, self.h_sollines, self.v_sollines, self.lw) 175 | 176 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 177 | 178 | while distance > self.target_area and iters <= self.max_iters: 179 | iters += 1 180 | print("\n----------\nNo.", iters) 181 | 182 | F_att = self.attraction(front_p, target) 183 | self.F_atts.append(F_att) 184 | F_lane, use_sollinesx, use_sollinesy = self.keeplane(front_p, supply, iters) 185 | self.F_lanes.append(F_lane) 186 | F_obs = self.repulsion(front_p, emergency, use_sollinesx, use_sollinesy, iters) 187 | self.F_obss.append(F_obs) 188 | F_veh = self.leavecar(front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy, iters) 189 | self.F_vehs.append(F_veh) 190 | 191 | F_total = [F_att[0] + F_lane[0] + F_obs[0] + F_veh[0], F_att[1] + F_lane[1] + F_obs[1] + F_veh[1]] 192 | self.F_totals.append(F_total) 193 | F_direction = math.atan2(F_total[1], F_total[0]) 194 | # print("total force:", F_total) 195 | # print("force direction:", int(F_direction / math.pi * 180)) 196 | 197 | if F_direction - fi > 40 / 180 * math.pi: 198 | si = 40 / 180 * math.pi 199 | elif F_direction - fi < -40 / 180 * math.pi: 200 | si = -40 / 180 * math.pi 201 | else: 202 | si = F_direction - fi 203 | # print("steering angle:", int(si / math.pi * 180)) 204 | 205 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 206 | print("rear velocity:", rear_v, ", rear position:", rear_p) 207 | # print("front velocity:", front_v, ", front position:", front_p) 208 | # print("vehicle direction:", int(fi / math.pi * 180)) 209 | 210 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 211 | # print("difference between formula and subtract:", dif_distance - dif_distance2) 212 | # print("distance difference:", dif_distance) 213 | 214 | self.car.draw_car(rear_p, front_p) 215 | moving_vehicles, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 216 | # plt.pause(0.1) 217 | p1.remove() 218 | p2.remove() 219 | p3.remove() 220 | 221 | self.rear_velocities.append(self.car.v) 222 | self.path.append([rear_p[0], rear_p[1]]) 223 | self.direction.append(fi) 224 | self.front_velocities.append(vf) 225 | self.front_points.append([front_p[0], front_p[1]]) 226 | self.steering.append(si) 227 | 228 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 229 | 230 | if distance <= self.target_area and iters <= self.max_iters: 231 | print("\n----------\nAPF is successful!\n----------") 232 | else: 233 | print("\n----------\nAPF is failed!\n----------") 234 | 235 | return rear_p, front_p, fi, iters, moving_vehicles 236 | -------------------------------------------------------------------------------- /hybrid/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, del_block): 9 | self.length = length 10 | self.width = width 11 | self.del_block = del_block 12 | self.no_block_x = int(length / del_block) 13 | self.no_block_y = int(width / del_block) 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | # xx, yy 37 | else: 38 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 39 | 40 | for dotted_line in dotted_lines: 41 | if dotted_line[3] == 'h': 42 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 43 | linewidth=2) 44 | else: 45 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 46 | linewidth=2) 47 | 48 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 49 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 50 | fig.add_patch(arc1) 51 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 52 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 53 | fig.add_patch(arc2) 54 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 55 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 56 | fig.add_patch(arc3) 57 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 58 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 59 | fig.add_patch(arc4) 60 | 61 | return 62 | 63 | def create_lanes(self, fig): 64 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 65 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 66 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 67 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 68 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 69 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 70 | 71 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 72 | 73 | return solid_lines, dotted_lines, transition_lines 74 | 75 | def draw_cycle(self, fig, obstacles): 76 | for obstacle in obstacles: 77 | if obstacle[3] == 2: 78 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 79 | else: 80 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 81 | fig.add_patch(cir) 82 | 83 | return 84 | 85 | def get_obstacles(self, fig): 86 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 87 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 88 | self.draw_cycle(fig, obstacles) 89 | 90 | return obstacles 91 | 92 | def draw_vehicle(self, fig, vehicles, L, B): 93 | for vehicle in vehicles: 94 | if vehicle[2] == 'h': 95 | x1 = vehicle[0] - L / 2 96 | y1 = vehicle[1] - B / 2 97 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='blue') 98 | fig.add_patch(rec) 99 | else: 100 | x1 = vehicle[0] - B / 2 101 | y1 = vehicle[1] - L / 2 102 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='blue') 103 | fig.add_patch(rec) 104 | 105 | return 106 | 107 | def draw_initmovingvehi(self, fig, init_movingvehis, L, B): 108 | for moving_vehicle in init_movingvehis: 109 | if moving_vehicle[2] == 'h': 110 | x1 = moving_vehicle[0] - L / 2 111 | y1 = moving_vehicle[1] - B / 2 112 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 113 | fig.add_patch(rec) 114 | else: 115 | x1 = moving_vehicle[0] - B / 2 116 | y1 = moving_vehicle[1] - L / 2 117 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 118 | fig.add_patch(rec) 119 | 120 | return 121 | 122 | def get_vehicles(self, fig, L, B): 123 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 124 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 125 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 126 | 127 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 128 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 129 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 130 | 131 | self.draw_vehicle(fig, vehicles, L, B) 132 | # self.draw_initmovingvehi(fig, init_movingvehis, L, B) 133 | 134 | return vehicles, init_movingvehis, moving_vehicles 135 | 136 | def create_areas(self): 137 | infeasible_areas = [[0, 14, 50, 73], [64, 14, 80, 73]] 138 | 139 | return infeasible_areas 140 | 141 | def get_nodes(self, infeasible_areas, solid_lines): 142 | nodes, free_nodes, new_x, new_y = [], [], [], [] 143 | 144 | for i in range(self.no_block_x + 1): 145 | new_x.append(int(i * self.del_block)) 146 | # plt.axvline(x=new_x[i], color='lightgray', lw=0.5) 147 | 148 | for j in range(self.no_block_y + 1): 149 | new_y.append(int(j * self.del_block)) 150 | # plt.axhline(y=new_y[j], color='lightgray', lw=0.5) 151 | 152 | for i in range(self.no_block_x + 1): 153 | for j in range(self.no_block_y + 1): 154 | nodes.append([new_x[i], new_y[j]]) 155 | free_nodes.append([new_x[i], new_y[j]]) 156 | 157 | for area in infeasible_areas: 158 | for node in nodes: 159 | if area[0] <= node[0] <= area[2] and area[1] <= node[1] <= area[3]: 160 | if node in free_nodes: 161 | free_nodes.remove([node[0], node[1]]) 162 | 163 | for solid_line in solid_lines: 164 | for node in nodes: 165 | if solid_line[3] == "h": 166 | if solid_line[0] <= node[0] <= solid_line[1] and node[1] == solid_line[2]: 167 | if node in free_nodes: 168 | free_nodes.remove([node[0], node[1]]) 169 | else: 170 | if node[0] == solid_line[0] and solid_line[1] <= node[1] <= solid_line[2]: 171 | if node in free_nodes: 172 | free_nodes.remove([node[0], node[1]]) 173 | 174 | ''' 175 | for free_node in free_nodes: 176 | plt.plot(free_node[0], free_node[1], 'x', color='lightblue') 177 | ''' 178 | 179 | return nodes, free_nodes 180 | 181 | def get_startandtarget(self): 182 | # start = [5, 2] 183 | # target = [63, 60] 184 | start = [5, 2] 185 | target = [78, 78] 186 | 187 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 188 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 189 | 190 | return start, target 191 | 192 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 193 | front_x, front_y, path_x, path_y = [], [], [], [] 194 | for i in front_points: 195 | front_x.append(i[0]) 196 | front_y.append(i[1]) 197 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 198 | for j in path: 199 | path_x.append(j[0]) 200 | path_y.append(j[1]) 201 | # plt.plot(front_x, front_y, c='green', linewidth=2) 202 | plt.plot(path_x, path_y, c='r', linewidth=2) 203 | 204 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 205 | if v1[2] == 'h': 206 | x1 = v1[0] - L / 2 207 | y1 = v1[1] - B / 2 208 | h = v2[0] + L / 2 - x1 209 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 210 | fig.add_patch(rec) 211 | else: 212 | x1 = v1[0] - B / 2 213 | y1 = v1[1] - L / 2 214 | h = v2[1] + L / 2 - y1 215 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 216 | fig.add_patch(rec) 217 | 218 | for vehicle in moving_vehicles: 219 | if vehicle[2] == 'h': 220 | x1 = vehicle[0] - L / 2 221 | y1 = vehicle[1] - B / 2 222 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 223 | fig.add_patch(rec) 224 | else: 225 | x1 = vehicle[0] - B / 2 226 | y1 = vehicle[1] - L / 2 227 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 228 | fig.add_patch(rec) 229 | 230 | # plt.pause(0.1) 231 | 232 | return 233 | -------------------------------------------------------------------------------- /hybrid/astar.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class ASTAR: 6 | 7 | class NODE: 8 | 9 | def __init__(self, current_node, father_node, target, cost=0): 10 | self.current_node = current_node 11 | self.father_node = father_node 12 | self.target = target 13 | self.direction = math.atan2(self.current_node[1] - self.father_node[1], 14 | self.current_node[0] - self.father_node[0]) 15 | self.g = cost 16 | self.h = abs(self.target[0] - self.current_node[0]) + abs(self.target[1] - self.current_node[1]) 17 | self.f = self.g + self.h 18 | 19 | def __init__(self, del_block, astar_start, astar_target, free_nodes): 20 | self.del_block = del_block 21 | self.astar_start = astar_start 22 | self.astar_target = astar_target 23 | self.open_set = free_nodes 24 | self.close_set = [] 25 | 26 | def possible_moves(self, current_node): 27 | next_nodes, next_costs = [], [] 28 | 29 | possible_nodes = [[current_node[0] + self.del_block, current_node[1]], 30 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 31 | [current_node[0], current_node[1] + self.del_block], 32 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 33 | [current_node[0] - self.del_block, current_node[1]], 34 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 35 | [current_node[0], current_node[1] - self.del_block], 36 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 37 | 38 | possible_costs = [self.del_block, math.hypot(self.del_block, self.del_block), 39 | self.del_block, math.hypot(self.del_block, self.del_block), 40 | self.del_block, math.hypot(self.del_block, self.del_block), 41 | self.del_block, math.hypot(self.del_block, self.del_block)] 42 | 43 | for i in range(8): 44 | if possible_nodes[i] in self.open_set: 45 | next_nodes.append(possible_nodes[i]) 46 | next_costs.append(possible_costs[i]) 47 | 48 | return next_nodes, next_costs 49 | 50 | def get_minf(self, best, next): 51 | if next.f < best.f: 52 | best = next 53 | 54 | return best 55 | 56 | def findway(self): 57 | path, direction = [], [] 58 | success = 0 59 | 60 | father_node = self.astar_start 61 | current_node = self.astar_start 62 | current = ASTAR.NODE(current_node, father_node, self.astar_target) 63 | 64 | while current.current_node != self.astar_target: 65 | 66 | if current.current_node in self.open_set: 67 | self.open_set.remove(current.current_node) 68 | self.close_set.append(current.current_node) 69 | 70 | path.append(current.current_node) 71 | print("current node:", current.current_node, ", previous angle:", current.direction, ", current g:", 72 | current.g, ", h:", current.h, ", f:", current.f) 73 | # plt.plot(current.current_node[0], current.current_node[1], '.k', markersize=5) 74 | # plt.pause(0.1) 75 | 76 | next_nodes, next_costs = self.possible_moves(current.current_node) 77 | next_set = [] 78 | for next_node, next_cost in zip(next_nodes, next_costs): 79 | next = ASTAR.NODE(next_node, current.current_node, self.astar_target, next_cost + current.g) 80 | next_set.append(next) 81 | 82 | current.f = float("inf") 83 | best = current 84 | for next in next_set: 85 | best = self.get_minf(best, next) 86 | 87 | current_node = best.current_node 88 | father_node = best.father_node 89 | current_cost = best.g 90 | current = ASTAR.NODE(current_node, father_node, self.astar_target, current_cost) 91 | direction.append(current.direction) 92 | 93 | else: 94 | print("A* pre path planning is failed!\n----------") 95 | 96 | return path, direction, success 97 | 98 | print("A* pre path planning is successful!\n----------") 99 | success = 1 100 | path.append(current.current_node) 101 | # plt.plot(current.current_node[0], current.current_node[1], '.k', markersize=5) 102 | # plt.pause(0.1) 103 | 104 | return path, direction, success 105 | 106 | def find_local(self, path, direction): 107 | local_targets = [] 108 | 109 | path.remove(path[0]) 110 | path.remove(path[-1]) 111 | 112 | angle = [] 113 | for i in range(len(direction) - 1): 114 | angle.append(direction[i + 1] - direction[i]) 115 | 116 | i = 1 117 | for pa, ang in zip(path, angle): 118 | if ang == 0: 119 | continue 120 | else: 121 | if pa[0] == self.astar_start[0] or pa[1] == self.astar_start[1]: 122 | local_targets.append(pa) 123 | plt.plot(pa[0], pa[1], 'o', color='orangered', markersize=10) 124 | # plt.pause(0.1) 125 | elif pa[0] == self.astar_target[0] or pa[1] == self.astar_target[1]: 126 | continue 127 | else: 128 | if i % 2 == 0: 129 | local_targets.append(pa) 130 | plt.plot(pa[0], pa[1], 'o', color='orangered', markersize=10) 131 | # plt.pause(0.1) 132 | i += 1 133 | else: 134 | i += 1 135 | 136 | return local_targets 137 | -------------------------------------------------------------------------------- /hybrid/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, p1, p2, p3, = [], [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | ''' 89 | new_moving_vehicles, p, = [], [] 90 | 91 | for moving_vehicle in moving_vehicles: 92 | if moving_vehicle[2] == 'h': 93 | x1 = moving_vehicle[0] - self.l / 2 94 | y1 = moving_vehicle[1] 95 | x2 = moving_vehicle[0] + self.l / 2 96 | y2 = moving_vehicle[1] 97 | draw_x0 = [x1, x2] 98 | draw_y0 = [y1, y2] 99 | p, = plt.plot(draw_x0, draw_y0, c='green', linewidth=10) 100 | 101 | coop_x = moving_vehicle[0] + moving_vehicle[3] * del_t 102 | coop_y = moving_vehicle[1] 103 | new_moving_vehicles.append([coop_x, coop_y, 'h', moving_vehicle[3]]) 104 | else: 105 | x1 = moving_vehicle[0] 106 | y1 = moving_vehicle[1] - self.l / 2 107 | x2 = moving_vehicle[0] 108 | y2 = moving_vehicle[1] + self.l / 2 109 | draw_x0 = [x1, x2] 110 | draw_y0 = [y1, y2] 111 | p, = plt.plot(draw_x0, draw_y0, c='green', linewidth=10) 112 | 113 | coop_x = moving_vehicle[0] 114 | coop_y = moving_vehicle[1] + moving_vehicle[3] * del_t 115 | new_moving_vehicles.append([coop_x, coop_y, 'v', moving_vehicle[3]]) 116 | 117 | moving_vehicles.clear() 118 | moving_vehicles = new_moving_vehicles 119 | ''' 120 | 121 | return moving_vehicles, p1, p2, p3, 122 | -------------------------------------------------------------------------------- /hybrid/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from astar import * 6 | from car import * 7 | from apf import * 8 | 9 | 10 | # main 11 | # Note: If there is an output image during the path planning process, it means failure. 12 | if __name__ == "__main__": 13 | print("Creating a map:\n----------") 14 | length = 80 15 | width = 80 16 | del_block = 2 17 | 18 | map = MAP(length, width, del_block) 19 | fig = map.create_map() 20 | 21 | print("\nCreating lanes:\n----------") 22 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 23 | print("solid lines:", solid_lines) 24 | print("dotted lines:", dotted_lines) 25 | print("transition lines:", transition_lines) 26 | 27 | print("\nCreating obstacles:\n----------") 28 | obstacles = map.get_obstacles(fig) 29 | print("obstacles:", obstacles) 30 | 31 | print("\nCreating vehicles:\n----------") 32 | L = 4.8 33 | B = 1.8 34 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig, L, B) 35 | print("vehicles:", vehicles) 36 | print("moving vehicles:", moving_vehicles) 37 | 38 | print("\nCreating infeasible areas:\n----------") 39 | infeasible_areas = map.create_areas() 40 | print("infeasible areas:", infeasible_areas) 41 | 42 | print("\nCreating nodes:\n----------") 43 | nodes, free_nodes = map.get_nodes(infeasible_areas, solid_lines) 44 | print("nodes:", nodes) 45 | print("free nodes:", free_nodes) 46 | 47 | print("\nSetting start and target:\n----------") 48 | start, target = map.get_startandtarget() 49 | print("start:", start, ", target:", target) 50 | 51 | # A* algorithm for finding local targets 52 | t1 = time.time() 53 | 54 | print("\nEstablishing local targets by A* algorithm:\n----------") 55 | astar_start = [int(start[0] / del_block) * del_block, int(start[1] / del_block) * del_block] 56 | astar_target = [int(target[0] / del_block) * del_block, int(target[1] / del_block) * del_block] 57 | print("A* start:", astar_start, ", A* target:", astar_target) 58 | 59 | astar = ASTAR(del_block, astar_start, astar_target, free_nodes) 60 | astar_path, astar_direction, success = astar.findway() 61 | print("A* pre-path:", astar_path) 62 | print("A* pre-direction:", astar_direction) 63 | 64 | if success == 0: 65 | local_targets = [] 66 | exit() 67 | else: 68 | local_targets = astar.find_local(astar_path, astar_direction) 69 | print("local targets:", local_targets) 70 | 71 | t2 = time.time() 72 | astar_time = t2 - t1 73 | 74 | # vehicle model 75 | print("\nEstablishing the vehicle model:\n----------") 76 | l = 3 77 | # b = 1.6 78 | velocity = 20 # changeable 79 | init_force_direction = 0 80 | init_fi = 0 # changeable 81 | init_si = init_force_direction - init_fi # -40°~40° 82 | init_position = [start[0], start[1], init_fi] 83 | 84 | car = CAR(L, B, l, velocity) 85 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 86 | print("initial steering angle:", int(init_si / math.pi * 180)) 87 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 88 | print("initial front velocity:", front_v, ", initial front position:", front_p) 89 | print("initial vehicle direction:", int(fi / math.pi * 180)) 90 | 91 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 92 | print("difference between formula and subtract:", dif_distance - dif_distance2) 93 | print("distance difference:", dif_distance) 94 | 95 | car.draw_car(rear_p, front_p) 96 | moving_vehicles, p1, p2, p3, = car.draw_movingcar(moving_vehicles) # keeping static 97 | # plt.pause(0.1) 98 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 99 | p1.remove() 100 | p2.remove() 101 | p3.remove() 102 | 103 | # APF path planning algorithm 104 | t3 = time.time() 105 | 106 | print("\nAPF algorithm path planning:\n----------") 107 | k_att = 40 108 | k_rep = 10 109 | r_apf = 5 110 | k_replane = 5 111 | k_lane = 10 112 | k_car = 500 113 | lane_width = 3.5 114 | target_area = 3.5 115 | F_e = 50 116 | del_t = 0.05 # changeable 117 | max_iters = 1000 118 | 119 | rear_velocities = [car.v] 120 | path = [[rear_p[0], rear_p[1]]] 121 | direction = [fi] 122 | front_velocities = [vf] 123 | front_points = [[front_p[0], front_p[1]]] 124 | steering = [init_si] 125 | 126 | apf = APF(k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 127 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, direction, 128 | front_velocities, front_points, steering) 129 | 130 | targets = local_targets + [target] 131 | iters = 0 132 | 133 | for target in targets: 134 | rear_p, front_p, fi, iters, moving_vehicles = apf.pathplanning(rear_p, front_p, fi, target, iters, 135 | moving_vehicles) 136 | 137 | t4 = time.time() 138 | apf_time = t4 - t3 139 | 140 | print("total rear velocity:", apf.rear_velocities) 141 | print("total path:", apf.path) 142 | print("total direction:", apf.direction) 143 | print("total front velocity:", apf.front_velocities) 144 | print("total front position:", apf.front_points) 145 | print("total steering:", apf.steering) 146 | print("total iters:", iters) 147 | print("pre time:", astar_time) 148 | print("apf time:", apf_time) 149 | print("total distance difference:", car.dif_distances) 150 | 151 | print("attractive forces:", apf.F_atts) 152 | print("lane forces:", apf.F_lanes) 153 | print("obstacle forces:", apf.F_obss) 154 | print("vehicles forces:", apf.F_vehs) 155 | print("total forces:", apf.F_totals) 156 | 157 | map.final_draw(fig, apf.front_points, apf.path, init_movingvehis, moving_vehicles, L, B) 158 | plt.pause(0.1) 159 | 160 | plt.show() 161 | -------------------------------------------------------------------------------- /result/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_1.png -------------------------------------------------------------------------------- /result/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_2.png -------------------------------------------------------------------------------- /result/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_3.png -------------------------------------------------------------------------------- /result/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_4.png -------------------------------------------------------------------------------- /result/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_5.png -------------------------------------------------------------------------------- /result/Figure_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Figure_6.png -------------------------------------------------------------------------------- /result/Uatt.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Uatt.jpg -------------------------------------------------------------------------------- /result/Ucar.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Ucar.jpg -------------------------------------------------------------------------------- /result/Ulane.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Ulane.jpg -------------------------------------------------------------------------------- /result/Uobs.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Uobs.jpg -------------------------------------------------------------------------------- /result/Utotal.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/Utotal.jpg -------------------------------------------------------------------------------- /result/a1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/a1.png -------------------------------------------------------------------------------- /result/a2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/a2.png -------------------------------------------------------------------------------- /result/a3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/a3.png -------------------------------------------------------------------------------- /result/a4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/a4.png -------------------------------------------------------------------------------- /result/b1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/b1.png -------------------------------------------------------------------------------- /result/b2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/b2.png -------------------------------------------------------------------------------- /result/b3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/b3.png -------------------------------------------------------------------------------- /result/b4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/b4.png -------------------------------------------------------------------------------- /result/dynamic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/dynamic.png -------------------------------------------------------------------------------- /result/dynamic_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/dynamic_2.png -------------------------------------------------------------------------------- /result/static.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/static.png -------------------------------------------------------------------------------- /result/static_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/result/static_2.png -------------------------------------------------------------------------------- /singleAPF (new)/__pycache__/apf.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAPF (new)/__pycache__/apf.cpython-37.pyc -------------------------------------------------------------------------------- /singleAPF (new)/__pycache__/assistant.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAPF (new)/__pycache__/assistant.cpython-37.pyc -------------------------------------------------------------------------------- /singleAPF (new)/__pycache__/car.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAPF (new)/__pycache__/car.cpython-37.pyc -------------------------------------------------------------------------------- /singleAPF (new)/__pycache__/supplement.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAPF (new)/__pycache__/supplement.cpython-37.pyc -------------------------------------------------------------------------------- /singleAPF (new)/apf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | from supplement import * 5 | 6 | 7 | class APF: 8 | 9 | def __init__(self, k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 10 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, 11 | direction, front_velocities, front_points, steering): 12 | self.k_att = k_att 13 | self.k_rep = k_rep 14 | self.r_apf = r_apf 15 | self.k_replane = k_replane 16 | self.k_lane = k_lane 17 | self.k_car = k_car 18 | self.lw = lane_width 19 | self.target_area = target_area 20 | self.F_e = F_e 21 | self.del_t = del_t 22 | self.max_iters = max_iters 23 | self.h_sollines, self.v_sollines = self.solid(solid_lines) 24 | self.t_lines = transition_lines 25 | self.obstacles = obstacles 26 | self.vehicles = vehicles 27 | self.L = L 28 | self.B = B 29 | self.car_Lmin = 2 * L 30 | self.car_Bmin = B 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | self.F_atts = [] 39 | self.F_lanes = [] 40 | self.F_obss = [] 41 | self.F_vehs = [] 42 | self.F_totals = [] 43 | 44 | def solid(self, solid_lines): 45 | h_sollines, v_sollines = [], [] 46 | 47 | for solid_line in solid_lines: 48 | if solid_line[3] == 'h': 49 | h_sollines.append(solid_line) 50 | else: 51 | v_sollines.append(solid_line) 52 | 53 | return h_sollines, v_sollines 54 | 55 | def attraction(self, front_p, target): 56 | force_att = self.k_att 57 | theta_att = math.atan2(target[1] - front_p[1], target[0] - front_p[0]) 58 | F_attx = force_att * math.cos(theta_att) 59 | F_atty = force_att * math.sin(theta_att) 60 | F_att = [F_attx, F_atty] 61 | 62 | return F_att 63 | 64 | def keeplane(self, front_p, supply): 65 | use_sollinesx, use_sollinesy = supply.useful_lines(front_p) 66 | 67 | if use_sollinesx != [] and use_sollinesy != []: 68 | F_lane = supply.lane_xy(front_p, use_sollinesx, use_sollinesy) 69 | elif use_sollinesx != [] and use_sollinesy == []: 70 | F_lane = supply.lane_x(front_p, use_sollinesx) 71 | elif use_sollinesy != [] and use_sollinesx == []: 72 | F_lane = supply.lane_y(front_p, use_sollinesy) 73 | else: # 十字路口 74 | F_lane = supply.lane(front_p) 75 | 76 | return F_lane, use_sollinesx, use_sollinesy 77 | 78 | def repulsion(self, front_p, emergency, use_sollinesx, use_sollinesy): 79 | F_obsx, F_obsy = 0, 0 80 | 81 | for obstacle in self.obstacles: 82 | r_obs = math.sqrt((front_p[0] - obstacle[0]) ** 2 + (front_p[1] - obstacle[1]) ** 2) - 1.5 * obstacle[2] 83 | r_min = obstacle[3] * self.r_apf 84 | obs_Le = obstacle[3] * obstacle[2] 85 | obs_Be = 0.5 * obstacle[3] * self.B 86 | 87 | if obstacle[4] == 'h': 88 | obs_L = abs(front_p[0] - obstacle[0]) 89 | obs_B = abs(front_p[1] - obstacle[1]) 90 | else: 91 | obs_L = abs(front_p[1] - obstacle[1]) 92 | obs_B = abs(front_p[0] - obstacle[0]) 93 | 94 | if r_obs > r_min: 95 | continue 96 | elif r_obs <= 0: 97 | print("APF is failed, because the vehicle moves into the safe area.") 98 | exit() 99 | elif obs_L <= obs_Le and obs_B <= obs_Be: 100 | print("The vehicle meets the local minimum.") 101 | Fe_obsx, Fe_obsy = emergency.obstacle_force(obstacle, use_sollinesx, use_sollinesy) 102 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 103 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 104 | F_obsx += force_rep * math.cos(theta_rep) + Fe_obsx 105 | F_obsy += force_rep * math.sin(theta_rep) + Fe_obsy 106 | else: 107 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 108 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 109 | F_obsx += force_rep * math.cos(theta_rep) 110 | F_obsy += force_rep * math.sin(theta_rep) 111 | 112 | F_obs = [F_obsx, F_obsy] 113 | 114 | return F_obs 115 | 116 | def leavecar(self, front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy): 117 | F_vehx, F_vehy = 0, 0 118 | car_Le = self.L 119 | car_Be = 0.5 * self.B 120 | all_vehicles = self.vehicles + moving_vehicles 121 | 122 | for vehicle in all_vehicles: 123 | if vehicle[2] == 'h': 124 | car_L = abs(front_p[0] - vehicle[0]) 125 | car_B = abs(front_p[1] - vehicle[1]) 126 | else: 127 | car_L = abs(front_p[1] - vehicle[1]) 128 | car_B = abs(front_p[0] - vehicle[0]) 129 | 130 | if car_L > self.car_Lmin or car_B > self.car_Bmin: 131 | continue 132 | elif car_L == 0 and car_B == 0: 133 | print("APF is failed, because the vehicle vehicle crashes a car.") 134 | exit() 135 | elif car_L <= car_Le and car_B <= car_Be: 136 | print("The vehicle meets the local minimum.") 137 | Fe_vehx, Fe_vehy = emergency.vehicle_force(vehicle, use_sollinesx, use_sollinesy) 138 | if vehicle[2] == 'h': 139 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 140 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 141 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehx 142 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 143 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 144 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehy 145 | else: 146 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 147 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 148 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehx 149 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 150 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 151 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehy 152 | else: 153 | if vehicle[2] == 'h': 154 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 155 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 156 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 157 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 158 | else: 159 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 160 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 161 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 162 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 163 | 164 | F_veh = [F_vehx, F_vehy] 165 | 166 | return F_veh 167 | 168 | def pathplanning(self, rear_p, front_p, fi, target, iters, moving_vehicles): 169 | supply = SUPPLY(self.k_replane, self.k_lane, self.lw, self.h_sollines, self.v_sollines, self.t_lines) 170 | emergency = EMERGENCY(self.F_e, self.h_sollines, self.v_sollines, self.lw) 171 | 172 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 173 | 174 | while distance > self.target_area and iters <= self.max_iters: 175 | iters += 1 176 | print("\n----------\nNo.", iters) 177 | 178 | F_att = self.attraction(front_p, target) 179 | self.F_atts.append(F_att) 180 | F_lane, use_sollinesx, use_sollinesy = self.keeplane(front_p, supply) 181 | self.F_lanes.append(F_lane) 182 | F_obs = self.repulsion(front_p, emergency, use_sollinesx, use_sollinesy) 183 | self.F_obss.append(F_obs) 184 | F_veh = self.leavecar(front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy) 185 | self.F_vehs.append(F_veh) 186 | 187 | F_total = [F_att[0] + F_lane[0] + F_obs[0] + F_veh[0], F_att[1] + F_lane[1] + F_obs[1] + F_veh[1]] 188 | self.F_totals.append(F_total) 189 | F_direction = math.atan2(F_total[1], F_total[0]) 190 | print("total force:", F_total) 191 | print("force direction:", int(F_direction / math.pi * 180)) 192 | 193 | if F_direction - fi > 40 / 180 * math.pi: 194 | si = 40 / 180 * math.pi 195 | elif F_direction - fi < -40 / 180 * math.pi: 196 | si = -40 / 180 * math.pi 197 | else: 198 | si = F_direction - fi 199 | print("steering angle:", int(si / math.pi * 180)) 200 | 201 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 202 | print("rear velocity:", rear_v, ", rear position:", rear_p) 203 | # print("front velocity:", front_v, ", front position:", front_p) 204 | # print("vehicle direction:", int(fi / math.pi * 180)) 205 | 206 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 207 | print("difference between formula and subtract:", dif_distance - dif_distance2) 208 | print("distance difference:", dif_distance) 209 | 210 | self.car.draw_car(rear_p, front_p) 211 | moving_vehicles, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 212 | # plt.pause(self.del_t) 213 | p1.remove() 214 | p2.remove() 215 | p3.remove() 216 | 217 | self.rear_velocities.append(self.car.v) 218 | self.path.append([rear_p[0], rear_p[1]]) 219 | self.direction.append(fi) 220 | self.front_velocities.append(vf) 221 | self.front_points.append([front_p[0], front_p[1]]) 222 | self.steering.append(si) 223 | 224 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 225 | 226 | if distance <= self.target_area and iters <= self.max_iters: 227 | print("\n----------\nAPF is successful!\n----------") 228 | else: 229 | print("\n----------\nAPF is failed!\n----------") 230 | 231 | return rear_p, front_p, fi, iters, moving_vehicles 232 | -------------------------------------------------------------------------------- /singleAPF (new)/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, del_block): 9 | self.length = length 10 | self.width = width 11 | self.del_block = del_block 12 | self.no_block_x = int(length / del_block) 13 | self.no_block_y = int(width / del_block) 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | else: 37 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 38 | 39 | for dotted_line in dotted_lines: 40 | if dotted_line[3] == 'h': 41 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 42 | linewidth=2) 43 | else: 44 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 45 | linewidth=2) 46 | 47 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 48 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 49 | fig.add_patch(arc1) 50 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 51 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 52 | fig.add_patch(arc2) 53 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 54 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 55 | fig.add_patch(arc3) 56 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 57 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 58 | fig.add_patch(arc4) 59 | 60 | return 61 | 62 | def create_lanes(self, fig): 63 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 64 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 65 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 66 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 67 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 68 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 69 | 70 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 71 | 72 | return solid_lines, dotted_lines, transition_lines 73 | 74 | def draw_cycle(self, fig, obstacles): 75 | for obstacle in obstacles: 76 | if obstacle[3] == 2: 77 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 78 | else: 79 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 80 | fig.add_patch(cir) 81 | 82 | return 83 | 84 | def get_obstacles(self, fig): 85 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 86 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 87 | self.draw_cycle(fig, obstacles) 88 | 89 | return obstacles 90 | 91 | def draw_vehicle(self, fig, vehicles, L, B): 92 | for vehicle in vehicles: 93 | if vehicle[2] == 'h': 94 | x1 = vehicle[0] - L / 2 95 | y1 = vehicle[1] - B / 2 96 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='blue') 97 | fig.add_patch(rec) 98 | else: 99 | x1 = vehicle[0] - B / 2 100 | y1 = vehicle[1] - L / 2 101 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='blue') 102 | fig.add_patch(rec) 103 | 104 | return 105 | 106 | def get_vehicles(self, fig, L, B): 107 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 108 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 109 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 110 | 111 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 112 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 113 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 114 | 115 | self.draw_vehicle(fig, vehicles, L, B) 116 | 117 | return vehicles, init_movingvehis, moving_vehicles 118 | 119 | def get_startandtarget(self): 120 | # start = [5, 2] 121 | # target = [63, 60] 122 | start = [5, 2] 123 | target = [78, 78] 124 | 125 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 126 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 127 | 128 | return start, target 129 | 130 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 131 | front_x, front_y, path_x, path_y = [], [], [], [] 132 | for i in front_points: 133 | front_x.append(i[0]) 134 | front_y.append(i[1]) 135 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 136 | for j in path: 137 | path_x.append(j[0]) 138 | path_y.append(j[1]) 139 | # plt.plot(front_x, front_y, c='green', linewidth=2) 140 | plt.plot(path_x, path_y, c='r', linewidth=2) 141 | 142 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 143 | if v1[2] == 'h': 144 | x1 = v1[0] - L / 2 145 | y1 = v1[1] - B / 2 146 | h = v2[0] + L / 2 - x1 147 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 148 | fig.add_patch(rec) 149 | else: 150 | x1 = v1[0] - B / 2 151 | y1 = v1[1] - L / 2 152 | h = v2[1] + L / 2 - y1 153 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 154 | fig.add_patch(rec) 155 | 156 | for vehicle in moving_vehicles: 157 | if vehicle[2] == 'h': 158 | x1 = vehicle[0] - L / 2 159 | y1 = vehicle[1] - B / 2 160 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 161 | fig.add_patch(rec) 162 | else: 163 | x1 = vehicle[0] - B / 2 164 | y1 = vehicle[1] - L / 2 165 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 166 | fig.add_patch(rec) 167 | 168 | # plt.pause(0.1) 169 | 170 | return 171 | -------------------------------------------------------------------------------- /singleAPF (new)/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, p1, p2, = [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | return moving_vehicles, p1, p2, p3, 89 | -------------------------------------------------------------------------------- /singleAPF (new)/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from car import * 6 | from apf import * 7 | 8 | 9 | # main 10 | if __name__ == "__main__": 11 | print("Creating a map:\n----------") 12 | length = 80 13 | width = 80 14 | del_block = 2 15 | 16 | map = MAP(length, width, del_block) 17 | fig = map.create_map() 18 | 19 | print("\nCreating lanes:\n----------") 20 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 21 | print("solid lines:", solid_lines) 22 | print("dotted lines:", dotted_lines) 23 | print("transition lines:", transition_lines) 24 | 25 | print("\nCreating obstacles:\n----------") 26 | obstacles = map.get_obstacles(fig) 27 | print("obstacles:", obstacles) 28 | 29 | print("\nCreating vehicles:\n----------") 30 | L = 4.8 31 | B = 1.8 32 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig, L, B) 33 | print("vehicles:", vehicles) 34 | print("moving vehicles:", moving_vehicles) 35 | 36 | print("\nSetting start and target:\n----------") 37 | start, target = map.get_startandtarget() 38 | print("start:", start, ", target:", target) 39 | 40 | # vehicle model 41 | print("\nEstablishing the vehicle model:\n----------") 42 | l = 3 43 | # b = 1.6 44 | velocity = 20 # changeable 45 | init_force_direction = 0 46 | init_fi = 0 # changeable 47 | init_si = init_force_direction - init_fi # -40°~40° 48 | init_position = [start[0], start[1], init_fi] 49 | 50 | car = CAR(L, B, l, velocity) 51 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 52 | print("initial steering angle:", int(init_si / math.pi * 180)) 53 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 54 | print("initial front velocity:", front_v, ", initial front position:", front_p) 55 | print("initial vehicle direction:", int(fi / math.pi * 180)) 56 | 57 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 58 | print("difference between formula and subtract:", dif_distance - dif_distance2) 59 | print("distance difference:", dif_distance) 60 | 61 | car.draw_car(rear_p, front_p) 62 | moving_vehicles, p1, p2, p3, = car.draw_movingcar(moving_vehicles) 63 | # plt.pause(0.1) 64 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 65 | p1.remove() 66 | p2.remove() 67 | p3.remove() 68 | 69 | # APF path planning algorithm 70 | t1 = time.time() 71 | 72 | print("\nAPF algorithm path planning:\n----------") 73 | k_att = 40 74 | k_rep = 10 75 | r_apf = 5 76 | k_replane = 5 77 | k_lane = 10 78 | k_car = 500 79 | lane_width = 3.5 80 | target_area = 3.5 81 | F_e = 50 82 | del_t = 0.05 # changeable 83 | max_iters = 105 84 | 85 | rear_velocities = [car.v] 86 | path = [[rear_p[0], rear_p[1]]] 87 | direction = [fi] 88 | front_velocities = [vf] 89 | front_points = [[front_p[0], front_p[1]]] 90 | steering = [init_si] 91 | 92 | apf = APF(k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 93 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, direction, 94 | front_velocities, front_points, steering) 95 | 96 | iters = 0 97 | rear_p, front_p, fi, iters, moving_vehicles = apf.pathplanning(rear_p, front_p, fi, target, iters, moving_vehicles) 98 | 99 | t2 = time.time() 100 | apf_time = t2 - t1 101 | 102 | print("total rear velocity:", apf.rear_velocities) 103 | print("total path:", apf.path) 104 | print("total direction:", apf.direction) 105 | print("total front velocity:", apf.front_velocities) 106 | print("total front position:", apf.front_points) 107 | print("total steering:", apf.steering) 108 | print("total iters:", iters) 109 | print("apf time:", apf_time) 110 | print("total distance difference:", car.dif_distances) 111 | 112 | print("attractive forces:", apf.F_atts) 113 | print("lane forces:", apf.F_lanes) 114 | print("obstacle forces:", apf.F_obss) 115 | print("vehicles forces:", apf.F_vehs) 116 | print("total forces:", apf.F_totals) 117 | 118 | map.final_draw(fig, apf.front_points, apf.path, init_movingvehis, moving_vehicles, L, B) 119 | plt.pause(0.1) 120 | 121 | plt.show() 122 | -------------------------------------------------------------------------------- /singleAPF/apf.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | from supplement import * 5 | 6 | 7 | class APF: 8 | 9 | def __init__(self, k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 10 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, 11 | direction, front_velocities, front_points, steering): 12 | self.k_att = k_att 13 | self.k_rep = k_rep 14 | self.r_apf = r_apf 15 | self.k_replane = k_replane 16 | self.k_lane = k_lane 17 | self.k_car = k_car 18 | self.lw = lane_width 19 | self.target_area = target_area 20 | self.F_e = F_e 21 | self.del_t = del_t 22 | self.max_iters = max_iters 23 | self.h_sollines, self.v_sollines = self.solid(solid_lines) 24 | self.t_lines = transition_lines 25 | self.obstacles = obstacles 26 | self.vehicles = vehicles 27 | self.L = L 28 | self.B = B 29 | self.car_Lmin = 2 * L 30 | self.car_Bmin = B 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | self.F_atts = [] 39 | self.F_lanes = [] 40 | self.F_obss = [] 41 | self.F_vehs = [] 42 | self.F_totals = [] 43 | 44 | def solid(self, solid_lines): 45 | h_sollines, v_sollines = [], [] 46 | 47 | for solid_line in solid_lines: 48 | if solid_line[3] == 'h': 49 | h_sollines.append(solid_line) 50 | else: 51 | v_sollines.append(solid_line) 52 | 53 | return h_sollines, v_sollines 54 | 55 | def attraction(self, front_p, target): 56 | force_att = self.k_att 57 | theta_att = math.atan2(target[1] - front_p[1], target[0] - front_p[0]) 58 | F_attx = force_att * math.cos(theta_att) 59 | F_atty = force_att * math.sin(theta_att) 60 | F_att = [F_attx, F_atty] 61 | 62 | return F_att 63 | 64 | def keeplane(self, front_p, supply): 65 | use_sollinesx, use_sollinesy = supply.useful_lines(front_p) 66 | 67 | if use_sollinesx != [] and use_sollinesy != []: 68 | F_lane = supply.lane_xy(front_p, use_sollinesx, use_sollinesy) 69 | elif use_sollinesx != [] and use_sollinesy == []: 70 | F_lane = supply.lane_x(front_p, use_sollinesx) 71 | elif use_sollinesy != [] and use_sollinesx == []: 72 | F_lane = supply.lane_y(front_p, use_sollinesy) 73 | else: # 十字路口 74 | F_lane = supply.lane(front_p) 75 | 76 | return F_lane, use_sollinesx, use_sollinesy 77 | 78 | def repulsion(self, front_p, emergency, use_sollinesx, use_sollinesy): 79 | F_obsx, F_obsy = 0, 0 80 | 81 | for obstacle in self.obstacles: 82 | r_obs = math.sqrt((front_p[0] - obstacle[0]) ** 2 + (front_p[1] - obstacle[1]) ** 2) - 1.5 * obstacle[2] 83 | r_min = obstacle[3] * self.r_apf 84 | obs_Le = obstacle[3] * obstacle[2] 85 | obs_Be = 0.5 * obstacle[3] * self.B 86 | 87 | if obstacle[4] == 'h': 88 | obs_L = abs(front_p[0] - obstacle[0]) 89 | obs_B = abs(front_p[1] - obstacle[1]) 90 | else: 91 | obs_L = abs(front_p[1] - obstacle[1]) 92 | obs_B = abs(front_p[0] - obstacle[0]) 93 | 94 | if r_obs > r_min: 95 | continue 96 | elif r_obs <= 0: 97 | print("APF is failed, because the vehicle moves into the safe area.") 98 | exit() 99 | elif obs_L <= obs_Le and obs_B <= obs_Be: 100 | print("The vehicle meets the local minimum.") 101 | Fe_obsx, Fe_obsy = emergency.obstacle_force(obstacle, use_sollinesx, use_sollinesy) 102 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 103 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 104 | F_obsx += force_rep * math.cos(theta_rep) + Fe_obsx 105 | F_obsy += force_rep * math.sin(theta_rep) + Fe_obsy 106 | else: 107 | force_rep = 2 * self.k_rep * obstacle[3] * (1 / r_obs - 1 / r_min) / r_obs ** 2 108 | theta_rep = math.atan2(front_p[1] - obstacle[1], front_p[0] - obstacle[0]) 109 | F_obsx += force_rep * math.cos(theta_rep) 110 | F_obsy += force_rep * math.sin(theta_rep) 111 | 112 | F_obs = [F_obsx, F_obsy] 113 | 114 | return F_obs 115 | 116 | def leavecar(self, front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy): 117 | F_vehx, F_vehy = 0, 0 118 | car_Le = self.L 119 | car_Be = 0.5 * self.B 120 | all_vehicles = self.vehicles + moving_vehicles 121 | 122 | for vehicle in all_vehicles: 123 | if vehicle[2] == 'h': 124 | car_L = abs(front_p[0] - vehicle[0]) 125 | car_B = abs(front_p[1] - vehicle[1]) 126 | else: 127 | car_L = abs(front_p[1] - vehicle[1]) 128 | car_B = abs(front_p[0] - vehicle[0]) 129 | 130 | if car_L > self.car_Lmin or car_B > self.car_Bmin: 131 | continue 132 | elif car_L == 0 and car_B == 0: 133 | print("APF is failed, because the vehicle vehicle crashes a car.") 134 | exit() 135 | elif car_L <= car_Le and car_B <= car_Be: 136 | print("The vehicle meets the local minimum.") 137 | Fe_vehx, Fe_vehy = emergency.vehicle_force(vehicle, use_sollinesx, use_sollinesy) 138 | if vehicle[2] == 'h': 139 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 140 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 141 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehx 142 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 143 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - 144 | (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) + Fe_vehy 145 | else: 146 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 147 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 148 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehx 149 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 150 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - 151 | (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) + Fe_vehy 152 | else: 153 | if vehicle[2] == 'h': 154 | F_vehx += (front_p[0] - vehicle[0]) / 3 / self.L * self.k_car * math.exp( 155 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 156 | F_vehy += (front_p[1] - vehicle[1]) / self.B * self.k_car * math.exp( 157 | -(front_p[0] - vehicle[0]) ** 2 / 6 / self.L - (front_p[1] - vehicle[1]) ** 2 / 2 / self.B) 158 | else: 159 | F_vehx += (front_p[0] - vehicle[0]) / self.B * self.k_car * math.exp( 160 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 161 | F_vehy += (front_p[1] - vehicle[1]) / 3 / self.L * self.k_car * math.exp( 162 | -(front_p[0] - vehicle[0]) ** 2 / 2 / self.B - (front_p[1] - vehicle[1]) ** 2 / 6 / self.L) 163 | 164 | F_veh = [F_vehx, F_vehy] 165 | 166 | return F_veh 167 | 168 | def pathplanning(self, rear_p, front_p, fi, target, iters, moving_vehicles): 169 | supply = SUPPLY(self.k_replane, self.k_lane, self.lw, self.h_sollines, self.v_sollines, self.t_lines) 170 | emergency = EMERGENCY(self.F_e, self.h_sollines, self.v_sollines, self.lw) 171 | 172 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 173 | 174 | while distance > self.target_area and iters <= self.max_iters: 175 | iters += 1 176 | print("\n----------\nNo.", iters) 177 | 178 | F_att = self.attraction(front_p, target) 179 | self.F_atts.append(F_att) 180 | F_lane, use_sollinesx, use_sollinesy = self.keeplane(front_p, supply) 181 | self.F_lanes.append(F_lane) 182 | F_obs = self.repulsion(front_p, emergency, use_sollinesx, use_sollinesy) 183 | self.F_obss.append(F_obs) 184 | F_veh = self.leavecar(front_p, moving_vehicles, emergency, use_sollinesx, use_sollinesy) 185 | self.F_vehs.append(F_veh) 186 | 187 | F_total = [F_att[0] + F_lane[0] + F_obs[0] + F_veh[0], F_att[1] + F_lane[1] + F_obs[1] + F_veh[1]] 188 | self.F_totals.append(F_total) 189 | F_direction = math.atan2(F_total[1], F_total[0]) 190 | print("total force:", F_total) 191 | print("force direction:", int(F_direction / math.pi * 180)) 192 | 193 | if F_direction - fi > 40 / 180 * math.pi: 194 | si = 40 / 180 * math.pi 195 | elif F_direction - fi < -40 / 180 * math.pi: 196 | si = -40 / 180 * math.pi 197 | else: 198 | si = F_direction - fi 199 | print("steering angle:", int(si / math.pi * 180)) 200 | 201 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 202 | print("rear velocity:", rear_v, ", rear position:", rear_p) 203 | # print("front velocity:", front_v, ", front position:", front_p) 204 | # print("vehicle direction:", int(fi / math.pi * 180)) 205 | 206 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 207 | print("difference between formula and subtract:", dif_distance - dif_distance2) 208 | print("distance difference:", dif_distance) 209 | 210 | self.car.draw_car(rear_p, front_p) 211 | moving_vehicles, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 212 | # plt.pause(0.1) 213 | p1.remove() 214 | p2.remove() 215 | p3.remove() 216 | 217 | self.rear_velocities.append(self.car.v) 218 | self.path.append([rear_p[0], rear_p[1]]) 219 | self.direction.append(fi) 220 | self.front_velocities.append(vf) 221 | self.front_points.append([front_p[0], front_p[1]]) 222 | self.steering.append(si) 223 | 224 | distance = math.sqrt((target[0] - rear_p[0]) ** 2 + (target[1] - rear_p[1]) ** 2) 225 | 226 | if distance <= self.target_area and iters <= self.max_iters: 227 | print("\n----------\nAPF is successful!\n----------") 228 | else: 229 | print("\n----------\nAPF is failed!\n----------") 230 | 231 | return rear_p, front_p, fi, iters, moving_vehicles 232 | -------------------------------------------------------------------------------- /singleAPF/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, del_block): 9 | self.length = length 10 | self.width = width 11 | self.del_block = del_block 12 | self.no_block_x = int(length / del_block) 13 | self.no_block_y = int(width / del_block) 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | else: 37 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 38 | 39 | for dotted_line in dotted_lines: 40 | if dotted_line[3] == 'h': 41 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 42 | linewidth=2) 43 | else: 44 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 45 | linewidth=2) 46 | 47 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 48 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 49 | fig.add_patch(arc1) 50 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 51 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 52 | fig.add_patch(arc2) 53 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 54 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 55 | fig.add_patch(arc3) 56 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 57 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 58 | fig.add_patch(arc4) 59 | 60 | return 61 | 62 | def create_lanes(self, fig): 63 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 64 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 65 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 66 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 67 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 68 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 69 | 70 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 71 | 72 | return solid_lines, dotted_lines, transition_lines 73 | 74 | def draw_cycle(self, fig, obstacles): 75 | for obstacle in obstacles: 76 | if obstacle[3] == 2: 77 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 78 | else: 79 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 80 | fig.add_patch(cir) 81 | 82 | return 83 | 84 | def get_obstacles(self, fig): 85 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 86 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 87 | self.draw_cycle(fig, obstacles) 88 | 89 | return obstacles 90 | 91 | def draw_vehicle(self, fig, vehicles, L, B): 92 | for vehicle in vehicles: 93 | if vehicle[2] == 'h': 94 | x1 = vehicle[0] - L / 2 95 | y1 = vehicle[1] - B / 2 96 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='blue') 97 | fig.add_patch(rec) 98 | else: 99 | x1 = vehicle[0] - B / 2 100 | y1 = vehicle[1] - L / 2 101 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='blue') 102 | fig.add_patch(rec) 103 | 104 | return 105 | 106 | def get_vehicles(self, fig, L, B): 107 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 108 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 109 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 110 | 111 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 112 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 113 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 114 | 115 | self.draw_vehicle(fig, vehicles, L, B) 116 | 117 | return vehicles, init_movingvehis, moving_vehicles 118 | 119 | def get_startandtarget(self): 120 | # start = [5, 2] 121 | # target = [63, 60] 122 | start = [5, 2] 123 | target = [78, 78] 124 | 125 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 126 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 127 | 128 | return start, target 129 | 130 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 131 | front_x, front_y, path_x, path_y = [], [], [], [] 132 | for i in front_points: 133 | front_x.append(i[0]) 134 | front_y.append(i[1]) 135 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 136 | for j in path: 137 | path_x.append(j[0]) 138 | path_y.append(j[1]) 139 | # plt.plot(front_x, front_y, c='green', linewidth=2) 140 | plt.plot(path_x, path_y, c='r', linewidth=2) 141 | 142 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 143 | if v1[2] == 'h': 144 | x1 = v1[0] - L / 2 145 | y1 = v1[1] - B / 2 146 | h = v2[0] + L / 2 - x1 147 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 148 | fig.add_patch(rec) 149 | else: 150 | x1 = v1[0] - B / 2 151 | y1 = v1[1] - L / 2 152 | h = v2[1] + L / 2 - y1 153 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 154 | fig.add_patch(rec) 155 | 156 | for vehicle in moving_vehicles: 157 | if vehicle[2] == 'h': 158 | x1 = vehicle[0] - L / 2 159 | y1 = vehicle[1] - B / 2 160 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 161 | fig.add_patch(rec) 162 | else: 163 | x1 = vehicle[0] - B / 2 164 | y1 = vehicle[1] - L / 2 165 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 166 | fig.add_patch(rec) 167 | 168 | # plt.pause(0.1) 169 | 170 | return 171 | -------------------------------------------------------------------------------- /singleAPF/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, p1, p2, = [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | return moving_vehicles, p1, p2, p3, 89 | -------------------------------------------------------------------------------- /singleAPF/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from car import * 6 | from apf import * 7 | 8 | 9 | # main 10 | if __name__ == "__main__": 11 | print("Creating a map:\n----------") 12 | length = 80 13 | width = 80 14 | del_block = 2 15 | 16 | map = MAP(length, width, del_block) 17 | fig = map.create_map() 18 | 19 | print("\nCreating lanes:\n----------") 20 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 21 | print("solid lines:", solid_lines) 22 | print("dotted lines:", dotted_lines) 23 | print("transition lines:", transition_lines) 24 | 25 | print("\nCreating obstacles:\n----------") 26 | obstacles = map.get_obstacles(fig) 27 | print("obstacles:", obstacles) 28 | 29 | print("\nCreating vehicles:\n----------") 30 | L = 4.8 31 | B = 1.8 32 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig, L, B) 33 | print("vehicles:", vehicles) 34 | print("moving vehicles:", moving_vehicles) 35 | 36 | print("\nSetting start and target:\n----------") 37 | start, target = map.get_startandtarget() 38 | print("start:", start, ", target:", target) 39 | 40 | # vehicle model 41 | print("\nEstablishing the vehicle model:\n----------") 42 | l = 3 43 | # b = 1.6 44 | velocity = 20 # changeable 45 | init_force_direction = 0 46 | init_fi = 0 # changeable 47 | init_si = init_force_direction - init_fi # -40°~40° 48 | init_position = [start[0], start[1], init_fi] 49 | 50 | car = CAR(L, B, l, velocity) 51 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 52 | print("initial steering angle:", int(init_si / math.pi * 180)) 53 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 54 | print("initial front velocity:", front_v, ", initial front position:", front_p) 55 | print("initial vehicle direction:", int(fi / math.pi * 180)) 56 | 57 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 58 | print("difference between formula and subtract:", dif_distance - dif_distance2) 59 | print("distance difference:", dif_distance) 60 | 61 | car.draw_car(rear_p, front_p) 62 | moving_vehicles, p1, p2, p3, = car.draw_movingcar(moving_vehicles) 63 | # plt.pause(0.1) 64 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 65 | p1.remove() 66 | p2.remove() 67 | p3.remove() 68 | 69 | # APF path planning algorithm 70 | t1 = time.time() 71 | 72 | print("\nAPF algorithm path planning:\n----------") 73 | k_att = 40 74 | k_rep = 10 75 | r_apf = 5 76 | k_replane = 5 77 | k_lane = 10 78 | k_car = 500 79 | lane_width = 3.5 80 | target_area = 3.5 81 | F_e = 50 82 | del_t = 0.05 # changeable 83 | max_iters = 105 84 | 85 | rear_velocities = [car.v] 86 | path = [[rear_p[0], rear_p[1]]] 87 | direction = [fi] 88 | front_velocities = [vf] 89 | front_points = [[front_p[0], front_p[1]]] 90 | steering = [init_si] 91 | 92 | apf = APF(k_att, k_rep, r_apf, k_replane, k_lane, k_car, lane_width, target_area, F_e, del_t, max_iters, 93 | solid_lines, transition_lines, obstacles, vehicles, L, B, car, rear_velocities, path, direction, 94 | front_velocities, front_points, steering) 95 | 96 | iters = 0 97 | rear_p, front_p, fi, iters, moving_vehicles = apf.pathplanning(rear_p, front_p, fi, target, iters, moving_vehicles) 98 | 99 | t2 = time.time() 100 | apf_time = t2 - t1 101 | 102 | print("total rear velocity:", apf.rear_velocities) 103 | print("total path:", apf.path) 104 | print("total direction:", apf.direction) 105 | print("total front velocity:", apf.front_velocities) 106 | print("total front position:", apf.front_points) 107 | print("total steering:", apf.steering) 108 | print("total iters:", iters) 109 | print("apf time:", apf_time) 110 | print("total distance difference:", car.dif_distances) 111 | 112 | print("attractive forces:", apf.F_atts) 113 | print("lane forces:", apf.F_lanes) 114 | print("obstacle forces:", apf.F_obss) 115 | print("vehicles forces:", apf.F_vehs) 116 | print("total forces:", apf.F_totals) 117 | 118 | map.final_draw(fig, apf.front_points, apf.path, init_movingvehis, moving_vehicles, L, B) 119 | plt.pause(0.1) 120 | 121 | plt.show() 122 | -------------------------------------------------------------------------------- /singleAstar (new)/__pycache__/assistant.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAstar (new)/__pycache__/assistant.cpython-37.pyc -------------------------------------------------------------------------------- /singleAstar (new)/__pycache__/astar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAstar (new)/__pycache__/astar.cpython-37.pyc -------------------------------------------------------------------------------- /singleAstar (new)/__pycache__/car.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MingyangLiu1995-s/A-hybrid-path-planning-algorithm-based-on-APF-and-Astar/734de6e43ee9ca145ef8d16b6077a1b4e13ccbbe/singleAstar (new)/__pycache__/car.cpython-37.pyc -------------------------------------------------------------------------------- /singleAstar (new)/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, lane_width, L, B): 9 | self.length = length 10 | self.width = width 11 | self.lw = lane_width 12 | self.L = L 13 | self.B = B 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | # xx, yy 37 | else: 38 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 39 | 40 | for dotted_line in dotted_lines: 41 | if dotted_line[3] == 'h': 42 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 43 | linewidth=2) 44 | else: 45 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 46 | linewidth=2) 47 | 48 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 49 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 50 | fig.add_patch(arc1) 51 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 52 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 53 | fig.add_patch(arc2) 54 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 55 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 56 | fig.add_patch(arc3) 57 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 58 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 59 | fig.add_patch(arc4) 60 | 61 | return 62 | 63 | def create_lanes(self, fig): 64 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 65 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 66 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 67 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 68 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 69 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 70 | 71 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 72 | 73 | return solid_lines, dotted_lines, transition_lines 74 | 75 | def create_lineareas(self, solid_lines): 76 | line_areas = [] 77 | 78 | for solid_line in solid_lines: 79 | if solid_line[3] == 'h': 80 | line_areas.append([solid_line[0] - 2.5, solid_line[2] - self.lw / 3, 81 | solid_line[1] + 2.5, solid_line[2] + self.lw / 3]) 82 | else: 83 | line_areas.append([solid_line[0] - self.lw / 3, solid_line[1] - 2.5, 84 | solid_line[0] + self.lw / 3, solid_line[2] + 2.5]) 85 | 86 | return line_areas 87 | 88 | def draw_cycle(self, fig, obstacles): 89 | for obstacle in obstacles: 90 | if obstacle[3] == 2: 91 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 92 | else: 93 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 94 | fig.add_patch(cir) 95 | 96 | return 97 | 98 | def get_obstacles(self, fig): 99 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 100 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 101 | self.draw_cycle(fig, obstacles) 102 | 103 | return obstacles 104 | 105 | def get_obsareas(self, obstacles): 106 | obstacle_areas = [] 107 | 108 | for obstacle in obstacles: 109 | obs_Le = 1.1 * obstacle[3] * obstacle[2] 110 | obs_Be = 0.7 * obstacle[3] * self.B 111 | if obstacle[4] == 'h': 112 | obstacle_areas.append([obstacle[0] - obs_Le, obstacle[1] - obs_Be, 113 | obstacle[0] + obs_Le, obstacle[1] + obs_Be]) 114 | else: 115 | obstacle_areas.append([obstacle[0] - obs_Be, obstacle[1] - obs_Le, 116 | obstacle[0] + obs_Be, obstacle[1] + obs_Le]) 117 | 118 | return obstacle_areas 119 | 120 | def draw_vehicle(self, fig, vehicles): 121 | for vehicle in vehicles: 122 | if vehicle[2] == 'h': 123 | x1 = vehicle[0] - self.L / 2 124 | y1 = vehicle[1] - self.B / 2 125 | rec = Rectangle(xy=(x1, y1), width=self.L, height=self.B, color='blue') 126 | fig.add_patch(rec) 127 | else: 128 | x1 = vehicle[0] - self.B / 2 129 | y1 = vehicle[1] - self.L / 2 130 | rec = Rectangle(xy=(x1, y1), width=self.B, height=self.L, color='blue') 131 | fig.add_patch(rec) 132 | 133 | return 134 | 135 | def get_vehicles(self, fig): 136 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 137 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 138 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 139 | 140 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 141 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 142 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 143 | 144 | self.draw_vehicle(fig, vehicles) 145 | 146 | return vehicles, init_movingvehis, moving_vehicles 147 | 148 | def get_vehareas(self, vehicles, moving_vehicles): 149 | vehicle_areas, moving_areas = [], [] 150 | car_Le = 1.2 * self.L 151 | car_Be = 1.2 * self.B 152 | 153 | for vehicle in vehicles: 154 | if vehicle[2] == 'h': 155 | vehicle_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 156 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 157 | else: 158 | vehicle_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 159 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 160 | 161 | for vehicle in moving_vehicles: 162 | if vehicle[2] == 'h': 163 | moving_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 164 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 165 | else: 166 | moving_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 167 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 168 | 169 | return vehicle_areas, moving_areas 170 | 171 | def create_areas(self): 172 | infeasible_areas = [[0, 14, 50, 73], [64, 14, 80, 73]] 173 | 174 | return infeasible_areas 175 | 176 | def get_startandtarget(self): 177 | # start = [5, 2] 178 | # target = [63, 60] 179 | start = [5, 2] 180 | target = [78, 78] 181 | 182 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 183 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 184 | 185 | return start, target 186 | 187 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 188 | front_x, front_y, path_x, path_y = [], [], [], [] 189 | for i in front_points: 190 | front_x.append(i[0]) 191 | front_y.append(i[1]) 192 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 193 | for j in path: 194 | path_x.append(j[0]) 195 | path_y.append(j[1]) 196 | # plt.plot(front_x, front_y, c='green', linewidth=2) 197 | plt.plot(path_x, path_y, c='r', linewidth=2) 198 | 199 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 200 | if v1[2] == 'h': 201 | x1 = v1[0] - L / 2 202 | y1 = v1[1] - B / 2 203 | h = v2[0] + L / 2 - x1 204 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 205 | fig.add_patch(rec) 206 | else: 207 | x1 = v1[0] - B / 2 208 | y1 = v1[1] - L / 2 209 | h = v2[1] + L / 2 - y1 210 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 211 | fig.add_patch(rec) 212 | 213 | for vehicle in moving_vehicles: 214 | if vehicle[2] == 'h': 215 | x1 = vehicle[0] - L / 2 216 | y1 = vehicle[1] - B / 2 217 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 218 | fig.add_patch(rec) 219 | else: 220 | x1 = vehicle[0] - B / 2 221 | y1 = vehicle[1] - L / 2 222 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 223 | fig.add_patch(rec) 224 | 225 | # plt.pause(0.1) 226 | 227 | return 228 | -------------------------------------------------------------------------------- /singleAstar (new)/astar.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | 5 | 6 | class ASTAR: 7 | 8 | class NODE: 9 | 10 | def __init__(self, current_node, father_node, target, cost=0): 11 | self.current_node = current_node 12 | self.father_node = father_node 13 | self.target = target 14 | self.direction = math.atan2(self.current_node[1] - self.father_node[1], 15 | self.current_node[0] - self.father_node[0]) 16 | self.g = cost 17 | self.h = abs(self.target[0] - self.current_node[0]) + abs(self.target[1] - self.current_node[1]) 18 | self.f = self.g + self.h 19 | 20 | def __init__(self, del_block, target, target_area, del_t, max_iters, line_areas, obstacle_areas, vehicle_areas, 21 | infeasible_areas, car, rear_velocities, path, direction, front_velocities, front_points, steering): 22 | self.del_block = del_block 23 | self.target = target 24 | self.target_area = target_area 25 | self.del_t = del_t 26 | self.max_iters = max_iters 27 | self.line_areas = line_areas 28 | self.obstacle_areas = obstacle_areas 29 | self.vehicle_areas = vehicle_areas 30 | self.infeasible_areas = infeasible_areas 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | 39 | def possible_moves(self, current_node, moving_areas): 40 | next_nodes, next_costs = [], [] 41 | 42 | possible_nodes = [[current_node[0] + self.del_block, current_node[1]], 43 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 44 | [current_node[0], current_node[1] + self.del_block], 45 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 46 | [current_node[0] - self.del_block, current_node[1]], 47 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 48 | [current_node[0], current_node[1] - self.del_block], 49 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 50 | possiblenodes2 = [[current_node[0] + self.del_block, current_node[1]], 51 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 52 | [current_node[0], current_node[1] + self.del_block], 53 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 54 | [current_node[0] - self.del_block, current_node[1]], 55 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 56 | [current_node[0], current_node[1] - self.del_block], 57 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 58 | 59 | possible_costs = [self.del_block, math.hypot(self.del_block, self.del_block), 60 | self.del_block, math.hypot(self.del_block, self.del_block), 61 | self.del_block, math.hypot(self.del_block, self.del_block), 62 | self.del_block, math.hypot(self.del_block, self.del_block)] 63 | possiblecosts2 = [self.del_block, math.hypot(self.del_block, self.del_block), 64 | self.del_block, math.hypot(self.del_block, self.del_block), 65 | self.del_block, math.hypot(self.del_block, self.del_block), 66 | self.del_block, math.hypot(self.del_block, self.del_block)] 67 | 68 | areas = self.line_areas + self.obstacle_areas + self.infeasible_areas + self.vehicle_areas + moving_areas 69 | 70 | for area in areas: 71 | for i in range(len(possiblenodes2)): 72 | if area[0] <= possiblenodes2[i][0] <= area[2] and area[1] <= possiblenodes2[i][1] <= area[3]: 73 | if possiblenodes2[i] in possible_nodes: 74 | possible_nodes.remove(possiblenodes2[i]) 75 | possible_costs.remove(possiblecosts2[i]) 76 | 77 | for i in range(len(possible_nodes)): 78 | next_nodes.append(possible_nodes[i]) 79 | next_costs.append(possible_costs[i]) 80 | 81 | return next_nodes, next_costs 82 | 83 | def get_minf(self, best, next): 84 | if next.f < best.f: 85 | best = next 86 | 87 | return best 88 | 89 | def pathplanning(self, rear_p, front_p, fi, iters, moving_vehicles, moving_areas): 90 | father_node = [front_p[0], front_p[1]] 91 | current_node = [front_p[0], front_p[1]] 92 | current = ASTAR.NODE(current_node, father_node, self.target) 93 | 94 | distance = math.sqrt((self.target[0] - rear_p[0]) ** 2 + (self.target[1] - rear_p[1]) ** 2) 95 | 96 | while distance > self.target_area and iters <= self.max_iters: 97 | iters += 1 98 | print("\n----------\nNo.", iters) 99 | print("current node:", current.current_node, ", previous angle:", current.direction, ", current g:", 100 | current.g, ", h:", current.h, ", f:", current.f) 101 | 102 | next_nodes, next_costs = self.possible_moves(current.current_node, moving_areas) 103 | 104 | next_set = [] 105 | for next_node, next_cost in zip(next_nodes, next_costs): 106 | next = ASTAR.NODE(next_node, current.current_node, self.target, next_cost + current.g) 107 | next_set.append(next) 108 | 109 | current.f = float("inf") 110 | best = current 111 | for next in next_set: 112 | best = self.get_minf(best, next) 113 | 114 | if best == current: 115 | print("\n----------\nA* is failed!\n----------") 116 | exit() 117 | 118 | current_node = best.current_node 119 | father_node = best.father_node 120 | current_cost = best.g 121 | current = ASTAR.NODE(current_node, father_node, self.target, current_cost) 122 | si = current.direction - fi 123 | print("steering angle:", int(si / math.pi * 180)) 124 | 125 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 126 | print("rear velocity:", rear_v, ", rear position:", rear_p) 127 | # print("front velocity:", front_v, ", front position:", front_p) 128 | # print("vehicle direction:", int(fi / math.pi * 180)) 129 | 130 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 131 | print("difference between formula and subtract:", dif_distance - dif_distance2) 132 | print("distance difference:", dif_distance) 133 | 134 | self.car.draw_car(rear_p, front_p) 135 | moving_vehicles, moving_areas, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 136 | # plt.pause(self.del_t) 137 | p1.remove() 138 | p2.remove() 139 | p3.remove() 140 | 141 | self.rear_velocities.append(self.car.v) 142 | self.path.append([rear_p[0], rear_p[1]]) 143 | self.direction.append(fi) 144 | self.front_velocities.append(vf) 145 | self.front_points.append([front_p[0], front_p[1]]) 146 | self.steering.append(si) 147 | 148 | father_node = current.father_node 149 | current_node = [front_p[0], front_p[1]] 150 | current = ASTAR.NODE(current_node, father_node, self.target, current_cost) 151 | 152 | distance = math.sqrt((self.target[0] - rear_p[0]) ** 2 + (self.target[1] - rear_p[1]) ** 2) 153 | 154 | if distance <= self.target_area and iters <= self.max_iters: 155 | print("\n----------\nA* is successful!\n----------") 156 | else: 157 | print("\n----------\nA* is failed!\n----------") 158 | 159 | return rear_p, front_p, fi, iters, moving_vehicles 160 | 161 | -------------------------------------------------------------------------------- /singleAstar (new)/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, moving_areas, p1, p2, p3, = [], [], [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | ''' 89 | car_Le = 1.2 * self.L 90 | car_Be = 1.2 * self.B 91 | for vehicle in moving_vehicles: 92 | if vehicle[2] == 'h': 93 | moving_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 94 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 95 | else: 96 | moving_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 97 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 98 | ''' 99 | 100 | return moving_vehicles, moving_areas, p1, p2, p3, 101 | -------------------------------------------------------------------------------- /singleAstar (new)/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from astar import * 6 | from car import * 7 | 8 | 9 | # main 10 | if __name__ == "__main__": 11 | print("Creating a map:\n----------") 12 | length = 80 13 | width = 80 14 | lane_width = 3.5 15 | L = 4.8 16 | B = 1.8 17 | 18 | map = MAP(length, width, lane_width, L, B) 19 | fig = map.create_map() 20 | 21 | print("\nCreating lanes:\n----------") 22 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 23 | print("solid lines:", solid_lines) 24 | print("dotted lines:", dotted_lines) 25 | print("transition lines:", transition_lines) 26 | line_areas = map.create_lineareas(solid_lines) 27 | print("infeasible line areas:", line_areas) 28 | 29 | print("\nCreating obstacles:\n----------") 30 | obstacles = map.get_obstacles(fig) 31 | print("obstacles:", obstacles) 32 | obstacle_areas = map.get_obsareas(obstacles) 33 | print("infeasible obstacle areas:", obstacle_areas) 34 | 35 | print("\nCreating vehicles:\n----------") 36 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig) 37 | print("vehicles:", vehicles) 38 | print("moving vehicles:", moving_vehicles) 39 | vehicle_areas, moving_areas = map.get_vehareas(vehicles, moving_vehicles) 40 | print("infeasible vehicle areas:", vehicle_areas) 41 | print("infeasible moving vehicle areas:", moving_areas) 42 | 43 | print("\nCreating infeasible areas:\n----------") 44 | infeasible_areas = map.create_areas() 45 | print("infeasible areas:", infeasible_areas) 46 | 47 | print("\nSetting start and target:\n----------") 48 | start, target = map.get_startandtarget() 49 | print("start:", start, ", target:", target) 50 | 51 | # vehicle model 52 | print("\nEstablishing the vehicle model:\n----------") 53 | l = 3 54 | # b = 1.6 55 | velocity = 20 # changeable 56 | init_force_direction = 0 57 | init_fi = 0 # changeable 58 | init_si = init_force_direction - init_fi # random 59 | init_position = [start[0], start[1], init_fi] 60 | 61 | car = CAR(L, B, l, velocity) 62 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 63 | print("initial steering angle:", int(init_si / math.pi * 180)) 64 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 65 | print("initial front velocity:", front_v, ", initial front position:", front_p) 66 | print("initial vehicle direction:", int(fi / math.pi * 180)) 67 | 68 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 69 | print("difference between formula and subtract:", dif_distance - dif_distance2) 70 | print("distance difference:", dif_distance) 71 | 72 | car.draw_car(rear_p, front_p) 73 | moving_vehicles, moving_areas, p1, p2, p3, = car.draw_movingcar(moving_vehicles) 74 | # plt.pause(0.1) 75 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 76 | p1.remove() 77 | p2.remove() 78 | p3.remove() 79 | 80 | # A* path planning algorithm 81 | t1 = time.time() 82 | 83 | print("\nA* algorithm path planning:\n----------") 84 | target_area = 3.5 85 | del_t = 0.05 # changeable 86 | del_block = velocity * del_t 87 | max_iters = 1000 88 | 89 | rear_velocities = [car.v] 90 | path = [[rear_p[0], rear_p[1]]] 91 | direction = [fi] 92 | front_velocities = [vf] 93 | front_points = [[front_p[0], front_p[1]]] 94 | steering = [init_si] 95 | 96 | astar = ASTAR(del_block, target, target_area, del_t, max_iters, line_areas, obstacle_areas, vehicle_areas, 97 | infeasible_areas, car, rear_velocities, path, direction, front_velocities, front_points, steering) 98 | 99 | iters = 0 100 | rear_p, front_p, fi, iters, moving_vehicles = astar.pathplanning(rear_p, front_p, fi, iters, moving_vehicles, 101 | moving_areas) 102 | 103 | t2 = time.time() 104 | astar_time = t2 - t1 105 | 106 | print("total rear velocity:", astar.rear_velocities) 107 | print("total path:", astar.path) 108 | print("total direction:", astar.direction) 109 | print("total front velocity:", astar.front_velocities) 110 | print("total front position:", astar.front_points) 111 | print("total steering:", astar.steering) 112 | print("total iters:", iters) 113 | print("astar time:", astar_time) 114 | print("total distance difference:", car.dif_distances) 115 | 116 | map.final_draw(fig, astar.front_points, astar.path, init_movingvehis, moving_vehicles, L, B) 117 | plt.pause(0.1) 118 | 119 | plt.show() 120 | -------------------------------------------------------------------------------- /singleAstar/assistant.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from matplotlib.pyplot import MultipleLocator 3 | from matplotlib.patches import Arc, Circle, Rectangle 4 | 5 | 6 | class MAP: 7 | 8 | def __init__(self, length, width, lane_width, L, B): 9 | self.length = length 10 | self.width = width 11 | self.lw = lane_width 12 | self.L = L 13 | self.B = B 14 | 15 | def create_map(self): 16 | subplot = plt.figure(figsize=(8, 8)) 17 | fig = subplot.add_subplot(111) 18 | fig.set_xlabel('X-distance (m)', size=15) 19 | fig.set_ylabel('Y-distance (m)', size=15) 20 | fig.xaxis.set_ticks_position('bottom') 21 | fig.yaxis.set_ticks_position('left') 22 | plt.tick_params(labelsize=15) 23 | x_major = MultipleLocator(10) 24 | y_major = MultipleLocator(10) 25 | fig.xaxis.set_major_locator(x_major) 26 | fig.yaxis.set_major_locator(y_major) 27 | plt.xlim(0, self.length) 28 | plt.ylim(0, self.width) 29 | 30 | return fig 31 | 32 | def draw_lines(self, fig, solid_lines, dotted_lines, transition_lines): 33 | for solid_line in solid_lines: 34 | if solid_line[3] == 'h': 35 | plt.plot([solid_line[0], solid_line[1]], [solid_line[2], solid_line[2]], c='black', linewidth=2) 36 | # xx, yy 37 | else: 38 | plt.plot([solid_line[0], solid_line[0]], [solid_line[1], solid_line[2]], c='black', linewidth=2) 39 | 40 | for dotted_line in dotted_lines: 41 | if dotted_line[3] == 'h': 42 | plt.plot([dotted_line[0], dotted_line[1]], [dotted_line[2], dotted_line[2]], c='gray', linestyle='--', 43 | linewidth=2) 44 | else: 45 | plt.plot([dotted_line[0], dotted_line[0]], [dotted_line[1], dotted_line[2]], c='gray', linestyle='--', 46 | linewidth=2) 47 | 48 | arc1 = Arc(xy=(transition_lines[0][0], transition_lines[0][1]), width=2*transition_lines[0][2], 49 | height=2*transition_lines[0][2], angle=90, theta1=-180, theta2=-90, color='black', linewidth=2) 50 | fig.add_patch(arc1) 51 | arc2 = Arc(xy=(transition_lines[1][0], transition_lines[1][1]), width=2*transition_lines[1][2], 52 | height=2*transition_lines[1][2], angle=90, theta1=-90, theta2=0, color='black', linewidth=2) 53 | fig.add_patch(arc2) 54 | arc3 = Arc(xy=(transition_lines[2][0], transition_lines[2][1]), width=2*transition_lines[2][2], 55 | height=2*transition_lines[2][2], angle=90, theta1=90, theta2=180, color='black', linewidth=2) 56 | fig.add_patch(arc3) 57 | arc4 = Arc(xy=(transition_lines[3][0], transition_lines[3][1]), width=2*transition_lines[3][2], 58 | height=2*transition_lines[3][2], angle=90, theta1=0, theta2=90, color='black', linewidth=2) 59 | fig.add_patch(arc4) 60 | 61 | return 62 | 63 | def create_lanes(self, fig): 64 | solid_lines = [[0, 80, 0, 'h'], [0, 47.5, 14, 'h'], [66.5, 80, 14, 'h'], [50, 16.5, 70.5, 'v'], 65 | [64, 16.5, 70.5, 'v'], [0, 47.5, 73, 'h'], [66.5, 80, 73, 'h'], [0, 80, 80, 'h']] 66 | dotted_lines = [[0, 50, 3.5, 'h'], [0, 50, 7, 'h'], [0, 50, 10.5, 'h'], [64, 80, 3.5, 'h'], 67 | [64, 80, 7, 'h'], [64, 80, 10.5, 'h'], [53.5, 14, 73, 'v'], [57, 14, 73, 'v'], 68 | [60.5, 14, 73, 'v'], [0, 50, 76.5, 'h'], [64, 80, 76.5, 'h']] 69 | transition_lines = [[47.5, 16.5, 2.5], [47.5, 70.5, 2.5], [66.5, 16.5, 2.5], [66.5, 70.5, 2.5]] 70 | 71 | self.draw_lines(fig, solid_lines, dotted_lines, transition_lines) 72 | 73 | return solid_lines, dotted_lines, transition_lines 74 | 75 | def create_lineareas(self, solid_lines): 76 | line_areas = [] 77 | 78 | for solid_line in solid_lines: 79 | if solid_line[3] == 'h': 80 | line_areas.append([solid_line[0] - 2.5, solid_line[2] - self.lw / 3, 81 | solid_line[1] + 2.5, solid_line[2] + self.lw / 3]) 82 | else: 83 | line_areas.append([solid_line[0] - self.lw / 3, solid_line[1] - 2.5, 84 | solid_line[0] + self.lw / 3, solid_line[2] + 2.5]) 85 | 86 | return line_areas 87 | 88 | def draw_cycle(self, fig, obstacles): 89 | for obstacle in obstacles: 90 | if obstacle[3] == 2: 91 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='yellow') 92 | else: 93 | cir = Circle(xy=(obstacle[0], obstacle[1]), radius=obstacle[2], facecolor='red') 94 | fig.add_patch(cir) 95 | 96 | return 97 | 98 | def get_obstacles(self, fig): 99 | # obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v'], [64, 35, 1.5, 2, 'v']] 100 | obstacles = [[17, 1, 1.5, 2, 'h'], [30, 4, 2, 3, 'h'], [50.5, 28, 2, 2, 'v']] 101 | self.draw_cycle(fig, obstacles) 102 | 103 | return obstacles 104 | 105 | def get_obsareas(self, obstacles): 106 | obstacle_areas = [] 107 | 108 | for obstacle in obstacles: 109 | obs_Le = 1.1 * obstacle[3] * obstacle[2] 110 | obs_Be = 0.7 * obstacle[3] * self.B 111 | if obstacle[4] == 'h': 112 | obstacle_areas.append([obstacle[0] - obs_Le, obstacle[1] - obs_Be, 113 | obstacle[0] + obs_Le, obstacle[1] + obs_Be]) 114 | else: 115 | obstacle_areas.append([obstacle[0] - obs_Be, obstacle[1] - obs_Le, 116 | obstacle[0] + obs_Be, obstacle[1] + obs_Le]) 117 | 118 | return obstacle_areas 119 | 120 | def draw_vehicle(self, fig, vehicles): 121 | for vehicle in vehicles: 122 | if vehicle[2] == 'h': 123 | x1 = vehicle[0] - self.L / 2 124 | y1 = vehicle[1] - self.B / 2 125 | rec = Rectangle(xy=(x1, y1), width=self.L, height=self.B, color='blue') 126 | fig.add_patch(rec) 127 | else: 128 | x1 = vehicle[0] - self.B / 2 129 | y1 = vehicle[1] - self.L / 2 130 | rec = Rectangle(xy=(x1, y1), width=self.B, height=self.L, color='blue') 131 | fig.add_patch(rec) 132 | 133 | return 134 | 135 | def get_vehicles(self, fig): 136 | # vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h'], [55, 40, 'v'], [58.5, 50, 'v'], [18, 12, 'h']] 137 | # init_movingvehis = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 138 | # moving_vehicles = [[10, 85, 'h', 3], [10, 88, 'h', 3], [10, 90, 'h', 3]] 139 | 140 | vehicles = [[45, 8.5, 'h'], [5, 5.5, 'h']] 141 | init_movingvehis = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 142 | moving_vehicles = [[55, 55, 'v', -3], [58.5, 50, 'v', 3], [15, 12, 'h', 3]] 143 | 144 | self.draw_vehicle(fig, vehicles) 145 | 146 | return vehicles, init_movingvehis, moving_vehicles 147 | 148 | def get_vehareas(self, vehicles, moving_vehicles): 149 | vehicle_areas, moving_areas = [], [] 150 | car_Le = 1.2 * self.L 151 | car_Be = 1.2 * self.B 152 | 153 | for vehicle in vehicles: 154 | if vehicle[2] == 'h': 155 | vehicle_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 156 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 157 | else: 158 | vehicle_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 159 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 160 | 161 | for vehicle in moving_vehicles: 162 | if vehicle[2] == 'h': 163 | moving_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 164 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 165 | else: 166 | moving_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 167 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 168 | 169 | return vehicle_areas, moving_areas 170 | 171 | def create_areas(self): 172 | infeasible_areas = [[0, 14, 50, 73], [64, 14, 80, 73]] 173 | 174 | return infeasible_areas 175 | 176 | def get_startandtarget(self): 177 | # start = [5, 2] 178 | # target = [63, 60] 179 | start = [5, 2] 180 | target = [78, 78] 181 | 182 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) 183 | plt.plot(target[0], target[1], 'o', color='purple', markersize=10) 184 | 185 | return start, target 186 | 187 | def final_draw(self, fig, front_points, path, init_movingvehis, moving_vehicles, L, B): 188 | front_x, front_y, path_x, path_y = [], [], [], [] 189 | for i in front_points: 190 | front_x.append(i[0]) 191 | front_y.append(i[1]) 192 | plt.plot(front_x, front_y, 'o', c='green', markersize=2) 193 | for j in path: 194 | path_x.append(j[0]) 195 | path_y.append(j[1]) 196 | # plt.plot(front_x, front_y, c='green', linewidth=2) 197 | plt.plot(path_x, path_y, c='r', linewidth=2) 198 | 199 | for v1, v2 in zip(init_movingvehis, moving_vehicles): 200 | if v1[2] == 'h': 201 | x1 = v1[0] - L / 2 202 | y1 = v1[1] - B / 2 203 | h = v2[0] + L / 2 - x1 204 | rec = Rectangle(xy=(x1, y1), width=h, height=B, color='green', alpha=0.3) 205 | fig.add_patch(rec) 206 | else: 207 | x1 = v1[0] - B / 2 208 | y1 = v1[1] - L / 2 209 | h = v2[1] + L / 2 - y1 210 | rec = Rectangle(xy=(x1, y1), width=B, height=h, color='green', alpha=0.3) 211 | fig.add_patch(rec) 212 | 213 | for vehicle in moving_vehicles: 214 | if vehicle[2] == 'h': 215 | x1 = vehicle[0] - L / 2 216 | y1 = vehicle[1] - B / 2 217 | rec = Rectangle(xy=(x1, y1), width=L, height=B, color='green') 218 | fig.add_patch(rec) 219 | else: 220 | x1 = vehicle[0] - B / 2 221 | y1 = vehicle[1] - L / 2 222 | rec = Rectangle(xy=(x1, y1), width=B, height=L, color='green') 223 | fig.add_patch(rec) 224 | 225 | # plt.pause(0.1) 226 | 227 | return 228 | -------------------------------------------------------------------------------- /singleAstar/astar.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | from car import * 4 | 5 | 6 | class ASTAR: 7 | 8 | class NODE: 9 | 10 | def __init__(self, current_node, father_node, target, cost=0): 11 | self.current_node = current_node 12 | self.father_node = father_node 13 | self.target = target 14 | self.direction = math.atan2(self.current_node[1] - self.father_node[1], 15 | self.current_node[0] - self.father_node[0]) 16 | self.g = cost 17 | self.h = abs(self.target[0] - self.current_node[0]) + abs(self.target[1] - self.current_node[1]) 18 | self.f = self.g + self.h 19 | 20 | def __init__(self, del_block, target, target_area, del_t, max_iters, line_areas, obstacle_areas, vehicle_areas, 21 | infeasible_areas, car, rear_velocities, path, direction, front_velocities, front_points, steering): 22 | self.del_block = del_block 23 | self.target = target 24 | self.target_area = target_area 25 | self.del_t = del_t 26 | self.max_iters = max_iters 27 | self.line_areas = line_areas 28 | self.obstacle_areas = obstacle_areas 29 | self.vehicle_areas = vehicle_areas 30 | self.infeasible_areas = infeasible_areas 31 | self.car = car 32 | self.rear_velocities = rear_velocities 33 | self.path = path 34 | self.direction = direction 35 | self.front_velocities = front_velocities 36 | self.front_points = front_points 37 | self.steering = steering 38 | 39 | def possible_moves(self, current_node, moving_areas): 40 | next_nodes, next_costs = [], [] 41 | 42 | possible_nodes = [[current_node[0] + self.del_block, current_node[1]], 43 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 44 | [current_node[0], current_node[1] + self.del_block], 45 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 46 | [current_node[0] - self.del_block, current_node[1]], 47 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 48 | [current_node[0], current_node[1] - self.del_block], 49 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 50 | possiblenodes2 = [[current_node[0] + self.del_block, current_node[1]], 51 | [current_node[0] + self.del_block, current_node[1] + self.del_block], 52 | [current_node[0], current_node[1] + self.del_block], 53 | [current_node[0] - self.del_block, current_node[1] + self.del_block], 54 | [current_node[0] - self.del_block, current_node[1]], 55 | [current_node[0] - self.del_block, current_node[1] - self.del_block], 56 | [current_node[0], current_node[1] - self.del_block], 57 | [current_node[0] + self.del_block, current_node[1] - self.del_block]] 58 | 59 | possible_costs = [self.del_block, math.hypot(self.del_block, self.del_block), 60 | self.del_block, math.hypot(self.del_block, self.del_block), 61 | self.del_block, math.hypot(self.del_block, self.del_block), 62 | self.del_block, math.hypot(self.del_block, self.del_block)] 63 | possiblecosts2 = [self.del_block, math.hypot(self.del_block, self.del_block), 64 | self.del_block, math.hypot(self.del_block, self.del_block), 65 | self.del_block, math.hypot(self.del_block, self.del_block), 66 | self.del_block, math.hypot(self.del_block, self.del_block)] 67 | 68 | areas = self.line_areas + self.obstacle_areas + self.infeasible_areas + self.vehicle_areas + moving_areas 69 | 70 | for area in areas: 71 | for i in range(len(possiblenodes2)): 72 | if area[0] <= possiblenodes2[i][0] <= area[2] and area[1] <= possiblenodes2[i][1] <= area[3]: 73 | if possiblenodes2[i] in possible_nodes: 74 | possible_nodes.remove(possiblenodes2[i]) 75 | possible_costs.remove(possiblecosts2[i]) 76 | 77 | for i in range(len(possible_nodes)): 78 | next_nodes.append(possible_nodes[i]) 79 | next_costs.append(possible_costs[i]) 80 | 81 | return next_nodes, next_costs 82 | 83 | def get_minf(self, best, next): 84 | if next.f < best.f: 85 | best = next 86 | 87 | return best 88 | 89 | def pathplanning(self, rear_p, front_p, fi, iters, moving_vehicles, moving_areas): 90 | father_node = [front_p[0], front_p[1]] 91 | current_node = [front_p[0], front_p[1]] 92 | current = ASTAR.NODE(current_node, father_node, self.target) 93 | 94 | distance = math.sqrt((self.target[0] - rear_p[0]) ** 2 + (self.target[1] - rear_p[1]) ** 2) 95 | 96 | while distance > self.target_area and iters <= self.max_iters: 97 | iters += 1 98 | print("\n----------\nNo.", iters) 99 | print("current node:", current.current_node, ", previous angle:", current.direction, ", current g:", 100 | current.g, ", h:", current.h, ", f:", current.f) 101 | 102 | next_nodes, next_costs = self.possible_moves(current.current_node, moving_areas) 103 | 104 | next_set = [] 105 | for next_node, next_cost in zip(next_nodes, next_costs): 106 | next = ASTAR.NODE(next_node, current.current_node, self.target, next_cost + current.g) 107 | next_set.append(next) 108 | 109 | current.f = float("inf") 110 | best = current 111 | for next in next_set: 112 | best = self.get_minf(best, next) 113 | 114 | if best == current: 115 | print("\n----------\nA* is failed!\n----------") 116 | exit() 117 | 118 | current_node = best.current_node 119 | father_node = best.father_node 120 | current_cost = best.g 121 | current = ASTAR.NODE(current_node, father_node, self.target, current_cost) 122 | si = current.direction - fi 123 | print("steering angle:", int(si / math.pi * 180)) 124 | 125 | rear_v, rear_p, front_v, front_p, fi, vf = self.car.model(rear_p, fi, si, self.del_t) 126 | print("rear velocity:", rear_v, ", rear position:", rear_p) 127 | # print("front velocity:", front_v, ", front position:", front_p) 128 | # print("vehicle direction:", int(fi / math.pi * 180)) 129 | 130 | dif_distance, dif_distance2 = self.car.difference(rear_p, front_p, si, self.del_t) 131 | print("difference between formula and subtract:", dif_distance - dif_distance2) 132 | print("distance difference:", dif_distance) 133 | 134 | self.car.draw_car(rear_p, front_p) 135 | moving_vehicles, moving_areas, p1, p2, p3, = self.car.draw_movingcar(moving_vehicles, self.del_t) 136 | # plt.pause(0.1) 137 | p1.remove() 138 | p2.remove() 139 | p3.remove() 140 | 141 | self.rear_velocities.append(self.car.v) 142 | self.path.append([rear_p[0], rear_p[1]]) 143 | self.direction.append(fi) 144 | self.front_velocities.append(vf) 145 | self.front_points.append([front_p[0], front_p[1]]) 146 | self.steering.append(si) 147 | 148 | father_node = current.father_node 149 | current_node = [front_p[0], front_p[1]] 150 | current = ASTAR.NODE(current_node, father_node, self.target, current_cost) 151 | 152 | distance = math.sqrt((self.target[0] - rear_p[0]) ** 2 + (self.target[1] - rear_p[1]) ** 2) 153 | 154 | if distance <= self.target_area and iters <= self.max_iters: 155 | print("\n----------\nA* is successful!\n----------") 156 | else: 157 | print("\n----------\nA* is failed!\n----------") 158 | 159 | return rear_p, front_p, fi, iters, moving_vehicles 160 | 161 | -------------------------------------------------------------------------------- /singleAstar/car.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | 4 | 5 | class CAR: 6 | 7 | def __init__(self, L, B, l, velocity): 8 | self.L = L 9 | self.B = B 10 | self.l = l 11 | self.v = velocity 12 | self.dif_distances = [] 13 | 14 | def model(self, current_position, fi, si, del_t=0): 15 | w = self.v * math.tan(si) / self.l 16 | vf = self.v / math.cos(si) 17 | 18 | rear_v = [self.v * math.cos(fi), self.v * math.sin(fi), w] 19 | rear_p = [current_position[0] + del_t * rear_v[0], 20 | current_position[1] + del_t * rear_v[1], 21 | current_position[2] + del_t * rear_v[2]] 22 | 23 | current_front = [current_position[0] + self.l * math.cos(fi), current_position[1] + self.l * math.sin(fi), 24 | fi + si] 25 | front_v = [vf * math.cos(fi + si), vf * math.sin(fi + si), w] 26 | front_p = [current_front[0] + del_t * front_v[0], 27 | current_front[1] + del_t * front_v[1], 28 | current_front[1] + del_t * front_v[2]] 29 | 30 | new_fi = rear_p[2] 31 | 32 | return rear_v, rear_p, front_v, front_p, new_fi, vf 33 | 34 | def difference(self, rear_p, front_p, si, del_t=0): 35 | dif_distance = math.sqrt(self.l ** 2 + (del_t * self.v * math.tan(si)) ** 2) - self.l 36 | dif_distance2 = math.sqrt((front_p[0] - rear_p[0]) ** 2 + (front_p[1] - rear_p[1]) ** 2) - self.l 37 | self.dif_distances.append(dif_distance) 38 | 39 | return dif_distance, dif_distance2 40 | 41 | def draw_car(self, rear_p, front_p): 42 | draw_x = [rear_p[0], front_p[0]] 43 | draw_y = [rear_p[1], front_p[1]] 44 | plt.plot(draw_x, draw_y, c='orange', linewidth=10) 45 | 46 | return 47 | 48 | def draw_movingcar(self, moving_vehicles, del_t=0): 49 | new_moving_vehicles, moving_areas, p1, p2, p3, = [], [], [], [], [] 50 | 51 | x11 = moving_vehicles[0][0] 52 | y11 = moving_vehicles[0][1] - self.l / 2 53 | x12 = moving_vehicles[0][0] 54 | y12 = moving_vehicles[0][1] + self.l / 2 55 | draw_x1 = [x11, x12] 56 | draw_y1 = [y11, y12] 57 | p1, = plt.plot(draw_x1, draw_y1, c='green', linewidth=10) 58 | 59 | x21 = moving_vehicles[1][0] 60 | y21 = moving_vehicles[1][1] - self.l / 2 61 | x22 = moving_vehicles[1][0] 62 | y22 = moving_vehicles[1][1] + self.l / 2 63 | draw_x2 = [x21, x22] 64 | draw_y2 = [y21, y22] 65 | p2, = plt.plot(draw_x2, draw_y2, c='green', linewidth=10) 66 | 67 | x31 = moving_vehicles[2][0] - self.l / 2 68 | y31 = moving_vehicles[2][1] 69 | x32 = moving_vehicles[2][0] + self.l / 2 70 | y32 = moving_vehicles[2][1] 71 | draw_x3 = [x31, x32] 72 | draw_y3 = [y31, y32] 73 | p3, = plt.plot(draw_x3, draw_y3, c='green', linewidth=10) 74 | 75 | coop_x1 = moving_vehicles[0][0] 76 | coop_y1 = moving_vehicles[0][1] + moving_vehicles[0][3] * del_t 77 | coop_x2 = moving_vehicles[1][0] 78 | coop_y2 = moving_vehicles[1][1] + moving_vehicles[1][3] * del_t 79 | coop_x3 = moving_vehicles[2][0] + moving_vehicles[2][3] * del_t 80 | coop_y3 = moving_vehicles[2][1] 81 | new_moving_vehicles.append([coop_x1, coop_y1, 'v', moving_vehicles[0][3]]) 82 | new_moving_vehicles.append([coop_x2, coop_y2, 'v', moving_vehicles[1][3]]) 83 | new_moving_vehicles.append([coop_x3, coop_y3, 'h', moving_vehicles[2][3]]) 84 | 85 | moving_vehicles.clear() 86 | moving_vehicles = new_moving_vehicles 87 | 88 | ''' 89 | car_Le = 1.2 * self.L 90 | car_Be = 1.2 * self.B 91 | for vehicle in moving_vehicles: 92 | if vehicle[2] == 'h': 93 | moving_areas.append([vehicle[0] - car_Le, vehicle[1] - car_Be, 94 | vehicle[0] + car_Le, vehicle[1] + car_Be]) 95 | else: 96 | moving_areas.append([vehicle[0] - car_Be, vehicle[1] - car_Le, 97 | vehicle[0] + car_Be, vehicle[1] + car_Le]) 98 | ''' 99 | 100 | return moving_vehicles, moving_areas, p1, p2, p3, 101 | -------------------------------------------------------------------------------- /singleAstar/zong.py: -------------------------------------------------------------------------------- 1 | import math 2 | import matplotlib.pyplot as plt 3 | import time 4 | from assistant import * 5 | from astar import * 6 | from car import * 7 | 8 | 9 | # main 10 | if __name__ == "__main__": 11 | print("Creating a map:\n----------") 12 | length = 80 13 | width = 80 14 | lane_width = 3.5 15 | L = 4.8 16 | B = 1.8 17 | 18 | map = MAP(length, width, lane_width, L, B) 19 | fig = map.create_map() 20 | 21 | print("\nCreating lanes:\n----------") 22 | solid_lines, dotted_lines, transition_lines = map.create_lanes(fig) 23 | print("solid lines:", solid_lines) 24 | print("dotted lines:", dotted_lines) 25 | print("transition lines:", transition_lines) 26 | line_areas = map.create_lineareas(solid_lines) 27 | print("infeasible line areas:", line_areas) 28 | 29 | print("\nCreating obstacles:\n----------") 30 | obstacles = map.get_obstacles(fig) 31 | print("obstacles:", obstacles) 32 | obstacle_areas = map.get_obsareas(obstacles) 33 | print("infeasible obstacle areas:", obstacle_areas) 34 | 35 | print("\nCreating vehicles:\n----------") 36 | vehicles, init_movingvehis, moving_vehicles = map.get_vehicles(fig) 37 | print("vehicles:", vehicles) 38 | print("moving vehicles:", moving_vehicles) 39 | vehicle_areas, moving_areas = map.get_vehareas(vehicles, moving_vehicles) 40 | print("infeasible vehicle areas:", vehicle_areas) 41 | print("infeasible moving vehicle areas:", moving_areas) 42 | 43 | print("\nCreating infeasible areas:\n----------") 44 | infeasible_areas = map.create_areas() 45 | print("infeasible areas:", infeasible_areas) 46 | 47 | print("\nSetting start and target:\n----------") 48 | start, target = map.get_startandtarget() 49 | print("start:", start, ", target:", target) 50 | 51 | # vehicle model 52 | print("\nEstablishing the vehicle model:\n----------") 53 | l = 3 54 | # b = 1.6 55 | velocity = 20 # changeable 56 | init_force_direction = 0 57 | init_fi = 0 # changeable 58 | init_si = init_force_direction - init_fi # random 59 | init_position = [start[0], start[1], init_fi] 60 | 61 | car = CAR(L, B, l, velocity) 62 | rear_v, rear_p, front_v, front_p, fi, vf = car.model(init_position, init_fi, init_si) 63 | print("initial steering angle:", int(init_si / math.pi * 180)) 64 | print("initial rear velocity:", rear_v, ", initial rear position:", rear_p) 65 | print("initial front velocity:", front_v, ", initial front position:", front_p) 66 | print("initial vehicle direction:", int(fi / math.pi * 180)) 67 | 68 | dif_distance, dif_distance2 = car.difference(rear_p, front_p, init_si) 69 | print("difference between formula and subtract:", dif_distance - dif_distance2) 70 | print("distance difference:", dif_distance) 71 | 72 | car.draw_car(rear_p, front_p) 73 | moving_vehicles, moving_areas, p1, p2, p3, = car.draw_movingcar(moving_vehicles) 74 | # plt.pause(0.1) 75 | plt.plot(start[0], start[1], '*', color='purple', markersize=10) # drawing again 76 | p1.remove() 77 | p2.remove() 78 | p3.remove() 79 | 80 | # A* path planning algorithm 81 | t1 = time.time() 82 | 83 | print("\nA* algorithm path planning:\n----------") 84 | target_area = 3.5 85 | del_t = 0.05 # changeable 86 | del_block = velocity * del_t 87 | max_iters = 1000 88 | 89 | rear_velocities = [car.v] 90 | path = [[rear_p[0], rear_p[1]]] 91 | direction = [fi] 92 | front_velocities = [vf] 93 | front_points = [[front_p[0], front_p[1]]] 94 | steering = [init_si] 95 | 96 | astar = ASTAR(del_block, target, target_area, del_t, max_iters, line_areas, obstacle_areas, vehicle_areas, 97 | infeasible_areas, car, rear_velocities, path, direction, front_velocities, front_points, steering) 98 | 99 | iters = 0 100 | rear_p, front_p, fi, iters, moving_vehicles = astar.pathplanning(rear_p, front_p, fi, iters, moving_vehicles, 101 | moving_areas) 102 | 103 | t2 = time.time() 104 | astar_time = t2 - t1 105 | 106 | print("total rear velocity:", astar.rear_velocities) 107 | print("total path:", astar.path) 108 | print("total direction:", astar.direction) 109 | print("total front velocity:", astar.front_velocities) 110 | print("total front position:", astar.front_points) 111 | print("total steering:", astar.steering) 112 | print("total iters:", iters) 113 | print("astar time:", astar_time) 114 | print("total distance difference:", car.dif_distances) 115 | 116 | map.final_draw(fig, astar.front_points, astar.path, init_movingvehis, moving_vehicles, L, B) 117 | plt.pause(0.1) 118 | 119 | plt.show() 120 | --------------------------------------------------------------------------------