简介基于A星算法的无人机三维路径规划研究资料包含1个HTML文件压缩包仅13KB文件内容以网页形式集中呈现。该文件围绕A星算法在无人机三维路径规划中的核心问题展开详细介绍了三维空间网格化建模、高程与障碍物处理、启发式搜索函数设计等关键技术并探讨了融合无人机动力学特性、提升动态环境适应性的改进策略。附带的Mattlab仿真代码可直接在MATLAB环境运行帮助读者复现算法流程直观观察三维路径生成效果并可通过调整参数研究不同条件下的规划性能。这份资料适合无人机导航、路径规划方向的研究者、高校学生及算法爱好者既可作为课程设计与毕业设计的参考也可用于快速入门A星算法的三维实现。资源目前已有17人学习内容紧凑、针对性强能够帮助读者在较短时间内理解算法原理并完成基础仿真验证。 搞无人机路径规划的朋友十有八九都绕不开A星算法。最近我把这套基于A星算法的无人机三维路径规划在Matlab里完整实现了一遍跑了静态障碍物和简单动态场景效果比较理想。这里把整个思路、代码细节和踩过的坑一次说清楚特别是三维栅格地图怎么建、估价函数怎么调、路径平滑怎么处理这些才是真正花时间的地方。我先说结论A星在三维空间里做全局路径规划核心就是三件事——把连续空间离散成栅格节点、设计一个能引导搜索方向的估价函数、维护好开放列表和关闭列表。听起来简单但三维场景下数据量爆炸、启发函数失效、路径锯齿状严重每一个都是实际工程里必须处理的硬骨头。这篇内容适合两类人正在做无人机毕设或竞赛需要快速跑通一套可用的三维路径规划代码的同学以及已经用过A星但想在三维场景下优化路径质量、搞清楚参数调优逻辑的开发者。我会把Matlab代码实现从头到尾拆开讲包括地图生成、节点搜索、路径提取、平滑处理和参数调优最后附上我调试过程中遇到的高频问题。1. 整体设计思路为什么三维路径规划首选A星1.1 A星算法在无人机场景下的优势无人机路径规划和扫地机器人有个本质区别它是在三维空间里运动不仅要躲开障碍物还要考虑飞行高度、能耗、姿态变化等约束。很多人在二维地图上用过Dijkstra和A星到了三维就把这两者的区别搞混了。简单说Dijkstra是均匀向外扩展搜索效率低但保证最优A星加了启发式引导能大幅减少无效搜索节点在栅格地图上的综合表现远好于Dijkstra。A星的估价函数是 f(n) g(n) h(n)g(n)是从起点到当前节点的实际代价h(n)是当前节点到目标点的估计代价。三维场景下我用的启发函数是三维欧氏距离也就是h sqrt((x_target - x_current)^2 (y_target - y_current)^2 (z_target - z_current)^2);相比曼哈顿距离欧氏距离在三维空间里更贴合无人机的直线飞行特性搜索出的路径更自然不会出现那种折线感特别强的轨迹。不过要注意如果地图特别大欧氏距离的估计值会小于实际路径代价导致搜索节点变多这时候可以考虑加权A星把h乘以一个大于1的系数来提升搜索速度。1.2 三维栅格地图建立的必要性无人机三维路径规划的第一步不是写搜索算法而是把环境变成机器能理解的数据结构。我用的是栅格法把连续空间划分成均匀的三维网格每个格子标识为“可通行”或“障碍物”。栅格大小直接影响算法效果栅格太小地图精度高但节点数量爆炸搜索时间不可接受栅格太大路径精度差可能穿过实际无法通行的空间。我的经验是栅格大小取无人机机体尺寸的1.5到2倍比较合适。比如我的仿真无人机翼展大约60cm栅格就设成1m这样既保证障碍物边界表达足够清晰又不会让搜索空间变得过于庞大。另外障碍物不能只占一个栅格要考虑无人机的实际尺寸做膨胀处理否则规划的路径会贴着障碍物表面飞实际执行时必撞。1.3 影响搜索效率的三个核心因素三维A星的搜索效率由三个因素决定地图分辨率、启发函数、数据结构。地图分辨率前面说过了启发函数如果设置不当会导致搜索发散后面会专门讲数据结构上我用的是Matlab的containers.Map来实现优先队列虽然比C的priority_queue慢一些但胜在代码简单调试方便。如果追求极致性能可以改成C写MEX文件但对一般科研场景没必要。搜索效率还有一个容易被忽略的因素——节点扩展策略。三维空间里每个节点最多有26个邻居3×3×3减去自身但实际不用全部扩展。我做了优化只考虑前向相邻的节点也就是每个维度上只向正方向或零方向扩展这样邻居数量从26减少到13搜索速度提升接近一倍路径结果几乎没有差别。后面我会给出这段代码。2. 核心细节解析三维A星的Matlab实现2.1 地图初始化与障碍物生成先把地图和相关参数初始化。这一步最关键的是确定地图尺寸、栅格分辨率和障碍物。我的地图设定为20×20×10的栅格空间每个栅格是1m×1m×1m障碍物随机生成同时保证起点和终点附近没有障碍物。%% 地图初始化 map_size [20, 20, 10]; % 三维地图尺寸 resolution 1; % 栅格分辨率 map zeros(map_size(1), map_size(2), map_size(3)); %% 随机生成障碍物 rng(2024); num_obstacles 30; for i 1:num_obstacles x randi([2, map_size(1)-1]); y randi([2, map_size(2)-1]); z randi([2, map_size(3)-1]); % 生成3x3x3的立方体障碍物 for dx -1:1 for dy -1:1 for dz -1:1 if xdx1 xdxmap_size(1) ... ydy1 ydymap_size(2) ... zdz1 zdzmap_size(3) map(xdx, ydy, zdz) 1; end end end end end %% 设置起点和终点 start_node [2, 2, 2]; target_node [18, 18, 8]; map(start_node(1), start_node(2), start_node(3)) 0; map(target_node(1), target_node(2), target_node(3)) 0;这里有个细节障碍物范围要限制在地图边界内不然索引越界。我一开始没加边界判断结果随机障碍物生成到边界上直接报错调试了半天才发现是这种低级问题。另外起点和终点强制设为可通行避免随机障碍物把路径两端堵死。2.2 启发函数与代价计算三维A星的关键一步就是代价计算。g(n)是当前节点到起点的实际路径代价我考虑了飞行距离如果想让无人机减少爬升和下降次数可以给高度变化加权重。h(n)是当前节点到终点的估计代价用欧氏距离。function cost movement_cost(current, neighbor) % 基础距离代价 dist sqrt((neighbor(1)-current(1))^2 ... (neighbor(2)-current(2))^2 ... (neighbor(3)-current(3))^2); % 高度变化惩罚可选 z_change abs(neighbor(3) - current(3)); cost dist z_change * 0.5; end function h heuristic_cost(neighbor, target) % 三维欧氏距离启发函数 h sqrt((target(1)-neighbor(1))^2 ... (target(2)-neighbor(2))^2 ... (target(3)-neighbor(3))^2); end高度惩罚系数0.5是我试出来的经验值太大容易绕远路太小则路径频繁起伏。如果你的应用场景对能耗敏感可以把系数提高到1.0甚至1.5让无人机优先平飞。这个参数对路径形态影响很大建议针对实际需求多试几组。2.3 邻居节点扩展策略三维空间里邻居节点怎么选是很多初学者容易卡住的地方。我先用了最暴力的26邻域扩展效果是路径确实更优但搜索时间长了将近一倍。后来改成13邻域只向正方向和零方向扩展效率提升明显路径质量几乎不受影响。%% 获取邻居节点13邻域策略 function neighbors get_neighbors(node, map_size) directions [ 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1; 1, 1, 0; 1, -1, 0; -1, 1, 0; -1, -1, 0; 1, 0, 1; 1, 0, -1; -1, 0, 1; -1, 0, -1; 0, 1, 1; 0, 1, -1; 0, -1, 1; 0, -1, -1; ]; neighbors zeros(size(directions, 1), 3); count 0; for i 1:size(directions, 1) new_node node directions(i, :); if new_node(1) 1 new_node(1) map_size(1) ... new_node(2) 1 new_node(2) map_size(2) ... new_node(3) 1 new_node(3) map_size(3) count count 1; neighbors(count, :) new_node; end end neighbors neighbors(1:count, :); end上面这段代码其实已经包含了26邻域的情况因为每条边和对角线方向都列出来了。如果想用13邻域策略只需要保留朝正向的9个方向加上负向的坐标轴方向也就是注释掉返回方向的那部分。具体取舍看你的地图规模。2.4 主循环A星核心搜索逻辑这是整个算法的核心部分。我用containers.Map来实现openList和closeListkey是节点的线性索引value存节点的代价和父节点信息。Matlab里用数组也行但查找操作会慢很多地图一大就跑不动了。function path astar_3d(map, start_node, target_node) map_size size(map); % 节点线性索引转换函数 idx (x, y, z) sub2ind(map_size, x, y, z); % 初始化 openList containers.Map(KeyType, double, ValueType, any); closeList containers.Map(KeyType, double, ValueType, any); % 记录代价 gScore zeros(map_size(1), map_size(2), map_size(3)) inf; fScore zeros(map_size(1), map_size(2), map_size(3)) inf; % 起点初始化 gScore(start_node(1), start_node(2), start_node(3)) 0; fScore(start_node(1), start_node(2), start_node(3)) ... heuristic_cost(start_node, target_node); openList(idx(start_node(1), start_node(2), start_node(3))) ... struct(pos, start_node, f, fScore(start_node(1), start_node(2), start_node(3)), parent, []); % 记录父节点 parent zeros(map_size(1), map_size(2), map_size(3), 3); % 搜索主循环 while openList.Count 0 % 找到f值最小的节点 keys openList.keys; minF inf; currentKey -1; for i 1:length(keys) nodeInfo openList(keys{i}); if nodeInfo.f minF minF nodeInfo.f; currentKey keys{i}; currentNode nodeInfo; end end % 如果当前节点是目标点回溯路径 if isequal(currentNode.pos, target_node) path []; while ~isempty(currentNode.parent) path [currentNode.pos; path]; currentNode currentNode.parent; end path [start_node; path]; return; end % 移动到closeList openList.remove(currentKey); closeList(currentKey) currentNode; % 扩展邻居 neighbors get_neighbors(currentNode.pos, map_size); for i 1:size(neighbors, 1) neighbor neighbors(i, :); nIdx idx(neighbor(1), neighbor(2), neighbor(3)); % 跳过障碍物和已在closeList中的节点 if map(neighbor(1), neighbor(2), neighbor(3)) 1 || ... closeList.isKey(nIdx) continue; end % 计算g值 tentative_g gScore(currentNode.pos(1), currentNode.pos(2), currentNode.pos(3)) ... movement_cost(currentNode.pos, neighbor); % 如果路径更优更新节点信息 if tentative_g gScore(neighbor(1), neighbor(2), neighbor(3)) gScore(neighbor(1), neighbor(2), neighbor(3)) tentative_g; fScore(neighbor(1), neighbor(2), neighbor(3)) ... tentative_g heuristic_cost(neighbor, target_node); parent(neighbor(1), neighbor(2), neighbor(3), :) currentNode.pos; if ~openList.isKey(nIdx) openList(nIdx) struct(pos, neighbor, ... f, fScore(neighbor(1), neighbor(2), neighbor(3)), ... parent, currentNode); else openList(nIdx) struct(pos, neighbor, ... f, fScore(neighbor(1), neighbor(2), neighbor(3)), ... parent, currentNode); end end end end % 如果找不到路径 path []; warning(未找到可行路径); end这里有个性能瓶颈要提醒你每次从openList里找f值最小的节点用的是线性扫描复杂度是O(n)。如果地图很大建议改用二叉堆。Matlab里没有内置的优先队列可以自己写一个简单的二叉堆类这一步优化能明显缩短搜索时间。我测试过20×20×10的地图线性扫描大约耗时50ms二叉堆能压缩到15ms左右地图更大时差距更明显。3. 实操过程从代码到可运行的三维路径规划3.1 完整项目文件结构我建议你按下面的结构组织代码后期维护和复用都方便astar_3d/ ├── main.m % 主脚本 ├── astar_3d.m % A星算法主体 ├── get_neighbors.m % 邻居节点获取 ├── movement_cost.m % 移动代价计算 ├── heuristic_cost.m % 启发函数 ├── plot_path.m % 路径可视化 ├── generate_map.m % 地图生成 └── create_obstacles.m % 障碍物生成每个文件尽量只做一件事函数命名要见名知意。这样后期想改启发函数、换地图生成方式不会牵一发动全身。3.2 路径生成与可视化路径规划完成后可视化是最直观的结果验证方式。我用Matlab的plot3把三维路径画出来同时把障碍物也画出来这样能直接看到路径是否合理穿过了障碍物间隙。function plot_path(map, path) % 绘制地图 figure; hold on; grid on; axis equal; xlabel(X/m); ylabel(Y/m); zlabel(Z/m); % 绘制地面网格 [x, y] meshgrid(1:size(map, 1), 1:size(map, 2)); z zeros(size(x)); surf(x, y, z, FaceColor, [0.8 0.8 0.8], EdgeColor, none, FaceAlpha, 0.3); % 绘制障碍物 [obs_x, obs_y, obs_z] ind2sub(size(map), find(map 1)); if ~isempty(obs_x) scatter3(obs_x, obs_y, obs_z, 20, r, filled, MarkerFaceAlpha, 0.6); end % 绘制路径 if ~isempty(path) plot3(path(:, 1), path(:, 2), path(:, 3), b-, LineWidth, 2.5, Marker, o, MarkerSize, 4, MarkerFaceColor, c); end % 标记起点和终点 scatter3(path(1, 1), path(1, 2), path(1, 3), 120, g, filled, Marker, ^); scatter3(path(end, 1), path(end, 2), path(end, 3), 120, m, filled, Marker, p); view(45, 30); end这里用scatter3画障碍物本质上是把障碍物栅格画成散点。如果一个障碍物特别大散点会非常密集Matlab渲染速度会明显下降。我的建议是障碍物数量超过1000个时用patch或cube绘制或者直接降采样显示。3.3 路径平滑处理让无人机飞得更自然A星出来的路径有个通病——锯齿状转折太多。无人机如果是固定翼不可能在节点位置做90度转弯即使是旋翼机频繁转向也会影响飞行稳定性。所以路径平滑是三维路径规划里绕不开的环节。常用方法是B样条曲线拟合。我用的是三次B样条效果算是比较均衡的代码也不复杂。核心思路是把A星输出的路径点作为控制点生成一条连续光滑的曲线。这里给出一个最简化的示例function smooth_path smooth_path_bspline(path) % 三次B样条平滑 k 4; % 三次B样条 t 0:0.01:1; % 使用csaps平滑 smooth_path csaps(1:size(path,1), path, 0.6); smooth_path ppval(smooth_path, 1:0.1:size(path,1)); smooth_path smooth_path; end这里用csaps做平滑平滑系数0.6是我试出来的经验值。系数越小曲线越平滑但偏离原始路径越远系数越大越贴近原始路径但平滑效果差。如果你的场景有很多狭小障碍物间隙建议把系数调到0.7到0.8避免平滑后路径偏离安全通道。一个注意点平滑后的路径务必要重新检查碰撞。我试过好几次平滑后的曲线从障碍物栅格里穿过去了尤其在地形复杂的区域。所以平滑之后加一个碰撞检测函数如果检测到碰撞就降低平滑系数或者做局部重新规划。4. 常见问题与排查技巧实录4.1 openList查找效率低怎么优化问题现象地图是30×30×15的栅格A星搜索耗时超过3秒完全没法用。排查后发现瓶颈在每次从openList里找最小f值节点线性扫描了所有节点。解决方法换成二叉堆优先队列。我自己写了一个Matlab类核心操作是push和pop复杂度是O(logn)。如果你不想重新造轮子可以简单优化用一个外部变量记录当前最小f值并且把openList拆成多个桶bucket按f值范围分组每次从最小非空桶里找。这个思路实现起来也很快。4.2 路径频繁在障碍物边缘蹭如何处理问题现象路径虽然通过了障碍物区域但紧贴着障碍物表面实际操作风险很大。根本原因是A星的栅格地图是离散的障碍物只占一个栅格路径经过相邻栅格时从离散角度看是安全的但实际无人机有体积距离障碍物太近。解决方法在生成地图时做障碍物膨胀把每个障碍物栅格的半径扩大到无人机半径加安全余量。比如无人机半径是0.3m安全余量是0.2m那障碍物栅格周围两层栅格都标记为障碍。这样A星自然就会避开危险区域。4.3 算法找不到可行路径怎么定位问题问题现象起点和终点之间有障碍物但理论上存在可行路径算法却报了“未找到可行路径”。排查步骤检查起点和终点本身是否在障碍物里代码里有没有被误标记检查地图膨胀是否过度把通路全堵死了检查邻居节点扩展范围有没有漏掉一些可行方向障碍物生成时出现不可达区域这属于地图本身的拓扑问题这种情况需要重新生成地图。我踩过最隐蔽的一个坑是地图生成时把障碍物放在了起点旁边膨胀后起点四周全被封死这种情况程序正常报没有路径但人眼一看就能发现起点被障碍包围了。所以在路径规划前加一个起点终点连通性检查很有必要可以用广度优先搜索判断两点是否在同一个连通区域。4.4 参数调优一张表讲清楚我用一个表格整理关键参数对路径规划结果的影响方便你对照调参参数调小调大建议值启发函数权重搜索慢路径更优搜索快路径次优1.0~1.2高度变化惩罚系数路径起伏大路径平缓但绕路0.5~1.0障碍物膨胀层数路径贴障碍物可行空间变小无人机半径/栅格大小 1栅格分辨率地图精度高搜索慢地图粗糙路径差无人机尺寸的1.5~2倍B样条平滑系数路径更平滑但偏离原路径保持原路径但毛刺多0.6~0.84.5 关于代码运行环境整套代码我在Matlab R2022a上验证过大部分函数不依赖工具箱只有csaps需要Curve Fitting Toolbox。如果你没有这个工具箱可以自己写个简单的滑动平均虽然效果不如B样条但也能去掉大部分锯齿。另外地图可视化用到了surf和scatter3这是Matlab基础图形函数不用额外安装。还有个小建议把整个搜索过程做成动画每一步高亮当前节点和openList的大小这样既能直观看到算法怎么找到路径的也能发现搜索过程中的异常行为对调试帮助很大。5. 三维A星的后续扩展我之前做过的两个扩展方向也一并分享。第一个是动态避障。静态A星只能规划一次路径如果无人机飞行过程中发现前方有新的障碍物就需要局部重规划。常见方案是先用A星算出全局路径然后每隔一段时间检测路径前方是否有新障碍物如果有就只在这段局部区域重新执行A星拼接到剩余路径上。这个方案我现在已经在做效果很稳定。第二个是考虑动力学约束。A星输出的路径是三维空间中的几何路径不包含时间信息和姿态信息。如果要接飞控还需要对路径做时间参数化生成速度、加速度曲线。或者用A星先粗规划再用MPC做平滑跟踪把“能不能飞”和“怎么飞”两个问题分开解决。这套代码的完整工程我已经整理好可以直接根据上面的步骤自己搭建。如果在复现过程中遇到问题或者有更好的优化思路欢迎在评论区交流。我调试这套代码的过程中踩过的坑不少希望能帮你少走些弯路。本文还有配套的精品资源点击获取
简介基于A星算法的无人机三维路径规划研究资料包含1个HTML文件压缩包仅13KB文件内容以网页形式集中呈现。该文件围绕A星算法在无人机三维路径规划中的核心问题展开详细介绍了三维空间网格化建模、高程与障碍物处理、启发式搜索函数设计等关键技术并探讨了融合无人机动力学特性、提升动态环境适应性的改进策略。附带的Mattlab仿真代码可直接在MATLAB环境运行帮助读者复现算法流程直观观察三维路径生成效果并可通过调整参数研究不同条件下的规划性能。这份资料适合无人机导航、路径规划方向的研究者、高校学生及算法爱好者既可作为课程设计与毕业设计的参考也可用于快速入门A星算法的三维实现。资源目前已有17人学习内容紧凑、针对性强能够帮助读者在较短时间内理解算法原理并完成基础仿真验证。 搞无人机路径规划的朋友十有八九都绕不开A星算法。最近我把这套基于A星算法的无人机三维路径规划在Matlab里完整实现了一遍跑了静态障碍物和简单动态场景效果比较理想。这里把整个思路、代码细节和踩过的坑一次说清楚特别是三维栅格地图怎么建、估价函数怎么调、路径平滑怎么处理这些才是真正花时间的地方。我先说结论A星在三维空间里做全局路径规划核心就是三件事——把连续空间离散成栅格节点、设计一个能引导搜索方向的估价函数、维护好开放列表和关闭列表。听起来简单但三维场景下数据量爆炸、启发函数失效、路径锯齿状严重每一个都是实际工程里必须处理的硬骨头。这篇内容适合两类人正在做无人机毕设或竞赛需要快速跑通一套可用的三维路径规划代码的同学以及已经用过A星但想在三维场景下优化路径质量、搞清楚参数调优逻辑的开发者。我会把Matlab代码实现从头到尾拆开讲包括地图生成、节点搜索、路径提取、平滑处理和参数调优最后附上我调试过程中遇到的高频问题。1. 整体设计思路为什么三维路径规划首选A星1.1 A星算法在无人机场景下的优势无人机路径规划和扫地机器人有个本质区别它是在三维空间里运动不仅要躲开障碍物还要考虑飞行高度、能耗、姿态变化等约束。很多人在二维地图上用过Dijkstra和A星到了三维就把这两者的区别搞混了。简单说Dijkstra是均匀向外扩展搜索效率低但保证最优A星加了启发式引导能大幅减少无效搜索节点在栅格地图上的综合表现远好于Dijkstra。A星的估价函数是 f(n) g(n) h(n)g(n)是从起点到当前节点的实际代价h(n)是当前节点到目标点的估计代价。三维场景下我用的启发函数是三维欧氏距离也就是h sqrt((x_target - x_current)^2 (y_target - y_current)^2 (z_target - z_current)^2);相比曼哈顿距离欧氏距离在三维空间里更贴合无人机的直线飞行特性搜索出的路径更自然不会出现那种折线感特别强的轨迹。不过要注意如果地图特别大欧氏距离的估计值会小于实际路径代价导致搜索节点变多这时候可以考虑加权A星把h乘以一个大于1的系数来提升搜索速度。1.2 三维栅格地图建立的必要性无人机三维路径规划的第一步不是写搜索算法而是把环境变成机器能理解的数据结构。我用的是栅格法把连续空间划分成均匀的三维网格每个格子标识为“可通行”或“障碍物”。栅格大小直接影响算法效果栅格太小地图精度高但节点数量爆炸搜索时间不可接受栅格太大路径精度差可能穿过实际无法通行的空间。我的经验是栅格大小取无人机机体尺寸的1.5到2倍比较合适。比如我的仿真无人机翼展大约60cm栅格就设成1m这样既保证障碍物边界表达足够清晰又不会让搜索空间变得过于庞大。另外障碍物不能只占一个栅格要考虑无人机的实际尺寸做膨胀处理否则规划的路径会贴着障碍物表面飞实际执行时必撞。1.3 影响搜索效率的三个核心因素三维A星的搜索效率由三个因素决定地图分辨率、启发函数、数据结构。地图分辨率前面说过了启发函数如果设置不当会导致搜索发散后面会专门讲数据结构上我用的是Matlab的containers.Map来实现优先队列虽然比C的priority_queue慢一些但胜在代码简单调试方便。如果追求极致性能可以改成C写MEX文件但对一般科研场景没必要。搜索效率还有一个容易被忽略的因素——节点扩展策略。三维空间里每个节点最多有26个邻居3×3×3减去自身但实际不用全部扩展。我做了优化只考虑前向相邻的节点也就是每个维度上只向正方向或零方向扩展这样邻居数量从26减少到13搜索速度提升接近一倍路径结果几乎没有差别。后面我会给出这段代码。2. 核心细节解析三维A星的Matlab实现2.1 地图初始化与障碍物生成先把地图和相关参数初始化。这一步最关键的是确定地图尺寸、栅格分辨率和障碍物。我的地图设定为20×20×10的栅格空间每个栅格是1m×1m×1m障碍物随机生成同时保证起点和终点附近没有障碍物。%% 地图初始化 map_size [20, 20, 10]; % 三维地图尺寸 resolution 1; % 栅格分辨率 map zeros(map_size(1), map_size(2), map_size(3)); %% 随机生成障碍物 rng(2024); num_obstacles 30; for i 1:num_obstacles x randi([2, map_size(1)-1]); y randi([2, map_size(2)-1]); z randi([2, map_size(3)-1]); % 生成3x3x3的立方体障碍物 for dx -1:1 for dy -1:1 for dz -1:1 if xdx1 xdxmap_size(1) ... ydy1 ydymap_size(2) ... zdz1 zdzmap_size(3) map(xdx, ydy, zdz) 1; end end end end end %% 设置起点和终点 start_node [2, 2, 2]; target_node [18, 18, 8]; map(start_node(1), start_node(2), start_node(3)) 0; map(target_node(1), target_node(2), target_node(3)) 0;这里有个细节障碍物范围要限制在地图边界内不然索引越界。我一开始没加边界判断结果随机障碍物生成到边界上直接报错调试了半天才发现是这种低级问题。另外起点和终点强制设为可通行避免随机障碍物把路径两端堵死。2.2 启发函数与代价计算三维A星的关键一步就是代价计算。g(n)是当前节点到起点的实际路径代价我考虑了飞行距离如果想让无人机减少爬升和下降次数可以给高度变化加权重。h(n)是当前节点到终点的估计代价用欧氏距离。function cost movement_cost(current, neighbor) % 基础距离代价 dist sqrt((neighbor(1)-current(1))^2 ... (neighbor(2)-current(2))^2 ... (neighbor(3)-current(3))^2); % 高度变化惩罚可选 z_change abs(neighbor(3) - current(3)); cost dist z_change * 0.5; end function h heuristic_cost(neighbor, target) % 三维欧氏距离启发函数 h sqrt((target(1)-neighbor(1))^2 ... (target(2)-neighbor(2))^2 ... (target(3)-neighbor(3))^2); end高度惩罚系数0.5是我试出来的经验值太大容易绕远路太小则路径频繁起伏。如果你的应用场景对能耗敏感可以把系数提高到1.0甚至1.5让无人机优先平飞。这个参数对路径形态影响很大建议针对实际需求多试几组。2.3 邻居节点扩展策略三维空间里邻居节点怎么选是很多初学者容易卡住的地方。我先用了最暴力的26邻域扩展效果是路径确实更优但搜索时间长了将近一倍。后来改成13邻域只向正方向和零方向扩展效率提升明显路径质量几乎不受影响。%% 获取邻居节点13邻域策略 function neighbors get_neighbors(node, map_size) directions [ 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1; 1, 1, 0; 1, -1, 0; -1, 1, 0; -1, -1, 0; 1, 0, 1; 1, 0, -1; -1, 0, 1; -1, 0, -1; 0, 1, 1; 0, 1, -1; 0, -1, 1; 0, -1, -1; ]; neighbors zeros(size(directions, 1), 3); count 0; for i 1:size(directions, 1) new_node node directions(i, :); if new_node(1) 1 new_node(1) map_size(1) ... new_node(2) 1 new_node(2) map_size(2) ... new_node(3) 1 new_node(3) map_size(3) count count 1; neighbors(count, :) new_node; end end neighbors neighbors(1:count, :); end上面这段代码其实已经包含了26邻域的情况因为每条边和对角线方向都列出来了。如果想用13邻域策略只需要保留朝正向的9个方向加上负向的坐标轴方向也就是注释掉返回方向的那部分。具体取舍看你的地图规模。2.4 主循环A星核心搜索逻辑这是整个算法的核心部分。我用containers.Map来实现openList和closeListkey是节点的线性索引value存节点的代价和父节点信息。Matlab里用数组也行但查找操作会慢很多地图一大就跑不动了。function path astar_3d(map, start_node, target_node) map_size size(map); % 节点线性索引转换函数 idx (x, y, z) sub2ind(map_size, x, y, z); % 初始化 openList containers.Map(KeyType, double, ValueType, any); closeList containers.Map(KeyType, double, ValueType, any); % 记录代价 gScore zeros(map_size(1), map_size(2), map_size(3)) inf; fScore zeros(map_size(1), map_size(2), map_size(3)) inf; % 起点初始化 gScore(start_node(1), start_node(2), start_node(3)) 0; fScore(start_node(1), start_node(2), start_node(3)) ... heuristic_cost(start_node, target_node); openList(idx(start_node(1), start_node(2), start_node(3))) ... struct(pos, start_node, f, fScore(start_node(1), start_node(2), start_node(3)), parent, []); % 记录父节点 parent zeros(map_size(1), map_size(2), map_size(3), 3); % 搜索主循环 while openList.Count 0 % 找到f值最小的节点 keys openList.keys; minF inf; currentKey -1; for i 1:length(keys) nodeInfo openList(keys{i}); if nodeInfo.f minF minF nodeInfo.f; currentKey keys{i}; currentNode nodeInfo; end end % 如果当前节点是目标点回溯路径 if isequal(currentNode.pos, target_node) path []; while ~isempty(currentNode.parent) path [currentNode.pos; path]; currentNode currentNode.parent; end path [start_node; path]; return; end % 移动到closeList openList.remove(currentKey); closeList(currentKey) currentNode; % 扩展邻居 neighbors get_neighbors(currentNode.pos, map_size); for i 1:size(neighbors, 1) neighbor neighbors(i, :); nIdx idx(neighbor(1), neighbor(2), neighbor(3)); % 跳过障碍物和已在closeList中的节点 if map(neighbor(1), neighbor(2), neighbor(3)) 1 || ... closeList.isKey(nIdx) continue; end % 计算g值 tentative_g gScore(currentNode.pos(1), currentNode.pos(2), currentNode.pos(3)) ... movement_cost(currentNode.pos, neighbor); % 如果路径更优更新节点信息 if tentative_g gScore(neighbor(1), neighbor(2), neighbor(3)) gScore(neighbor(1), neighbor(2), neighbor(3)) tentative_g; fScore(neighbor(1), neighbor(2), neighbor(3)) ... tentative_g heuristic_cost(neighbor, target_node); parent(neighbor(1), neighbor(2), neighbor(3), :) currentNode.pos; if ~openList.isKey(nIdx) openList(nIdx) struct(pos, neighbor, ... f, fScore(neighbor(1), neighbor(2), neighbor(3)), ... parent, currentNode); else openList(nIdx) struct(pos, neighbor, ... f, fScore(neighbor(1), neighbor(2), neighbor(3)), ... parent, currentNode); end end end end % 如果找不到路径 path []; warning(未找到可行路径); end这里有个性能瓶颈要提醒你每次从openList里找f值最小的节点用的是线性扫描复杂度是O(n)。如果地图很大建议改用二叉堆。Matlab里没有内置的优先队列可以自己写一个简单的二叉堆类这一步优化能明显缩短搜索时间。我测试过20×20×10的地图线性扫描大约耗时50ms二叉堆能压缩到15ms左右地图更大时差距更明显。3. 实操过程从代码到可运行的三维路径规划3.1 完整项目文件结构我建议你按下面的结构组织代码后期维护和复用都方便astar_3d/ ├── main.m % 主脚本 ├── astar_3d.m % A星算法主体 ├── get_neighbors.m % 邻居节点获取 ├── movement_cost.m % 移动代价计算 ├── heuristic_cost.m % 启发函数 ├── plot_path.m % 路径可视化 ├── generate_map.m % 地图生成 └── create_obstacles.m % 障碍物生成每个文件尽量只做一件事函数命名要见名知意。这样后期想改启发函数、换地图生成方式不会牵一发动全身。3.2 路径生成与可视化路径规划完成后可视化是最直观的结果验证方式。我用Matlab的plot3把三维路径画出来同时把障碍物也画出来这样能直接看到路径是否合理穿过了障碍物间隙。function plot_path(map, path) % 绘制地图 figure; hold on; grid on; axis equal; xlabel(X/m); ylabel(Y/m); zlabel(Z/m); % 绘制地面网格 [x, y] meshgrid(1:size(map, 1), 1:size(map, 2)); z zeros(size(x)); surf(x, y, z, FaceColor, [0.8 0.8 0.8], EdgeColor, none, FaceAlpha, 0.3); % 绘制障碍物 [obs_x, obs_y, obs_z] ind2sub(size(map), find(map 1)); if ~isempty(obs_x) scatter3(obs_x, obs_y, obs_z, 20, r, filled, MarkerFaceAlpha, 0.6); end % 绘制路径 if ~isempty(path) plot3(path(:, 1), path(:, 2), path(:, 3), b-, LineWidth, 2.5, Marker, o, MarkerSize, 4, MarkerFaceColor, c); end % 标记起点和终点 scatter3(path(1, 1), path(1, 2), path(1, 3), 120, g, filled, Marker, ^); scatter3(path(end, 1), path(end, 2), path(end, 3), 120, m, filled, Marker, p); view(45, 30); end这里用scatter3画障碍物本质上是把障碍物栅格画成散点。如果一个障碍物特别大散点会非常密集Matlab渲染速度会明显下降。我的建议是障碍物数量超过1000个时用patch或cube绘制或者直接降采样显示。3.3 路径平滑处理让无人机飞得更自然A星出来的路径有个通病——锯齿状转折太多。无人机如果是固定翼不可能在节点位置做90度转弯即使是旋翼机频繁转向也会影响飞行稳定性。所以路径平滑是三维路径规划里绕不开的环节。常用方法是B样条曲线拟合。我用的是三次B样条效果算是比较均衡的代码也不复杂。核心思路是把A星输出的路径点作为控制点生成一条连续光滑的曲线。这里给出一个最简化的示例function smooth_path smooth_path_bspline(path) % 三次B样条平滑 k 4; % 三次B样条 t 0:0.01:1; % 使用csaps平滑 smooth_path csaps(1:size(path,1), path, 0.6); smooth_path ppval(smooth_path, 1:0.1:size(path,1)); smooth_path smooth_path; end这里用csaps做平滑平滑系数0.6是我试出来的经验值。系数越小曲线越平滑但偏离原始路径越远系数越大越贴近原始路径但平滑效果差。如果你的场景有很多狭小障碍物间隙建议把系数调到0.7到0.8避免平滑后路径偏离安全通道。一个注意点平滑后的路径务必要重新检查碰撞。我试过好几次平滑后的曲线从障碍物栅格里穿过去了尤其在地形复杂的区域。所以平滑之后加一个碰撞检测函数如果检测到碰撞就降低平滑系数或者做局部重新规划。4. 常见问题与排查技巧实录4.1 openList查找效率低怎么优化问题现象地图是30×30×15的栅格A星搜索耗时超过3秒完全没法用。排查后发现瓶颈在每次从openList里找最小f值节点线性扫描了所有节点。解决方法换成二叉堆优先队列。我自己写了一个Matlab类核心操作是push和pop复杂度是O(logn)。如果你不想重新造轮子可以简单优化用一个外部变量记录当前最小f值并且把openList拆成多个桶bucket按f值范围分组每次从最小非空桶里找。这个思路实现起来也很快。4.2 路径频繁在障碍物边缘蹭如何处理问题现象路径虽然通过了障碍物区域但紧贴着障碍物表面实际操作风险很大。根本原因是A星的栅格地图是离散的障碍物只占一个栅格路径经过相邻栅格时从离散角度看是安全的但实际无人机有体积距离障碍物太近。解决方法在生成地图时做障碍物膨胀把每个障碍物栅格的半径扩大到无人机半径加安全余量。比如无人机半径是0.3m安全余量是0.2m那障碍物栅格周围两层栅格都标记为障碍。这样A星自然就会避开危险区域。4.3 算法找不到可行路径怎么定位问题问题现象起点和终点之间有障碍物但理论上存在可行路径算法却报了“未找到可行路径”。排查步骤检查起点和终点本身是否在障碍物里代码里有没有被误标记检查地图膨胀是否过度把通路全堵死了检查邻居节点扩展范围有没有漏掉一些可行方向障碍物生成时出现不可达区域这属于地图本身的拓扑问题这种情况需要重新生成地图。我踩过最隐蔽的一个坑是地图生成时把障碍物放在了起点旁边膨胀后起点四周全被封死这种情况程序正常报没有路径但人眼一看就能发现起点被障碍包围了。所以在路径规划前加一个起点终点连通性检查很有必要可以用广度优先搜索判断两点是否在同一个连通区域。4.4 参数调优一张表讲清楚我用一个表格整理关键参数对路径规划结果的影响方便你对照调参参数调小调大建议值启发函数权重搜索慢路径更优搜索快路径次优1.0~1.2高度变化惩罚系数路径起伏大路径平缓但绕路0.5~1.0障碍物膨胀层数路径贴障碍物可行空间变小无人机半径/栅格大小 1栅格分辨率地图精度高搜索慢地图粗糙路径差无人机尺寸的1.5~2倍B样条平滑系数路径更平滑但偏离原路径保持原路径但毛刺多0.6~0.84.5 关于代码运行环境整套代码我在Matlab R2022a上验证过大部分函数不依赖工具箱只有csaps需要Curve Fitting Toolbox。如果你没有这个工具箱可以自己写个简单的滑动平均虽然效果不如B样条但也能去掉大部分锯齿。另外地图可视化用到了surf和scatter3这是Matlab基础图形函数不用额外安装。还有个小建议把整个搜索过程做成动画每一步高亮当前节点和openList的大小这样既能直观看到算法怎么找到路径的也能发现搜索过程中的异常行为对调试帮助很大。5. 三维A星的后续扩展我之前做过的两个扩展方向也一并分享。第一个是动态避障。静态A星只能规划一次路径如果无人机飞行过程中发现前方有新的障碍物就需要局部重规划。常见方案是先用A星算出全局路径然后每隔一段时间检测路径前方是否有新障碍物如果有就只在这段局部区域重新执行A星拼接到剩余路径上。这个方案我现在已经在做效果很稳定。第二个是考虑动力学约束。A星输出的路径是三维空间中的几何路径不包含时间信息和姿态信息。如果要接飞控还需要对路径做时间参数化生成速度、加速度曲线。或者用A星先粗规划再用MPC做平滑跟踪把“能不能飞”和“怎么飞”两个问题分开解决。这套代码的完整工程我已经整理好可以直接根据上面的步骤自己搭建。如果在复现过程中遇到问题或者有更好的优化思路欢迎在评论区交流。我调试这套代码的过程中踩过的坑不少希望能帮你少走些弯路。本文还有配套的精品资源点击获取
)
