企业官网建设流程全解析

1. 三维路径规划到底难在哪：为什么不能直接沿用二维方案

很多朋友拿到"无人机三维路径规划"这个题目，第一反应是把常见的二维A*算法代码拿过来，把(x, y)改成(x, y, z)，再加一层循环。这个思路大方向没错，但实际跑起来就会发现事情没那么简单——三维空间的搜索规模、障碍物建模方式、路径评价标准，和二维完全不是一个量级。

先说搜索规模。二维栅格地图里，如果每个节点有8个邻域方向，一张100×100的地图最多也就1万个节点；到了三维，100×100×100就是100万个节点，每个节点如果有26个邻域方向（3×3×3减去中心），光邻居关系就有2600万条。A虽然比Dijkstra聪明得多，但open list和close list的维护开销同样会指数级上涨。我见过不少人在二维地图上跑A只要几百毫秒，扩展到三维之后直接内存溢出或者跑了几分钟没结果，然后就开始怀疑算法写错了——其实算法没错，问题是搜索空间的设计不合理。

再说环境建模。二维路径规划里，障碍物是个圆或者矩形，判断碰撞就是算距离或者判断点是否在多边形内；到了三维，障碍物变成了圆柱体、球体、山体，无人机本身也有飞行高度限制，你不能贴着地面飞，也不能飞到云层之上（在仿真里不存在的物理约束可以不管，但地图边界和威胁区域必须定义清楚）。更关键的是，二维规划只关心"路径不穿过障碍物"，三维规划还要考虑坡度限制、转弯半径、安全高度等一系列运动学约束。A*本身是个几何搜索算法，它不天然懂这些约束，需要你在代价函数和邻居生成规则里手动把它们加进去。

最后是路径质量的评价。二维路径通常用路径长度作为唯一优化目标，但无人机三维路径规划里，路径长度只是一部分。飞行高度变化太剧烈，意味着能耗增加、姿态调整频繁；路径贴近威胁源，意味着被探测和击落的概率上升。所以实际做这个题目时，代价函数里往往要同时包含长度代价、高度代价、威胁代价三个分量，用加权系数调节。

这篇文章要做的，就是基于Matlab实现一套完整的三维A*路径规划代码，从环境建模、搜索算法到路径平滑，每一步都给出可运行的代码和背后的设计理由。代码会覆盖以下几个核心能力：

• 三维栅格地图的构建与可视化；
• 包含长度、高度、威胁因素的代价函数设计；
• 26邻域搜索与安全碰撞检测；
• 基于A*的最优路径搜索；
• 基于插值和样条的路径平滑处理。

代码用Matlab写，因为Matlab在矩阵运算和可视化上有天然优势，调试路径规划算法非常方便，而且做课程设计、毕业设计、论文仿真都够用。

2. 三维栅格地图建模：把抽象空间变成算法能算的数据结构

2.1 栅格尺寸与地图尺寸怎么定

A*算法运行的基础是栅格地图，所以第一步是把规划空间离散化。假设规划空间是一个长方体，长宽高分别是Map_X、Map_Y、Map_Z，单位是米。栅格尺寸太大，路径精度差，可能穿过狭窄通道；栅格尺寸太小，节点数量爆炸，搜索速度难以接受。这个度怎么把握，取决于无人机本身的尺寸和任务需求。

以常见的四旋翼为例，轴距大约0.5米左右，那栅格尺寸取1米就比较合理——既不会把无人机当成一个纯质点导致路径贴着障碍物边缘走，也不会因为栅格太大让路径绕远路。在栅格尺寸确定后，三个维度上的栅格数量分别是：

numX = ceil(Map_X / grid_size) + 1; numY = ceil(Map_Y / grid_size) + 1; numZ = ceil(Map_Z / grid_size) + 1;

注意要加1，否则边界节点会缺失，路径无法到达地图边缘。这一步很多人会漏，导致A*搜索出来的路径永远到不了设在边界上的目标点。

