1 """2 version1.1,-05-09
3 《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文4 《基于改进人工势场法的路径规划算法研究》硕士论文5
6 """7
8 import matplotlib.pyplot asplt9 import random10 import math11 import copy12
13 show_animation =True14
15
16 class Node(object):17 """18 RRT Node19 """20
21 def __init__(self, x, y):22 self.x =x23 self.y =y24 self.parent =None25
26
27 class RRT(object):28 """29 Class forRRT Planning30 """31
32 def __init__(self, start, goal, obstacle_list, rand_area):33 """34 Setting Parameter35
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_list50 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 returnnode62
63 @staticmethod64 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 innode_list]71 min_index =d_list.index(min(d_list))72 returnmin_index73
74 @staticmethod75 def collision_check(new_node, obstacle_list):76 a = 1
77 for (ox, oy, size) inobstacle_list:78 dx = ox -new_node.x79 dy = oy -new_node.y80 d = math.sqrt(dx * dx + dy *dy)81 if d <=size:82 a = 0# collision83
84 returna # safe85
86 def planning(self):87 """88 Path planning89
90 animation: flag foranimation on or off91 """92
93 whileTrue:94 # Random Sampling95 if random.random() >self.goalSampleRate:96 rnd =self.random_node()97 else:98 rnd =[self.end.x, self.end.y]99
100 # Find nearest node101 min_index =self.get_nearest_list_index(self.nodeList, rnd)102 # print(min_index)103
104 # expand tree105 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_index114
115 ifnot self.collision_check(new_node, self.obstacleList):116 continue
117
118 self.nodeList.append(new_node)119
120 # check goal121 dx = new_node.x -self.end.x122 dy = new_node.y -self.end.y123 d = math.sqrt(dx * dx + dy *dy)124 if d <=self.expandDis:125 print("Goal!!")126 break
127
128 ifTrue: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 isnot None:134 node =self.nodeList[last_index]135 path.append([node.x, node.y])136 last_index =node.parent137 path.append([self.start.x, self.start.y])138
139 returnpath140
141 def draw_graph(self, rnd=None):142 """143 Draw Graph144 """145 print('aaa')146 plt.clf() # 清除上次画的图147 if rnd isnot None:148 plt.plot(rnd[0], rnd[1], "^g")149 for node inself.nodeList:150 if node.parent isnot 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) inself.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 inself.nodeList:171 if node.parent isnot 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) inself.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 parameters199 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 path204 ifshow_animation:205 plt.close()206 rrt.draw_static(path)207
208
209 if __name__ == '__main__':210 main()
