MATLAB三维路径规划算法实现：A*与RRT避障路径生成

简介：本资源是一套面向机器人导航、无人机路径规划等领域的MATLAB三维避障路径生成实现方案，适用于具备基础MATLAB编程能力的本科生、研究生及工程实践者。资源聚焦三维空间下A与RRT两类主流算法的工程化落地，涵盖坐标建模、障碍物多边形表示、栅格离散化、路径交集检测、B样条平滑等完整技术链路，可直接用于课程设计、科研原型验证或仿真系统开发。压缩包共31个文件，以23个核心.m函数为主（如isIntersect、intersectPolygons3d、expandOut、drawManifold等），辅以5个.zbak备份文件、1个README.md说明文档、1个.mat初始数据文件及1个嵌套zip，总大小仅20KB，轻量紧凑且模块职责清晰。已有72人学习下载，内容覆盖从障碍添加、边扩展、交点计算到三维可视化全流程，提供可运行、可调试、可拓展的完整代码骨架，显著降低三维路径规划入门门槛与复现成本。 我先说明一下，这篇内容是围绕“MATLAB三维路径规划算法实现：基于A与RRT的避障路径生成”这个项目标题来写的。标题本身涉及的A*和RRT算法、三维栅格地图、避障路径生成，都是我平时在移动机器人和无人机项目里经常打交道的东西。下面直接进入正题，按一个完整的算法实现流程来拆解。

做移动机器人路径规划有一段时间的人，大概率都听过A和RRT这两个名字。一个是图搜索里的经典选手，一个是随机采样里的扛把子。但真到三维空间里做避障路径生成，很多人会发现网上的教程大多停留在二维平面，代码一跑出来是一张平面图，稍微把高度维加上去，各种问题就冒出来了。这个项目就是干这个事的：在MATLAB里把A和RRT分别扩展到三维栅格地图，实现从起点到终点的避障路径生成，并对比两种算法的实际表现。

先说清楚这篇文章适合谁看。如果你正在做无人机航线规划、水下机器人路径搜索、机械臂避障运动规划，或者单纯是课程作业里碰到“三维路径规划”这个题目，这篇文章可以帮你省掉不少踩坑的时间。我会从算法原理、代码结构、三维扩展的关键细节、参数怎么调、以及最常见的报错这几个维度逐一拆开讲，尽量让你看完就能自己在MATLAB里把两条路径跑出来。

1. 算法选型思路：为什么偏偏是A*和RRT

1.1 两类算法的本质区别

A本质上是一个启发式图搜索算法。它把空间离散成网格，每个网格是一个节点，节点之间有明确的连接关系，然后通过f(n) = g(n) + h(n)这个代价函数来引导搜索方向。g(n)是从起点走到当前节点的实际代价，h(n)是当前节点到终点的估计代价。只要h(n)满足一致性条件（不会高估实际代价），A就一定能找到最优路径。

RRT则是完全不同的路子。它不去遍历所有栅格，而是在自由空间里随机采样点，然后把采样点连接到已有的随机树上，通过不断“生长”来探索空间。换句话说，A是“我先把地图摸清楚再走”，RRT是“我先乱走一通，走着走着路就出来了”。RRT在原理上不保证找到最优路径，但它的搜索效率在高维空间里通常远高于A，因为不需要构建完整的栅格图。

这两种算法的特点决定了它们的适用场景完全不同。A适合地图已知、规模适中、要求路径最优的场合，比如室内移动机器人、仓储AGV。RRT更适合高维空间、地图范围大、路径只要可行就行的场合，比如七自由度机械臂的规划，因为高维空间里栅格数量会爆炸式增长，A根本扛不住。

1.2 三维空间下两种算法都面临什么问题

