企业官网建设流程全解析

简介：这套代码基于Matlab R2021b实现RRT（快速探索随机树）算法的三维路径规划，适合学习机器人运动规划、采样类搜索算法以及三维避障的开发者。算法以随机采样和局部规划为核心，通过逐步构建随机树来探索工作空间，最终连接起点与目标点；代码覆盖最近邻查找、节点扩展、碰撞检测等关键步骤，并支持球体、立方体、圆柱体等三维障碍物的定义与绘制，便于在复杂环境中直观比较不同布局下的规划结果。压缩包共17个文件，全部为.m脚本，整体仅9KB，结构紧凑，主程序与功能模块分离，可单独调用或修改采样步长、目标偏置等参数后重新运行。目前已有223人学习/浏览，适合作为课程设计、算法入门或三维路径规划实验的参考实现，也可为后续改进RRT*等变体提供基础。 做机器人路径规划的人，应该都绕不开RRT这个名字。RRT全称Rapidly-exploring Random Tree，中文叫快速扩展随机树，是一种在连续空间里做运动规划的经典算法。最近我把之前一直停在二维的RRT实现搬到了三维场景，用Matlab R2021b从零写了一套三维RRT，用来给机械臂和无人机这类需要空间避障的对象做全局路径搜索。今天这篇内容不是教科书式的原理复述，而是把我在三维实现过程中踩过坑、验证过可行的方法和代码完整分享出来。无论你是刚接触RRT的新手，还是已经会用工具箱但想自己实现一遍的进阶玩家，这篇都能给你一点参考。

1. 为什么在三维空间写RRT：算法原理与难点

1.1 RRT算法速览：一个随机点如何长成一条路

RRT的核心思想可以概括为四个字：随机撒点。想象你站在一个完全陌生的迷宫里，手边有一根可以无限延长的绳子，每次你闭着眼睛扔出一颗石子，石子落在某个位置，你就把绳子沿最短路径向那个方向拉出一截；如果这段绳子没有碰到墙，就把它固定住。重复这个过程，绳子最终会覆盖整个可通行区域，并且大概率能探到出口。这个“扔石子-捡绳子-固定绳子”的循环，就是RRT算法的本质。

具体到算法步骤，RRT会在每次迭代里做四件事：在空间里随机采样一个点；在已经长成的树里找出离采样点最近的节点；从这个最近节点朝采样点方向扩展一个固定步长，得到新节点；检测新节点和最近节点之间连线上有没有障碍物，如果没有就把新节点挂到树上。只要新节点落在目标附近，就沿着父节点指针一路回溯，得到一条从起点到目标的路径。这个流程在二维和三维里完全一致，难点在于三维的表达和计算。

1.2 三维实现的三个坑：采样、碰撞检测、可视化

第一个坑是采样。三维空间的体积随着尺度增加是立方级增长，同样数量随机点在三维里的覆盖密度远低于二维。如果地图范围是100×100×100，而二维是100×100，要在三维达到类似的密度，采样点数量需要多一个数量级。所以三维RRT如果采样策略太朴素，树很容易在局部区域转圈，迟迟碰不到目标。这也是后面要引入目标偏置的主要原因。

第二个坑是碰撞检测。二维判断线段与圆的交点，数学上是一元二次方程；三维判断线段与球、立方体或三角网格是否相交，虽然也是几何问题，但计算量明显上升，还要小心边界条件。尤其是当步长偏大时，线段可能“跳过”小障碍物，导致视觉上明显穿模，路径却判定为无碰撞。这个细节我在后面会专门讲，它是三维RRT最容易出bug的地方。

第三个坑是可视化。用plot3画树枝和路径很容易，但要把障碍物、树枝、路径放到同一张图里，还要能旋转观察遮挡关系，就需要仔细设置颜色、光照和透明度。否则调试的时候根本看不出路径是绕过去了还是穿过去了。这三个坑在后面的实现里都会给出对应的解决办法。

2. 代码设计：地图、节点与核心函数拆解

2.1 数据结构与地图表示

写Matlab代码，我习惯先定数据结构。三维RRT的树，我用一个struct数组来表示，每个元素是一个节点，字段只有两个：pos保存三维坐标，parent保存父节点在数组里的索引。这个索引链就是将来回溯路径的关键。不需要额外定义类，R2021b的struct数组足够轻量，而且向量化计算时也很方便。

