1 """ 2 version1.1,2018-05-09 3 《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文 4 《基于改进人工势场法的路径规划算法研究》硕士论文 5 6 """ 7 8 import matplotlib.pyplot as plt 9 import random 10 import math 11 import copy 12 13 show_animation = True 14 15 16 class Node(object): 17 """ 18 RRT Node 19 """ 20 21 def __init__(self, x, y): 22 self.x = x 23 self.y = y 24 self.parent = None 25 26 27 class RRT(object): 28 """ 29 Class for RRT Planning 30 """ 31 32 def __init__(self, start, goal, obstacle_list, rand_area): 33 """ 34 Setting Parameter 35 36 start:Start Position [x,y] 37 goal:Goal Position [x,y] 38 obstacleList:obstacle Positions [[x,y,size],...] 39 randArea:random sampling Area [min,max] 40 41 """ 42 self.start = Node(start[0], start[1]) 43 self.end = Node(goal[0], goal[1]) 44 self.min_rand = rand_area[0] 45 self.max_rand = rand_area[1] 46 self.expandDis = 1.0 47 self.goalSampleRate = 0.05 # 选择终点的概率是0.05 48 self.maxIter = 500 49 self.obstacleList = obstacle_list 50 self.nodeList = [self.start] 51 52 def random_node(self): 53 """ 54 产生随机节点 55 :return: 56 """ 57 node_x = random.uniform(self.min_rand, self.max_rand) 58 node_y = random.uniform(self.min_rand, self.max_rand) 59 node = [node_x, node_y] 60 61 return node 62 63 @staticmethod 64 def get_nearest_list_index(node_list, rnd): 65 """ 66 :param node_list: 67 :param rnd: 68 :return: 69 """ 70 d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list] 71 min_index = d_list.index(min(d_list)) 72 return min_index 73 74 @staticmethod 75 def collision_check(new_node, obstacle_list): 76 a = 1 77 for (ox, oy, size) in obstacle_list: 78 dx = ox - new_node.x 79 dy = oy - new_node.y 80 d = math.sqrt(dx * dx + dy * dy) 81 if d <= size: 82 a = 0 # collision 83 84 return a # safe 85 86 def planning(self): 87 """ 88 Path planning 89 90 animation: flag for animation on or off 91 """ 92 93 while True: 94 # Random Sampling 95 if random.random() > self.goalSampleRate: 96 rnd = self.random_node() 97 else: 98 rnd = [self.end.x, self.end.y] 99 100 # Find nearest node 101 min_index = self.get_nearest_list_index(self.nodeList, rnd) 102 # print(min_index) 103 104 # expand tree 105 nearest_node = self.nodeList[min_index] 106 107 # 返回弧度制 108 theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) 109 110 new_node = copy.deepcopy(nearest_node) 111 new_node.x += self.expandDis * math.cos(theta) 112 new_node.y += self.expandDis * math.sin(theta) 113 new_node.parent = min_index 114 115 if not self.collision_check(new_node, self.obstacleList): 116 continue 117 118 self.nodeList.append(new_node) 119 120 # check goal 121 dx = new_node.x - self.end.x 122 dy = new_node.y - self.end.y 123 d = math.sqrt(dx * dx + dy * dy) 124 if d <= self.expandDis: 125 print("Goal!!") 126 break 127 128 if True: 129 self.draw_graph(rnd) 130 131 path = [[self.end.x, self.end.y]] 132 last_index = len(self.nodeList) - 1 133 while self.nodeList[last_index].parent is not None: 134 node = self.nodeList[last_index] 135 path.append([node.x, node.y]) 136 last_index = node.parent 137 path.append([self.start.x, self.start.y]) 138 139 return path 140 141 def draw_graph(self, rnd=None): 142 """ 143 Draw Graph 144 """ 145 print('aaa') 146 plt.clf() # 清除上次画的图 147 if rnd is not None: 148 plt.plot(rnd[0], rnd[1], "^g") 149 for node in self.nodeList: 150 if node.parent is not None: 151 plt.plot([node.x, self.nodeList[node.parent].x], [ 152 node.y, self.nodeList[node.parent].y], "-g") 153 154 for (ox, oy, size) in self.obstacleList: 155 plt.plot(ox, oy, "sk", ms=10*size) 156 157 plt.plot(self.start.x, self.start.y, "^r") 158 plt.plot(self.end.x, self.end.y, "^b") 159 plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) 160 plt.grid(True) 161 plt.pause(0.01) 162 163 def draw_static(self, path): 164 """ 165 画出静态图像 166 :return: 167 """ 168 plt.clf() # 清除上次画的图 169 170 for node in self.nodeList: 171 if node.parent is not None: 172 plt.plot([node.x, self.nodeList[node.parent].x], [ 173 node.y, self.nodeList[node.parent].y], "-g") 174 175 for (ox, oy, size) in self.obstacleList: 176 plt.plot(ox, oy, "sk", ms=10*size) 177 178 plt.plot(self.start.x, self.start.y, "^r") 179 plt.plot(self.end.x, self.end.y, "^b") 180 plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) 181 182 plt.plot([data[0] for data in path], [data[1] for data in path], '-r') 183 plt.grid(True) 184 plt.show() 185 186 187 def main(): 188 print("start RRT path planning") 189 190 obstacle_list = [ 191 (5, 1, 1), 192 (3, 6, 2), 193 (3, 8, 2), 194 (1, 1, 2), 195 (3, 5, 2), 196 (9, 5, 2)] 197 198 # Set Initial parameters 199 rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list) 200 path = rrt.planning() 201 print(path) 202 203 # Draw final path 204 if show_animation: 205 plt.close() 206 rrt.draw_static(path) 207 208 209 if __name__ == '__main__': 210 main()
