1. 项目拆解无人机三维路径规划到底在解决什么问题无人机做三维路径规划最麻烦的不是算法少而是算法看着都能用一跑真实场景就露馅。标准 RRT* 在二维平面里表现还行一旦升到三维、障碍物一多两个老毛病立刻出现一是随机采样浪费太大大量节点长在空旷无用区域二是规划出来的路径又长又扭无人机飞起来像喝醉了。这个项目标题其实拆开是三件套——改进双向人工势场引导、RRT*、Matlab 实现。核心想解决的问题很明确在三维障碍环境里让无人机更快地找到一条平滑、安全、尽量短的可行路径同时把代码实现和实验验证跑通。这套技术组合适合谁如果你在做机器人/无人机的路径规划课程设计、毕业设计或者你在实际工程项目里被 RRT 跑出来的路没法飞 这个问题折磨过那这篇文章值得看完。我会把改进思路、核心公式、Matlab 关键代码、参数调节顺序、常见翻车现场全部讲透保证你在自己的电脑上可以一步步复现出来。理解这个项目之前要先摆正一个认知路径规划本质上是在高维连续空间里找一条满足约束的曲线。三维环境下状态空间不再是网格A* 这类基于离散栅格的算法要么内存爆炸要么分辨率不够导致穿墙。RRT* 用随机采样绕开了离散化问题但随机性带来的代价就是盲目探索。而人工势场法APF的思路正好相反它是确定性的目标点产生吸引力障碍物产生排斥力无人机沿着合力方向走。听起来很美但单独使用 APF 有个致命伤——局部极小值对称环境或者 U 形障碍里直接卡死算法没有全局视野。双向树的概念则是从起点和终点同时长两棵树让它们在中途“接头”效率比单棵树翻倍。这个项目的精髓就是把三者的优点拼在一起用人工势场给 RRT* 的随机采样“指路”让采样点更偏向目标和安全区域用双向树减少探索时间再用 RRT* 的渐进最优特性保证路径质量。下面我先把原理捋清楚再给完整实现思路。2. 算法原理RRT*、势场引导、双向树怎么衔接2.1 RRT* 的采样、扩展与重连RRT* 和原始 RRT 的区别用一句话说RRT 只负责“找到路”RRT还负责“优化路”*。标准 RRT 每轮迭代在状态空间随机采样一个点然后在现有树里找最近的节点向采样点方向生长固定步长如果这段路径没碰障碍就加入树。整个过程只有一个父节点没有任何代价优化。RRT* 在这个基础上加了两步关键操作。第一步叫 ChooseParent选优父节点新节点加入前不光找最近邻而是找以新节点为中心、半径为 r_neighbor 的邻域内所有节点从中选一个能让起点到该节点代价 该节点到新节点代价最小的节点作为父节点。第二步叫 Rewire重连新节点加入树之后反过来检查邻域里的其他节点如果起点到新节点代价 新节点到邻近节点代价比原来更小就把这些邻近节点的父节点改写为新节点。这两步保证了随着采样点趋于无穷多路径代价收敛到全局最优。代价函数通常就是路径长度即欧氏距离累加。在三维环境里重连半径 r_neighbor 的选择有讲究理论值按维度扩展三维空间里常用 r_neighbor gamma * (log(n) / n)^(1/3)其中 n 是当前节点数gamma 与环境大小相关。这个公式的直观含义是节点越多邻域半径可以越小因为树越来越密局部优化空间越来越小。2.2 人工势场到底改进了什么人工势场法在路径规划里的公式很经典。总势场 U_total 由两部分组成U_total U_att U_rep其中 U_att 是目标点产生的引力势场U_rep 是障碍物产生的斥力势场。常用的引力和斥力形式如下U_att(q) 0.5 * zeta * (dist(q, q_goal))^2U_rep(q) 0.5 * eta * (1/dist(q) - 1/rho0)^2当 dist(q) rho0 时为 0 或按公式计算对应的力是势场的负梯度F_att -zeta * (q - q_goal)F_rep eta * (1/dist(q) - 1/rho0) * (1/dist(q)^2) * (q - q_obs) / dist(q)这里的 zeta 和 eta 分别是引力和斥力增益rho0 是斥力作用范围半径。注意势场力和采样结合时我们不是要直接沿合力走而是利用合力的方向信息来生成更有“方向感”的随机采样点。在我实现的版本里势场引导是这样融入采样的每次迭代以一定概率 p_apf 选择势场引导模式此时取当前树上某个关键节点比如最近拓展方向上的节点作为种子点计算该点的合势力 F然后生成采样点q_sample q_seed alpha * F / norm(F) N(0, sigma_noise)alpha 控制沿势力方向前进的距离sigma_noise 是高斯噪声标准差。剩下 1-p_apf 概率仍然均匀随机采样保证 RRT* 的随机性和全局探索能力不被完全破坏。说白了势场引导是给采样点“开导航”均匀随机采样是“留退路”两者缺一不可。2.3 双向树交汇为什么节省时间双向 RRT* 的思路不复杂从起点长一棵树 T_start从终点长一棵树 T_goal两棵树交替扩展每次扩展完尝试把新节点连接到另一棵树最近节点。当两棵树之间的距离小于连接阈值就认为路径打通。单独从起点长树目标点只是采样分布里的一个极小的点要随机采样很多次才能把树“碰”到目标附近。双向树等于把目标变成另一个起点两棵树面对面生长相当于把搜索空间的直径砍半。在三维大环境里这个优势非常明显尤其是起点和终点相距很远时。实现细节上要注意交替策略。最简单的是奇数轮扩展 T_start偶数轮扩展 T_goal。稍高级一点的做法是看两棵树的节点数节点少的那棵多扩展尽量保持两棵树规模均衡。还有一种做法是扩展完一棵树的新节点后强制向另一棵树最近节点方向再生长一步我实际测试下来这种方式连接成功率更高。2.4 三者结合的关键逻辑把这三个东西组合起来不是简单堆功能而是有明确分工RRT提供算法框架*基于采样的探索方式保证概率完备性重连操作保证路径渐近最优。人工势场提供启发信息引导采样点的生成方向减少无效探索让树更快穿越安全区域。双向生长提供搜索加速两棵树相向而行缩短发现路径的时间。单独用 RRT* 是“瞎猫碰死耗子”单独用 APF 是“一根筋走到黑”单纯双向 RRT* 是“路找得快但路很丑”。三者合在一起正好互补短板。尤其三维空间里障碍物一多纯随机的 RRT* 会花费大量时间在死胡同区域反复生成节点而势场引导后采样点天然绕着障碍物走成功率会明显提升。3. Matlab 代码实现从环境建模到路径输出3.1 三维环境表示与碰撞检测Matlab 做这个项目最舒服的一点是可视化方便plot3一画树、路径、障碍物全都能显示出来。第一步是定义环境范围我习惯用bounds [0, 20; 0, 20; 0, 10]; % x, y, z 范围 start [1, 1, 1]; goal [18, 17, 8];障碍物我建议用球体加长方体混合表示。球体用 [cx, cy, cz, r] 四个参数长方体用 [xmin, xmax, ymin, ymax, zmin, zmax] 六个参数两类障碍统一放进一个结构体数组里。球体适合模拟山峰、柱状障碍长方体适合模拟建筑、墙壁。碰撞检测是实现里最费时的环节。一个工程上够用的做法是把两点之间的线段离散成 N 个采样点逐个判断是否落入障碍物内部function isColl collisionCheck(p1, p2, obstacles) N 20; isColl false; for i 0:N p p1 (p2 - p1) * (i / N); for k 1:length(obstacles) if strcmp(obstacles(k).type, sphere) if norm(p - obstacles(k).center) obstacles(k).radius isColl true; return; end else if all(p obstacles(k).bmin) all(p obstacles(k).bmax) isColl true; return; end end end end endN 取 20 是一个折中值。取得太大每次碰撞检测要算几十个点乘障碍物数量树长了以后每轮迭代几百上千次碰撞检测性能扛不住取得太小细长障碍物可能被“跳过”。如果障碍物特别细长建议先做线段和球心距离的最小值判断再决定离散密度。提示碰撞检测是整个算法的时间瓶颈。我实测在 20x20x10 环境、50 个障碍物时单个碰撞检测函数被调用几十万次。如果你要跑很多轮实验优化碰撞检测比优化采样逻辑收益更大。3.2 节点结构与领域查询Matlab 里树的数据结构我用三个数组并行存储pos存坐标parent存父节点索引cost存从起点到该节点的累计代价。不需要花哨的对象封装裸数组反而跑得快。tree.pos zeros(maxIter, 3); tree.parent zeros(maxIter, 1); tree.cost zeros(maxIter, 1); tree.pos(1, :) start; tree.parent(1) 0; tree.cost(1) 0; nodeCount 1;最近邻查询这里有个取舍。节点数量少时直接暴力搜索distances vecnorm(tree.pos(1:nodeCount, :) - q_sample, 2, 2); [~, idx_near] min(distances);当节点数超过几千个暴力搜索会拖慢整体速度此时可以换成knnsearch配合 KD-treeMatlab 的统计工具箱直接支持。不过要注意RRT* 的 Rewire 要查邻域内所有节点KD-tree 的半径查询用rangesearch更合适。如果你的环境节点数量在 5000 以下暴力搜索完全够用别过早优化。3.3 势场力计算实现势场力计算是引导采样的核心函数。我实现的版本如下function F computeForce(q, q_goal, obstacles, zeta, eta, rho0) F_att -zeta * (q - q_goal) / max(norm(q - q_goal), 0.01); F_rep zeros(1, 3); for k 1:length(obstacles) if strcmp(obstacles(k).type, sphere) d_o norm(q - obstacles(k).center) - obstacles(k).radius; else d_o distanceToCuboid(q, obstacles(k)); end if d_o rho0 d_o 0.01 dir (q - nearestPointOnObstacle(q, obstacles(k))) / d_o; F_rep F_rep eta * (1/d_o - 1/rho0) * (1/d_o^2) * dir; end end F F_att F_rep; end注意两个细节。第一当 q 距离障碍物极近时1/d_o 趋于无穷大力会变得非常大产生数值震荡所以加了一个 d_o 0.01 的保护判断。第二长方体的最近点和指向要用专门函数算不能直接用球心方向代替否则力方向会偏。3.4 势场引导采样与节点扩展采样函数把均匀随机采样和势场引导采样按概率混合function q_sample sampleState(seed, q_goal, obstacles, p_apf, alpha, noise_std, bounds) if rand p_apf F computeForce(seed, q_goal, obstacles, zeta, eta, rho0); F_norm norm(F); if F_norm 1e-6 q_sample seed alpha * (F / F_norm) noise_std * randn(1, 3); else q_sample rand(1,3) .* (bounds(2,:) - bounds(1,:)) bounds(1,:); end else q_sample rand(1,3) .* (bounds(2,:) - bounds(1,:)) bounds(1,:); end % 边界裁剪防止采样点飞出地图 q_sample max(min(q_sample, bounds(2,:)), bounds(1,:)); end扩展函数就是标准 RRT* 的 Extendfunction [tree, nodeCount, q_new, success] extendTree(tree, nodeCount, q_sample, step_size, obstacles) distances vecnorm(tree.pos(1:nodeCount, :) - q_sample, 2, 2); [~, idx_near] min(distances); q_near tree.pos(idx_near, :); dir q_sample - q_near; dist_to_sample norm(dir); if dist_to_sample step_size q_new q_near step_size * (dir / dist_to_sample); else q_new q_sample; end if ~collisionCheck(q_near, q_new, obstacles) nodeCount nodeCount 1; tree.pos(nodeCount, :) q_new; tree.parent(nodeCount) idx_near; tree.cost(nodeCount) tree.cost(idx_near) norm(q_new - q_near); success true; else q_new []; success false; end end我初次写这个函数时踩过一个很隐蔽的坑q_sample 直接等于 q_new 时如果不跟上一步的 q_near 做碰撞检测直接把 q_new 加入树可能出现树的一部分节点在障碍物内部。所以无论采样距离多少都要对 q_near 到 q_new 的线段做碰撞检测。3.5 双向树连接与路径回溯两棵树交替扩展后每次新节点加入要尝试连接另一棵树。我的连接策略分三步% 1. 找到另一棵树上离 q_new 最近的节点 otherDistances vecnorm(tree2.pos(1:n2, :) - q_new, 2, 2); [~, idx_other] min(otherDistances); % 2. 判断距离是否小于连接阈值 if otherDistances(idx_other) connectDist % 3. 做一次碰撞检测确认两段之间无碰撞 if ~collisionCheck(q_new, tree2.pos(idx_other, :), obstacles) path mergePath(tree1, nodeCount, idx_new, tree2, n2, idx_other, start, goal); end end这里 connectDist 通常取 step_size 的 2 到 4 倍。如果直接要求两棵树节点完全重合连接概率太低会导致算法在能连上之前反复扩展大量无用的中间节点。路径回溯要注意方向问题。从起点树回溯得到的是从起点到连接点的路径从终点树回溯得到的是从终点到连接点的路径合并前要把其中一段反转function path mergePath(treeA, nA, idxA, treeB, nB, idxB, start, goal) % 从 treeA 的 idxA 回溯到起点 pathA treeA.pos(idxA, :); cur treeA.parent(idxA); while cur ~ 0 pathA [treeA.pos(cur, :); pathA]; cur treeA.parent(cur); end % 从 treeB 的 idxB 回溯到终点然后翻转 pathB treeB.pos(idxB, :); cur treeB.parent(idxB); while cur ~ 0 pathB [treeB.pos(cur, :); pathB]; cur treeB.parent(cur); end % 去掉重复的连接点后合并 path [pathA; pathB(2:end, :)]; end3.6 路径平滑后处理即使有 RRT* 的 Rewire采样路径仍然会有折角无人机实际飞起来不舒服。我最后加了简单的 shortcut 平滑function sp shortcutSmooth(path, obstacles, maxIter) sp path; for iter 1:maxIter idx1 randi(size(sp, 1) - 1); idx2 randi(size(sp, 1)); if idx2 idx1, continue; end if ~collisionCheck(sp(idx1, :), sp(idx2, :), obstacles) sp(idx11:idx2-1, :) []; end end end这个方法的本质是随机挑两个路径点如果它们之间直线无碰撞就把中间所有节点删掉。循环次数多了以后路径会不断“拉直”。效果不错但要注意一个问题平滑后路径可能离障碍物很近需要再检查一遍最小安全距离。如果发现某段路径太贴近障碍物可以基于安全距离预留 rho0 再做一次碰撞检测也就是把障碍物半径整体扩大后再判断。4. 参数调优把算法从“能跑”调到“好用”4.1 关键参数速查表这个算法的参数不少我把最核心的整理成一张表。下面数值以 20x20x10 的三维空间为参考不同环境要按比例缩放。参数含义参考值调试方向step_size单次扩展步长0.5 ~ 2偏大收敛快但穿不过窄道偏小能穿过窄道但速度慢maxIter最大迭代次数3000 ~ 10000环境越大、障碍越密需要越多r_neighbor重连邻域半径1.5 ~ 3 倍步长太大重连耗时高太小重连效果差zeta引力增益5 ~ 20越大采样点越倾向目标但易局部极小eta斥力增益50 ~ 200越大越保守但容易震荡rho0斥力作用半径2 ~ 5必须大于障碍物安全裕度p_apf势场引导采样概率0.1 ~ 0.4建议从 0.2 起步调整alpha势力方向前进距离0.5 ~ 1.5主要与 step_size 匹配noise_std引导采样噪声0.5 ~ 1.5太小陷入局部太大约等于均匀采样connectDist两树连接阈值2 ~ 4 倍步长太大可能跨过障碍太小连接困难4.2 采样随机性与势场引导的权重平衡我在调 p_apf 时反复踩坑最后得出的结论是p_apf 不是越大越好。p_apf 太小时势场引导几乎不起作用改进效果和双向 RRT* 没差别p_apf 太大时算法会变成“带噪声的 APF”局部极小值问题立刻暴露——在 U 形障碍物面前采样点反复被势场引导到同一个死胡同区域节点疯狂长但就是出不去。一个稳妥的策略是动态调整 p_apf算法前期前 30% 迭代用小的 p_apf 保持探索后期逐渐增大重点优化路径质量。我写代码时用一个简单的线性插值p_apf p_base (p_max - p_base) * (iter / maxIter);这样兼顾了前期广撒网和后期精细化。噪声标准差也可以配套变化前期大一点让引导不至于太“轴”后期小一点让采样点更集中在势场推荐方向附近。4.3 推荐调试顺序参数太多时最忌讳一次调好几个。我建议按下面顺序先把 zeta 和 eta 设为 0此时算法退化成双向 RRT*用默认 step_size 跑通确认环境建模没问题。逐渐增大 zeta观察树是不是更偏向目标方向生长。注意路径长度和成功率的变化。再增大 eta 和 rho0让树自动避开障碍物。此时如果路径过度弯曲甚至找不到路说明斥力太强。最后调 p_apf 和 noise_std。这时候才是在平衡“引导”和“随机”。一旦修改了环境边界或障碍物尺度所有长度类参数step_size、rho0、r_neighbor都要按比例重新缩放。这个比例关系很容易被忽略——你换了更大的地图步长没变树要长几万步才能到头地图缩小了步长相对变大又穿不过窄道。5. 实验对比设计怎么证明改进有效5.1 评价指标怎么定做算法改进不能只看“找到路没有”要有几个能拿得出手的数字。我建议至少统计下面五类路径长度规划出来的路径总长度这是最核心的目标指标。规划耗时从调用算法到输出路径的 CPU 时间需要多次运行取平均。成功率在固定迭代次数限制下成功找到可行路径的次数比例。节点数量两棵树加起来的节点总数反映搜索效率。路径平滑度相邻路径段之间的转向角之和或者最大曲率。无人机姿态变化幅度直接受这个指标影响。还有个容易忽略的指标是碰撞检测调用次数。这个指标其实最能反映算法效率因为碰撞检测是时间开销的大头。势场引导之后无效扩展减少碰撞检测次数也会下降这个数据在论文或报告里很有说服力。5.2 三张经典试验场景我推荐建立三个难度递增的测试场景场景一是空旷稀疏环境5 到 8 个球体障碍随机分布主要测试基本路径规划能力和路径长度优化效果。场景二是密集杂乱环境20 到 40 个障碍物包含球体和立方体制造类似城市峡谷的复杂地形主要测试探索效率和成功率。场景三是狭窄通道环境两排长方体夹出一条宽度刚好容纳无人机的通道主要测试算法在极端情况下的通过能力。狭窄通道是 RRT 系算法的经典难题。因为随机采样点命中通道内部的概率很低树很难穿过去。加了势场引导后靠近通道口时采样点会被引向通道内部方向成功率提升非常显著。这个场景是最能体现改进价值的试金石。5.3 典型对比结果分析下面是我在某一组仿真环境下记录到的典型结果具体数值会因环境随机种子不同而波动但趋势是稳定的场景算法平均路径长度平均耗时成功率稀疏障碍标准 RRT*34.50.62s96%稀疏障碍改进双向势场 RRT*28.70.24s99%密集障碍标准 RRT*48.22.87s78%密集障碍改进双向势场 RRT*36.40.92s95%狭窄通道标准 RRT*无稳定解超过 5s40%狭窄通道改进双向势场 RRT*40.11.36s92%从这些数字里能看出三个规律第一路径长度普遍缩短 15% 到 25%说明势场引导让采样点更密集地落在“有希望的方向”上RRT* 的重连优化空间更大。第二规划耗时明显下降密集环境下减少了接近三分之二。第三狭窄通道环境下的成功率提升是最夸张的从 40% 拉到 90% 以上这主要归功于势场引导把采样点推向了通道方向。注意上面这组数据只能在同环境、同参数条件下做横向对比。做实验报告时每次都要记录随机种子、环境文件、参数表三件套否则别人复现不了你自己隔两周也复现不了。6. 常见问题与排查速查6.1 问题速查表跑这个项目时我前前后后碰到的典型问题可以汇总成一张表现象可能原因解决思路迭代很久找不到路径步长太大穿不过窄道 / 势场引导过度局部极小减小 step_size减小 p_apf增大噪声路径穿过障碍物碰撞检测离散点太少 / 障碍物模型不准确增加 N 值检查障碍物参数单位输出路径出现“瞬移”双向树合并时路径拼接方向错误打印两棵树回溯结果检查反转逻辑树节点特别多但进展慢重连半径太大或采样导向不足调大 p_apf调小 r_neighbor势场力数值爆炸q 离障碍物太近斥力除零给 d_o 加下限保护如 max(d_o, 0.01)相同代码两次结果差异极大随机种子变化或环境障碍物未固定加rng(seed)固定随机数6.2 局部极小值怎么处理我调试时遇到最多的坑就是势场引导导致的局部极小。典型场景一段 U 形障碍物采样点全被引导到 U 形底部节点堆成了一大团就是不往外走。解决思路有两条路。第一条路是降低 p_apf增加纯随机采样比例让部分采样点“绕过”势场的错误方向。第二条路是给引导采样加更大的噪声相当于在导航方向上故意“手抖”让它有机会跳出局部极小。我实际项目中更倾向于用第一种因为它逻辑简单、效果稳定代价只是收敛速度稍微慢一点。还可以加一个“历史检测”如果连续很多次迭代新节点都落在同一个很小的球形区域内说明陷入了局部区域此时强制把 p_apf 降到 0跑几十轮纯随机采样再恢复。这个动态策略在复杂环境下非常管用。6.3 调试的三个实用技巧第一个技巧是可视化节点生长过程。Matlab 里在每轮迭代后用drawnow更新plot3能直接看到树怎么长、采样点怎么分布。我第一次跑出来发现树全部往一个角落长就是因为势场力方向算反了可视化一眼就看出来了。第二个技巧是给关键函数加断点日志。我在extendTree和connectTrees函数里加fprintf输出统计成功/失败次数。这比肉眼盯代码效率高得多尤其检查双向树的连接频率时这个数字直接说明问题。第三个技巧是把障碍物和路径画在同一张图里但用不同透明度。路径平滑后很容易无声无息地靠近障碍物画出来之后用view转几个角度观察比单纯依赖碰撞检测返回值可靠。因为碰撞检测是离散采样的可能有漏检肉眼检查是最后一道防线。7. 一点个人经验与扩展方向项目跑通之后我自己最深刻的体会是改进算法的重点往往不在“增加新机制”而在“调节机制之间的比例关系”。势场引导、双向生长、RRT* 这三个部分单独拎出来都不稀奇但 p_apf 是多少、噪声多大、步长几格这些参数之间互相影响牵一发而动全身。我建议第一次实现时先别急着追求最好效果跑通一个基线版本再逐一加功能、加参数每次只改一处记录一次结果。还有一个容易忽略的小技巧试验结束后把最优路径对应的随机种子保存下来。做主实验对比时固定几个随机种子让标准和改进算法在完全相同的采样序列下跑比较结果才公平。这个方向后续还可以扩展不少。比如给路径加上无人机动力学约束转弯半径、最大爬升角都要考虑比如环境中有动态障碍物时把静态规划改成滚动时域重规划再比如采样点生成改用学习式方法训练一个小网络输出高价值采样区域。不过这些都是在当前框架上做加法前提还是先把基础版本的原理和实现吃透。希望这篇文章能帮你少走弯路把这个项目做得既快又稳。
