最近在做一套低空物流无人机的路径规划Demo核心算法选的是教科书里最经典的标准A星。说白了就是给无人机在栅格地图上找一条从起点到终点的无碰撞路径听起来简单但真正落地到无人机飞行场景时网格怎么建、启发函数怎么选、参数怎么调每一步都有不少值得掰开讲的东西。这篇文章把我从头到尾的思路、实现细节和踩过的坑都整理出来适合正准备入无人机路径规划的开发者也适合已经会用A星但想把它做得更贴合飞行任务的朋友参考。1. 项目背景与选型逻辑为什么是标准A星1.1 需求描述先求有路再求最优项目需求很朴素一台多旋翼无人机需要在一个中型园区内自主飞行从停机坪起飞绕过几栋楼、几排树和一些临时围挡降落到目标点。地图由实际环境预先构建障碍物区域已知飞行高度固定所以问题可以约化为二维平面上的路径规划。为什么要强调“先求有路再求最优”因为实际飞行对路径的第一要求是安全、可执行而不是数学意义上的最短。很多人在入门时急着上各种高级算法但连一条“能飞的路”都还没稳定跑通后面所有优化都无从谈起。标准A星天然适合这个阶段它确定、可控、可解释每一步扩展都能追溯出了问题也容易定位。我在项目里把它当基准路径生成器和后面的平滑模块、速度规划模块解耦。A星只负责给出“不撞障碍的粗略路线”后续再对转折点做处理。这样做的收益是即使后面要换成其他算法代价都只集中在一个模块内部。1.2 为什么不上RRT、JPS这些“更厉害”的算法刚开始我也犹豫过要不要直接上RRT快速探索随机树或者用JPS跳点搜索来提升性能。后来还是回到标准A星原因是RRT适合高维空间和连续状态空间但在二维栅格上它反而会引入随机性路径抖动大且不保证最优需要额外做平滑和剪枝。JPS在规则栅格上确实比A星快很多但它的实现细节多跳点规则容易写错而且对环境变化更敏感。标准A星在中小尺寸栅格上的性能完全够用比如500×500的栅格只要结构写对单次规划时间在毫秒级完全满足我当前的需求。还有一个更实际的理由在项目早期我需要一个绝对可信的“基准线”来验证建图是否正确。如果路径规划算法本身引入了不确定性出了问题就分不清是地图的问题还是算法的问题。标准A星在这个角色上非常称职。2. 标准A星的核心拆解fgh背后的实际含义2.1 状态空间与fgh的含义标准A星的核心公式是f(n) g(n) h(n)但很多初学者只记公式没理解每个量在无人机场景下的物理意义。g(n)从起点走到当前节点n已付出的实际代价。对无人机来说这个代价可以是飞行距离也可以是预估能耗。h(n)从当前节点n到终点的估计代价。它是启发式估计不要求精确但要求不高估真实代价否则会丢失最优性。f(n)从起点经过n再到终点的总代价估计。A星每次从开放列表中取出f值最小的节点扩展直到终点被弹出。可以类比成一个送货问题你已经走了5公里g目测还有3公里到目的地h总预计要跑8公里f。A星会优先尝试那些总预计最短的路线而不是只看已经走了多远。在无人机路径规划中这里的“距离”可以用物理距离也可以用能耗模型。标准A星本身不关心代价值怎么定义只要满足非负且可相加即可。这给后面调优留了很大空间。2.2 启发函数的选择曼哈顿距离还是Octile距离这是最容易被忽略、影响却很大的一个选择。很多教程直接写曼哈顿距离abs(dx)abs(dy)但那是给四邻域移动设计的。如果无人机允许斜向飞行再用曼哈顿距离就会高估真实代价A星会失去最优性保证跑出来的路径可能明显不自然。八邻域移动对应的启发函数叫Octile距离公式是h(n) min(dx, dy) * 1.414 abs(dx - dy)其中dx abs(n.x - goal.x)dy abs(n.y - goal.y)。这个公式的含义是先尽量斜着走剩下的部分再走直线对应八邻域下的最短可能路径。这个细节直接决定路径质量。我用一个简单例子验证过起点(0,0)、终点(5,5)如果所有格子可通行八邻域下最优路径就是斜着走5步代价约7.07。如果用曼哈顿距离做启发估计值是10远超真实代价算法就会优先向“看起来更近”的横向/纵向扩展导致搜索范围明显变大甚至可能因为高估而给出非最短路径。换成Octile距离后估计值等于真实值算法几乎一路直奔终点扩展节点数大幅下降。2.3 开闭表与路径回溯的标准实现直接给一个可运行的核心伪代码实现。这里用Python风格的伪代码重点是结构不是语法糖import heapq def octile_distance(a, b): dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) return min(dx, dy) * 1.414 abs(dx - dy) def reconstruct_path(came_from, current): path [current] while came_from[current] is not None: current came_from[current] path.append(current) path.reverse() return path def a_star(grid, start, goal, w1.0): # 开放列表用最小堆保持f值最小的节点在堆顶 open_heap [] counter 0 # 用于打破相同f值时的平局 heapq.heappush(open_heap, (0.0, counter, start)) came_from {start: None} g_score {start: 0.0} closed_set set() while open_heap: _, _, current heapq.heappop(open_heap) if current goal: return reconstruct_path(came_from, current) if current in closed_set: continue closed_set.add(current) for neighbor, move_cost in neighbors(grid, current): if neighbor in closed_set: continue tentative_g g_score[current] move_cost if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f tentative_g w * octile_distance(neighbor, goal) counter 1 heapq.heappush(open_heap, (f, counter, neighbor)) return None # 无可行路径几点说明closed_set用Python的set查找O(1)比列表快得多。open_heap用最小堆每次弹出f值最小的节点不用全列表扫描。counter字段很重要。当两个节点f值相同时完全相同的元组比较会导致比较错误或跳过节点增加一个自增序号可以安全避免。路径回溯从终点沿着came_from一路回到起点再反转就是最终路径。3. 栅格地图建模决定路径质量的第一层关卡3.1 栅格化流程从障碍到可通行标记无人机飞行时拿到的不是整齐的格子而是一堆障碍物轮廓、高度数据。我是这样处理成A星能用的栅格地图的把园区地图按固定分辨率栅格化比如每格0.5米×0.5米。每个格子根据是否有障碍物标记为0可通行或1障碍。无人机固定飞行高度所以只考虑二维投影遇到高于飞行安全高度的物体才标记为障碍。把障碍物边界向外扩展这个环节后面单独说。栅格粒度直接影响结果。粒度太粗窄通道会被吞掉路径可能不存在粒度太细地图巨大搜索速度下降。0.5米分辨率对一个翼展约0.6米的小型无人机来说是一个比较合理的起点但最终要根据实际飞行环境反复调整。3.2 八邻域和对角穿墙检测允许斜向移动后必须考虑一个经典问题斜着从障碍物的角上穿过去。假如当前节点在(0,0)目标对角节点在(1,1)而(0,1)或(1,0)是障碍物那这条斜线实际上是在“擦边”走一个没有完全膨胀的障碍物边缘可能会刮到无人机。解决方式是在生成邻居时增加一次阻挡判断def neighbors(grid, node): x, y node rows, cols len(grid), len(grid[0]) # 四邻域 for dx, dy in [(1,0), (-1,0), (0,1), (0,-1)]: nx, ny xdx, ydy if 0 nx rows and 0 ny cols and grid[nx][ny] 0: yield (nx, ny), 1.0 # 八邻域对角移动 for dx, dy in [(1,1), (1,-1), (-1,1), (-1,-1)]: # 检查相邻的两个正交方向是否可通行 if grid[xdx][y] 0 and grid[x][ydy] 0: nx, ny xdx, ydy if 0 nx rows and 0 ny cols and grid[nx][ny] 0: yield (nx, ny), 1.414这里的判断逻辑是斜向移动被允许的前提是两侧的正交邻居都可行。换句话说无人机不是“穿墙”过去的而是从一个开放空间斜向滑进另一个开放空间。3.3 膨胀半径把无人机当成一个有体积的物体这是所有路径规划里最容易忽略、却至关重要的一步。A星规划出的路径只是一条几何线但无人机是有物理尺寸的。如果直接用原始障碍物地图规划路径很可能从距离墙壁5厘米的地方掠过现实中早就撞上了。我的做法是在规划前对栅格地图执行形态学膨胀把所有障碍物向外扩展若干格。膨胀半径需要考虑几个因素因素说明无人机半翼展/半径最基本的物理尺寸定位误差GPS或室内定位系统的偏差控制误差飞行控制器跟踪路径时的横向偏差安全距离根据飞行环境和任务要求额外保留的裕度实际项目中如果无人机半径0.3米、定位误差0.2米、控制误差0.2米再加上安全的0.3米膨胀半径大约1.0米。在0.5米分辨率的栅格上就是向外扩2格。这个裕度宁多勿少因为路径规划更差一点可以接受撞上障碍物则是任务失败。提示膨胀阶段的半径过大虽然安全但可能导致窄通道完全被堵死路径规划直接返回“无解”。先检查膨胀后地图的连通性再决定是否调整分辨率。4. 无人机调参实战权重、转弯代价与平滑处理4.1 启发权重w更快但不保证最优标准A星里h(n)前面经常乘一个权重w这就是加权A星。w1.0时是标准最优A星w1时搜索更快但路径可能是次优的。为什么要在这个项目里讨论w因为无人机平台的算力有时受限而且任务场景下的路径不必是严格最短。实际测试中w1.2左右可以在几乎不影响路径长度的前提下减少大约20%到30%的扩展节点规划速度明显提升。但要注意w不是越大越好。w过大时算法会过度偏向终点方向容易被局部障碍骗进死胡同反而需要回头扩展大量节点甚至在极端情况下给出明显绕路的路径。我在测试w2.0时多次出现无人机贴着障碍物绕行的“蠢路径”看起来完全不像经验丰富的飞行器该走的路线。建议从w1.0开始在保证最优路径可用的前提下逐步提高每次跑同一组测试用例对比规划时间和路径长度找到一个对当前场景最合适的折中值。4.2 避免高频转向代价函数与平滑标准A星规划出的路径经常是“锯齿状”的尤其是在复杂障碍环境中一个个45度转角连接起来无人机如果要完全照飞就会频繁横向摆动不仅能量消耗大姿态传感器在这时候也容易震荡。我在项目中做了两层处理第一层在代价函数中加入微小的转向惩罚。标准A星的节点状态只有坐标并不知道无人机从哪个方向来所以严格意义上无法精确计算转向代价。一个变通做法是把状态空间从(x, y)扩展为(x, y, heading)其中heading表示上一段的移动方向。这样在计算g(n)时可以判断当前移动方向是否和上一方向一致不一致就追加一个额外代价。代价扩展后的节点数量变成原来的8倍但换来的是更平滑的路径。第二层做路径后处理平滑。把A星输出的粗略路径作为控制点做三次样条插值或者保留短路优化。我的做法是从起点开始尝试跳过中间节点如果两点连线不经过膨胀后的障碍物就删掉中间节点继续向前尝试。这个过程能把一段20个节点的锯齿路径压缩到五六个关键的直线段飞行姿态稳定很多。4.3 实时重规划的时间预算标准A星是静态规划算法但无人机实际飞行时几乎总会遇到动态变化——临时出现的人、移动的车辆、突发的风场影响。我当时的应对很简单把规划模块做成可重入的以每秒钟几次的频率检查地图变化一旦发现原路径上出现了新的障碍节点就立即以当前位置为起点、原目标为终点重新规划。这里有个时间预算的坑在较大的地图上重新规划不能卡顿太久否则无人机悬停等待时已经在漂移了。解决方案是限制单次规划的最大执行时间如果超时就用上一次成功的路径先维持飞行等待重新规划完成。标准A星在500×500栅格、8邻域、未加权的情况下单次规划通常不超过几十毫秒所以通过代码层面的优化实时重规划的压力并不大。5. 仿真测试与踩坑记录三个影响飞行效果的问题5.1 基础用例与基准结果我在仿真环境里布置了几组典型场景普通障碍随机分布、U形障碍通道、窄缝通道、大范围空旷区域加少量点状障碍。每组场景都记录规划时间、路径长度、转折点数量三个指标。基准数据如下表用例栅格尺寸起点到终点直线距离规划耗时转折点数说明简单障碍300×300280格8ms6路径合理U形障碍400×400180格25ms12绕行正确窄缝通道400×400120格30ms5未卡死空旷区域500×500450格3ms2基本直线这些数据说明了标准A星在常规场景下性能足够好真正的问题并不在算法本体而在使用细节。5.2 坑一对角“擦边”路径第一次跑完仿真直接看路径最明显的问题是很多转折点距离障碍物的角非常近。当时我还没有加对角穿墙判断路径会从两个对角相邻的障碍物之间斜穿过去视觉上就是一个“擦着墙皮走”的路线。排查过程其实不复杂我在某个出现擦边路径的节点处打印邻居信息发现算法在生成斜向邻居时根本没有检查相邻正交格子。加完grid[xdx][y] 0 and grid[x][ydy] 0这个判断后擦边现象立刻消失。这个坑给所有做栅格路径规划的人提了个醒八邻域不是简单地把8个方向都列出来就叫八邻域斜向移动是有前提条件的。5.3 坑二转折点过多导致横滚反复仿真中有一段路径命中了一串交替的45度转向无人机按路径飞行时横向姿态来回切换飞控日志里能看到期望横滚角在短时间内反复快速变化。虽然路径在几何上是合法的但飞起来非常难受。这个问题在纯A星层面很难彻底解决因为它产生的原因是栅格离散化后的折线本质。我最终的方案是规划后加平滑和关键点压缩把路径拆成更少的大转折段然后在每个转折点附近用一段圆弧过渡。这样既保留了A星安全有界的优点又满足实际飞控的平滑性要求。5.4 坑三固定网格粒度与任务不匹配有一段时间我一直在0.5米分辨率上调试没有思考这个值是不是合理。后来在仿真里放了一条只有0.4米宽的小通道0.5米分辨率的栅格直接把通道格子标记成了障碍路径规划返回无解。我最初以为是膨胀半径设大了调整膨胀参数后依然无解这才意识到是分辨率的问题。把栅格分辨率改成0.3米后通道被正确识别A星也顺利找到了路径。这个教训说明建图和规划是一个整体不能只调规划算法建图分辨率拉跨了再好用的A星也无能为力。6. 跑通标准A星之后可扩展方向与我的建议6.1 双向A星和JPS提升规划速度的经典思路如果地图特别大标准A星的扩展节点数会成为瓶颈。两个经典优化方向是双向A星和JPS跳点搜索。双向A星的本质是从起点和终点同时向外搜索让两棵搜索树在中间相遇。这么做的收益是在高维或大尺寸栅格中搜索面积从圆形变成更小的纺锤形需要处理的节点数量会明显少于单向A星。JPS则是在规则栅格中利用了“直路无分支”的特性只扩展有“跳点”的方向跳过大量方向单调的格子。在开阔区域JPS带来的加速非常显著可以轻松达到标准A星几倍到十几倍的速度。代价是代码复杂度上升且地图必须是规则栅格不规则代价场里它发挥不出来。6.2 D* Lite处理动态障碍的增量式解法标准A星在静态地图上表现很好但一旦地图频繁变化每次都全量重规划会造成浪费。D* Lite这类增量式算法可以复用上一次规划的搜索结果只更新受影响的部分适合无人机在未知环境中边飞行边建图边规划的场景。从标准A星过渡到D* Lite有一个比较好的路径先把A星的开放列表、节点代价这些概念吃透再去理解D* Lite的rhs值、优先队列更新逻辑会平滑很多。直接上手D* Lite往往会陷入各种队列更新细节里出不来。6.3 三维空间与能耗代价模型如果后续任务需要无人机跨高度飞行二维栅格A星可以扩展为三维栅格节点从(x, y)变成(x, y, z)八邻域变成二十六邻域。启发函数从二维Octile距离扩展成三维版本。代码结构不变但地图数据量和计算量会成倍增长这时候就需要考虑JPS或分层规划了。回到这个项目本身我个人最大的体会是标准A星的价值不在于算法有多高级而在于它足够简单、足够可靠可以把复杂的真实问题一层层剥出来让建图、膨胀、平滑、飞行控制这些环节都分得清清楚楚。如果你也要做无人机路径规划我强烈建议先把标准A星按这个思路完整跑通一次再往更花哨的方向走。这个基础打牢了后面换算法、加约束、调参数都会非常有底气。