大家好，今天和各位分享一下机器人路径规划中的 RRT 算法，感兴趣的点个关注，文末有 python 代码，那我们开始吧。
RRT 算法是由学者 S.M.LaValle 提出来的路径规划算法。该算法是将空间中的一个起始点当作根节点，在空间中利用增量方式进行随机采样，然后向外延伸扩展增加叶节点。重复该步骤直到获得一个从起点到目标点的树，最后回溯找到从根节点到目标点的无碰撞的路径。
快速搜索随机树（RRT）算法是一种增量式采样的搜索方法，将它应用于路径规划时不需要任何参数整定，也不需要事先对环境信息进行处理和存储，具有良好的适应性，适合解决复杂环境以及多种约束条件的路径规划问题。
算法优点：
（1）RRT 算法不需要启发式函数，可以在存在复杂障碍物的环境里快速规划出一条安全路径，且更适合实际应用，目前常被用于无人机的运动规划中。
（2）RRT 的快速迭代生长方式决定了其在高维非凸空间的搜索优势。
算法缺点：
（1）稳定性不佳，且内存消耗较大。扩展树生长方向随机，重复规划结果却有很大差异。
（2）随机性采样导致相对效率较低。由于快速扩展随机树有随机采样性，没有目标启发导向，因此在采样过程中有很多无效采样节点，致使效率相对变低。
（3）路径曲折冗余点较多，光滑性不佳。在 RRT 生成的路径中有许多无用的点，这些冗余点使得路径长度增多。
（4）无法在动态障碍物中有效检测路径，不适合于动态环境规划与避障。
传统 RRT 算法计算流程可分为两个阶段：
第一个阶段。在任务环境中，以算法初始点作为根节点，通过随机采样、逐步迭代地方式，向任务环境中进行扩展，形成一棵随机树，通过判定采样点是否包含目标点或在目标范围内，来判断是否完成随机树的构造，结束采样；
第二阶段。在随机树中从目标点依次往回遍历，直到寻回根节点，即可得到一条从初始点到目标点的有效路径。RRT的具体扩展方式如下图所示。
首先，以初始点 Pinit 作为随机树的根节点，在任务环境中随机选取一个采样点 Prand，从节点树中找出离采样点 Prand 最近的一个子节点 Pnear，以点 Pnear 为端点，沿着 Pnear——Prand 方向扩展一定的步长 𝜌，将另一端点记为 Pnew，
若 Pnear 与 Pnew 的连线并未与环境障碍发生交叉，则将点 Pnew 和线段 Pnear--Pnew 加入到随机树中，成为新的子节点和新的路径；
若线段 Pnear 与 Pnew 和障碍发生交叉碰撞，则舍弃点 Pnew，重新选取新的采样点 Prand。重复上述步骤，直至点 Pnew与目标点 Pgoal 不存在障碍，且步长不大于𝜌，则将 Pgoal 加入到随机树中，至此，随机树构造完成。
而计算所得的可行路径便是从离 Pgoal 最近的一个子节点 Pnew 依次向上搜索父节点，直到初始点 Pinit，基于 RRT 算法的路径规划结束，其主要流程框图如下图所示。
RRT仿真结果如下图所示，左图为快速扩展随机树随机采样过程，右图为最终提取到的运动路径规划轨迹。设置起始点（10，10）和终止点位置（490，490）坐标。
如下图，绿色线条代表RRT生长的树枝，红色线条代表搜索到的最优路径
完整代码如下，每段都有注释，有问题在评论区留言
import math
import random
import matplotlib.pyplot as plt
import numpy as np
class RRT:
class Node: # 创建节点
def __init__(self, x, y):
self.x = x # 节点坐标
self.y = y
self.path_x = [] # 路径，作为画图的数据
self.path_y = []
self.parent = None #父节点
class AreaBounds:
"""区域大小
"""
def __init__(self, area):
self.xmin = float(area[0])
self.xmax = float(area[1])
self.ymin = float(area[2])
self.ymax = float(area[3])
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=1.0, # 树枝长度
goal_sample_rate=5,
max_iter=500,
play_area=None,
robot_radius=0.0,
):
"""
Setting Parameter
start:起点 [x,y]
goal:目标点 [x,y]
obstacleList:障碍物位置列表 [[x,y,size],...]
rand_area: 采样区域 x,y ∈ [min,max]
play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]
robot_radius: 机器人半径
expand_dis: 扩展的步长
goal_sample_rate: 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点
"""
self.start = self.Node(start[0], start[1]) # 根节点(0,0)
self.end = self.Node(goal[0], goal[1]) # 终点(6,10)
self.min_rand = rand_area[0] # -2 树枝生长区域xmin
self.max_rand = rand_area[1] # 15 xmax
if play_area is not None:
self.play_area = self.AreaBounds(play_area) # 树枝生长区域，左下(-2,0)==>右上(12,14)
else:
self.play_area = None # 数值无限生长
self.expand_dis = expand_dis # 树枝一次的生长长度
self.goal_sample_rate = goal_sample_rate # 多少概率直接选终点
self.max_iter = max_iter # 最大迭代次数
self.obstacle_list = obstacle_list # 障碍物的坐标和半径
self.node_list = [] # 保存节点
self.robot_radius = robot_radius # 随机点的搜索半径
# 路径规划
def planning(self, animation=True,camara=None):
# 将起点作为根节点x_{init}，加入到随机树的节点集合中。
self.node_list = [self.start] # 先在节点列表中保存起点
for i in range(self.max_iter):
# 从可行区域内随机选取一个节点x_{rand}
rnd_node = self.sample_free()
# 已生成的树中利用欧氏距离判断距离x_{rand}最近的点x_{near}。
# 从已知节点中选择和目标节点最近的节点
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node) # 最接近的节点的索引
nearest_node = self.node_list[nearest_ind] # 获取该最近已知节点的坐标
# 从 x_{near} 与 x_{rand} 的连线方向上扩展固定步长 u，得到新节点 x_{new}
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
# 如果在可行区域内，且x_{near}与x_{new}之间无障碍物
# 判断新点是否在规定的树的生长区域内，新点和最近点之间是否存在障碍物
if self.is_inside_play_area(new_node, self.play_area) and \
self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
# 都满足才保存该点作为树节点
self.node_list.append(new_node)
# 如果此时得到的节点x_new到目标点的距离小于扩展步长，则直接将目标点作为x_rand。
if self.calc_dist_to_goal(self.node_list[-1].x,self.node_list[-1].y) <= self.expand_dis:
# 以新点为起点，向终点画树枝
final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
# 如果最新点和终点之间没有障碍物True
if self.obstacle_free(final_node, self.obstacle_list, self.robot_radius):
# 返回最终路径
return self.generate_final_course(len(self.node_list) - 1)
if animation and i % 5 ==0:
self.draw_graph(rnd_node, camara)
return None # cannot find path
# 距离最近的已知节点坐标，随机坐标，从已知节点向随机节点的延展的长度
def steer(self, from_node, to_node, extend_length=float("inf")):
# d已知点和随机点之间的距离，theta两个点之间的夹角
d, theta = self.calc_distance_and_angle(from_node, to_node)
# 如果$x_{near}$与$x_{rand}$间的距离小于步长，则直接将$x_{rand}$作为新节点$x_{new}$
if extend_length >= d: # 如果树枝的生长长度超出了随机点，就用随机点位置作为新节点
new_x = to_node.x
new_y = to_node.y
else: # 如果树生长长度没达到随机点长度，就截取长度为extend_length的节点作为新节点
new_x = from_node.x + math.cos(theta)*extend_length # 最近点 x + cos * extend_len
new_y = from_node.y + math.sin(theta)*extend_length # 最近点 y + sin * extend_len
new_node = self.Node(new_x,new_y) # 初始化新节点
new_node.path_x = [from_node.x] # 最近点
new_node.path_y = [from_node.y] #
new_node.path_x.append(new_x) # 新点
new_node.path_y.append(new_y)
new_node.parent = from_node # 根节点变成最近点，用来指明方向
return new_node
def generate_final_course(self, goal_ind): # 终点坐标的索引
"""生成路径
Args:
goal_ind (_type_): 目标点索引
Returns:
_type_: _description_
"""
path = [[self.end.x, self.end.y]] # 保存终点节点
node = self.node_list[goal_ind]
while node.parent is not None: # 根节点
path.append([node.x, node.y])
node = node.parent
path.append([node.x, node.y])
return path
def calc_dist_to_goal(self, x, y):
"""计算(x,y)离目标点的距离
"""
dx = x - self.end.x # 新点的x-终点的x
dy = y - self.end.y
return math.hypot(dx, dy) # 计算新点和终点之间的距离
def sample_free(self):
# 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长
if random.randint(0, 100) > self.goal_sample_rate: # 大于5%就不选终点方向作为下一个节点
rnd = self.Node(
random.uniform(self.min_rand, self.max_rand), # 在树枝生长区域中随便取一个点
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
# 绘制搜索过程
def draw_graph(self, rnd=None, camera=None):
if camera==None:
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
# 画随机点
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
if self.robot_radius > 0.0:
self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
# 画已生成的树
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
# 画障碍物
for (ox, oy, size) in self.obstacle_list:
self.plot_circle(ox, oy, size)
# 如果约定了可行区域，则画出可行区域
if self.play_area is not None:
plt.plot([self.play_area.xmin, self.play_area.xmax,
self.play_area.xmax, self.play_area.xmin,
self.play_area.xmin],
[self.play_area.ymin, self.play_area.ymin,
self.play_area.ymax, self.play_area.ymax,
self.play_area.ymin],
"-k")
# 画出起点和目标点
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
plt.axis("equal")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
if camera!=None:
camera.snap()
# 静态方法无需实例化，也可以实例化后调用，静态方法内部不能调用self.的变量
@staticmethod
def plot_circle(x, y, size, color="-b"): # pragma: no cover
deg = list(range(0, 360, 5))
deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
plt.plot(xl, yl, color)
@staticmethod
def get_nearest_node_index(node_list, rnd_node): # 已知节点list，随机的节点坐标
# 计算所有已知节点和随机节点之间的距离
dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
for node in node_list]
# 获得距离最小的节点的索引
minind = dlist.index(min(dlist))
return minind
@staticmethod
def is_inside_play_area(node, play_area):
if play_area is None:
return True # no play_area was defined, every pos should be ok
if node.x < play_area.xmin or node.x > play_area.xmax or \
node.y < play_area.ymin or node.y > play_area.ymax:
return False # outside - bad
else:
return True # inside - ok
@staticmethod
def obstacle_free(node, obstacleList, robot_radius): # 目标点，障碍物中点和半径，移动时的占地半径
if node is None:
return False
for (ox, oy, size) in obstacleList:
dx_list = [ox - x for x in node.path_x]
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= (size+robot_radius)**2:
return False # collision
return True # safe
@staticmethod
def calc_distance_and_angle(from_node, to_node):
"""计算两个节点间的距离和方位角
Args:
from_node (_type_): _description_
to_node (_type_): _description_
Returns:
_type_: _description_
"""
dx = to_node.x - from_node.x
dy = to_node.y - from_node.y
d = math.hypot(dx, dy) # 平方根
theta = math.atan2(dy, dx) # 夹角的弧度值
return d, theta
def main(gx=6.0, gy=10.0):
print("start " + __file__)
fig = plt.figure(1)
# camera = Camera(fig) # 保存动图时使用
camera = None # 不保存动图时，camara为None
show_animation = True
# ====Search Path with RRT====
obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
(9, 5, 2), (8, 10, 1)] # [x, y, radius]
# Set Initial parameters
rrt = RRT(
start=[0, 0], # 起点位置
goal=[gx, gy], # 终点位置
rand_area=[-2, 15], # 树枝可生长区域[xmin,xmax]
obstacle_list=obstacleList, # 障碍物
play_area=[-2, 12, 0, 14], # 树的生长区域，左下[-2,0]==>右上[12,14]
robot_radius=0.2 # 搜索半径
)
path = rrt.planning(animation=show_animation,camara=camera)
if path is None:
print("Cannot find path")
else:
print("found path!!")
# 绘制最终路径
if show_animation:
rrt.draw_graph(camera=camera)
plt.grid(True)
plt.pause(0.01)
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
if camera!=None:
camera.snap()
# animation = camera.animate()
# animation.save('trajectory.gif')
plt.show()
if __name__ == '__main__':
main()