企业官网建设流程全解析

先放一个结论：在三维空间里裸跑 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_dir

w 是势场权重，取值范围大致在 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_goal=0.1、p_apf=0.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 end

growTree 内部负责四件事：采样、找最近节点、势场修正方向、碰撞检测后插入节点，然后在邻域内完成父节点选择和 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_len=2，r_neighbor=6，p_goal=0.1，w_max=0.6。

最让我惊讶的是场景二。普通 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 函数，会导致代价比较不满足可加性，规划结果会震荡。我最终把安全距离全部折算进碰撞检测的 margin，cost 里只留路径长度，稳定很多。二是随机采样点在靠近地图边界时，扩展方向会被无效修剪，建议把采样范围稍微收缩 5% 的边界，避免节点反复长到地图外。

5. 仿真之后的增量工作：路径平滑、安全余量和动力学约束

5.1 让折线变成可飞轨迹的三个做法

RRT* 直接输出的路径是折线，无人机不可能按这种路径做连续飞行。最简单的后处理是用三次样条做插值，但直接对全部顶点做样条很容易过冲，飞机会冲出安全区域。我实际用的是“局部贝塞尔过渡”：对每三个连续路径点生成一段贝塞尔曲线，中间点作为控制点，这样折角被圆滑处理，又不会破坏障碍物安全距离。

更严格的做法是先用 Douglas-Peucker 之类的算法做路径抽稀，把冗余顶点去掉，再对剩余折线做样条二次插值，最后检查平滑路径上每个点与最近障碍物的距离。这一步非常必要，因为样条插值可能在障碍物拐角处产生“切角”效果，视觉上看没事，实际飞行会擦碰。

5.2 把安全距离从“检测层”下沉到“规划层”

很多初版实现只在碰撞检测函数里加 margin，这是对的做法，但要注意 margin 的尺度要和地图尺度匹配。地图边长 100，无人机自半径如果算 0.5，控制器跟踪误差再算 0.5，margin 至少取 1.0。如果地图缩小到 50，步长也要相应缩小，不然 margin 会吃掉整条通道。

我在三维场景里还遇到过一个问题：球体障碍物的半径膨胀之后，障碍物之间的“可通行间隙”可能比无人机直径还小，此时 RRT* 需要很高的采样密度才能发现这条通道。解决办法是不要把所有障碍物都膨胀同样的量，对狭窄通道区域单独做一次采样密度提升，或者把势场斥力范围缩小到只影响附近 1.5 倍步长范围，保证树的生长能“看得到”窄缝。

5.3 输出给飞控之前还需要什么

路径从 RRT* 出来后，最好先做一个曲率检测。无人机固定翼和旋翼的转弯能力差别很大，旋翼可以近似原地转弯，固定翼则必须保留最小转弯半径。我在平滑完成后会逐段计算曲率半径，如果某段小于无人机允许的最小半径，就在该段插入一个扇形的过渡航点，重新做贝塞尔过渡。

另外，如果是动态环境下的重规划，建议把最终路径输入给控制器之前先加一层“速度分配”：路径点间距越大，允许速度越高；越靠近障碍物或转弯点，速度越低。这个速度规划不一定要在 RRT* 里做，但路径点密度会直接影响速度分配的质量。

最后一次迭代完成后还有个很容易被忽略的工作：对 bestPath 再跑一次“短路优化”。原因是 RRT* 的 rewire 虽然能优化树内连接，但从树到树之间的连接点附近仍可能存在小的绕路，用简单的“从起点开始不断跳过冗余航点并做碰撞检测”的算法，能再压缩 3% 到 8% 的路径长度。这个操作实现成本很低，收益却相当可观。

我在实际项目里把这套双向势场引导 RRT* 作为快速原型骨架，后面接上平滑、曲率校验和速度分配，跑下来整体表现比我最初用纯 RRT* 的方案强很多。如果你也在做三维航迹规划，建议先复现这套代码，再按自己的地图和无人机参数逐步改：先调 p_goal 和 w_max，再看连接步长，最后做平滑和后处理。每个阶段的效果都是能直观看到的，这也是这套方案最适合入门的原因——每一步改进都能用一条路径曲线验证，而不是凭空追求复杂的理论增量。