基于因子图优化的GNSS/INS松组合导航

基于因子图优化的GNSS/INS松组合导航

罗明强

中国市政工程西南设计研究总院有限公司  四川省成都市 610000

摘要：随着智能时代的到来，智能无人系统如自动驾驶、机器人、无人机等受到了广泛关注和快速发展。在这些系统中，高精度、高可靠的定位定姿信息是进行有效环境感知、路径规划和运动控制的基础。因此，多源融合导航技术成为了实现智能无人系统自主化的核心技术之一。传统的GNSS/INS组合导航系统可以提供连续高精度的导航信息。然而，在GNSS信号受阻的环境下，该系统将退化为纯惯性导航，如果使用低成本的IMU，定位定姿误差将快速发散，无法满足要求。视觉惯性导航是智能无人系统中常用的导航技术之一。作为一种递推导航系统，它的定位和航向误差也会发散，并且容易受到外部环境视觉纹理条件的影响。虽然回环校正可以在一定程度上消除累积漂移，但在实际工作场景中很难获得修正信息。相比之下，多源融合导航技术同时利用视觉、惯性和GNSS定位信息，充分利用它们在导航能力方面的互补性，克服了仅使用单一或两种信息源导航的局限性。

关键词：多传感器融合导航；GNSS/INS；图优化、

1引言

惯性导航系统（InertialNavigationSystem,INS）是一种基于牛顿力学的航迹推算系统，其核心部件是惯性测量单元（InertialMeasurementUnit，IMU）。IMU由三组相互正交的陀螺仪和加速度计刚性固连在一起构成，通过对IMU量测进行投影变换和多次数值积分，即可对载体的位置、速度和姿态信息进行连续推算，具有完全自主工作和短期精度高的特点。然而，作为一种航迹推算导航系统，受到IMU自身量测误差和INS解算算法误差等因素的影响，INS的推算误差会随时间不断地累积，误差累积的速度主要与IMU的精度等级有关。在视觉导航方面，同步定位与建图（SimultaneousLocalizationAndMapping,SLAM）问题于上个世纪90年代首次提出，其核心目的是利用机器人自身所搭载的传感器实现运动估计，同时构建出关于周围环境的地图，以解决机器人在未知环境下的导航问题。因为相机具有结构紧凑、质量轻、成本低、图像信息丰富等特点，所以基于相机的SLAM方案得到了广泛研究，这种SLAM方案也被称为视觉SLAM。视觉里程计（VisualOdometry,VO）可认为是视觉SLAM的部分实现，其主要负责对相机位姿的连续递推估计，而不专注于地图构建。若同时使用相机与IMU对载体的运动状态进行估计，则称这种导航系统为视觉惯性里程计（Visual-InertialOdometry,VIO），对应的SLAM方案也被称为视觉惯性SLAM。这两种异构传感器在很多方面是互补的，因此可以相互辅助。一方面，INS的短期推算结果可以恢复帧间运动，还可以辅助视觉特征匹配；若视觉使用的是单目相机，惯性信息还可以恢复系统的真实尺度；并且加入惯性信息后，因重力可观测，使得系统的水平姿态误差不会随时间或空间而累积。另一方面，基于视觉的帧间约束信息，系统估计器可以对IMU的可建模误差进行在线估计，提高INS的短期推算精度。

2面向中高精度IMU的改进预积分算法

通常情况下，IMU的采样频率高达几百赫兹，对于基于滤波器实现的多源融合导航系统，可以利用INS误差模型构建状态转移矩阵，以预测未来状态和传播不确定度，再利用其他低频传感器的量测完成状态更新。但对于基于优化的多源融合导航系统，以滤波器频率执行更新，并估计每个IMU采样时刻的运动状态是不现实的。一种可行的方案就是对低频传感器采样间隔内的所有IMU量测进行积分，以形成一个统一的相对运动约束。如果以世界系作为积分参考系，取积分起始时刻的状态估计作为积分起点，那么在优化器每次迭代更新之后，积分起点都会变化，需要重新积分，且起始时刻的不确定度也需要通过反向传播（BackPropagation）重新确定，再伴随积分过程一起传播，显然这种方式会造成计算成本的大幅增加。IMU预积分技术的巧妙之处在于，其选取积分起始时刻的历史固定IMU量测参考系作为积分参考系，在起始时刻，IMU相对其自身的运动状态完全确定，且不会随优化过程发生改变，凭此解决上述积分起点变化的问题。现有的一些预积分算法都在针对姿态的参数化方式和数值积分方法进行改进，而没有将地球自转和重力变化考虑在内。与此形成鲜明对比的是，INS解算算法和以INS为核心的组合导航相关算法均对地球自转角速度和重力矢量进行了周密建模。就地球自转而言，其数值为15h，这意味着如果不对其进行妥善建模和考虑，即使是在高精度的陀螺仪也会退化到战术级水平，无法发挥出其应有的价值，而高等级的IMU常用于复杂环境下的建图工作。为了使IMU的量测精度在预积分算法中得以充分发挥，本文所提的改进预积分算法将充分考虑地球自转和重力变化，并加入INS解算算法中的圆锥效应补偿和划船效应补偿。为与其他传感器量测进行融合，在构建优化目标函数时，需要先利用均方根信息矩阵对预积分残差函数加权，受均方根信息滤波器的工作原理启发，本文将直接使用均方根信息矩阵来度量和传播预积分误差的不确定性。

