简介面向路径规划算法研究与无人机应用开发者的MATLAB A算法实现包专注于城市空中交通场景下的无人机包裹递送路径规划。围绕A启发式搜索机制代码示例给出地图构建、代价函数设定、动态航路调整等关键环节的实现方法帮助解决复杂环境中无人机避障与实时交通适应问题。压缩包共9个文件以7个m源文件为主覆盖主程序、A-star核心逻辑、最小堆、图构建等模块另含1个png效果图与1个docx学术说明文档整体大小仅159KB便于快速下载与阅读。目前已有98人学习下载。使用者可基于代码直接运行示例理解优先级队列选择、启发函数设计等细节同时结合文档中的原理分析和运行步骤快速验证A*算法在该场景下的可行性与优越性适合作为科研实验或课程设计的参考实现。1. A*算法与城市无人机配送为什么它仍是路径规划的首选城市空中交通UAT场景下无人机包裹递送要在楼宇缝隙、临时禁飞区、移动障碍物之间完成快速转运这不是单纯的二维最短路径问题。A算法在栅格地图上通过启发式函数引导搜索方向既保留Dijkstra的最优性又把搜索范围收缩到目标附近的锥形区域。对科研人员和算法工程师来说MATLAB实现A的价值在于能快速验证代价函数、邻域扩展和动态重规划等关键环节不必纠结工程语言细节。我拆解了一个完整的MATLAB项目包含UAV.m、Min-Heap.m、main.m、Initialize.m、UAT.m、A-star.m、Graph.m这几个文件下面从原理到实跑逐层展开。这套代码适合做路径规划入门、算法对比也适合作为无人机航线设计的前期仿真原型。即使你已经用过ROS2或Python版的A*用MATLAB重写一遍也会对堆的操作和节点结构有更深理解。2. A*算法核心原理与MATLAB数据结构选型2.1 代价函数与启发式函数的设计A*的搜索核心是评估函数 f(n)g(n)h(n)。g(n)是从起点到当前节点 n 的实际飞行代价h(n)是从 n 到终点的估计代价。城市空中交通中g值通常拆成飞行距离、能量消耗和高度变化惩罚三部分h值必须满足可采纳性即不大于真实最小代价才能确保路径最优。无人机在空域内可以斜向飞行因此欧氏距离比曼哈顿距离更贴近实际运动学约束。2.1.1 启发式函数与代价计算的MATLAB表达在A-star.m中启发式函数单独抽出便于替换成对角距离或三维空间距离function h heuristic(node, goal) % 欧氏距离启发式适用于可斜向飞行的无人机 dx node.x - goal.x; dy node.y - goal.y; h sqrt(dx * dx dy * dy); end function g stepCost(parent, node, map, GSD) % 计算相邻节点间的移动代价 base GSD * sqrt((parent.x - node.x)^2 (parent.y - node.y)^2); g parent.g base * map.cost(node.x, node.y); end说明heuristic接收两个结构体节点返回浮点数。GSD是栅格每格代表的实际距离比如2米。stepCost中map.cost就是前面构造的通行系数矩阵Inf区域会被A*自然排除。这里用parent.g加上单步代价即使采用八邻域也能保证g值累积正确。参数方面node和goal都必须有x和y字段map.cost是二维矩阵下标不要越界。如果地图中有非结构化障碍物map.cost需要先用插值或膨胀处理否则路径会贴着障碍物边缘实际飞行时容易碰撞。2.2 开放列表与Min-Heap优先级队列A*每次从开放列表取出f值最小的节点。如果每次线性扫描整个数组在300x300栅格地图上的复杂度会变成O(n^2)一次仿真就要数秒。Min-Heap采用完全二叉树结构插入和弹出最小值都是O(log n)。MATLAB的标准库里没有泛型堆因此项目中的Min-Heap.m是整个算法的节奏骨架写得好不好直接影响重规划性能。2.2.1 Min-Heap的MATLAB实现要点下面是最小堆的核心代码用handle类封装避免反复值复制classdef MinHeap handle properties heap [] % 堆数组每个元素是带f字段的节点结构体 end methods function insert(obj, node) obj.heap(end1) node; obj.siftUp(numel(obj.heap)); end function node popMin(obj) if isempty(obj.heap) node []; return; end node obj.heap(1); obj.heap(1) obj.heap(end); obj.heap(end) []; if ~isempty(obj.heap) obj.siftDown(1); end end function siftUp(obj, idx) while idx 1 parent floor(idx/2); if obj.heap(idx).f obj.heap(parent).f [obj.heap(idx), obj.heap(parent)] deal(obj.heap(parent), obj.heap(idx)); idx parent; else break; end end end function siftDown(obj, idx) n numel(obj.heap); while true left 2*idx; right 2*idx1; smallest idx; if left n obj.heap(left).f obj.heap(smallest).f smallest left; end if right n obj.heap(right).f obj.heap(smallest).f smallest right; end if smallest ~ idx [obj.heap(idx), obj.heap(smallest)] deal(obj.heap(smallest), obj.heap(idx)); idx smallest; else break; end end end end end逻辑说明popMin先把堆顶f最小暂存再把末尾元素提到堆顶并下沉siftUp在insert后恢复堆序。整个实现约40行比借用Python的heapq库更直观。节点结构体里的f字段必须先赋值否则比较时会报错。参数方面heap数组中的每个节点结构体必须包含f字段如果要回溯路径还要包含parent字段。注意MATLAB的handle类对象是引用语义所以不要在不同地图间复用同一个MinHeap实例否则状态会串。2.3 邻域扩展与移动代价表UAT场景下无人机一般不会沿着小角度飞行八邻域扩展是合理折中。如果飞行器需要更细的角度可以改用十六邻域但代价函数也要相应修改。下面是八邻域移动的偏移参数移动类型x偏移y偏移相对距离代价正向±101.0垂向0±11.0对角±1±11.414在expandNode函数里我一般这样生成邻居节点function neighbors expandNode(current, map, goal) dirs [-1 -1; -1 0; -1 1; 0 -1; 0 1; 1 -1; 1 0; 1 1]; neighbors []; for i 1:size(dirs, 1) nx current.x dirs(i, 1); ny current.y dirs(i, 2); if nx 1 || ny 1 || nx map.nx || ny map.ny continue; end if map.cost(nx, ny) Inf continue; end nb struct(x, nx, y, ny); stepDist sqrt(dirs(i,1)^2 dirs(i,2)^2) * map.GSD; nb.g current.g stepDist * map.cost(nx, ny); nb.h heuristic(nb, goal); nb.f nb.g nb.h; nb.parent current; neighbors(end1) nb; %#okAGROW end end逻辑说明通过对角线和正方向的stepDist区分开就相当于把表格里的代价打进g值。map.cost(nx, ny)在禁飞区为Inf所以那些节点根本不会被加进neighbors相当于硬约束。#okAGROW是消除循环内数组增长的警告。参数方面current和goal是结构体一定要有x、y字段map.nx/map.ny是地图边界map.GSD是栅格分辨率。实际项目中如果地图很大可以把dirs定义成常量矩阵而不是放在函数里重复创建能省一点时间。3. UAT场景下的地图构建与动态路径调整3.1 从Initialize.m到Graph.mUAT地图的栅格化Initialize.m的作用是把城市环境转成A*可计算的栅格图Graph.m则把栅格图包装成带查询接口的图对象。UAT.m是场景入口它调用Initialize生成城市环境并读取UAV.m中定义的无人机属性转弯半径、感知距离为每次任务生成独立的起点终点列表。Graph.m负责把栅格地图包装成图对象供A-star.m查询邻接关系。在城市空中交通里障碍物包括不透空建筑物、禁飞区、临时气象风险区甚至还有高层建筑上的风切变区域。Initialize.m通常读入一张PNG或栅格图像像素值为0表示可通行255表示障碍物再膨胀一圈用于建模无人机安全半径。3.1.1 初始化地图的常用代码下面是我常用的一种初始化方式直接绘制模拟城市块function map Initialize() map.nx 200; map.ny 200; map.GSD 2; % 每格2米 map.cost ones(map.nx, map.ny); % 画出几栋建筑物和禁飞区 map.cost(20:40, 30:45) Inf; map.cost(60:80, 70:90) Inf; map.cost(100:110, 100:120) Inf; % 建筑周边设置1.2倍通行代价模拟气流扰动 map.cost(18:42, 28:47) 1.2; map.cost(58:82, 68:92) 1.2; end说明先把整张图设为畅通再写入障碍物最后给建筑周边加软代价。这里的1.2不是必须的但它能让A*自动避开建筑物附近的湍流区域即使没有硬性阻挡也会更倾向走开阔空域。注意这里是用矩阵下标表示坐标所以x对应行、y对应列。参数方面map结构体里的nx、ny是行列维数GSD决定实际距离cost矩阵的每个元素表示该格通过难度。用Inf表示不可通行用大于1的有限值表示缓堵。如果你要导入真实地图可以用imread读入灰度图再做二值化和膨胀。3.2 代价函数设定能耗、禁飞区与转弯惩罚前面的stepCost只考虑了距离和通行系数实际城市空中交通还需要把垂直升降和转弯考虑进去。比如无人机从停机坪起飞后需要迅速爬升这个阶段的能耗是平飞的2倍左右。所以在重规划场景里我会在代价函数中叠加一个高度变化惩罚。代价分量表达式说明直线飞行GSD * 欧氏距离基础能耗高度变化K * abs(dz)K3 时爬升惩罚显著转弯惩罚K_yaw * turnAngle避免频繁大转角禁飞区Inf硬约束3.2.1 在MATLAB中实现多维代价如果无人机在高程地图上飞行把二维节点扩展为(x,y,height)nb.g current.g GSD * stepDist 3.0 * abs(nb.z - current.z); nb.g nb.g 0.2 * turningAngle(current, nb, parentOfCurrent);说明turningAngle需要根据当前节点、父节点和邻居节点计算前序航向与当前航向的夹角。这个值在密集城区中很容易超过45度加入惩罚后A会更倾向走直路避免生成抖动的折线。参数方面3.0和0.2是要调的权重不同飞行平台差异很大。我给小四旋翼比如DJI Mavic级别配的权重是高度2.0、转弯0.15给固定翼配的是高度0.5、转弯1.0因为固定翼不能急转弯需要根据实际飞机特性调整。如果你倾向对比车辆路径规划里的混合A会发现混合A用连续状态空间和RS曲线处理运动学约束而这里A仍然是在离散栅格上工作但加入转弯惩罚后路径平滑度已经接近车辆级的规划结果。3.3 动态路径调整局部重规划与避障城市空中交通最大的变量是临时空域管制和突然出现的其他飞行器。A不是感知算法所以它适合做反应式规划器的决策内核当新障碍物出现在前方时把对应栅格标记为Inf然后从当前节点重新运行A。但全局重规划在200x200地图上约需几十毫秒如果每秒钟都要重算必须限制重规划窗口。下面的rePlanAroundObstacle中viewRange可以从UAV.m中定义的无人机感知距离读取。function newPath rePlanAroundObstacle(map, currentPos, goal, viewRange) localMap map; % 只把当前无人机感知范围内的障碍物写入localMap xmin max(1, currentPos.x - viewRange); xmax min(map.nx, currentPos.x viewRange); ymin max(1, currentPos.y - viewRange); ymax min(map.ny, currentPos.y viewRange); % 假设senseObstacle返回检测到的障碍物坐标 obstacles senseObstacle(); for i 1:size(obstacles, 1) if obstacles(i,1) xmin obstacles(i,1) xmax ... obstacles(i,2) ymin obstacles(i,2) ymax localMap.cost(obstacles(i,1), obstacles(i,2)) Inf; end end % 重新规划 path AStar(localMap, currentPos, goal); newPath smoothPath(path); end逻辑说明senseObstacle代表无人机感知模块返回的障碍物坐标列表这里只更新局部的代价图不破坏全局地图同时调用AStar返回新的路径点序列再用平滑函数处理。viewRange是感知半径城市高楼环境下一般是50到100米。参数方面xmin/xmax/ymin/ymax四个边界限定了更新的范围避免感知噪声污染远处区域。注意边界判断一定要用min/max否则在边界处会越界报错。smoothPath可以简单调用后面的样条插值函数。如果感知频率高还可以用上一轮AStar得到的路径作为先验信息只在局部窗口内重搜这就是增量式A*的思路理解MinHeap的键值更新后自己扩展起来并不难。4. 从main.m到A-star.m完整运行流程与验证4.1 main.m的执行流程main.m是整个项目的入口它按“初始化地图-创建起点终点-调用A*-绘制结果”的顺序执行。在我的测试机上运行一次完整仿真约0.3秒。start.g初始化为0h和cost由启发式函数计算goal.g不需要显式设置因为在AStar中只用到goal的x和y。% main.m 脚本 clear; clc; map Initialize(); goal.x 180; goal.y 180; goal.h 0; goal.g Inf; start.x 5; start.y 5; start.g 0; start.h heuristic(start, goal); start.f start.h; start.parent []; path AStar(map, start, goal); if isempty(path) error(No path found); else plotMap(map, path); disp([Path length: , num2str(length(path))]); end说明goal必须在start之前定义因为heuristic需要传入goal对象。start.g初始化为0h和cost由启发式函数计算。AStar返回的path是一个节点结构体数组通过parent指针回溯得到。参数方面start和goal的x、y必须在map.nx/map.ny范围内如果地图太大可以增大GSD但路径精度会下降。4.1.1 初始化中的常见错误初始化起点目标时很多人会忘了给start.h赋值导致f值为空或0堆排序失效。另一个常见错误是将goal放在障碍物内部这种情况下AStar会返回空数组然后main.m会报错。我在调试时通常加一个断言assert(map.cost(start.x, start.y) ~ Inf, start in obstacle); assert(map.cost(goal.x, goal.y) ~ Inf, goal in obstacle);4.2 A-star.m的核心逻辑AStar函数使用MinHeap管理开放集除了2.2节的堆操作这里需要注意节点状态管理。标准A*要求避免重复扩展同一节点所以closedMap用于快速跳过。下面这段代码是AStar的骨架function path AStar(map, start, goal) openList MinHeap(); openList.insert(start); closedMap false(map.nx, map.ny); while ~isempty(openList.heap) current openList.popMin(); if current.x goal.x current.y goal.y path constructPath(current); return; end if closedMap(current.x, current.y) continue; end closedMap(current.x, current.y) true; neighbors expandNode(current, map, goal); for i 1:length(neighbors) nb neighbors(i); if ~closedMap(nb.x, nb.y) openList.insert(nb); end end end path []; end逻辑说明closedMap用二维逻辑矩阵而不是list因为判断一个节点是否被扩展只需要O(1)时间。在重规划场景中每次调用AStar都会新建closedMap避免全局变量污染。expandNode里已经跳过了障碍物所以这里不再重复检查。参数方面openList.heap是MinHeap的公开属性while条件判断堆是否为空constructPath从目标节点的parent链回溯到起点返回按顺序排列的路径点。需要特别注意的是在expandNode中生成的新节点其parent直接指向current结构体而current本身又携带了链式祖先。由于MATLAB的结构体是值类型这不会导致内存泄漏但如果在重规划过程中把current的parent改了之前插入openList的邻居节点也会一起变化所以最好在expandNode传入current后再复制一份。4.2.1 路径回溯constructPathconstructPath的实现很直接function path constructPath(node) path node; while ~isempty(node.parent) node node.parent; path [node; path]; %#okAGROW end end说明从目标节点沿parent链回溯到起点最终得到从起点到目标的有序数组。由于使用了提前拼接复杂度为O(k^2)k是路径长度在k小于几百时问题不大。如果追求性能可以改成先收集再flip。4.3 运行结果与性能对比我在20x20、50x50、100x100三张栅格地图上对比了A*和不带启发式的Dijkstra把heuristic恒置0记录扩展节点数和运行时间。以下是一组代表性数据地图大小算法扩展节点数运行时间(ms)路径代价20x20A*871238.620x20Dijkstra2143138.650x50A*35646102.450x50Dijkstra1048129102.4100x100A*1092143210.9100x100Dijkstra4128561210.9数据在Windows 11、MATLAB R2023b、i5-1240P上跑出只代表该实现的大致量级。A*扩展节点数明显少于Dijkstra运行时间也更快但路径代价相同说明启发式没有牺牲最优性。如果你得到的路径代价与Dijkstra不同优先检查h是否大于真实代价或者移动代价表里对角距离填错。如果你的机器上时间差异更大通常是堆实现不够优化比如popMin里没有使用索引而是用了delete函数或者堆的siftDown循环中频繁调用struct比较可以对照2.2节代码检查。5. 进阶技巧Min-Heap键值更新与路径平滑5.1 处理节点重复插入时的键值更新标准A在发现某节点已经位于openList中但新的f值更小时需要执行decrease-key操作。2.2节的MinHeap只实现了insert和popMin如果重复插入同一节点堆里会有多个副本虽然A仍可运行但扩展节点数会增加且closedMap会忽略后弹出的陈旧副本。常见做法是在MinHeap中增加一个索引数组保存每个节点在堆中的位置。当节点的f值被降低时直接在该位置调用siftUp完成decrease-key。这个技巧对地图中障碍物密集、路径反复重规划的场景很有效。function updateNode(obj, node, newF) idx obj.index(node.x, node.y); if idx 0 obj.heap(idx).f newF; obj.siftUp(idx); end end说明index是一个二维矩阵存储每个坐标在堆中的下标不存在时存0。这样重规划时不需要重建堆避免每次更新局部地图后全量排序。5.2 路径平滑与飞行可行性验证A输出的折线在固定翼无人机上不可直接执行因为转角太急。我会用三次样条插值平滑路径再检查平滑后的路径是否离障碍物太近。如果某段样条侵入障碍物膨胀层就对这个航段重新跑A并限制搜索角度形成迭代式平滑。下面是一段示意代码pp csape([path.x; path.y], variational); t linspace(1, numel(path), 200); smoothed fnval(pp, t);说明csape生成的样条会穿过所有路径点但可能在转角处产生过度摆动所以需要检查每个插值点与最近障碍物的距离小于安全半径就加大该处的平滑权重或插入新的航路点。参数方面t的密度决定插值点数一般取路径点数的5到10倍。平滑后的路径不能直接作为命令序列发给飞控还需要用速度规划生成带时间戳的航点。如果你在做视觉感知避障可以把平滑后的航路点通过MAVLink发给PX4或ArduPilot的offboard模式做位置控制。本文还有配套的精品资源点击获取