GNSS/INS组合导航算法长期以来是Kalman滤波的天下；图优化算法由于在视觉定位中的优异表现而备受认可，正在延伸到传统组合导航中。为了方便组合导航同行学习和应用图优化算法，我们开源了一套GNSS/INS图优化算法，其中带有精化的IMU预积分并兼容轮式里程计，也具有可扩展的模块化架构。欢迎各位同行下载、引用、吐槽和建议。
随着自动驾驶、机器人、无人机的兴起，GNSS/INS组合导航系统已经成为智能无人系统必不可少的一部分。传统的组合导航一般采用卡尔曼滤波架构，它基于马尔科夫性假设，认为当前时刻的状态只与上一时刻的状态有关，并要求系统噪声和观测噪声为符合正态分布的白噪声。这在复杂环境中往往是不成立的，GNSS观测粗差也极易造成整个状态估计的系统性偏差。图优化算法通过牺牲一定的计算效率进行反复的线性化处理，实现了最优的状态估计。随着视觉SLAM的发展，图优化算法被广泛采用和快速演进，获得了越来越广泛的认可，这为组合导航算法研究开辟了一条新路。然而，开展相关研究不但需要学习理解各类传感器误差、图优化融合算法相关理论、方法，还需要在图优化融合算法实现上耗费很多精力，具有较高的学习门槛。
武汉大学i2Nav团队开源一套基于图优化的GNSS/INS组合导航系统（Optimization-Based GNSS/INS Integrated Navigation System，OB_GINS），为同行提供一款通用的、模块化的、易扩展的图优化GNSS/INS组合定位算法研究平台。该平台不仅为大家开展GNSS/INS组合导航研究提供了参照，也为基于图优化的多源融合定位研究奠定了基础。在我们的方案中，IMU预积分算法参考了经典惯导算法，充分考虑了地球自转等误差影响因素（唐海亮等，2021；蒋郡祥等，2020），并实现了IMU/ODO（轮式里程计）预积分算法（常乐等，2020）。
我们的源代码已经上传到GitHub，大家可以通过以下链接访问：
https://github.com/i2Nav-WHU/OB_GINS
OB_GINS是一套基于图优化的GNSS/INS组合导航代码，实现了GNSS定位和IMU原始测量的融合定位。OB_GINS采用滑动窗口的方式实现状态估计，图优化框架如图1所示。OB_GINS采用C++编程语言实现，通过调用配置文件的方式实现解算配置，OB_GINS的主要功能和特点如下所示：
1) 滑动窗口估计器：
a. 滑动窗口估计器的实现；
b. 融合IMU预积分因子、GNSS定位因子和先验因子实现状态估计；
2) 边缘化：
a. 边缘化数据结构的实现；
b. 边缘化求解，以及先验因子构建的实现；
3) 预积分：
a. 预积分状态数据结构的抽象化；
b. 预积分算法抽象化，基于C++类的多态特性实现了四种不同形式的预积分，包括：不考虑地球自转的一般IMU预积分算法、不考虑地球自转的一般IMU/ODO预积分算法、考虑地球自转的精确IMU预积分算法、考虑地球自转的精确IMU/ODO预积分算法；
c. 通用的IMU预积分接口的实现；
4) 姿态参数化工具实现：欧拉角、方向余弦矩阵、四元数和等效旋转矢量的相互转换；
5) 地理坐标系转换工具实现：
a. WGS84椭球模型参数计算工具；
b. 纬经高和XYZ坐标之间相互转换；
c. 局部直角坐标和地理坐标的相互转换；
6) 文件接口实现：
a. 文件接口抽象类实现，支持通用的文件加载和存储；
b. GNSS定位文件和IMU原始数据文件加载接口实现；
7) 配置文件接口实现：采用标准yaml格式配置文件接口；
8) 注释：中文注释为主，主程序和配置文件则使用中英文双语注释；
图1：OB_GINS图优化算法框架 [唐海亮，2021]
OB_GINS是一款通用的、模块化的图优化GNSS/INS组合定位算法研究平台，仅需要修改配置文件即可实现对不同的数据进行处理。可以使用的数据集如下：
1) 我们提供了一组示例数据，位于源代码仓库的dataset文件夹，包括GNSS定位数据、IMU数据、轮式里程计数据、参考真值以及对应的配置文件；
2) 我们开源了一套新颖的数据集，提供了四种不同精度的MEMS IMU原始数据、GNSS定位数据和对应的参考真值结果，可以在以下链接获取：https://github.com/i2Nav-WHU/awesome-gins-datasets；
3) 使用用户自己的数据集，OB_GINS使用的数据格式定义可以在我们开源数据集awesome-gins-datasets的说明文档中获得。
OB_GINS提供了一个良好的平台，以满足GNSS/INS组合导航领域的多种算法研究。针对组合导航系统的特点，我们给出了一些可尝试的研究方向，供大家参考：
1) 组合导航系统的初始对准算法研究：利用GNSS轨迹和IMU预积分，实现未知姿态情况下的初始姿态角估计；
2) 组合导航系统的快速初始化算法研究：不同于卡尔曼滤波马尔科夫假设，图优化能够通过反复线性化实现最优状态估计。因此，当IMU常值零偏误差较大时，卡尔曼滤波算法需要较长时间的数据才能够收敛；而图优化算法则在较短时间数据内就能收敛；
3) GNSS抗差算法研究：卡尔曼滤波算法中一旦出现量测粗差而没有即刻判断和剔除，对后续的状态估计都会造成持续影响；图优化算法则可以充分利用滑窗内的良好GNSS观测数据对滑窗内的GNSS粗差进行准确甄别和剔除，从而得到干净的当前状态估计；
4) 里程计内参和外参的估计算法研究：类似于基于滤波的组合导航算法，我们也可以增广里程计的内参和外参到预积分算法中，对其进行在线或者离线估计；
5) 平滑算法对比研究：当我们把滑动窗口的长度设置到很大时，整个图优化状态估计等效于全时段的状态估计，可以实现全时段的最优状态估计，此时图优化算法的抗粗差能力可以被更充分地表现出来（相比于传统RTS反向平滑算法或双向滤波算法）；
6) GNSS/INS紧组合算法研究：OB_GINS是模块化的可扩展架构，目前是以松组合为范例来实现的，感兴趣的同行可以换成紧组合的GNSS量测因子，进而由松组合方案进阶为紧组合方案。
[1] Tang Hailiang, Niu Xiaoji, Zhang Tisheng, Fan Jing, Liu Jingnan.Exploring the Accuracy Potential of IMU Preintegration in Factor Graph Optimization[J/OL].2021.[2021–09–08]. https://arxiv.org/abs/2109.03010
[2] Jiang Junxiang, Niu Xiaoji, Liu Jingnan. Improved IMU Preintegrationwith Gravity Change and Earth Rotation for Optimization-Based GNSS/VINS[J]. RemoteSensing, 2020, 12(18): 3048.
[3] Chang Le, Niu Xiaoji, Liu Tianyi. GNSS/IMU/ODO/LiDAR-SLAM IntegratedNavigation System Using IMU/ODO Pre-Integration[J]. Sensors, 2020,20(17): 4702.
版权所有:武汉大学多源智能导航实验室(微信公众号：i2Nav) 当前访问量: 技术支持:武汉楚玖科技有限公司