简介面向无人机路径规划与导航的Python项目以A星算法为核心适合希望在无人机自动巡检、物流配送等场景中实现高效避障路径规划的开发者也可作为路径规划算法学习与课程设计的参考。压缩包共二十二个文件核心为六个源码文件与七个编译文件另有四个配置、两张规划结果图及若干工程文件整体仅两百余KB轻量便于快速阅读。目前已有五千五百三十四人学习下载具备一定参考价值。项目采用模块化设计完整覆盖地图构建、节点管理、启发式函数、开放与关闭列表搜索及最优路径回溯等关键环节并划分主控、规划、绘图、工具等脚本模块方便逐段理解A星实现细节同时提供多任务相关扩展脚本与可视化图片便于对比单机与多任务规划的差异适合在此基础上进行二次开发和算法优化。通过对A星估价函数与搜索过程的拆解还能加深对启发式算法设计与数据结构运用的理解。 无人机从“会飞”到“会自己找路飞”中间隔着一整套路径规划与导航的知识体系。很多人从组装硬件、调飞控参数入门折腾到最后发现电机选型再合理、PID再稳飞机也只会按遥控指令运动换个未知环境就抓瞎。真正让无人机从“遥控玩具”变成“自主机器人”的核心其实是路径规划与导航。这篇内容我会围绕Python实现路径规划与导航这条主线把环境建模、全局规划、局部避障、实机部署这些环节逐一拆开讲并结合我自己调试过程中的实际经验给出一套可以直接上手的实践路径。适合正在做无人机毕设、准备机器人比赛或者刚接触自主导航想快速入门的开发者参考。1. 先把“路径规划”和“导航”这两个概念彻底分清很多初学者会把“路径规划”和“导航”混为一谈觉得导航就是路径规划路径规划就是导航。真做起来才会发现这俩根本不是一回事搞混了后面全是坑。1.1 路径规划是“想”导航是“走”路径规划解决的是“从A点到B点走哪条路”的问题本质是在地图上搜索一条满足约束条件的路径。约束条件包括避障、路径最短、能耗最低、转弯次数最少等等。它不关心飞机当前的速度、姿态、实际位置偏差只关心“路线几何形状”。导航解决的是“怎么沿着这条路走”的问题它需要把规划出来的路径转换成无人机能执行的指令。导航系统至少要包含定位我现在在哪、路径跟随沿着路径怎么飞、避障中途出现障碍物怎么办三个环节。换句话说路径规划是导航系统里的一个子模块而导航是一个完整的闭环控制系统。我用一个生活化的类比路径规划相当于你用地图App查“从家到公司怎么走”导航相当于你开着车实际走这条路边开边看路况、边调整车速和方向。没有规划你只能原地打转没有导航规划出来的路线也只是一张没法执行的图。1.2 全局规划与局部规划的分工逻辑路径规划又细分为全局规划和局部规划两套体系它们的分工可以用“远见”和“本能”来理解。全局规划Global Planning基于已知的静态地图提前计算出一条从起点到终点的粗略路线。它假设环境是已知的、障碍物是静止的因此计算出来的路径在全局视角下是最优或接近最优的。常见的算法有Dijkstra、A*、RRT等。全局规划的缺点也很明显——它对地图的准确性要求高一旦实际环境与地图不一致这条路径就可能失效。局部规划Local Planning是在无人机实际飞行过程中根据传感器实时感知的数据对全局路径进行动态修正用于应对地图中没有的障碍物和运动中的障碍物。常见算法有动态窗口法DWA、时间弹性带算法TEB、人工势场法等。局部规划只关注无人机前方几米到几十米范围内的局部区域计算频率高要求响应速度快。在一个完整的导航系统里这两者是配合使用的全局规划负责“大方向”局部规划负责“避突发”。如果只做全局规划不做局部规划无人机撞上突然出现的行人如果只做局部规划不做全局规划无人机大概率会陷入局部最优——在原地绕来绕去走不出来。1.3 一套完整导航系统的组成基于Python搭建一套可用的无人机导航系统通常需要这几个模块传感器驱动通过串口、USB或网络协议读取IMU、GPS、激光雷达或视觉传感器的原始数据。状态估计融合多传感器数据推算无人机当前的位姿位置和姿态。工程上常用EKF扩展卡尔曼滤波或者直接使用PX4飞控自带的SDK来获取估计结果。地图模块维护一张描述环境的地图可以是二维栅格地图、三维八叉树地图或拓扑地图。全局规划器在地图上搜索全局路径。局部规划器根据实时传感器数据修正轨迹。控制接口向飞控发送期望速度或期望位置指令。我最初做的时候把这几个模块全部塞进一个Python脚本里代码又臭又长一改就崩。后来按模块拆开以后调试效率和可复用性完全不一样了。我的建议是哪怕只是练手也尽量按模块化思路来写后面接ROS、接真机都方便。2. 环境建模让无人机先学会“看懂”空间路径规划的第一步不是写算法而是先把环境表示出来。环境建模的好坏直接决定规划算法的效率和可行性。2.1 栅格地图是最容易上手的空间表示栅格地图Occupancy Grid Map把空间划分成一个个大小相同的格子每个格子标记为空闲、占用或未知。它的思想非常简单就像一个棋盘飞机不能落在有棋子的格子棋子就是障碍物。栅格地图有两个核心参数地图分辨率cell size每格代表实际空间的大小比如0.1米/格。分辨率越高地图越精细但计算量成平方增长。地图尺寸长宽对应的格数比如一个50米×50米的区域分辨率0.1米那就是500×50025万个格子。用Python实现一个栅格地图非常简单本质上就是一个二维数组。我贴一个最基础的地图生成和可视化代码import numpy as np import matplotlib.pyplot as plt # 创建 50m x 50m 地图分辨率 0.1m 500 x 500 栅格 grid_size 500 grid_map np.zeros((grid_size, grid_size), dtypenp.int8) # 在地图中间画一个长方形障碍物占满格子 grid_map[200:300, 250:300] 1 # 在地图右下角画一个圆形障碍物 cx, cy, radius 420, 400, 40 for i in range(grid_size): for j in range(grid_size): if (i - cx)**2 (j - cy)**2 radius**2: grid_map[i, j] 1 # 可视化 plt.imshow(grid_map, originlower, cmapgray_r) plt.title(2D Occupancy Grid Map) plt.show()这段代码会生成一张带两个障碍物的简单栅格地图。实际工程中地图数据通常来自激光雷达或视觉SLAM算法。比如ROS生态里常用的map_server节点就是加载一张PGM图片以及对应的YAML配置文件然后发布成nav_msgs/OccupancyGrid消息路径规划节点订阅这个消息就能获取地图数据。如果你用Python独立实现不走ROS也可以直接读取图片作为地图。常用做法是用OpenCV读取一张二值化图片黑色像素当作占用格白色像素当作空闲格。这样你可以用任何地图编辑工具手绘障碍物布局非常直观cv2.imread(map.png, cv2.IMREAD_GRAYSCALE)然后通过cv2.threshold把灰度图转成二值图再映射成栅格数组。2.2 栅格地图的膨胀处理这是新手最容易忽略的一步直接拿原始栅格地图去做规划算法大概率会规划出一条贴着墙走的路线。原因很简单规划算法把无人机当成一个“质点”而实际无人机是有尺寸的。解决方法是膨胀inflation——把障碍物周围的格子也标记为不可通行膨胀半径至少是无人机半径。我以前用A*算法做全局规划时路线贴着障碍物边缘走实际飞的时候螺旋桨几乎蹭到树枝。后来在规划前先做膨胀处理把障碍物周围3到5个格子全部标记为占用路径质量立刻改善。膨胀的核心代码逻辑如下from scipy.ndimage import binary_dilation # obstacle_map 是二值栅格1表示障碍物 obstacle_map grid_map 1 # 膨胀5个格子 inflated binary_dilation(obstacle_map, iterations5) # 把膨胀后的区域合并回地图 inflated_map grid_map.copy() inflated_map[inflated] 1膨胀的本质是把无人机的物理尺寸因素考虑进搜索空间。理论上当膨胀半径等于无人机最大外接圆半径时规划出来的路径就等价于“质点能通过的路径”。间距较小的两个障碍物如果中间缝隙小于无人机直径膨胀以后会变成一个连通区域算法自然就能判断出不可通行。另外还有一种决策就是把地图从栅格地图转成其他形式。栅格地图在空旷场景下浪费大量内存因此有人用四叉树地图有人用概率路图PRM的预采样思路。但在入门阶段我强烈建议先老老实实用栅格地图。它逻辑清晰、调试直观、可视化方便后续做性能优化时再引入复杂结构。3. 三种主流路径规划算法的Python实现与对比环境建模完成之后就进入核心环节——路径规划算法。我挑三种最常用、也最适合入门的算法来讲Dijkstra、A*和RRT。它们分别代表了广度优先搜索、启发式搜索和随机采样规划三种思路理解这三兄弟后面看再复杂的算法都有底气。3.1 Dijkstra稳定但偏慢的“无差别扩张”Dijkstra算法的核心思想是从起点开始一层一层向外扩展每次都找代价最小的格子作为下一跳候选直到扩展到终点。它不做任何方向性的引导所以最终一定能找到最短路径前提是存在这样一条路径。用Python实现时最简版本是用一个优先队列heapq来维护候选节点import heapq def dijkstra(grid, start, goal): grid: 0可通行1障碍物start/goal均为(x, y)元组 rows, cols grid.shape open_set [(0, start)] came_from {start: None} cost_so_far {start: 0} neighbors [(1,0), (-1,0), (0,1), (0,-1)] while open_set: current_cost, current heapq.heappop(open_set) if current goal: break for dx, dy in neighbors: nx, ny current[0]dx, current[1]dy if 0 nx rows and 0 ny cols and grid[nx, ny] 0: new_cost cost_so_far[current] 1 # 每步代价为1 if (nx, ny) not in cost_so_far or new_cost cost_so_far[(nx, ny)]: cost_so_far[(nx, ny)] new_cost heapq.heappush(open_set, (new_cost, (nx, ny))) came_from[(nx, ny)] current # 从goal回溯路径 path [] node goal while node is not None: path.append(node) node came_from.get(node) return path[::-1]这段代码在每步代价为1的情况下等价于广度优先搜索的最优解版本。实际中如果每个格子的真实通行代价不同比如草地通行代价高于硬地只需改new_cost的计算公式即可。Dijkstra最大的问题是搜索范围太广。在一个500×500的地图上起点和终点各在地图一角它会漫无目的地对整个地图做地毯式搜索时间开销非常大。我第一次跑的时候500×500的地图搜了接近两万次循环肉眼可见地卡顿。3.2 A*给Dijkstra装上“导航方向感”A*是对Dijkstra的改进核心区别是引入了启发式函数h(n)估算当前节点到终点的“预计代价”。在优先队列的排序依据中从单纯的g(n)从起点到当前点的实际代价变成了f(n) g(n) h(n)。由于启发函数的存在A会优先扩展“看上去离终点更近”的节点搜索方向非常明确搜索量大幅减少。只要启发函数满足“不超过真实代价”的条件即一致性/可采纳性A仍然可以保证找到最优路径。A*的代码与Dijkstra几乎一样只需要修改两个地方一是队列排序依据改为cost_so_far[current] heuristic(current, goal)二是邻居可能扩展到斜方向8邻域或更多取决于地图设计。import heapq import math def heuristic(a, b): # 欧氏距离作为启发函数 return math.hypot(a[0]-b[0], a[1]-b[1]) def astar(grid, start, goal): rows, cols grid.shape open_set [(heuristic(start, goal), 0, start)] came_from {start: None} cost_so_far {start: 0} # 8邻域适合允许斜向运动的场景 neighbors [(1,0), (-1,0), (0,1), (0,-1), (1,1), (1,-1), (-1,1), (-1,-1)] while open_set: _, current_cost, current heapq.heappop(open_set) if current goal: break for dx, dy in neighbors: nx, ny current[0]dx, current[1]dy if not (0 nx rows and 0 ny cols): continue if grid[nx, ny] 1: continue # 斜向移动代价为 sqrt(2)直线为1 step_cost math.sqrt(2) if dx ! 0 and dy ! 0 else 1.0 new_cost cost_so_far[current] step_cost if (nx, ny) not in cost_so_far or new_cost cost_so_far[(nx, ny)]: cost_so_far[(nx, ny)] new_cost priority new_cost heuristic((nx, ny), goal) heapq.heappush(open_set, (priority, new_cost, (nx, ny))) came_from[(nx, ny)] current path [] node goal while node is not None: path.append(node) node came_from.get(node) return path[::-1]A*在大多数场景下表现都非常好而且实现简单是无人机静态全局规划的默认首选。注意启发函数的选择会对性能产生很大影响。用欧氏距离得到的方向感最好如果地图只允许上下左右四个方向移动也可以改用曼哈顿距离搜索会更快但需要保证启发值不超过实际最小代价否则最优性就没保障了。3.3 RRT在高维复杂空间里的随机探索路线前面两个算法都要求环境用栅格表示。当环境维度变高、地图规模变大或者路径要求带动力学约束比如无人机不能直角转弯时基于栅格的搜索会变得非常笨重。RRTRapidly-exploring Random Tree用随机采样的思路绕开了这个问题——不需要预先离散化整个地图而是从起点开始向随机方向扩展一棵树直到树的分支触达目标区域。代码实现比较简洁import random import math def rrt(grid, start, goal, max_iter2000, step_size5): 简化版RRT不考虑动力学约束 tree {start: None} for _ in range(max_iter): # 以一定概率把终点作为采样点提高收敛率 if random.random() 0.1: sample goal else: sample (random.randint(0, grid.shape[0]-1), random.randint(0, grid.shape[1]-1)) # 找树中离采样点最近的节点 nearest_node min(tree, keylambda n: math.hypot(n[0]-sample[0], n[1]-sample[1])) # 从最近节点向采样点延伸step_size距离 dist math.hypot(sample[0]-nearest_node[0], sample[1]-nearest_node[1]) if dist step_size: new_node sample else: ratio step_size / dist new_node (int(nearest_node[0] (sample[0]-nearest_node[0])*ratio), int(nearest_node[1] (sample[1]-nearest_node[1])*ratio)) # 简单碰撞检测检查新节点是否在障碍物上 if grid[new_node] 0: tree[new_node] nearest_node if math.hypot(new_node[0]-goal[0], new_node[1]-goal[1]) step_size: tree[goal] new_node # 回溯路径 path [] node goal while node is not None: path.append(node) node tree[node] return path[::-1] return None # 没找到路径RRT最大的优势是不需要对地图做离散化适合连续空间和高维构型空间的规划。代码里还做了一个小优化——以10%的概率直接把终点作为采样点这样树可以在随机伸展的同时保持收敛方向收敛速度提升非常明显。但RRT也有明显短板随机采样导致每次规划的路径都不一样而且路径往往非常曲折。工程上通常会用RRT*在扩展时重新选择父节点并做rewire来优化路径或者对RRT生成的路径做后处理平滑比如用B样条曲线或Dubins曲线拟合。三者在实际选型时我用过一张简单的对照表算法最优性计算效率适用场景实现难度Dijkstra保证最优低搜索范围大小地图、教学演示低A*保证最优启发函数可取时高静态全局规划、二维栅格低RRT不保证最优高大空间尤其明显高维空间、带动力学约束的路径搜索中4. 把规划结果变成飞行控制指令局部避障与轨迹跟踪全局路径规划完成后工作并没有结束。无人机要真正沿着路径飞还需要解决两个问题第一飞行过程中遇到地图外的障碍物怎么办第二规划出的折线路径如何转成平滑可控的飞行速度指令。4.1 用动态窗口法DWA实现飞行过程中的实时避障DWA的核心思想听起来很朴素在每个控制周期里根据无人机当前的速度和加速度限制采样一组可行的速度组合线速度角速度然后模拟在这些速度下飞行一小段时间看会走到哪里再通过一个评估函数选一个“综合得分最高”的速度作为指令下发。评估函数通常由三个子项组成目标项模拟终点与全局目标点的距离越近得分越高。速度项速度越大得分越高鼓励尽快到达。障碍物项模拟轨迹上离障碍物的最近距离越远得分越高。这三项的权重分配是DWA调参的首要任务。我的经验是从目标项权重0.5、速度项0.2、障碍物项0.3起步再根据仿真效果调整。障碍物项权重太高飞机会绕很远的路太低又会贴着障碍物飞非常危险。DWA的计算量不大Python完全可以在几十毫秒内完成一轮采样评估足够满足室内低速无人机的实时性要求。ROS中著名的dwa_local_planner就是这套思路配合costmap_2d的局部代价地图使用。4.2 坐标系、速度指令与飞控控制接口路径规划器计算出的路径点坐标通常是基于地图坐标系例如以起飞点为原点的局部坐标系的。但飞控需要接收的指令通常是机体坐标系下的期望速度或者全局坐标系下的期望位置这中间涉及坐标系转换。最常用的做法是先用经纬度/局部坐标系做好统一再由PX4飞控的对外接口来衔接如果你的无人机用的是PX4飞控可以通过MAVLink协议发送SET_POSITION_TARGET_LOCAL_NED消息其中包含位置或速度指令。如果你用ROS和PX4推荐直接用offboard模式在cmd_loop里周期性地发布geometry_msgs/TwistStamped或mavros_msgs/PositionTarget飞控会自动完成姿态层面的底层控制。我在自己搭建的树莓派无人机上就是通过MAVLink的SET_POSITION_TARGET_LOCAL_NED接口下发速度指令的。这里有一个非常关键的细节发送频率必须稳定且不低于10Hz否则飞控会认为链路丢失直接退出offboard模式并触发降落。这个坑我踩过一次飞行过程中代码某个线程阻塞了半秒无人机立刻切换到降落模式差点炸机。后来我在主循环里加了一个线程监控确保所有可能阻塞的操作都放到子线程里执行。4.3 路径平滑直线转折对无人机极不友好全局规划出来的路径是一系列折线点在拐角处直接切换方向会让无人机产生很大的侧向加速度。实际飞行时通常会做一个路径平滑处理常用的方法有两种B样条拟合对路径点序列做三次B样条插值得到一条平滑曲线。单纯形法/梯度下降平滑在保持路径与障碍物距离的前提下最小化路径的曲率。我在自己的项目里最简单有效的方案是“路径点抽稀圆弧过渡”把距离很近的路径点合并成关键点在相邻关键点之间的拐角处用一段圆弧代替瞬间转向圆弧半径根据无人机最大允许侧向加速度计算。这样既不需要引入复杂的优化库飞行的平滑度也能满足低速室内场景。5. 从仿真到实机部署必须注意的坑这块内容是我在实操过程中真正打磨出来的经验。从一个能跑的Python仿真脚本到一台能可靠自主飞行的真无人机中间的事情远比想象中多。5.1 先用仿真是对钱包和飞机最大的保护不要一上来就拿着真机测试路径规划效果。最经济的方案是先在纯Python仿真环境里验证算法逻辑再进入机器人仿真平台最后才上真机。纯Python仿真你可以自己写一个简单的“飞机模型”模拟位置更新、速度响应和简单传感器感知。这一步主要验证路径规划算法本身在随机生成的地图上A*能否稳定找到路径找不到时输出什么异常。RRT扩展到数万次是否会造成内存膨胀。地图中出现U型凹槽时算法是否会陷入死循环。进阶一点的仿真环境我推荐两种AirSim微软开源的高保真无人机仿真平台提供Python API可以获取相机图像、深度图、IMU数据环境还能用社区提供的城市模型替换。Gazebo PX4 SITLGazebo模拟物理环境PX4 SITL在电脑上跑飞控固件ROS负责中间通信。这套组合是无人机行业最常见的验证方案能够覆盖飞控层面的验证。这个阶段你会接触到大量和ROS相关的概念比如mavros、octomap、map_server。如果你是初学者建议先把ROS基础节点通信搞明白再逐步往里加功能。5.2 实机踩过的几个高频坑第一地图坐标系与飞控坐标系不统一。我在仿真里使用的是东北天ENU坐标而PX4飞控内部用的是北东地NED坐标。第一次上真机时无人机起飞后直接向反方向猛窜吓得我立刻切回手动模式。后来统一约定所有接口层坐标都换算成NED坐标系问题才彻底解决。这个坐标系转换问题是所有从仿真转到实机的人都会遇到的经典问题务必提前做好转换模块并充分测试。第二传感器的可靠性与延时。真机上的激光雷达或视觉里程计数据是有噪声和延迟的不像仿真里那么干净。路径规划算法在理想数据上表现很好但在真机上可能频繁报错。我的处理方案是在状态估计模块里加一个数据有效性检查连续几帧无效数据就触发急停逻辑而不是带病执行规划指令。第三路径重规划的次数过多会导致飞行姿态剧烈抖动。局部规划器检测到动态障碍物后会频繁更新路径如果更新频率过快无人机可能来回摆动。我做的优化措施是为局部规划增加一个死区阈值——只有检测到障碍物距离小于1米时才触发重新规划否则继续沿当前路径飞行。5.3 提升导航鲁棒性的几个Python层面的小技巧再分享几个代码层面的小技巧都是实测中有效果的所有耗时的计算比如稠密地图的膨胀、RRT大规模采样尽量放到后台线程或异步执行保证主控制循环的频率稳定。状态机设计一定要完善待机、起飞、飞行中、紧急避障、降落每个状态都要有超时保护和异常处理。不要出现“程序卡在那个状态都不知道飞机去哪了”的情况。日志记录是排查问题的第一利器。在每次发布速度指令前同步记录当前坐标、目标路径点序号、期望速度和实际速度出问题之后回放日志才能定位原因。第一次实机调试时我加了完整日志后来排查一个飞行振荡问题30秒内就锁定了原因是速度项权重过高。从我个人的实操体会来说无人机路径规划与导航用Python来学习和实现是目前门槛相对较低、反馈又比较快的路径。它不需要一开始就去啃C和嵌入式底层只要掌握地图表示、算法选型、控制指令对接这几个核心环节就能在电脑上复现一整套无人机自主导航的关键链路。当然从仿真到真机还有一段路要走但那份“代码一运行飞机按规划飞往目标点”的成就感值得你把这些坑都踩一遍。本文还有配套的精品资源点击获取