1 """2 version1.1,2018-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()