2.2 障碍物建模的三种思路

三维环境里的障碍物，在Matlab里常见的建模方式有三种：

• 球体障碍物：定义球心坐标和半径，判断节点和球心的距离是否小于等于半径。
• 圆柱体障碍物：定义圆心坐标、半径和高度范围，判断节点在XY平面的投影是否落入圆内，同时Z坐标是否落在高度范围内。
• 山体/地形障碍：用高度函数Z = f(X, Y)描述地形起伏，低于地形高度的栅格视为不可通行。

实际项目中，球体和圆柱体用得最多，因为参数简单，碰撞检测计算量小。球体适合模拟爆炸物、高压线塔基座等点状威胁；圆柱体适合模拟建筑物、信号塔、雷达站等块状威胁。我在代码里默认支持前两种方式，同时预留了地形函数的接口。

% 障碍物定义示例 obstacles = [ 30, 35, 10, 8; % 球形障碍: x, y, z, r 60, 70, 0, 15, 30; % 圆柱障碍: x, y, z_start, z_end, r ];

这里每一行的列数不同，处理时用cell数组或结构体数组更灵活。我习惯用结构体数组：

obs(1).type = 'sphere'; obs(1).center = [30, 35, 10]; obs(1).radius = 8; obs(2).type = 'cylinder'; obs(2).center = [60, 70]; obs(2).z_range = [0, 30]; obs(2).radius = 15;

2.3 地图数据的组织方式

三维栅格地图在Matlab里最自然的存储方式是三维逻辑数组：

map3D = zeros(numX, numY, numZ); % 0表示自由空间，1表示障碍物

然后遍历所有栅格，判断每个栅格是否位于障碍物范围内：

for i = 1:numX for j = 1:numY for k = 1:numZ pt = [i-1, j-1, k-1] * grid_size; % 实际坐标 for idx = 1:length(obs) if isCollision(obs(idx), pt) map3D(i, j, k) = 1; break; end end end end end

这个三重循环看起来笨重，但胜在直观易理解。实际跑的时候，如果地图尺寸特别大（比如300×300×50），建议用矩阵化运算把障碍物判断向量化，速度能快几个数量级。核心思路是先生成网格坐标矩阵，再对每个障碍物计算布尔掩膜，最后取并集。

可视化这一步很重要，建议用scatter3画障碍物点云，或者用isosurface画等值面，这样后续路径展示才有直观效果。

3. A*核心算法设计：三维邻域扩展、启发函数与代价函数

3.1 节点数据结构与邻域生成

A*搜索的基本单位是节点。在Matlab里我习惯用结构体表示：

node = struct(... 'pos', [x, y, z], ... % 栅格坐标 'g', inf, ... % 起点到当前节点的实际代价 'h', 0, ... % 当前节点到目标点的启发估计 'f', inf, ... % 总代价 f = g + h 'parent', [] ... % 父节点索引，用于路径回溯 );

在二维A*里，常用4邻域或8邻域；三维场景下，对应的是6邻域和26邻域。6邻域只允许上下左右前后移动，路径段数多、角度生硬；26邻域允许对角移动，路径更平滑，但节点扩展量更大。实际无人机路径规划里，26邻域是主流选择，因为飞行方向本身是连续的，26个方向能提供更好的灵活性。

26邻域生成代码：

neighbors = [ -1 -1 -1; -1 -1 0; -1 -1 1; -1 0 -1; -1 0 0; -1 0 1; ... -1 1 -1; -1 1 0; -1 1 1; 0 -1 -1; 0 -1 0; 0 -1 1; ... 0 0 -1; 0 0 1; 0 1 -1; 0 1 0; 0 1 1; ... 1 -1 -1; 1 -1 0; 1 -1 1; 1 0 -1; 1 0 0; 1 0 1; ... 1 1 -1; 1 1 0; 1 1 1 ]; % 注意其中的 [0 0 0] 被手动剔除了

