基于自适应因子图优化的导航系统信息融合方法

2023-02-16戴海发卞鸿巍杜红松

中国惯性技术学报 2023年1期

戴海发，卞鸿巍，杜红松，姚曜，王涵

（1.中国人民解放军91977 部队，北京 100036；2.海军工程大学电气工程学院，武汉 430033）

惯性导航系统具备自主性、连续性和隐蔽性等优势，在军事和民用领域得到了广泛应用。然而，由于惯性元件误差，惯性导航系统误差会随着时间累积而发散。为了抑制或者减少误差累积，在过去几十年中，国内外学者进行了深入研究，主要通过融合其他传感器信息来修正惯性导航系统误差，从而抑制惯导误差的发散。

信息融合最常用的方法主要为卡尔曼滤波(Kalman Filter,KF)及其扩展方法[1,2]，但是基于KF 的方法处理不同步且存在延迟的多源信息融合问题需要较为复杂的处理，计算冗余且只能得到次优解[3]，因此，文献[3]提出采用基于因子图的方法解决惯性导航系统的多源信息融合问题，该方法具有即插即用、可处理多源信息异步与延迟问题的优点。因子图是一种概率图模型，最早由Kschis[4]提出，Kaess[5]将其用于同时定位与地图构建(Simultaneous Localization and Mapping,SLAM)，解决批量优化算法的计算量问题，从而保证实时运行。Indelman[6]将其拓展到惯性组合导航系统中[3]，并证明用因子图方法求解非线性问题的精度要优于传统的扩展卡尔曼滤波方法（Extended Kalman Filter,EKF），而且与固定滞后的滤波方法相比，在计算量和性能上都有所改进。文献[7,8]分别将因子图的方法应用于惯性传感器的在线校准问题和无人艇及直升机的多源组合导航问题，解决了传感器不同步以及信息滞后等问题。

对于因子图优化方法，测量值的误差模型非常重要。通常情况下误差模型都是根据实验得到或者依靠经验给定，然而实际中并不总能得到基准数据，因此准确的误差模型很难得到，而通过经验确定的模型往往不够准确。此外，即使得到了先验误差模型，误差模型也可能会随着环境的变化而变化，在这种情况下使用固定的误差模型可能会导致得不到最优解[9]。

一种解决方法是使用鲁棒代价函数的方法，包括常规的Huber 代价函数[10]、可变换约束法（Switchable Constraints）[11]、动态尺度协方差法[12]、最大最小法（Max-Mix）[13]和一致性检查[14]等方法。对于最优估计问题，鲁棒性和精度通常是矛盾的，鲁棒性方法是以牺牲一定的精度为代价来换取系统的抗干扰能力，因此获得的解不是最优的。另一种解决方法是同时估计状态和协方差矩阵。尽管在这方面已经有了一些研究，如文献[15][16]分别提出了基于CELLO 和基于PROBE 的自适应协方差估计算法，不过它们都依赖真实值。文献[17]在文献[16]的基础上结合了最大期望（Expectation-Maximization,EM）算法实现了无基准的自适应估计方法，相似方法的使用和估计在文献[18]也被提出。上述文献都是针对图像特征提出的误差建模方法，而对于常规传感器的误差建模问题，目前研究的较少。文献[19]针对常规的传感器提出了一种基于单点残差的自适应因子图方法(Adaptive Factor Graph Optimization Method Using Single Residual,SRAFGO)，但是该方法依赖于较准确的先验知识，而且只利用了单个残差，对协方差矩阵的估计并不是最优的。表1 总结了目前主要自适应估计方法的限制条件，从表中可看出，由于约束条件及应用对象的限制，传统的自适应估计方法对于常规传感器并不适用。

表1 自适应估计方法对比Tab.1 Summary of adaptive estimation methods

