4. RRT*与渐进最优性：从“找到路径”到“找到好路径”
上一章我们聊了RRT，它能快速找到一条路径。但说实话，很多时候找到的那条路，弯弯绕绕的，看着就难受。我刚开始做机械臂抓取时，RRT规划出来的路径经常让机械臂在空间里扭成麻花，虽然能抓到，但效率低，关节磨损也大。
这时候，RRT*就派上用场了。它不光是找到路径，还要让路径越来越好。说白了，就是给你一个“越用越优”的承诺。
4.1 RRT*的核心思想：边探索边优化
RRT*和RRT最大的区别在哪？我总结为一句话：RRT只管连上，RRT*还要管连得好不好。
它做了两件关键的事：
- 选择父节点时精打细算：新节点生成后，不是直接连到最近的节点，而是在附近找一圈，选一个能让路径代价最小的节点当爹。
- 重布线（Rewire）：新节点加入后，看看能不能让附近的旧节点改认它当爹，从而降低整棵树的代价。
你想想看，这就像修路。RRT是随便挖一条路能走就行，RRT*则是每修一段路，都要回头看看能不能把之前的路改得更直、更短。
核心理解：RRT*的“*”号，代表的是渐进最优性。随着采样点无限增多，规划出的路径会无限逼近全局最优解。这不是玄学，是有数学证明的。
4.2 RRT*算法流程：三步走
我个人习惯把RRT*的迭代过程拆成三步，这样逻辑更清晰。
步骤一：采样与生长
这一步和RRT一样。在状态空间随机采样一个点，找到树上最近的节点，然后朝采样点方向生长一段距离，得到新节点 x_new。
步骤二：寻找最优父节点
这里开始不一样了。以 x_new 为中心，画一个半径为 r 的圆（或者用k近邻），找出圆内所有已有的节点。然后挨个算：如果 x_new 连到这些节点上，路径代价是多少？选代价最小的那个当父节点。
def find_best_parent(tree, x_new, radius):
near_nodes = tree.get_nodes_in_radius(x_new, radius)
best_parent = None
min_cost = float('inf')
for node in near_nodes:
# 检查连线是否碰撞
if not collision_check(node.position, x_new.position):
cost = node.cost + distance(node.position, x_new.position)
if cost < min_cost:
min_cost = cost
best_parent = node
return best_parent, min_cost避坑指南：我曾经在调参时把搜索半径设得太小，结果RRT*退化成了RRT，完全没发挥出重布线的优势。建议半径至少设为采样步长的2-3倍，具体要看状态空间的维度。
步骤三：重布线（Rewire）
这是RRT*的精髓。新节点 x_new 加入后，检查附近那些旧节点。如果 x_new 能成为它们的父节点，并且让它们的路径代价变小，那就改！
def rewire(tree, x_new, near_nodes):
for node in near_nodes:
# 如果x_new到node的路径更短且无碰撞
new_cost = x_new.cost + distance(x_new.position, node.position)
if new_cost < node.cost:
if not collision_check(x_new.position, node.position):
# 断开旧父节点，连接新父节点
tree.remove_edge(node.parent, node)
tree.add_edge(x_new, node)
node.cost = new_cost
node.parent = x_new嗯，这里要注意：重布线会改变树的结构，但不会破坏树的连通性。每次改动，整棵树依然是一棵以起点为根的树。
4.3 代价函数：怎么才算“好”？
RRT*里的一切优化，都围绕着一个东西——代价函数。最常见的代价函数就是路径长度。但实际项目中，我经常需要定制。
| 代价类型 | 计算公式 | 适用场景 |
|---|---|---|
| 欧氏距离 | ||p1 - p2|| | 自由空间无障碍，追求最短路径 |
| 加权距离 | w1*dx + w2*dy + w3*dz | 不同轴运动代价不同（如重力方向） |
| 能量代价 | Σ(关节力矩²) | 需要节能或减少关节磨损 |
| 平滑度代价 | Σ(曲率²) | 要求路径平滑，避免急转弯 |
我在做协作机器人抓取时，遇到过一个问题：单纯用欧氏距离，规划出的路径虽然短，但机械臂末端会突然加速减速，导致工件晃动。后来我把平滑度代价加进去，路径虽然长了5%，但运动平稳多了。
注意：代价函数必须是单调递增的，即从起点到终点的路径代价，不能出现“走回头路反而更便宜”的情况。否则RRT*的渐进最优性证明就不成立了。
4.4 渐进最优性证明思路：为什么RRT*能保证越来越好？
这部分有点数学，但我尽量用大白话讲。你不需要记住全部推导，理解核心逻辑就行。
渐进最优性说的是：当采样点数 n → ∞ 时，规划出的路径代价会收敛到全局最优值。
证明的核心思路分三步：
- 稠密性：随着采样点增多，状态空间会被“填满”。任何一条最优路径上的点，都能被采样点无限逼近。
- 连通性：RRT*的重布线机制保证，只要采样点足够密，树就能覆盖到最优路径附近。
- 代价收敛：因为每次重布线都在降低局部代价，而全局最优路径的代价是下界，所以最终会收敛到这个下界。
说白了，就是采样越多，树越密，路径越直。我当年第一次看到这个证明时，觉得挺妙的——它不依赖任何启发式信息，纯粹靠随机采样和局部优化就达到了全局最优。
关键条件：渐进最优性成立需要两个前提——
1. 采样是均匀随机的（或至少是稠密的）
2. 重布线的搜索半径 r 要足够大，通常取 r = γ * (log(n)/n)^(1/d)，其中 d 是状态空间维度，γ 是常数。
4.5 RRT* vs RRT：一张图看懂区别
4.6 实际项目中的RRT*调参经验
理论讲完了，说点实在的。我在实际部署RRT*时，踩过不少坑，这里分享几个关键点：
- 搜索半径 r 的选取：理论上 r 要随采样数变化，但实际中我常用固定半径。对于二维平面，r 取步长的3倍；对于六维关节空间，r 取步长的5-8倍。太小了重布线没效果，太大了计算量爆炸。
- 碰撞检测的频率：RRT*的碰撞检测次数是RRT的2-3倍，因为每次重布线都要做碰撞检测。我建议用增量式碰撞检测，只检测新加入的线段，不要每次都从头检测整棵树。
- 终止条件：不要等采样到无穷大。实际中我设两个条件：要么找到一条可行路径后，再优化N次（比如500次）；要么路径代价在最近M次迭代中变化小于阈值，就提前终止。
个人技巧：我习惯在RRT*的代价函数里加一个很小的随机扰动项（比如0.01 * 随机数），这样能避免路径陷入局部最优的“死胡同”。虽然理论上破坏了单调性，但实践中效果很好。
4.7 小结
RRT*是RRT的进化版，它用“选最优父节点”和“重布线”两个操作，换来了渐进最优性的保证。代价是计算量大了不少，但换来的是路径质量的持续提升。
我个人觉得，如果你的应用场景对路径质量要求高（比如精密装配、医疗手术），或者你有充足的计算资源，RRT*是比RRT更好的选择。但如果只是快速找一个可行路径，RRT就够用了。
嗯，这一章的内容就到这。记住那个核心思想：RRT*不是一次性找到最优解，而是保证随着时间推移，解会越来越好。这个“渐进”的思路，在很多工程问题里都很有启发。