地图方面，为了不让文章被网格碰撞检测淹没，我用球体障碍物列表来建模。每行是[x, y, z, r]，分别表示球心坐标和半径。球体虽然简单，但足以验证算法框架；如果要换成长方体或任意凸多面体，只需要替换碰撞检测函数，树的主循环完全不用动，这也是把碰撞检测单独封装成函数的好处。

% 树节点示例 tree(1).pos = [0, 0, 0]; tree(1).parent = 0; % 障碍物示例：球心(50,50,50)，半径12；球心(20,80,40)，半径15 obstacles = [50, 50, 50, 12; ... 20, 80, 40, 15];

2.2 随机采样和最近邻查找的实现

随机采样是RRT的驱动力。我在采样时加入了一个小技巧：以一定概率直接返回目标点，称为目标偏置。这样做的好处是树不会完全盲目生长，而是有一部分“意识”朝目标方向扩展。概率一般设在0.05到0.2之间，太低效果不明显，太高容易把树拽进障碍物里，反而不利于探索。下面是采样函数，输入目标点、地图范围和目标偏置概率，返回一个采样点。

function sample = samplePoint(goal, bounds, goalProb) if rand < goalProb sample = goal; else sample = [bounds(1) + rand * (bounds(2) - bounds(1)), ... bounds(3) + rand * (bounds(4) - bounds(3)), ... bounds(5) + rand * (bounds(6) - bounds(5))]; end end

最近邻查找在二维和三维没有本质区别，都是算欧氏距离。树节点多的时候，全量遍历会慢，但R2021b里用向量化操作就没那么夸张。把树里所有节点的位置一次性放到一个N×3矩阵里，然后算差值平方和，取最小值的索引即可。这里要注意vertcat的写法，如果写成tree.pos会直接报错，必须用vertcat展开所有节点的pos字段。

function idx = findNearest(tree, sample) positions = vertcat(tree.pos); diff = positions - sample; dist2 = sum(diff.^2, 2); [~, idx] = min(dist2); end

2.3 碰撞检测：球体障碍物下的线段相交判断

碰撞检测是RRT里最不能省的部分。我的做法是把最近节点和新节点连成一条线段，然后判断这条线段是否与任何一个球体相交。数学上，把线段写成参数形式p(t) = p1 + t * (p2 - p1)，其中t从0到1。任意一点到球心的距离平方就是norm(p(t) - center)^2，求这个函数在[0,1]区间里的最小值，如果最小值小于半径的平方，就说明线段穿过球体。这个最小值很容易求，它发生在t = clamp(dot(center - p1, v) / dot(v, v), 0, 1)处。

function collided = collisionCheck(p1, p2, obstacles) v = p2 - p1; v2 = dot(v, v); % 如果线段长度为0，直接比较端点 if v2 == 0 collided = any(vecnorm(obstacles(:,1:3) - p1, 2, 2) < obstacles(:,4)); return; end collided = false; for i = 1:size(obstacles, 1) center = obstacles(i, 1:3); radius = obstacles(i, 4); t = dot(center - p1, v) / v2; t = max(0, min(1, t)); closest = p1 + t * v; if norm(closest - center) < radius collided = true; return; end end end

这里有个容易踩的坑：如果只检查新节点是否落在障碍物内部，而忽略连线，那么步长比较大的时候，新节点在障碍物外面，但连线穿过了障碍物，路径实际上不可达。所以必须做线段级别的检测。上面的代码已经处理了t的边界，确保计算的是线段上的最近点，而不是无限直线上的最近点。实测下来，这套球体碰撞检测在绝大多数场景下够用，而且计算速度比较快。

3. 在Matlab R2021b中完整跑通三维RRT

3.1 主函数与参数配置

把前面几个函数串起来，就是一个可运行的三维RRT。主函数我在R2021b里实测过，输入起点、终点、障碍物、地图范围、步长和最大迭代次数，最后返回一条路径。参数上，步长stepSize是最关键的，它决定树的生长速度，也影响路径质量。我习惯把步长设成地图最长维度的大约5%，太小树长得很慢，太大又容易跳过小的障碍物。最大迭代次数先给到5000，复杂场景再往上加。