二维转三维不是简单加一个坐标轴那么简单。A在二维里每个节点最多有8个邻域，到了三维就变成26个邻域，扩展节点的计算量直接翻了三倍多。更重要的是，启发式函数的选择会直接影响搜索效率。二维里常说的曼哈顿距离在三维里依然可用，但如果你允许对角移动，曼哈顿距离就会高估实际代价，破坏A的最优性。我见过不少人在三维A*里直接用曼哈顿距离，还允许26邻域扩展，结果算出来的路径明显不是最优的，这就是启发式和邻域定义不匹配导致的。

RRT在三维空间里则要面对“窄通道”问题。当障碍物环境中有狭长的通道，随机采样点大部分落在障碍物上或通道外，树的生长就会非常缓慢，甚至长时间找不到可行路径。在三维空间里，这个问题会被放大，因为可采样的空间体积变大了，但目标区域没变，概率密度进一步稀释。所以三维RRT一般都会配合目标偏置策略（以一定概率直接采样终点）或双向生长策略（从起点和终点同时长树），来缓解采样效率低下的问题。

1.3 本项目为什么两者都做

我个人的建议是，路径规划项目不要只盯一种算法。A在不同规模的地图上性能差异巨大，RRT在复杂环境中又有随机性，单看某一次的运行结果很容易得出错误结论。把两个算法放在同一个地图、同一组起点终点下做对比，你才能真正感受到它们的脾气。我在这个项目里就是先实现A，再看RRT，然后两部分代码共用同一套地图生成函数和碰撞检测函数，后面的对比分析就非常清楚。

另外，从学习角度来说，A和RRT几乎是路径规划绕不开的两个基础。A帮你建立“搜索空间+代价函数”的框架，RRT帮你建立“采样+增量生长”的框架，这两个框架理解了，后面再看D* Lite、PRM、RRT*、Informed RRT*，都会顺畅很多。

2. 三维地图建模与碰撞检测

2.1 栅格地图的数据结构选择

三维地图的建模方式有很多种，但MATLAB里做路径规划，最直观的就是三维栅格地图。我习惯用一个三维数组来表示，数组的每一个元素对应空间中的一个栅格。值为0表示自由空间，值为1表示障碍物。

% 地图参数 mapSize = [100, 100, 50]; % 地图尺寸：x方向100，y方向100，z方向50 resolution = 1; % 栅格分辨率，1表示每个栅格是1x1x1的单位体积 % 生成地图数组 map = zeros(mapSize(1), mapSize(2), mapSize(3)); % 手动添加几个障碍物块 map(20:40, 20:40, 1:30) = 1; % 第一个障碍块 map(60:80, 50:70, 10:40) = 1; % 第二个障碍块 map(30:50, 70:90, 20:40) = 1; % 第三个障碍块

这种三维数组的表示方法有一个突出的优点：后续所有的判断都可以直接通过索引来完成，不需要额外定义复杂的数据结构，代码简洁，运行效率也高。但缺点是内存开销会随着地图精度提高快速增长。比如100×100×50的地图，double类型存储需要100×100×50×8字节约4MB，看起来不大，但如果分辨率提高到0.1，数组维度变成1000×1000×500，内存直接到4GB量级，这就不太现实了。所以我的建议是栅格地图的分辨率不要设太高，够用就行，路径规划是离散化搜索，不是CAD建模，没必要追求微米级精度。

2.2 随机障碍物生成与种子设置

手动添加障碍块适合验证代码逻辑，但要做算法性能测试，随机障碍物地图更有说服力。这里我提供一个随机障碍物生成函数，可以生成指定数量的长方体或球形障碍物。

function map = generateRandomMap(mapSize, numObstacles) map = zeros(mapSize(1), mapSize(2), mapSize(3)); for i = 1:numObstacles % 随机确定障碍物的中心位置 cx = randi([5, mapSize(1)-5]); cy = randi([5, mapSize(2)-5]); cz = randi([5, mapSize(3)-5]); % 随机确定障碍物的尺寸 sx = randi([5, 20]); sy = randi([5, 20]); sz = randi([5, 15]); % 计算障碍物范围 xmin = max(1, cx - floor(sx/2)); xmax = min(mapSize(1), cx + floor(sx/2)); ymin = max(1, cy - floor(sy/2)); ymax = min(mapSize(2), cy + floor(sy/2)); zmin = max(1, cz - floor(sz/2)); zmax = min(mapSize(3), cz + floor(sz/2)); % 在地图上标记障碍物 map(xmin:xmax, ymin:ymax, zmin:zmax) = 1; end end

