1. 项目概述从随机采样到确定路径在机器人、自动驾驶乃至游戏AI的寻路逻辑里路径规划始终是核心挑战。想象一下你要让一个机器人在一个布满障碍物的仓库里从A点移动到B点。传统的网格搜索法比如A*算法需要把整个空间划分成一个个小格子然后逐个搜索在复杂或高维空间里计算量会爆炸。而今天要聊的快速扩展随机树则是一种截然不同的思路它不试图穷尽整个空间而是像一棵不断生长的树通过随机采样来探索未知区域高效地找到一条可行路径。我第一次接触RRT是在做一个机械臂避障项目时当时用A*在三维关节空间里规划速度慢得让人抓狂。直到尝试了RRT才发现这种“随机生长”的方式在高维空间里有多么巨大的优势。它本质上是一种基于采样的概率完备算法意思是只要时间足够长它几乎肯定能找到一条路径如果存在的话。虽然找到的路径通常不是最优的但“有”和“快”往往是工程实践中的首要考量后续我们可以再对这条初始路径进行平滑优化。这个项目我们将深入RRT的核心原理并用MATLAB和Python两种语言实现一个基础的二维路径规划仿真。你会看到从一棵树、一个随机点开始如何一步步“探索”出通往目标的道路。这不仅是一个算法实现更是一种解决复杂空间搜索问题的思维范式。2. RRT算法核心原理与设计思路拆解2.1 为什么是“快速扩展随机树”要理解RRT得先拆解它的名字。快速扩展指的是它的生长策略每次迭代都试图向一个随机点方向迈出尽可能大的一步受步长限制这使它能够迅速覆盖大片未探索区域而不是在局部精细搜索。随机树则描述了它的数据结构整个探索过程形成一棵树树根是起点每个树枝的末端树节点都代表一个已经被探索过的、无碰撞的位姿位置和姿态。随机性体现在采样上算法不断地在自由空间无障碍物区域中随机撒点引导树的生长方向。这种设计思路直接针对了高维空间规划的两个痛点维度灾难在机械臂的6维或7维关节空间中网格法的节点数呈指数级增长。RRT通过随机采样避免了显式地对整个空间进行离散化从而绕开了维度灾难。计算效率它不追求一次性找到最优解而是优先保证在可接受的时间内找到一个可行解。这种“可行解优先”的策略在实时性要求高的场景如自动驾驶的紧急避障中非常关键。算法的基本流程可以概括为一个循环在规划空间内随机采样一个点q_rand。在当前树的所有节点中找到距离q_rand最近的那个节点q_near。从q_near朝着q_rand的方向以预设的步长step_size生长一段距离得到一个新节点q_new。检查从q_near到q_new的这段路径是否与障碍物发生碰撞。如果无碰撞则将q_new加入树中作为q_near的子节点。重复上述过程直到q_new进入了目标点的邻域范围内则认为路径找到。注意这里有一个关键细节q_rand是纯粹随机采样的这保证了算法探索的全局性。但为了提高收敛到目标的速度实际实现中通常会采用“目标偏置采样”即以一个小概率如5%直接采样目标点作为q_rand引导树向目标生长。2.2 与A*等传统算法的本质区别为了更清晰地理解RRT的定位我们可以将其与A*算法做一个对比特性维度A* 算法RRT 算法空间表示显式离散化网格、图隐式连续空间采样搜索策略确定性的启发式搜索如Dijkstra的扩展概率性的随机采样搜索完备性在离散空间内是完备的一定能找到最优解概率完备的时间趋于无穷则找到解概率为1最优性可以找到全局最优路径当启发函数可采纳时通常只能找到可行路径非最优适用维度低维空间2D, 3D网格表现优异尤其擅长高维空间3维计算效率在状态空间大时开放列表维护成本高无需维护全局开放列表每次迭代计算量相对固定路径输出由一系列网格中心点组成可能不平滑由树节点连线组成通常需要后处理平滑从对比可以看出RRT和A是两种哲学。A像是有一个详细地图的规划师会仔细计算每条路的成本而RRT更像一个在陌生森林里的探险家通过不断向随机方向扔石头听回响来摸索出一条能走的路。在机器人学中我们经常将两者结合用RRT在关节空间进行粗规划再用优化方法对路径进行平滑和优化。2.3 算法变种与改进方向基础RRT虽然有效但也有很多可以优化的地方由此衍生出许多变种RRT-Connect同时从起点和目标点生长两棵树交替进行扩展和连接尝试能显著提高收敛速度。RRT*这是RRT的“最优”版本。它在加入新节点q_new后还会在其附近邻域内寻找是否存在更优的“父节点”使得从起点到q_new的路径成本更低并执行“重布线”操作优化树的结构。随着采样点增多RRT* 的路径会渐进收敛到最优解。Informed RRT*在找到一条初始路径后它将采样范围限制在一个以起点和终点为焦点的椭圆或超椭球内因为这个区域外的点不可能提供更优的路径从而大幅提升后续优化的采样效率。在我们的基础实现中我们聚焦于最原始的RRT理解其骨架。掌握了它你就能轻松理解这些更高级的变种。3. MATLAB实战一步步构建RRT路径规划器3.1 环境与问题定义我们首先在MATLAB中搭建一个简单的二维仿真环境。假设我们有一个100x100单位的工作空间里面有几个多边形障碍物。机器人的起点是[10, 10]目标是[90, 90]。机器人在这个空间里可以被视为一个点点机器人模型或者一个圆形便于碰撞检测。我们这里采用点模型但碰撞检测时需要考虑机器人的半径。% 1. 初始化环境 clear; clc; close all; % 定义工作空间边界 xlim_range [0, 100]; ylim_range [0, 100]; % 定义起点和终点 start [10, 10]; goal [90, 90]; goal_radius 5; % 认为进入目标点周围此半径内即算到达 % 定义障碍物 (每个障碍物用一组顶点表示这里是矩形和三角形) obstacles { [30, 30; 30, 70; 70, 70; 70, 30], % 矩形障碍物 [10, 50; 40, 80; 70, 50] % 三角形障碍物 }; % 绘制环境 figure(1); hold on; axis equal; grid on; xlim(xlim_range); ylim(ylim_range); plot(start(1), start(2), go, MarkerSize, 10, MarkerFaceColor, g); plot(goal(1), goal(2), ro, MarkerSize, 10, MarkerFaceColor, r); for i 1:length(obstacles) obs obstacles{i}; fill(obs(:,1), obs(:,2), k, FaceAlpha, 0.3, EdgeColor, k); end title(RRT Path Planning Environment); xlabel(X); ylabel(Y);3.2 核心函数实现采样、最近邻、碰撞检测接下来是实现算法的三个核心函数。1. 随机采样函数这个函数在规划空间内生成一个随机点。为了提高效率我们加入一个小的目标偏置概率。function q_rand sample_point(xlim, ylim, goal, goal_bias) % 在空间内随机采样一个点 % goal_bias: 目标偏置概率例如0.05表示有5%的概率直接返回目标点 if rand() goal_bias q_rand goal; else q_rand [xlim(1) (xlim(2)-xlim(1))*rand(), ... ylim(1) (ylim(2)-ylim(1))*rand()]; end end2. 最近邻查找函数需要从当前树的所有节点中找到距离随机点q_rand欧氏距离最近的那个节点。这是RRT中计算量较大的部分如果节点数很多可以考虑使用KD-Tree等数据结构加速。我们这里先用简单遍历实现。function [q_near, idx] nearest_neighbor(tree, q_rand) % 在树的所有节点中查找离q_rand最近的节点 % tree: Nx2矩阵每一行是一个节点坐标[x, y] % q_rand: 1x2向量 % q_near: 最近的节点坐标 % idx: 最近节点在tree中的行索引 distances sqrt(sum((tree - q_rand).^2, 2)); % 计算所有节点到q_rand的距离 [~, idx] min(distances); q_near tree(idx, :); end3. 碰撞检测函数这是路径规划的灵魂决定了规划的安全性。我们需要检查两点连成的线段是否与任何障碍物相交。对于多边形障碍物可以转化为检查线段是否与多边形的任何边相交。这里我们实现一个简单的线段-多边形相交检测。更稳健的做法是使用MATLAB自带的polyxpoly函数。function collision check_collision(q1, q2, obstacles) % 检查线段q1-q2是否与障碍物集合中的任何一个相交 % q1, q2: 线段的两个端点 [x, y] % obstacles: 细胞数组每个元素是一个多边形顶点矩阵 % collision: true表示发生碰撞 collision false; for i 1:length(obstacles) poly obstacles{i}; % 检查线段与多边形每条边是否相交 for j 1:size(poly,1) p1 poly(j, :); p2 poly(mod(j, size(poly,1)) 1, :); % 下一个顶点形成闭环 % 调用线段相交判断函数 if is_lines_intersect(q1, q2, p1, p2) collision true; return; end end % 可选额外检查点是否在多边形内部针对起点或终点在障碍物内的情况 % if inpolygon(q1(1), q1(2), poly(:,1), poly(:,2)) || ... % inpolygon(q2(1), q2(2), poly(:,1), poly(:,2)) % collision true; % return; % end end end function intersect is_lines_intersect(p1, p2, p3, p4) % 使用向量叉积法判断两条线段p1p2和p3p4是否相交 % 参考快速排斥实验 跨立实验 intersect false; % 快速排斥实验 if max(p1(1),p2(1)) min(p3(1),p4(1)) || max(p3(1),p4(1)) min(p1(1),p2(1)) || ... max(p1(2),p2(2)) min(p3(2),p4(2)) || max(p3(2),p4(2)) min(p1(2),p2(2)) return; end % 跨立实验 if (((p1(1)-p3(1))*(p4(2)-p3(2)) - (p1(2)-p3(2))*(p4(1)-p3(1))) * ... ((p2(1)-p3(1))*(p4(2)-p3(2)) - (p2(2)-p3(2))*(p4(1)-p3(1))) 0) || ... (((p3(1)-p1(1))*(p2(2)-p1(2)) - (p3(2)-p1(2))*(p2(1)-p1(1))) * ... ((p4(1)-p1(1))*(p2(2)-p1(2)) - (p4(2)-p1(2))*(p2(1)-p1(1))) 0) return; end intersect true; end实操心得碰撞检测的精度和效率是路径规划器的关键。在复杂或动态环境中可能需要分层检测先粗检后精检或使用预先计算好的距离场。对于圆形机器人可以将障碍物进行“膨胀”Minkowski Sum处理然后将机器人视为点来处理这会大大简化碰撞检测逻辑。3.3 主循环与路径提取将上述模块组合起来形成RRT的主算法循环。% 2. RRT算法参数设置 max_iter 5000; % 最大迭代次数 step_size 5.0; % 扩展步长 goal_bias 0.05; % 目标偏置概率 % 3. 初始化树 tree start; % 树节点集合每一行是一个节点 parent 0; % 父节点索引集合根节点起点的父节点为0 goal_reached false; path []; % 最终路径 % 4. 主循环 for iter 1:max_iter % 4.1 随机采样 q_rand sample_point(xlim_range, ylim_range, goal, goal_bias); % 4.2 寻找最近邻 [q_near, idx_near] nearest_neighbor(tree, q_rand); % 4.3 向随机点方向生长 direction q_rand - q_near; distance norm(direction); if distance 0 direction direction / distance; % 单位化 q_new q_near direction * min(step_size, distance); % 步长限制 else continue; % 如果随机点就是最近点跳过 end % 4.4 碰撞检测 if ~check_collision(q_near, q_new, obstacles) % 无碰撞将新节点加入树 tree [tree; q_new]; parent [parent; idx_near]; % 可视化生长过程可选每100次画一次避免图形卡顿 if mod(iter, 100) 0 plot([q_near(1), q_new(1)], [q_near(2), q_new(2)], b-, LineWidth, 0.5); drawnow limitrate; end % 4.5 检查是否到达目标区域 if norm(q_new - goal) goal_radius disp([目标在迭代 , num2str(iter), 次时到达]); goal_reached true; % 回溯路径 path q_new; current_idx size(tree, 1); % 当前节点即q_new的索引 while current_idx ~ 1 current_idx parent(current_idx); path [tree(current_idx, :); path]; end break; end end end % 5. 结果可视化 if goal_reached % 绘制最终路径 plot(path(:,1), path(:,2), r-, LineWidth, 2); plot(tree(:,1), tree(:,2), b., MarkerSize, 5); % 绘制所有树节点 title([RRT Path Found (Iterations: , num2str(iter), )]); else title(RRT Failed to Find Path within Max Iterations); end运行这段代码你会看到一棵蓝色的树从绿色起点开始生长逐渐蔓延至整个空间直到有一条树枝触及红色目标点周围最终形成一条红色的路径。步长step_size是一个关键参数太大可能导致碰撞检测失败率高穿过狭窄通道能力差太小则生长缓慢探索效率低。通常需要根据环境尺度进行调整。4. Python复现面向对象与可视化增强用Python实现RRT我们可以采用更面向对象的方式并且利用matplotlib的动画功能直观展示树的生长过程。这对于教学和调试非常有帮助。4.1 定义RRT规划器类我们将算法封装成一个类提高代码的可复用性和可读性。import numpy as np import matplotlib.pyplot as plt import matplotlib.patches as patches from matplotlib.animation import FuncAnimation class RRTPlanner: def __init__(self, start, goal, obstacles, xlim, ylim, step_size5.0, goal_radius5.0, max_iter5000, goal_bias0.05): self.start np.array(start) self.goal np.array(goal) self.obstacles obstacles # list of polygon vertices self.xlim xlim self.ylim ylim self.step_size step_size self.goal_radius goal_radius self.max_iter max_iter self.goal_bias goal_bias # 树结构用列表存储节点和父节点索引 self.tree_nodes [self.start] # 节点列表 self.tree_parents [-1] # 父节点索引列表-1表示根节点 self.path None self.goal_reached False def sample(self): 随机采样一个点 if np.random.rand() self.goal_bias: return self.goal else: return np.array([np.random.uniform(self.xlim[0], self.xlim[1]), np.random.uniform(self.ylim[0], self.ylim[1])]) def nearest(self, q_rand): 找到树中离q_rand最近的节点 nodes_array np.array(self.tree_nodes) distances np.linalg.norm(nodes_array - q_rand, axis1) idx np.argmin(distances) return nodes_array[idx], idx def steer(self, q_near, q_rand): 从q_near向q_rand方向生长一步 direction q_rand - q_near dist np.linalg.norm(direction) if dist 0: direction direction / dist q_new q_near direction * min(self.step_size, dist) return q_new else: return q_near def is_collision_free(self, q1, q2): 检查线段q1q2是否与任何障碍物相交 for obstacle in self.obstacles: poly np.array(obstacle) # 检查与多边形每条边是否相交 for i in range(len(poly)): p1 poly[i] p2 poly[(i1) % len(poly)] # 下一个顶点形成闭环 if self._segments_intersect(q1, q2, p1, p2): return False return True def _segments_intersect(self, a1, a2, b1, b2): 判断线段a1a2和b1b2是否相交向量叉积法 def ccw(A, B, C): return (C[1]-A[1]) * (B[0]-A[0]) (B[1]-A[1]) * (C[0]-A[0]) return ccw(a1, b1, b2) ! ccw(a2, b1, b2) and ccw(a1, a2, b1) ! ccw(a1, a2, b2) def plan(self, animationFalse): 执行RRT规划主循环 fig, ax plt.subplots(figsize(8,8)) self._plot_environment(ax) if animation: line_tree, ax.plot([], [], b-, lw0.5, alpha0.6) # 用于动态绘制树枝 line_path, ax.plot([], [], r-, lw2) # 用于绘制最终路径 nodes_scatter ax.scatter([], [], cb, s5) # 用于绘制树节点 def update(frame): if self.goal_reached or frame self.max_iter: ani.event_source.stop() # 找到路径或达到最大迭代则停止动画 if self.goal_reached: # 绘制最终路径 path_array np.array(self.path) line_path.set_data(path_array[:,0], path_array[:,1]) return line_tree, line_path, nodes_scatter # 一次RRT迭代 q_rand self.sample() q_near, idx_near self.nearest(q_rand) q_new self.steer(q_near, q_rand) if self.is_collision_free(q_near, q_new): self.tree_nodes.append(q_new) self.tree_parents.append(idx_near) # 更新动画数据 x_data [q_near[0], q_new[0]] y_data [q_near[1], q_new[1]] # 累积绘制所有树枝简单实现实际应更新数据列表 # 这里为简化我们直接在当前轴上画线 ax.plot(x_data, y_data, b-, lw0.5, alpha0.6) # 更新节点散点图 nodes_array np.array(self.tree_nodes) nodes_scatter.set_offsets(nodes_array) # 检查是否到达目标 if np.linalg.norm(q_new - self.goal) self.goal_radius: self.goal_reached True self._extract_path(len(self.tree_nodes)-1) # 提取路径 print(f目标在迭代 {frame1} 次时到达) return line_tree, line_path, nodes_scatter ani FuncAnimation(fig, update, framesself.max_iter, interval10, blitFalse, repeatFalse) plt.show() else: # 非动画模式快速运行 for iter in range(self.max_iter): q_rand self.sample() q_near, idx_near self.nearest(q_rand) q_new self.steer(q_near, q_rand) if self.is_collision_free(q_near, q_new): self.tree_nodes.append(q_new) self.tree_parents.append(idx_near) # 每100次迭代绘制一次树避免图形卡顿 if iter % 100 0: ax.plot([q_near[0], q_new[0]], [q_near[1], q_new[1]], b-, lw0.5, alpha0.6) if np.linalg.norm(q_new - self.goal) self.goal_radius: self.goal_reached True self._extract_path(len(self.tree_nodes)-1) print(f目标在迭代 {iter1} 次时到达) break # 绘制最终结果 if self.goal_reached: path_array np.array(self.path) ax.plot(path_array[:,0], path_array[:,1], r-, lw2, labelFinal Path) ax.scatter([node[0] for node in self.tree_nodes], [node[1] for node in self.tree_nodes], cb, s5, alpha0.5, labelTree Nodes) ax.legend() plt.show() return self.path def _extract_path(self, goal_idx): 从目标节点回溯到起点提取路径 path [self.tree_nodes[goal_idx]] current_idx goal_idx while self.tree_parents[current_idx] ! -1: current_idx self.tree_parents[current_idx] path.append(self.tree_nodes[current_idx]) path.reverse() self.path path def _plot_environment(self, ax): 绘制规划环境 ax.set_xlim(self.xlim) ax.set_ylim(self.ylim) ax.grid(True, whichboth, linestyle--, alpha0.7) ax.set_aspect(equal) ax.set_xlabel(X) ax.set_ylabel(Y) ax.set_title(RRT Path Planning) # 绘制起点和终点 ax.plot(self.start[0], self.start[1], go, markersize10, labelStart, markeredgecolork) ax.plot(self.goal[0], self.goal[1], ro, markersize10, labelGoal, markeredgecolork) # 绘制障碍物 for obstacle in self.obstacles: poly patches.Polygon(obstacle, closedTrue, facecolorgray, alpha0.5, edgecolork) ax.add_patch(poly) ax.legend() # 使用示例 if __name__ __main__: # 定义环境与MATLAB示例一致 start (10, 10) goal (90, 90) obstacles [ np.array([[30,30], [30,70], [70,70], [70,30]]), # 矩形 np.array([[10,50], [40,80], [70,50]]) # 三角形 ] xlim (0, 100) ylim (0, 100) # 创建规划器并执行规划开启动画 planner RRTPlanner(start, goal, obstacles, xlim, ylim, step_size5.0, max_iter3000) path planner.plan(animationTrue) # 设置 animationFalse 可快速运行 if path: print(路径规划成功) print(f路径节点数{len(path)}) else: print(未能在最大迭代次数内找到路径。)这个Python实现将整个RRT规划过程封装成了一个类RRTPlanner。plan方法中的animation参数允许你选择是否观看树生长的动态过程。动态可视化能让你清晰地看到RRT如何探索空间特别是在狭窄通道处如何反复尝试最终找到突破口。4.2 关键参数调优与影响分析无论是MATLAB还是Python实现以下几个参数对算法性能有决定性影响需要根据具体场景调整步长step_size太大探索速度快但可能“穿过”狭窄通道导致碰撞检测失败率高在复杂环境中容易失败。太小探索精细能通过狭窄区域但生长缓慢规划时间长。调优建议初始值可以设为环境对角线长度的2%~5%。对于有狭窄通道的环境可以尝试动态步长在开阔区域用大步长接近障碍物时用小步长。目标偏置概率goal_bias太大如0.2树会过于贪婪地冲向目标可能忽略对关键区域的探索在障碍物复杂时容易陷入局部死胡同。太小如0完全随机探索收敛到目标的速度慢但探索更全面。调优建议通常设置在0.05到0.1之间是一个较好的平衡。也可以设计自适应偏置例如随着迭代次数增加而略微提高。最大迭代次数max_iter这是算法的安全阀。设置太小可能在找到路径前就停止了设置太大在无解环境中会浪费计算时间。调优建议可以根据环境大小和复杂度经验性设置。一个实用的技巧是同时设置一个最大运行时间限制。最近邻搜索效率当树节点超过几千个时线性遍历查找最近邻会成为性能瓶颈。强烈建议在Python实现中集成scipy.spatial.cKDTree或sklearn.neighbors.KDTree来加速查询这是工程应用中的必备优化。# 使用scipy的cKDTree加速最近邻搜索示例片段 from scipy.spatial import cKDTree # 在类初始化时 self.kd_tree None self._rebuild_tree() # 初始化构建树 def _rebuild_tree(self): 重建KD-Tree if len(self.tree_nodes) 0: self.kd_tree cKDTree(self.tree_nodes) def nearest(self, q_rand): 使用KD-Tree查找最近邻 if self.kd_tree is not None: dist, idx self.kd_tree.query(q_rand, k1) return self.tree_nodes[idx], idx else: # 回退到线性搜索 nodes_array np.array(self.tree_nodes) distances np.linalg.norm(nodes_array - q_rand, axis1) idx np.argmin(distances) return nodes_array[idx], idx # 注意每次添加新节点后需要调用 _rebuild_tree() 或使用增量更新更复杂。5. 常见问题、调试技巧与进阶思考5.1 算法运行失败的可能原因与排查在实际运行中你可能会遇到算法找不到路径的情况。别急着怀疑算法按以下步骤排查检查碰撞检测这是最常见的问题源。绘制出每次被拒绝的q_near-q_new线段用浅红色虚线看看它们是否真的与障碍物相交或者你的碰撞检测函数是否有误判特别是多边形边界的处理。确保你的障碍物顶点顺序是顺时针或逆时针一致的。检查起点/终点是否在障碍物内一个常见的疏忽是起点或终点本身就设置在障碍物内部。可以在初始化后立即用inpolygon(MATLAB) 或射线法 (Python) 检查一下。调整步长如果环境中有狭窄的通道宽度为w你的步长step_size必须显著小于w否则新节点很容易“跳过”通道口导致算法永远找不到通过的路。尝试将步长减小到通道宽度的1/3或更小。增加迭代次数对于复杂环境5000次迭代可能不够。尝试增加到10000或20000次。同时观察树的生长情况如果树已经覆盖了大部分空间但仍未到达目标可能是目标区域被障碍物完全封闭或者存在极其狭窄的路径。检查随机采样范围确保你的采样函数sample_point确实覆盖了整个自由空间没有因为边界设置错误而漏掉了某些区域。5.2 路径后处理从可行到“好用”RRT找到的路径通常是锯齿状的因为它是随机采样连接的。这样的路径不适合机器人直接跟踪。我们需要进行后处理路径修剪遍历路径上的节点尝试连接不相邻的节点如path[i]和path[i3]如果连线无碰撞则删除中间的所有节点。这可以缩短路径拉直一些弯折。路径平滑使用曲线拟合技术如B样条曲线或贝塞尔曲线对路径点进行平滑生成一条连续且曲率可控的轨迹。更简单的方法是使用梯度下降平滑将路径节点作为控制点定义一个包含碰撞代价和光滑度代价的损失函数然后迭代调整节点位置以最小化损失。# 一个简单的路径修剪函数示例 def simplify_path(path, obstacles): 对路径进行修剪尝试连接更远的点以缩短路径 if len(path) 3: return path simplified [path[0]] i 0 while i len(path) - 1: for j in range(len(path)-1, i, -1): if not check_collision(path[i], path[j], obstacles): # 复用碰撞检测函数 simplified.append(path[j]) i j break else: # 如果没有找到可连接的点则连接到下一个点 simplified.append(path[i1]) i 1 return simplified5.3 从二维到高维关节空间规划我们演示的是二维平面上的点机器人。在机械臂规划中状态空间是关节角度空间例如6维。将上述算法扩展到高维非常简单状态表示将[x, y]替换为关节角度向量[theta1, theta2, ..., theta6]。距离度量欧氏距离可能不再适用。关节空间的距离需要考虑每个关节的运动范围和物理意义通常使用加权欧氏距离或曼哈顿距离。碰撞检测这是最复杂的部分。需要有一个机器人模型和环境的3D表示。对于每个候选的关节角度q_new需要使用正向运动学计算出末端执行器和所有连杆在三维空间中的位置然后与三维环境中的障碍物进行碰撞检测。这通常依赖于物理引擎如Bullet, FCL或简化的包围盒检测。采样在关节角度的上下限内随机采样。尽管碰撞检测变复杂了但RRT算法的框架完全不变。这也是它强大的地方——算法逻辑与维度无关。5.4 工程实践中的注意事项确定性 vs 随机性RRT是随机算法每次运行结果都可能不同。在需要确定性的工业应用中可以固定随机数种子但这会牺牲探索的全局性。更好的做法是运行多次选择最优最短、最平滑的路径或者使用RRT*这类渐进最优的变种。动态环境基础RRT适用于静态环境。对于动态环境需要引入重规划策略。例如可以定期检查当前路径是否仍然无碰撞如果发生碰撞则以机器人当前位置为新的起点重新运行RRT或者使用动态RRT变种。实时性要求如果规划时间要求非常严格如无人机避障可以考虑设置一个时间预算。当预算时间用完时即使未找到完整路径也可以输出当前树中离目标最近的点所对应的路径作为一条“次优”但及时的参考轨迹。我个人在多个机器人项目中使用RRT及其变种的体会是它更像一个“探索框架”而非一个“死板的算法”。理解其核心思想随机采样、最近邻扩展、碰撞检测后你可以根据具体问题灵活调整它的每一个组件采样策略如在高概率区域增加采样密度、距离度量、步长策略、甚至树的生长方式如双向RRT-Connect。它可能不是最快或最优的但其简单性、通用性以及对高维问题的处理能力使其成为机器人路径规划工具箱中不可或缺的利器。最后一个小技巧在调试时将树、采样点、被拒绝的路径都可视化出来是理解算法行为、定位问题最快的方式。