做无人机路径规划有一段时间了从二维A星一路踩到三维中间换了不下三版地图模型最后在Matlab里把三维A星跑通的那一刻说实话挺爽的。这篇文章我打算把整套思路和代码细节都摊开讲一遍从三维栅格怎么建、启发函数怎么选到Matlab里节点数据结构怎么组织、路径平滑怎么处理再到我实际调试中遇到的几个坑一次性说清楚。如果你正在做基于A星算法的无人机三维路径规划或者刚开始用Matlab做路径规划仿真这篇文章应该能帮你少走不少弯路。三维路径规划跟二维最大的不同不只是多了一个Z轴坐标那么简单。二维A星你只需要考虑四个方向或者八个方向的邻居扩展但三维空间里无人机的运动约束、地形遮挡、障碍物的空间分布全都变了节点的扩展方式、启发式估计、代价函数的设计都得重新想。更现实的问题是Matlab里三维栅格地图的存储和遍历比二维要费内存得多如果不做优化网格稍微密一点算法跑起来能把台式机的风扇转成直升机。在这篇文章里我会先把三维A星的核心设计思路拆开讲然后给出完整的Matlab实现包括地图构建、核心搜索、路径平滑、三维可视化这几个模块最后整理一份我自己调试过程中遇到的常见问题排查清单希望对你有实际帮助。1. 三维A星路径规划的整体设计思路1.1 从二维A星到三维A星变的不只是坐标系如果你已经写过二维A星肯定知道它的核心框架维护一个open list和一个closed list从起点开始每次从open list里取f值最小的节点出来扩展把邻居节点塞进open list直到终点被取出然后从终点倒退父节点重建路径。这套框架放到三维里依然成立但有几个关键点会变。首先是节点的邻居扩展。二维八方向扩展只需要修改当前节点的x和y坐标偏移量是[-1, 0, 1]的组合。到了三维如果做26邻域扩展就要同时考虑x、y、z每个维度上的1、0、-1组合总共3^3 - 1 26个邻居节点。26邻域的优点是路径更自由转弯更灵活但代价是每一次扩展的计算量涨了三倍多。如果只做6邻域扩展只沿坐标轴方向运动计算量小很多但路径会很僵硬无人机的运动会变得很不自然经常出现直上直下或者贴着障碍物走的轨迹。我实际测试下来对于多旋翼无人机这种运动比较灵活的平台用26邻域扩展是值得的。原因很简单多旋翼可以全向运动不像固定翼有最小转弯半径的限制26邻域生成的路径经过平滑之后更接近真实可飞轨迹。其次是代价函数的设计。二维A星里g(n)通常就是起点到当前节点的路径长度h(n)用曼哈顿距离或者欧几里得距离。三维空间里曼哈顿距离的问题会被放大因为多了一个维度曼哈顿距离对对角运动的估计偏差会更明显。所以在三维A星里我强烈建议用欧几里得距离作为启发函数也就是g(n) g(parent) 当前节点到父节点的实际距离 h(n) sqrt((x - goal_x)^2 (y - goal_y)^2 (z - goal_z)^2)欧几里得启发函数是consistency的也就是说它满足三角不等式这保证了A星算法在这个启发函数下一定能找到最优路径而且不需要重新打开closed list里的节点。1.2 为什么A星仍是无人机三维路径规划的优选方案可能有人会问现在深度学习、强化学习那么火为什么还在用A星这种传统算法我的理解是这样的路径规划问题本质上是带约束的搜索问题A星的优势在于它能在已知环境里给出最优解而且理论保证完备。深度强化学习适合的是环境未知、或者环境动态变化的场景但训练成本高、收敛不稳定而且很难证明系统的安全性。对于很多实际应用比如固定航线巡检、已知地图条件下的任务预规划A星仍然是落地方案里最稳的选择。和Dijkstra算法相比A星在启发函数的引导下搜索效率高出一大截。在三维栅格地图里Dijkstra会均匀向外扩展搜索空间是起点到终点的一个大球体而A星有了h(n)的引导搜索空间会被压缩成一个沿起点到终点方向的漏斗形。我做过对比在100x100x30的栅格地图里起点在左下前方、终点在右上后方Dijkstra需要扩展的节点数量大概是A星的三到五倍这个差距在三维环境里只会更明显。当然A星也有自己的弱点比如面对动态障碍物需要反复重规划但这些可以通过DWA局部规划或者A星的D*变种来补足不影响它在全局规划里的地位。2. 三维环境建模与核心参数设计2.1 三维栅格地图的Matlab表示方式做三维路径规划的第一步不是写算法而是先把地图建出来。我常用的方法是构造一个三维0-1矩阵0代表可通行1代表障碍物。假设飞行区域是500米x500米x100米栅格分辨率是5米那么地图的尺寸就是100x100x20。这里栅格分辨率的选择非常关键栅格太大路径会丢失很多可行空间甚至出现看起来能飞过去实际障碍物把通道堵死了的情况栅格太小地图存储和搜索的计算量都会爆炸。我自己的经验公式是对于巡检类无人机栅格大小取机体尺寸的1.5到2倍比较合适。比如轴距450mm的无人机栅格取1米左右就够用。如果只是做算法验证不需要绑定具体机型那栅格大小主要取决于你对路径精度的要求。三维栅格的地形可以用一个高度矩阵来模拟。常用的方法是生成多个高斯型山峰[X, Y] meshgrid(1:100, 1:100); Z zeros(size(X)); % 叠加几个山峰 Z Z 50 * exp(-((X-30).^2 (Y-40).^2) / 300); Z Z 35 * exp(-((X-70).^2 (Y-60).^2) / 400); Z Z 20 * exp(-((X-50).^2 (Y-20).^2) / 200);然后把Z矩阵栅格化凡是高于设定飞行高度的格点标记为障碍物。这个做法的好处是你可以直观看到一个包含了地形起伏的三维环境而不是一个纯随机的障碍物空间。实际工程里这个Z矩阵完全可以替换成数字高程模型DEM数据算法逻辑不用改。2.2 启发函数与代价函数的选定逻辑前面提过我推荐用欧几里得距离作为启发函数但实际写代码的时候还有几个细节。第一个细节是启发函数的代价权重。在某些场景下你希望无人机尽量走直线、少拐弯可以在h(n)前面乘上一个大于1的权重系数。比如h(n)乘1.5算法会表现得更贪婪搜索速度更快但代价是可能错过最优解。如果你做的是实时重规划这个特性反而有用因为你不一定非要全局最优路径能快速找一条可行路径更重要。但如果你的目标是理论最优路径权重系数就保持1.0。第二个细节是代价函数里要不要加入高度惩罚。无人机的能耗很大一部分来自克服重力做功所以飞得越高越费电。我通常会在g(n)的计算里加入一个高度项比如g(n) g(parent) 水平距离 垂直距离 * 爬升惩罚系数爬升惩罚系数取1.2到2.0之间这样算法会倾向于在相同路程下选择更低的路径减少能耗。这个设计在山区地形里效果特别明显。第三个细节是拐弯代价。无人机的飞行控制有个特性频繁转向会降低飞行效率甚至引发姿态震荡。可以在代价函数里加入转向角惩罚如果当前节点的扩展方向和上一段路径方向夹角过大就加一个惩罚值。但要注意方向信息的存储会让节点结构变复杂代码实现难度会上升如果你的需求没那么强可以先不加。2.3 安全约束离地高度与障碍物膨胀路径规划有一句老话规划出来的路径不是给你看的是给无人机飞的。所以路径上的每个点不仅要保证自己不落在障碍物栅格里还要保证无人机在飞行过程中机身不会蹭到障碍物。这就需要做障碍物膨胀处理。最简单的做法是形态学膨胀把三维地图里的每个障碍物栅格沿着x、y、z三个方向向外扩张若干个栅格。膨胀半径取决于无人机的几何尺寸和定位误差。我一般取机身半径的1.5倍再加一个栅格的定位余量。在Matlab里可以用imerode配合自定义的结构元素来做三维膨胀也可以用三重循环遍历实现。地图尺寸小的时候三重循环没问题但地图一大就得注意效率了后面我会专门讲性能优化的问题。除了障碍物本身的膨胀无人机还有一个最小离地高度的约束。这个不能简单地通过膨胀地形来实现因为如果你把地形整体抬高了那一些原本可以穿过的山谷通道就被封死了。更好的做法是在代价函数里增加一个地形贴地惩罚项当节点的Z坐标低于地形高度加上安全高度阈值时直接把g(n)设为一个很大的数让算法自动绕开。这样做既保证了安全又不至于过度保守。3. Matlab完整实现与代码架构拆解3.1 节点数据结构与地图初始化Matlab不是C写A星的时候数据结构的选择直接影响代码的简洁性和性能。我推荐用结构体数组来表示节点信息每个节点包含以下字段% 节点数据结构 node.g Inf; % 起点到当前节点的实际代价 node.h 0; % 当前节点到终点的启发估计 node.f Inf; % 总代价 g h node.parent []; % 父节点坐标用于回溯路径 node.closed false; % 是否已扩展 node.open false; % 是否已在open list中 node.xyz [x, y, z]; % 节点三维坐标我的做法是建一个三维cell数组或者三维结构体数组nodes_map{x, y, z}每一格保存这个节点对象。初始化的时候把所有节点的g设为Infclosed和open都设为false。% 地图参数 map_size [100, 100, 20]; % x, y, z方向的栅格数 resolution 5; % 每个栅格代表的实际距离米 start_node [2, 2, 5]; % 起点栅格坐标 goal_node [90, 95, 15]; % 终点栅格坐标 % 构建三维栅格地图obstacle_map obstacle_map generate_terrain_map(map_size); % 地形生成函数返回0-1矩阵open list的实现最简单的方式是用Matlab的数组来模拟优先队列每次从open list里找f值最小的节点。但这个方法在节点数量多的时候效率非常低因为每次查找都是O(n)。我在项目中测试过当扩展节点数超过一万个时这种线性查找的耗时占了整个算法的90%以上。更好的办法是用二叉堆。Matlab虽然没有内置的优先队列类但可以用Java对象接口Matlab底层支持调用Java的PriorityQueue。这是我实测很稳的一个方案父节点回溯也用结构体字段实现具体做法是每个节点保存父节点的索引等算法结束后从终点倒推。3.2 A星核心搜索循环下面这一版是我跑了很久的核心代码去掉了无关的判断逻辑保留了主干。while ~isempty(open_list) % 从open list取出f值最小的节点 [current_node, idx] extract_min(open_list); remove_from_open_list(open_list, idx); % 如果当前节点是终点回溯路径 if isequal(current_node.xyz, goal_node) path reconstruct_path(current_node); break; end current_node.closed true; % 生成26个邻居 neighbors generate_neighbors(current_node.xyz, map_size); for each neighbor in neighbors if obstacle_map(neighbor) 1 continue; % 跳过障碍物 end if neighbor_node.closed continue; end % 计算从起点经当前节点到邻居的代价 move_cost euclidean_distance(current_node.xyz, neighbor.xyz); tentative_g current_node.g move_cost altitude_penalty(neighbor, terrain); % 如果tentative_g更小更新邻居节点 if tentative_g neighbor_node.g neighbor_node.g tentative_g; neighbor_node.h euclidean_distance(neighbor.xyz, goal_node); neighbor_node.f neighbor_node.g neighbor_node.h; neighbor_node.parent current_node; % 如果邻居不在open list中加入 if ~neighbor_node.open add_to_open_list(open_list, neighbor_node); neighbor_node.open true; end end end end这段逻辑有几个值得注意的地方。第一检查邻居是否在closed list里的时机要放在代价计算之前放在后面会浪费大量计算。第二更新代价时要先判断tentative_g neighbor_node.g这是A星能保证最优路径的核心机制。第三在三维环境里euclidean_distance要写对三个坐标分量都要参与计算不能漏了z轴。如果你做的是26邻域扩展generate_neighbors函数里对z坐标的处理要特别注意。z方向是飞行高度邻居的z偏移可以取[-1, 0, 1]但实际飞行中无人机从低空到高空需要爬升从高空到低空需要下降这两个过程的代价在物理上是不对称的爬升能耗更大。如果希望路径更贴近真实飞行性能可以在z偏移为1时额外增加爬升代价。3.3 地形随动与碰撞检测的工程化处理三维环境里最容易被忽视的是路径贴着地形走的问题。纯A星搜索如果只把地形最高点当成障碍物生成出来的路径往往会在地形上方平飞。这看起来很安全但实际上很耗电而且如果遇到强侧风贴近地形的山脊处反而更危险。我的做法是在代价函数里增加一个地形随动因子。具体来说每个节点的g值会乘上一个系数这个系数跟当前节点离地高度有关height_above_terrain neighbor.xyz(3) * resolution - terrain_height(neighbor.xyz(1), neighbor.xyz(2)); if height_above_terrain safe_height tentative_g tentative_g 1e6; % 距离地面太近直接惩罚掉 elseif height_above_terrain cruise_height % 贴地飞行区域适当惩罚 tentative_g tentative_g 5; end这里有两点要说明一是terrain_height函数需要利用插值从DEM数据中获取x、y坐标对应的地形高度如果地图本身是栅格化的直接查表就行。二是cruise_height是期望的巡航高度你可以根据任务来设置比如巡检任务要求无人机在目标物上方20米左右的高度飞行那这个参数就设成20米。还有一个很实用的技巧碰撞检测不要只检测当前节点本身还要检测当前节点和父节点的连线是否穿过障碍物。在三维栅格地图里如果栅格分辨率比较大两个相邻节点连线穿越障碍物栅格是可能的。这个检测可以用Bresenham三维直线算法来实现判断连线经过的所有栅格是否有障碍物。这个细节在二维A星里很多人就忽略了到三维环境里问题会放大得更多因为空间里的障碍物分布比平面更复杂。我的实测经验是加上连线碰撞检测之后生成路径的可飞性大幅提升几乎不会出现路径穿模障碍物的情况。3.4 路径平滑后处理A星直接搜索出来的路径本质上是栅格中心点的连线会有很多折角直接给飞控执行飞机会表现得非常抽搐。所以路径平滑是不可缺少的后处理步骤。我常用的平滑方案是B样条曲线拟合法。Matlab里可以直接用spapi函数实现B样条插值% path_points是A星生成的路径点序列每一行是[x, y, z] k 3; % 三次B样条 spl spapi(k, 1:size(path_points,1), path_points); smooth_path fnplt(spl);这里的核心逻辑是把路径点按顺序归一化到参数轴上然后做三次B样条插值得到一条光滑的曲线。用B样条而不是简单的线性插值是因为B样条能保证曲线经过控制点附近不会偏离原始路径太多。不过B样条平滑有个问题平滑后的曲线可能会偏离原始路径甚至穿过障碍物栅格。解决办法是平滑完之后做一次碰撞检测如果检测到碰撞就把平滑曲线上的碰撞点替换回原始路径点或者增大B样条的节点密度让曲线更贴近原始路径。我实际项目中用的是冗余节点判断控制点权重调整的方法。具体来说B样条拟合时给每个路径点分配一个权重关键节点比如路径拐点、靠近障碍物的点权重调高让曲线在这些点附近更贴一些其他中间点权重降低让曲线更顺滑。这样既保证了安全性又保证了可飞性。4. Matlab代码的性能优化与调试技巧4.1 大数据集下Matlab的运行效率瓶颈很多第一次用Matlab写A星的人会遇到一个尴尬的情况地图稍微大一点程序跑半天也不出结果。这背后的原因是Matlab的数组操作是解释执行的循环里大量动态修改数组内容会触发反复的内存分配和类型判定。我踩过的坑是用[current, idx] min([open_list.f])来取最小节点这个操作本身不慢但问题是它返回的是索引然后你要通过这个索引去结构体数组里拿节点信息这一步在Matlab里的开销比我预想的要大得多。后来我换成了Java的PriorityQueueMatlab里可以直接这样调用pq java.util.PriorityQueue; pq.add(node_f); % 节点以[f, x, y, z]元组形式加入Java自带堆排序插入和删除最小元素都是O(log n)跟Matlab原生数组的线性查找比起来效率差距在节点数量上百后就开始显现了。在100x100x20的地图里A星往往需要扩展两三万个节点用Java优先队列可以把规划时间从几十秒降到几秒。还有一个小优化是提前把地图的障碍物检测结果保存成一个三维逻辑数组不要在A星循环里重复调用地形生成函数。地形的计算量很大如果每次扩展节点都重新算一遍时间全浪费在重复计算上了。4.2 A星搜索异常行为排查我在调试过程中遇到过几个比较典型的异常现象这里集中整理一下。第一个异常是路径起点和终点明明都可达但算法就是找不到路径。最可能的原因是终点被标记成了障碍物。检查方式是在main函数开头打印一下obstacle_map(goal_x, goal_y, goal_z)的值如果是1那就是地图生成逻辑有bug。另外还有一种可能是你的邻居生成函数在边界处漏掉了某些方向导致搜索无法到达终点。三维边界处理比二维复杂四个边变六个面很容易漏写。第二个异常是生成的路径穿过了障碍物。这个我之前提到过多半是因为没有做连线碰撞检测。还有一个隐蔽的原因是路径平滑阶段B样条把路径拉偏了。调试的时候可以先不启用平滑看原始路径是否安全如果原始路径安全而平滑后穿模问题就出在平滑参数上。第三个异常是算法运行时间特别长而且路径不远。这个大概率是启发函数设置有问题。如果你把h(n)设成了0A星就退化成Dijkstra搜索范围会大很多。检查一下h sqrt(...)那行是不是写对了z轴分量是不是漏掉了。第四个异常比较有意思是路径来回折返。A星正常情况下不会产生折返路径除非你的代价函数里加入了某些奇怪的惩罚项导致绕远路比走直线更划算。我遇到过的情况是爬升惩罚系数设得过大算法宁可水平绕一大圈也不肯爬升10米生成的路径看起来非常离谱。解决办法是控制爬升惩罚系数的取值范围别超过2.5。4.3 从仿真到实机部署的三点建议仿真跑通了下一步必然是往实机上搬这里说三个我实际碰到的坎。第一仿真里的栅格地图到实机导航坐标系有一个对齐问题。仿真里你用的是行列号实机里你需要通过经纬度、高度来换算到局部坐标系再换算到栅格坐标系。这一步如果没做好算法算出来的路径在地图上是对的但无人机实际飞起来完全对不上。建议在代码里封装一个坐标转换模块单独测试这个模块的精度和稳定性。第二A星规划出来的路径是几何路径不包含时间信息和速度信息。实机部署时必须把路径点发送给飞控的航点执行模块由机载SDK或飞控固件完成航点间的速度规划和轨迹跟踪。这里要特别注意路径点间距不能太大否则飞控在航点之间走直线的时候可能会切掉拐角导致偏离规划路径。我一般是把A星路径点稠密化每隔一两个栅格插值出一个中间点再发给飞控。第三仿真里不考虑风的干扰但实机飞行的风是常态。如果无人机在强风环境中飞行A星规划出的路径可能无法精确跟踪。这种情况下需要增加一个局部避障模块比如用激光雷达或视觉传感器做实时障碍物检测发现前方动态障碍物时在A星全局路径的基础上做局部绕行。A星负责全局最优局部模块负责避障应急两者配合才能构成一个完整可部署的系统。5. 三维A星结果可视化与任务场景扩展5.1 Matlab三维轨迹可视化实现路径规划算法跑完了如果没有一个清晰的三维可视化调试起来会非常费力。我用的是Matlab的三维绘图函数做整体展示效果比较直观。% 绘制地形表面 surf(X * resolution, Y * resolution, Z, EdgeColor, none, FaceAlpha, 0.6); hold on; % 标记起点和终点 scatter3(start_xyz(1), start_xyz(2), start_xyz(3), 100, g, filled); scatter3(goal_xyz(1), goal_xyz(2), goal_xyz(3), 100, r, filled); % 绘制规划路径 plot3(path(:,1), path(:,2), path(:,3), b-, LineWidth, 2); % 绘制平滑后路径 plot3(smooth_path(:,1), smooth_path(:,2), smooth_path(:,3), r--, LineWidth, 2); legend(地形, 起点, 终点, A星路径, B样条平滑路径);有几个细节值得注意。FaceAlpha参数控制地形表面的透明度设成0.5到0.6比较合适既能看清地形起伏又不会完全遮挡路径。LineWidth设成2路径曲线在图形窗口里会更醒目。另外建议把坐标轴的纵横比设成等比例否则z轴被拉伸或压缩之后你很难判断路径的实际坡度变化axis equal;如果地图范围很大还可以添加视角控制用view([-30, 20])设置一个倾斜视角方便观察三维路径的空间走向。5.2 从静态全局规划到动态重规划A星本质上是静态全局规划算法但很多任务场景要求无人机在飞行过程中根据环境变化动态调整航线。我尝试过的一种有效方案是把A星作为全局规划器每隔一段时间比如5秒在无人机当前位置和目标点之间重新规划一次完整路径。对于中等规模的地图A星的规划时间在秒级以内完全满足周期性重规划的时间要求。如果环境变化特别频繁这种全局重规划的计算开销就会比较大这种情况下可以把重规划范围限制在局部窗口内只重新规划无人机前方一定距离内的路径这样既能保证对环境的响应速度又不会频繁推翻全局路径。另外一个思路是把A星扩展到动态场景比如用D* Lite算法但实现复杂度会明显上升。如果你只是做毕设或者工程验证A星加周期重规划已经完全够用了。5.3 结合Google Earth或者真实DEM数据的扩展方向如果你的项目需要更高的真实感可以把Matlab里模拟的数学地形换成真实DEM数据。网上有很多公开的DEM下载渠道文件格式一般是GeoTIFF。Matlab里用readgeotiff函数就能读取DEM数据然后直接替换掉我之前代码里的generate_terrain_map函数。这里有个坑我必须提醒你真实DEM数据的分辨率通常很高比如30米分辨率但在小范围三维路径规划里可能不够精细。你需要根据飞行区域的大小和栅格规划需求对DEM进行重采样。重采样方法在Matlab里可以用interp2或resample函数实现重采样成和你的栅格地图分辨率匹配的尺寸。还有一点真实DEM数据的坐标系是地理坐标系包含经纬度信息。在做路径规划之前先把经纬度坐标投影到平面直角坐标系建议用UTM投影或者本地高斯投影。如果你的飞行范围不大用简单的近似等距投影也能保证精度。把真实地形接入路径规划系统之后整个项目的应用前景会开阔很多比如山区电力巡检的航线预规划、灾后区域的无人机物资投送路径规划、林业巡检的无人机全覆盖路径规划。A星作为全局搜索的核心模块和这些业务场景做组合能发挥的价值比单纯做一个仿真实验要大得多。6. 常见问题速查表与最后的几点经验6.1 三维A星常见问题速查表我把调试过程中遇到的高频问题汇总成一个表方便读者对照排查。现象可能原因排查方法算法找不到路径终点是障碍物、邻居生成不完整打印终点栅格值检查6个边界面的邻居索引规划时间过长open list线性查找、地图过大换Java优先队列缩小地图范围或增大栅格尺寸路径穿入障碍物缺少连线碰撞检测、平滑曲线偏移加Bresenham连线检测调整B样条权重路径非常曲折启发函数偏差、障碍物形态复杂检查h计算式确认z轴参与计算路径绕远路惩罚系数设置不当降低爬升惩罚系数到1.5以下路径高度剧烈波动地形随动因子设计问题给贴地区域设置硬惩罚优化高度惩罚曲线6.2 我实际踩过的坑与心得写到这里最后分享一点个人的体会。第一次做三维A星的时候我踩的最大的一个坑就是盲目追求地图精度把栅格分辨率设得非常细结果Matlab内存直接爆掉程序还没跑起来就崩了。后来才意识到栅格分辨率不是越高越好它需要跟无人机的实际尺寸、传感器的定位精度相匹配。算法仿真阶段栅格分辨率精度高于实际需要除了拖慢运行速度之外没有任何好处。第二个心得是A星代码写完之后一定要先跑一个最简单的地图做单元测试。比如在一个没有障碍物的空旷三维空间里起点和终点斜对角放置算法应该生成一条近乎直线的路径。这个测试能快速暴露启发函数、邻居生成、边界处理这些模块的问题比你直接跑复杂地形然后在一堆乱糟糟的路径里找bug要高效得多。第三个是路径平滑一定要放在碰撞检测之前但最终验证的时候一定要把平滑后的路径重新过一遍碰撞检测。我在实际项目中连续被这个问题坑了很多次B样条平滑虽然好看但安全验证永远要做在最后的最后这是工程习惯问题不是技术问题。无人机路径规划是一个非常典型的、把基础算法和工程需求结合起来的领域。A星算法本身不复杂难的是在真实环境下如何设计合理的代价函数、如何处理安全约束、如何让算法和实机硬件正确配合。希望这篇文章能够给刚踏入这个方向的朋友提供一份相对完整的参考。如果后面有机会我还会继续分享动态障碍物环境下的路径重规划、以及多无人机协同路径规划方面的实战经验。 
长河君 · 2026/10/1 12:00:44 · 阅读 1.2k Matlab实现三维A星无人机路径规划：从建图到平滑全解析
做无人机路径规划有一段时间了从二维A星一路踩到三维中间换了不下三版地图模型最后在Matlab里把三维A星跑通的那一刻说实话挺爽的。这篇文章我打算把整套思路和代码细节都摊开讲一遍从三维栅格怎么建、启发函数怎么选到Matlab里节点数据结构怎么组织、路径平滑怎么处理再到我实际调试中遇到的几个坑一次性说清楚。如果你正在做基于A星算法的无人机三维路径规划或者刚开始用Matlab做路径规划仿真这篇文章应该能帮你少走不少弯路。三维路径规划跟二维最大的不同不只是多了一个Z轴坐标那么简单。二维A星你只需要考虑四个方向或者八个方向的邻居扩展但三维空间里无人机的运动约束、地形遮挡、障碍物的空间分布全都变了节点的扩展方式、启发式估计、代价函数的设计都得重新想。更现实的问题是Matlab里三维栅格地图的存储和遍历比二维要费内存得多如果不做优化网格稍微密一点算法跑起来能把台式机的风扇转成直升机。在这篇文章里我会先把三维A星的核心设计思路拆开讲然后给出完整的Matlab实现包括地图构建、核心搜索、路径平滑、三维可视化这几个模块最后整理一份我自己调试过程中遇到的常见问题排查清单希望对你有实际帮助。1. 三维A星路径规划的整体设计思路1.1 从二维A星到三维A星变的不只是坐标系如果你已经写过二维A星肯定知道它的核心框架维护一个open list和一个closed list从起点开始每次从open list里取f值最小的节点出来扩展把邻居节点塞进open list直到终点被取出然后从终点倒退父节点重建路径。这套框架放到三维里依然成立但有几个关键点会变。首先是节点的邻居扩展。二维八方向扩展只需要修改当前节点的x和y坐标偏移量是[-1, 0, 1]的组合。到了三维如果做26邻域扩展就要同时考虑x、y、z每个维度上的1、0、-1组合总共3^3 - 1 26个邻居节点。26邻域的优点是路径更自由转弯更灵活但代价是每一次扩展的计算量涨了三倍多。如果只做6邻域扩展只沿坐标轴方向运动计算量小很多但路径会很僵硬无人机的运动会变得很不自然经常出现直上直下或者贴着障碍物走的轨迹。我实际测试下来对于多旋翼无人机这种运动比较灵活的平台用26邻域扩展是值得的。原因很简单多旋翼可以全向运动不像固定翼有最小转弯半径的限制26邻域生成的路径经过平滑之后更接近真实可飞轨迹。其次是代价函数的设计。二维A星里g(n)通常就是起点到当前节点的路径长度h(n)用曼哈顿距离或者欧几里得距离。三维空间里曼哈顿距离的问题会被放大因为多了一个维度曼哈顿距离对对角运动的估计偏差会更明显。所以在三维A星里我强烈建议用欧几里得距离作为启发函数也就是g(n) g(parent) 当前节点到父节点的实际距离 h(n) sqrt((x - goal_x)^2 (y - goal_y)^2 (z - goal_z)^2)欧几里得启发函数是consistency的也就是说它满足三角不等式这保证了A星算法在这个启发函数下一定能找到最优路径而且不需要重新打开closed list里的节点。1.2 为什么A星仍是无人机三维路径规划的优选方案可能有人会问现在深度学习、强化学习那么火为什么还在用A星这种传统算法我的理解是这样的路径规划问题本质上是带约束的搜索问题A星的优势在于它能在已知环境里给出最优解而且理论保证完备。深度强化学习适合的是环境未知、或者环境动态变化的场景但训练成本高、收敛不稳定而且很难证明系统的安全性。对于很多实际应用比如固定航线巡检、已知地图条件下的任务预规划A星仍然是落地方案里最稳的选择。和Dijkstra算法相比A星在启发函数的引导下搜索效率高出一大截。在三维栅格地图里Dijkstra会均匀向外扩展搜索空间是起点到终点的一个大球体而A星有了h(n)的引导搜索空间会被压缩成一个沿起点到终点方向的漏斗形。我做过对比在100x100x30的栅格地图里起点在左下前方、终点在右上后方Dijkstra需要扩展的节点数量大概是A星的三到五倍这个差距在三维环境里只会更明显。当然A星也有自己的弱点比如面对动态障碍物需要反复重规划但这些可以通过DWA局部规划或者A星的D*变种来补足不影响它在全局规划里的地位。2. 三维环境建模与核心参数设计2.1 三维栅格地图的Matlab表示方式做三维路径规划的第一步不是写算法而是先把地图建出来。我常用的方法是构造一个三维0-1矩阵0代表可通行1代表障碍物。假设飞行区域是500米x500米x100米栅格分辨率是5米那么地图的尺寸就是100x100x20。这里栅格分辨率的选择非常关键栅格太大路径会丢失很多可行空间甚至出现看起来能飞过去实际障碍物把通道堵死了的情况栅格太小地图存储和搜索的计算量都会爆炸。我自己的经验公式是对于巡检类无人机栅格大小取机体尺寸的1.5到2倍比较合适。比如轴距450mm的无人机栅格取1米左右就够用。如果只是做算法验证不需要绑定具体机型那栅格大小主要取决于你对路径精度的要求。三维栅格的地形可以用一个高度矩阵来模拟。常用的方法是生成多个高斯型山峰[X, Y] meshgrid(1:100, 1:100); Z zeros(size(X)); % 叠加几个山峰 Z Z 50 * exp(-((X-30).^2 (Y-40).^2) / 300); Z Z 35 * exp(-((X-70).^2 (Y-60).^2) / 400); Z Z 20 * exp(-((X-50).^2 (Y-20).^2) / 200);然后把Z矩阵栅格化凡是高于设定飞行高度的格点标记为障碍物。这个做法的好处是你可以直观看到一个包含了地形起伏的三维环境而不是一个纯随机的障碍物空间。实际工程里这个Z矩阵完全可以替换成数字高程模型DEM数据算法逻辑不用改。2.2 启发函数与代价函数的选定逻辑前面提过我推荐用欧几里得距离作为启发函数但实际写代码的时候还有几个细节。第一个细节是启发函数的代价权重。在某些场景下你希望无人机尽量走直线、少拐弯可以在h(n)前面乘上一个大于1的权重系数。比如h(n)乘1.5算法会表现得更贪婪搜索速度更快但代价是可能错过最优解。如果你做的是实时重规划这个特性反而有用因为你不一定非要全局最优路径能快速找一条可行路径更重要。但如果你的目标是理论最优路径权重系数就保持1.0。第二个细节是代价函数里要不要加入高度惩罚。无人机的能耗很大一部分来自克服重力做功所以飞得越高越费电。我通常会在g(n)的计算里加入一个高度项比如g(n) g(parent) 水平距离 垂直距离 * 爬升惩罚系数爬升惩罚系数取1.2到2.0之间这样算法会倾向于在相同路程下选择更低的路径减少能耗。这个设计在山区地形里效果特别明显。第三个细节是拐弯代价。无人机的飞行控制有个特性频繁转向会降低飞行效率甚至引发姿态震荡。可以在代价函数里加入转向角惩罚如果当前节点的扩展方向和上一段路径方向夹角过大就加一个惩罚值。但要注意方向信息的存储会让节点结构变复杂代码实现难度会上升如果你的需求没那么强可以先不加。2.3 安全约束离地高度与障碍物膨胀路径规划有一句老话规划出来的路径不是给你看的是给无人机飞的。所以路径上的每个点不仅要保证自己不落在障碍物栅格里还要保证无人机在飞行过程中机身不会蹭到障碍物。这就需要做障碍物膨胀处理。最简单的做法是形态学膨胀把三维地图里的每个障碍物栅格沿着x、y、z三个方向向外扩张若干个栅格。膨胀半径取决于无人机的几何尺寸和定位误差。我一般取机身半径的1.5倍再加一个栅格的定位余量。在Matlab里可以用imerode配合自定义的结构元素来做三维膨胀也可以用三重循环遍历实现。地图尺寸小的时候三重循环没问题但地图一大就得注意效率了后面我会专门讲性能优化的问题。除了障碍物本身的膨胀无人机还有一个最小离地高度的约束。这个不能简单地通过膨胀地形来实现因为如果你把地形整体抬高了那一些原本可以穿过的山谷通道就被封死了。更好的做法是在代价函数里增加一个地形贴地惩罚项当节点的Z坐标低于地形高度加上安全高度阈值时直接把g(n)设为一个很大的数让算法自动绕开。这样做既保证了安全又不至于过度保守。3. Matlab完整实现与代码架构拆解3.1 节点数据结构与地图初始化Matlab不是C写A星的时候数据结构的选择直接影响代码的简洁性和性能。我推荐用结构体数组来表示节点信息每个节点包含以下字段% 节点数据结构 node.g Inf; % 起点到当前节点的实际代价 node.h 0; % 当前节点到终点的启发估计 node.f Inf; % 总代价 g h node.parent []; % 父节点坐标用于回溯路径 node.closed false; % 是否已扩展 node.open false; % 是否已在open list中 node.xyz [x, y, z]; % 节点三维坐标我的做法是建一个三维cell数组或者三维结构体数组nodes_map{x, y, z}每一格保存这个节点对象。初始化的时候把所有节点的g设为Infclosed和open都设为false。% 地图参数 map_size [100, 100, 20]; % x, y, z方向的栅格数 resolution 5; % 每个栅格代表的实际距离米 start_node [2, 2, 5]; % 起点栅格坐标 goal_node [90, 95, 15]; % 终点栅格坐标 % 构建三维栅格地图obstacle_map obstacle_map generate_terrain_map(map_size); % 地形生成函数返回0-1矩阵open list的实现最简单的方式是用Matlab的数组来模拟优先队列每次从open list里找f值最小的节点。但这个方法在节点数量多的时候效率非常低因为每次查找都是O(n)。我在项目中测试过当扩展节点数超过一万个时这种线性查找的耗时占了整个算法的90%以上。更好的办法是用二叉堆。Matlab虽然没有内置的优先队列类但可以用Java对象接口Matlab底层支持调用Java的PriorityQueue。这是我实测很稳的一个方案父节点回溯也用结构体字段实现具体做法是每个节点保存父节点的索引等算法结束后从终点倒推。3.2 A星核心搜索循环下面这一版是我跑了很久的核心代码去掉了无关的判断逻辑保留了主干。while ~isempty(open_list) % 从open list取出f值最小的节点 [current_node, idx] extract_min(open_list); remove_from_open_list(open_list, idx); % 如果当前节点是终点回溯路径 if isequal(current_node.xyz, goal_node) path reconstruct_path(current_node); break; end current_node.closed true; % 生成26个邻居 neighbors generate_neighbors(current_node.xyz, map_size); for each neighbor in neighbors if obstacle_map(neighbor) 1 continue; % 跳过障碍物 end if neighbor_node.closed continue; end % 计算从起点经当前节点到邻居的代价 move_cost euclidean_distance(current_node.xyz, neighbor.xyz); tentative_g current_node.g move_cost altitude_penalty(neighbor, terrain); % 如果tentative_g更小更新邻居节点 if tentative_g neighbor_node.g neighbor_node.g tentative_g; neighbor_node.h euclidean_distance(neighbor.xyz, goal_node); neighbor_node.f neighbor_node.g neighbor_node.h; neighbor_node.parent current_node; % 如果邻居不在open list中加入 if ~neighbor_node.open add_to_open_list(open_list, neighbor_node); neighbor_node.open true; end end end end这段逻辑有几个值得注意的地方。第一检查邻居是否在closed list里的时机要放在代价计算之前放在后面会浪费大量计算。第二更新代价时要先判断tentative_g neighbor_node.g这是A星能保证最优路径的核心机制。第三在三维环境里euclidean_distance要写对三个坐标分量都要参与计算不能漏了z轴。如果你做的是26邻域扩展generate_neighbors函数里对z坐标的处理要特别注意。z方向是飞行高度邻居的z偏移可以取[-1, 0, 1]但实际飞行中无人机从低空到高空需要爬升从高空到低空需要下降这两个过程的代价在物理上是不对称的爬升能耗更大。如果希望路径更贴近真实飞行性能可以在z偏移为1时额外增加爬升代价。3.3 地形随动与碰撞检测的工程化处理三维环境里最容易被忽视的是路径贴着地形走的问题。纯A星搜索如果只把地形最高点当成障碍物生成出来的路径往往会在地形上方平飞。这看起来很安全但实际上很耗电而且如果遇到强侧风贴近地形的山脊处反而更危险。我的做法是在代价函数里增加一个地形随动因子。具体来说每个节点的g值会乘上一个系数这个系数跟当前节点离地高度有关height_above_terrain neighbor.xyz(3) * resolution - terrain_height(neighbor.xyz(1), neighbor.xyz(2)); if height_above_terrain safe_height tentative_g tentative_g 1e6; % 距离地面太近直接惩罚掉 elseif height_above_terrain cruise_height % 贴地飞行区域适当惩罚 tentative_g tentative_g 5; end这里有两点要说明一是terrain_height函数需要利用插值从DEM数据中获取x、y坐标对应的地形高度如果地图本身是栅格化的直接查表就行。二是cruise_height是期望的巡航高度你可以根据任务来设置比如巡检任务要求无人机在目标物上方20米左右的高度飞行那这个参数就设成20米。还有一个很实用的技巧碰撞检测不要只检测当前节点本身还要检测当前节点和父节点的连线是否穿过障碍物。在三维栅格地图里如果栅格分辨率比较大两个相邻节点连线穿越障碍物栅格是可能的。这个检测可以用Bresenham三维直线算法来实现判断连线经过的所有栅格是否有障碍物。这个细节在二维A星里很多人就忽略了到三维环境里问题会放大得更多因为空间里的障碍物分布比平面更复杂。我的实测经验是加上连线碰撞检测之后生成路径的可飞性大幅提升几乎不会出现路径穿模障碍物的情况。3.4 路径平滑后处理A星直接搜索出来的路径本质上是栅格中心点的连线会有很多折角直接给飞控执行飞机会表现得非常抽搐。所以路径平滑是不可缺少的后处理步骤。我常用的平滑方案是B样条曲线拟合法。Matlab里可以直接用spapi函数实现B样条插值% path_points是A星生成的路径点序列每一行是[x, y, z] k 3; % 三次B样条 spl spapi(k, 1:size(path_points,1), path_points); smooth_path fnplt(spl);这里的核心逻辑是把路径点按顺序归一化到参数轴上然后做三次B样条插值得到一条光滑的曲线。用B样条而不是简单的线性插值是因为B样条能保证曲线经过控制点附近不会偏离原始路径太多。不过B样条平滑有个问题平滑后的曲线可能会偏离原始路径甚至穿过障碍物栅格。解决办法是平滑完之后做一次碰撞检测如果检测到碰撞就把平滑曲线上的碰撞点替换回原始路径点或者增大B样条的节点密度让曲线更贴近原始路径。我实际项目中用的是冗余节点判断控制点权重调整的方法。具体来说B样条拟合时给每个路径点分配一个权重关键节点比如路径拐点、靠近障碍物的点权重调高让曲线在这些点附近更贴一些其他中间点权重降低让曲线更顺滑。这样既保证了安全性又保证了可飞性。4. Matlab代码的性能优化与调试技巧4.1 大数据集下Matlab的运行效率瓶颈很多第一次用Matlab写A星的人会遇到一个尴尬的情况地图稍微大一点程序跑半天也不出结果。这背后的原因是Matlab的数组操作是解释执行的循环里大量动态修改数组内容会触发反复的内存分配和类型判定。我踩过的坑是用[current, idx] min([open_list.f])来取最小节点这个操作本身不慢但问题是它返回的是索引然后你要通过这个索引去结构体数组里拿节点信息这一步在Matlab里的开销比我预想的要大得多。后来我换成了Java的PriorityQueueMatlab里可以直接这样调用pq java.util.PriorityQueue; pq.add(node_f); % 节点以[f, x, y, z]元组形式加入Java自带堆排序插入和删除最小元素都是O(log n)跟Matlab原生数组的线性查找比起来效率差距在节点数量上百后就开始显现了。在100x100x20的地图里A星往往需要扩展两三万个节点用Java优先队列可以把规划时间从几十秒降到几秒。还有一个小优化是提前把地图的障碍物检测结果保存成一个三维逻辑数组不要在A星循环里重复调用地形生成函数。地形的计算量很大如果每次扩展节点都重新算一遍时间全浪费在重复计算上了。4.2 A星搜索异常行为排查我在调试过程中遇到过几个比较典型的异常现象这里集中整理一下。第一个异常是路径起点和终点明明都可达但算法就是找不到路径。最可能的原因是终点被标记成了障碍物。检查方式是在main函数开头打印一下obstacle_map(goal_x, goal_y, goal_z)的值如果是1那就是地图生成逻辑有bug。另外还有一种可能是你的邻居生成函数在边界处漏掉了某些方向导致搜索无法到达终点。三维边界处理比二维复杂四个边变六个面很容易漏写。第二个异常是生成的路径穿过了障碍物。这个我之前提到过多半是因为没有做连线碰撞检测。还有一个隐蔽的原因是路径平滑阶段B样条把路径拉偏了。调试的时候可以先不启用平滑看原始路径是否安全如果原始路径安全而平滑后穿模问题就出在平滑参数上。第三个异常是算法运行时间特别长而且路径不远。这个大概率是启发函数设置有问题。如果你把h(n)设成了0A星就退化成Dijkstra搜索范围会大很多。检查一下h sqrt(...)那行是不是写对了z轴分量是不是漏掉了。第四个异常比较有意思是路径来回折返。A星正常情况下不会产生折返路径除非你的代价函数里加入了某些奇怪的惩罚项导致绕远路比走直线更划算。我遇到过的情况是爬升惩罚系数设得过大算法宁可水平绕一大圈也不肯爬升10米生成的路径看起来非常离谱。解决办法是控制爬升惩罚系数的取值范围别超过2.5。4.3 从仿真到实机部署的三点建议仿真跑通了下一步必然是往实机上搬这里说三个我实际碰到的坎。第一仿真里的栅格地图到实机导航坐标系有一个对齐问题。仿真里你用的是行列号实机里你需要通过经纬度、高度来换算到局部坐标系再换算到栅格坐标系。这一步如果没做好算法算出来的路径在地图上是对的但无人机实际飞起来完全对不上。建议在代码里封装一个坐标转换模块单独测试这个模块的精度和稳定性。第二A星规划出来的路径是几何路径不包含时间信息和速度信息。实机部署时必须把路径点发送给飞控的航点执行模块由机载SDK或飞控固件完成航点间的速度规划和轨迹跟踪。这里要特别注意路径点间距不能太大否则飞控在航点之间走直线的时候可能会切掉拐角导致偏离规划路径。我一般是把A星路径点稠密化每隔一两个栅格插值出一个中间点再发给飞控。第三仿真里不考虑风的干扰但实机飞行的风是常态。如果无人机在强风环境中飞行A星规划出的路径可能无法精确跟踪。这种情况下需要增加一个局部避障模块比如用激光雷达或视觉传感器做实时障碍物检测发现前方动态障碍物时在A星全局路径的基础上做局部绕行。A星负责全局最优局部模块负责避障应急两者配合才能构成一个完整可部署的系统。5. 三维A星结果可视化与任务场景扩展5.1 Matlab三维轨迹可视化实现路径规划算法跑完了如果没有一个清晰的三维可视化调试起来会非常费力。我用的是Matlab的三维绘图函数做整体展示效果比较直观。% 绘制地形表面 surf(X * resolution, Y * resolution, Z, EdgeColor, none, FaceAlpha, 0.6); hold on; % 标记起点和终点 scatter3(start_xyz(1), start_xyz(2), start_xyz(3), 100, g, filled); scatter3(goal_xyz(1), goal_xyz(2), goal_xyz(3), 100, r, filled); % 绘制规划路径 plot3(path(:,1), path(:,2), path(:,3), b-, LineWidth, 2); % 绘制平滑后路径 plot3(smooth_path(:,1), smooth_path(:,2), smooth_path(:,3), r--, LineWidth, 2); legend(地形, 起点, 终点, A星路径, B样条平滑路径);有几个细节值得注意。FaceAlpha参数控制地形表面的透明度设成0.5到0.6比较合适既能看清地形起伏又不会完全遮挡路径。LineWidth设成2路径曲线在图形窗口里会更醒目。另外建议把坐标轴的纵横比设成等比例否则z轴被拉伸或压缩之后你很难判断路径的实际坡度变化axis equal;如果地图范围很大还可以添加视角控制用view([-30, 20])设置一个倾斜视角方便观察三维路径的空间走向。5.2 从静态全局规划到动态重规划A星本质上是静态全局规划算法但很多任务场景要求无人机在飞行过程中根据环境变化动态调整航线。我尝试过的一种有效方案是把A星作为全局规划器每隔一段时间比如5秒在无人机当前位置和目标点之间重新规划一次完整路径。对于中等规模的地图A星的规划时间在秒级以内完全满足周期性重规划的时间要求。如果环境变化特别频繁这种全局重规划的计算开销就会比较大这种情况下可以把重规划范围限制在局部窗口内只重新规划无人机前方一定距离内的路径这样既能保证对环境的响应速度又不会频繁推翻全局路径。另外一个思路是把A星扩展到动态场景比如用D* Lite算法但实现复杂度会明显上升。如果你只是做毕设或者工程验证A星加周期重规划已经完全够用了。5.3 结合Google Earth或者真实DEM数据的扩展方向如果你的项目需要更高的真实感可以把Matlab里模拟的数学地形换成真实DEM数据。网上有很多公开的DEM下载渠道文件格式一般是GeoTIFF。Matlab里用readgeotiff函数就能读取DEM数据然后直接替换掉我之前代码里的generate_terrain_map函数。这里有个坑我必须提醒你真实DEM数据的分辨率通常很高比如30米分辨率但在小范围三维路径规划里可能不够精细。你需要根据飞行区域的大小和栅格规划需求对DEM进行重采样。重采样方法在Matlab里可以用interp2或resample函数实现重采样成和你的栅格地图分辨率匹配的尺寸。还有一点真实DEM数据的坐标系是地理坐标系包含经纬度信息。在做路径规划之前先把经纬度坐标投影到平面直角坐标系建议用UTM投影或者本地高斯投影。如果你的飞行范围不大用简单的近似等距投影也能保证精度。把真实地形接入路径规划系统之后整个项目的应用前景会开阔很多比如山区电力巡检的航线预规划、灾后区域的无人机物资投送路径规划、林业巡检的无人机全覆盖路径规划。A星作为全局搜索的核心模块和这些业务场景做组合能发挥的价值比单纯做一个仿真实验要大得多。6. 常见问题速查表与最后的几点经验6.1 三维A星常见问题速查表我把调试过程中遇到的高频问题汇总成一个表方便读者对照排查。现象可能原因排查方法算法找不到路径终点是障碍物、邻居生成不完整打印终点栅格值检查6个边界面的邻居索引规划时间过长open list线性查找、地图过大换Java优先队列缩小地图范围或增大栅格尺寸路径穿入障碍物缺少连线碰撞检测、平滑曲线偏移加Bresenham连线检测调整B样条权重路径非常曲折启发函数偏差、障碍物形态复杂检查h计算式确认z轴参与计算路径绕远路惩罚系数设置不当降低爬升惩罚系数到1.5以下路径高度剧烈波动地形随动因子设计问题给贴地区域设置硬惩罚优化高度惩罚曲线6.2 我实际踩过的坑与心得写到这里最后分享一点个人的体会。第一次做三维A星的时候我踩的最大的一个坑就是盲目追求地图精度把栅格分辨率设得非常细结果Matlab内存直接爆掉程序还没跑起来就崩了。后来才意识到栅格分辨率不是越高越好它需要跟无人机的实际尺寸、传感器的定位精度相匹配。算法仿真阶段栅格分辨率精度高于实际需要除了拖慢运行速度之外没有任何好处。第二个心得是A星代码写完之后一定要先跑一个最简单的地图做单元测试。比如在一个没有障碍物的空旷三维空间里起点和终点斜对角放置算法应该生成一条近乎直线的路径。这个测试能快速暴露启发函数、邻居生成、边界处理这些模块的问题比你直接跑复杂地形然后在一堆乱糟糟的路径里找bug要高效得多。第三个是路径平滑一定要放在碰撞检测之前但最终验证的时候一定要把平滑后的路径重新过一遍碰撞检测。我在实际项目中连续被这个问题坑了很多次B样条平滑虽然好看但安全验证永远要做在最后的最后这是工程习惯问题不是技术问题。无人机路径规划是一个非常典型的、把基础算法和工程需求结合起来的领域。A星算法本身不复杂难的是在真实环境下如何设计合理的代价函数、如何处理安全约束、如何让算法和实机硬件正确配合。希望这篇文章能够给刚踏入这个方向的朋友提供一份相对完整的参考。如果后面有机会我还会继续分享动态障碍物环境下的路径重规划、以及多无人机协同路径规划方面的实战经验。