做四旋翼无人机三维路径规划的时候，我被单棵随机树的随机性折磨过很久。二维小地图上RRT跑得轻松，一放进三维楼宇环境就原形毕露：路径绕、收敛慢、终止条件还特别玄学。后来换成双向快速探索随机树B-RRT，两颗树一颗从起点、一颗从终点往中间对掘，原本一两分钟才能搞定的规划任务，几秒钟就能收进怀里。这篇博文就把我实际调试这套第14923期Matlab源码的过程完整复盘一遍，从B-RRT的核心原理、三维地图建模、四旋翼约束融合，到真正能跑的代码结构、参数调优和迁移思路都会讲到。适合刚接触RRT系列算法、准备在Matlab里跑无人机航线规划的同学，也适合已经能跑通单树RRT、想进一步提升收敛速度的工程师。

1. 为什么是B-RRT：单树RRT在三维无人机规划里的三个痛点

1.1 “往哪儿长”全靠缘分：探索惰性拖垮路径质量

单树RRT的基本逻辑一句话就能说清：从起点出发，在环境中随机采样一个点，从当前树中找到离采样点最近的节点，朝这个节点方向扩展固定步长，重复这个过程直到收到终点附近的点。

听起来很聪明，实际操作时你会发现一个致命问题：树的生长方向几乎不关心终点在哪。每次扩展都朝随机采样点跳一下，而采样点在整个三维自由空间里均匀分布，终点只是无数个采样可能性里的一个。地图越大，树越像一团向四面八方缓慢膨胀的棉絮，直到某一刻某个分支“碰巧”长到终点附近，规划才结束。

在100米见方的三维环境里做无人机路径规划，单树RRT经常出现两种后果：一是收敛时间不可控，有时几秒完成，有时狂跑数千次迭代还够不到终点；二是路径质量奇差，树为了够到终点绕了很大一圈，折线路径和真实可飞航线偏差特别大。本质上，单树RRT把“找到终点”这个强约束完全交给了随机性，这种探索惰性是它最大的短板。

1.2 三维空间让“最近邻查找”和“碰撞检测”成本飞速上升

二维RRT里，每棵树的节点只有几百个，线性遍历找最近邻节点完全没问题。到了三维，地图体量变大，树为了覆盖空间必然有大量节点，直线遍历和逐段碰撞检测就会变成性能瓶颈。

一个很直观的计算：树里有N个节点，每次采样后要遍历N个节点找最近邻，扩展出新节点后又要做一次碰撞检测。如果碰撞检测把一条步长线段再细分判断是否穿过障碍物，那一次扩展就要做M次cube查询。N到几千、M到几十之后，单次迭代的耗时明显上升，规划总时间被拉得很难看。

很多代码卡在这一步就开始加各种优化，比如用KD-tree做最近邻查询、用稀疏体素做碰撞检测。但对B-RRT而言，优化空间其实更大：因为两棵树同时生长，单棵树所需节点数远小于单树RRT，最近邻查询和碰撞检测的总次数都会降下来。

1.3 终点方向的缺失：单树RRT的“收敛密码”只有半串

单树RRT不是没有改进方案，最常见的是目标偏置采样：每次以一定概率直接采样终点方向，让树更偏向终点生长。但加了偏置之后有一个尴尬现象：树的主干确实往终点方向去了，遇到障碍物却容易钻进去出不来，在障碍边缘反复试探。

B-RRT的解法完全不同。它不再要求起点树独自完成“从起点到终点”的完整路径，而是从终点再长出一棵树，两棵树同时往中间探索。起点那棵树只需要走到能和终点树连接的位置，这种“握手”发生的概率远大于起点树单棵树长到终点的概率。本质上，B-RRT把终点这个强约束换成了两棵树之间的弱连接，收敛难度大幅下降。

2. B-RRT算法拆解：两颗树怎么在三维空间里“对向生长”

2.1 主循环结构：交替生长与角色互换

B-RRT的初始化很直接：一棵树T_start以起点为根，另一棵树T_goal以终点为根。主循环里并不是两棵树同时扩展一次，而是交替进行，每轮只让一棵树扩展一个节点，然后尝试连接对方。

遍历逻辑可以用一段伪代码说明：

初始化 T_start 和 T_goal for iter = 1 : maxIter p_rand = 采样(目标偏置概率) p_new = 扩展(T_current, p_rand, stepSize) if p_new 存在 p_near = 最近邻(T_other, p_new) if 无碰撞连接(p_new, p_near) 返回 路径(起点 -> p_new -> p_near -> 终点) end end T_current 与 T_other 交换角色 end

