企业官网建设流程全解析

前阵子帮一个做农用巡田无人机的朋友整航线预规划原型，需求很朴素：已知起降点和目标测区，中间有山头、信号塔和一排高压线走廊，希望自动生成一条不撞障碍物的三维航线，而不是靠人坐在电脑前手动点几个航点。我第一反应就是上A星算法，直接拿Matlab把三维路径规划先跑通验证——这可能是所有路径规划方案里最容易被想到、也最适合快速验证逻辑的思路。这篇文章就把这套基于A星算法的无人机三维路径规划完整实现过程写出来，从栅格地图建模、核心搜索代码、路径平滑，到参数调优和上真机前要补的坑，一次讲清楚。适合正在做无人机相关毕设、竞赛的同学，也想给想快速在Matlab里验证路径规划算法的工程师做个参考。

先说结论：A星算法在“静态已知三维环境里求一条较优参考航点”这件事上非常能打，但工程落地时真正决定成败的往往是地图建模和路径后处理，而不是那一百多行搜索主循环。下面按我当时推进的顺序一步步拆解。

1. 为什么把路径规划敲定给A星算法：候选算法排查与选型逻辑

1.1 无人机三维路径规划到底在规划什么

无人机从A点飞到B点，听起来就是一条线段的事，但实际上要满足不少约束：地形不能穿越、障碍物要留安全间隙、高度不能低于最低安全高度，也不能超过飞行升限。如果用数学语言描述，就是在一个三维连续空间里找出满足约束的曲线，同时尽量让路径最短或者耗能最小。

问题在于连续空间是无限维的，没法直接拿计算机去穷举。所以通常的做法是先把空间离散化成一个个小格子，也就是体素网格，然后在这个离散图结构上搜索。A星的搜索对象就是这种栅格化之后的三维地图，输出是一条由离散点连成的折线路径，后续再交给平滑模块转成可飞行的航线。

1.2 和RRT、遗传算法放一起比，A星的位置在哪

我当时也把其他几个常见方案放在桌面上比了一圈，毕竟“路径规划算法”这顶帽子底下选择性太多，不逐个排查容易选错。

对照这个表，我当时的任务很明确：环境是静态已知的，离线规划可以接受，目标就是给出一个确定、可靠的航点序列。这种情况下A星是最划算的选择。RRT虽然灵活，但每次跑出来的路径都不一样，还得花时间平滑和去抖；遗传算法这类群体优化方法在这些年经常出现在论文里，但作为工程验证的起手式有点杀鸡用牛刀，而且参数不好调。

1.3 A星的适用边界

A星也有明显的天花板：搜索空间随栅格数量指数膨胀，30×30×20的体素网格跑起来很轻松，一旦到几百乘几百的高程范围就会卡顿。另外A星本质是几何路径搜索，不包含飞机动力学或者转弯半径约束，所以它输出的路径只能算“航点参考线”，不能直接说这就是飞行轨迹。我对它的定位一直是“全局规划的第一层”，后面必须跟着平滑、速度规划这些模块。

2. 三维栅格地图建模：把真实空域变成Matlab能搜的矩阵

2.1 从真实环境数据到体素栅格的映射流程

A星搜索的输入是一张三维布尔矩阵，0表示可通行，1表示障碍。真实环境数据怎么变成这个矩阵，是整个流程里最容易出错的一环，因为后面搜索算法再漂亮，地图错了也是白费。

我习惯的流程是：先获取测试区域的数字高程模型（DEM）或者点云数据，再按照设定分辨率把水平面切成x、y方向的格子，每个格子根据地面高度和地面以上的障碍物高度，填充对应层的体素。简单说就是先在地面上建一个“高度柱”，柱子里有物体就全部标成1。如果地形上有一座山，山体范围内的(x, y)格子里，从地面到山顶高度这一段的z层都要标记成障碍。

如果只用公开的高程数据做仿真，通常没有建筑物和树木的细节，这时候我会手动叠加一些模拟的障碍物，比如信号塔、高楼、高压线走廊。仿真阶段不需要百分之百还原真实地形，但障碍物的相对空间关系一定要正确，否则调出来的路径完全没有参考性。

2.2 障碍物膨胀和安全高度层：不能忽略的两件事

真实无人机有翼展和旋翼半径，不可能贴着障碍物表面飞行，所以在地图建模时必须对障碍做膨胀处理。通俗说就是把每个障碍物周围一定半径内的格子也标记为不可通行。比如飞机半径假设0.5米，栅格分辨率1米，那就把障碍物外扩至少1格。膨胀不足，后面生成的路径会穿过危险区域；膨胀过头，又会把可飞走廊堵死。