function path = rrt3d(start, goal, obstacles, bounds, stepSize, maxIter) tree.pos = start; tree.parent = 0; goalProb = 0.1; threshold = stepSize * 1.2; for i = 1:maxIter sample = samplePoint(goal, bounds, goalProb); idx = findNearest(tree, sample); nearestPos = tree(idx).pos; delta = sample - nearestPos; dist = norm(delta); if dist < stepSize newPos = sample; else newPos = nearestPos + delta / dist * stepSize; end if ~collisionCheck(nearestPos, newPos, obstacles) newIdx = numel(tree) + 1; tree(newIdx).pos = newPos; tree(newIdx).parent = idx; if norm(newPos - goal) < threshold path = tracePath(tree, newIdx); disp(['path found at iteration ', num2str(i)]); return; end end end error('Reached max iterations, no feasible path found.'); end

threshold是判断是否到达目标的阈值，我设成stepSize的1.2倍，留了一点余量，避免新节点刚好卡在目标边缘但差零点几个单位就判定失败。goalProb这里设0.1，在大多数情况下能在不牺牲探索性的前提下明显加快收敛。你也可以把goalProb改成0.2试试，树会更激进地冲目标，但障碍物多的时候容易反复被挡，反而增加无效迭代。

3.2 从起点到目标：树构建与路径回溯

路径找到后，回溯的过程非常直接：从目标对应的节点开始，沿着parent索引一直往前找，直到parent为0。因为我把起点放在tree(1)，所以回溯链最终会回到索引1。这里有个容易写错的地方，回溯得到的是倒序节点列表，记得翻转回来，否则后面画图时路径是反的。

function path = tracePath(tree, goalIdx) idx = goalIdx; path = []; while idx > 0 path = [path; tree(idx).pos]; idx = tree(idx).parent; end path = flipud(path); end

如果你的树结构里节点存的是完整结构体数组，不断执行path = [path; tree(idx).pos]会触发数组拼接，当路径很长时效率偏低。更好的办法是预分配路径矩阵，然后从最后往前填。这里为了可读性用了简单写法，读者可以自行优化。实际跑起来，路径点通常只有几十个，性能影响可以忽略。

3.3 可视化：把障碍物、树和路径画到同一张三维图里

R2021b里画三维场景没有太多花样，但要注意层次。先用plot3把所有树枝画成浅灰色线条，再画障碍物，最后画最终路径，这样路径不会被其他树枝盖住。障碍物我用sphere函数生成球面网格，再平移到球心位置，缩放成对应半径。注意要设置EdgeColor为none，否则网格线会把画面弄得很乱。

function plotScene(tree, path, obstacles, start, goal) figure; hold on; grid on; axis equal; view(3); xlabel('X'); ylabel('Y'); zlabel('Z'); % 画树 for i = 2:numel(tree) p1 = tree(i).pos; p2 = tree(tree(i).parent).pos; plot3([p1(1) p2(1)], [p1(2) p2(2)], [p1(3) p2(3)], '-', ... 'Color', [0.8 0.8 0.8], 'LineWidth', 0.5); end % 画障碍物 for k = 1:size(obstacles, 1) [X, Y, Z] = sphere(20); X = X * obstacles(k, 4) + obstacles(k, 1); Y = Y * obstacles(k, 4) + obstacles(k, 2); Z = Z * obstacles(k, 4) + obstacles(k, 3); surf(X, Y, Z, 'FaceColor', [0.8 0.2 0.2], 'EdgeColor', 'none', 'FaceAlpha', 0.7); end % 画路径 if ~isempty(path) plot3(path(:,1), path(:,2), path(:,3), 'b-', 'LineWidth', 2.5); scatter3(path(1,1), path(1,2), path(1,3), 100, 'g', 'filled'); scatter3(path(end,1), path(end,2), path(end,3), 100, 'r', 'filled'); end scatter3(start(1), start(2), start(3), 80, 'k', 'filled'); scatter3(goal(1), goal(2), goal(3), 80, 'm', 'filled'); legend('tree', 'obstacle', 'path', 'start', 'goal'); end

可视化时有个小经验：如果你的树节点成千上万，循环里每帧都画所有线，Matlab会卡到怀疑人生。调试阶段可以只画最终路径，或者每隔20个节点画一条树枝。等到确认算法没问题了，再一次性画完整棵树用于展示。我自己通常先在命令行跑出path，再单独调用plotScene，这样能避免可视化拖慢主循环。

4. 调试实录：常见问题与优化建议