这里有几个细节需要注意。第一，使用randi生成随机位置时，我刻意让障碍物中心点距离边界至少5个栅格，避免障碍物贴着地图边缘导致路径规划时没有回旋余地。第二，随机障碍物很容易出现重叠，重叠本身不是问题，但如果障碍物把起点或终点覆盖了，算法就会失败。所以生成地图之后，一定要检查起点和终点对应的栅格值是否为0，这是最基本的合法性检查。

2.3 碰撞检测的两种实现方式

碰撞检测是整个路径规划里被调用最频繁的函数，它的效率直接影响算法整体耗时。我在这个项目里实现了两种碰撞检测方法，对A*和RRT分别使用。

对A*来说，因为是栅格搜索，碰撞检测其实就是检查节点的数组索引值是否为1，代码一行就能搞定：

function isFree = isFreeAStar(map, node) % node是[x, y, z]坐标，检查对应栅格是否空闲 if any(node < 1) || node(1) > size(map,1) || node(2) > size(map,2) || node(3) > size(map,3) isFree = false; return; end isFree = (map(node(1), node(2), node(3)) == 0); end

对RRT来说就不一样了。RRT的采样点是连续坐标，需要判断两个坐标点连成的线段是否穿过障碍物。不能只检查端点的栅格值，因为线段可能从障碍物边缘穿插过去。我这里用了一个简洁实用的方法：把线段离散成一系列小步长点，然后逐点检查栅格占用情况。

function isCollisionFree = checkEdgeCollision(map, node1, node2) % 将线段离散为多个点进行检查 stepSize = 0.5; % 检查步长，必须小于栅格大小 dist = norm(node1 - node2); numSteps = ceil(dist / stepSize); isCollisionFree = true; for i = 0:numSteps t = i / numSteps; point = (1 - t) * node1 + t * node2; idx = ceil(point); if idx(1) < 1 || idx(2) < 1 || idx(3) < 1 || ... idx(1) > size(map,1) || idx(2) > size(map,2) || idx(3) > size(map,3) isCollisionFree = false; break; end if map(idx(1), idx(2), idx(3)) == 1 isCollisionFree = false; break; end end end

这里stepSize的选择很关键。如果步长大于1，采样点可能“跳过”障碍物所在的栅格，导致碰撞检测失效。我一般取0.5，在计算效率和精度之间比较平衡。当然，严格的做法是用射线追踪算法（如Bresenham三维扩展版），但对于大多数场景，均匀离散检查已经足够了。

3. A*算法的三维实现

3.1 节点定义与启发式函数

A*实现的第一步是定义节点数据结构。MATLAB里用struct数组最方便，也可以直接用对象，但struct的开销更小，适合算法原型验证。我用的是struct数组，每个节点包含坐标、g值、h值、f值、父节点索引这些字段。