遍历邻居时，要依次检查三个条件：新位置是否在地图边界内、新位置是否不是障碍物、新位置是否不在close list中。都满足才能加入open list。这里有一个很多人会忽略的细节：对角移动穿过角落障碍物的问题。比如从(0,0,0)移动到(1,1,0)，即使目标格不是障碍物，如果(1,0,0)和(0,1,0)有一个是障碍物，实际飞行时无人机可能会擦碰到障碍物边缘。严格的做法是，对角移动时检查相邻的轴向格是否同时为空：

% 检查对角移动是否安全 if abs(dx) == 1 && abs(dy) == 1 if map3D(x+dx, y, z) == 1 || map3D(x, y+dy, z) == 1 continue; end end % 其他维度组合同理

这个细节我建议一定加上，虽然代价是搜索略微变慢，但生成路径的可飞性会明显提升。

3.2 代价函数：不只是路径长度

A*的代价函数分为两部分：从起点到当前节点的实际代价g，以及从当前节点到目标点的启发估计h。三维路径规划里，g应该包含哪些项？

最朴素的做法是把g设为路径的欧氏距离累加。但实际无人机飞行中，频繁改变高度、靠近威胁区域都会增加真实代价，所以更合理的定义是：

g_new = g_current + step_cost + height_penalty + threat_penalty;

其中：

• step_cost：当前节点到邻居节点的距离。对角移动的距离是grid_size * sqrt(3)，轴向移动是grid_size。这一步能给搜索一个沿直线前进的偏好。
• height_penalty：高度变化惩罚。如果|z_new - z_current| > 0，则加上一个与高度差成正比的惩罚项。这能有效避免路径在竖直方向上剧烈抖动。
• threat_penalty：威胁代价。如果路径经过靠近障碍物的栅格，即使没有碰撞，也给予额外代价，迫使算法尽可能远离障碍物。

一个常见的威胁代价计算方法是高斯衰减：

function threat = calcThreat(node, obstacles) threat = 0; for i = 1:length(obstacles) d = norm(node - obstacles(i).center); if d < obstacles(i).radius + safety_margin threat = threat + 1 / (d^2 + 0.01); end end end

这里safety_margin是安全距离余量，建议设为grid_size的一半，防止路径紧贴障碍物表面。

3.3 启发函数选哪个

启发函数h必须满足两个条件：可采纳性（admissible）和一致性（consistent）。可采纳意味着估计值不大于真实代价；一致性意味着三角不等式成立。满足这两个条件，A*才能保证找到最优路径。

三维空间中，最常用的可采纳启发函数有三种：

实际编码中，我推荐优先采用欧氏距离。原因有三：第一，代码简单，一行搞定；第二，虽然扩展节点数比对角线距离多一些，但在栅格规模不太大的场景下（比如150×150×50），差距在几十毫秒内，完全可接受；第三，欧氏距离也是最容易向读者解释清楚的——它就是两点间的直线距离。

h = norm((goal - current) * grid_size);

注意这里需要把栅格坐标转换回实际物理距离，否则单位不一致会导致g和h的尺度不匹配。我见过有人忽略这一步，结果A*变成了贪心算法，效果非常差。

4. Matlab代码实现：主循环、碰撞检测与路径回溯

4.1 主循环：open list和close list的管理

A*的主流程不复杂，但代码细节决定了它能处理的地图规模。先说数据结构，Matlab最直接的做法是用数组当open list，然后每次找f值最小的节点——这个操作的复杂度是O(n)，in open list检查也是O(n)。地图小没问题，地图大的时候性能就会有明显问题。

如果追求高性能，可以考虑用Java的PriorityQueue对象。Matlab支持java.util.PriorityQueue，配合比较器可以使用，但类型转换比较麻烦。

考虑到演示代码的可读性优先，我保留了结构清晰的数组方案，但做了一些优化：

• close list用一个三维逻辑数组closed = false(numX, numY, numZ)存储，查找复杂度为O(1)，不用反复遍历。
• g值和f值分别用三维数组gScore和fScore存储，避免在结构体数组里反复查询。
• open list仍然用数组，但只保存节点的索引号。

