"""
《基于智能优化与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):
= x
= y
= 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]
"""
= Node(start[0], start[1])
= Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
= 1.0
= 0.05 # 选择终点的概率是0.05
= 500
= obstacle_list
= []
def random_node(self):
"""
产生随机节点
:return:
"""
node_x = (self.min_rand, self.max_rand)
node_y = (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 = [( - rnd[0]) ** 2 + ( - 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 = (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 () > :
rnd = self.random_node()
else:
rnd = [, ]
# Find nearest node
min_index = self.get_nearest_list_index(, rnd)
# print(min_index)
# expand tree
nearest_node = [min_index]
# 返回弧度制
theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
new_node = (nearest_node)
new_node.x += * (theta)
new_node.y += * (theta)
new_node.parent = min_index
if not self.collision_check(new_node, ):
continue
(new_node)
# check goal
dx = new_node.x -
dy = new_node.y -
d = (dx * dx + dy * dy)
if d <= :
print("Goal!!")
break
if True:
self.draw_graph(rnd)
path = [[, ]]
last_index = len() - 1
while [last_index].parent is not None:
node = [last_index]
([, ])
last_index =
([, ])
return path
def draw_graph(self, rnd=None):
"""
Draw Graph
"""
print('aaa')
() # 清除上次画的图
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^g")
for node in :
if is not None:
([, [].x], [
, [].y], "-g")
for (ox, oy, size) in :
(ox, oy, "sk", ms=10*size)
(, , "^r")
(, , "^b")
([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
(True)
(0.01)
def draw_static(self, path):
"""
画出静态图像
:return:
"""
() # 清除上次画的图
for node in :
if is not None:
([, [].x], [
, [].y], "-g")
for (ox, oy, size) in :
(ox, oy, "sk", ms=10*size)
(, , "^r")
(, , "^b")
([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
([data[0] for data in path], [data[1] for data in path], '-r')
(True)
()
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 = ()
print(path)
# Draw final path
if show_animation:
()
rrt.draw_static(path)
if __name__ == '__main__':
main()