另一个容易被忽略的是安全高度层。很多区域不是硬障碍，但无人机不适合飞太低，比如高压线走廊上方、居民区屋顶上方、树木冠层上方。这类约束我用两种方式处理：要么直接在地图里把z轴低层全部标记为1，强制飞机爬升到设定高度以上；要么在代价函数里加一个高度惩罚项，让A星尽量选择高空航线，但不把低空彻底封死。实际使用中，如果任务没有强制低空飞行的需求，我更推荐直接抬高最低飞行高度层，简单可靠。

2.3 生成模拟测试地图的Matlab代码

在Matlab里生成一张测试地图我通常这么写：

function map = createTestMap(nx, ny, nz) % nx, ny, nz 分别是x轴、y轴、z轴方向的栅格数量 map = zeros(nx, ny, nz); % 中央山体：以(xc, yc)为中心，半径8格，高度随距离递减 xc = round(nx * 0.5); yc = round(ny * 0.5); for x = 1:nx for y = 1:ny d = sqrt((x - xc)^2 + (y - yc)^2); if d <= 8 h = max(2, round(16 - d * 1.2)); % 山顶约16层，边缘逐渐变矮 if h <= nz map(x, y, 1:h) = 1; end end end end % 两座信号塔/高楼，模拟竖直柱状障碍 towers = [8, 22, 10; 24, 6, 12]; for k = 1:size(towers, 1) tx = towers(k, 1); ty = towers(k, 2); th = towers(k, 3); map(tx:tx+1, ty:ty+1, 2:th) = 1; end % 最底层全部设为障碍，防止搜索跑到“地下” map(:, :, 1) = 1; end

这段代码里有个细节：最底层直接全部填成障碍。原因很简单，如果z=1这层能通行，A星经常会把路径压到地图最下面走，遇到地形数据缺失或者边界处理不当，就容易穿地。保守一点，把地面层封死，让搜索只能在安全高度以上活动。

我第一次跑通三维A星时犯过一个典型的坐标系混淆错误：地图里map(x, y, z)我把x和y对应反了，画出来的路径直接斜穿信号塔，Visualization看起来非常滑稽。建议在代码文件顶部用注释写清楚“map(x, y, z)，x对应横坐标，y对应纵坐标，z对应高度”，后面画图、取坐标时都统一按这个约定。

2.4 为什么不能简单从二维栅格“升维”成三维

很多人觉得二维A星写熟了，三维不就是多一层z循环吗？其实没那么简单。二维地图如果边长是n，栅格总数是n^2，三维就是n^3，搜索空间体量完全不同。我之前帮人改过一个二维A星demo，强行把高度方向复制成20层，结果原本几毫秒跑完的搜索直接卡了几十秒。原因是原来二维地图里很多自由空间到了三维会变成巨大空洞，A星把大量时间花在探索这些不必要的高度组合上。

实际工程里我一般不会一上来就用全分辨率的完整三维体素，而是先做分层规划：第一层用低分辨率粗网格快速得到一个粗路径，第二层只在粗路径所在的带状区域内用细网格精修。这样既保留了三维搜索的避障能力，又不会让计算量爆炸。

3. A星搜索核心拆解：启发式、邻域扩展与回溯的实现细节

3.1 数据组织：用三维矩阵替代数组遍历的Open表

网上大量A星教程习惯用open表和closed表存节点对象，每个节点包含坐标、g值、h值、父节点指针。这个方法在二维小地图上没问题，但三维栅格地图动辄几十万格，每次都要在open表里线性扫一遍找最小f值，Matlab会慢到无法接受。

我在实现里换成了一种更贴合Matlab思维方式的数据组织：直接用三维矩阵存g值和f值，每个格子对应一个固定位置，查找和更新都是常数时间，不需要反复对象比较。open表只需要存候选坐标，每次迭代时从open表里取f值最小的节点。closed逻辑则用一个inOpen布尔矩阵加inf判断来处理。

这样做的优势很明显：不管地图多大，判断一个节点是否已经搜索过，只需要查一次矩阵，代码也更简洁。

3.2 启发式函数的三维形态与可采纳性

A星的效率核心是启发式函数h(n)。在二维里常见的是曼哈顿距离和欧氏距离，三维里我几乎只用欧氏距离：

h(n) = w * sqrt((x_n - x_g)^2 + (y_n - y_g)^2 + (z_n - z_g)^2)