注意“交换角色”这一句，它是B-RRT的灵魂。每轮迭代后，处于扩展状态的那棵树变成待连接方，另一棵树参与下一轮扩展。这样两棵树的生长节奏基本同步，避免了某棵树一枝独秀、另一棵树长时间停滞的失衡状态。

2.2 连接尝试不是碰运气：贪心连接更快“握手”

两棵树之间如何判断“连上了”？常见做法是每次新增节点后，立刻到另一棵树里找最近邻节点，然后尝试从新节点一步步朝这个最近邻节点走，走一步查一次碰撞，能走到就直接返回路径。

这套贪心连接的实现比单树RRT的终点判定更稳。单树RRT里你要判断“新节点是否离终点小于阈值”，阈值设太大路径粗糙，设太小又半天收敛不了。B-RRT的连接判定天然是一个“能不能一直走到对方节点”的问题，只要两棵树的某两个节点之间有足够大的自由空间，连接就直接成立。

我调参时发现一个经验：贪心连接时每步走的子步长不要用整个stepSize，最好用小步长比如stepSize的一半去循环碰撞检测。原因很简单，两个节点之间的直线通道如果刚好贴着障碍物，大步长一步跨过去会直接漏检，路径回溯后看起来没撞，实际飞行时离障碍物太近。

2.3 节点采样与近邻选择：均匀采样之外的“小动作”

B-RRT在采样策略上可以保持均匀随机，也可以做目标偏置，但目标偏置不再指向原本的终点，而是指向另一棵树的根节点方向。在仿真测试里，目标偏置概率设在0.05到0.1之间就足够，太高会让树的“探索性”明显下降，两棵树都笔直往对方根上怼，遇到大障碍物反而绕不开。

近邻选择方面，三维环境下建议用KD-tree。尤其当你把地图分辨率调到1米以内，树节点奔着上万去的时候，线性遍历大概率成为整个算法的瓶颈。Matlab里可以直接用KDTreeSearcher，也可以用mex加速模块，具体算法后面第4节一并给出。

3. 四旋翼约束怎么融进RRT：路径规划不只是“几何找路”

3.1 机身边界与安全膨胀：把无人机当成“球”还是“棺材板”

四旋翼无人机不是质点，它在三维空间里占据一个实体体积。RRT生成的路径如果只把无人机当作一个点，穿越障碍物间隙时看起来安全，实际上桨叶可能已经撞墙了。

工程上的常见做法是给障碍物做膨胀处理：将每个体素障碍物向外扩展一定半径R，这个R等于无人机最大外形半径加安全余量。例如某四旋翼轴距450毫米、桨叶展开半径为200毫米，膨胀半径至少要300毫米，其中100毫米是留给传感器误差和环境不确定性的。

在Matlab里我通常用两种膨胀策略。栅格地图直接对占据栅格做形态学膨胀，或者碰撞检测时判断“路径点到障碍物的最小距离是否小于安全阈值”。后者更灵活，因为不需要真的修改地图数据，只需要在检测函数里把距离阈值调大一些。判断点到体素障碍物的距离时，把体素当作一个AABB包围盒求解最接近点，三维向量长度就是最小距离，这样一个盒一个盒遍历就能完成碰撞检测。

3.2 折线路径平滑：样条与贝塞尔的后处理

RRT直接输出的是折线路径，无人机不可能在每个拐点原地急转弯。首先要把折线转成满足动力学约束的光滑曲线。

常见做法有三种：三次B样条、五次多项式路径、Dubins路径。三次B样条最简单，把RRT原始折线的控制点丢给样条函数，就能得到一条C2连续的曲线，但样条不一定严格贴着原路径，有可能会穿过障碍物；五次多项式路径适合做时间参数化，因为它在位置、速度、加速度层面都连续，常用于生成轨迹指令。

我的建议是：先用B样条做几何平滑，再对平滑后的曲线重新执行逐点碰撞检测，被样条“削”到障碍物里的区段，用局部重新规划修补。这一步很多人会跳过，结果仿真里飞得很好，一放到真实环境就蹭墙。

3.3 飞行可行性校验：速度、加速度、姿态角一次算完

几何路径过平滑后，还需要做动力学可行性校验。核心是检查三点：路径点迹的切向速度是否超过无人机最大飞行速度；曲率半径对应的向心加速度是否小于无人机最大允许加速度；连续航向角变化是否超出姿态控制能力。

