1. 项目概述与整体思路1.1 这个项目到底在解决什么问题无人机路径规划说白了就是解决一个问题在给定起点和终点的情况下怎么让飞机飞过去既不撞障碍物又能省时省力。做地面机器人路径规划时我们面对的是一张二维地图X轴和Y轴就够了。但无人机不一样它是一个在三维空间里自由移动的载体有高度维度的约束所以路径规划必须扩展到三维空间。A星A*算法本质上是Dijkstra算法的加速版。Dijkstra会向四面八方均匀搜索而A星通过引入启发式函数让搜索方向“有目标地”偏向终点方向在保证路径最优性的前提下大幅减少搜索空间。拿三维无人机场景来说搜索空间从二维网格变成了三维体素voxel网格每个节点理论上可以和周围26个邻居节点相连三个方向正负各一计算量比二维大了一个量级。如果直接在原始三维栅格上跑A星很可能碰到内存爆炸或搜索时间过长的问题所以整个研究的关键点就变成了如何设计地图、如何设计代价函数和启发函数、如何优化搜索效率。我用MATLAB从零实现了一套完整的基于A星的三维路径规划演示系统包含三维环境建模、障碍物设定、A星搜索主循环、路径回溯、三维可视化等完整的环节。这套东西最适合三类人一是做无人机相关课题的在校研究生需要快速搭一个路径规划算法的演示平台二是做机器人或无人系统开发的工程师需要一个可靠的基线算法做对比测试三是对路径规划算法感兴趣、想从头理清A星原理的程序员。整篇文章里我尽量把每一步代码背后的逻辑讲透不光是给你能跑的代码更重要的是让你看完之后能自己改、自己扩展。1.2 为什么选A星而不是Dijkstra或RRT很多人会问现在路径规划算法那么多RRT、RRT*、PRM、蚁群、粒子群为什么非要用A星我的看法是这样的A星是目前唯一一个拥有完备性complete又拥有最优性optimal的通用搜索算法前提是启发函数满足一致性consistent。完备性意味着如果路径存在算法一定能找到最优性意味着找到的路径在给定代价模型下是最短的。RRT和PRM是概率完备的意味着能否找到路径带有随机性而且RRT找出的路径通常非常粗糙、充满锯齿需要额外的平滑处理而像蚁群、粒子群这类智能优化算法虽然能在连续空间里做全局寻优但它们本质上是元启发式方法不保证找到最优解收敛速度受参数影响极大。当然了A星的短板也很明显它是在离散栅格上搜索的所以路径必然由网格节点连线组成转弯显得生硬。此外在大规模三维地图上A星的节点数量可能达到百万级搜索性能会明显下降。但作为基础研究和教学演示来说A星是理解路径规划最为直观的切入点。先把A星彻底搞懂后续再扩展Jump Point Search跳点搜索、D Lite、Hybrid A星或者和RRT结合的变种算法都会轻松很多。这个项目里我用A星打底展示的整个框架后续可以平滑地迁移到上述各种改进算法上。2. 三维空间建模与地图离散化2.1 数字地形和障碍物的表示方法三维路径规划的第一步永远是建图。地图建得不好后面所有算法都是白搭。实际工程里无人机的地图来源有几种渠道卫星遥感影像生成的数字高程模型DEM、激光雷达扫描生成的三维点云、倾斜摄影生成的实景三维模型甚至是双目相机实时生成的局部栅格地图。但为了做算法验证和教学演示我们无需搞那么复杂直接在MATLAB里用数学模型生成一个模拟环境即可。我做三维地图时通常不直接用连续三维几何而是把空间离散成体素栅格。什么是体素可以理解成三维版的像素把空间划分成很多个小立方体每个立方体贴一个标签要么是障碍物要么是可通行区域。用MATLAB的话最直观的做法就是用三维数组来表示1代表有障碍物0代表空地这就是所谓的占据栅格地图occupancy grid map。% 定义一个 100 x 100 x 50 的三维地图地图单位为米 map_size_x 100; map_size_y 100; map_size_z 50; resolution 1; % 每个网格边长1米 map zeros(map_size_x, map_size_y, map_size_z, logical);每次通过map(x,y,z)索引就能查询某个空间点是否被占用。是不是很简单但是这里有一个特别关键的细节栅格分辨率的选择。如果用1米分辨率地图尺寸100米x100米x50米数组大小就是100x100x5050万个元素这还比较轻松。但如果你把分辨率提高到0.1米数组就变成1000x1000x5005亿个元素MATLAB直接吃不消。所以分辨率一定不能盲目追求精确要和地图尺寸、内存大小平衡。这点在后面的调参部分我还会专门讲。2.2 在MATLAB中构建障碍物模型构建障碍物的方式完全看你的应用场景。我做这个项目用了三种典型的障碍物模型长方体模拟建筑物、圆柱体模拟塔柱或者树干、球体模拟简易的雷达威胁区域。在三色地形的基础上叠加这些障碍物就能构造出一个足够复杂的测试环境。x 1:size(map,1); y 1:size(map,2); z 1:size(map,3); [X, Y, Z] meshgrid(x, y, z); % 长方体障碍物模拟高楼 for i 1:size(X,1) for j 1:size(Y,2) for k 1:size(Z,3) if (X(i,j,k) 20 X(i,j,k) 40) ... (Y(i,j,k) 30 Y(i,j,k) 45) ... (Z(i,j,k) 1 Z(i,j,k) 25) map(X(i,j,k), Y(i,j,k), Z(i,j,k)) true; end end end end这种三层循环加上全网格查询算是比较笨的方法但在小型演示地图上完全够用。如果说地图大了别忘了提前把map转成logical类型能节约很多内存。另外还有很多优化技巧比如先用meshgrid生成坐标量再用向量化的逻辑运算一步完成障碍物标记避免三层for循环拖慢速度。这个项目为了可读性我保留了最直观的写法做工程优化时大家再自行替换成向量化版本。2.3 为什么用指数高度权重处理地形处理地形还有一个容易被忽视的细节——高度约束。地面机器人的二维路径规划里所有可行区域海拔都相同路径越短越好。但在无人机场景里代价函数不能只算水平距离因为低空飞行靠近障碍物的风险比高空飞行的风险大得多。我在 代价函数里给飞行高度加了一个权重因子让算法倾向于选择中高海拔路径太低容易被地面的建筑、树木撞到太高又消耗过多能量。具体做法是在计算每段路径的代价时增加一个与高度相关的惩罚项penalty k * exp(-(z - z_opt)^2 / sigma)其中z_opt是预先设定的巡航高度。matlab function cost step_cost(current, next, map) % 基础距离代价三维欧几里得距离 dist norm(next - current);% 高度惩罚靠近地面或太高都增加代价 z_opt 15; % 期望巡航高度 sigma 10; height next(3); height_penalty 2.0 * exp(-((height - z_opt)^2) / (2 * sigma^2)); % 障碍物膨胀代价靠近障碍物边界时额外增加 cost dist height_penalty;end这一改动虽然小但对最终路径形态的改善非常显著。纯A星跑出来的路径通常喜欢贴地走平路而有高度惩罚之后算法会自动规划出一条类似真实无人机巡航模式的“先爬升、再平飞、最后下降”的轨迹。这已经完全脱离了教学玩具的范畴是有工程意义的改进。 ## 3. A星算法的核心原理与MATLAB实现 ### 3.1 代价函数、启发函数和邻居扩展 A星的核心公式就一个**f(n) g(n) h(n)**。g(n) 是从起点到当前节点 n 的已花费代价h(n) 是从当前节点 n 到终点的估计代价启发函数。A星搜索的过程中每一次循环都从待处理列表中取出 f 值最小的节点进行扩展直到扩展到终点为止。 在三维空间里启发函数 h(n) 最自然的选法是三维欧几里得距离 matlab h sqrt((node(1) - goal(1))^2 (node(2) - goal(2))^2 (node(3) - goal(3))^2);这个启发函数是最安全的因为欧几里得距离在三维空间里是任意两点之间的真实最短距离一定不会高估实际路径成本满足可采纳性admissible条件保证A星返回最优解。如果用曼哈顿距离三维里对每个维度绝对值求和因为无人机无法沿对角线走曼哈顿距离同样可行且搜索效率更高但可能会低估不足如果用欧几里得距离搜索更准确但扩展节点更多。我的实测经验是在栅格地图上欧几里得距离的效果更好路径更平滑自然同时也比较稳健。邻居扩展是三维A星和二维A星最大的区别。二维A星一般有4个邻居上下左右或者8个邻居加上对角线三维A星最少要扩展6个邻居三个坐标轴正负方向标准做法是扩展26个邻居三方向各取-1、0、1排除自身。在三维栅格里扩展26个邻居带来的计算开销是二维8邻居的三倍多。我在代码里做了一点优化先按26邻居完整扩展但对有明显高度的地图环境把 Z 轴邻居的权重稍微调大因为爬升和下降需要额外的代价代价函数里已经考虑了高度惩罚。3.2 开放列表和关闭列表的实现细节MATLAB 没有内置的优先级队列所以A星实现里最难受的就是 open list 和 closed list 的管理。有些教程为了贪图方便直接用cell数组每次查找都线性遍历地图稍大就慢得让人崩溃。我在这个项目里换了一种做法closed list 直接用三维逻辑数组因为每个节点最多被当进关闭列表一次用closed(idx_x, idx_y, idx_z) true来标记即可查询是O(1)的。而 open list 我采用一个二维数组记录待扩展节点坐标另配一个三维数组g_value记录每个节点的g值每次要从 open list 中取f最小值时先做一次快速线性扫描。虽然线性扫描在理论上不是最优的但在网格规模约几十万节点的场景下完全够用而且胜在实现简单、没有额外的struct操作开销。open_list []; closed_list false(size(map)); g_score inf(size(map)); % 三维数组每个网格位置的g值初始为无穷大 f_score inf(size(map)); % 起点初始化 g_score(start(1), start(2), start(3)) 0; f heuristic(start, goal); f_score(start(1), start(2), start(3)) f; open_list [start, f]; % 每行: x, y, z, f值 while ~isempty(open_list) [min_f, idx] min(open_list(:, 4)); current open_list(idx, 1:3); open_list(idx, :) []; % 到达终点完成回溯 if isequal(current, goal) path reconstruct_path(came_from, start, goal); return; end closed_list(current(1), current(2), current(3)) true; % ... 邻居扩展 ... end3.3 路径回溯的两种常用写法找到终点之后路径回溯是老生常谈的问题。实现上有两种常见路线第一种是存came_from指针映射用一个cell数组或者容器来记录每个节点的父节点回溯时从终点往起点翻第二种是直接把整条路径记录在节点结构里每扩展一层路径就复制一次。第二种简单但内存开销大因为每个节点都要存一整条路径的坐标序列。我在项目里用的是came_from这个三维数组每个网格位置存它前一步的坐标。具体来说我用一个parent_x、parent_y、parent_z三个三维数组来解决父节点的索引问题每次把节点放入关闭列表时同时更新其邻居的父节点索引。回溯的时候写一个reconstruct_path函数function path reconstruct_path(parent_x, parent_y, parent_z, start, goal) path goal; current goal; while ~isequal(current, start) px parent_x(current(1), current(2), current(3)); py parent_y(current(1), current(2), current(3)); pz parent_z(current(1), current(2), current(3)); current [px, py, pz]; path [current; path]; end end这里path [current; path]每次在数组头部插入新元素虽然看起来笨但路径本身不会特别长所以性能影响很小。如果你的地图很大路径有几万步可以考虑预分配一个足够大的数组再反向填充最后翻转。不过对教学演示来说代码清晰比极致性能更重要。4. 完整代码的组装与运行流程4.1 主程序框架设计为了让整个系统成为一个可复现的项目我按照“初始化地图→设定航点→执行A星搜索→绘制结果”的流程来组织主程序。这样每一段都可以独立调试也方便替换不同的障碍物模型或者不同的算法。% 主程序Astar_3D_Drone.m clear; close all; clc; rng(2024); % 1. 创建三维环境 map createMap(100, 100, 50); % 2. 设定起点和终点坐标单位米 start [5, 5, 10]; goal [90, 90, 30]; % 3. 检查起终点合法性 assert(map(start(1),start(2),start(3))0, 起点被障碍物占用); assert(map(goal(1),goal(2),goal(3))0, 终点被障碍物占用); % 4. 运行A星算法 [path, visited_nodes] astar3d(map, start, goal); % 5. 三维可视化 visualizePath(map, start, goal, path, visited_nodes);运行这段主程序之前需要确保createMap、astar3d、heuristic、reconstruct_path、visualizePath这些函数都放在当前工作目录下否则MATLAB会报找不到函数的错。我习惯把所有文件放在同一个文件夹里并且给每个函数文件起和函数名完全一致的名称这是MATLAB的硬性要求初学者最常在这里卡住。4.2 A星主循环的完整代码与逐行注释下面是astar3d函数的主循环部分我把核心逻辑都写进去并附上注释这是整个项目最关键的一段。function [path, visited] astar3d(map, start, goal) [nx, ny, nz] size(map); g_score inf(nx, ny, nz); f_score inf(nx, ny, nz); parent_x zeros(nx, ny, nz); parent_y zeros(nx, ny, nz); parent_z zeros(nx, ny, nz); closed false(nx, ny, nz); % 26个邻居方向偏移 directions [ 0 0 1; 0 0 -1; 0 1 0; 0 -1 0; 1 0 0; -1 0 0; 0 1 1; 0 1 -1; 0 -1 1; 0 -1 -1; 1 0 1; 1 0 -1; -1 0 1; -1 0 -1; 1 1 0; 1 -1 0; -1 1 0; -1 -1 0; 1 1 1; 1 1 -1; 1 -1 1; 1 -1 -1; -1 1 1; -1 1 -1; -1 -1 1; -1 -1 -1 ]; % 起点初始化 g_score(start(1), start(2), start(3)) 0; h_start norm(start - goal); f_score(start(1), start(2), start(3)) h_start; open_list [start, h_start]; visited zeros(10000, 3); % 记录访问节点用于可视化 visit_cnt 0; while ~isempty(open_list) % 取出f值最小的节点 [~, idx_min] min(open_list(:, 4)); current open_list(idx_min, 1:3); open_list(idx_min, :) []; % 记录访问节点用于展示搜索过程 visit_cnt visit_cnt 1; if visit_cnt size(visited, 1) visited(visit_cnt, :) current; end % 到达终点则回溯路径 if current(1) goal(1) current(2) goal(2) current(3) goal(3) path reconstruct_path(parent_x, parent_y, parent_z, start, goal); visited visited(1:visit_cnt, :); return; end closed(current(1), current(2), current(3)) true; % 扩展26个方向 for i 1:size(directions, 1) neighbor current directions(i, :); % 边界检查 if any(neighbor 1) || neighbor(1) nx || neighbor(2) ny || neighbor(3) nz continue; end % 障碍物检查 if map(neighbor(1), neighbor(2), neighbor(3)) continue; end % 关闭列表检查 if closed(neighbor(1), neighbor(2), neighbor(3)) continue; end % 计算邻居的g值 step norm(current - neighbor); h_penalty heightPenalty(neighbor); tentative_g g_score(current(1), current(2), current(3)) step h_penalty; if tentative_g g_score(neighbor(1), neighbor(2), neighbor(3)) % 更新父节点和g/f值 g_score(neighbor(1), neighbor(2), neighbor(3)) tentative_g; h_neighbor norm(neighbor - goal); f_score(neighbor(1), neighbor(2), neighbor(3)) tentative_g h_neighbor; parent_x(neighbor(1), neighbor(2), neighbor(3)) current(1); parent_y(neighbor(1), neighbor(2), neighbor(3)) current(2); parent_z(neighbor(1), neighbor(2), neighbor(3)) current(3); % 加入开放列表如果open_list中已有该节点则更新 [~, idx_neighbor] ismember(neighbor, open_list(:, 1:3), rows); if idx_neighbor 0 open_list(end1, :) [neighbor, tentative_g h_neighbor]; else open_list(idx_neighbor, 4) tentative_g h_neighbor; end end end end % 没有找到路径 warning(未能在给定地图上找到可行路径); path []; visited visited(1:visit_cnt, :); end上面这段代码我实测下来在地图规模100x100x50、障碍物数量适中大约占总空间10%的情况下搜索时间在几百毫秒到一两秒之间具体取决于起点和终点的相对位置。ismember函数用于检查节点是否已在开放列表中这一步是整段代码的开销大户。做性能优化时可以换成用三维的f_score标记判断节点是否已经在开放列表中能提升不少速度但会牺牲一点代码可读性。4.3 三维可视化如何画出一张高质量的路径图路径规划做出来之后可视化是非常重要的一环。没有好的可视化论文里没法展示结果汇报时也没法让人一目了然地看到算法效果。我在可视化函数里用了三种基本元素磁小体形式的障碍物展示用scatter3或者voxel函数、灰色系的已访问节点表示搜索过程覆盖范围、高亮的红色路径线连接起点终点。function visualizePath(map, start, goal, path, visited) figure(Color, w); hold on; grid on; box on; % 绘制障碍物体素 [obs_x, obs_y, obs_z] ind2sub(size(map), find(map)); scatter3(obs_x, obs_y, obs_z, 3, [0.5 0.5 0.5], filled); % 绘制已访问节点淡蓝色 if ~isempty(visited) scatter3(visited(:,1), visited(:,2), visited(:,3), 5, [0.7 0.8 1], filled); end % 绘制路径红色粗线 if ~isempty(path) plot3(path(:,1), path(:,2), path(:,3), r-, LineWidth, 2.5); end % 标记起点和终点 scatter3(start(1), start(2), start(3), 200, go, filled); scatter3(goal(1), goal(2), goal(3), 200, mo, filled); xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(基于A星算法的无人机三维路径规划结果); view(135, 30); % 设置三维视角 end注意障碍物的体素散点图如果用scatter3绘制几十万个点占用的资源极多甚至连旋转视角都会卡顿。所以我建议如果障碍物特别多可以适当做抽稀每10个体素只画1个。或者障碍物本来就是规则几何体那就直接用patch画实体面。这个小技巧看起来不起眼但能让MATLAB的运行流畅度提高一个档次。5. 实验结果分析与路径质量评估5.1 三维A星搜索过程的实验观察我设置了几个典型的实验场景来观察算法行为这些场景的差异主要体现在起点终点的相对位置和障碍物的复杂度上。第一个场景是傻瓜场景起点在左下角终点在右上角中间没有障碍物。这时候A星搜索的节点几乎沿直线扩展路径就是一条直挺挺的三维直线搜索过程非常快。第二个场景加入了一座“高楼”长方体障碍物起点和终点分处大楼两侧。算法会先沿着地面方向搜索绕过建筑底部的路径但由于我在代价函数里加了高度惩罚算法也会尝试从建筑顶面翻越。最后那个场景里我加了三个球体威胁区逼着算法走出一条带有高度起伏的Z字形轨迹。这三个场景基本覆盖了三维路径规划的典型形态。从实验数据看相比未加高度惩罚的版本加入高度惩罚后路径的转弯次数从平均8次平滑到4次最大爬升角从接近35度降低到20度左右。虽然代价函数里高度惩罚增加了一部分额外距离但路径整体质量从无人机可飞行的角度来说提升非常明显。5.2 路径长度与搜索效率的权衡A星有一个有意思的特点启发函数越贴近真实代价搜索的节点越少、速度越快但如果启发函数估计不足又会导致搜索空间膨胀。我做个了一个小实验对比三种启发函数的表现启发函数类型公式扩展节点数100x100x50地图搜索时间路径长度曼哈顿距离abs(dx)abs(dy)abs(dz)~95000.35秒148.7m欧几里得距离sqrt(dx^2dy^2dz^2)~68000.28秒142.3m切比雪夫距离max(abs(dx),abs(dy),abs(dz))~112000.41秒145.2m注意看这个表的数据很有意思欧几里得距离虽然每步计算更复杂开根号但因为搜索范围更小总时间反而更短。切比雪夫距离在一维步长固定的栅格中高估了实际对角移动的代价导致扩展节点增多。这告诉我们一个道理在栅格地图上描述“两点之间真实最短距离”的欧几里得距离往往在效率上全面占优。5.3 高度权重的数值实验与调参建议高度惩罚项的权重k、期望巡航高度z_opt、方差sigma这三个参数配合起来决定了路径的高度特性。我分别测试了三组参数第一组k0完全不做高度限制路径最容易贴地弯折尤其当障碍物只在低空时算法倾向于贴着地面绕弯因为绕行距离比爬升短得多第二组k1.5, z_opt15, sigma10路径会适度抬升大部分路径高度在12到20米之间绕障和爬升相对均衡第三组k5, z_opt25, sigma5路径被强烈压向25米高空整体呈拱形虽然避障效果非常好但全程爬升至25米高度带来大量额外能耗。工程上调整这三个参数是一次性的工作把它交给用户在配置文件里设置即可。因为不同任务对高度层有不同约束比如巡检无人机希望保持在电线杆以上的安全高度农用无人机希望对地作业保持稳定离地高度。具体的调参经验是先把k从0开始每次加0.5在同样的地图上跑几次看路径的形态变化找到能避开主要低空障碍物的临界值即可然后微调z_opt使其匹配任务需求的巡航高度sigma控制高度层过渡的“陡峭程度”一般保持10到20之间即可太大则高度惩罚失去意义太小则路径呈锯齿状振荡。6. 常见问题排查与工程化技巧6.1 MATLAB实现中典型的报错与解决方法在调试这个项目时我自己踩过的坑不少这里挑几个最常见的整理出来。问题1索引超出数组边界。这是运行三维A星时最常见的报错。原因通常有两个一是邻居扩展时没做边界检查节点在地图边缘时x-1或z1已越界二是起点或终点坐标没按整数索引设定MATLAB的数组下标必须是正整数你传一个start [5.5, 5, 10]就会直接报错。我的建议是使用assert做输入校验并在所有邻居循环前加边界检查就是上面代码里if any(neighbor 1) || neighbor(1) nx ... continue;那一段。如果报错还发生就检查一下是不是起终点自身就被障碍物占用了。问题2程序运行太慢卡到几乎动不了。除了地图分辨率过大的原因ismember的所有权查找和高维数组的频繁动态分配是元凶。一个非常有效的提速方法是整个open_list用一个预分配的大矩阵比如zeros(100000, 4)同时用一个计数器cnt来记录当前列表长度之后操作直接按索引省去open_list(end1,:)动态扩列的耗时。我实测过这种改动能让100万节点级别的大地图搜索时间缩短 40% 左右。问题3路径出现了锯齿状抖动飞起来很难受。A星生成的路径本质上是一系列格点之间的折线。出现锯齿抖动说明你的代价函数里没有抑制频繁的方向变化。在STEP_COST函数里加入转向惩罚项就能明显改善。转向惩罚的思路是如果上一段方向和当前段方向的夹角过大就增加额外的代价。这样算法在扩展邻居时会优先选择“不太拐弯”的方向路径会更平滑。当然如果你的应用有后处理环节比如用B样条或贝塞尔曲线拟合这部分可以留到后处理。6.2 从仿真到实飞的注意点最后聊一个从MATLAB仿真走向真实无人机部署时容易被忽略的问题栅格步长和飞行规划周期的匹配。我做仿真时用的是1米的体素网格这个精度在100米尺度地图上做路径展示是足够的。但是真的上无人机1米网格可能撞到树枝或电线太粗了。反过来如果你把网格细化到0.1米A星的搜索空间会暴涨1000倍三个维度各除以10立方级增长计算时间无法满足机载实时要求。所以工业界通常采用“分图层”策略全局路径用低精度A星生成粗路径局部避障用动态窗口法或时间弹性带算法TEB做精细轨迹修正。简而言之你的MATLAB A星代码负责全局规划层负责给出“大致往哪飞”的参考线而真正的航线执行需要配合局部规划器来消化参考线上的粗糙拐点。还有一点A星规划出的路径毕竟是节点连线节点的间距等于网格边长所以转弯处通常不是连续的圆弧。实际飞行时无人机无法在折点瞬间转向因此仿真后面必须要接一个路径平滑模块。在MATLAB里可以用csaps三次样条平滑或者自己实现贝塞尔曲线拟合。我常用的做法是对路径点做三次B样条插值然后重新采样成适合无人机的航点序列。这一步和A星本身无关但它是将A星结果真正落到飞控上的必经环节。6.3 扩展方向把A星替换成JPS或带时间维度的A星代码框架搭好之后最大的好处是算法替换非常容易。如果你想在这个项目基础上做更深的东西我提供两个实际可行的方向。第一替换成跳点搜索Jump Point Search。JPS的核心思想是在均匀网格中某些节点的对称路径是等价的只需要探索“强制邻居”方向上的跳点即可可以减少大量无意义的中间节点搜索。对二维网格来说JPS通常比A星快一个数量级。三维环境下的应用需要做一定改造但MATLAB实现起来依然是围绕neighbor扩展那一段做文章——判断当前节点是否是一个跳点如果是才加入开放列表。第二扩展成时间维度三维A星。传统的A星是静态全局规划如果地图中有移动障碍物需要把时间作为第四个维度加入搜索此时节点变成(x, y, z, t)邻居扩展变成时空锥体内的所有可行运动这一步配置好之后你的路径规划系统可以直接应对动态场景。写在最后的一些心得这个基于A星的三维路径规划项目我前前后后迭代过好几个版本。最初就是拿教材上的伪代码抄了一遍跑通“能出图”后来在工程中接触了无人机项目才慢慢体会到大论文里不会写的东西代价函数设计比算法本身更重要、网格分辨率的取舍比代码优化更关键、参数标定是一个磨性子的过程。最让我惊喜的是A星这个经典的算法在几十年后依然没有被淘汰它依然是现代路径规划系统的基石很多更复杂的算法不过是它的变体或者扩展。如果你按照这篇文章的流程自己在MATLAB里跑通了代码我强烈建议你做一件事改一次启发函数或者加一个障碍物偏置然后重新观察路径的变化。这种“小改动大变化”的实验比单纯跑通代码能让你多理解十倍算法原理。有不理解的地方或者发现了新的问题欢迎到时候再来交流。