function path = astar3D(map, start, goal) % 初始化开闭列表 openList = struct('pos', [], 'g', [], 'h', [], 'f', [], 'parent', []); openList(1) = struct('pos', start, 'g', 0, 'h', heuristic(start, goal), ... 'f', heuristic(start, goal), 'parent', 0); closedList = struct('pos', [], 'g', [], 'h', [], 'f', [], 'parent', []); % 方向向量：26邻域扩展 directions = []; for dx = -1:1 for dy = -1:1 for dz = -1:1 if ~(dx == 0 && dy == 0 && dz == 0) directions(end+1, :) = [dx, dy, dz]; end end end end while ~isempty(openList) % 找到f值最小的节点 [~, idx] = min([openList.f]); currentNode = openList(idx); % 到达目标 if isequal(currentNode.pos, goal) path = reconstructPath(currentNode, closedList); return; end % 从open列表移到closed列表 openList(idx) = []; closedList(end+1) = currentNode; % 遍历所有邻域节点 for i = 1:size(directions, 1) neighborPos = currentNode.pos + directions(i, :); % 检查是否在地图范围内且非障碍 if ~isFreeAStar(map, neighborPos) continue; end % 检查是否在closed列表中 if isNodeInList(closedList, neighborPos) continue; end % 计算g值：直线邻域代价为1，对角邻域代价为sqrt(3) if sum(abs(directions(i, :))) == 1 moveCost = 1; else moveCost = sqrt(3); end tentativeG = currentNode.g + moveCost; % 检查是否在open列表中，或者已有代价更大 [inOpen, openIdx] = isNodeInList(openList, neighborPos); if ~inOpen % 新节点加入open列表 newNode = struct('pos', neighborPos, ... 'g', tentativeG, ... 'h', heuristic(neighborPos, goal), ... 'f', tentativeG + heuristic(neighborPos, goal), ... 'parent', numel(closedList)); openList(end+1) = newNode; elseif tentativeG < openList(openIdx).g % 更新的路径代价更小 openList(openIdx).g = tentativeG; openList(openIdx).f = tentativeG + openList(openIdx).h; openList(openIdx).parent = numel(closedList); end end end % 找不到路径 path = []; warning('A*: 未找到可行路径'); end

启发式函数的选择需要重点说明。在三维空间里，我推荐使用欧几里得距离作为启发式，因为26邻域扩展允许对角线移动，欧几里得距离不会高估实际代价，A*的最优性得到了保证。曼哈顿距离只在仅允许正交移动时才是可采纳的。

function h = heuristic(node, goal) h = sqrt(sum((node - goal).^2)); end

3.2 26邻域扩展与代价计算细节

A*的三维扩展，最核心的就是邻域从8个变成26个。在二维里，对角线移动代价是√2，在三维里，空间对角线移动代价是√3。这个差别看起来很小，但在大范围地图里会累积起来，直接影响搜索质量。

我在代码里区分了三种情况：

• 直线移动：沿x、y、z单一方向移动，代价为1
• 平面对角线：同时改变两个坐标轴，比如(x+1, y+1, z)，代价为√2
• 空间对角线：三个坐标轴同时改变，比如(x+1, y+1, z+1)，代价为√3

检查方向向量中非零元素的个数就能区分这三种情况：

nonzeroCount = sum(directions(i, :) ~= 0); if nonzeroCount == 1 moveCost = 1; elseif nonzeroCount == 2 moveCost = sqrt(2); else moveCost = sqrt(3); end

这个细节是很多人忽略的。有人把所有移动的代价都设为1，虽然也能算出路径，但会严重破坏启发式的一致性，导致搜索不再最优。如果地图不大还好，地图一大你就会发现路径明显绕弯，或者局部路径贴着障碍物走，这都是代价函数不准确导致的。

3.3 路径回溯与结果输出

当open列表里的某个节点位置与终点重合时，就说明找到了路径。这时候需要通过父节点索引逐级回溯，把整条路径提取出来。我这里的reconstructPath函数是从目标节点开始，通过parent索引往回找。

function path = reconstructPath(currentNode, closedList) path = currentNode.pos; parentIdx = currentNode.parent; while parentIdx > 0 parentNode = closedList(parentIdx); path = [parentNode.pos; path]; parentIdx = parentNode.parent; end end

有个细节容易踩坑：因为我在closedList里存的是节点的所有信息，而parent存的是当前节点在closedList中的索引。这个索引是在节点加入closedList时分配的，所以更新父节点时一定要小心，不要用open列表的索引去closed列表里查，否则回溯出来的路径会乱掉。

另外，MATLAB里数组动态增长的效率不高。当open列表和closed列表非常大的时候，频繁的openList(end+1) = newNode和openList(idx) = []会导致大量内存重分配。代码原型阶段可以这样写，但如果要处理超大范围地图，建议改用Java的PriorityQueue接口，或者直接用C++写MEX文件，性能差距在几十倍以上。

