快速拓展随机树路径规划（RRT) python
快速拓展随机树路径规划（RRT) python
快速拓展随机树路径规划（RRT)基本方法：
1.创建节点类，属性有xy坐标值和父节点；
2.将起点加入节点列表中；
3.在地图中随机产生一个点，
遍历节点列表中每个节点，计算与该随机点的距离，
找到与该随机节点a距离最短的节点b，
计算两节点的方向夹角
以节点b(列表中的节点)为起点，沿夹角方向生长单位长度，并在终点生成一个新节点
将该节点加入节点列表中
if 新节点距离终点距离小于给定值，将终点加入节点列表，路径找到
else
重复上述循环遍历
下面是引用别人的python代码：
import numpy as np
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):
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
def get_nearest_list_index(self,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
def collision_check(self,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):
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, 2),
(3, 6, 3),
(3, 8, 2),
(3, 5, 2),
(6, 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()
对程序进行分步说明：
class Node(object):
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
创建节点类，属性有xy坐标值和父节点；
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]
定义RRT类的初始属性，
def random_node(self):
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
在0-1内产生一个随机的节点坐标
def get_nearest_list_index(self,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
遍历nodelist每个点，找到距离随机点最近的节点，返回节点在该列表中的指针
def collision_check(self,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]
设置随机点有goalSampleRate = 0.05概率选到终点
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)
找到与该随机节点a距离最短的节点b，
计算两节点的方向夹角
以节点b(列表中的节点)为起点，沿夹角方向生长单位长度，并在终点生成一个新节点
将该节点加入节点列表中
新节点距离终点距离小于给定值，将终点加入节点列表，路径找到
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
last_index为节点列表中除终点以外最后一个点的指针，
如果除终点以外最后一个点的父节点不是None：
path列表加入这个节点，再将指针指向其父节点，其父节点的父节点还不为None,则继续循环。
最后返回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):
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, 2),
(3, 6, 3),
(3, 8, 2),
(3, 5, 2),
(6, 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()
主函数
程序循行效果如下图：
