如果你的飞机要从A点飞到B点中间隔着几个碍事的障碍物你会怎么规划我最早接触四旋翼无人机三维路径规划时第一反应就是A*加三维栅格结果环境一复杂内存立刻爆炸。后来换成RRT快速探索随机树才发现基于采样的算法在高维空间里是真的省心。最近又把单向RRT改成双向版本B-RRT配合四旋翼无人机在Matlab里完整跑通了一套三维路径规划流程这篇就把它拆开讲透。这里要说的不是那种丢给你一个main函数按一下就跑的“傻瓜教程”而是从原理到实现、再到参数调优的完整思路。内容围绕B-RRT双向扩展和三维空间碰撞检测展开适合正在做无人机避障路径规划、刚接触RRT系列算法、或者拿到了源码想搞清内部逻辑的读者。读完你至少能回答三个问题B-RRT比单向RRT到底快在哪、三维碰撞检测怎么写得稳、规划出来的折线路径怎么让四旋翼真正飞得起来。1. 为什么四旋翼的三维路径规划值得用RRT而不是A*1.1 三维环境与二维栅格之间的巨大鸿沟做地面机器人路径规划时栅格地图是默认选择二维数组遍历起来非常直观。但把同样思路搬到四旋翼无人机上问题立刻变味。无人机飞行的空间是一个有一定长宽高的立体区域栅格法意味着要在三维数组里做搜索。假设地图范围是1公里乘1公里飞行高度约200米如果栅格精度取1米那么需要维护2亿个栅格节点。即使栅格精度放宽到5米也要1600万个节点内存和搜索效率都很难看。RRT类算法完全不同它不在离散栅格上做搜索而是直接在连续空间里随机采样。整个环境只需要维护障碍物列表每条采样得到的路径点交给碰撞检测函数判断是否合法即可。这种方式不需要预先离散化整个空间采样点稀疏时计算量小采样点密集时精度也会自然提高把“搜索”和“地图精度”解耦了。1.2 单向RRT在远距离飞行规划中的致命缺陷RRT的基本逻辑是从起点开始长一棵树每次随机采样一个点然后在树上找离采样点最近的节点朝采样点方向扩展一步。这么做的探索能力很强但实际用起来有几个问题。首先是目标导向性差。随机采样是均匀分布的当起点和目标点距离较远时大量采样点集中在无用的方向上树的生长很“散”。为了改善可以加目标偏置也就是以一定概率直接采样目标点但这个比例调高了容易陷入局部障碍调低了又等于没用。其次是终点连接困难。由于树只在起点周围生长树枝末梢要想精确连到目标点往往非常费劲。我跑单向RRT时有相当多的情况是迭代已经进行了几千次树长得非常茂盛但就是因为没抽到能让树末端“够到”目标的采样点最后只能宣布超时。尤其在目标区域有障碍物遮蔽时单向RRT会显得很无力。所以当目标点离起点足够近、或者环境本身比较空旷时单向RRT是好用的。可一旦目标距离远、地形复杂就必须考虑从两端同时生长的思路这就是B-RRT存在的意义。2. B-RRT双向树的加速逻辑两棵树如何从两端“会师”2.1 双向树生长的基本框架B-RRT的思路听起来像废话文学但确实有效既然从起点出发搜到目标很慢那为什么不从起点和终点同时出发两棵树相向生长具体流程是这样的。初始化时生成一棵以起点为根的树生成另一棵以终点为根的树。每一轮迭代里先随机采样一个点从当前扩展的树上找最近节点向采样点扩展得到新节点q_new。如果扩展成功就尝试从另一棵树找最近的节点向q_new方向连过去。如果两棵树之间的距离小于某个阈值通常取扩展步长说明它们成功“握手”一条完整路径就找到了。注意这里有一个细节两棵树并不是每轮都同时各长一步。常见写法是每轮只让一棵树生长下一轮换另一棵生长交替进行。这样有一个明显的好处两棵树的节点数量大致均衡树的形状也比较均匀不容易出现一棵树疯狂生长、另一棵还停留在原始区域的情况。2.2 为什么双树结构能显著降低采样浪费从搜索空间利用率来看单向RRT需要从起点覆盖到目标的整个区域相当于一个人在一间大房间里从门口走到墙角每一步都从门口附近出发寻找方向。而B-RRT相当于两个人分别从两端往中间走各自只需要覆盖一半甚至更少的区域。把问题抽象到概率层面也说得通。在单向RRT中新增节点只有落到目标邻域附近才可能找到目标点。而在B-RRT中新增节点只要落到另一棵树附近就算成功连接。另一棵树的形状是不断长大的覆盖范围随着迭代增加而扩大因此“中彩票”的机会远大于单向RRT。实际跑起来也确实如此相同迭代次数下B-RRT的连通率比单向RRT高很多。2.3 目标偏置和连接策略对算法的影响为了让双向树更快相遇常见的做法是给采样加一点“偏心”。比如在采样时以一定概率直接选择另一棵树的节点或者路径终点方向。这种做法在Matlab里很好实现核心是rand函数和一个概率阈值if rand goalBias samplePoint target; % 当前扩展树的目标端 else samplePoint [rand * mapRange(1), rand * mapRange(2), rand * mapRange(3)]; end我一般把goalBias设置到0.05到0.1之间过高会让树过于集中地往一条直线方向探索反而削弱了随机性。另一个容易被忽略的点是连接阈值。两棵树的节点之间不一定非要精确到同一个坐标才算连接成功只要距离小于等于步长就可以认为它们能通过一条直线段联通同时要保证这条直线段不撞障碍物。这样大大降低了“精确对接”的要求让两条搜索链能在较大范围内轻松握手。2.4 狭窄通道场景下B-RRT的局限性必须老实说标准B-RRT并不是所有场景都占优。如果环境中存在一条特别狭窄的通道比如两栋楼之间只有一条很细的缝隙那么两棵树很可能会在缝隙两侧各自“徘徊”所生成的节点沿着通道边缘分布很难恰好穿过缝隙完成对接。这种情况下单向RRT和B-RRT都会面临挑战更实用的做法是引入采样区域偏置或者使用基于概率完备性的RRT*系列变体。所以B-RRT提升的是常规复杂环境的搜索效率而不是解决狭窄通道的银弹。3. Matlab三维空间建模障碍物表示与碰撞检测的数学细节3.1 三维环境怎么用Matlab描述最方便在Matlab里做三维路径规划时环境一般可以抽象成一块长方体区域。定义区域范围可以直接用一个三行两列的数组保存每个维度的上下界方便后面做边界判断。障碍物则可以根据实际场景用不同几何体组合表示最省事的是球体和长方体因为判断点是否在内部都只需要几条数学公式。我是这样定义障碍物结构体的obstacles(1).type sphere; obstacles(1).center [50, 50, 30]; obstacles(1).radius 12; obstacles(2).type box; obstacles(2).center [80, 60, 20]; obstacles(2).halfSize [10, 8, 15];球障碍物适合模拟树木、路灯、空中浮球这类近似各向同性的障碍长方体适合模拟楼宇、塔吊、建筑的方正外形。定义好之后后面所有碰撞检测都只需要遍历这个结构体数组。3.2 点碰撞检测核心数学判断点碰撞检测是路径规划中最基础的操作。对于球体障碍点到球心的欧氏距离小于等于半径就认为发生碰撞。对于长方体障碍需要把点坐标与长方体中心坐标做差判断每个轴上的差值是否都落在半尺寸范围内。function collide isCollisionPoint(p, obstacles) collide false; for k 1:numel(obstacles) ob obstacles(k); if strcmp(ob.type, sphere) if norm(p - ob.center) ob.radius collide true; return; end elseif strcmp(ob.type, box) d abs(p - ob.center); if all(d ob.halfSize) collide true; return; end end end end这个函数写得快也容易懂但工程上建议在障碍物数量较多时提前把障碍物中心、半径、半尺寸等字段索引到独立数组中避免每次调用都做字符串比较。3.3 线段碰撞检测不能只判断端点很早之前我犯过一个错只检查新节点和最近邻节点这两个端点是否在障碍物内部。结果经常出现一种诡异情况两端点都在安全区但节点之间的线段正好斜穿障碍物棱角。原因很简单三维空间里的障碍物是有体积的两个点绕过了球心不代表两点之间的连线能绕过整个球。正确的做法是沿线段做离散化检查。把线段按一定分辨率分成多个采样点逐个调用点碰撞检测。比如每隔0.1米检查一次对于长度为5米的扩展线段需要检查约50个点计算量也完全可接受。function collide isCollisionSegment(p1, p2, obstacles, resolution) dist norm(p2 - p1); numSamples max(2, ceil(dist / resolution)); collide false; for i 0:numSamples t i / numSamples; p p1 t * (p2 - p1); if isCollisionPoint(p, obstacles) collide true; return; end end end这里分辨率的选择要结合步长来看。如果扩展步长是5分辨率设0.5就够如果步长很大比如20分辨率就得加密否则线段可能直接“穿”过小障碍物。3.4 最近邻搜索的加速问题在标准RRT里每轮迭代都要在已有树的所有节点中寻找距离采样点最近的节点。树长了之后这个线性搜索会逐渐成为性能瓶颈。Matlab里最简单的优化是用自带的knnsearch配合KD树对象尤其当节点规模超过1000时速度差距非常明显。tree KDTreeSearcher(nodes); idx knnsearch(tree, samplePoint); nearestNode nodes(idx, :);不过要注意如果使用自定义的结构体数组存储节点需要先把节点坐标提取成标准矩阵再建树。KD树在动态加节点时重建开销较大所以通常是每隔一定迭代次数重建一次而不是每一轮都重建。4. 双向树主循环与路径回溯一篇能直接照抄的Matlab核心代码4.1 节点数据结构设计实现B-RRT时节点树最简单的存储方式是一个N×4的矩阵前三列存三维坐标第四列存父节点在矩阵中的行号。起点树的根节点父号设为0终点树的根节点父号同样设为0。tree1 zeros(maxIter, 4); tree1(1,:) [startPoint, 0]; tree2(1,:) [goalPoint, 0]; tree1Count 1; tree2Count 1;之所以用矩阵而不是结构体数组主要原因是Matlab对矩阵操作的性能比结构体要稳定而且后面做最近邻搜索时可以直接把坐标列交给KDTreeSearcher不需要额外转换。4.2 主循环核心框架我把主循环框架写成下面这样去掉了一些边界检查的细枝末节保证逻辑足够清晰maxIter 5000; stepSize 5; goalBias 0.1; resolution 0.5; for iter 1:maxIter % 随机采样点以一定概率偏向另一端目标 if rand goalBias samplePoint goalPoint; else samplePoint mapRange(:,1) rand(1,3) .* (mapRange(:,2) - mapRange(:,1)); end % 在当前树上求最近节点并扩展 nearestIdx knnsearch(tree1(1:tree1Count, 1:3), samplePoint); nearestNode tree1(nearestIdx, 1:3); direction (samplePoint - nearestNode) / norm(samplePoint - nearestNode); qNew nearestNode direction * stepSize; % 检查扩展线段是否碰撞 if ~isCollisionSegment(nearestNode, qNew, obstacles, resolution) if isInBoundary(qNew, mapRange) tree1Count tree1Count 1; tree1(tree1Count, :) [qNew, nearestIdx]; % 尝试连接另一棵树 nearestIdx2 knnsearch(tree2(1:tree2Count, 1:3), qNew); qNear2 tree2(nearestIdx2, 1:3); if norm(qNew - qNear2) stepSize ~isCollisionSegment(qNew, qNear2, obstacles, resolution) % 成功握手记录连接信息 connectIdx nearestIdx2; path extractBidirectionalPath(tree1, tree2, tree1Count, connectIdx); return; end end end % 交换两棵树下一轮生长另一棵 tempTree tree1; tree1 tree2; tree2 tempTree; tempCount tree1Count; tree1Count tree2Count; tree2Count tempCount; end注意交换之后tree1和tree2的身份会互换所以下一轮采样时“偏向的目标”也要跟着换。这就是为什么我上面伪代码里用goalPoint和startPoint时心里要清楚当前树到底是起点树还是目标树。4.3 路径回溯别把方向搞反了找到连接成功的节点后需要从两个方向回溯整条路径。一个方向是从连接节点直接沿着树1的父节点链条回溯到起点另一个方向是从连接节点沿着树2的父节点链条回溯到终点。由于两棵树的父节点指针方向不同最常见的问题就是回溯完成后路径顺序错乱。function path extractBidirectionalPath(tree1, tree2, idx1, idx2) path1 []; current idx1; while current 0 path1 [tree1(current, 1:3); path1]; current tree1(current, 4); end path2 []; current idx2; while current 0 path2 [path2; tree2(current, 1:3)]; current tree2(current, 4); end path [path1; path2]; end这里path1在回溯时用了“头部插入”的方式保证从起点开始到连接点结束的次序。path2则直接用“尾部追加”连接点已经在第一位一路追加到终点。两者拼接后就是从起点到终点的完整路径。如果发现拼接后方向反了大概率是path1扩展时没有做头部插入或者path2的顺序反了这是最常见的新手坑。4.4 浮点误差与边界条件的坑三维坐标运算大量涉及浮点数因此用判断坐标相等属于危险操作。我在实际代码里从来不用坐标相等来判定两棵树连接只判断距离是否小于阈值。边界判断同样要留余量不要写p(1) bounds(1,1) p(1) bounds(1,2)而应该在右边界减去一个极小量防止生成的节点恰好落在边界外壳上导致后续线段碰撞检测时出现越界采样点。5. 路径不是画出来就能飞平滑、剪枝与四旋翼可行性检查5.1 原生RRT路径为什么不能直接下发飞控RRT生成的路径本质上是一串折线节点。虽然折线在几何上连通了起点和终点但四旋翼无人机在实际飞行中不能瞬间改变速度方向它受动力学限制存在最小转弯半径和最大爬升角。如果你直接把折线路径丢给底层飞控飞机到每个转折点时都要做剧烈减速甚至悬停转向既费电又危险。此外RRT路径带有随机采样带来的锯齿问题路径长度往往不是最优的某些中途节点完全是多余采样的产物。所以在把路径交给飞控前必须做后处理。5.2 路径剪枝优先移除冗余节点剪枝的思路很直接从起点开始尝试直接连接一个较远的非相邻节点如果这条短线段不碰障碍就可以把中间那一串节点全部删掉。用代码实现时我一般配一个循环simplifiedPath path(1, :); i 1; while i size(path, 1) % 尝试向前找最远的可直达节点 j size(path, 1); while j i 1 isCollisionSegment(path(i, :), path(j, :), obstacles, resolution) j j - 1; end simplifiedPath [simplifiedPath; path(j, :)]; i j; end剪枝之后的路径节点数量往往能减少三到五成路径长度也会缩短转折次数变少。当然剪枝后的线段仍然可能是快速转角形式所以还需要下一步平滑。5.3 用B样条在Matlab里做轨迹平滑平滑曲线的经典做法是B样条。Matlab里可以直接用spcrv函数它能把一组离散控制点拟合成一条光滑曲线。我通常的做法是先把剪枝后的路径点作为控制点传入然后用一个比较大的点数做采样密集化。smoothPath spcrv(simplifiedPath, 3, 100);spcrv的用法不需要额外工具箱属于Matlab基础函数。三个参数分别是控制点矩阵、曲线阶数我常用3也就是三次B样条和采样点数。输出结果是一串更密集的三维坐标点原路径的几何轮廓被保留下来但转折被抹圆了。要提醒一句B样条曲线不一定会严格经过所有控制点它是被控制点“牵引”的因此平滑后的路径有可能略微偏离障碍物。如果对安全距离有硬性要求最好在平滑后逐点检查碰撞一旦发现碰撞点就需要局部删除碰撞点附近的关键转折点并重新拟合或者把安全距离作为控制点偏移量。5.4 四旋翼可行性检查的几个硬指标平滑之后还不能直接松一口气至少要检查这几个指标最大爬升角把路径按时间离散化计算相邻点的高度差与水平距离之比。四旋翼一般爬升角在35度到45度之间这个值要根据机型评估。如果某一段比值过大就应该加密该段路径点或者局部重规划。转弯半径计算连续三个点的转向角度变化。连续小角度转折在平滑路径上通常不会有问题但幅度陡变的地方仍可能造成横向过载。路径总长度无人机续航有限过长的路径意味着耗电更高。如果剪枝和平滑之后路径长度仍然比直线代价高出一倍以上我会考虑调整采样参数重新规划。把这些检查逻辑写成一个constraintCheck函数输出布尔值来判断路径是否可行在开发阶段非常有用。6. 参数调优与单双向对比B-RRT实际能快多少6.1 关键参数及其作用范围B-RRT里面的参数不多但每个都对结果影响巨大。我做了一个表格供参考地图尺度大约在100×100×60障碍物数量10到20个之间参数建议初始值作用与影响stepSize扩展步长3~8步长越小路径越精细但迭代次数越多步长太大容易穿过窄缝maxIter最大迭代次数3000~8000过低可能找不到路径过高增加无效耗时goalBias目标偏置概率0.05~0.15越大越倾向目标方向但容易丢失随机探索能力resolution碰撞检测分辨率0.3~0.8越小检测越精确但计算越耗时connectThreshold握手阈值等于stepSize阈值太小两棵树难以汇合太大会把障碍附近的非法连接判定为合法如果路径经常找不到优先增大maxIter而不是狂调goalBias。如果路径经常穿墙最可能是resolution设得太大。如果路径整体锯齿感很强先检查stepSize是否过大。6.2 同一场景下单向RRT与B-RRT的对比实验为了直观评估B-RRT优势我在Matlab里固定同一张随机障碍物地图、同一个起点和终点分别跑了单向RRT和双向B-RRT各30次统计平均值。环境范围100×100×60障碍物为10个随机生成的球体起点在左下角终点在右上角初始航向无约束。指标单向RRT双向B-RRT平均搜索时间秒4.821.56平均迭代次数612238平均路径长度146.8141.230次内连通失败次数41这个结果基本符合预期。B-RRT在搜索时间上快了大约三倍迭代次数减少了一半以上路径长度也略微缩短。值得注意的是即便如此B-RRT仍有一次连通失败这说明基于采样的算法本质上有随机性工程使用时必须要设定最大迭代次数兜底不能无限等下去。6.3 随机种子与复现问题Matlab里rng函数控制随机数生成器。我复现实验时习惯在每个算法运行前调用rng(42)这样每次生成的障碍物和采样点序列完全一致便于对比不同参数的差异。也有一种情况是肉眼看起来“随机地图一样”但两个算法运行结果不同那可能是因为两棵树交替扩展时调用rand的顺序不一致导致路径完全不同。这不是bug只是随机过程对调用次序敏感。6.4 工程落地时的建议组合拳在真机使用前B-RRT通常还要配合局部避障模块。全局规划器负责在宏观尺度上规划一条可行路径而局部避障器负责检测新出现的动态障碍并做微小偏转。如果不加局部避障几十秒的飞行中一旦有动态障碍物迎面而来全局路径是无法及时响应的。所以我的完整建议是B-RRT负责全局粗路径剪枝平滑负责轨迹优化局部避障负责实时安全三者缺一不可。7. 源码运行常见问题与我的收尾体会7.1 Matlab环境准备与运行注意事项不管是用自己的实现还是拿到别人的源码第一步永远是确认Matlab版本。B-RRT主循环用到了矩阵索引、结构体、knnsearch等基础功能R2016b及以后的版本基本都能运行。如果遇到某些函数显示“未定义”大概率是版本过老到对应版本的文档里查一下替代函数就行。中文注释乱码是另一个常见问题。在旧版Matlab中打开别人写的源码注释有时会显示成乱码因为文件保存编码和系统编码不一致。用记事本打开.m文件另存为UTF-8编码再重新打开一般就能解决。7.2 运行结果非预期时的排查顺序如果B-RRT跑出来的路径明显绕路或者找不到路径不要急着改参数先按顺序排查先看障碍物定义是否合理起点和终点是不是不小心放在了障碍物内部然后看碰撞检测函数在单独测试时能否准确判断出已知的碰撞点再看最近邻搜索是否索引正确经常有人把tree1和tree2的索引搞混导致回溯时连接点断链。如果以上都正常最后才调参数。我会把maxIter设到足够大比如先设5000跑一次看迭代过程是否在两棵树快要相遇时因为阈值太小而失败。如果是把connectThreshold稍微调大一点通常设为stepSize * 1.2也能接受。7.3 一点个人经验我从这个项目里最大的收获是RRT系列算法的关键是“采样策略”和“碰撞检测精度”而不是那些花哨的改进名字。B-RRT相比单向RRT确实给力但也不要神话它。真正能落地的路径规划方案永远是把算法当成一个组件再配上平滑、剪枝、约束检查这些后处理环节。如果你现在正卡在某一步跑不通建议先用手工构造的最简单三维场景两三个球障碍物起点终点放得很远但中间空旷验证主循环逻辑通了之后再逐步增加障碍物密度这样能省下大量调试时间。