针对上述问题，本文提出了一种基于滑动窗的自适应因子图优化方法(Adaptive Factor Graph Optimization Method Based on the Sliding Window,SWAFGO)，利用滑动窗内的残差序列来估计协方差矩阵。首先介绍常规的因子图优化算法，然后通过最大后验概率分别推导了基于单点残差的自适应因子图方法和基于滑动窗的自适应因子图方法，最后通过仿真实验和KITTI 数据集实验验证了SWAFGO 算法的可行性。

1 基于因子图的优化方法分析

假设导航系统配置了输出频率不同步的传感器，包括输出频率较高的惯性测量装置(Inertial Measurement Unit,IMU）以及输出频率相对较低的全球导航卫星系统(Global Navigation Satellite System,GNSS)、视觉里程计(Visual Odometer,VO)等。基于部分传感器在复杂环境下可能变得不可用或只在特定情况下可用，组合导航通过融合所有可用的信息源得到最优导航解，从而实现组合导航的目的。

假设x表示导航状态，包含载体的位置p、速度v和姿态ψ信息；c表示IMU 的校正参数，包括加速度计零偏和陀螺零偏。令xi、ci分别表示ti时刻的导航状态和校正参数，定义t1时刻到当前tk时刻的一组导航状态和校正参数为X=，C=。

假设Λ为t1时刻到当前tk时刻的所有变量集合Λ={X,C}，ti时刻的变量集合Λi={xi,ci}。

根据上述定义，后验概率分布函数为p(Λ|Z)。其中，Z为t1时刻到当前tk时刻接收到的所有观测值。假设ti时刻的观测值为zi，则Z=，那么待估计参数的最大后验估计为：

根据贝叶斯公式，后验概率分布函数可以分解成一个先验信息和独立过程以及测量模型。令p(Λ0) 表示所有的可用先验信息，则后验概率分布函数的因式分解可写成：

式(2)可以表示成因子图模型，如图1 所示。因子图是一种双边图G=(F,Λ,Ω)，G表示因子图，F表示因子节点，Ω表示因子节点和变量节点的关系。图中包含两种类型的节点：因子节点fi∈F和变量节点υi∈Λ，边eij∈Ω表示因子节点fi和变量节点υj存在关系。每个因子节点都可表示式(2)中的一个独立项，因此：

图1 高频率IMU 测量值、低频率GNSS、VO 测量值的因子图模型示意图Fig.1 Diagram of factor graph model: high-frequency IMU measurements,lower-frequency GNSS and VO measurements

式中，∝表示正比关系。

每个因子fi表示一个应当最小化的误差函数。假设这个误差函数为err(Λi,zi)，则因子fi可定义为：

式中，d(·)表示代价函数。

对于高斯噪声分布，因子fi满足如下形式：

特别地，表示测量模型的因子定义为：

式中，h(·)为非线性测量函数，zi为ti时刻的观测值。

式(1)的最大后验估计问题变成了非线性最小二乘函数的最小值求解问题：

2 常规传感器因子

先验信息p(Λ0)可以进一步分解为某些变量的独立信息，每一个独立信息可以表示成一个分离的先验因子。变量υ∈Λ0的先验因子是一个一元因子：

对于高斯分布，先验信息以均值μυ和方差Συ方式给出：

2.1 IMU 等效因子

导航状态x随时间变化关系可以用非线性微分方程表示为：

式中，h为惯性导航系统的微分方程，fb、ωb分别为惯性传感器测量得到的比力和角速度。c表示IMU校正参数，可根据IMU 误差模型修正IMU 测量值fb和ωb。IMU 误差模型的估计通常与导航状态x的估计结合在一起。采用KF，式(10)的线性过程会产生雅克比矩阵和过程噪声状态方程，具有较复杂计算过程。

通常，c随时间的变化关系可以用自身的非线性模型即随机游走来描述：

其中，g 表示校正参数的非线性函数。

式(10)(11)的离散化过程为：

2.2 GNSS 测量模型因子

一般的，GNSS 测量方程可表示为：

式中，nGNSS为GNSS 的测量噪声，hGNSS(xk)为GNSS的测量函数，表示载体位置与测量值之间的关系。当存在杆臂时，姿态也是测量函数的一部分。式(15)定义了一个一元因子：

该因子只与表示当前导航状态的节点xk有关。当GNSS 的测量值为位置pk时，hGNSS(xk)=pk，GNSS因子为：

当GNSS 的测量值为伪距时，也可以按照同样的方法得到GNSS 的因子表达式。

2.3 视觉里程计测量模型因子

对于单目视觉里程计建模，可参考文献[3]。对于双目视觉里程计，它可测量tj～tk时刻之间的位姿变化，其测量函数为：

式中，nVO为VO 的测量噪声，hVO(xj,xk)为VO 的测量函数，表示载体位姿变化与测量值之间的关系，Tk、Tj分别表示这两个时刻的位姿，其测量模型因子可以表示为：

3 自适应协方差估计

上述的因子图优化方法是在假设协方差矩阵Σi是在已知条件下得到的，但是实际情况是这一点很难保证。实际应用过程中，导航系统一般都没有基准数据，因此准确的先验协方差矩阵通常很难得到，此外协方差还将随着时间、环境的变化而变化。针对这些问题，一般需要自适应地估计协方差矩阵。本节首先推导单点残差协方差估计方法，然后进一步推导滑动窗协方差估计方法。

3.1 单点自适应协方差估计

在第1 节基于因子图的优化方法中，最大后验估计问题可以写成：

式中，k表示测量值的总个数，ei(x)表示第i个节点的估计误差信息。

如果协方差矩阵未知，因子图优化方法就变成了求解估计状态和协方差的问题。此时，代价函数变为：

式中，Σ={Σ1,Σ2…Σk}表示所有未知的协方差矩阵。

从而，最大后验估计问题可修改为：

为了避免估计产生过拟合的问题，需要提供协方差矩阵的先验值。一般的协方差矩阵先验满足Inverse-Wishart 分布[19]：

式中，Ψi＞0为尺度矩阵，λi＞ni-1为自由度参数，并且ni=dim(Σi)，其概率密度函数满足下列形式：

其中，det(·)表示求矩阵的行列式，tr(·)是迹函数，表示多变量Gamma 函数。

最大后验问题的目标函数的概率形式为：

对后验概率进行分解：

式中，p(Λi|zi,Σi)为协方差矩阵Σi已知条件下的后验概率密度函数，根据贝叶斯公式可以写为：

式中，p(Λi)、p(zi)分别为状态Λi和量测zi的先验概率密度，由于p(zi)与Λi和Σ都无关，因此可忽略。p(zi|Λi,Σi)为似然概率密度，其表达式为：

式中，测量噪声vi的表达式为：

将式(27)(28)代入式(26)，得到：

其中，αi=λi+ni+2，且忽略了与Λi或Σ无关的项。

为了使算法能够实时运行，需要先估计最优的协方差矩阵Σi，以便从表达式中把它消除。对式(31)关于求导并等于0，得到协方差矩阵的估计：

通过式(32)可以看出，估计的协方差矩阵由两部分组成：常量部分和与量测噪声相关的部分。

3.2 滑动窗函数协方差估计

3.1节方法是在假设有较准确的先验协方差矩阵的条件下推导得到的，如果没有先验的协方差矩阵知识，而且实际中也无法得到准确的状态估计值Λi，那么就只能得到状态的近似值，似然概率密度p(zi|Λi,Σi)将变成：

式中，Gi表示第i个观测后的残差协方差矩阵。

将式(33)代入式(26)(29)得到：

为了方便分析，先考虑只有一种观测量的情形，假设同一种观测量的协方差矩阵相同，即G1=G2=…=GN=G。式(34)对G-1求导并等于0，得到残差协方差矩阵的估计为：

残差序列为：

式中，H为测量函数的雅克比矩阵，为导航状态估计误差。

残差序列满足零均值高斯分布，即E(e)=0。因此，残差序列的协方差矩阵可表示成：

根据式(12)，IMU 的运动方程为：

式中，

其中，Ak为IMU 的运动方程fIMU对位置、速度、姿态的偏导，Bk、Fk分别为运动方程对比力和角速度的偏导，分别是加速度计和陀螺的量测噪声协方差矩阵。

根据式(37)(39)得到量测协方差矩阵的估计为：

为了使算法能够实时运行，本文选取滑动窗自适应协方差估计技术[20]，即利用窗口大小为N的滑动窗内的残差序列来估计残差的协方差矩阵：

滑动窗原理如图2 所示。滑动窗技术可以减少过去残差的影响，提高动态估计能力，通过保持一定数量的残差减少计算量和数据存储量。

图2 滑动窗原理图Fig.2 Diagram of sliding window

4 实验验证

本节使用仿真实验和 KITTI 数据（http://www.cvlibs.net/datasets/kitti）实验来验证所提出算法的有效性，并与传统因子图优化方法(Factor Graph Optimization,FGO)和单点残差自适应因子图SRAFGO 方法进行对比。算法性能均方根误差定义如下：

式中，xk∈xk为tk时刻状态的某个标量分量，M为Montel Carlo 实验次数，表示第i次试验tk时刻状态真值，表示第i次试验tk时刻状态估计值。

选择估计结果的最后L个RMSE 的平均值作为衡量统计尺度，定义如下：

在本文中选取M=50，L=100，根据经验设定自适应窗口大小N=60 。

4.1 仿真实验

假设水面无人艇以15m/s 的速度航行，初始位置：经度λ=108.9178°，纬度L=34.2483°，初始航向为北向，仿真时长为400 s。运动轨迹如图3 所示，红色五角星表示水面无人艇初始位置，蓝色实心圆表示无人艇的终止位置。传感器参数设置如表2 所示。

图3 真实运动轨迹Fig.3 Real trajectory

表2 传感器配置Tab.2 Sensors specification

（1）协方差矩阵影响实验

设置协方差矩阵R为真实协方差矩阵Rtrue的0.1、1、10 倍，得到传统因子图优化方法的结果如图4-5所示。表3 为这三种情况下FGO 方法的位置和航向的TRMSE。

由图4 可以看出，传统的因子图优化方法对协方差矩阵比较敏感，协方差矩阵过大或过小都会引起位置误差的增大。由图5 可以看出，当协方差矩阵过小时，航向误差逐渐发散。

图4 不同条件下FGO 方法的位置RMSEFig.4 Position RMSE of the FGO method versus different covariance matrix

图5 不同条件下FGO 方法的航向RMSEFig.5 Yaw RMSE of the FGO method versus different covariance matrix

由表3 可以看出，当协方差矩阵为真实协方差矩阵的0.1 倍时，与真实协方差矩阵相比，其位置和航向的TRMSE 分别从0.98 m 和0.23 °增大到2.26 m 和6.92 °；而当协方差矩阵为真实协方差矩阵的10 倍时，与真实协方差矩阵相比，其位置和航向的TRMSE 分别从0.98 m 和0.23 °增大到2.25 m 和1.01 °。由此可以得出以下结论：因子图优化方法对于协方差矩阵比较敏感，在不知道协方差矩阵的条件下，设置较大或较小的协方差矩阵都将引起较大系统误差。

表3 定位误差和航向误差的TRMSETab.3 TRMSE of position and yaw

（2）无先验知识实验

设置初始协方差矩阵为真实协方差矩阵的10 倍,即R=10Rture，分别采用FGO、SRAFGO、SWAFGO三种方法得到的结果如图6-8 所示，表4 为用三种方法所得到的位置和航向TRMSE。

表4 定位和航向TRMESTab.4 TRMSE of position and yaw

从图6 可以看出，SRAFGO 的位置精度要好于传统的FGO 方法，SWAFGO 的位置精度要稍好于SRAFGO。从图7 可以看出，FGO 方法的航向误差开始发散，而后两者逐渐收敛，且SWAFGO 的最终航向精度要稍好于SRAFGO 的结果。从图8 估计协方差矩阵与真实协方差矩阵差值的Frobenius 范数曲线可以看出，SWAFGO 方法较为准确地估计出了测量值的协方差矩阵。

图6 R=10 Rture 条件下不同方法的位置RMSEFig.6 Position RMSE for different methods

图7 R=10 Rture 条件下不同方法的航向RMSEFig.7 Yaw RMSE for three methods

图8 SWAFGO 方法的估计协方差矩阵与真实协方差矩阵偏差的Frobenius 范数曲线Fig.8 Frobenius norm of the error between the estimated and true noise covariance for SWAFGO

由表4 可以看出，在大的协方差矩阵的条件下，传统的FGO 方法的位置和航向TRMSE 分别为2.25 m和 1.01 °；SRAFGO 方法减小了位置和航向的TRMSE，提升幅值分别为18%和78%；SWAFGO 方法的提升幅度最大，分别达到58%和32%。这说明，SWAFGO 方法的性能要优于SRAFGO，且位置精度提升 49%。

4.2 KITTI 数据集实验

为了在真实环境中验证算法的有效性，选择非线性测量模型的双目视觉里程计VO，具体使用车载KITTI 数据集。该车搭载了几种不同的传感器，在城市道路中行驶，轨迹如图9 所示。在数据集中，差分GPS 提供位置基准，MEMS IMU 测量载体的比力和角速度信息，双目视觉里程计VO 测量相对位置和姿态。传感器的性能参数如表5 所示。

图9 KITTI 真实轨迹Fig.9 KITTI car trajectory

试验运行环境为台式计算机，ubuntu 麒麟操作系统，单核3.4 GHz i7 处理器，16 GB 内存。利用本文所提出的方法组合IMU/VO 并与传统的非自适应因子图方法和SPAFG 进行对比，结果如图10 和表6 所示。

图10 为三种方法的位置RMSE，从图中可以看出，三种方法的位置RMSE 在前20 s 的时间内迅速增大，这是因为在本实验中为了使姿态可观，积累了前20 s 的数据后才进行了一次因子图的更新，也就是说在这段时间内，是按照纯惯导的方式工作。此外，在开始因子图更新后，位置误差逐渐收敛，其中SWAFGO 方法收敛速度最快，SRAFGO 方法次之，FGO 方法最慢。在220 s～360 s 区间，位置RMSE 出现了振荡，这可能是因为在这段时间里VO 测量值出现了较大的噪声干扰，此时SRAFGO 方法的振荡幅度最小为5 m，SWAFGO 方法稍大达9 m，FGO 方法振荡最激烈，达到16 m，说明SRAFGO 更具鲁棒性。

图10 大失准角条件下不同方法的位置RMSE 曲线Fig.10 Position RMSE curve of different methods under large misalignment conditions

从最后100 s 的局部放大图看，SWAFGO 方法位置RMSE 最小为0.22 m，SRAFGO 方法居中为0.25 m，FGO 方法最大为0.6 m。这是因为前两者一定程度上自适应地估计了协方差矩阵，使得算法性能更加接近最优。

表6 为三种方法的位置TRMSE 和计算时间，由表中可以看出，SWAFGO 计算时间与SRAFGO 基本相当；FGO 方法的位置TRMSE 为0.60 m，SRAFGO和SWAFGO 方法的位置精度分别提升了58%和64%。这说明，SWAFGO 方法的性能要优于SRAFGO，位置估计精度提升12%。