主循环核心代码：

while ~isempty(openList) % 找到f值最小的节点 [minF, idx] = min(fScore(openList)); current = openList(idx); % 到达目标点 if isequal(current, goal) path = reconstructPath(cameFrom, start, goal); return; end % 移出open list openList(idx) = []; closed(current(1), current(2), current(3)) = true; % 遍历邻居 for i = 1:size(neighbors, 1) nb = current + neighbors(i, :); % 边界检测 if any(nb < 1) || nb(1) > numX || nb(2) > numY || nb(3) > numZ continue; end % 障碍物检测 if map3D(nb(1), nb(2), nb(3)) == 1 continue; end % 对角穿越检测 if ~isDiagonalSafe(current, nb, map3D) continue; end % close list检测 if closed(nb(1), nb(2), nb(3)) continue; end % 计算新g值 dist = norm(nb - current) * grid_size; tentative_g = gScore(current(1), current(2), current(3)) + dist + ...; % 更新条件判断 if ~isKey(nb) || tentative_g < gScore(nb(1), nb(2), nb(3)) cameFrom(nb(1), nb(2), nb(3)) = sub2ind([numX, numY, numZ], current(1), current(2), current(3)); gScore(nb(1), nb(2), nb(3)) = tentative_g; fScore(nb(1), nb(2), nb(3)) = tentative_g + h(nb, goal); openList = [openList; nb]; end end end

4.2 碰撞检测与安全边界

碰撞检测是路径规划安全性的核心，前面提过对角穿越的问题，这里再补充两个实际会踩到的坑。

第一个坑是无人机尺寸与栅格尺寸的关系。很多人用map3D(i,j,k)==1作为唯一判断条件，这意味着无人机被当成一个体积为零的质点。实际上无人机是有尺寸的，需要做膨胀处理。最省事的办法是在离线建图阶段就把障碍物膨胀一圈：

% 对障碍物进行膨胀，膨胀距离为无人机半径 inflated_map = imdilate(map3D, strel('sphere', ceil(uav_radius / grid_size)));

用strel('sphere', r)做三维膨胀操作，然后所有碰撞检测都基于inflated_map进行。这样搜索代码不用做任何调整，路径自然就避开了障碍物边缘。

第二个坑是起点和终点本身就落在障碍物内，或者落在了膨胀区域内。这种情况A*会直接返回空路径，而且代码不会报错，你只会得到path == []。所以在算法启动前，必须做一次合法性检查：

assert(map3D(start(1), start(2), start(3)) == 0, '起始点位于障碍物内'); assert(map3D(goal(1), goal(2), goal(3)) == 0, '目标点位于障碍物内');

4.3 路径回溯与合理终止条件

搜索完成后，回溯路径的逻辑和二维版本完全一致。由于我们在cameFrom数组里保存的是父节点的线性索引，回溯时用ind2sub转回三维坐标即可：

function path = reconstructPath(cameFrom, start, goal) path = goal; current = goal; while ~isequal(current, start) idx = cameFrom(current(1), current(2), current(3)); if isempty(idx) error('路径断裂'); end [x, y, z] = ind2sub(size(cameFrom), idx); current = [x, y, z]; path = [current; path]; end end

这里有个容易出bug的细节：cameFrom数组需要初始化为全0，否则A*没有扩展到的节点在回溯时可能被误认为是起点。回溯循环中加入isempty(idx)判空是个好习惯，能避免路径断裂时陷入死循环。

关于终止条件，很多人只检查"当前节点是否等于目标节点"，这在栅格精度很低时没问题，但栅格尺寸较大时，目标点不一定恰好落在某个栅格中心。更稳健的做法是：当当前节点与目标点的距离小于某个阈值（比如一个栅格尺寸）时，就认为到达目标，然后直接把目标点接到路径末尾。