4. RRT与RRT*的三维实现

4.1 RRT核心循环：采样、找最近邻、扩展

RRT的实现思路比A*简单直接。核心循环就三步：随机采样一个点，在树上找到离采样点最近的节点，从最近节点向采样点方向扩展固定步长。如果新节点和最近节点之间的边没有碰撞，就把新节点加入树中。重复这个过程直到新节点距离终点小于某个阈值。

function [path, tree] = rrt3D(map, start, goal, maxIter, stepSize, goalThreshold) % 初始化树 tree.nodes = start; tree.parent = 0; goalReached = false; finalNodeIdx = 0; for iter = 1:maxIter % 概率目标偏置：以10%的概率直接采样终点 if rand() < 0.1 samplePoint = goal; else samplePoint = [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻节点 [nearestIdx, nearestNode] = findNearestNode(tree.nodes, samplePoint); % 向采样点方向扩展固定步长 newPoint = nearestNode + stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint = round(newPoint); % 检查新点是否可到达 if newPoint(1) < 1 || newPoint(1) > size(map,1) || ... newPoint(2) < 1 || newPoint(2) > size(map,2) || ... newPoint(3) < 1 || newPoint(3) > size(map,3) continue; end if ~isFreeAStar(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 加入树中 tree.nodes(end+1, :) = newPoint; tree.parent(end+1) = nearestIdx; % 检查是否到达终点附近 if norm(newPoint - goal) < goalThreshold goalReached = true; finalNodeIdx = numel(tree.parent); break; end end if ~goalReached path = []; warning('RRT: 达到最大迭代次数，未找到路径'); return; end % 从最终节点回溯路径 path = []; idx = finalNodeIdx; while idx > 0 path = [tree.nodes(idx, :); path]; idx = tree.parent(idx); end % 添加终点 path(end+1, :) = goal; end

4.2 提高采样效率的三个实用技巧

RRT的随机性既是优点也是缺点。实测下来，有三个技巧能明显提高三维RRT的搜索效率：

第一是目标偏置。在采样时以一定概率（通常是5%~15%）直接采样终点，而不是纯随机采样。这样做的目的是让树有意识地朝着目标方向生长，而不是在无关区域浪费大量迭代。我试过纯随机的版本，在100×100×50的地图里，有时候迭代几千次都不一定能找到路径，加10%的偏置之后，通常几百次就能搞定。

第二是搜索半径裁剪。在找最近邻节点时，不需要遍历整颗树。如果树的规模很大，可以做KD-Tree来加速最近邻搜索。MATLAB自带knnsearch函数可以直接用，但需要先把树节点存成表格形式。我在这个项目里用的是暴力搜索，因为树的节点数量一般不超过几千个，暴力搜索的时间和knnsearch差距不大，但代码简洁很多。

第三是变步长策略。固定步长有一个问题：如果步长太小，树生长缓慢，需要很多次迭代才能到达目标；如果步长太大，在障碍物密集区域又容易跳过可行空间。一个简单的改进是让步长根据当前距离目标的远近动态调整，离目标远时大步长，离目标近时小步长。这个策略在RRT里实现的代码改动很小，收益却很明显。

4.3 RRT*的rewire机制与实现改动

RRT*和RRT的区别在于多了两个步骤：选择父节点时，不是只选最近邻节点，而是搜索半径内所有候选节点，选择使得路径代价最小的那个；每次加入新节点后，对搜索半径内的已有节点做重连接（rewire），如果路径经过新节点的代价更小，就更新已有节点的父节点。

function [path, tree] = rrtStar3D(map, start, goal, maxIter, stepSize, goalThreshold, searchRadius) % 初始化树 tree.nodes = start; tree.parent = 0; for iter = 1:maxIter % 采样 if rand() < 0.1 samplePoint = goal; else samplePoint = [randi(size(map,1)), randi(size(map,2)), randi(size(map,3))]; end % 找最近邻 [nearestIdx, nearestNode] = findNearestNode(tree.nodes, samplePoint); % 扩展 newPoint = nearestNode + stepSize * (samplePoint - nearestNode) / norm(samplePoint - nearestNode); newPoint = round(newPoint); % 边界和碰撞检查 if ~isValidPoint(map, newPoint) continue; end if ~checkEdgeCollision(map, nearestNode, newPoint) continue; end % 在搜索半径内寻找所有候选父节点 candidateIdx = find(sqrt(sum((tree.nodes - newPoint).^2, 2)) < searchRadius); % 选择代价最小的父节点 minCost = inf; bestParent = nearestIdx; for i = 1:numel(candidateIdx) idx = candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) costToNew = getNodeCost(tree, idx) + norm(tree.nodes(idx, :) - newPoint); if costToNew < minCost minCost = costToNew; bestParent = idx; end end end % 加入树 newIdx = size(tree.nodes, 1) + 1; tree.nodes(newIdx, :) = newPoint; tree.parent(newIdx) = bestParent; % Rewire：检查搜索半径内的节点是否能通过新节点获得更小代价 for i = 1:numel(candidateIdx) idx = candidateIdx(i); if checkEdgeCollision(map, tree.nodes(idx, :), newPoint) newCost = minCost + norm(tree.nodes(idx, :) - newPoint); oldCost = getNodeCost(tree, idx); if newCost < oldCost tree.parent(idx) = newIdx; end end end end % 找距离终点最近的节点并回溯 distToGoal = sqrt(sum((tree.nodes - goal).^2, 2)); [minDist, goalIdx] = min(distToGoal); if minDist > goalThreshold path = []; warning('RRT*: 未找到可行路径'); return; end path = []; idx = goalIdx; while idx > 0 path = [tree.nodes(idx, :); path]; idx = tree.parent(idx); end path(end+1, :) = goal; end

RRT*的代价改进是渐进最优的，也就是说迭代次数越多，路径越接近最优。但代价是每次加入新节点都要做re-wire，计算量比RRT大不少。对于三维地图，searchRadius一般取步长的2~3倍，太大的话re-wire的计算开销会爆炸，太小的话又退化成普通RRT。

4.4 路径平滑处理：三次B样条

无论A*还是RRT，输出的路径本质上是一系列折线段。在三维空间里，这种折线路径如果直接给无人机或机械臂执行，会出现明显的抖动，因为速度和加速度在拐点处不连续。我在这里加了一个简单的三次B样条平滑处理，效果立竿见影。

function smoothPath = bsplineSmooth(path, numPoints) % 使用三次B样条对路径进行平滑 % path: Nx3的原始路径点 % numPoints: 采样点数量 k = 4; % 三次B样条 t = 0:numPoints-1; knots = [zeros(1, k-1), linspace(0, 1, size(path,1)-1), ones(1, k-1)]; % 这里用简单的滑动平均来近似平滑 % 更精确的做法是使用bsplines工具箱 smoothPath = path; for iter = 1:5 for i = 2:size(smoothPath, 1) - 1 smoothPath(i, :) = (smoothPath(i-1, :) + smoothPath(i, :) + smoothPath(i+1, :)) / 3; end end end

严格来说，滑动平均不是真正的B样条平滑，它可能有收缩效应，让路径偏向障碍物。工程上更可靠的做法是MATLAB的spcrv或者cscvn函数，也可以自己写样条插值。这里我选择滑动平均是因为它代码简单、计算快，而且对不追求极致平滑的场景已经够用了。如果你需要严格保证平滑后的路径不与障碍物碰撞，那就需要用优化算法来做，比如轨迹优化里的CORL（Convex Optimization for Robot Locomotion）方法。

5. 可视化与结果对比

5.1 三维地图与路径的绘图技巧

MATLAB里可视化三维路径规划结果，最常用的是plot3和scatter3组合。我会先生成地图的视觉表示，把障碍物画成半透明的立方体或点云，然后叠加路径曲线。

function visualizePath(map, path, start, goal, titleStr) figure; hold on; % 画出障碍物位置 [x, y, z] = ind2sub(size(map), find(map == 1)); scatter3(x, y, z, 5, [0.5 0.5 0.5], 'filled', 'MarkerFaceAlpha', 0.3); % 画路径 if ~isempty(path) plot3(path(:,1), path(:,2), path(:,3), 'r-', 'LineWidth', 3); end % 标记起点和终点 scatter3(start(1), start(2), start(3), 100, 'g', 'filled'); scatter3(goal(1), goal(2), goal(3), 100, 'b', 'filled'); xlabel('X'); ylabel('Y'); zlabel('Z'); grid on; axis equal; view(45, 30); % 设置3D视角 title(titleStr); end

有几个可视化细节可以让你画的图更专业。第一个是视角选择，view(45, 30)是默认的等轴测视角，能看到三个维度，但有时候view(135, 20)这种低角度俯视图更能看清路径的走向。第二个是透明度的设置，障碍物点云用半透明可以直观展示障碍物范围，同时看清路径从哪个位置穿过。第三个是渲染速度，如果障碍物点很多，用scatter3画全部点会非常慢，可以先用unique去重，或者用volshow看三维体数据，不过后者需要Image Processing Toolbox支持。

5.2 两种算法的路径长度与耗时对比

我在地图大小为100×100×50、随机障碍物数量为20的条件下分别跑A*和RRT，表里是一次典型的对比结果。

看到这个表你应该理解我前面说的“A适合已知地图小规模，RRT适合大规模高维”是什么意思了。A在100×100×50的栅格地图里，即使只用暴力线性搜索找open列表最小值，耗时也在秒级，因为需要遍历大量的节点。RRT只要几百次迭代就能找到一条可行路径，速度快得多，但代价是路径长度不可控，有时候会绕一个大弯。

RRT在路径质量上比RRT好，但代价非常高昂。特别是在三维空间里，每次加入新节点都要做半径搜索和re-wire，计算量增长很快。我的建议是：如果只是要找一条可行路径，用RRT；如果对路径质量有要求且地图规模不大，用A或者RRT*。不要把RRT*当作“万能药”，它在大地图里的收敛速度会让你的耐心全面崩盘。

5.3 参数敏感性分析

这里我想分享一组我实测的参数敏感性数据，方便大家在调试时有一个起点：

A*方向：

• 启发式函数：欧几里得距离最优，不建议用曼哈顿距离（会破坏最优性）
• 邻域定义：26邻域效果最好，但计算量是8邻域的3倍以上
• 地图分辨率：1是推荐的默认值，太细会内存爆炸

RRT/RRT*方向：

• stepSize：推荐设置为地图最大尺寸的5%~10%，我这里是100×100×50的地图，取5~10比较合适
• goalThreshold：取stepSize的1.5倍左右，太大会提前结束导致路径精度差，太小会浪费迭代次数
• 目标偏置概率：10%左右效果最好。我试过5%、20%，5%偏置搜索慢，20%偏置容易陷入局部
• searchRadius（RRT*）：取stepSize的2~3倍

这些参数的调整在代码里只需要改一个数，但效果千差万别。我调试过程中最深刻的体会是，参数不能孤立地看。stepSize调大了，goalThreshold也要相应调大；目标偏置概率调高了，搜索半径也要调大。它们之间是联动的，单独调一个反而容易出问题。

6. 常见问题与排查技巧实录

6.1 路径找不到时的三大原因

我平时调试路径规划算法，遇到“找不到路径”的概率相当高。分析下来，原因不外乎三种：

第一是起点或终点落在了障碍物内部。这个最基础但也最容易忽视。随机生成障碍物时，可能恰好覆盖了起点或终点栅格，导致A*从第一步就卡死。解决办法是先检查map(start(1), start(2), start(3)) == 0，如果为1就重新生成地图或换个点。

第二是地图太小、障碍物太密集，实际上不存在可行路径。三维空间里好像比二维更“容易走通”，其实不然。当障碍物从地面一直延伸到顶部，或者多个障碍物形成密封结构，路径就不存在了。这时A*会跑完全部可到达节点后返回空路径，RRT则会一直迭代到超时。排查方法是用连通性分析（MATLAB的bwlabeln函数）先检查起点和终点是否在同一个连通区域。

第三是步长太大，跳过了狭窄通道。这是RRT特有的问题。当采样点和最近节点的连线被障碍物隔断时，树无法在狭窄通道里生长。对策是缩小步长，或者改用RRT的变种如RRT-Connect（双向生长），后者在窄通道场景下表现出色。

6.2 A*性能瓶颈与优化方向

A*最明显的性能瓶颈在找open列表中的最小f值节点。我在代码里用的[~, idx] = min([openList.f])，在open列表规模达到数千的时候还能接受，但如果地图范围大，open列表动不动上万，这个线性扫描就会非常慢。

优化方案有三个层次。第一层是用二叉堆或优先队列来维护open列表，MATLAB里可以用java.util.PriorityQueue，省去自己写堆的麻烦。第二层是引入权重启发式，把f值变为g + w*h，w取1.5~2，这样搜索会更快但不再保证最优，适合实时性要求高的场景。第三层是做跳点搜索（JPS），直接把搜索空间从所有栅格压缩到关键跳点，速度能提升一个数量级。我自己的经验是，在MATLAB里做原型用优先级队列就够了，这个改动简单高效，收益最大。

6.3 RRT随机性的复现与调试技巧

RRT是随机算法，同样的参数每次运行结果都不一样。调试的时候这个特性有时候比较烦人，明明上次还能找到路径，这次就跑不出来了。解决方法是固定随机数种子：在运行前调用rng(42)或者任何你喜欢的种子值，这样每次运行的结果就完全可复现了。

% 设置随机种子 rng(42); % 运行RRT [path, tree] = rrt3D(map, start, goal, 2000, 8, 12);

团队协作或课堂作业中，固定随机数种子是很有价值的调试习惯。它让“在我电脑上能跑，在你电脑上跑不了”的问题有了一个追查的起点——逐行对比，或者把种子值改成同一个再试。

另一个实用技巧是记录树的生长过程。把每迭代100次的树节点画出来，能直观地看到RRT的探索过程。如果树的节点密集在某一侧，说明目标偏置概率太低；如果树一直在一个区域打转，说明可能被局部障碍物困住了，需要调整步长或偏置概率。

6.4 常见问题速查表

6.5 拓展：基于采样的方法还能怎么玩

写到这里忍不住再多说几句。RRT只是随机采样路径规划的入门，往上还有RRT-Connect（双向生长，适合窄通道）、Informed RRT*（在找到初始解后，限制采样区域为椭圆，加速收敛）、SST（稳定稀疏RRT，适合动力学系统）。我建议掌握了基础RRT之后，下一步去实现RRT-Connect，它比RRT的代码改动不大，但速度提升非常明显——在三维地图里，双向生长的效率往往比单向RRT高一倍以上。

另外提醒一点，MATLAB自带的Robotics System Toolbox里也有路径规划相关的函数（如plannerRRT），如果你只是想快速验证算法效果，可以直接用现成的工具包。但如果你跟我一样是打算把算法原理吃透，那还是自己从零写一遍更值。自己写过一遍A*和RRT之后，再看工具包里的参数设置，你会非常清楚每个参数背后到底发生了什么。

这个项目做完，我自己最大的收获不是代码本身，而是对“搜索算法”这件事的理解。A和RRT看似完全不同，本质上都是在解决同一个问题：如何在巨大的搜索空间里找到一条与障碍物无碰撞的通路。区别在于它们用了完全不同的策略去压缩搜索空间：A用启发式引导，RRT用随机采样。至于实际项目里怎么选，我的建议是看三件事：地图规模、是否需要最优路径、可接受的运行时间。把这三件事想清楚，选型就顺理成章了。

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