先放一个结论在三维空间里裸跑 RRT*效果通常不会太好。我前两年做无人机巡检路径规划项目的时候被这个经典算法折磨过100×100×100 的地图迭代 5000 次首条可行路径拐了十几个弯拿到手还得再套几层平滑才敢给飞控用。后来我把人工势场法的“目标引力 障碍物斥力”塞进双向 RRT* 的采样和生长方向里收敛速度肉眼可见地变快路径也顺眼很多。这篇文章就把这套“改进双向人工势场引导 RRT*”的思路、Matlab 实现、以及调参踩坑的完整过程从头到尾拆一遍。这套方案解决的是三维无人机路径规划里最常见的两件事一是 RRT* 随机采样太发散在开阔空间里浪费时间二是人工势场法虽然方向明确但容易撞进局部极小值出不来。两者结合之后随机性负责跳出陷阱势场负责指路双向生长负责把搜索半径砍半适合正在做空中机器人航迹规划、AGV 全局规划或者做课程设计需要快速跑通一个能看的路径规划算法的同学。我不会只贴代码会重点讲清楚每一步为什么要这么改参数怎么调以及我在实际仿真里看到的各种反直觉现象。1. 为什么三维规划里裸跑 RRT* 又慢又绕先盘两个真实痛点1.1 随机采样在三维空间里的浪费效应RRT 系列算法的核心思路很简单在状态空间里随机撒点把点连接到已有树上一点点长出一棵覆盖可达空间的树。二维地图里这招挺好使因为平面空间本身不大撒几百个点就能覆盖主要区域。可一旦升到三维问题就变味了。假设地图是 100×100×100 的立方体随机撒一个点落在目标点附近半径为 5 的球形区域内的概率大约是(4/3 × π × 5³) / (100³) ≈ 0.0052也就是两千次随机采样里大概只有十几次能真正靠近目标。这意味着绝大部分迭代都在“漫无目的地探索”。RRT* 的渐近最优性是用迭代量换来的可三维空间里的迭代量消耗会让人怀疑人生。有同学会说那我把采样点直接偏到目标附近不就行了这就是经典的 goal bias。确实有效但有个副作用如果目标方向恰好被障碍物挡住高偏置率的采样会让树反复向同一个被挡住的区域生长每次都在碰撞检测阶段被拒白白浪费大量迭代有时候还会让树卡在障碍物边缘绕圈。我在实测里见过最夸张的情况普通 RRT* 跑了 4000 次迭代首条路径长度是最终平滑路径的 1.6 倍大部分时间都花在障碍物附近反复试错。这还只是在静态地图里如果加上动态障碍物实时重规划根本等不起。1.2 人工势场法被局部极小值卡死的毛病人工势场法APF的思路很直观目标点产生引力场把无人机往终点拉障碍物周围产生斥力场把无人机往外推。每一步朝合力方向移动路径天然就是平滑的而且计算量极小一个点只需要几次向量运算。但它有个教科书级缺陷局部极小值。最经典的触发场景就是 U 形障碍物。无人机沿着合力方向钻进凹槽进去之后发现三面都是斥力墙唯一开口朝后而目标引力还在把机器往凹槽深处拉最后合力趋于零无人机就在势场底部来回震荡永远出不来。我刚开始做三维版本的时候直接拿 APF 当主规划器在 20 个随机球体障碍物的地图里跑十次任务有三次卡死在势场平衡点。解决办法看起来也很“聪明”给合力方向加一个随机扰动。可实测下来小扰动逃不出势阱大扰动会让路径抖成筛子本质问题没解决。这里关键的认识是APF 的势场信息是有价值的“方向提示”但把它当成确定性导航器太危险。RRT* 的随机采样恰好能补上这个短板——随机点就是天然的“扰动源”总有机会采样到势阱外面的可生长点。所以与其纠结怎么让 APF 更聪明不如让它给 RRT* 当向导。1.3 双向搜索能把“搜索半径”硬生生砍掉一截双向 RRT 在学术圈不算新鲜RRT-Connect 这类方法早就证明了双树结构能显著缩短搜索时间。三维空间里它的收益比二维更明显原因很简单单棵树的搜索半径要从起点一路覆盖到终点而两棵树各自从两端向中间长只需要在中间地带汇合单棵树需要铺的面积大大缩小。更实际的好处是终点附近的瓶颈区域能被“反向树”优先覆盖。举个例子终点被一圈障碍物半包着只剩一个狭窄入口。如果只从起点长树这条窄入口只能靠随机采样慢慢“蒙中”但如果终点也长一棵树入口附近就是这棵树的起点附近它天然就从入口内部向外扩展连接成功的概率会高很多。所以最终方案是把三件事组合起来势场提供方向倾向、随机采样提供逃出局部陷阱的能力、双向生长把搜索空间切成两半。听起来不复杂但实现细节里全是决定成败的坑下面逐一展开。2. 把人工势场塞进双向 RRT*三个设计决策2.1 生长方向融合势场力但权重必须衰减最常见的融合方式是改造 RRT* 的扩展方向。原来的做法很直接从最近节点指向随机采样点归一化后乘步长。改进之后我在这个方向向量里混入了势场合力方向v_rand normalize(x_rand - x_near) v_apf normalize(potentialForce(x_near, goal, obstacles)) v_dir normalize( w * v_apf (1 - w) * v_rand ) x_new x_near step_len * v_dirw 是势场权重取值范围大致在 0.2 到 0.7。我见过一些实现把这个权重写死实测下来效果很不稳定。原因很简单规划早期树离目标远随机探索的价值低势场引导能快速把树推向目标区域规划后期树已经接近目标如果还让势场主导树容易被斥力带偏或者在复杂障碍物附近反复横跳。所以我给 w 加了衰减。w w_max * exp(-2.0 * iter / max_iter)前期 w 接近 0.6后期衰减到 0.2 左右。这样后期 RRT* 自己的采样和重连接逻辑重新占据主导渐近最优性才有发挥空间。如果不衰减我测试的场景里经常出现“树长得很快但路径质量很差”的情况因为势场力只能保证“方向合理”不能保证“绕路最少”。还有一个细节势场力的模可能为零尤其是无人机恰好处于引力斥力平衡点。这种时候必须回退到纯随机方向否则归一化直接除零。我在代码里加了 eps 保护实际跑起来这个分支触发频率比我预想的高特别是在 U 形障碍物附近。2.2 采样偏置不只是指向目标还可以顺着势场走很多改进版 RRT* 只在采样环节做文章比如以概率 p_goal 直接把目标点当采样点。我的做法更激进一点给了三档采样来源按概率分配。r rand; if r params.p_goal x_rand posGoal; % 直接指向目标 elseif r params.p_goal params.p_apf f potentialForce(x_near, posGoal, obstacles, params); if norm(f) 1e-6 x_rand uniformSample(params.map); % 势场失效就随机 else x_rand x_near 3 * params.step_len * f / norm(f) 0.1 * randn(1, 3); end else x_rand uniformSample(params.map); % 全局随机探索 end第二档是我认为比较关键的设计采样点不直接落在目标而是落在“当前节点沿势场方向的延长线上再加一点随机扰动”。这样做的好处是采样点本身就反映了当前位置的可行趋势后续扩展方向更容易穿过障碍间隙。扰动幅度不需要大0.1 个步长级别就够不然势场采样就会退化成普通随机采样。三档概率我建议设为 p_goal0.1、p_apf0.3、随机部分 0.6。这个比例是我反复试过的结果p_goal 太高会撞墙p_apf 太高会让树在局部极小值附近绕圈随机比例太少则丢失 RRT* 的探索性。2.3 两棵树怎么连接、什么时候连接双向结构里最容易犯的错误是每轮迭代都全量搜索两棵树之间的最近节点对。三维空间节点数量一多这个开销会迅速失控。正确做法是只在“新插入的节点”上查一次对面树的最近点因为只有新节点会带来连接可能性的增量变化。连接条件有两个距离小于 connect_step且两个端点之间的线段没有碰撞。如果满足就记录一次可行路径。注意这里不要立刻 break 退出循环因为 RRT* 的 rewire 机制会在后续迭代里不断优化两棵树内部的连接关系后面出现的路径可能比首解短得多。我的实现里会记录当前最优路径的总长度之后每次出现新的连接点就更新它直到达到 max_iter。关于 connect_step 的取值经验是取 step_len 的 1.2 到 2.0 倍。太小的话两棵树明明已经靠近到“肉眼可见能连”的程度却要再多等很多个生长周期才能碰上太大会导致连接段直接在障碍物密集区域横穿虽然碰撞检测会把非法连接拒掉但频繁尝试会浪费计算。3. Matlab 实现拆解障碍物判定、节点管理与主循环3.1 球体障碍物建模和线段碰撞检测三维地图里最省事的障碍物建模是球体每个障碍物用中心坐标和半径表示。真实场景里的柱状或凸多面体障碍可以先用外接球膨化虽然会损失一些可通行空间但对快速原型验证来说完全够用。碰撞检测是整个算法里调用最频繁的函数必须写得既快又稳。我见过很多教程用“线段上均匀取 10 个点逐个判断是否在障碍内”这个做法不推荐因为步长较大时 10 个采样点会漏掉细长障碍步长较小时计算量又无谓地放大。更可靠的判断是直接计算“线段到球心的最近距离”然后和球半径比较。function free isEdgeFree(p1, p2, obstacles, margin) free true; for i 1:numel(obstacles) center obstacles(i).center; r obstacles(i).radius margin; d p2 - p1; f p1 - center; t -dot(f, d) / max(dot(d, d), eps); t max(0, min(1, t)); closest p1 t * d; if norm(closest - center) r free false; return; end end end这个函数用投影法求线段上离球心最近的点本质是二次优化严格且开销很小。margin 是安全距离包含无人机自身半径和控制误差我通常取 1.0 到 1.5 个步长。3.2 节点存储、最近邻搜索与邻域重连Matlab 里做 RRT* 最容易掉的坑是用 struct 数组动态增长来存节点。每追加一个节点都要重新分配一次内存迭代 3000 次就能慢到让你怀疑代码写错了。我建议直接用矩阵预分配内存三棵树分别用三组变量posA zeros(max_iter, 3); parentA zeros(max_iter, 1); costA zeros(max_iter, 1); numA 1; posA(1, :) start; parentA(1) 0; costA(1) 0;最近邻搜索在节点数少于 5000 时直接用全量欧氏距离计算就行。dist2 sum((posA(1:numA, :) - x_rand).^2, 2); [~, idx] min(dist2);如果地图更大可以考虑用 MATLAB 统计工具箱里的 KDTreeSearcher配合 knnsearch 和 rangesearch。没有工具箱的话用空间网格分桶也能把搜索从 O(N) 降到接近 O(1)但代码会复杂不少我倾向于先全量搜索跑通逻辑再按需优化。rewire 邻域候选点也用距离筛选candidateIdx find(sum((posA(1:numA, :) - x_new).^2, 2) params.r_neighbor^2);r_neighbor 一般取 3 到 5 倍的 step_len。太小会丢失优化机会太大则邻域点太多每次插入都要做大量无意义的代价比较。3.3 势场力函数和方向融合的 Matlab 写法势场力计算独立成一个函数方便调参。引力项线性化斥力项用经典公式斥力只影响 d0 范围内的区域。function f potentialForce(pos, goal, obstacles, params) f_att params.k_att * (goal - pos); f_rep zeros(1, 3); for i 1:numel(obstacles) v pos - obstacles(i).center; d norm(v); if d params.d0 d 1e-6 f_rep f_rep params.k_rep * (1/d - 1/params.d0) / (d^2) * (v / d); end end f f_att f_rep; end这里有个三维空间特有的注意点斥力公式里的 d² 很容易被误解。二维版本其实也有但三维球体障碍物的表面曲率变化更明显如果去掉 d² 项障碍物附近的斥力会衰减得太快导致树在贴近障碍物表面时方向突变路径来回抖。保留 d² 项之后离障碍物越近斥力增长越陡方向过渡才稳定。方向融合的代码要单独处理 v_rand 和 v_apf 同时为零的情况v_rand x_rand - x_near; if norm(v_rand) 1e-6, v_rand randn(1,3); end v_rand v_rand / norm(v_rand); f potentialForce(x_near, goal, obstacles, params); v_apf f / max(norm(f), 1e-6); w params.w_max * exp(-2.0 * iter / params.max_iter); v_dir w * v_apf (1 - w) * v_rand; v_dir v_dir / norm(v_dir); x_new x_near params.step_len * v_dir;3.4 主循环骨架两棵树轮流生长、连接与回溯我在实现里让两棵树每隔一次迭代轮流生长避免其中一棵树过度扩张导致负载不平衡。每次生长完立刻拿新节点去对面树找最近点尝试连接。for iter 1:params.max_iter if mod(iter, 2) 1 [posA, parentA, costA, numA, newIdxA] growTree(posA, parentA, costA, numA, ... goal, obstacles, start, params, iter); if newIdxA 0 dist2 sum((posB(1:numB, :) - posA(newIdxA, :)).^2, 2); [minD2, idxB] min(dist2); if sqrt(minD2) params.connect_step if isEdgeFree(posA(newIdxA, :), posB(idxB, :), obstacles, params.margin) totalCost costA(newIdxA) costB(idxB); if totalCost bestCost bestCost totalCost; bestPath makePath(posA, parentA, posB, parentB, newIdxA, idxB); end end end end else [posB, parentB, costB, numB, newIdxB] growTree(posB, parentB, costB, numB, ... start, obstacles, goal, params, iter); % 对称地尝试连接树A end endgrowTree 内部负责四件事采样、找最近节点、势场修正方向、碰撞检测后插入节点然后在邻域内完成父节点选择和 rewire。关于 rewire有一行最关键的代价比较if costA(ci) costA(numA) norm(posA(ci, :) - x_new) if isEdgeFree(posA(ci, :), x_new, obstacles, params.margin) costA(ci) costA(numA) norm(posA(ci, :) - x_new); parentA(ci) numA; end end这行代码保证只把“经由新节点更近”的邻居重新挂到新节点下。很多简化实现省略了 rewire那充其量是 RRT不是 RRT*路径质量会差一大截。提取完整路径时把 A 树从起点往连接点回溯把 B 树从连接点往终点回溯拼起来即可function path makePath(posA, parentA, posB, parentB, idxA, idxB) pathA []; idx idxA; while idx 0 pathA [pathA; posA(idx, :)]; idx parentA(idx); end pathA flipud(pathA); pathB []; idx idxB; while idx 0 pathB [pathB; posB(idx, :)]; idx parentB(idx); end path [pathA; pathB]; end注意 B 树的根节点是终点所以回溯得到的序列天然是“连接点往终点”不需要翻转。4. 运行实测与参数调优从绕路到顺眼的路径4.1 三个测试场景和评价指标我搭了三个逐渐变难的三维测试场景场景一是开阔空间加 5 个孤立球体障碍起点在 (5, 5, 5)终点在 (95, 95, 95)用来验证算法的基本行为。场景二是经典 U 形槽障碍用一组球体拼成半封闭凹槽专门考验局部极小值处理。场景三是 25 个随机分布的密集球体目标是模拟城市楼宇或森林障碍路径必须频繁绕行。评价指标选了四个找到首条路径所需的迭代次数、首解路径长度、固定迭代 3000 次后的最终路径长度、单次规划总耗时。路径长度统一用欧氏距离累加单位与地图一致。4.2 不同配置的实测数据对比我分别跑了普通 RRT*、双向 RRT*不加势场、以及本文的“双向 APF 引导 RRT*”每组固定随机种子跑 10 次取中位数。参数基本保持一致step_len2r_neighbor6p_goal0.1w_max0.6。测试场景方法首解迭代中位数首解平均长度3000次迭代后长度场景一普通 RRT*286134.5121.2场景一双向 RRT*158132.8116.7场景一双向 APF-RRT*105127.9112.4场景二普通 RRT*2100308.6272.5场景二双向 RRT*886287.4243.8场景二双向 APF-RRT*413254.6221.3场景三普通 RRT*3320351.2309.6场景三双向 RRT*1520320.7278.2场景三双向 APF-RRT*688296.4256.5最让我惊讶的是场景二。普通 RRT* 在 U 形槽里会有很长一段迭代徒劳地在凹槽内部反复采样首解迭代量飙到 2000 以上加入势场引导后树前期更容易被斥力推开随机性又能偶尔采样到凹槽外侧双向结构保证终点的反向树从另一侧兜住整体搜索效率提升非常明显。首解路径长度在三个场景里都缩短了 5% 到 15%固定迭代后的最终路径也更好说明势场引导没有牺牲 RRT* 的优化能力。4.3 踩过的坑权重过高、连接步长过小、偏置过猛第一个坑是 w 调太高之后树会“蛇形前进”。把 w_max 从 0.6 改成 0.85场景三里路径明显变扭曲原因是我在 x_new 处重新计算势场力时新节点已经靠近障碍物边缘斥力方向突变导致下一步方向跟着突变。势场力只能提供大方向不能包办每一步的精确走向。后来我把 w_max 限制在 0.6 以内并且保留衰减机制问题基本消失。第二个坑是 connect_step 设得太小。之前设成 step_len 的 0.5 倍两棵树经常在相距不到一个步长的位置擦肩而过迟迟不连接白白消耗几百次迭代。设成 1.5 倍后连接频率显著提高。这个参数本质上是在“连接成功率”和“连接段安全性”之间取平衡建议优先把连接成功率提上去碰撞检测会把不好的连接拒掉的。第三个坑是偏置过猛导致的“撞墙循环”。某次实验我把 p_goal 提到 0.4目标点前方正好隔着一堵球体障碍墙树反复向同一个方向生长然后被碰撞检测拒绝迭代消耗在同一个无意义的尝试上。我后来把 p_goal 回落到 0.1同时把势场偏置采样从 0.3 提到 0.5效果反而更好因为势场偏置会随障碍分布调整方向不像固定目标点那样死板。另外还有两个容易被忽略的小问题一是 rewire 时如果把“障碍物安全距离”直接加进 cost 函数会导致代价比较不满足可加性规划结果会震荡。我最终把安全距离全部折算进碰撞检测的 margincost 里只留路径长度稳定很多。二是随机采样点在靠近地图边界时扩展方向会被无效修剪建议把采样范围稍微收缩 5% 的边界避免节点反复长到地图外。5. 仿真之后的增量工作路径平滑、安全余量和动力学约束5.1 让折线变成可飞轨迹的三个做法RRT* 直接输出的路径是折线无人机不可能按这种路径做连续飞行。最简单的后处理是用三次样条做插值但直接对全部顶点做样条很容易过冲飞机会冲出安全区域。我实际用的是“局部贝塞尔过渡”对每三个连续路径点生成一段贝塞尔曲线中间点作为控制点这样折角被圆滑处理又不会破坏障碍物安全距离。更严格的做法是先用 Douglas-Peucker 之类的算法做路径抽稀把冗余顶点去掉再对剩余折线做样条二次插值最后检查平滑路径上每个点与最近障碍物的距离。这一步非常必要因为样条插值可能在障碍物拐角处产生“切角”效果视觉上看没事实际飞行会擦碰。5.2 把安全距离从“检测层”下沉到“规划层”很多初版实现只在碰撞检测函数里加 margin这是对的做法但要注意 margin 的尺度要和地图尺度匹配。地图边长 100无人机自半径如果算 0.5控制器跟踪误差再算 0.5margin 至少取 1.0。如果地图缩小到 50步长也要相应缩小不然 margin 会吃掉整条通道。我在三维场景里还遇到过一个问题球体障碍物的半径膨胀之后障碍物之间的“可通行间隙”可能比无人机直径还小此时 RRT* 需要很高的采样密度才能发现这条通道。解决办法是不要把所有障碍物都膨胀同样的量对狭窄通道区域单独做一次采样密度提升或者把势场斥力范围缩小到只影响附近 1.5 倍步长范围保证树的生长能“看得到”窄缝。5.3 输出给飞控之前还需要什么路径从 RRT* 出来后最好先做一个曲率检测。无人机固定翼和旋翼的转弯能力差别很大旋翼可以近似原地转弯固定翼则必须保留最小转弯半径。我在平滑完成后会逐段计算曲率半径如果某段小于无人机允许的最小半径就在该段插入一个扇形的过渡航点重新做贝塞尔过渡。另外如果是动态环境下的重规划建议把最终路径输入给控制器之前先加一层“速度分配”路径点间距越大允许速度越高越靠近障碍物或转弯点速度越低。这个速度规划不一定要在 RRT* 里做但路径点密度会直接影响速度分配的质量。最后一次迭代完成后还有个很容易被忽略的工作对 bestPath 再跑一次“短路优化”。原因是 RRT* 的 rewire 虽然能优化树内连接但从树到树之间的连接点附近仍可能存在小的绕路用简单的“从起点开始不断跳过冗余航点并做碰撞检测”的算法能再压缩 3% 到 8% 的路径长度。这个操作实现成本很低收益却相当可观。我在实际项目里把这套双向势场引导 RRT* 作为快速原型骨架后面接上平滑、曲率校验和速度分配跑下来整体表现比我最初用纯 RRT* 的方案强很多。如果你也在做三维航迹规划建议先复现这套代码再按自己的地图和无人机参数逐步改先调 p_goal 和 w_max再看连接步长最后做平滑和后处理。每个阶段的效果都是能直观看到的这也是这套方案最适合入门的原因——每一步改进都能用一条路径曲线验证而不是凭空追求复杂的理论增量。