4.1 树就是不往目标方向长怎么办

最有可能是目标偏置概率太低，或者步长太小，导致树虽然在不断生长，但一直离目标很远。我遇到过迭代到4000次仍然一无所获的情况，把goalProb从0.05改成0.15后，几百次迭代就找到路径了。另外也要检查障碍物定义是否把目标点给包住了，如果目标本身落在障碍物内部，那无论怎么迭代都不可能在线段级别通过碰撞检测，最后只能报max iterations错误。

还有一种情况是地图范围设置得太随意，比如把bounds设得很大，而实际障碍物和目标都集中在一个很小的角落。这样随机采样点会大量落在空白区域，树扩展到目标附近的概率被稀释。解决办法是把地图范围收紧到实际任务可到达的区域，或者采用高斯采样，让采样点更集中在起点和目标周围。

4.2 碰撞检测明明没障碍却穿过障碍

这个问题的根子在于检测粒度。如果步长比障碍物的直径还大，线段可能从障碍物旁边擦过，但两端都在障碍物外面，视觉上却明显穿过了一截。解决办法有两个：第一，把stepSize调小到比最小障碍物半径的一半还小；第二，在碰撞检测里对长线段做细分，把线段等分成多个小段，逐段检测。第二种方案更稳健，代价是计算量增加。我的做法是优先调步长，步长实在不能小再引入细分。

细分实现起来不复杂，在collisionCheck里判断线段长度，如果长度超过细分阈值，就拆成若干段分别判断。不过在我自己的项目里，步长通常是地图尺寸的5%，而障碍物半径至少也有10个单位，很少出现漏检。如果你用自由地形或者更薄的障碍物，细分逻辑就必须补上。

4.3 性能太慢：R2021b下的加速技巧

代码写清楚后，接下来要面对的是性能。三维地图比二维大很多，最近邻搜索的vertcat每次迭代都要重新拼接整个树的pos，如果树有几千个节点，这个拼接本身就是不小的开销。R2021b里可以尝试两种优化：一是用cell数组按块缓存节点，定期合并；二是检查是否安装了Statistics and Machine Learning Toolbox，装了就直接用knnsearch，它会自动用KD树加速最近邻查找，在树节点超过5000时效果非常明显。

另一个容易忽略的加速点是目标判断里的norm调用。如果目标点固定，理论上可以把目标判断改成比较距离平方，省掉多次开方运算。虽然单次差距很小，但迭代几千次之后，累计收益还是可观的。我在最终代码里保留了norm，因为可读性更好，但如果你要跑大场景，建议把norm(newPos - goal) < threshold改成sum((newPos - goal).^2) < threshold^2，速度能快一截。

4.4 扩展思路：从RRT到RRT*和避障平滑

这套三维实现跑通之后，想进一步提升路径质量，下一步自然就是RRT*。RRT*与基础RRT的区别在于，找到新节点之后不只挂到最近节点上，还会在邻域内重新选择父节点，并对邻域内的节点做rewiring，让代价不断下降。代价函数在三维里直接取路径长度，所以公式和二维完全通用。R2021b的Robotics System Toolbox里其实内置了plannerRRT和plannerRRTStar，封装得很好，如果你懒得自己优化，直接调工具箱也能出效果。不过自己实现一遍之后再去看工具箱，才能理解参数背后的含义。

路径平滑也是常见需求。RRT给出的折线往往贴着障碍物边缘，直接给机械臂用会看到明显的棱角。我常用的做法是在得到路径点之后，用三次样条插值做均匀重采样，然后把插值后的点再做一次碰撞检测，剔除那些撞到障碍物的局部点。这样既保留RRT的探索能力，又能让最终轨迹更平滑。这部分不是本次重点，但如果你要做实际控制，建议补上。

最后说个题外话，我在调试这套代码时，最常用的工具反而不是调试器，而是把每次迭代后的树节点数量打印出来——如果树在连续200次迭代里都没有新增节点，那基本可以断定stepSize太大或者障碍物定义有误。这个习惯帮我省了很多排查时间。这套代码我已经在项目里跑过很多次，从二维移植到三维时最常见的坑就是碰撞检测和参数调节。如果你按上面的步骤写一遍，应该能比较顺畅地把路径跑出来。后面如果再写RRT*的三维版本，我会继续补上实际对比。

本文还有配套的精品资源，点击获取