基于改进图优化的惯性/卫星/视觉组合导航方法

Inertial/satellite/vision integrated navigation method based on improved graph optimization

摘要

惯性/卫星/视觉组合导航在城市复杂环境下可以获得较高精度的连续定位信息，因此应用越来越广泛。但外界环境变化引起的传感器误差会导致组合导航精度下降，甚至造成组合导航结果发散。针对低成本惯性/卫星/视觉组合导航过程中因环境因素产生的传感器误差问题，提出基于改进图优化算法的惯性/卫星/视觉组合导航算法。在传统图优化算法的基础上，在代价函数中引入鲁棒核函数对环境引起的传感器误差进行过滤，并且在惯性测量单元（inertial measurement unit，IMU）预积分中加入地球自转加速度补偿来提高预积分精度，进而提高组合导航精度。最后基于kaist城市行驶数据集设计了对比实验，验证结果表明，提出的改进图优化方法可以有效过滤因外界环境因素产生的传感器误差，相比于传统的图优化方法导航精度提升了7%。

Abstract

Inertial/satellite/visual integrated navigation can obtain highly accurate continuous positioning information in complex urban environments， and therefore has become increasingly widely used. However， sensor gross errors caused by changes in the external environment can lead to a decrease in the accuracy of integrated navigation， and even cause divergence in the results of integrated navigation. In response to the problem of sensor gross errors caused by environmental factors in the low-cost inertial/satellite/visual integrated navigation process， an inertial/satellite/visual integrated navigation algorithm based on an improved graph optimization algorithm was proposed. A robust kernel function was introduced to the cost function based on the traditional graph optimization algorithm to filter the sensor gross errors caused by environmental factors， and earth rotation acceleration compensation to the IMU （inertial measurement unit） pre-integration was added to improve the accuracy of pre-integration， thus， the accuracy of integrated navigation was improved. Finally， a comparative experiment was designed based on the kaist urban driving dataset， and the results show that the improved graph optimization method proposed can effectively filter sensor gross errors caused by external environmental factors， and the navigation accuracy improves by 7% compared to the traditional graph optimization method.

关键词

组合导航 / 图优化算法 / 传感器误差 / 代价函数 / 鲁棒核函数

Key words

integrated navigation / graph optimization algorithm / sensor gross errors / cost function / robust kernel function

惯性/卫星/视觉组合导航的方式可以在城市复杂环境下获得高精度、高可靠性的连续定位信息^［1］。但是视觉信息会受到外界环境的影响，当环境中可移动物体较多或者光照变化时视觉里程计会出现较大误差。卫星信号因受到遮挡和反射影响，产生较大误差^［2］。这些误差都会对组合导航的精度产生影响，当误差较大时甚至会导致组合导航结果发散，失去导航作用。尽管惯性导航作为无源自主的导航方法，位姿信息主要来源于对陀螺仪和加速度计的积分，但是依然会受到地球自转的影响，随着时间增长，这种影响会因为加速度积分产生累积误差，影响组合导航精度^［3］。因此为了提高组合导航精度，需要过滤因环境因素引起的视觉里程计和卫星导航误差，并改进惯性导航积分方法，消除地球自转带来的影响。

为了过滤环境因素引起的传感器误差，研究者们提出了基于联邦卡尔曼滤波的容错数据融合方法^［4-5］，利用残差卡方检测的方法将子滤波器误差较大的数据进行滤除，以减少传感器误差对定位结果的污染。但是由于卡尔曼滤波方法服从一阶马尔可夫假设，所以更新过程中无法应用更多的历史信息。而图优化的方法可以使用更多的历史数据，因此在时间同步和精度上都更有优势^［6-7］。Strasdat等^［8］通过实验证明同等计算资源下，基于图优化的方法较滤波算法能获得更高的精度。特别是当传感器信号失锁时，图优化方法可以在传感器信号恢复后对失锁区间进行再处理。因此尽管图优化算法计算量较大，依然被大量应用于组合导航之中。韩勇强等^［9］基于图优化算法提出了基于滑动窗口的误差检测方法，对误差较大的数据进行剔除。Qin等^［10］在图优化算法框架下利用鲁棒核函数过滤因环境因素引起的传感器误差，实验表明该方法可以有效地降低环境因素对视觉导航的影响。现有的基于图优化的组合导航方案多采用低精度的IMU，因此往往忽视地球自转和有害加速度对解算过程的影响。Tang等^［11］通过实验验证了即便对于消费级IMU，传统的积分算法也不够精确。为了提高IMU的精度，研究者们提出了考虑地球自转的IMU预积分模型^［12-13］。