向心加速度和曲率半径的关系式是a = v² / r。假设某四旋翼最大飞行速度为12米每秒，最大水平加速度为5米每秒平方，那么路径上任意位置的曲率半径都必须满足r > v² / a，也就是r > 28.8米。很多仿真代码完全忽视这个约束，生成的路径转弯半径只有几米，看起来没问题，实际飞行必然失控。

在Matlab校验时，可以先对平滑曲线密集采样，计算每个采样点的曲率半径，再结合预设速度剖面检查约束是否满足。如果某个区段曲率半径太小，两个办法：降低该区段飞行速度，或者回到平滑阶段增加控制点的张力。

4. Matlab源码实现要点：从伪代码到可运行代码

4.1 地图建模与碰撞检测模块

这套第14923期源码里，地图建模用一个三维逻辑矩阵costmap表示，true代表被占据。地图尺寸在初始化时直接传入坐标大小：

map = false(100, 100, 50); % 100m x 100m x 50m 自由空间 obsList = loadObstacle('obsData.mat'); % 障碍物中心点与半尺寸 for i = 1:size(obsList, 1) c = obsList(i, 1:3); h = obsList(i, 4:6); map(c(1)-h(1):c(1)+h(1), c(2)-h(2):c(2)+h(2), c(3)-h(3):c(3)+h(3)) = true; end

碰撞检测函数单独封装，同时考虑机身体积膨胀：

function isOk = isCollisionFree(map, p1, p2, safetyRadius) d = p2 - p1; dist = norm(d); steps = ceil(dist / 0.5); for i = 0:steps p = p1 + d * (i / steps); if pointCollides(map, p, safetyRadius) isOk = false; return; end end isOk = true; end

4.2 节点数据结构与最近邻查找

源码里用结构体数组维护树节点，每个节点包含位置、父节点索引、所在树编号：

tree(k).pos = [x, y, z]; tree(k).parent = idx; tree(k).treeID = 1; % 1表示起点树，2表示终点树

初始时两棵树各一个节点，tree数组动态扩展。最近邻查找我强烈建议升级为KD-tree，尤其是地图大到100米量级之后：

mdl = KDTreeSearcher(currentTreePositions); [nearIdx, nearDist] = knnsearch(mdl, pRand);

如果节点总数少于500，线性遍历就可以；超过500，直接用KD-tree能省十几倍的时间。

4.3 主循环与路径回溯

主循环严格按双树交替逻辑写：

for iter = 1:maxIter % 当前扩展树是active树 pRand = sampleFree(map, goalBias, targetPos); pNew = extendTree(activeTree, pRand, stepSize, map); if ~isempty(pNew) otherTree = activeTree == 1 ? 2 : 1; pNear = findNearest(otherTree, pNew.pos); if isCollisionFree(map, pNew.pos, pNear.pos, safetyRadius) path = backtrackPath(pNew, activeTree, pNear, otherTree); break; end end % 交换角色 activeTree = 3 - activeTree; end

回溯路径时有一个容易踩的坑：两棵树连接成功后，要从连接点分别向各自的根节点回溯，然后把起点树这一段和终点树那一段拼接起来，注意把终点树那一段的节点顺序反过来，否则会出现路径“倒着走”的问题。

4.4 可视化渲染与结果输出

Matlab三维可视化是这套源码的一大优势。用plot3画原始折线路径，用fill3画障碍物表面，再用scatter3画两棵树的节点分布，调试时能一眼看出问题出在哪里：

plot3(path(:,1), path(:,2), path(:,3), 'r-', 'LineWidth', 2); hold on; scatter3(tree1(:,1), tree1(:,2), tree1(:,3), 10, 'b', 'filled'); scatter3(tree2(:,1), tree2(:,2), tree2(:,3), 10, 'g', 'filled');

实际运行时，我习惯把两棵树节点颜色分开，蓝色是起点树，绿色是终点树，碰撞连接点单独放大标记，这样能清楚看到两棵树是从哪个位置完成握手的。

5. 实测效果与参数调优：我在仿真环境里踩过的坑

5.1 两棵树握不上手：连接判定太“严格”但又能少见

第一次把B-RRT跑起来，出现最多的问题是两棵树明明已经在空间上交错，却一直判定为连接失败。查到最后发现根因是连接时贪心步长设置得太大，两个节点之间明明有通道，因为步长穿越检测的点太少而误判为碰撞。

