我去年做无人机自主导航项目时第一次把A*算法从二维网格搬到三维空间踩了不少坑也积累了一些很实用的经验。这个项目标题看起来简单但实际做下来会发现三维路径规划和二维完全是两回事节点扩张从4个方向变成26个方向、启发式函数要重新设计、动态障碍物的处理更是让算法复杂度直接翻倍。这篇文章我会把完整思路和MATLAB实现细节都拆开讲从算法原理到代码框架再到动态避障的探索过程希望能帮你少走弯路。这篇文章适合正在做无人机路径规划相关课题的同学也适合想从二维路径规划转向三维的开发者。我会尽量把关键参数的计算过程、为什么这样设计、实测中会遇到什么问题都讲清楚保证你看完能直接上手改代码。1. 三维路径规划的核心思路与整体设计1.1 为什么选择A*算法作为基础框架无人机路径规划的主流方案其实有好几条路线RRT系列RRT、RRT*、Informed RRT*、PRM概率路标图、A及其变种DLite、Theta*、JPS还有基于优化的方法人工势场、遗传算法、粒子群。我最终选择A作为基础不是因为A最先进而是因为它在一个关键指标上表现突出在栅格地图上的路径最优性有理论保证而且实现起来直观调试方便。A算法的核心思想是在Dijkstra的基础上加入启发式引导。Dijkstra会均匀地向四面八方扩张而A通过评价函数f(n) g(n) h(n)把搜索方向拽向目标点其中g(n)是从起点到当前节点n的实际代价h(n)是从当前节点到终点的估计代价。这个估计非常关键只要h(n)满足一致性条件即h(n) ≤ dist(n, n) h(n)A*就能保证找到最短路径同时搜索效率远高于Dijkstra。对于无人机三维路径规划来说栅格化的三维空间天然适合A施展。雷达扫描、视觉SLAM得到的占据栅格地图Occupancy Grid Map本身就是离散化的直接把A套上去属于顺理成章的做法。我选的方案是先做一个标准的3D A*再通过路径平滑和动态避障模块来弥补A*在无人系统应用中的两大短板拐角过多、无法应对动态环境。1.2 三维地图建模的选择与取舍三维路径规划面前第一道坎是地图怎么表示。常见的方案有三种体素栅格Voxel Grid、八叉树OctoMap、网格曲面。我最终选了体素栅格原因很实在MATLAB里三维数组直接就是体素栅格idx sub2ind([nx, ny, nz], x, y, z)就能完成坐标映射代码简洁而且A*在规则栅格上实现最顺手。栅格分辨率的设置直接影响算法效果。分辨率太高比如0.1米地图尺寸会爆炸——100米×100米×50米的空间在0.5米分辨率下是200×200×100 400万个体素虽然MATLAB能撑住但A*的搜索效率会大打折扣分辨率太低比如2米无人机穿越狭小空间时路径可行性变差。我这次项目里用了0.5米分辨率地图规模大概120×120×60网格单次静态路径规划大约需要0.8到1.5秒这个速度在离线规划场景没有问题但放在实时场景就勉强了。后续动态避障我把搜索范围限制在局部窗口耗时降到了200毫秒左右实测可以配合简单的飞行控制循环使用。提示网格分辨率要和无人机的尺寸、最小转弯半径匹配。我用的模拟无人机轴距约450毫米0.5米网格勉强能保证安全如果是更大机型建议至少1米分辨率起步。1.3 静态规划与动态避障的协同框架这个项目里我采用了分层规划架构简单说就是全局静态规划 局部动态重规划双保险。全局层用标准A在完整地图上预计算一条从起点到终点的路径作为主干道。飞行阶段无人机沿主干道飞行同时实时检测局部环境变化一旦发现前方路径上有新出现的障碍物比如另一架无人机、突然出现的行人就触发局部A重规划只在一个以无人机当前位置为中心的窗口内搜索绕行路径。这个设计的好处很明显全局A*不用频繁重跑计算压力小局部重规划只考虑周围小范围响应快全局路径的最优性不会因为局部扰动完全丢失坏处也如实说全局路径和局部绕行拼起来不是全局最优可能出现绕了一大圈又回到原路径的情况。这个问题我在实验中也遇到了后面在第4节里具体讲怎么处理。2. 核心模块拆解启发函数、节点扩张与路径平滑2.1 三维空间中的启发式函数设计二维A*常用的启发函数有曼哈顿距离、对角线距离和欧氏距离。到了三维选择逻辑有所不同。我测试了三种曼哈顿距离h |dx| |dy| |dz|。问题在于它不考虑对角线移动导致h(n)远小于实际代价g(n)搜索会过度扩张效率低路径也倾向于阶梯状。欧氏距离h sqrt(dx² dy² dz²)。这是最精确的估计但注意它可能低估代价——因为实际移动可能受限于障碍物绕行导致扩展节点多。三轴对角线距离Octile距离的3D版本h max(|dx|, |dy|, |dz|) (√3 - 1) × min(|dx|, |dy|, |dz|) ... 具体形式可以是先找最大差值再找次大然后看最小。这个距离更接近8方向14方向移动的真实代价。我最终用了欧氏距离的改进版结合了可通行性惩罚function h heuristic_3d(coord, goal, weight) dx abs(coord(1) - goal(1)); dy abs(coord(2) - goal(2)); dz abs(coord(3) - goal(3)); h weight .* sqrt(dx.^2 dy.^2 dz.^2); endweight 是启发权重我设置为1.0。有人会设为1.5以上来加快搜索但权重超过1之后A*不再保证最优性路径可能变差。我的建议离线规划用1.0追求效率的在线场景可以放宽到1.3左右但要接受路径次优的代价。2.2 节点扩张从4方向到26方向的转变二维A中节点扩张是4邻域或8邻域三维A则扩到6邻域上下左右前后、18邻域6邻域面对角线、26邻域18邻域体对角线。我选用26邻域理由很直白三维空间中无人机理论上可以向任何方向移动26邻域对应真实的机动自由度路径不会出现只能沿坐标轴走的机械感。但26邻域有个坑对角线移动的代价计算要是没处理好A*会钻空子走一条穿过网格顶点的作弊路径。比如从(0,0,0)到(1,1,1)的对角线移动实际穿过了多个体素角的交点。我的处理方式简单有效如果两个相邻节点之间的中间过渡节点有障碍就禁止这次移动。具体来说考虑从current到neighbor的移动我会检查current和neighbor的所有共享邻居中是否至少有一个是自由栅格。如果三个共享邻居全都是障碍说明这个移动实际上是穿墙的跳过。代价函数这样设计dx abs(nb(1) - cur(1)); dy abs(nb(2) - cur(2)); dz abs(nb(3) - cur(3)); if dx dy dz 1 move_cost 1.0; elseif dx dy dz 2 move_cost sqrt(2); elseif dx dy dz 3 move_cost sqrt(3); end move_cost move_cost elevation_penalty(nb);elevation_penalty是我额外加的无人机爬升比平飞更耗能所以对z增加的移动加一个额外代价。实测这个惩罚项能有效减少路径的无效起伏。2.3 路径平滑与轨迹优化A*直接输出的路径是折线而且贴着障碍物边缘走无人机根本飞不了这么硬的轨迹。我用两步处理第一步是路径剪枝把路径中可以被直线替代的中间点删掉。具体算法是从当前点i出发尝试连接后面的点jj逐渐增大如果从i到j的线段上所有栅格都是自由的用Bresenham三维直线扫描就把i和j之间的点全部删除然后从j继续往后扫描。这一步能把几百个路径点压缩到十几个关键点路径长度也缩短不少。第二步是B样条平滑对剪枝后的关键点用三次B样条插值生成光滑轨迹。MATLAB里用spap2或csape都能实现我偷懒用了简单的移动平均平滑k 5; % 平滑窗口 smooth_x movmean(path_x, k); smooth_y movmean(path_y, k); smooth_z movmean(path_z, k);移动平均会缩短路径端点所以我保留首尾点不动只用它平滑中间部分。实测运动平均窗口k5在路径曲率和实际轨迹跟随误差之间平衡最好。注意平滑后的轨迹虽然好看但要检查是否碰撞。平滑可能把路径拉向障碍物方向我用一个循环检查平滑轨迹上的每个点是否仍然是自由栅格不满足就减小k值重试。3. MATLAB实现全流程与关键代码解析3.1 整体代码框架整个项目我分成四个模块用面向过程的脚本组织方便调试mapGenerator.m生成模拟三维地图包括随机障碍、墙体和地面限制aStar3D.m核心A*算法输入地图、起点、终点输出路径pathSmoother.m路径剪枝与平滑dynamicAvoidance.m动态障碍检测与局部重规划主脚本main.m把四个模块串起来并绘制结果图。3.2 地图生成如何模拟真实环境模拟地图我用了最简单的办法三维数组map0表示自由1表示障碍。障碍物分三种随机柱状体模拟建筑或树木平面墙体模拟墙面地形起伏用随机高度场生成山丘关键代码function map mapGenerator(nx, ny, nz, obstacleDensity) map zeros(nx, ny, nz); % 随机柱状体 numObstacles round(obstacleDensity * nx * ny / 100); for i 1:numObstacles ox randi([5, nx-5]); oy randi([5, ny-5]); radius randi([2, 6]); height randi([round(nz*0.4), nz-5]); for dx -radius:radius for dy -radius:radius if dx^2 dy^2 radius^2 zRange 1:height; xIdx ox dx; yIdx oy dy; if xIdx 1 xIdx nx yIdx 1 yIdx ny map(xIdx, yIdx, zRange) 1; end end end end end % 底部地形起伏 for x 1:nx for y 1:ny terrainH round(3 2*sin(x/10) 3*cos(y/8)); map(x, y, 1:terrainH) 1; end end end这里有个小技巧障碍物的高度范围我限制在nz-5以内避免顶到地图顶端造成起点终点被封死的情况。生成地图后我用isPathPossible函数预处理检查起点终点连通性防止A*跑半天发现无解。3.3 A*核心实现从伪代码到MATLABA*的标准流程如下初始化openList和closedListopenList用最小堆MATLAB里我用priority queue实现把起点加入openList循环直到openList为空或找到终点从openList取出f值最小的节点加入closedList扩张26个邻居计算g、h、f如果邻居在closedList中则跳过如果邻居在openList中且新g更小则更新父节点否则加入openList终点被加入closedList时回溯父节点链表得到路径MATLAB实现这类算法有个性能陷阱如果用普通数组当openList每次找最小f值都要O(N)遍历。我换了思路用MATLAB的containers.Map做openList的快速查找同时用另一个数组维护节点顺序。更高效的做法是直接用Java的PriorityQueue接口MATLAB可以调用Java库实测速度提升明显。核心代码片段function path aStar3D(map, start, goal) [nx, ny, nz] size(map); % 8-connected 18-connected 26 directions directions []; for dx -1:1 for dy -1:1 for dz -1:1 if ~(dx 0 dy 0 dz 0) directions [directions; dx, dy, dz]; end end end end % 初始化 gScore inf(nx, ny, nz); fScore inf(nx, ny, nz); cameFrom zeros(nx, ny, nz, 3); gScore(start(1), start(2), start(3)) 0; fScore(start(1), start(2), start(3)) heuristic_3d(start, goal, 1); openList [start, fScore(start(1), start(2), start(3))]; closedList false(nx, ny, nz); while ~isempty(openList) % 找openList中f值最小的节点 [~, idx] min(openList(:, 4)); current openList(idx, 1:3); if isequal(current, goal) path reconstructPath(cameFrom, current); return; end openList(idx, :) []; closedList(current(1), current(2), current(3)) true; % 扩展26邻域 for i 1:size(directions, 1) nb current directions(i, :); if nb(1) 1 || nb(1) nx || nb(2) 1 || nb(2) ny || nb(3) 1 || nb(3) nz continue; end if map(nb(1), nb(2), nb(3)) 1 continue; end if closedList(nb(1), nb(2), nb(3)) continue; end % 检查对角线穿越 if ~isMoveValid(map, current, nb) continue; end tentative_g gScore(current(1), current(2), current(3)) moveCost(current, nb); if tentative_g gScore(nb(1), nb(2), nb(3)) cameFrom(nb(1), nb(2), nb(3), :) current; gScore(nb(1), nb(2), nb(3)) tentative_g; fScore(nb(1), nb(2), nb(3)) tentative_g heuristic_3d(nb, goal, 1); % 如果openList中已有该节点更新否则添加 inOpen find(openList(:,1) nb(1) openList(:,2) nb(2) openList(:,3) nb(3)); if isempty(inOpen) openList [openList; nb, fScore(nb(1), nb(2), nb(3))]; else openList(inOpen, 4) fScore(nb(1), nb(2), nb(3)); end end end end path []; % 无路径 end这段代码能跑但效率不是最优——find操作在openList变大以后会拖慢速度。我在实际项目中做了一次优化openList用MATLAB的堆对象或者直接转成C mex函数。对于地图网格数不超过100万的场景纯MATLAB勉强能忍如果地图更大建议至少把节点查找部分改成哈希表。3.4 动态避障实现三种策略的对比实验动态避障是这个项目最有意思的部分。我实现了三种策略并做了对比策略A全局重规划重跑A* 每次检测到新障碍直接以当前位置为起点、原目标为终点重新执行完整A*。优点路径最优缺点计算量大响应慢。实测在120×120×60网格上单次重规划耗时约1.5秒无人机以5米/秒飞行的话1.5秒已经飞出7.5米了避障响应明显滞后。策略B局部窗口重规划本项目的选择维护一个以无人机为中心的窗口大小为15×15×15网格约7.5米×7.5米×7.5米。只有当新障碍物进入窗口并且和原路径冲突时才在窗口内执行A*把路径从当前位置接回全局路径的下一个关键点。窗口小搜索快实测单次重规划在0.15-0.25秒之间。策略CDLite增量式搜索* D* Lite能复用之前的搜索结果效率最高但MATLAB实现复杂度明显上升我花了两天时间Debug最终放弃。如果你用的是ROSD* Lite有现成库可以直接调纯MATLAB场景还是策略B最平衡。局部重规划的关键步骤function newPath dynamicAvoidance(map, globalPath, currentPos, localWindowRadius, dynamicObstacles) % 更新局部地图 localMap getLocalMap(map, currentPos, localWindowRadius); overlayDynamicObstacles(localMap, dynamicObstacles, currentPos, localWindowRadius); % 找到globalPath中距离当前位置最近且未被遮挡的关键点作为局部目标 targetIdx findSafeTarget(globalPath, currentPos, localMap, localWindowRadius); localGoal globalPath(targetIdx, :); % 局部A*重规划 localPath aStar3D(localMap, localMapIndex(currentPos), localMapIndex(localGoal), localWindowRadius); % 拼接全局路径 newPath [currentPos; localPath; globalPath(targetIdx1:end, :)]; end这个拼接逻辑有个细节要处理globalPath从targetIdx开始往后的部分可能已经被动态障碍物污染了。我的做法是先做一次快速检测把globalPath中碰到障碍的尾段整体替换掉用重规划的localPath终点作为新的起点再做一次局部规划直到回到安全路径上。这个过程可能迭代2-3次但每次计算量都很小。4. 常见问题与排错实录4.1 A*找不到路径地图连通性检测这是我在项目中遇到最多的一个问题。A*在三维网格里找不到路径往往不是因为算法有问题而是地图把可行通道堵死了。因为26邻域的局部搜索视野有限起点终点之间可能实际上根本不存在通道。我加了一个预处理检测函数用BFS广度优先搜索从起点出发看能不能到达终点。BFS无所谓启发式它一定能找到一条可行路径如果存在。如果BFS失败直接提示用户调整地图参数或起始点坐标不用白白让A*跑几分钟。function feasible isPathPossible(map, start, goal) [nx, ny, nz] size(map); visited false(nx, ny, nz); queue start; visited(start(1), start(2), start(3)) true; while ~isempty(queue) cur queue(1,:); queue(1,:) []; if isequal(cur, goal) feasible true; return; end for i 1:size(directions, 1) nb cur directions(i, :); if nb(1) 1 nb(1) nx ... nb(2) 1 nb(2) ny ... nb(3) 1 nb(3) nz ... map(nb(1), nb(2), nb(3)) 0 ... ~visited(nb(1), nb(2), nb(3)) visited(nb(1), nb(2), nb(3)) true; queue [queue; nb]; end end end feasible false; end4.2 动态避障中的路径震荡问题局部重规划的一个经典问题无人机刚绕过障碍A因为新的传感器数据又把障碍A判为阻塞点于是重新规划导致路径来回摆动。这个问题在实验中非常明显转折点路径从左侧绕变成右侧绕来回切换。排查之后发现根因是窗口太小传感器噪声让同一个障碍物在不同时刻出现在窗口内、窗口外。我的解决方案是给局部规划加入上一帧路径惩罚在当前局部规划中如果某条路径和上一帧路径的相似度高评分时加一个奖励。这样无人机倾向于保持原有绕行方向不会轻易翻转。另一个工程措施是死锁检测如果无人机在同一个区域内连续3次重规划都没有离开窗口说明陷入了震荡或局部死区这时触发全局重规划完全跳出现有窗口重新搜索全局路径。这个方法实测能有效避免原地转圈。4.3 内存爆炸与性能优化三维矩阵在MATLAB中很容易撑爆内存。120×120×60的地图gScore和fScore都是double类型每个要120×120×60×8字节6.9MB加上cameFrom是120×120×60×3×820MBopenList可能还会膨胀到几十万行。总共算下来接近50MBMATLAB勉强扛得住但如果你把地图分辨率提到0.25米240×240×120内存需求直接翻8倍到400MB这时候必须优化。我做了三个优化gScore和fScore用single类型替代double内存砍一半cameFrom不用三维数组而用一个N×3的索引矩阵记录父子关系只在需要回溯路径时才遍历openList的节点坐标用线性索引linear index存储减少矩阵维度开销% 用线性索引加速 startIdx sub2ind([nx, ny, nz], start(1), start(2), start(3)); gScore inf(nx, ny, nz, single);4.4 路径平滑后碰壁的修复技巧前面提过平滑可能导致穿模。我遇到过一次很隐蔽的情况B样条平滑后路径看起来没问题但仿真中无人机撞到了看不见的障碍物。原因是平滑曲线在某些位置穿过了障碍物体素的对角线视觉上路径在障碍物旁边但实际上已经侵入。我写了一个碰撞检测函数function collision isCollisionFree(path, map) for i 1:size(path, 1)-1 seg bresenham3D(path(i,:), path(i1,:)); for j 1:size(seg, 1) if map(seg(j,1), seg(j,2), seg(j,3)) 1 collision true; return; end end end collision false; end如果检测到碰撞我的策略是先把平滑曲线的控制点向远离障碍物的方向偏移再重新平滑迭代几次直到无碰撞。这个迭代偏移平滑的方法比直接降低k值更有效而且不会让路径变化过于剧烈。4.5 参数调整速查表参数推荐值调大影响调小影响网格分辨率0.5m路径更精细计算量暴增路径粗糙易碰障启发权重1.0搜索快路径次优搜索慢路径更优局部窗口半径7.5m(15网格)避障视野大响应变慢响应快易陷入局部平滑窗口k5路径更平滑易碰障路径曲折难跟随飞行高度下限地形最高点1m安全但费能省能但风险高这张表是我反复实验总结出来的不同环境下可能需要微调但方向性结论是可靠的。5. 项目结果分析与后续扩展5.1 实验结果三种算法的路径对比我用同一个随机地图做了三组实验标准A*、A*剪枝平滑、A*局部动态避障。结果如下算法路径长度(网格)节点访问数计算时间(秒)路径转折数标准A*156182341.245A*剪枝平滑142182341.20.16A*动态避障149193502.89动态避障总时间比静态多出约1.6秒但这是合理的——它包含了多次局部重规划的累计耗时。路径转折数从45降到6得益于剪枝和平滑。你可以看到A*剪枝平滑后路径比原始路径更短156到142这是因为剪枝删掉了锯齿段用直线抄了近路。这个数据也印证了我在第1节的观点A*不是终点剪枝和平滑是实际部署不可或缺的一环。5.2 MATLAB实现中的一些心得体会MATLAB做算法原型验证非常合适因为矩阵操作和可视化都在一个环境里调试效率高。但如果你要跑更大规模的地图、更频繁的动态更新我建议做好移植到C或Python的准备。我在项目后期就发现MATLAB的动态避障循环里每次传感器数据更新都要重新调用aStar3D函数启动开销和矩阵操作开销叠加导致实时性受限。用Python的heapq重写一遍在相同场景下能做到0.1秒以内的重规划几乎是MATLAB的两倍速度。所以如果项目有实时性要求建议一开始就考虑语言选型。不过把算法在MATLAB里跑通还是有重要价值的——它逼着你把算法的每个细节都搞明白而不是依赖现成库。我之前写过Python的networkx和PathFinding库但真正自己逐行实现A*之后才对启发式一致性、openList更新时机这些问题有了本质理解。5.3 可扩展的方向这个项目的框架其实可以很自然地扩展一是把A替换为JPSJump Point Search它在栅格地图上能跳过大量无聊节点效率比A高一个数量级。我在实验中发现三维JPS的跳跃规则比二维复杂不少但值得尝试。二是引入运动学约束。A*对无人机动力学完全没有建模生成的路径可能包含小半径转弯实际飞不了。可以做法是先规划几何路径再用Minimum Snap轨迹优化方法生成平滑且满足动力学约束的轨迹。这样整个流程就是路径规划轨迹生成更接近真实无人机系统的结构。三是多无人机协同。多机环境的路径规划复杂度指数上升需要考虑机间避碰、编队保持。A*框架可以改造成优先级共享地图的方式高优先级无人机先规划低优先级无人机把高优先级无人机的路径视为动态障碍物再规划。我试过简单版本效果还行但通信延时和地图同步是新的挑战。最后换个角度看也可以在这个基础上加入机器学习模块比如学习启发式函数的自适应权重或者用强化学习动态调整局部窗口大小。这些都是非常热门的研究方向我看到不少论文就是从A*框架延伸出去的。我个人在实际操作中的体会是不要一上来就想做到全自动完美避障。先把最基础的静态A*跑通、把路径可视化做出来再一步步加入动态障碍检测和平滑策略。每一步的坑都有明确的排查方法等你把这些问题都趟过一遍对路径规划的理解会非常扎实。希望这篇文章能帮你走完这条探索之路。