RRT(Rapidly-exploring Random Tree)快速扩展随机树是一种采样式路径规划算法,广泛应用于机器人运动规划、自动驾驶、无人机路径设计等领域。它特别适用于高维空间中的路径规划问题。下面是对 RRT 算法的详细介绍:
一、RRT 算法的核心思想
RRT 的核心思想是通过在空间中随机采样点并逐步构建一棵树形结构(搜索树),来快速探索空间并找到从起点到终点的可行路径。 RRT 偏向于快速探索未被探索的空间区域,从而快速覆盖整个搜索空间。
二、基本流程
输入:
- 起点
q_start - 终点
q_goal - 空间约束(如障碍物、边界等)
- 步长
Δq - 最大迭代次数
N
步骤:
- 初始化一棵树
T,树的根节点为起点q_start。 - 对于每次迭代:
- 随机采样一个点
q_rand(可以是完全随机,也可以有一定概率采样为q_goal,称为'目标偏向')。 - 在树中找到距离
q_rand最近的节点q_nearest。 - 从
q_nearest向q_rand移动一个固定步长Δq,得到新的节点q_new。 - 如果
q_new不在障碍物中,则将其加入树中,并将其父节点设为q_nearest。 - 如果
q_new距离q_goal很近,可以认为找到了可行路径。
- 随机采样一个点
- 如果找到路径,沿父节点回溯得到路径;否则直到达到最大迭代次数。
三、RRT 算法伪代码
def RRT(q_start, q_goal, N, Δq):
T = Tree(q_start)
for i in range(N):
q_rand = random_sample()
q_nearest = nearest_node(T, q_rand)
q_new = steer(q_nearest, q_rand, Δq)
if is_valid(q_nearest, q_new):
T.add_node(q_new, parent=q_nearest)
if distance(q_new, q_goal) < threshold:
return extract_path(T, q_new)
return failure
四、Python 实现
下面提供了一个简化版的 Python 实现示例,并配合图示说明 RRT 的执行过程。
import numpy as np
import matplotlib.pyplot as plt
import random
:
():
.x = x
.y = y
.parent =
():
np.hypot(n1.x - n2.x, n1.y - n2.y)
():
random.random() < goal_sample_rate:
Node(goal.x, goal.y)
Node(random.uniform(, ), random.uniform(, ))
():
dist = distance(from_node, to_node)
theta = np.arctan2(to_node.y - from_node.y, to_node.x - from_node.x)
new_x = from_node.x + extend_length * np.cos(theta)
new_y = from_node.y + extend_length * np.sin(theta)
new_node = Node(new_x, new_y)
new_node.parent = from_node
new_node
():
():
nodes = [start]
_ (max_iter):
rnd = get_random_node(goal_sample_rate, goal)
nearest = (nodes, key= n: distance(n, rnd))
new_node = steer(nearest, rnd)
is_collision(new_node):
nodes.append(new_node)
distance(new_node, goal) < :
goal.parent = new_node
nodes.append(goal)
nodes
():
path = []
node = last_node
node:
path.append((node.x, node.y))
node = node.parent
path = path[::-]
plt.plot([x x, y path], [y x, y path], )
():
node nodes:
node.parent:
plt.plot([node.x, node.parent.x], [node.y, node.parent.y], )
start = Node(, )
goal = Node(, )
nodes = rrt(start, goal)
draw_tree(nodes)
draw_path(goal)
plt.plot(start.x, start.y, , label=)
plt.plot(goal.x, goal.y, , label=)
plt.legend()
plt.grid()
plt.axis([, , , ])
plt.title()
plt.show()