其中w是启发式权重，通常取1.0。这里必须强调可采纳性概念：如果启发式函数永远不高估真实代价，A星保证能找到最优路径。欧氏距离在允许对角移动的三维栅格中是可采纳的，因为任意两点间最短真实距离不会小于直线距离。但曼哈顿距离在三维对角移动场景下会高估，导致路径虽然搜得快，但可能丢掉最优解。所以三维A星里我默认用欧氏距离，只有在强制6邻域移动时才考虑曼哈顿距离。

3.3 邻域扩展与移动代价修正

三维A星的邻域扩展和二维最大的区别在于移动代价的修正。我的做法是遍历dx、dy、dz从-1到1的所有组合，排除全零组合，这样一共是26个候选方向。

移动代价不是统一按1算，而是按欧氏距离：轴向移动代价1，面对角移动代价sqrt(2)，体对角移动代价sqrt(3)。这一步非常关键，否则算法会“偏爱”斜着走，因为斜走消耗的g值被低估了，直接后果就是路径会扭曲成奇怪的45度斜线，并且总路径长度算出来明显偏短。

对于固定翼无人机来说，体对角移动还要额外谨慎，因为真实飞机无法“瞬间横移一个格子”，这种大角度机动在平滑阶段会被修正掉。但如果只是做多旋翼的航点预规划，允许体对角没太大问题。

3.4 主循环代码：核心搜索骨架

这是整个实现里最核心的一段代码，保存为astar3d.m：

function [path, cost] = astar3d(map, start, goal, hWeight) % 三维A星搜索 % 输入: % map - 三维逻辑/数值矩阵, 0=可通行, 1=障碍 % start - 起点坐标 [x, y, z] % goal - 终点坐标 [x, y, z] % hWeight - 启发式权重, 默认1.0 % 输出: % path - Nx3 路径点序列 % cost - 总路径代价 [sx, sy, sz] = size(map); start = round(start(:)'); goal = round(goal(:)'); if map(start(1), start(2), start(3)) == 1 error('起点位于障碍物内部'); end if map(goal(1), goal(2), goal(3)) == 1 error('终点位于障碍物内部'); end gScore = inf(sx, sy, sz); gScore(start(1), start(2), start(3)) = 0; fScore = inf(sx, sy, sz); fScore(start(1), start(2), start(3)) = hWeight * norm(start - goal); cameFrom = zeros(sx, sy, sz, 3); % 每个位置记录父节点坐标 openList = struct('x', start(1), 'y', start(2), 'z', start(3)); isInOpen = false(sx, sy, sz); isInOpen(start(1), start(2), start(3)) = true; expandCount = 0; while ~isempty(openList) % 取出f值最小的节点 fVals = arrayfun(@(n) fScore(n.x, n.y, n.z), openList); [~, idx] = min(fVals); cur = openList(idx); openList(idx) = []; isInOpen(cur.x, cur.y, cur.z) = false; % 到达终点则回溯 if cur.x == goal(1) && cur.y == goal(2) && cur.z == goal(3) path = reconstructPath(cameFrom, cur); cost = gScore(goal(1), goal(2), goal(3)); fprintf('扩展节点数: %d\n', expandCount); return; end expandCount = expandCount + 1; % 遍历26邻域 for dx = -1:1 for dy = -1:1 for dz = -1:1 if dx == 0 && dy == 0 && dz == 0 continue; end nx = cur.x + dx; ny = cur.y + dy; nz = cur.z + dz; if nx < 1 || nx > sx || ny < 1 || ny > sy || nz < 1 || nz > sz continue; end if map(nx, ny, nz) == 1 continue; end stepCost = norm([dx, dy, dz]); % 1 / sqrt(2) / sqrt(3) tentativeG = gScore(cur.x, cur.y, cur.z) + stepCost; if tentativeG < gScore(nx, ny, nz) gScore(nx, ny, nz) = tentativeG; fScore(nx, ny, nz) = tentativeG + hWeight * norm([nx, ny, nz] - goal); cameFrom(nx, ny, nz, :) = [cur.x, cur.y, cur.z]; if ~isInOpen(nx, ny, nz) openList(end+1) = struct('x', nx, 'y', ny, 'z', nz); isInOpen(nx, ny, nz) = true; end end end end end end % 搜索失败 path = []; cost = inf; disp(['搜索失败，扩展了 ', num2str(expandCount), ' 个节点']); end

回溯函数单独写一个reconstructPath.m：

function path = reconstructPath(cameFrom, cur) path = [cur.x, cur.y, cur.z]; while true px = cameFrom(cur.x, cur.y, cur.z, 1); py = cameFrom(cur.x, cur.y, cur.z, 2); pz = cameFrom(cur.x, cur.y, cur.z, 3); if px == 0 && py == 0 && pz == 0 break; end cur = struct('x', px, 'y', py, 'z', pz); path = [cur.x, cur.y, cur.z; path]; end end