把贪心连接的检测步长从1米改成0.5米之后，连接成功率立刻上去了。所以这里有一个悖论：树扩展步长可以大，但连接尝试的内部检测步长必须小。这与一般人的直觉相反，也是调参时最容易被忽视的点。

5.2 树扩展步长的选择：地图尺寸的2%到5%是个安全区间

在100米x100米x50米的地图里，我试过stepSize从1米到10米的多个配置。小于2米时，树的节点数量爆炸，单次规划动辄上万次迭代；大于8米时，路径变得很粗，折线经常穿入障碍物边缘，刚才说的连接检测小步长也救不回来。

最佳区间基本落在地图最大边长的2%到5%，也就是2米到5米之间。具体取值还要考虑障碍物密度，障碍物越密集，步长越要往小调。

5.3 目标偏置概率：0.05到0.1是甜蜜点，太高会破坏探索

目标偏置概率我一开始设到0.3，两棵树几乎直奔对方根方向，地图简单时收敛极快，地图一复杂就频繁卡死在障碍物凹陷处。后来一路下调到0.05，虽然单次规划慢了一点，但整体成功率大幅上升，最复杂的地图也能稳定跑通。

还有一种小技巧是动态偏置：迭代前100次偏置概率设低一点，让两棵树先充分探索，后面再逐步提高偏置概率，加速握手。源码里没有写这个，改起来也就多两行if判断。

5.4 随机数种子与调试复现

Matlab默认随机数流每次运行都不同，同一段代码两次运行结果完全不同，调试时非常容易“灵异”。我在源码入口固定了随机数种子：

rng(42);

固定种子之后，同一个地图和参数组合就能稳定复现路径结果。排查算法逻辑时非常好用。后期批量测试不同地图时再松开种子，避免结果偏向固定场景。

6. 如何把这份B-RRT源码迁移到其它场景

6.1 从三维栅格到OctoMap：换掉碰撞检测的底层数据结构

Matlab仿真里用三维逻辑矩阵做地图很方便，但真机或室外环境里障碍物数据往往来自SLAM或点云，不能直接塞进一个稠密三维矩阵，内存根本不够。工程上会换成八叉树地图，或者直接使用体素地图。

迁移思路其实很干净：碰撞检测接口函数isCollisionFree保持不动，把内部的occupancy查询从matrix(idx1, idx2, idx3)换成octree.isOccupied(x, y, z)。这样一来，算法层的树扩展、近邻查找、路径回溯完全不用改，只改地图后端就能跑在新数据上。

6.2 加入速度和加速度约束：从几何路径升级到时间轨迹

这份源码输出的是纯几何路径。要真正给无人机飞控使用，下一步是把它升级为带时间戳的轨迹。我习惯的做法是沿平滑路径进行梯形速度规划：先计算路径总长度，按最大速度Vmax和最大加速度Amax生成速度剖面，再对每个路径点分配时间戳。

时间分配之后，原本均匀的路径点密度会变成“直线段时间短、弯道时间也短”的不均匀分布，需要重新采样成等时间间隔的轨迹指令。这个步骤在Matlab里用interp1就能完成，但要注意插值前先做样条平滑，否则加速度跳变会很大。

6.3 与飞控系统对接：输出哪些指令才能飞

把轨迹交给飞控时，最方便的是给位置环控制用的轨迹点列表。通常包含四列：期望x、期望y、期望z、期望yaw，再加上每个点对应的时间戳。如果飞控支持位置速度加速度前馈，就把梯形速度规划里求得的vx、vy、vz和ax、ay、az也一起打包。

实际对接时还有一个容易遗漏的点：yaw角。RRT根本不关心无人机朝向，但飞控必须知道机头朝哪。常见做法是把yaw设为路径切线的水平方向角，或者在遇到较大转向点时提前预置yaw变化，让无人机在到达拐点前就开始转头，而不是到了再猛转。

我在实际迁移这套14923期源码的过程中，最大的体会就是不要被“B-RRT已经很快了”迷惑。双向扩展解决的是收敛速度问题，之后还要认真处理安全膨胀、平滑约束和动力学可行性，这三步不到位，再快的规划算法也只是在仿真里自嗨。如果你现在刚跑通单树RRT，建议直接拿B-RRT版本对照着看，两棵树的生长过程用三维散点图画出来，对算法理解的帮助极其明显。调参时优先固定随机数种子、放宽连接检测步长，把这两件事做好，剩下的就是慢慢打磨路径质量了。