if norm(current - goal) * grid_size <= 1.5 * grid_size path = [reconstructPath(cameFrom, start, current); goal]; return; end

这个"先搜索到目标附近，再修正到精确目标"的策略，比严格要求栅格重合要实用得多。

5. 路径平滑与安全距离校验：让算法结果真正可飞

5.1 A*路径为什么会有一堆折线

A*搜索出来的是由栅格中心点连成的折线路径，虽然拐点被限制在26个方向，但在栅格尺寸较大时，路径依然会出现明显的锯齿状——路径段一会儿斜着向上，一会儿斜着向下，频率很高。无人机飞这样的路径，不仅要频繁调整姿态，而且实际飞行距离远大于规划距离。

原因很好理解：A的最优是"栅格意义上的最优"，不是"几何意义上的最优"。栅格把连续空间离散化了，最优折线路径不一定等于最优光滑曲线。这是所有基于栅格的搜索算法的通病，不是A特有的问题。

所以路径平滑是三维路径规划里必不可少的一个环节。这里的"平滑"不是简单的低通滤波，而是要在保留路径大致走向的前提下，把多余的拐点去掉。

5.2 路径节点抽稀：去掉冗余拐点

最简单有效的抽稀方法是贪婪算法：

1. 从起点开始，尝试连接后续的节点，检查这条线段是否会穿过障碍物；
2. 如果能直线到达某个节点，则中间的所有节点都可以删除；
3. 从当前节点重复上述过程，直到到达终点。

这个算法本质上是把问题简化成了"尽可能用长直线段逼近原路径"。用生活类比，就像你走了一条弯弯绕绕的小路，后来发现大多数弯道都没有必要，直接用一条大直路穿过去就行。

function smoothed = greedySmooth(path, map3D, grid_size) smoothed = path(1, :); cur = 1; i = 2; while i <= size(path, 1) if ~isSegmentSafe(path(cur, :), path(i, :), map3D, grid_size) % 无法直线到达path(i)，则保留path(i-1) smoothed = [smoothed; path(i-1, :)]; cur = i - 1; i = cur + 1; else i = i + 1; end end smoothed = [smoothed; path(end, :)]; end

isSegmentSafe函数沿线段离散采样若干点，逐一检查是否碰撞。离散采样间距取grid_size / 3比较稳妥，既不会漏检障碍物，也不会因为采样过密拖慢速度。

需要特别提醒的是：抽稀后的路径点必须校验安全距离。由于抽稀会用长直线段代替原来的小步进路径，原来贴着障碍物绕行的路径在拉直后，线段中段可能逼近障碍物。安全校验的算法很简单，计算采样点到每个障碍物的距离，如果小于安全裕度就拒绝抽稀。

5.3 三次样条插值平滑

抽稀之后，路径只保留了少数关键节点，此时用三次样条插值生成光滑曲线。Matlab自带的cscvn函数（对三维点列做自然三次样条插值）可以直接用：

