7.2.5 RRT*算法
RRT*（Rapidly-exploring Random Tree Star）算法是 RRT（Rapidly-exploring Random Tree）算法的改进版本，旨在提高路径规划的效率和路径质量。RRT* 算法最早是由 Steven M. LaValle 和 James J. Kuffner Jr在2000年提出的。
RRT* 算法主要通过优化现有路径树的方式来改进 RRT 算法，而不是像 RRT 那样简单地在空间中不断生成随机样本和扩展树。其基本思想是通过引入代价函数来评估树中的各个节点，并在树的生长过程中不断优化树结构，以使生成的路径更加高效和优化。
RRT* 算法的主要特点和优势包括：
- 最优性保证：RRT* 算法可以保证找到的路径是最优或接近最优的，特别是在迭代次数趋于无穷大的极限情况下。
- 连续优化：算法通过对树中节点的代价进行优化，使得树的结构在不断扩展的过程中能够更好地反映问题的最优解。
- 自适应性：RRT* 算法能够根据问题的具体情况和要求，自适应地调整代价函数和优化策略，以获得更好的路径质量和性能。
总的来说，RRT* 算法是一种强大的路径规划算法，适用于需要高效、优化和最优路径的问题领域，如机器人运动规划、自动驾驶车辆路径规划等。
2010 年，MIT 的 Sertac 和 Emilio 提出了完整的RRT*算法。这个算法与上面的算法相比，不但可以找到可行解，还可以找到一条相对次优的算法。例如下面的实例实现了RRT*算法，通过随机扩展树、优化树结构等步骤，用于寻找从起始点到目标点的最优路径。
实例7-4：使用RRT*算法寻找路径（codes/7/RRTStar.py）
实例文件RRTStar.py的具体实现流程如下所示。
（1）定义节点类 Node，其中节点具有 x 和 y 坐标属性，以及父节点指针和代价属性。这个类用于在路径规划算法中表示空间中的节点，以便记录节点的位置、连接关系和代价信息。
# 定义 RRT* 类
class RRTStar:
def __init__(self, start, goal, obstacles, max_iter=1000, step_size=5, min_distance=5):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.obstacles = obstacles
self.max_iter = max_iter
self.step_size = step_size
self.min_distance = min_distance
self.nodes = [self.start]（2）定义RRT* 类 RRTStar，用于实现 RRT* 路径规划算法。在初始化时，它接受起始点、目标点、障碍物信息以及其他可选参数，并设置了初始状态。在算法运行过程中，它会维护一个节点列表，其中包含了从起始点开始扩展的树的所有节点。
# 定义 RRT* 类
class RRTStar:
def __init__(self, start, goal, obstacles, max_iter=1000, step_size=5, min_distance=5):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.obstacles = obstacles
self.max_iter = max_iter
self.step_size = step_size
self.min_distance = min_distance
self.nodes = [self.start]（3）定义方法distance计算两个节点之间的欧几里得距离。给定两个节点 node1 和 node2，它通过计算这两个节点在二维空间中的距离来计算它们之间的欧几里得距离。
# 欧几里得距离
def distance(self, node1, node2):
return np.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)（4）定义方法sample_point，用于在空间中随机采样一个点，以用于 RRT* 算法中的扩展步骤。在大多数情况下，它将在整个空间中均匀随机采样一个点。但是在一定概率下（这里是 10% 的概率），它会返回目标点的坐标，以便有机会直接扩展到目标点附近。
# 在随机点周围采样
def sample_point(self):
if np.random.rand() < 0.9:
return np.random.uniform(0, 600), np.random.uniform(0, 400) # 定义随机采样的范围
else:
return self.goal.x, self.goal.y # 使得采样点有概率落在目标点附近（5）下面的代码定义了 类RRT*中的 nearest_node 方法。这个方法接受一个点的坐标作为输入，并找到树中距离该点最近的节点。它首先将输入点转换为节点对象，然后计算输入点与树中每个节点的距离，并找到最小距离对应的节点，即最近节点。
# 寻找最近节点
def nearest_node(self, point):
point_node = Node(point[0], point[1])
distances = [self.distance(node, point_node) for node in self.nodes]
nearest_node = self.nodes[np.argmin(distances)]
return nearest_node（6）下面的代码定义了 类RRT*中的 is_collision_free 方法。这个方法用于检查两个节点之间是否存在障碍物。它遍历障碍物列表，对于每个障碍物，检查两个节点是否位于障碍物的范围内。如果任何一个节点位于障碍物范围内，则返回 False，表示存在障碍物；否则返回 True，表示两个节点之间没有障碍物。
# 检查两点之间是否存在障碍物
def is_collision_free(self, node1, node2):
for obstacle in self.obstacles:
if obstacle[0] <= node1.x <= obstacle[2] and obstacle[1] <= node1.y <= obstacle[3]:
return False
if obstacle[0] <= node2.x <= obstacle[2] and obstacle[1] <= node2.y <= obstacle[3]:
return False
return True（7）定义方法expand_tree，这个方法实现了 RRT* 算法的主要逻辑，用于不断扩展树直到找到目标点或达到最大迭代次数。它通过随机采样一个点，在树中找到最近的节点，然后沿着该方向扩展一步。如果新节点与最近节点之间没有障碍物，且路径更优（代价更低），则将新节点添加到树中。如果新节点接近目标点，则将目标点添加到树中并返回。
# 扩展树
def expand_tree(self):
for _ in range(self.max_iter):
rand_point = self.sample_point()
nearest_node = self.nearest_node(rand_point)
theta = np.arctan2(rand_point[1] - nearest_node.y, rand_point[0] - nearest_node.x)
new_point = (nearest_node.x + self.step_size * np.cos(theta), nearest_node.y + self.step_size * np.sin(theta))
new_node = Node(new_point[0], new_point[1])
if self.is_collision_free(nearest_node, new_node):
near_nodes = [node for node in self.nodes if self.distance(node, new_node) < self.min_distance]
min_cost_node = nearest_node
min_cost = self.distance(nearest_node, new_node)
for near_node in near_nodes:
if self.is_collision_free(near_node, new_node):
cost = near_node.cost + self.distance(near_node, new_node)
if cost < min_cost:
min_cost = cost
min_cost_node = near_node
new_node.cost = min_cost
new_node.parent = min_cost_node
self.nodes.append(new_node)
for near_node in near_nodes:
if self.distance(near_node, new_node) < self.min_distance and near_node.cost + self.distance(near_node, new_node) < new_node.cost:
new_node.parent = near_node
new_node.cost = near_node.cost + self.distance(near_node, new_node)
if self.distance(new_node, self.goal) < self.min_distance and self.is_collision_free(new_node, self.goal):
self.goal.parent = new_node
self.nodes.append(self.goal)
return（8）定义方法backtrack_path，用于回溯从起始点到目标点的路径。它从目标节点开始，沿着每个节点的父节点链向上回溯，直到到达起始节点。在回溯过程中，它将每个节点的坐标添加到路径列表中。最后，它反转路径列表以确保路径按照正确的顺序返回。
# 回溯路径
def backtrack_path(self):
path = []
current_node = self.goal
while current_node is not None:
path.append((current_node.x, current_node.y))
current_node = current_node.parent
path.reverse()
return path（9）定义方法plot，用于绘制 RRT* 算法生成的路径以及障碍物。它创建了一个新的图形窗口，并设置了坐标轴的范围。然后，它在图形中添加了每个障碍物的轮廓，并绘制了树的连接关系。最后，它绘制了找到的路径，并标记了起始点和目标点。这个方法使用 matplotlib 库来进行绘图。
# 绘制路径和障碍物
def plot(self, path):
fig, ax = plt.subplots()
ax.set_xlim(0, 600)
ax.set_ylim(0, 400)
for obstacle in self.obstacles:
ax.add_patch(plt.Rectangle((obstacle[0], obstacle[1]), obstacle[2] - obstacle[0], obstacle[3] - obstacle[1], color='gray'))
for node in self.nodes:
if node.parent:
ax.plot([node.x, node.parent.x], [node.y, node.parent.y], color='blue')
ax.plot([node[0] for node in path], [node[1] for node in path], color='red')
plt.plot(self.start.x, self.start.y, 'go')
plt.plot(self.goal.x, self.goal.y, 'ro')
plt.show()（10）在下面的代码中，首先创建了一个包含两个矩形障碍物的列表 obstacles，然后初始化了一个 RRT* 算法实例 rrt_star，并设置了起始点 (50, 50) 和目标点 (550, 350)，以及障碍物信息。接着，调用 expand_tree 方法扩展树，寻找从起始点到目标点的路径。然后，调用 backtrack_path 方法回溯路径，并将结果存储在 path 变量中。最后，调用 plot 方法绘制路径和障碍物，以可视化展示RRT* 算法生成的路径。
# 定义障碍物
obstacles = [(100, 100, 200, 200), (300, 300, 400, 400)]
# 初始化 RRT* 算法
rrt_star = RRTStar(start=(50, 50), goal=(550, 350), obstacles=obstacles)
# 扩展树
rrt_star.expand_tree()
# 回溯路径
path = rrt_star.backtrack_path()
# 绘制路径和障碍物
rrt_star.plot(path)执行后会可视化展示RRT* 算法生成的路径，如图7-5所示。
图7-5 RRT*算法生成的路径可视化图
扫一扫
2357
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
