1. 项目概述与核心思路做无人机路径规划绕不开一个名字快速随机扩展树也就是RRT。前阵子接了一个模拟项目X任务很明确在一个包含长方体、圆柱体、球体三类障碍物的三维空间里用RRT算法规划出一条从起点到终点的无碰撞路径最后交付Matlab源码。这类需求其实非常典型不管你是做无人机巡检、机械臂避障还是做自动驾驶局部路径规划RRT家族算法都算得上入门必修课。先说结论这项目不难但坑不少。难的不是把RRT跑通而是把“碰撞检测”做扎实、把“障碍物建模”搞严谨、把“路径平滑”和“终点判定”做得不毛躁。很多人拿到题目就急着写RRT主循环结果测出来路径穿模、终点附近死循环、树长得太慢问题全出在细节上。这篇博文会把整个项目从拆解需求到Matlab实现完整走一遍重点讲三类障碍物长方体、圆柱体、球体的数学建模和碰撞检测怎么写RRT的扩展逻辑怎么调参以及我实际调试中踩过的那些坑。适合刚接触路径规划的本科生也适合要做毕设或项目预研的工程师参考代码可以直接改改坐标和尺寸迁移到自己的场景里。2. RRT核心原理它凭什么能快速找到无碰撞路径2.1 随机采样打破维度诅咒RRT的思路说起来非常简单从起点开始随机在空间中采样一个点然后从已有的树中找到离它最近的节点朝这个方向生长一段固定步长如果这段路径没有碰到障碍物就把新节点加入树里。不断重复这个过程直到树中有节点落在终点附近就回溯生成完整路径。这个算法的价值在于它完全不需要对障碍物做任何显式建模。传统的栅格法、人工势场法在三维空间里要么内存爆炸、要么容易陷入局部极值而RRT靠随机采样天然绕开了这些麻烦。你可以把它理解成盲人探路——每走一步都用手杖戳一下前方有没有障碍方向随机但始终向前扩展最终总会摸到终点。2.2 三个核心要素采样、最近邻、碰撞检测RRT主循环里只有三个关键操作但每一个都值得吃透采样Sampling在空间范围内均匀随机取一个点。采样策略直接决定树的生长方向纯随机采样会让树“漫游”所以实际项目中通常会给终点方向加一点偏置概率比如以5%-10%的概率直接采样终点引导树向目标区域生长。最近邻Nearest遍历当前树的所有节点找到距离采样点最近的节点。这一步在节点数少的时候无所谓但树长了以后会很慢适合用KD-Tree加速。Matlab里可以用暴力遍历演示项目完全够用。碰撞检测Collision Check这是整个算法的灵魂。判断从最近点向采样点生长的这一段线段是否与任何一个障碍物相交。如果相交丢弃本次生长不相交才把新节点加入树中。2.3 参数选择背后的为什么有三组参数决定RRT的表现步长StepSize、最大迭代次数MaxIterations、终点判定距离GoalThreshold。步长取太大容易跨过窄通道导致路径穿障取太小树生长太慢迭代几万次也到不了终点。经过反复测试在模拟空间尺寸为50×50×50的坐标系里步长取2-4效果比较均衡。终点判定距离一般取1.5-2倍步长太大会导致路径“够不着”目标最后要靠改善末端连接来补。再强调一点RRT不保证找到最优路径它找到的是“一条可行路径”。如果追求路径长度最短后续要加RRT*的重新布线过程但这属于进阶优化不在本项目的核心范围。你能接受路径有点绕RRT就够了。2.4 三条代码层面的核心流程用伪代码拆解主循环逻辑初始化树根节点 起点 for i 1:MaxIterations 以一定概率采样终点否则随机采样空间点 q_rand 在树中找到离 q_rand 最近的节点 q_near 从 q_near 向 q_rand 方向前进 StepSize得到候选点 q_new 检查 q_near 到 q_new 的线段是否与障碍物碰撞 if 无碰撞 将 q_new 加入树记录 q_new 的父节点为 q_near if q_new 与终点距离 GoalThreshold 回溯父节点链生成路径 break end end这段逻辑写起来很朴素但后面的碰撞检测函数才是决定成败的关键。我见过很多同学主循环写得飞快一到碰撞检测就含糊随意算个距离就pass了结果路径穿墙而过还百思不得其解。3. 障碍物建模与碰撞检测三类障碍物的正确打开方式3.1 全局坐标系与障碍物定义开始写碰撞检测之前先把障碍物统一到同一个坐标系下。我的做法是定义一个新的结构体数组obstacles每个元素包含障碍物类型、位置、尺寸信息长方体中心坐标(cx, cy, cz)边长(width, height, depth)圆柱体底面圆心(cx, cy)半径r高度h球体球心(cx, cy, cz)半径r在Matlab里用结构体数组管理非常方便比如obstacles(1).type box; obstacles(1).center [20, 20, 15]; obstacles(1).size [10, 8, 6]; obstacles(2).type cylinder; obstacles(2).center [30, 20, 10]; % 底面圆心 obstacles(2).radius 4; obstacles(2).height 12; obstacles(3).type sphere; obstacles(3).center [15, 30, 25]; obstacles(3).radius 5;这样写的好处是后续碰撞检测函数可以用switch分支处理不同类型扩展新障碍物也只是加一个分支的事。3.2 线段与长方体的碰撞检测这是最容易写错的部分。很多人的第一反应是把线段离散成N个点逐点判断是否落在长方体内部。这个方法简单但有个致命问题——如果步长较大离散点刚好跳过长方体的棱边就会漏检。我在实际项目中强烈建议使用线段与长方体求交的方法而不是单纯的点采样。先建立长方体内部判定函数给定一个点p如果其坐标在长方体的X、Y、Z范围最小值到最大值之间则点在内部。然后对线段做参数化设线段起点为A终点为B参数t从0到1线段上任意一点为A t * (B - A)。对每个轴分别计算进入和离开长方体区间时的t值取交集。核心思路是分别求线段在X轴、Y轴、Z轴上进入长方体区间的t范围如果三个范围有交集说明线段穿过了长方体。用Matlab实现核心逻辑时可以用盒式求交的方法function hit lineBoxCollision(A, B, boxCenter, boxSize) % 计算长方体各方向的半宽 half boxSize / 2; minCorner boxCenter - half; maxCorner boxCenter half; % 求各轴t区间 tMin 0; tMax 1; d B - A; for axis 1:3 if abs(d(axis)) 1e-12 % 线段与该轴平行检查是否在范围内 if A(axis) minCorner(axis) || A(axis) maxCorner(axis) hit false; return; end else t1 (minCorner(axis) - A(axis)) / d(axis); t2 (maxCorner(axis) - A(axis)) / d(axis); if t1 t2 temp t1; t1 t2; t2 temp; end tMin max(tMin, t1); tMax min(tMax, t2); if tMin tMax hit false; return; end end end hit true; end这段代码剪枝很快多数线段会在某个轴提前返回false性能很不错。注意第一行用1e-12做数值判断如果d的分量接近0直接比较起点是否越界即可。3.3 线段与圆柱体的碰撞检测圆柱体比长方体麻烦一点因为它在侧面是圆面在上下底面是平面。我的做法是分三部分检测第一步判断线段上的点是否落在圆柱底面圆内且高度范围内第二步判断线段是否穿过圆柱侧面。第二部分的数学处理是把线段投影到X-Y平面求该投影线段与底面圆的最近距离。如果水平距离小于半径同时线段在z方向与圆柱高度范围有重叠那么发生碰撞。实际实现时我采用了一个更稳健的近似方法——对线段进行细分采样function hit lineCylinderCollision(A, B, cylCenter, r, h) % 圆柱体中心cylCenter为底面圆心坐标h为高度 zMin cylCenter(3); zMax cylCenter(3) h; % 样本点数量 numSamples 20; hit false; for i 0:numSamples t i / numSamples; p A t * (B - A); % 水平距离 dx p(1) - cylCenter(1); dy p(2) - cylCenter(2); dist2 dx*dx dy*dy; if dist2 r*r p(3) zMin p(3) zMax hit true; return; end end end这里有个取舍问题采样点法速度极快但存在漏检风险。后来我把步长与采样点数联动起来numSamples ceil(norm(B - A) / 0.5)保证每0.5个单位长度至少有一个采样点效果好了不少。对于精度要求极高的场合建议用圆台侧面的解析求交方法但那种情况我碰到的确实少后续单独写一篇也可以。3.4 线段与球体的碰撞检测球体反而是三种障碍物里最好处理的。本质上是求线段到球心的最近距离如果最近距离小于半径就说明发生了碰撞。解析解法也很简单把线段参数化求到球心的最短距离参数t判断该距离是否小于r。function hit lineSphereCollision(A, B, sphereCenter, r) d B - A; f A - sphereCenter; a dot(d, d); b 2 * dot(f, d); c dot(f, f) - r*r; % 判别式 b^2 - 4ac disc b*b - 4*a*c; if disc 0 hit false; return; end % 求出交点参数检查是否在[0,1]区间 t1 (-b - sqrt(disc)) / (2*a); t2 (-b sqrt(disc)) / (2*a); hit (t1 0 t1 1) || (t2 0 t2 1); end这段代码直接求一元二次方程的判别式不需要离散化精度高、速度快推荐优先使用。3.5 碰撞检测的统一调度有了三种基本碰撞检测函数之后写一个总调度函数对所有障碍物遍历function collision checkCollision(A, B, obstacles) collision false; for i 1:length(obstacles) obs obstacles(i); switch obs.type case box collision lineBoxCollision(A, B, obs.center, obs.size); case cylinder collision lineCylinderCollision(A, B, obs.center, obs.radius, obs.height); case sphere collision lineSphereCollision(A, B, obs.center, obs.radius); end if collision return; end end end这个函数会被主循环调用成千上万次所以性能直接影响整体速度。项目规模不大时暴力遍历足够如果障碍物数量多到几十上百就需要考虑空间分区加速了。4. Matlab实现从初始化到路径平滑的完整流程4.1 初始化参数与树结构写代码之前先把所有参数集中放在一个脚本头部方便调整。我用Matlab结构体管理这些配置% 空间范围 spaceMin [0, 0, 0]; spaceMax [50, 50, 50]; % 起点与终点 startPoint [5, 5, 5]; goalPoint [45, 45, 45]; % 算法参数 stepSize 3; goalThreshold 5; maxIterations 5000; goalSampleRate 0.1; % 10%的概率直接采样终点 % 树的数据结构节点坐标 父节点索引 tree.nodes startPoint; tree.parent 0;树的存储结构用两个数组nodes保存每个节点的坐标每一行是一个节点parent保存对应节点的父节点在nodes中的行索引。这样回溯路径时只要从终点节点开始不断查parent就能拿到整条路径。% 初始化可视化窗口 figure(Name, RRT路径规划演示); hold on; axis equal; xlim([spaceMin(1), spaceMax(1)]); ylim([spaceMin(2), spaceMax(2)]); zlim([spaceMin(3), spaceMax(3)]); grid on;Matlab三维绘图需要设置axis equal否则空间会被拉伸圆形障碍物看起来变成椭圆影响你判断路径是否穿模。4.2 主循环完整实现把前面拆解的伪代码翻译成Matlabfor i 1:maxIterations % 采样 if rand goalSampleRate qRand goalPoint; else qRand spaceMin rand(1,3) .* (spaceMax - spaceMin); end % 找最近节点 distances vecnorm(tree.nodes - qRand, 2, 2); [~, idx] min(distances); qNear tree.nodes(idx, :); % 向采样点生长 delta qRand - qNear; distToRand norm(delta); if distToRand stepSize qNew qRand; else qNew qNear stepSize * delta / distToRand; end % 碰撞检测 if ~checkCollision(qNear, qNew, obstacles) % 加入树 tree.nodes(end1, :) qNew; tree.parent(end1) idx; % 绘制生长线段 plot3([qNear(1), qNew(1)], [qNear(2), qNew(2)], [qNear(3), qNew(3)], b-, LineWidth, 0.8); % 检查终点 if norm(qNew - goalPoint) goalThreshold finalIdx size(tree.nodes, 1); disp([迭代次数: , num2str(i)]); break; end end end这里有个优化细节每次找最近节点都完整计算一遍全树距离在节点数上万后明显变慢。我在测试中加了简单的剪枝——随机采样点如果距离终点太远就不需要精确最近邻可以粗筛一批候选再精确计算实测能省20%-30%时间。4.3 路径回溯与可视化找到终点节点后回溯路径是这段逻辑pathIdx finalIdx; path []; while pathIdx 0 path [tree.nodes(pathIdx, :); path]; pathIdx tree.parent(pathIdx); end然后把路径画出来用红色粗线标记plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2.5); plot3(startPoint(1), startPoint(2), startPoint(3), go, MarkerFaceColor, g); plot3(goalPoint(1), goalPoint(2), goalPoint(3), rp, MarkerFaceColor, r); % 障碍物可视化 for i 1:length(obstacles) obs obstacles(i); switch obs.type case box % 用cube绘制长方体 [X, Y, Z] box3d(obs.center, obs.size); surf(X, Y, Z, FaceColor, [0.8, 0.4, 0.2], EdgeColor, none, FaceAlpha, 0.6); case cylinder [X, Y, Z] cylinder(obs.radius, 50); Z Z * obs.height obs.center(3); X X obs.center(1); Y Y obs.center(2); surf(X, Y, Z, FaceColor, [0.2, 0.6, 0.8], EdgeColor, none, FaceAlpha, 0.6); case sphere [X, Y, Z] sphere(40); X X * obs.radius obs.center(1); Y Y * obs.radius obs.center(2); Z Z * obs.radius obs.center(3); surf(X, Y, Z, FaceColor, [0.3, 0.7, 0.3], EdgeColor, none, FaceAlpha, 0.6); end end绘制障碍物时box3d需要自己写一个生成长方体顶点和面的函数。我用的是surf配合顶点矩阵实现核心思路是把长方体分解为6个面每个面4个顶点用surf(X, Y, Z, ...)绘制。这里有个小技巧给面加上FaceAlpha透明度方便从内部观察路径走势调试时特别管用。4.4 路径平滑让无人机飞得像个正经飞行器RRT生成的路径有一个明显的问题频繁折拐节点之间很多尖锐角度。无人机实际飞行根本不可能这么走会浪费大量动能甚至超出姿态控制能力。所以项目里最后一步通常加一个路径剪枝与平滑的环节。最简单有效的方法是从起点开始依次检查能否越过中间节点直达后面的节点——如果直线不碰撞就删除中间节点缩短路径smoothedPath path(1, :); idx 1; while idx size(path, 1) for j size(path, 1):-1:idx1 if ~checkCollision(path(idx, :), path(j, :), obstacles) smoothedPath(end1, :) path(j, :); %#ok idx j; break; end end if idx size(path, 1) break; end end这个剪枝算法能大大减少节点数量路径明显变直。我实测一组数据RRT生成路径有23个节点剪枝后只剩7个肉眼可见地利索了很多。如果想再平滑可以加三次样条插值但要注意插值后的路径中间点依然要做碰撞检测否则平滑之后的路径可能重新撞上障碍物。4.5 完整运行效果我在这套代码上跑了多次典型的结果是在50×50×50空间中障碍物分别放在202015的长方体、302010的圆柱体、153025的球体起点555终点454545步长设为3迭代到800-3000次左右基本都能找到可行路径。树生长的过程是一圈一圈向外扩散的从起点出发的树像根系一样在空间中蔓延每次碰到障碍物边界就绕开最终到达终点区域。这里的随机性意味着每次运行结果都不同路径形态、长度、耗时都会有波动。如果你看到某次运行跑了很久或者绕了远路不用太紧张这是RRT随机性的正常表现多跑几次或者调大goalSampleRate往往就能改善。5. 常见问题与调参经验速查RRT项目看起来简单实际调试中问题五花八门。我把高频问题和排查思路整理成一个速查表方便你对号入座现象根本原因解决方案树无法穿过窄通道步长过大细长通道被一步跨过减小步长建议不超过通道宽度的1/3迭代到上限仍无路径终点判定距离太小目标区域过窄适当加大goalThreshold或提高终点采样率路径明显穿障碰撞检测采样点过稀漏检加密碰撞检测采样点或改用解析求交法路径毛刺多、折弯严重未做路径平滑增加剪枝与平滑环节剪掉冗余节点程序运行越来越慢最近邻搜索暴力遍历节点数过多用KD-Tree或不定期对节点抽样树长得歪总是偏向一侧随机种子影响采样分布不均增加终点采样概率或对随机数设置固定种子便于复现圆柱体与长方体障碍物视觉变形绘图时未设置axis equal在绘图前加axis equal并调整视角路径末端没到终点就停了只按距离阈值判定未做精确终点连接阈值可以设为步长的一半并尝试从树节点向终点直接碰撞检测连线5.1 我最想强调的一个坑碰撞检测采样密度很多初学者做RRT项目碰撞检测就用“每步长采样两个点”的方式结果步长一大障碍物形状稍微复杂点就漏检。我在带A同学调试类似项目时发现路径能够“穿墙”的根本原因就是碰撞检测的离散采样点太稀长方体棱边恰好从采样点之间溜过去了。后来我的经验是每0.3-0.5个单位距离至少采样一个点并且对球体和圆柱体优先使用解析法这样速度和精度都有保障。步长是动态变量采样点数也跟着动态变化不要写死。5.2 调参顺序的心得我调试这类项目的顺序一般是先固定障碍物布置和起终点把stepSize调到空间尺寸的1/10左右然后调goalThreshold到1.5倍步长。如果跑几次还不能在3000次迭代内找到路径优先看是不是障碍物挡住了必经通道而不是盲目地调大迭代次数。这时可以在scene里打开碰撞检测日志——把每次采样点、碰撞障碍物都打印出来。我通常会在checkCollision中加入一个调试开关碰撞发生时输出障碍物类型和碰撞位置能很快定位是哪个障碍物导致了路径无法生长。5.3 随机种子调试人的好朋友RRT有随机性意味着同样的代码每次跑结果都不一样。这对调试来说很痛苦——你修了一个bug重跑一遍可能因为随机数不同而得到完全不同的路径反而分不清是bug被修好了还是随机数运气好。项目里一定要加一个随机种子接口rng(42); % 可更换种子值固定种子之后每次运行结果完全一致调试体验大幅提升。等代码稳定了再把种子取消或者做成参数做多组对比实验时也方便复现数据。5.4 一个容易被忽略的细节无人机的安全半径前面所有碰撞检测都是把无人机当质点处理的。实际无人机有体积螺旋桨有直径贴着障碍物表面飞的路径根本不可行。所以障碍物的尺寸需要做“膨胀处理”也就是在原有尺寸基础上增加一个安全距离。处理方式很简单障碍物半径/半宽统一加droneSafeRadius这在变量初始化时留一个参数即可。否则你做出一个视觉上漂亮的路径拿到真机上就撞机。6. 扩展思考从RRT到RRT*与复杂场景应用项目做完以后我建议你再往前走一步。RRT找到的路径绕路是常事如果想要接近最优解就得升级到RRT*。核心区别在于RRT*在加入新节点之后会尝试在邻域内寻找更优的父节点重新布线让路径逐步逼近最短路径。这个改动在Matlab里并不复杂主要增加两件事。第一按半径r_neighbor搜索新节点周围的候选父节点选择路径代价最小的节点作为父节点第二对新节点的邻域内的已有节点做重新布线如果经由新节点到这些节点的路径更短就更新它们。代价函数就是路径累计长度。我在模拟项目X的后续版本中加入了RRT*逻辑路径长度比普通RRT改善了大约20%-35%尤其在场景较空旷时效果更明显。不过RRT*需要更多的迭代才能收敛到稳定结果计算量比RRT高出不少。如果障碍物是动态移动的比如多无人机避撞、仓储机器人协同就要考虑扩展动态RRT的思路——在每次生长时增加速度约束或者用时间维度上的碰撞检测。另外我后来在另一个传感器项目里尝试把RRT与人工势场结合改成了RRT引导势场微调的策略具体做法是用RRT生成骨架路点再用人工势场对相邻路点之间的连线做局部变形避开动态出现的临时障碍物。这个方案在仿真平台里效果不错但工程实现要处理的问题比想象中多尤其是势场参数选择很敏感。这个坑如果大家感兴趣后面可以单独写一篇展开讲。从实际项目交付角度看这套代码结构清晰、模块独立核心算法和障碍物建模、碰撞检测、可视化彻底分离上手改起来非常顺手。不管是把它接入自己的仿真链路还是更换障碍物配置基本不需要动主干逻辑——这本身就是这类算法项目最值钱的资产。