% 将路径分为X,Y,Z三个分量，用cscvn生成样条曲线 spline_curve = cscvn(smoothed'); % 采样生成密集轨迹 t = linspace(0, spline_curve.breaks(end), 500); points = fnval(spline_curve, t);

这样得到的路径是一条C2连续的光滑曲线，适合直接作为无人机轨迹的几何参考。这里要注意，样条插值后的点是否碰撞需要再校验一次，因为样条曲线会在节点之间产生"过冲"，可能侵入障碍物区域。如果发现碰撞，可以减小插值密度，或者在碰撞段附近插入额外的引导点。

6. 代码实测效果与三类高频踩坑记录

6.1 典型场景的实测参数与结果

我测试时用的地图尺寸为150×150×50米，栅格尺寸1米，起点在(5, 5, 5)，终点在(145, 145, 40)，中间设置了两个球形障碍物和一个圆柱障碍物。运行环境是Matlab R2023a，普通笔记本（i5-1135G7 + 16GB内存）。

实测数据如下：

这组数据有个很有意思的结论：26邻域虽然扩展方向是6邻域的4倍多，但总扩展节点数反而更少。原因是26邻域下启发函数更准确，算法能更直接地朝目标方向搜索，不会像6邻域那样大量走回头路。同时也验证了理论上"26邻域+对角线距离"性能最佳，但和"26邻域+欧氏距离"差距并没有想象中大——所以如果你只想用最简单的实现，欧氏距离也完全够用。

6.2 坑记录一：启发函数高估导致次优路径

有个粉丝拿我的代码去跑自己的地图，跑来问为什么路径绕了远路。我一看他的代码，启发函数用的是曼哈顿距离，h = abs(goal(1)-current(1)) + abs(goal(2)-current(2)) + abs(goal(3)-current(3))，而且g的计算里包含了45度方向和54.7度方向的真实距离。问题立刻清楚了。

曼哈顿距离假设只能沿坐标轴移动，这在四邻域下是对的；但在26邻域下，真实移动距离总是小于等于曼哈顿距离（比如从(0,0)到(1,1,1)，曼哈顿距离是3，真实距离是1.732），所以h严重高估了剩余代价。A*的核心前提被破坏后，算法会把f值较小的"看起来快到了"的节点优先扩展，结果找到的路径不是最短路径。这种现象在三维空间里比二维更明显，因为对角方向的自由度更大。

排查方法很简单，修改启发函数后用同一张地图跑一遍，对比路径总长度。如果修正后的路径更短，说明之前确实高估了。

6.3 坑记录二：open list去重逻辑缺失造成内存爆炸

三维A*里节点数量大，open list去重是一个非常影响性能的因素。我在第一版代码里犯过一个错误：没有在加入open list时检查节点是否已经在里面，而是直接追加。这个做法在二维地图上问题不大，但在三维地图上，同一个节点可能被多个邻居重复加入几十次，open list以指数级膨胀，搜索到一半内存就爆了。

解决办法其实简单，维护一个与地图同尺寸的逻辑数组：

inOpenList = false(numX, numY, numZ); % 加入open list时 if ~inOpenList(nb(1), nb(2), nb(3)) openList = [openList; nb]; inOpenList(nb(1), nb(2), nb(3)) = true; end

这里还有个小优化：如果节点已经被加入open list且获得了更小的g值，不需要把节点重复添加，只需要更新对应的gScore和fScore数组。这样open list的长度始终不超过地图总节点数。

6.4 坑记录三：可视化阶段坐标轴比例不一致造成假碰撞

最后这个坑不是算法本身的问题，而是可视化的问题。Matlab的plot3在默认情况下，X、Y、Z轴的单位长度是一致的，但如果你手动设置了axis equal，或者绘制时把地图尺寸缩放过，就可能出现"看起来路径穿过障碍物，实际没有"或"看起来没碰，实际碰了"的假象。

三维地图长宽高差异很大时，建议用axis equal保持坐标轴比例一致，否则视觉上路径和障碍物的相对位置会失真。另外，scatter3和plot3混用时，建议先用hold on锁定图形，再逐步叠加绘制障碍物和路径。

6.5 后续可以怎么扩展

这一版代码已经能跑通"静态环境下的三维路径规划"，但离实际工程应用还有几步距离：

• 动态避障：把A和DWA（动态窗口法）结合，全局路径用A规划，局部避障用DWA实时调整；
• 多目标点规划：对起点到多个任务点做TSP求解，再用A*实现段间路径；
• 非栅格地图扩展：改成概率路线图（PRM）或RRT*，在连续空间搜索路径，适用于更复杂的山地环境。

我个人觉得，对于做课程设计或论文仿真的同学，把这版代码吃透已经完全足够。A*算法本身不难，难的是你在实现过程中是否真正理解了每一步的"为什么"——为什么启发函数不能高估、为什么对角移动要做碰撞检测、为什么要做路径抽稀。把这些想清楚，你就能做到举一反三，换成任何算法都能快速上手。