探索启发式算法中的RRT*算法：Python实现深度剖析
文章引言
本篇内容是博主在学习人工智能（AI）领域时，用于个人学习、研究及自我总结的学习记录与笔记，基于博主对相关领域的理解进行整理，若有不当或侵权之处，烦请指出并会及时修正，还望海涵。文章归类于👉启发式算法
专栏：
【启发式算法】（9）---《RRT*算法详细讲解（Python）》
【启发式算法】RRT*算法详细阐述（Python）
目录
- RRT*算法概述
- 算法原理剖析
- RRT*与RRT的差异
- 算法步骤解析
- 步骤详细说明
- RRT*的关键原理探究
- 树的扩展机制
- 路径优化过程
- 最短路径连接方式
- 渐进最优特性
- Python实现RRT*算法
- 运行结果展示
- 注意事项提示
- 优点与不足分析
- 总结
1. RRT*算法概述
RRT算法（Rapidly-exploring Random Tree Star）是用于机器人路径规划的一种算法，其目标是为机器人找出从起点到目标点的最短路径，同时避开障碍物。它是在RRT（Rapidly-exploring Random Tree）算法基础上改进而来的，具备更高的路径质量与优化能力。RRT的核心特点是在搜索进程中逐步优化路径，最终可得到接近最短的路径。
2. 算法原理剖析
想象你身处一片森林，从某一点出发（起点），想要找到通往小屋（终点）的路径，而森林里有诸多树木（障碍物），你看不到小屋的具体位置。你开始随机朝着不同方向行进，并记录走过的路线，若碰到障碍就绕开，这便是RRT的基本思路：“快速且随机地扩展路径”。
而RRT在此基础上更为智能：每走一步，都会考量是否存在更短的路线来替换原先的路线*。
RRT*与RRT的差异
RRT算法和普通的RRT算法相比，最大的区别在于优化流程。RRT仅能快速探索空间，但无法保证找到最优路径。而RRT通过持续优化已找到的路径，能够保证找到一条接近最优的路径。
3. 算法步骤解析
RRT的核心思想是通过不断扩展一个随机树，对搜索空间进行探索，同时在扩展过程中优化路径。算法的基本步骤如下：
1. 初始化树：从起点出发，构建一个初始树，树的根节点为起点。
2. 随机采样：在空间中随机选取一个点，该点是机器人可能经过的位置。
3. 扩展树：找到树中距离该随机采样点最近的节点，然后朝着这个随机点扩展一小步，树会不断向外扩展以探索新区域。
4. 连接路径：在扩展树的过程中，算法会检查能否找到更优的路径。若新节点能够连接到树中某个更接近目标的节点，或者能够替换原有的路径，就会更新树的结构，以寻找更短的路径。
5. 优化路径*：算法会在树扩展的过程中反复优化路径，以确保最终路径是最优的。
步骤详细说明
假设存在一个机器人，起点在(0, 0)，目标点在(10, 10)，空间中有障碍物。下面来看RRT算法的工作过程：
- 初始化树：从起点(0, 0)开始构建初始树，此树仅有起点这一个节点。
- 随机采样：从空间中随机选取一个点，比如(7, 4)，该点不一定在树上，但期望树能通过某种方式扩展到该点。
- 扩展树：找到树中离(7, 4)最近的节点，例如起点(0, 0)，然后从(0, 0)朝着(7, 4)方向扩展，假设扩展步长为1，那么新节点的位置可能是(1, 0.57)，并将该新节点加入树中，使树规模增大。
- 检查路径优化：若新节点在扩展时发现了更短的路径，或者有可能替代之前的路径，算法会更新路径。比如，在扩展过程中发现从(1, 0.57)到目标点(10, 10)的路径比原路径更短，算法就会更新路径。
- 重复操作：算法会不断重复上述步骤，直到找到起点到目标点的路径。随着树的扩展，路径不断优化，最终得到一条最短路径。
- 结束*：当树扩展到目标点附近时，算法停止，输出路径。
4. RRT*的关键原理探究
RRT的核心原理不仅在于树的扩展，更在于每次扩展时进行路径优化。为保障路径的最优性，RRT会在每次扩展时检查能否找到更短的路径，这是一个渐进最优的过程。
1. 树的扩展
树的扩展是RRT的基础操作，即在空间中随机选取一个点并扩展树的结构。这一过程与RRT算法中的“树扩展”步骤类似，但RRT在扩展时增加了路径优化步骤。
给定一个随机采样点 ( q_{\text{rand}} )，树中某个节点 ( q_{\text{nearest}} )，RRT*的目标是从 ( q_{\text{nearest}} ) 朝着 ( q_{\text{rand}} ) 生成新节点 ( q_{\text{new}} )，计算公式为：
( q_{\text{new}} = q_{\text{nearest}} + \Delta t \cdot \frac{q_{\text{rand}} - q_{\text{nearest}}}{|q_{\text{rand}} - q_{\text{nearest}}|} )
其中，( \Delta t ) 是扩展步长，( q_{\text{new}} ) 是新节点。
计算新节点的代价：假设树中有一个节点 ( q_{\text{parent}} )，从该节点扩展到新节点 ( q_{\text{new}} )，则新节点的代价可定义为：
( C(q_{\text{new}}) = C(q_{\text{parent}}) + \text{cost}(q_{\text{parent}}, q_{\text{new}}) )
其中，( \text{cost}(q_{\text{parent}}, q_{\text{new}}) ) 是从父节点到新节点的代价，通常计算为两点之间的欧几里得距离：
( \text{cost}(q_{\text{parent}}, q_{\text{new}}) = |q_{\text{new}} - q_{\text{parent}}| )
2. 路径优化
在RRT*中，路径优化是至关重要的步骤，它能保证路径的最优性。优化过程的核心思想是检查新扩展的节点是否能通过某些已有节点形成更短的路径。具体而言，如果有两个节点 ( q_{\text{new}} ) 和 ( q_{\text{neighbour}} )，若经过 ( q_{\text{new}} ) 到 ( q_{\text{neighbour}} ) 的路径比原有路径更短，则更新路径。
在此过程中，RRT*需要计算从新节点到树中某些节点的路径长度，并选择能带来路径优化的节点。该步骤在每次扩展时都会进行，最终生成一条近似最优的路径。
假设新节点 ( q_{\text{new}} ) 通过与树中的节点 ( q_{\text{neighbour}} ) 连接来优化路径。若经过 ( q_{\text{neighbour}} ) 的路径比原路径更短，则更新路径。即若满足：
( C(q_{\text{neighbour}}) > C(q_{\text{new}}) + \text{cost}(q_{\text{new}}, q_{\text{neighbour}}) )
则将 ( q_{\text{neighbour}} ) 的父节点更新为 ( q_{\text{new}} )，并更新路径。
3. 最短路径连接
RRT*的优化不仅体现在路径本身，还包括通过连接新节点和树中的某些节点来生成最短路径。假设已有一条路径 ( \pi_1 ) 从起点到目标点，希望通过添加新节点来更新该路径。若新路径 ( \pi_2 ) 比原路径更短，则替换原路径。更新过程可通过“Rewiring”来实现，即调整树结构以缩短路径。
4. 渐进最优性
RRT的一个显著特性是其渐进最优性。随着扩展步数的增加，RRT会不断优化路径，最终趋近于最优路径。这种渐进最优性可通过以下公式描述：
在给定空间中，若对树进行足够多的扩展，路径的代价 ( C_{\text{path}} ) 会趋近于最优路径的代价 ( C^ )，满足：
( C_{\text{path}} \to C^ \quad \text{as} \quad n \to \infty )
其中，( n ) 是扩展的次数。
Python实现RRT*算法
RRT*算法是在一位大佬算法的基础上进行应用并做了少许修改。
```python
"""《RRT*算法实现》
时间：2025.06.25
"""
import math
import os
import sys
import matplotlib.pyplot as plt
from celluloid import Camera # 保存动图时用，pip install celluloid
sys.path.append("../RRT")
try:
from rrt_planning import RRT
except ImportError:
raise
show_animation = True
class RRTStar(RRT):
"""
Class for RRT Star planning
"""
class Node(RRT.Node):
def __init__(self, x, y):
super().__init__(x, y)
self.cost = 0.0
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=3.0,
goal_sample_rate=20,
max_iter=50000,
connect_circle_dist=50.0,
search_until_max_iter=False,
robot_radius=0.0):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
"""
super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
goal_sample_rate, max_iter,
robot_radius=robot_radius)
self.connect_circle_dist = connect_circle_dist
self.goal_node = self.Node(goal[0], goal[1])
self.search_until_max_iter = search_until_max_iter
def planning(self, animation=True, camera=None):
"""
rrt star path planning
animation: flag for animation on or off .
"""
self.node_list = [self.start]
for i in range(self.max_iter):
if i % 100 == 0:
print("Iter:", i, ", number of nodes:", len(self.node_list))
# print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.sample_free()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd,
self.expand_dis)
near_node = self.node_list[nearest_ind]
# 计算代价,欧氏距离
new_node.cost = near_node.cost + math.hypot(new_node.x - near_node.x, new_node.y - near_node.y)
if self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
near_inds = self.find_near_nodes(new_node) # 找到x_new的邻近节点
node_with_updated_parent = self.choose_parent(new_node, near_inds) # 重新选择父节点
# 如果父节点更新了
if node_with_updated_parent:
# 重布线
self.rewire(node_with_updated_parent, near_inds)
self.node_list.append(node_with_updated_parent)
else:
self.node_list.append(new_node)
if animation and i % 100 == 0:
self.draw_graph(rnd, camera)
if ((not self.search_until_max_iter) and new_node): # if reaches goal
last_index = self.search_best_goal_node()
if last_index is not None:
return self.generate_final_course(last_index)
print("reached max iteration")
last_index = self.search_best_goal_node()
if last_index is not None:
return self.generate_final_course(last_index)
return None
def choose_parent(self, new_node, near_inds):
"""
在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$，
作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选
我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价，
取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点
Arguments:
--------
new_node, Node
randomly generated node with a path from its neared point
There are not coalitions between this node and th tree.
near_inds: list
Indices of indices of the nodes what are near to new_node
Returns.
------
Node, a copy of new_node
"""
if not near_inds:
return None
# search nearest cost in near_inds
costs = []
for i in near_inds:
near_node = self.node_list[i]
t_node = self.steer(near_node, new_node)
if t_node and self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
costs.append(self.calc_new_cost(near_node, new_node))
else:
costs.append(float("inf")) # the cost of collision node
min_cost = min(costs)
if min_cost == float("inf"):
print("There is no good path.(min_cost is inf)")
return None
min_ind = near_inds[costs.index(min_cost)]
new_node = self.steer(self.node_list[min_ind], new_node)
new_node.cost = min_cost
return new_node
def search_best_goal_node(self):
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
goal_inds = [
dist_to_goal_list.index(i) for i in dist_to_goal_list
if i <= self.expand_dis
]
safe_goal_inds = []
for goal_ind in goal_inds:
t_node = self.steer(self.node_list[goal_ind], self.goal_node)
if self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
safe_goal_inds.append(goal_ind)
if not safe_goal_inds:
return None
min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
for i in safe_goal_inds:
if self.node_list[i].cost == min_cost:
return i
return None
def find_near_nodes(self, new_node):
"""
1) defines a ball centered on new_node
2) Returns all nodes of the three that are inside this ball
Arguments:
---------
new_node: Node
new randomly generated node, without collisions between
its nearest node
Returns:
-------
list
List with the indices of the nodes inside the ball of
radius r
"""
nnode = len(self.node_list) + 1
r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
# if expand_dist exists, search vertices in a range no more than
# expand_dist
if hasattr(self, 'expand_dis'):
r = min(r, self.expand_dis)
dist_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2
for node in self.node_list]
near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
return near_inds
def rewire(self, new_node, near_inds):
"""
For each node in near_inds, this will check if it is cheaper to
arrive to them from new_node.
In such a
相关文章
