python⽆⼈机路径规划算法_RRT算法在Python中的实现，快
速，拓展，随机，树
"""
《基于智能优化与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.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.maxIter = 500
self.obstacleList = obstacle_list
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)
a = 0 # collision
random在python中的意思
return a # safe
def planning(self):
"""
Path planning
animation: flag for animation on or off
"""
while True:
# Random Sampling
if random.random() > alSampleRate:
rnd = self.random_node()
else:
rnd = [d.x, d.y]
# Find nearest node
min_index = _nearest_list_deList, rnd)
# print(min_index)
# expand tree
nearest_node = deList[min_index]
# 返回弧度制
theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) new_node = copy.deepcopy(nearest_node)
new_node.x += pandDis * s(theta)
new_node.y += pandDis * math.sin(theta)
new_node.parent = min_index
if llision_check(new_node, self.obstacleList):
continue
# check goal
dx = new_node.x - d.x
dy = new_node.y - d.y
d = math.sqrt(dx * dx + dy * dy)
if d <= pandDis:
print("Goal!!")
if True:
self.draw_graph(rnd)
path = [[d.x, d.y]]
last_index = deList) - 1
deList[last_index].parent is not None:
node = deList[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 deList:
if node.parent is not None:
plt.plot([node.x, deList[node.parent].x], [
node.y, deList[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.d.x, d.y, "^b")
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) id(True)
plt.pause(0.01)
def draw_static(self, path):
"""
画出静态图像
:return:
plt.clf() # 清除上次画的图
for node deList:
if node.parent is not None:
plt.plot([node.x, deList[node.parent].x], [
node.y, deList[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.d.x, d.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.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()

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。