简介面向ROS1开发者与无人机导航学习者的覆盖路径规划算法实例封装了多边形覆盖规划开源包的核心实现可帮助理解如何将目标区域分解并规划出无遗漏的全覆盖路径适合环境监测、农业植保等需要区域遍历的自主飞行场景。资源共160个文件以C源码为主体配合ROS的启动文件、可视化配置、参数文件以及自定义消息与服务类型压缩包仅471KB轻量且目录结构清晰。已有1981人学习或下载。通过该实例可系统观察节点、话题、服务与动作的协作方式掌握从环境地图构建、覆盖路径生成到控制指令输出的完整流程并了解扫描式覆盖、BCD分解、可见性图等规划算法的工程化实现对深入理解无人机自主飞行系统设计与算法落地具有直接参考价值。代码中附有详细注释与模块化设计便于二次开发与算法验证。 搞无人机自动飞行的朋友大概率都绕不过覆盖路径规划这道坎。农田植保要按垄扫电力巡检要沿塔巡查灾后搜索要把整片区域跑遍这些任务的本质都是同一个问题给定一片区域让无人机自动飞出一条路径既要全覆盖、不重不漏又要尽量省电、少转弯。这篇文章我就把我的ROS1实现思路、完整代码结构和调试经验整理出来用的无人机平台是Pixhawk飞控加机载电脑通信走MAVROS仿真在Gazebo里跑通了再上真机。1. 覆盖路径规划的问题定义与算法选型依据先聊一个容易被新手忽略的问题覆盖路径规划Coverage Path Planning, CPP和一般的点对点路径规划比如A*、RRT找一条从A到B的路完全是两回事。点对点路径规划的目标是从哪走到哪最短而覆盖路径规划的目标是让传感器视场覆盖整个目标区域路径本身要充满整个面而不是一条线。具体来说一个合格的覆盖规划算法要同时满足三个指标覆盖率目标区域内没有被覆盖到的盲区要尽可能少理想情况是100%重复率同一块区域被扫了多遍的比例要低重复率高意味着浪费电量转弯次数无人机转弯时往往需要减速甚至悬停能耗比直线巡航高得多次优路径往往转弯特别多这三个指标其实是互相矛盾的。想覆盖率100%路径就要密集重复率和转弯次数也会上来想少转弯路径就稀疏可能漏掉区域。所以实际工程里我们做的不是最优解而是满足任务要求的可行解。算法选型上我对比过几种主流方案算法适用场景优点缺点实现难度牛耕式往返扫描规则矩形/凸多边形区域简单直接、转弯少对凹多边形和障碍物处理能力弱低单元分解法含梯形分解含障碍物的复杂区域能处理凹多边形、障碍物分解逻辑复杂、子区域拼接要考虑衔接中螺旋式扫描不规则近圆形区域适合由外向内覆盖凹边界容易出问题中基于随机采样的覆盖极不规则环境灵活性高覆盖率不稳定、路径杂乱高我的项目选的是牛耕式往返扫描作为基础策略。原因很简单实际作业里大部分覆盖任务的目标区域经过简单处理都可以近似成凸多边形农田是长条形操场是矩形开阔水域也是多边形。牛耕式的路径质量在这些场景下已经接近最优而且代码逻辑清晰调试成本低。不过如果目标区域有凹角或者内部有禁飞区纯牛耕式就会产生大量绕路。为了解决这个问题我在算法里加了一个区域预处理环节把凹多边形先做凸分解得到几个互不重叠的凸子多边形然后在每个子多边形内部独立做牛耕式扫描子区域之间用最短连接线衔接。这里简单推导一下转弯次数的计算公式方便大家做可行性验证。假设覆盖区域的长度为L沿航线方向宽度为W垂直航线方向传感器覆盖宽度为d那么完整的覆盖航线需要的直线段数量是n ceil(W / d)转弯次数大约是n - 1。这个公式看起来简单但它决定了飞行时间。比如一块100m x 50m的区域覆盖宽度10m那么n 5整条路径就是5条百米直线加4次转弯。如果无人机巡航速度是8m/s每次转弯含减速、转向、加速耗时约10s那么总飞行时间大约是直线时间 5 * 100 / 8 62.5s 转弯时间 4 * 10 40s 总时间 102.5s转弯时间占了接近40%这就是为什么很多实际的覆盖规划算法会把减少转弯放在比减少总路径长度更优先的位置。有些论文里提到的最优覆盖方向策略本质就是通过旋转航线方向找出一个能让W/d值最小、从而转弯次数最少的角度一般这个角度会取区域的主方向或最长轴方向。2. ROS1工程结构设计与核心模块划分我的ROS1工程是在Ubuntu 18.04 Melodic上开发的飞控是Pixhawk 4运行PX4固件机载电脑是NVIDIA Jetson Xavier NX。整个项目的功能包名叫uav_coverage_planner依赖的核心库有rospyPython ROS接口开发速度快、numpy路径计算、shapely多边形几何处理、MAVROS和飞控通信。功能包结构如下uav_coverage_planner/ ├── CMakeLists.txt ├── package.xml ├── launch/ │ ├── planner.launch # 主启动文件 │ └── gazebo_sim.launch # 仿真用 ├── scripts/ │ ├── coverage_planner_node.py # 主节点区域处理路径生成 │ └── waypoint_publisher.py # 航点发布节点 ├── config/ │ ├── planner_params.yaml # 算法参数 │ └── mission_area.yaml # 任务区域定义 ├── rviz/ │ └── coverage_planner.rviz # 可视化配置 └── maps/ └── test_area.png # 测试地图模块划分上我把整个系统拆成三个独立节点让它们各司其职出了问题也好定位区域管理模块coverage_planner_node.py负责读取任务区域定义做凸分解计算最优扫描方向生成航点数组。这是整个系统的核心算法都在这里。航点执行模块waypoint_publisher.py订阅核心节点输出的航点数组按顺序通过MAVROS的/mavros/mission/push接口上传任务给飞控或者直接通过/mavros/setpoint_position/global逐点发送位置指令。状态监控模块可选订阅飞控的GPS状态、电池电量、飞行模式负责在紧急情况时暂停任务或返回起飞点。用ROS节点的方式而不是全都塞在一个程序里最大的好处是每一层都能独立测试。我在开发中经常遇到飞控不给反应的情况这时候先把航点执行节点停掉手动用rostopic echo检查核心节点的输出一条命令就能确定问题出在规划还是通信不需要整个系统重启。ROS话题设计上我用了四个自定义话题类型和说明如下话题名消息类型说明/coverage_planner/waypointsvisualization_msgs/MarkerArray可视化航点同时在Rviz里显示路径/coverage_planner/mission_polygongeometry_msgs/PolygonStamped上报当前规划的目标区域方便核对/coverage_planner/statusstd_msgs/String节点状态信息比如正在规划“上传航点/coverage_planner/takeoff_cmdstd_msgs/Bool自动起飞指令接飞控端3. 牛耕式覆盖航线的核心代码实现核心算法的实现我分成了三个步骤区域预处理、扫描方向计算、航线生成。下面逐个讲每个步骤我都会给出关键代码因为我觉得看代码比看一堆公式要直观得多。第一步区域预处理与凸分解这一步的目标是把用户输入的任意多边形转成多个凸多边形。Shapely库提供了polygonize和triangulate函数我优先推荐三角剖分转凸分解的方式因为对凹角处理得更干净。import numpy as np from shapely.geometry import Polygon from shapely.ops import triangulate def preprocess_region(boundary_points): 输入边界坐标点输出凸多边形列表 # 构造多边形 poly Polygon(boundary_points) if not poly.is_valid: poly poly.buffer(0) # 处理自交情况 # 如果本身就是凸多边形直接返回 if poly.convex_hull.equals(poly): return [poly] # 凹多边形三角剖分后按邻接关系合并成凸子区域 triangles list(triangulate(poly)) sub_regions [] for tri in triangles: if tri.centroid.within(poly): sub_regions.append(tri) return sub_regions这段代码里有个细节值得说poly.buffer(0)是Shapely里处理无效几何的常用技巧原理是给多边形加一个约等于0的缓冲距离让Shapely重新计算几何关系能修正自交或退化的边界。这个操作在实战中经常用到特别是当输入坐标是从地图软件里手工点出来、带了轻微误差的时候。第二步计算最优扫描方向子区域可能是任意朝向的四边形直接沿x轴扫描可能产生不必要的转弯。我的做法是计算子区域的主方向Main Direction即旋转扫描线找到一个角度使得在这个角度下覆盖次数最少。实际上最优角度往往是区域最长边方向或者使用PCA主成分分析求点集的第一个主成分方向近似效果已经很好。def compute_scan_direction(polygon): 用PCA计算多边形顶点的主方向 该方向作为牛耕式扫描的航线方向。 coords np.array(polygon.exterior.coords) centroid coords.mean(axis0) centered coords - centroid # 协方差矩阵 cov np.cov(centered.T) # 特征值分解最大特征值对应的特征向量即主方向 eig_vals, eig_vecs np.linalg.eig(cov) main_dir eig_vecs[:, np.argmax(eig_vals)] angle np.arctan2(main_dir[1], main_dir[0]) return anglePCA求主方向的方法参考了粒子群算法里寻找最优方向的思想——虽然不是全局最优但胜在快、稳定。如果你对这个方向特别敏感比如农田有播种垄向必须沿垄向飞也可以在参数文件里直接手动指定scan_angle覆盖自动计算值。第三步生成航点序列拿到了扫描方向和子区域之后生成航点就是个几何计算问题。我把子区域旋转到扫描方向为水平生成水平往返线再旋转回原坐标系。def generate_waypoints(polygon, scan_angle, coverage_width): 根据子多边形、扫描方向和覆盖宽度 生成牛耕式往返航点序列。 # 旋转到扫描角度为0 rotated_poly rotate(polygon, -scan_angle) minx, miny, maxx, maxy rotated_poly.bounds # 按照覆盖宽度生成平行线 y miny waypoints [] direction 1 # 1表示从左向右-1表示从右向左 while y maxy: x_start minx x_end maxx if direction -1: x_start, x_end x_end, x_start wp_start (x_start, y) wp_end (x_end, y) waypoints.append((wp_start, wp_end)) y coverage_width direction * -1 # 把航线旋转回原坐标系 final_waypoints [] for seg in waypoints: p1 rotate_point(seg[0], scan_angle) p2 rotate_point(seg[1], scan_angle) final_waypoints.extend([p1, p2]) return final_waypoints这里我给出的是简化的版本实际工程里还需要处理几个关键点航点插值飞控执行任务时两个航点之间的距离如果超过一定阈值需要插值出中间点。我一般设置最大段距为15m超过就线性插值。航线外扩实际作业时无人机的传感器视场中心是飞行轨迹而覆盖区域是这个中心组成的条带。所以航线不能刚好从区域边界开始而应外扩coverage_width / 2确保边缘也能覆盖到。这个偏移量虽然不大但很关键不然边界会漏。转弯弧线对于固定翼或高速飞行的无人机直线航线之间的转弯是弧线。这个在ROS里一般通过加转弯航点实现但我这个项目是旋翼机转弯半径小直接生成直角转角就行。在上面的代码基础上我把完整的主节点coverage_planner_node.py的核心逻辑补成一个可以直接跑的流程。#!/usr/bin/env python import rospy import numpy as np from shapely.geometry import Polygon from shapely.affinity import rotate from geometry_msgs.msg import PolygonStamped, Point32 from visualization_msgs.msg import Marker, MarkerArray import yaml import math class CoveragePlannerNode: def __init__(self): rospy.init_node(coverage_planner_node) self.waypoint_pub rospy.Publisher( /coverage_planner/waypoints, MarkerArray, queue_size1) self.mission_polygon_pub rospy.Publisher( /coverage_planner/mission_polygon, PolygonStamped, queue_size1) # 加载参数 self.coverage_width rospy.get_param(~coverage_width, 8.0) self.safe_offset rospy.get_param(~safe_offset, 2.0) # 任务区域从yaml读取 self.area_points self.load_area() def load_area(self): 从mission_area.yaml读取目标区域经纬度或本地坐标 with open(rospy.get_param(~area_config), r) as f: config yaml.safe_load(f) return config[area] def run(self): # 简化主流程 polygon Polygon(self.area_points) angle compute_scan_direction(polygon) wps generate_waypoints(polygon, angle, self.coverage_width) # 发布marker用于可视化 marker_array MarkerArray() # ... 构建marker ... self.waypoint_pub.publish(marker_array) rospy.spin() if __name__ __main__: try: CoveragePlannerNode().run() except rospy.ROSInterruptException: pass4. Gazebo仿真验证与航点测试算法写完之后直接上真机是危险的我习惯先在Gazebo里跑PX4的软件在环仿真SITL。这个环节不复杂但有个关键点仿真环境里要有一个能反映真实场景的地图模型不然验证效果打折。我用的仿真搭建方式安装PX4 SITL环境和Gazebo# 在Ubuntu 18.04上克隆PX4固件 git clone https://github.com/PX4/PX4-Autopilot.git --recursive cd PX4-Autopilot make px4_sitl gazebo启动MAVROS让ROS和仿真飞控建立通信roslaunch mavros px4.launch fcu_url:udp://:14540127.0.0.1:14557启动我的规划功能包roslaunch uav_coverage_planner planner.launch在Rviz里可以直观看到规划出来的航线是否覆盖了整个区域同时也能看飞控是否按航线飞行。我一般是先发一个简单的小矩形区域测试比如20m x 15m覆盖宽度8m这样理论上是3条航线、2次转弯。如果这个都跑不对后面复杂区域不用测了。仿真中我遇到的最典型问题有两个这里先说第一个第二个放在后面踩坑章节细讲。第一个问题是坐标系的混乱。Gazebo里PX4飞控发出的GPS坐标是模拟的球面坐标经纬度而我的规划算法是在平面直角坐标系里计算的。如果直接把算法生成的平面坐标上传给飞控飞控会当成经纬度解析航线就会跑到印度洋去。解决方法是在MAVROS里做坐标转换把规划航线的平面坐标结合起飞点的经纬度和航向角转换为经纬度坐标再上传。MAVROS提供了/mavros/global_position/global话题里的坐标信息可以直接拿来做转换基准。def local_to_global(local_x, local_y, origin_lat, origin_lon, origin_yaw): 将本地平面坐标转换为GPS经纬度WGS84 EARTH_RADIUS 6378137.0 # 先把本地坐标旋转到原坐标系因为规划时可能旋转过 cos_yaw math.cos(origin_yaw) sin_yaw math.sin(origin_yaw) dx local_x * cos_yaw - local_y * sin_yaw dy local_x * sin_yaw local_y * cos_yaw # 经纬度增量 d_lat dy / EARTH_RADIUS d_lon dx / (EARTH_RADIUS * math.cos(math.radians(origin_lat))) return origin_lat math.degrees(d_lat), origin_lon math.degrees(d_lon)第二个问题是航点跟随精度。Gazebo里飞控用默认的L1控制器跟随时如果航点间距太大或变化方向太陡飞机容易冲出航线尤其是在转弯处。我后来在航线密集的地方加了航点插值并且把最大水平速度限制在8m/s以内效果好了很多。仿真验证通过后我的最后一步是在实际场地跑一次小规模任务一块50m x 30m的矩形空地覆盖宽度10m飞行高度30m。实际飞行结果航线总长约300m飞行时间接近90s覆盖率通过事后检查相机拍摄画面达到了95%以上重复率在10%以下。对于这个规模的任务结果完全可以接受。5. 调试过程中避开的坑与参数调优心得这个项目做下来我踩了不少坑也总结了一些经验。前面讲过坐标系转换问题这里再说几个影响比较大的希望后来的人少走弯路。第一个坑ROS1的tf树过期导致飞控拒收航点。现象是规划完整条航线MAVROS推送mission的时候提示Reject waypoint飞控那边没有任何反应。查了好几天才定位到原因是我的机载电脑上时间同步有问题导致ROS的/tf坐标变换树过期。PX4飞控在接收外部任务时会检查时间戳的有效性如果tf过期时间超过设定阈值会直接拒绝任务。解决办法就是在起飞前执行sudo apt install chrony sudo chronyd -q pool cn.pool.ntp.org iburst确保系统时间和GPS时间同步。这个坑在真机上特别容易踩反而是Gazebo仿真里因为时间由仿真器控制没有这个问题。第二个坑MAVROS的mission模式切换时序。自动执行任务需要先把飞控切到AUTO.MISSION模式再上传航点或者先上传再切换。我一开始是上传完航点立刻发模式切换指令但飞控经常报错。后来查PX4源码这部分我参考了西门子无人机编程实例里的飞控逻辑发现PX4在接收到MAV_CMD_MISSION_START指令后需要确认航点已经全部写入并且在当前状态机的正确状态才能切换。解决办法是在发布模式切换指令前查询飞控的mission状态rosservice call /mavros/mission/clear rosservice call /mavros/mission/push # 等待mission确认 sleep 3 rosservice call /mavros/set_mode base_mode: 0 custom_mode: AUTO.MISSION这个3秒的sleep看起来很粗糙但确实有效因为飞控内部处理mission列表需要时间不给它这个时间窗口后面就会出各种奇奇怪怪的问题。第三个坑覆盖宽度的标定不能只看传感器名义值。我最初把覆盖宽度设成无人机相机视场在地面上的投影宽度但实际飞出来发现覆盖率根本不够。原因是相机安装在无人机下方会有一点倾斜角度而且飞控的高度控制有误差±0.5m左右导致实际视场比理论值小。用我的经验安全起见应该把覆盖宽度设成理论视场宽度的75%这样虽然重复率会上升一些但能保证不漏覆盖。如果你用的是RTK高精度定位飞控高度控制很稳定这个系数可以放宽到85%。最后一个实用的调参心得是关于航点高度。从Pixhawk飞控执行mission的经验来看PX4在AUTO.MISSION模式下航点之间会做一个平滑的高度变化。如果你在一条航线上频繁改变高度飞控的垂直速度会跟不上容易触发高度误差过大报警。建议覆盖任务尽量保持同一飞行高度只有在跨子区域时才有必要改变高度并且高度差要小于5m否则就拆成两个单独的任务执行。6. 从仿真到实机的完整测试链路与下一步扩展方向整理一下我的完整测试流程给打算复现这个项目的朋友一个参考阶段验证内容耗时估算Gazebo仿真算法逻辑、航点正确性、坐标转换1-2天室内视线内测试小区域20m以下航点跟随精度半天室外小规模测试50m级矩形区域完整任务1天室外复杂区域测试凹多边形、多子区域衔接1-2天测试中我养成了一个习惯每次试验前先记录起飞点的经纬度和朝向飞行结束后把PX4的ULog日志导出来用plotjuggler查看实际飞行轨迹和规划航线的偏差。这样做一次就能发现很多肉眼看不到的问题比如某个航点漂移、转弯处轨迹外扩等。关于下一步的扩展方向我目前在做两个改进。第一个是加入实时避障模块在机载电脑上接入一个前视摄像头用深度学习目标检测识别飞行路径上的障碍物一旦识别到障碍就把未完成的航线临时挂起让飞控切换到OFFBOARD模式绕过障碍后回来继续执行。第二个是动态覆盖现在我的规划是静态的如果目标区域有动态变化比如搜救任务中被困者在移动就需要在飞行过程中重规划。目前我在调研粒子群算法和快速探索随机树RRT结合的方法希望能让系统在几秒钟内完成局部重规划。最后提一句ROS1虽然年纪不小了但在无人机领域生态依然很成熟尤其MAVROS和PX4社区的文档积累非常多。这个项目用ROS1做原型验证、算法迭代效率比直接从ROS2开始高很多。如果你所在的项目组没有强制的ROS2迁移要求我建议先从ROS1搭起这套思路后续平移ROS2也不算太难。本文还有配套的精品资源点击获取