无人机竞赛算法复盘：从Fast-LIVO定位到PX4 Offboard的工程实践

最近听到一句话叫“复活赛寄了”放在无人机竞赛圈子里翻译过来就是正赛没打好加赛也没救回来。我们队伍这几个月备赛“狼牙”无人机挑战赛主攻的是无人机自主飞行方向。如果只看整个系统的框图好像每一个模块都点到了感知有了、定位有了、规划有了、控制有了、仿真也能起机。但真正上真机跑任务的时候问题一个接一个地冒出来定位漂移、航线偏了、降落点找不到、切 offboard 模式后飞机直接乱窜。这篇文章不是一篇“成功的标准教程”而是一次完整的竞赛解题复盘。我会把我们在狼牙无人机算法中踩过的坑、最终确定的技术方案、以及从失败里提炼出来的可复用经验系统性地写出来。如果你正在做无人机竞赛备赛、四旋翼自主飞行开发或者想了解一个实际无人机算法系统的完整技术链路这篇文章值得你收藏下来慢慢看。1. 这篇文章真正要解决的问题很多同学看无人机算法的文章上来就是“粒子群算法原理”“A* 路径规划 Python 实现”“卡尔曼滤波推导”单看每一篇都很有道理但一到真机就不知道从哪下手。这恰恰暴露了一个问题无人机算法不是多个算法模块的简单拼装而是一条需要从传感器数据流出发、一路打通到执行器控制的完整链路。其中任何一个环节质量拉胯整个系统就会崩掉。这篇文章要解决的核心问题用一个场景来说任务要求无人机从 A 点起飞经过 B、C、D 三个航点在 E 点完成识别并降落。你要在有限时间内跑通一个“能真飞”的算法系统而不是“看起来像那么回事”的仿真 demo。围绕这个场景我会讲清楚以下问题狼牙无人机这类竞赛任务对整个算法栈到底考核什么定位模块选择 fast-livo 这类激光雷达方案时真正的难点在哪路径规划用全局规划还是局部规划怎么选offboard 模式起飞时那些“莫名其妙的失败”到底怎么排查为什么每次仿真都完美、一到真机就翻车如果重新组队准备比赛我会优先做哪几件事这里的核心判断是竞赛无人机的成败不是由某个算法的论文级别决定的而是由系统集成后的鲁棒性决定的。这也是我们这次“复活赛寄了”最痛的领悟。2. 狼牙无人机算法赛制考的是什么先说清楚“狼牙”是什么场景。从我们参赛的情况来看这类无人机挑战赛的主流赛制是任务制 时间分无人机需要自主完成起飞、巡航、目标识别、路径飞行、定点降落等一系列动作。部分赛段要求全自主不允许遥控器介入。评分维度包括任务完成度、飞行稳定性、轨迹精度、识别准确率、完成时间等。这意味着算法栈至少包含四个核心模块模块解决的问题常见选型感知与定位无人机“我在哪”“我看到了什么”Fast-LIVO、VINS-Fusion、RTK-GPS路径规划无人机“怎么飞到目标点”A*、RRT、粒子群算法、TEB控制无人机“怎么稳定地执行轨迹”PX4 官方 offboard 模式、PID 串级控制任务决策无人机“下一步该做什么”有限状态机、行为树如果参赛团队是第一次做无人机最容易犯的错误是把全部精力投入到路径规划算法的“高级感”上忽略了定位和控制层的稳定性。我们就在粒子群算法上花了大量时间做路径优化测试结果真机一跑定位漂移直接把规划出来的“最优路径”变成了绕远路。所谓“基础不牢地动山摇”。3. 我们最终采用的系统总体方案复盘下来我们决定参加下一场比赛时不再追求“每个模块都用论文最新算法”而是采用一套更务实的系统方案。3.1 硬件平台与机载算力以我们使用的四旋翼无人机为例基本配置是飞控Pixhawk 系列运行 PX4 固件机载电脑NVIDIA Jetson Orin NX 或 Xavier NX跑感知和规划算法传感器激光雷达 双目相机 IMU通信数传电台用于地面站监控遥控器比赛允许的情况下保留 RC 作为安全兜底但不参与自主飞行需要注意不同竞赛对硬件配置的要求不一样有些赛题不允许使用 RTK有些赛段则禁止使用 GPS。所以不要急着抄作业先看清规则再选硬件。3.2 软件架构分层我们最终确定的软件架构是一个经典三层结构感知层接收激光雷达点云、IMU 数据、相机图像输出里程计位姿和障碍物信息。决策规划层接收感知层的定位结果和任务指令输出局部轨迹点或速度指令。执行控制层通过 MAVROS 或 PX4 Offboard 模式将目标速度/位姿发送给飞控执行。架构上听起来很简单但实际工程里每个模块之间的数据流和时序关系非常容易出问题。比如定位频率是 10Hz但控制指令要求 30Hz 以上数据来不及更新怎么办路径规划出一个新目标点但无人机当前速度太快直接跟踪会冲出跑道怎么办视觉识别结果跳变上一帧是“目标”下一帧就丢了怎么平滑处理这些问题在模块单独测试时从来不会暴露只有把整个系统跑起来才会激化。3.3 关键数据流一个简化的数据流是这样的激光雷达/IMU - Fast-LIVO 里程计 - 局部代价地图 ↓ 任务状态机 - 全局路径规划 - 局部轨迹生成 - 速度指令 - PX4 Offboard ↓ 相机图像 - YOLO 识别 - 目标位置估计 - 任务状态切换这个链路看起来条理清晰但每个箭头都可能引入几百毫秒到几秒的延迟而无人机这玩意延迟 500ms 就可能已经飞出去好几米了。4. 定位模块选型Fast-LIVO 的坐标转换到底怎么处理定位是整个系统的基石。如果无人机不知道“自己在哪”后面所有规划和控制都没有意义。我们最初试过视觉 SLAM 方案后来因为比赛场地光照变化大、纹理稀疏最终转向了 LiDAR-Inertial 融合方案重点调研了 Fast-LIVO。4.1 Fast-LIVO 解决什么问题Fast-LIVO 是香港大学火星实验室提出的 LiDAR-Inertial-Visual 融合里程计方案。它把激光雷达、IMU 和相机数据紧耦合在一起核心卖点是在快速运动、光照变化、纹理缺失环境下定位比纯视觉方案更稳。相比传统 LOAM 类方案计算效率更高适合嵌入式平台。输出的是包含平移和旋转的六自由度位姿。说白了就是在无人机高速翻转、快速经过白色墙面、走到光线忽明忽暗角落的时候它能比摄像头更不容易“丢”。4.2 最大的坑多个坐标系之间的转换关系标题里有一个很重要的热搜词“fast livo 自带的坐标转换能用于无人机吗”。这是我们在备赛期间反复纠结的问题也是我们真机测试时定位“看起来没问题实际上全歪了”的根本原因。Fast-LIVO 输出的位姿通常定义在激光雷达坐标系下或者是地图坐标系下。但无人机飞控PX4使用的是 ENU东-北-天坐标系而且 MAVROS 的坐标系约定是ENU FLU 机体坐标系。如果你直接拿 Fast-LIVO 输出的odometry去给 PX4 发位置指令往往会出现以下现象无人机想往前飞结果往左跑了。无人机想悬停结果自己绕圈。定位输出的轨迹在 rviz 里看是好的但真机上偏差却越来越大。原因是坐标系没有对齐或者 TF 树没有正确发布。4.3 处理方式建议这里给出我们在测试中验证过可行的思路统一使用 ENU 坐标系作为全系统的“世界坐标系”。在 Fast-LIVO 启动参数中将点云和 IMU 的外参标定准确。如果 Fast-LIVO 输出的是 map 坐标系需要用静态坐标变换发布map - odom - base_link的 TF 关系。给 PX4 发指令时不要直接把激光里程计的位姿塞进去而是经过map到odom的变换再转换成 PX4 能理解的局部位置。下面是一个基于 MAVROS 发送位置指令的 Python 示例片段#!/usr/bin/env python3 # 文件路径offboard_position_control.py import rospy from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Odometry rospy.init_node(offboard_position_control) pose_pub rospy.Publisher(/mavros/setpoint_position/local, PoseStamped, queue_size10) odom_sub rospy.Subscriber(/fast_livo/odom, Odometry, odom_callback) def odom_callback(msg): global current_pos current_pos msg.pose.pose rate rospy.Rate(30) target PoseStamped() target.header.frame_id map target.pose.position.x 2.0 target.pose.position.y 1.0 target.pose.position.z 1.5 target.pose.orientation.w 1.0 while not rospy.is_shutdown(): target.header.stamp rospy.Time.now() pose_pub.publish(target) rate.sleep()注意这段代码只发了目标位置并没有进行坐标偏移补偿。真实使用中你一般还需要订阅 Fast-LIVO 的位姿计算它与真实目标点之间的偏差再做闭环控制。4.4 坐标转换能用吗回到那个热搜问题fast-livo 自带的坐标转换能用于无人机吗我的判断是能但要满足两个前提。第一你明确知道 Fast-LIVO 代码里使用的是哪种坐标系约定并且从代码或参数中确认输出到odometry的位姿所在坐标系。第二你拥有一套正确的静态坐标变换把sensor坐标系和base_link以及map坐标系之间的关系标定清楚。不要指望“跑起来 rviz 里看着对”就等于“坐标转换没问题”。真机上差之毫厘落地时谬以千里。5. 路径规划别急着上粒子群先保证可行性按热搜词来看很多人搜“无人机路径规划算法”可能是为了准备竞赛。我们当初为了追求创新点在路径规划层用了改进的粒子群算法来做全局路径搜索。结果发现算法本身效果是不错但工程落地时有两个致命问题计算耗时高粒子群算法需要迭代多次才能收敛在机载端实时运行时路径更新跟不上环境变化。最优路径不等于可飞路径算法规划出的路径可能是直线穿墙的“纸面最短路径”没有考虑无人机最小转弯半径和动力学约束。5.1 更推荐的做法在竞赛和工程实践中我更推荐混合策略全局规划用 A* 或 RRT 系列算法在已知地图中生成一条粗路径。局部规划用 TEBTimed Elastic Band算法或 DWA 算法结合实时传感器数据对全局路径进行平滑和避障。轨迹平滑对路径点做多项式拟合或贝塞尔曲线平滑保证无人机飞起来不会一顿一顿的。下面是一个基于 A* 的二维栅格地图路径规划最小示例可以作为全局规划的入门参考# 文件路径astar_demo.py import heapq def heuristic(a, b): return abs(a[0] - b[0]) abs(a[1] - b[1]) def astar(grid, start, goal): rows, cols len(grid), len(grid[0]) open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {start: 0} f_score {start: heuristic(start, goal)} while open_set: _, current heapq.heappop(open_set) if current goal: path [] while current in came_from: path.append(current) current came_from[current] path.append(start) return path[::-1] for dx, dy in [(1,0), (-1,0), (0,1), (0,-1)]: neighbor (current[0]dx, current[1]dy) if not (0 neighbor[0] rows and 0 neighbor[1] cols): continue if grid[neighbor[0]][neighbor[1]] 1: continue tentative_g g_score[current] 1 if neighbor not in g_score or tentative_g g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g heuristic(neighbor, goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None if __name__ __main__: grid [ [0, 0, 0, 0, 1], [1, 1, 0, 1, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0], [0, 1, 1, 0, 0] ] path astar(grid, (0, 0), (4, 4)) print(规划结果:, path)5.2 粒子群算法还能用吗并不是说粒子群算法不能用而是要看场景。如果比赛要求无人机在静态已知地图中寻找多目标最优访问顺序这是一个典型的 TSP旅行商问题变种粒子群算法非常适合。如果比赛要求无人机在动态未知环境中实时避障那么粒子群算法的实时性就会成为瓶颈。我在实际备赛中得到的结论是不要为了创新而创新。竞赛拼的是系统稳定度和任务完成率不是算法新颖度。6. 控制层的坑Offboard 模式起飞与悬停在竞赛任务中最常见也最容易让队伍“寄掉”的是起飞阶段。PX4 的 Offboard 模式本身只是一个“外部指令输入模式”它并不会自动让无人机稳定悬停。如果你发送的位置指令有问题飞机会按照错误指令执行结果就是起飞后迅速漂移甚至翻机。6.1 Offboard 模式起飞的标准流程根据 PX4 官方规范和社区实践推荐流程如下无人机上电飞控完成初始化进入 Preflight 状态。地面站确认 GPS/定位数据有效如果依赖视觉定位则要确认里程计输出正常。将飞控切到 Offboard 模式。以 10Hz 以上频率持续发送setpoint_position或setpoint_velocity指令。发送解锁指令arm。发送当前无人机位置作为目标位置让飞控进入位置保持。确认悬停稳定后再发送新的目标位置。很多人一上来就发送“目标点 2 米高”的指令然后立刻解锁飞机会猛然加油门造成“弹射起飞”。这就是不对的。6.2 MAVROS 示例安全起飞下面是一个使用 MAVROS 完成 Offboard 解锁和起飞的最小 C 示例// 文件路径offboard_takeoff.cpp #include ros/ros.h #include mavros_msgs/CommandBool.h #include mavros_msgs/SetMode.h #include geometry_msgs/PoseStamped.h int main(int argc, char** argv) { ros::init(argc, argv, offboard_takeoff); ros::NodeHandle nh; ros::Publisher setpoint_pub nh.advertisegeometry_msgs::PoseStamped(/mavros/setpoint_position/local, 10); ros::ServiceClient arming_client nh.serviceClientmavros_msgs::CommandBool(/mavros/cmd/arming); ros::ServiceClient set_mode_client nh.serviceClientmavros_msgs::SetMode(/mavros/set_mode); geometry_msgs::PoseStamped pose; pose.pose.position.x 0; pose.pose.position.y 0; pose.pose.position.z 1.5; pose.pose.orientation.w 1.0; ros::Rate rate(20); for (int i 0; i 100; i) { pose.header.stamp ros::Time::now(); setpoint_pub.publish(pose); ros::spinOnce(); rate.sleep(); } mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode OFFBOARD; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value true; if (set_mode_client.call(offb_set_mode) offb_set_mode.response.mode_sent) { ROS_INFO(Offboard mode enabled); } if (arming_client.call(arm_cmd) arm_cmd.response.success) { ROS_INFO(Vehicle armed); } while (ros::ok()) { pose.header.stamp ros::Time::now(); setpoint_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; }这段代码的逻辑是这样的先以 20Hz 持续发送目标点 20 秒让飞控知道“有外部指令源”。发送 Offboard 模式切换。请求解锁。持续发送目标位置让无人机飞向 1.5 米高度。这里真正容易踩坑的地方是有些飞控固件版本要求必须先收到持续的 setpoint 再切换 Offboard否则会拒绝进入该模式。6.3 常见 Offboard 问题问题现象可能原因排查方式解决方案无法切入 Offboard 模式没有持续发送 setpoint检查 ROS 话题频率确保 10Hz 以上持续发送解锁失败飞控检测到定位源异常查看 QGC 状态显示检查里程计/GPS 是否有效起飞后漂移位置指令坐标系不对对比 rviz 与 QGC 坐标修正坐标变换到达高度后抖动位置控制器 PID 参数不合适查看日志并调整调节 MPC_XY_P、MPC_Z_P 等参数飞机无法降落高度判定异常检查气压计或激光测距根据实际传感器调整7. 仿真与真机差距一个“复活赛寄了”的根本原因我们这次比赛的失败最深层的原因就是仿真中跑得太顺真机上所有隐藏问题集中爆发。很多人以为仿真跑通了真机就是“换一台机器再跑一遍”。实际上仿真和真机的差距是全方位的环节仿真表现真机实际定位数据无噪声频率稳定有噪声频率波动偶尔跳变电机响应理想力矩输出有延迟有饱和有电池电压下降风场干扰无风有风甚至下洗气流传感器外参完美对齐标定存在误差时间同步理想各传感器时间戳不一致如果备赛只有两周我宁愿花 60% 的时间做真机测试30% 时间做数据回放分析10% 时间做仿真验证。而我们这次等于把比例反过来结果就是复活赛寄了。7.1 如何更好地利用仿真仿真不是没用而是要会“骗自己”。建议把仿真的目标设定为验证消息流是否完整。验证代码逻辑是否有崩溃风险。验证状态机在正常流程下的状态迁移。验证规划算法能不能输出有效路径。仿真很难验证的是定位漂移后的系统容错能力。控制指令延迟后的稳定性。传感器瞬时丢失后的行为。后者只能通过真机测试来验证。没有第二条路。8. 如果重新准备复赛我会优先做哪些改进基于这次“复活赛寄了”的教训下面是我总结出的下一轮备赛行动计划。这些建议也同样适用于任何准备无人机算法比赛或项目的开发者。8.1 先做一个“最小闭环”不要一上来就把感知、规划、控制全接上。先做一个最小闭环固定位置悬停 - 发送目标点 - 飞过去 - 悬停 - 飞回来这个闭环只涉及定位和控制不需要视觉识别不需要复杂的路径规划。只有这个闭环稳定跑通 10 次以上才考虑加障碍物避障再加目标识别最后才整合成完整任务。8.2 建立真机日志回放机制每次真机测试都要记录以下内容Fast-LIVO 输出的里程计轨迹。PX4 的 ulog 日志。机载电脑端的 ROS bag。相机拍摄的原始视频。事后分析时把 ROS bag 的定位轨迹和 PX4 ulog 的实际位置对齐才能定位到问题到底出在“感知层”还是“控制层”。8.3 开发一个“安全开关”机制比赛允许的情况下强烈建议设置一个遥控器安全开关。可以是这样的设计当系统检测到定位发散连续 N 帧位置跳变超过阈值时自动切换为“悬停”或“降落”。当任务状态机卡死超过 T 秒时自动执行“返航”或“降落”。RC 通道保留一档“手动接管”。不要指望你设计的算法永远正确。无人机的第一原则是不炸机第二原则才是完成任务。8.4 把“坐标系对齐”做成启动自检我们这次吃的最大的亏就是坐标系问题。建议在系统启动后执行一个“坐标自检”流程将无人机放置在已知朝向的地面位置。读取 Fast-LIVO 里程计输出的初始位姿。读取 MAVROS 提供的飞控当前位姿。对比两者的旋转矩阵和平移向量是否一致。如果不一致输出报警并禁止起飞。这一步可以用一个简单的配置检查脚本实现成本很低收益极大。9. 常见问题与排查思路这里汇总我们在备赛中真实遇到并且排查过的问题按照问题现象、可能原因、排查方式、解决方案来写可以直接作为项目排查手册使用。问题现象可能原因排查方式解决方案Fast-LIVO 输出频率低点云输入频率低查看激光雷达驱动话题频率调整雷达驱动参数降低点云分辨率无人机航向偏移IMU 外参标定不准Fast-LIVO 启动日志查看外参重新标定外参并固化配置文件规划路径反复跳动全局路径与局部路径目标冲突订阅路径与目标点话题对比统一局部规划器的目标点来源目标识别闪烁单帧 YOLO 误检查看检测置信度阈值加入多帧投票或状态机锁存无人机响应迟钝控制频率过低查看 setpoint 发布频率将控制指令频率提高到 30Hz任务流程卡住状态机缺少超时逻辑打印状态转移日志每个状态增加 watchdog 超时真机轨迹与仿真不符动力学参数差异对比 PX4 ulog 与仿真数据根据实测辨识 PID 参数降落点识别不准确相机高度与内参误差采集多高度样本验证增加视觉测距或融合高度传感器9.1 排查步骤的优先级当系统故障时建议的排查顺序是先看日志 - 再复现 - 再拆模块 - 最后改代码不要一上来就怀疑算法模型不够高级。大多数问题都是数据流不通、坐标系不对、频率不足这些基础问题。10. 最佳实践与工程建议这部分内容是我在备赛过程中提炼的“如果重来一次我会怎么做”的具体建议希望能帮你少走弯路。10.1 版本管理要细化到配置文件无人机算法项目涉及多个子模块代码版本、模型版本、配置文件版本需要统一管理。建议做法代码用 Git 管理每次真机测试前打 tag例如test_20250321_01。所有传感器外参标定结果保存到固定目录不允许手工改动。相机模型权重、激光雷达驱动参数、PID 参数都纳入 Git。这样出问题时才能精确回溯到底是“代码变了”还是“标定参数变了”。10.2 日志记录是系统的一部分没有日志的系统出了事故只能靠猜。建议在项目初期就统一日志规范ROS topic 持续记录到 bag 文件。控制指令、模式切换、任务状态变化输出到文本日志。PX4 ulog 每次飞行后必须导出归档。日志不是开发完以后才补的而是开发时就要做的。10.3 异常处理要走“安全优先”原则当出现异常时默认动作按优先级排列悬停 降落 返航 继续任务尤其是竞赛任务很多队伍为了追求时间分出现一点小异常也强行继续执行任务。结果往往是小异常演变成大事故直接炸机退赛。10.4 不要忽略飞行场地的地面效应最后说一个容易被忽视的工程细节起飞和降落阶段地面效应会显著影响无人机的高度控制。如果只用气压计定高无人机在地面附近高度数据会受到桨叶气流干扰导致高度控制不稳。建议在降落阶段融合激光雷达或超声波测距数据同时把降落目标识别放在更低的高度下执行而不是在 3 米高度就直接触发“降落到识别点”的指令。11. 总结与后续学习方向“复活赛寄了”这段话看起来是一句玩笑但对我们来说是一次非常扎实的技术教训。从这次比赛中我最想分享给大家的一个判断是无人机竞赛比的不只是算法能力更是系统工程能力。你在定位、规划、控制任何一个单点上积累再多论文复现经验也不如在真机上把最小闭环跑通 50 次更有价值。如果你正要参加下一场无人机竞赛或者正在做无人机自主导航相关的毕业设计、工程实习下面几个方向值得继续深入学习状态估计与传感器融合理解卡尔曼滤波、因子图优化、IMU 预积分这些底层原理不要只停留在调用 Fast-LIVO。路径规划算法的工程约束学习 A*、RRT、TEB 时重点关注“如何满足动力学约束”和“如何实时更新”而不是只看论文效果图。PX4 Offboard 控制协议把 MAVROS 和 MAVLink 协议吃透尤其是 Coordinate Frame 和 Flight Mode 的设计意图。仿真环境的合理使用先用仿真验证逻辑再用真机验证工程两者要交替迭代而不是线性推进。故障排查方法论一个稳定的无人机系统需要有一套完整的“日志采集-问题复现-模块隔离-回归验证”流程。如果你现在还在备赛阶段我的建议是今天就去定一个最小闭环目标比如“无人机从一个点自主飞到另一个点并安全降落”。先完成它再考虑加花活。否则到了复活赛可能真的只能寄了。希望这篇“狼牙无人机算法回顾”能对正在准备无人机项目和竞赛的朋友有帮助。建议收藏备用后面真机上出了问题再回来对照排查。