这段代码在30×30×20的地图上跑起来是毫秒到百毫秒级别，算得上“开箱即用”。

3.5 画图验证时最容易发现的问题

搜索代码写完别急着往下走，一定要先把结果画出来看。我用的是：

map = createTestMap(30, 30, 20); start = [2, 2, 3]; goal = [28, 28, 15]; [path, cost] = astar3d(map, start, goal, 1.4); plot3(path(:, 1), path(:, 2), path(:, 3), 'r-', 'LineWidth', 2);

画出来如果路径出现“贴墙走”“穿山体”“在地图最底层乱窜”，百分之九十是地图标记或坐标轴方向的问题。特别是z轴方向，我最初把高度方向压到了矩阵的第一维，后面所有索引全乱了。调试这种问题最快的方法是在地图里放一个只在特定高度存在的障碍，比如塔楼，然后看路径是否真的绕过了塔顶。

4. 从栅格骨架到平滑航线：后处理环节不能省

4.1 为什么不能把A星的折线直接丢给飞控

A星返回的路径本质上是沿着体素中心走出来的折线，转角都是硬生生90度或者45度，真实无人机根本不可能这样飞。多旋翼在这种航线上会大幅减速，固定翼更不可能完成这种瞬间转向。所以一拿到路径就开始读飞控文档是不现实的，中间必须补平滑这一步。

平滑的目标只有一个：在保持路径大致形状和避障结果的前提下，把锐利的拐角磨圆，让航迹变得连续可飞。

4.2 B样条平滑的具体操作

如果装了Robotics System Toolbox，可以直接用bsplinepolytraj对路径做B样条拟合，方便但不保证避障。如果没装，也可以手写B样条基函数，核心是先确定控制点，再沿曲线采样得到平滑后的航点序列。

我的经验是不要对整条路径做全局B样条拟合，因为容易把障碍物附近的折线拉直，导致撞障碍。更稳妥的做法是“安全走廊平滑”：先把A星路径上每个点向外扩展一个安全半径，形成一条管道，平滑后的路径点只允许在管道内移动，这样既光滑又不会偏离原避险轮廓太多。具体实现不复杂，但能大幅降低后面碰撞检测失败的概率。

4.3 平滑后的碰撞检测：必须做，不能信感觉

平滑算法不会感知障碍物，所以平滑后的每个采样点都要重新查一遍地图，确认没有落到障碍格里。我给这段写了个小函数：

function ok = validateCollision(pathSmooth, map, sampleStep) % 沿平滑后路径逐段采样，检查是否碰撞障碍物 ok = true; for i = 1:size(pathSmooth, 1) - 1 segLen = norm(pathSmooth(i+1, :) - pathSmooth(i, :)); nSamples = max(2, ceil(segLen / sampleStep)); for k = 0:nSamples pt = pathSmooth(i, :) + (pathSmooth(i+1, :) - pathSmooth(i, :)) * k / nSamples; idx = round(pt); if idx(1) < 1 || idx(1) > size(map, 1) || ... idx(2) < 1 || idx(2) > size(map, 2) || ... idx(3) < 1 || idx(3) > size(map, 3) continue; end if map(idx(1), idx(2), idx(3)) == 1 ok = false; return; end end end end

碰撞检测失败后，不要无脑调低平滑强度，大多数情况是地图膨胀半径不够或者安全高度下限太低。把膨胀半径调大一点重新规划，往往比在平滑阶段反复试参数更有效。

4.4 航点抽稀与安全高度修正

A星在空旷区域也会一格一格地走，生成的航点可能有几百个，直接传给飞控既浪费带宽，部分飞控还有航点数量上限。抽稀我常用Ramer-Douglas-Peucker算法，把在一条直线上冗余的中间点删掉，只保留必须的转折点。如果主控对航点数量有严格限制（比如只有50个航点槽位），这一步必不可少。

还有一个细节：如果地图里最低安全飞行高度设置过低，A星可能会给出贴着屋顶高度飞行的路径。真实飞行里这种路径没法用，我会在规划前就把z轴下限整体抬高到安全高度以上，而不是等路径出来后再去修改。

5. 启发式权重、网格密度和邻域模式：我实测过的三组调参数据

这一节分享一下我在这套代码里实际做过的三组对比测试。测试环境是Matlab R2022b，处理器i7-12700H，地图用的是30×30×20的体素网格，中央一座山体加两座塔楼，起点(2,2,3)，终点(28,28,15)。

5.1 启发式权重w：一个旋钮，两种性格