为了减少复杂环境下传感器误差对惯性/卫星/视觉组合导航结果产生的影响，本文提出基于改进图优化的惯性/卫星/视觉组合导航算法，在IMU预积分阶段，采用改进的IMU预积分模型消除地球自转对积分结果产生的影响。在数据融合过程中，采用SoftLOne鲁棒核函数对来自卫星和视觉里程计的数据进行滤波处理，过滤掉误差较大的数据。

1 多源融合图优化理论

因子图是概率图模型的一种^［14］，因子图优化即利用图模型来表达一个非线性优化问题。在基于因子图优化的组合导航模型中，采用节点表示待优化状态变量，采用端点表示与特定状态相关的传感器量测因子，模型的边表示各传感器的残差项。惯性/卫星/视觉组合导航的融合框架如图1所示。

在惯性/卫星/视觉组合导航中， k时刻的状态误差变量节点的最小化残差函数为

e \left(x_k\right) = h_k \left(x_k\right) - z_k （1）

基于因子图优化的组合导航算法，即最小化所有测量残差的先验和马氏距离之和^［13］，以获得最大后验估计。对于惯性/卫星/视觉组合导航，传统的图优化代价函数可表示为

\underset{X}{\mathrm{m}\mathrm{i}\mathrm{n}} \left\{\begin{array}{l}{r}_p-H_p{\mathit{X}}^{\mathrm{2}}+\sum _{k\in \left[\mathrm{1},n\right]}{r}_{\mathrm{I}\mathrm{M}\mathrm{U}}{\left({\widehat{z}}_{b_k}^{b_{k-\mathrm{1}}},\mathit{X}\right)}^{\mathrm{2}}+\\ \sum _{l\in L}{r}_c{\left({\widehat{z}}_l,\mathit{X}\right)}^{\mathrm{2}}+\sum _{d\in \left[\mathrm{0},m\right]}{r}_{\mathrm{G}\mathrm{P}\mathrm{S}}{\left({\widehat{z}}_d,\mathit{X}\right)}^{\mathrm{2}}\end{array}\right\} （2）

式中： r_{\mathrm{I}\mathrm{M}\mathrm{U}}为IMU预积分量测残差； r_c为视觉里程计量测残差； r_{\mathrm{G}\mathrm{P}\mathrm{S}}为GPS量测残差； \left\{r_p,H_p\right\}为边缘化后得到的先验信息。

2 IMU预积分

为了消除由地球自转产生的科氏加速度对IMU预积分产生的影响，需要对传统的IMU预积分残差函数进行改进，首先需要得到改进后的IMU动力学模型，进而得到改进后的IMU预积分残差函数。

2.1 动力学模型

IMU通常包括三轴加速度计和陀螺仪，用来测量机体坐标系下的角速度 {\tilde{\omega }}^b和比力 {\tilde{f}}^b。IMU的测量模型^［13］为

