基于视觉 SLAM 与 GNSS 融合的无人机自主定位系统

本科毕业设计（2025.03 — 2025.06）

项目概述：设计并实现一套 VO/VIO 与 GNSS 松耦合的无人机自主定位系统，在 GNSS 拒止环境下仍能输出全局坐标系下的连续定位

我的角色：独立完成方案设计、算法实现、仿真与真机实验全流程

技术栈：C++ · ROS · ORB-SLAM3 · PX4/Gazebo · QGroundControl · Eigen · EVO

核心成果：轨迹复杂度判定机制使真机融合定位 RMSE 从 18.27 m 降至 9.32 m（提升 54.7%）

Situation（背景）

无人机在室内、城市峡谷、灾后废墟等场景中常面临 GNSS 信号拒止：传统卫星定位失效，无人机失去全局位置参考。而视觉（惯性）里程计 VO/VIO 虽能在无 GNSS 环境下靠相机 + IMU 自主定位，却存在误差随时间累积漂移的固有缺陷——单一技术都无法满足全场景自主定位需求：

• GNSS：全局覆盖、无累积误差，但室内/遮挡环境信号差，且精度仅米级；
• VO/VIO：局部精度高、实时性好，但长时间运行漂移严重，且结果表达在自身初始化的局部坐标系中。

两者恰好优势互补：GNSS 可用时修正视觉漂移，GNSS 拒止时由视觉接管定位。如何设计一套融合架构，让无人机在 GNSS 有↔无的切换中始终输出全局坐标系下的连续、可靠位姿，就是本课题要解决的问题。

Task（任务）

设计并实现一套 ROS 平台下基于 VO/VIO 与 GNSS 松耦合的无人机自主定位系统，具体目标：

1. GNSS 可用时：融合 GNSS 与 VO/VIO 数据，输出全局定位结果；
2. GNSS 拒止时：无缝切换到视觉定位，并仍以全局坐标（WGS84）输出；
3. 搭建 Gazebo 仿真平台验证系统可行性；
4. 固定翼真机飞行实验验证系统在真实环境的可靠性。

Action（行动）

1. 系统总体架构设计

系统基于 ROS topic 通信构建为三个模块：数据预处理 → 融合 → 全局地图输出。视觉图像先经 ORB-SLAM3 处理得到 VIO 位姿，与 GNSS 数据在融合模块完成对齐与融合；GNSS 失效时直接将 VIO 位姿经预计算变换转换到全局坐标系输出。

2. 数据预处理：统一时空基准

融合的前提是两路数据在同一坐标系、同一时间轴上：

• 坐标转换：GNSS 原始数据（经纬高）按 WGS84 椭球参数转换到 ECEF 地心地固坐标系，作为融合的全局基准；VO/VIO 输出的局部世界坐标则通过 Sim(3) 变换映射到 ECEF。
• 时间同步：GNSS 频率（10 Hz）远低于视觉（30 Hz），采用双端队列缓冲 + 线性插值将 GNSS 数据对齐到 VO/VIO 时间戳上，实现逐帧融合。

3. 融合核心：Sim(3) 相似变换求解

VO/VIO 局部坐标系与 ECEF 坐标系之间不仅存在旋转平移，还存在尺度差异（单目视觉尺度不确定），因此采用带尺度因子的 Sim(3) 广义相似变换 $T=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$ 衔接两套坐标。求解采用 Horn’s 闭式解法：对滑动窗口内两路位置点集去质心后构造对称矩阵 $N$，特征分解得旋转矩阵 $R$，再由矩阵迹解出尺度 $s$ 与平移 $t$。

4. 关键改进：轨迹复杂度判定机制（创新点）

真机实验中发现一个关键问题：当滑动窗口内轨迹近似直线时，绕该直线的旋转不可观，导致矩阵 $N$ 特征分解不唯一，Sim(3) 解算出现大角度偏差甚至方向反转，融合轨迹严重发散（真机 RMSE 高达 18.27 m）。

为此设计了两级机制：

• 轨迹复杂度判定：以滑动窗口内轨迹矢量方向变化量作为复杂度指标，仅当轨迹包含足够方向变化（如转弯段）时才更新 Sim(3) 矩阵，直线段冻结上次有效变换；
• 阈值触发强制更新：当融合结果与 GNSS 偏差超阈值时，跳过复杂度评估强制重算，防止变换矩阵长期不更新导致漂移。

5. 仿真与真机实验验证

• 仿真：基于 XTDrone 的 Gazebo 场景 + PX4 四旋翼（下视单目相机），用 QGroundControl 规划航线飞行，rosbag 录制 GNSS/图像/IMU 三类话题；通过发布 /use_gnss 话题模拟 GNSS 拒止，用 EVO 工具以 APE 指标定量评估。
• 真机：固定翼无人机（ArduPilot 飞控 + 机载计算单元）在开阔场地按预设航线自主飞行，30 Hz 图像 + 10 Hz GNSS 同步采集，对比分析有无轨迹复杂度机制下的定位精度。

Result（结果）

仿真实验

GNSS 可用段融合轨迹与 GNSS 参考轨迹高度吻合；拒止段误差缓慢累积，信号恢复后能快速修正回参考轨迹，仿真 RMSE 5.94 m（含初始化段），验证了系统可行性。

真机实验

轨迹复杂度机制带来显著提升：

指标 (m)	无机制	加入机制
max	49.93	34.83
mean	14.83	6.72
rmse	18.27	9.32
std	10.67	6.47

平均误差降低 54.7%；三轴分解显示 x/y 水平方向跟踪良好，z 轴高度方向误差也明显减小。

成果

• 完成毕业论文一篇，顺利通过答辩；
• 完整实现从数据预处理、Sim(3) 融合、轨迹复杂度判定到 Rviz 可视化、EVO 评估的全链路 C++/ROS 代码（data_process 功能包）。