w越大，算法越“贪心”，越倾向于朝目标方向猛冲，代价就是可能错过真正的最优绕行通道。如果地图通道很窄，比如两个障碍物之间只有一格缝隙，w过大可能直接找不到路。所以我在环境复杂的场景里宁可用1.0到1.2，只有在空旷大平原这种场景才会调到1.5以上去压时间。

5.2 网格密度：精度和效率的权衡

同样一块区域，把分辨率从1米改成0.5米，栅格总量变成8倍。比如从30×30×20到60×60×40，体素数量从18000涨到144000，gScore和fScore各占一个double数组，cameFrom是四维数组，内存直接飙升。实测下来运行时间从不足1秒变成十几秒，路径的改善却微乎其微。

碰到这种情况我的做法是两层规划：第一层用粗网格快速抓全局轮廓，第二层在粗路径周围取一个窄带，再在这个窄带里用细网格重新跑A星。效果接近全局高分辨率搜索，时间却少一个量级。

5.3 邻域模式对比：6/18/26邻域的实测差异

三维路径里体对角移动可以让路径更“斜”地穿过空旷空间，减少折角，对固定翼尤其有意义。但如果地图障碍密集，26邻域会让搜索在障碍缝隙里浪费大量时间，18邻域反而更高效。

5.4 Matlab实现的性能瓶颈在哪

这套教学版代码在Matlab里的主要性能瓶颈有三个：openList删除元素、arrayfun遍历struct、三重循环的领域扩展。20万体素以下还能忍，再大就得换数据机构。我建议如果以后要跑大面积真地形数据，要么把openList改成最小堆，要么直接用Java的PriorityQueue（Matlab里可以直接new Java对象），要么干脆写成C++ mex。研究验证阶段Matlab完全够用，工程化之后再迁移不迟。

6. 从Matlab仿真到真机试飞前：还有四类问题要兜住

6.1 坐标系统一与航点坐标转换

Matlab里的栅格索引不是经纬度，也不是UTM坐标，而是“第几格”。真要导给飞控，必须先建立栅格坐标和真实地理坐标之间的映射关系。我的做法是给地图定义一个左下角原点经纬度，再定一个网格分辨率，然后把路径点换算成经纬高或者本地东北天坐标。

这个环节特别容易栽跟头。有一次我把WGS84高程和本地大地水准面高程混在一起，算出来的航线整体偏低了几十米，还好是在仿真阶段看出来的。建议从项目一开始就明确统一坐标系，并写进代码注释，别指望靠记忆。

6.2 动态障碍物与定期重规划

A星只能处理静态地图，飞着飞着如果突然有一座塔吊进入航线，之前规划的路径就失效了。工程上最简单的兜底方案是启动定期重规划：无人机每飞行固定距离或者间隔几秒，以当前位置为起点重新跑一次A星，目标点不变。这样虽然不能保证最优，但至少能应对突发障碍。如果场景里需要更快的重规划响应，可以研究一下D* Lite这类增量搜索算法。

注意重规划时无人机当前位置几乎不可能正好落在栅格中心，需要先取整到最近的可行体素，再把它作为新起点。否则规划起点落在障碍格里，程序会直接报错。

6.3 速度剖面：只有路径没有速度还是不能飞

A星输出的是位置点序列，飞控还需要知道每一段飞多快、在哪里减速。我通常在平滑后的航线上做梯形速度规划：每个航点设定一个目标速度，拐弯前的减速段按最大加速度约束倒推出来。更简单的做法是统一巡航速度，在拐点附近自动减速。不管用哪种方式，速度剖面都要在Matlab里同步可视化，和路径画在一起，看有没有加速度突跳。

6.4 改进方向：JPS+、混合A星和分层规划

文章最后列一下几条后续可以深挖的路线。JPS+在大量空旷自由区域里能跳过中间冗余格子，搜索效率会比A星高很多，前提是地图可以预处理。混合A星把无人机运动学模型并进搜索过程，输出的是直接可执行的轨迹，但实现复杂度高不少。分层规划前面已经提过，是放大规模时最务实的办法。如果对动态环境有强需求，D* Lite和RRT*也各有适用场景。

我自己在这套A星代码往后推了几个项目之后，最深的感受是：算法本身真的不难，真正让我花时间的是地图数据的质量、坐标系的一致性，还有平滑后那一次碰撞检测。地图一旦不可信，后面所有环节都是空中楼阁。所以如果你也想用Matlab快速验证无人机三维路径规划的想法，我的建议是把精力重点放在地图建模和路径校验上，A星主体代码一百多行写好之后，几乎不用动，耐下心来把环境和约束设计清楚，结果自然就稳了。