\left\{\begin{array}{c}{\tilde{\omega }}^b={\omega }^b+e^g+n^g\\ {\tilde{f}}^b=f^b+e^a+n^a\end{array}\right. （3）

式中： \tilde{\omega }、 \tilde{f}分别为陀螺仪和加速度计的测量值；e表示IMU的零偏； n表示噪声；上标b表示机体坐标系； a表示加速度计； g表示陀螺仪。

由IMU测量模型可知，IMU的测量值除了测量噪声和零偏外，还包含地球自转和地球重力加速度。为了提高IMU的精度，在传统的IMU预积分中考虑地球自转加速度 {\omega }_{ie}^w对预积分的影响，其中 {\omega }_{ie}^w = \mathrm{7.292} \times {\mathrm{10}}^{-\mathrm{5}} \mathrm{r} \mathrm{a} \mathrm{d} / \mathrm{s}。进而在预积分模型中引入了科氏力加速度修正 \mathrm{2} \left[{\omega }_{ie}^w\times \right] v_{wb}^w。如果陀螺仪的零偏小于 {\omega }_{ie}^w，说明陀螺仪可以在一定程度上感知地球自转，因此在预积分中应该考虑地球自转。可得IMU的动力学模型为

\left\{\begin{array}{l}{\dot{p}}_{wb}^w=v_{wb}^w\\ {\dot{v}}_{wb}^w={\mathit{R}}_b^w{f}^b+g^w-\mathrm{2}\left[{\omega }_{ie}^w\times \right]v_{wb}^w\\ {\dot{q}}_b^w=\frac{\mathrm{1}}{\mathrm{2}}{q}_b^w\otimes \left[\begin{array}{c}\mathrm{0}\\ \omega \end{array}\right],\omega ={\omega }_{ib}^b-{\mathit{R}}_w^b{\omega }_{ie}^w\end{array}\right. （4）

式中： {\dot{v}}_{wb}^w、 \omega分别为载体在世界系运动的加速度和角速度； {\mathit{R}}_b^w、 q_b^w分别为机体系到世界系的旋转矩阵和四元数； p_{wb}^w \mathrm{、} v_{wb}^w分别为载体在世界系中的位置和速度； g^w为世界系中的重力加速度。地球表面实用重力值

\begin{array}{l}g=\mathrm{9.780}\mathrm{325}\times \left(\begin{array}{l}\mathrm{1}+\mathrm{0.005}\mathrm{302}\mathrm{40}\times \mathrm{s}\mathrm{i}\mathrm{n}{L}^{\mathrm{2}}-\\ \mathrm{0.000}\mathrm{005}\mathrm{82}\times {\left(\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{2}L\right)}^{\mathrm{2}}\end{array}\right)\\ -\mathrm{3.08}\times {\mathrm{10}}^{-\mathrm{6}}h\end{array} （5）

式中： L为当地纬度； h为海拔高度。

2.2 IMU预积分残差函数

根据改进的IMU动力学模型，可以写出IMU的预积分残差函数

r_{\mathrm{I}\mathrm{M}\mathrm{U}} \left({\widehat{z}}_{b_k}^{b_{k-\mathrm{1}}},\mathit{X}\right) = \left[\begin{array}{c}{r}_p\\ r_v\\ r_q\\ r_{ba}\\ r_{bg}\end{array}\right] （6）

其中

\left[\begin{array}{c}{r}_p\\ r_v\\ r_q\\ r_{ba}\\ r_{bg}\end{array}\right] = \left[\begin{array}{l}{\left(q_{b_i}^w\right)}^{\mathrm{T}}\left(\begin{array}{l}{p}_{w{b}_j}^w-p_{b_i}^w-v_{b_i}^w\mathrm{\Delta }t\\ -\frac{\mathrm{1}}{\mathrm{2}}{g}^w\mathrm{\Delta }{t}^{\mathrm{2}}+\\ \mathrm{2}\left[{\omega }_{ie}^w\times \right]{\displaystyle \sum _{k=\mathrm{1}}^N}\left(p_{w{b}_k}^w-p_{w{b}_i}^w\right)\mathrm{\Delta }{t}_k\end{array}\right)-{\alpha }_{b_m{b}_j}\\ {\left(q_{b_m}^w\right)}^{\mathrm{T}}\left(\begin{array}{l}{v}_{w{b}_j}^w-v_{w{b}_m}^w\\ -g^w\mathrm{\Delta }t+\\ \mathrm{2}\left[{\omega }_{ie}^w\times \right]\left(p_{w{b}_k}^w-p_{w{b}_m}^w\right)\end{array}\right)-{\beta }_{b_m{b}_j}\\ \mathrm{2}{\left[{\gamma }_{b_j{b}_m}\otimes \left(q_{b_{m}w}\otimes q_{w{b}_j}\right)\right]}_{xyz}\\ b_j^a-b_m^a\\ b_j^g-b_m^g\end{array}\right] （7）

式中： {\alpha }_{b_m{b}_j}、 {\beta }_{b_m{b}_j}、 {\gamma }_{b_m{b}_j}分别为IMU在图像 m、 j之间的位移、速度、旋转预积分量； \mathrm{\Delta } t为相机采样时间间隔； N为在相机采样时间内IMU测量的个数。 b^a、 b^g分别为加速度计和陀螺仪的零偏。

3 改进图优化组合导航方法

在非线性优化过程中，传统的图优化方法将最小化二范数的平方和作为残差函数，因此，由环境变化等因素产生的异常值将在图优化过程中引入一条误差很大的边，该边的梯度会非常大，减弱其他正确边的影响，使优化算法专注于调整一个错误的值，最终导致优化结果误差增大甚至发散。为了增加系统的鲁棒性，提高组合导航精度，本文采用改进的图优化方法对惯性/卫星/视觉组合导航进行优化。

组合导航系统滑动窗口内待优化的状态估计向量 \mathit{X}表示为^［15］

\begin{array}{l}\mathit{X}=\left[x_{\mathrm{0}},x_{\mathrm{1}},\cdots ,x_n,x_c^b,{\lambda }_{\mathrm{0}},{\lambda }_{\mathrm{1}},\cdots ,{\lambda }_l\right]\\ {\mathit{x}}_k=\left[p_{w{b}_k}^w,q_{w{b}_k}^w,v_{w{b}_k}^w,b_{a_k},b_{g_k}\right],k\in \left[\mathrm{0},n\right]\\ x_c^b=\left[p_{bc}^b,q_c^b\right]\end{array} （8）

式中： {\mathit{x}}_k为第k关键帧的状态向量； x_c^b为相机到IMU的外参； \lambda为3D点的逆深度。

在传统图优化代价函数的基础上引入鲁棒核函数的代价函数为

\underset{\mathit{X}}{\mathrm{m}\mathrm{i}\mathrm{n}} \left\{\begin{array}{l}{r}_p-H_p{\mathit{X}}^{\mathrm{2}}+\sum _{k\in \left[\mathrm{1},n\right]}{r}_{\mathrm{I}\mathrm{M}\mathrm{U}}{\left({\widehat{z}}_{b_k}^{b_{k-\mathrm{1}}},\mathit{X}\right)}^{\mathrm{2}}+\\ \sum _{l\in L}\rho \left(r_c{\left({\widehat{z}}_l,\mathit{X}\right)}^{\mathrm{2}}\right)+\sum _{d\in \left[\mathrm{0},m\right]}\rho \left(r_{\mathrm{G}\mathrm{P}\mathrm{S}}{\left({\widehat{z}}_d,\mathit{X}\right)}^{\mathrm{2}}\right)\end{array}\right\} （9）

式中： \rho为鲁棒核函数。

本文采用SoftLOne核函数，该核函数可表示为

\rho \left(s\right) = \left\{\begin{array}{l}s\\ \mathrm{2}\left(\sqrt[]{\mathrm{1}-s}-\mathrm{1}\right)\end{array}\right. \begin{array}{cc}& s\le {\tau }^{\mathrm{2}}\\ & s>{\tau }^{\mathrm{2}}\end{array} （10）

式中： \tau为残差函数的阈值，当残差大于该阈值时，则认为该处误差超过正常误差范围。此时，函数的增长由二次变为一次，相当于限制了梯度的最大值，降低对异常值的敏感程度，进而过滤误差较大的数据。同时保证代价函数自身的光滑性，使得整个优化结果更加稳定。

4 实验验证

为验证本文算法的精度，采用Kaist urban38城市环境导航数据集^［16］进行仿真实验，数据集包含一个消费级低精度的IMU、低精度的GPS和双目相机，并由网络RTK提供真实轨迹。数据集总里程11 192 m，总时长超过2 800 s。行驶过程中，环境特征复杂，多次经过楼群、涵洞和红绿灯路口，光照变化明显，可移动特征较多，均对惯性/卫星/视觉组合导航产生不利影响。

为了验证改进算法的有效性，设计实验如表1所示。实验1采用传统的图优化方法对各传感器数据进行融合；实验2只对视觉里程计采用鲁棒核函数约束；实验3采用改进的因子图优化方法对视觉里程计和GPS均采用鲁棒核函数约束；实验4则在实验3的基础上在IMU预积分中加入地球自转科氏加速度修正。

将实验1和实验2获得的轨迹和真实轨迹对比，可得图2和图3。由图2和图3对比可知，采用传统图优化方法的实验1组合导航结果发散，因为城市环境中路况复杂，可移动目标较多，视觉图像容易发生误匹配，或者将可移动目标识别为特征点，使视觉里程计漂移，产生较大误差，造成组合导航系统严重发散丧失导航作用。实验2在采用鲁棒核函数约束视觉里程计残差函数后，由图3可知实验轨迹和真实轨迹基本拟合，因此证明鲁棒核函数可以有效过滤因外界环境原因产生的视觉里程计误差。

使用EVO工具比较实验2、3、4的轨迹和真实轨迹可得各实验的绝对位置误差（absolute pose error，APE）如图4~6所示。

由绝对误差可得实验2、3、4的平均误差、中值、均方根误差和标准差，如表2所示。

比较实验2、3、4的数据可知，对卫星导航采用鲁棒核函数滤波的实验3相比于未采用该技术的实验2，平均绝对误差减少2%，说明针对GPS传感器产生的误差，鲁棒核函数也具有一定的约束作用。采用本文提出的改进图优化方法的实验4相比仅对视觉里程计残差函数采用鲁棒核函数约束的实验2平均绝对误差减少了7%，中值、均方根误差和标准差各数据也低于实验2。

5 结 论

本文针对城市复杂环境下的环境干扰问题提出了基于改进图优化的惯性/卫星/视觉组合导航算法，采用改进的IMU预积分残差函数，并采用鲁棒核函数约束视觉里程计和GPS的残差函数。实验表明，改进的图优化算法可以有效地抑制因外界环境变化引起传感器误差，特别是对视觉里程计因环境因素产生的较大偏移具有明显作用，有效抑制了组合导航结果发散，提高了系统的鲁棒性。本文提出的改进图优化方法相比于传统图优化方法，导航结果的平均误差减少了7%。

参考文献

[1] 蒋郡祥.基于图优化的视觉/惯性/GNSS融合导航方法研究［D］.武汉：武汉大学，2021.

[2] 方文轩，丛佃伟.卫星/惯性/视觉组合导航多源融合技术现状及发展［J］.无线电工程，2022，52（10）：1813-1820.

[3] 杨显赐，乔书波，肖国锐，基于精化预积分的因子图GNSS/INS松组合定位［J］.测绘工程，2023，32（6）：29-37.

[4] 蒋锐，李俊，徐友云，基于联邦卡尔曼滤波器的容错GPS-AOA-SINS组合导航算法［J］.通信学报，2022，43（8）：78-89.

[5] 胡晓强，武东杰，彭侠夫.INS/GNSS/VO组合导航系统复合型异常检测与容错算法［J］.中国惯性技术学报，2023，31（2）：148-156.

[6] 赵壮，马国梁.自适应滤波协同图优化导航方法研究［J］.仪器仪表学报，2023，44（7）：271-281．

[7] 宋江波，姚荷雄，李婉清，卫惯视组合导航技术发展趋势［J］.导航定位学报，2023，11（2）：36-48.

[8] Strasdat H， Montiel J M M， Davison A J.Visual SLAM：why filter？［J］.Image and Vision Computing，2012，30（2）：65-77.

[9] 韩勇强，于潇颖，纪泽源，面向城市复杂环境的GNSS/INS高精度图优化算法［J］.中国惯性技术学报，2022，30（5）：582-588.

[10] Qin T， Li P L， Shen S J.VINS-mono：a robust and versatile monocular visual-inertial state estimator［J］.IEEE Transactions on Robotics，2018，34（4）：1004-1020.

[11] Tang H L， Niu X J， Zhang T S，et al.Exploring the accuracy potential of IMU preintegration in factor graph optimization［J］.ArXiv e-Prints，2021：arXiv：

[12] Barrau A， Bonnabel S.A mathematical framework for IMU error propagation with applications to preintegration［C］//2020 IEEE International Conference on Robotics and Automation （ICRA）.Paris，IEEE，2020：5732-5738.

[13] Jiang J X， Niu X J， Liu J N.Improved IMU preintegration with gravity change and earth rotation for optimization-based GNSS/VINS［J］.Remote Sensing，2020，12（18）：3048.

[14] 张小红，张元泰，朱锋.城市复杂场景下GNSS定位的因子图优化方法及其抗差性能分析［J］.武汉大学学报（信息科学版），2023，48（7）：1050-1057.

[15] 徐晓苏，吴贤.基于IMU预积分封闭解的单目视觉惯性里程计算法［J］.中国惯性技术学报，2020，28（4）：440-447.

[16] Jeong J， Cho Y， Shin Y S，et al.Complex urban dataset with multi-level sensors from highly diverse urban environments［J］.International Journal of Robotics Research，2019，38（6）：642-657.

基金资助

国家重点研发计划项目(2022YFC2903800)