"""
《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文
《基于改进人工势场法的路径规划算法研究》硕士论文
"""
import matplotlib.pyplot as plt
import random
import math
import copy
show_animation = True
class Node(object):
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
class RRT(object):
"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacle_list, rand_area):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:random sampling Area [min,max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expandDis = 1.0
self.goalSampleRate = 0.05 # 选择终点的概率是0.05
self.maxIter = 500
self.obstacleList = obstacle_list
self.nodeList = [self.start]
def random_node(self):
"""
产生随机节点
:return:
"""
node_x = random.uniform(self.min_rand, self.max_rand)
node_y = random.uniform(self.min_rand, self.max_rand)
node = [node_x, node_y]
return node
@staticmethod
def get_nearest_list_index(node_list, rnd):
"""
:param node_list:
:param rnd:
:return:
"""
d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list]
min_index = d_list.index(min(d_list))
return min_index
@staticmethod
def collision_check(new_node, obstacle_list):
a = 1
for (ox, oy, size) in obstacle_list:
dx = ox - new_node.x
dy = oy - new_node.y
d = math.sqrt(dx * dx + dy * dy)
if d <= size:
a = 0 # collision
return a # safe
def planning(self):
"""
Path planning
animation: flag for animation on or off
"""
while True:
# Random Sampling
if random.random() > self.goalSampleRate:
rnd = self.random_node()
else:
rnd = [self.end.x, self.end.y]
# Find nearest node
min_index = self.get_nearest_list_index(self.nodeList, rnd)
# print(min_index)
# expand tree
nearest_node = self.nodeList[min_index]
# 返回弧度制
theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
new_node = copy.deepcopy(nearest_node)
new_node.x += self.expandDis * math.cos(theta)
new_node.y += self.expandDis * math.sin(theta)
new_node.parent = min_index
if not self.collision_check(new_node, self.obstacleList):
continue
self.nodeList.append(new_node)
# check goal
dx = new_node.x - self.end.x
dy = new_node.y - self.end.y
d = math.sqrt(dx * dx + dy * dy)
if d <= self.expandDis:
print("Goal!!")
break
if True:
self.draw_graph(rnd)
path = [[self.end.x, self.end.y]]
last_index = len(self.nodeList) - 1
while self.nodeList[last_index].parent is not None:
node = self.nodeList[last_index]
path.append([node.x, node.y])
last_index = node.parent
path.append([self.start.x, self.start.y])
return path
def draw_graph(self, rnd=None):
"""
Draw Graph
"""
print('aaa')
plt.clf() # 清除上次画的图
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^g")
for node in self.nodeList:
if node.parent is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "sk", ms=10*size)
plt.plot(self.start.x, self.start.y, "^r")
plt.plot(self.end.x, self.end.y, "^b")
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.grid(True)
plt.pause(0.01)
def draw_static(self, path):
"""
画出静态图像
:return:
"""
plt.clf() # 清除上次画的图
for node in self.nodeList:
if node.parent is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "sk", ms=10*size)
plt.plot(self.start.x, self.start.y, "^r")
plt.plot(self.end.x, self.end.y, "^b")
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
plt.plot([data[0] for data in path], [data[1] for data in path], '-r')
plt.grid(True)
plt.show()
def main():
print("start RRT path planning")
obstacle_list = [
(5, 1, 1),
(3, 6, 2),
(3, 8, 2),
(1, 1, 2),
(3, 5, 2),
(9, 5, 2)]
# Set Initial parameters
rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list)
path = rrt.planning()
print(path)
# Draw final path
if show_animation:
plt.close()
rrt.draw_static(path)
if __name__ == '__main__':
main()
