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()