3视觉/惯性/GNSS融合导航动基座初始化算法

研究的视觉/惯性/GNSS融合导航方案使用优化器完成多源融合状态估计，由于优化目标函数具有较强的非线性，为使迭代估计尽快收敛至最优解位置，优化算法需要较为准确的迭代初值，否则，迭代估计的收敛速度会十分缓慢，甚至会出现无法收敛的情况。因为旋转的不可交互性是导致优化目标函数非线性的主要因素之一，所以旋转状态的初值精度就变得尤为重要，一般来说4~5度以内即可满足基于EKF滤波器的GNSS/INS组合导航应用需求，考虑到优化器应对非线性的能力更强，此标准在基于优化器的多源融合方案中应该可以适当放宽。由于相机与IMU均有各自的参考坐标系，且GNSS天线相位中心与IMU的量测中心也不重合，为实现不同导航信息源之间的信息融合，这些参考坐标系之间的相对关系是必须的，包括相机与IMU之间的相对平移与旋转，IMU量测中心到GNSS天线相位中心的杆臂（简称GNSS天线杆臂），为了表述方便，我们统一称这些相对关系为传感器间的外部参数，简称传感器外参。在实际应用中，传感器外参可能是未知的或者不方便进行离线标定，例如全新的设备或者传感器平台已经刚性固连于载体之上（尤其是分散安装在车辆的不同部位），为实现视觉/惯性/GNSS融合导航系统的快速部署，将为视觉/惯性/GNSS融合导航系统设计一种动基座初始化算法。该算法可以在载体运动状态下完成对载体位置、速度、姿态、IMU偏置、地图点位置，以及传感器外参的初始估计。设计的动基座初始化算法要求在初始化期间双目视觉里程计独立工作，初始化算法的输入即为双目视觉里程计输出的位姿以及IMU预积分和GNSS定位结果，初始化的具体流程为：首先，基于手眼标定法（hand-eyecalibration）计算相机与IMU之间的相对旋转（即安装角），再计算陀螺仪的常值偏置；接着，利用双目视觉里程计提供的位姿和IMU预积分计算重力矢量以及相机与IMU之间的相对平移（即杆臂），再结合不同时刻间的载体运动相对平移矢量，计算视觉参考世界系和多源融合导航参考世界系之间的对准矩阵，具体计算方法分GNSS天线杆臂已知和未知两种情况；然后，利用当前的传感器外参标定结果、IMU预积分、两个参考世界系间的对准矩阵，以及相机位姿来计算加速度计偏置；最后，恢复在多源融合导航参考世界系下的载体位姿和速度，以及地图点的位置。

4结束语

本文针对室外智能无人系统对连续、稳健、高精度定位定姿信息的需求搭建了一套视觉/惯性/GNSS融合导航系统，包括核心算法设计和软硬件平台实现，完成了预期目标。然而，视觉/惯性/GNSS融合导航本身是一种涉及到GNSS高精度定位、惯性导航、视觉导航的交叉学科研究课题，具有很强的挑战性，

参考文献

[1]许海.基于UWB/MEMS组合的室内定位算法研究[D].广西大学,2023

[2]何浩洋,夏荣斌.基于双因子抗差估计的SINS/BDS有轨电车组合导航算法研究[J].铁道学报,2022,44(08):50-59.

[3]王鑫耀,王双甲,何梓君等.基于地球坐标系的全球惯性导航与组合导航方法[J].测控技术,2022,41(06):112-117.