1 引言（Introduction）

位姿估计是移动机器人必须具备的一项核心功能，是实现地图构建、自主移动等功能的前提和基础^[1-3]。现今常用惯性测量单元（IMU）、编码器（encoder）、相机（camera）、激光雷达（LiDAR）等传感器构造移动机器人的位姿估计系统。其中，基于IMU构建的惯性导航系统（INS）的导航误差随时间的增加不断累积，无法单独承担长时间的导航定位任务。全球导航卫星系统（GNSS）的定位精度极易受到电磁干扰、卫星拒止等因素的影响，无法在城市、峡谷等复杂区域内提供连续可靠的位置信息。同时，视觉导航系统与激光导航系统分别会在低纹理环境与长廊环境下出现失效问题^[4]。因此使用单一传感器难以在复杂环境下完成移动机器人的位姿估计任务^[5]。

近些年，涌现出许多基于多传感器融合的机器人位姿估计算法。例如，IMU/编码器融合算法^[6-7]、IMU/卫星/磁力计融合算法^[8]、IMU/相机融合算法^[9-10]、IMU/相机/激光融合算法^[11-12]等。但这些算法均存在以下2个不足：(1) 通用性差，即仅针对特定传感器组或特定平台设计，在跨平台移植、传感器组变更上仍然十分繁琐；(2) 在线配置融合算法结构的能力缺乏，即无法根据可用信息源的变化动态调整算法结构，主要体现在无法在线添加或删除信息源、无法在线进行变量选择等方面。因此，除了利用融合技术提升系统定位精度外，研究人员也希望设计一种不依赖特定平台、可在线配置结构的融合算法。实际上，在物理空间变化复杂的环境中，这种在线配置融合结构的能力对于提高定位系统的可靠性和可用性具有重要的意义，例如，进入黑夜场景时隔离视觉定位子系统，进入隧道场景时仅选择激光定位系统的姿态信息进行融合等。

针对第(1) 个不足，研究人员也提出了一些不依赖于特定传感器、较为通用的融合算法，但并不具备在线变更算法结构的能力。Moore等^[13]在ROS（robot operating system）环境下设计了一个通用型多源融合框架robot-localization。该框架可以依照ROS预定义的信息格式，融合任意多个子系统的输出数据。但该算法的融合结构只能进行静态配置，无法在线变更融合结构，例如，一旦选定融合视觉子系统的速度数据，就无法在线变更为融合它的位姿数据。另外，该算法使用欧拉角表示姿态，无法被应用于大俯仰角运动的移动平台，如翼型无人机等。

对于第(2) 个不足，研究人员比较关注的方法是联邦滤波算法和因子图优化算法。联邦滤波器是一种具有良好容错结构的分布式融合框架，在结构上能够有效隔离失效的子滤波器^[14]，但新传感器需要先与公共参考系统构成子滤波器才能参与融合。并且联邦滤波框架不具备在线选择变量的能力，例如，将视觉子系统由速度融合变更为位置融合时，需要重新编写相关的代码。因子图优化是一种具备即插即用能力的融合算法^[15]，理论上具备可在线配置结构的能力。Qin等^[16]设计了一个通用的IMU/相机紧耦合融合框架，该框架将每一个传感器看作为一般的因子，再将所有因子和待优化变量构建出一个MAP（maximum a posteriori）问题进行非线性优化。该框架可以实现任意多个IMU或相机的融合，但对于其他类型的传感器则需要重新进行残差建模，无法摆脱对传感器测量模型的依赖。另外，因子图优化融合的效果显著依赖于平台的计算能力，目前还难以应用于低功率的运动平台，如小型无人机等。

在理论方面，戴海发^[17]从数学的角度将各种导航技术的输出抽象为位置、姿态、速度函数的概念，构建了多源融合估计的统一理论框架，为多源融合系统的设计提供了理论上的指导，但缺少工程上的可操作性。

综上，针对组合导航系统中融合估计算法通用性差和在线配置结构困难的问题，本文基于误差状态扩展卡尔曼滤波算法（ES-EKF）提出了一种通用的、可在线配置结构的多源融合估计框架。为了提高框架的通用性，对常见的观测数据类型进行归纳和建模，建立一个与平台无关的、包含8种标准观测类型的模型库。为了赋予框架在线配置结构的能力，将多源信息的每一种融合方式表示为一种融合模式，通过在线变更融合模式来实现信息源和变量的选择。融合算法会根据当前的融合模式自动地从标准观测模型库中调取对应的观测模型将所选的多源信息进行融合。最后，将该算法应用于IMU/磁力计/编码器/相机/激光雷达组合导航系统，验证其有效性和可行性。

2 总体框架（Overall framework）

图 1展示了多源融合估计总体框架，主要由传感器、子系统和融合中心3部分构成。各传感器以相应的导航技术构成彼此独立的子导航系统，各子系统输出在配置模块组织下进入ES-EKF融合估计器中参与融合。

融合中心由ES-EKF融合估计器、配置模块、标准观测模型库组成，是本文主要的研究内容。融合估计器基于ES-EKF实现，信息处理流程依次为全状态预测、误差状态预测、误差状态更新、全状态更新（见第3节）。配置模块根据预先制定的融合模式配置表和子系统失效报告，在线对融合估计器的结构进行变更，主要包括信息源选择和变量选择2项任务（见第5节）。标准观测模型库是一个具有通用性的卡尔曼观测更新库，在编程实现上表现为一个函数库，包括全局位置观测模型（GP）、全局姿态观测模型（GA）、全局位姿观测模型（GPA）、全局速度观测模型（GV）、局部位置增量观测模型（LIP）、局部姿态增量观测模型（LIA）、局部位姿增量观测模型（LIPA）、局部速度观测模型（LV），建模过程见第4节。

3 ES-EKF融合估计器（ES-EKF fusion estimator） 3.1 坐标系与状态变量

局部坐标系$ \varSigma_{\rm L} $：与移动机器人相固连的坐标系，通常选用IMU坐标系，其原点为IMU的量测中心。全局坐标系$ \varSigma_{\rm G} $：与地面固连的坐标系，其原点与初始局部坐标系的原点重合，在有GNSS环境中，通常定义其坐标轴指向为初始位置的东—北—天方向；在无GNSS环境中，通常定义其坐标轴指向为初始局部坐标系指向。

设$ SO(3) $为3维姿态空间，姿态的矩阵表示、旋转向量表示分别记为$ \mathit{\boldsymbol{R}}\in SO(3) $和$ \mathit{\boldsymbol{a}}\in \mathbb{R}^{3} $，它们之间的转换关系记为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{R}}= {\rm Exp\, }(\mathit{\boldsymbol{a}}) \\ \mathit{\boldsymbol{a}}= {\rm Log\, }(\mathit{\boldsymbol{R}}) \end{cases} \end{align} $

(1)

其中，Exp表示将旋转向量转换为旋转矩阵的函数，Log表示将旋转矩阵转化为旋转向量的函数，具体计算过程请参考文[18] 中的式(3)。

姿态误差的矩阵表示和旋转向量表示分别为$ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}} \in SO(3) $和$ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}} \in \mathbb{R}^{3} $，定义如下：

$ \begin{align} \begin{cases} \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}} =\mathit{\boldsymbol{R\hat{R}}}{}^{\rm T} \\ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}} ={\rm Log\, } (\mathit{\boldsymbol{\delta}}_{\rm R}) \end{cases} \end{align} $

(2)

其中，$ \hat{\mathit{\boldsymbol{R}}}\in SO(3) $为姿态矩阵的估计值。当姿态误差为小量时，$ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}} $和$ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}} $之间有如下近似关系^[18]：

$ \begin{align} \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}} \approx \mathit{\boldsymbol{I}}+[ \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}} ]_{\times} \end{align} $

(3)

其中，$ \mathit{\boldsymbol{I}} $为单位矩阵，$ [\, \; ]_{\times} $表示反对称矩阵。

系统的全状态$ \mathit{\boldsymbol{x}} $定义为

$ \begin{align} \mathit{\boldsymbol{x}} \triangleq [{\mathit{\boldsymbol{p}}, \mathit{\boldsymbol{v}}, \mathit{\boldsymbol{R}}, \mathit{\boldsymbol{b}}_{\rm f}, \mathit{\boldsymbol{b}}_{\omega}, \mathit{\boldsymbol{g}}}] \end{align} $

(4)

其中，位置$ \mathit{\boldsymbol{p}} $、姿态$ \mathit{\boldsymbol{R}} $、速度$ \mathit{\boldsymbol{v}} $和重力加速度$ \mathit{\boldsymbol{g}} $的参考系为全局坐标系$ \varSigma_{\rm G} $，加速度计零偏$ \mathit{\boldsymbol{b}}_{\rm f} $、陀螺仪零偏$ \mathit{\boldsymbol{b}}_{\omega} $的参考系为局部坐标系$ \varSigma_{\rm L} $。对应的估计值记为

$ \begin{align} \hat{\mathit{\boldsymbol{x}}} \triangleq \left[\hat{\mathit{\boldsymbol{p}}}, \hat{\mathit{\boldsymbol{v}}}, \hat{\mathit{\boldsymbol{R}}}, \hat{\mathit{\boldsymbol{b}}}_{\mathrm{f}}, \hat{\mathit{\boldsymbol{b}}}_{\omega}, \hat{\mathit{\boldsymbol{g}}}\right] \end{align} $

(5)

其中，$ \wedge $表示相应变量的估计值。误差状态$ {\mathit{\boldsymbol{\varepsilon}}} $定义为状态估计值与真实值的偏差，即：

$ \begin{align} {\mathit{\boldsymbol{\varepsilon}}} \triangleq [{\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{p}}}^{\rm T}} \; \; {\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}}^{\rm T}} \; \; {\mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}}^{\rm T}} \; \; {\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}}^{\rm T}} \; \; {\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}}^{\rm T}} \; \; {\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}}^{\rm T}} ]^{\rm T}\in \mathbb{R}^{18} \end{align} $

(6)

这里，$ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{p}}} = \mathit{\boldsymbol{p}}-\hat{\mathit{\boldsymbol{p}}} $，$ \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}} = \mathit{\boldsymbol{v}}-\hat{\mathit{\boldsymbol{v}}} $，$ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}} = \mathit{\boldsymbol{b}}_{\rm f} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}} $，$ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}\omega} =\mathit{\boldsymbol{b}}_{\omega} -\hat{\mathit{\boldsymbol{b}}}_{\omega} $，$ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}} =\mathit{\boldsymbol{g}}-\hat{\mathit{\boldsymbol{g}}} $.

3.2 状态预测

将比力（加速度计的测量对象）和角速度（陀螺仪的测量对象）的测量值分别记为$ \mathit{\boldsymbol{f}}_{\rm m} $和$ {\mathit{\boldsymbol{\omega}}}_{\rm m} $，建立系统全状态运动模型：

$ \begin{align} {\mathit{\boldsymbol{x}}}_{k} =\mathscr {{\mathit{\boldsymbol{A}}}} ( \mathit{\boldsymbol{x}}_{k-1}, \mathit{\boldsymbol{u}}_{{\rm m}, k-1}, \mathit{\boldsymbol{w}}_{{\rm{sys}}, k} ) \end{align} $

(7)

其中，下标$ k $和$ k-1 $分别对应着离散时刻$ t_{k} $和$ t_{k-1} $，控制输入记为$ \mathit{\boldsymbol{u}}_{{\rm m}, k-1} \triangleq [{\mathit{\boldsymbol{f}}_{{\rm m}, k-1}^{\rm T}} \; \; {{\mathit{\boldsymbol{\omega}}}_{{\rm m}, k-1}^{\rm T}} ]^{\rm T} $，系统的过程噪声记为$ \mathit{\boldsymbol{w}}_{{\rm{sys}}, k} \triangleq [\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k}^{\rm T}, $ $ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k}^{\rm T}, $ $ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}^{\rm T}, $ $ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k}^{\rm T}, $ $ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\omega}, k}^{\rm T}, $ $ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{g}}}, k}^{\rm T}]^{\rm T} $，该模型的具体表达式为

$ \begin{align*} \begin{cases} \mathit{\boldsymbol{p}}_{k} =\mathit{\boldsymbol{p}}_{k-1} +\mathit{\boldsymbol{v}}_{k-1} {\Delta} t+\frac{1}{2}( \mathit{\boldsymbol{R}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})+ \\ \qquad\; \; \mathit{\boldsymbol{g}}_{k-1} ){\Delta} t^{2}+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} \\ \mathit{\boldsymbol{v}}_{k} =\mathit{\boldsymbol{v}}_{k-1} +[{\mathit{\boldsymbol{R}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})+\mathit{\boldsymbol{g}}_{k-1}}]{\Delta} t\!+\! \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} \\ \mathit{\boldsymbol{R}}_{k} =\mathit{\boldsymbol{R}}_{k-1} {\rm Exp\, } ({({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\omega}, k-1}}){\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}) \\ \mathit{\boldsymbol{b}}_{{\rm f}, k} =\mathit{\boldsymbol{b}}_{{\rm f}, k-1} +\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} \\ \mathit{\boldsymbol{b}}_{{\omega}, k} =\mathit{\boldsymbol{b}}_{{\omega}, k-1} +\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\omega}, k} \\ \mathit{\boldsymbol{g}}_{k} =\mathit{\boldsymbol{g}}_{k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{g}}}, k} \end{cases}\!\!\!\!\!\!\!\! \end{align*} $

这里，$ \mathit{\boldsymbol{w}}_{{\rm{sys}}, k} $假设为高斯白噪声，即$ \mathit{\boldsymbol{w}}_{{\rm{sys}}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{{\rm{sys}}, k}) $，$ {\mathit{\boldsymbol{\varSigma}}}_{{\rm{sys}}, k} $为对应的协方差矩阵。$ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} $、$ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} $、$ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k} $、$ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} $、$ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\omega}, k} $和$ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{g}}}, k} $分别表示位置、速度、姿态、加速度计零偏、陀螺仪零偏和重力加速度的过程噪声。

假设系统噪声为0，获得全状态的预测方程为

$ \begin{align} \hat{\mathit{\boldsymbol{x}}}_{k}=\mathscr{{\mathit{\boldsymbol{A}}}} (\hat{\mathit{\boldsymbol{x}}}_{k-1}, \mathit{\boldsymbol{u}}_{{\rm m}, k-1}, \mathit{\boldsymbol{0}}) \end{align} $

(8)

根据误差状态的定义(6)，对比考虑噪声的全状态运动模型(7) 和不考虑噪声的全状态预测方程(8)，可得到误差状态的运动模型（详细推导过程参见附录A）。误差状态运动模型的矩阵表示为

$ \begin{align} \mathit{\boldsymbol{\varepsilon}}_{k} =\mathit{\boldsymbol{F}}_{k, k-1} \mathit{\boldsymbol{\varepsilon}}_{k-1} +\mathit{\boldsymbol{w}}_{{\rm{sys}}, k} \end{align} $

(9)

其中，

$ \mathit{\boldsymbol{F}}_{k, k-1} =\begin{bmatrix} {\mathit{\boldsymbol{I}}} & {\Delta t{\mathit{\boldsymbol{I}}}} & {-\dfrac{\Delta t^{2}}{2} [{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} \!-\!\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})} ]_{\times}} & {-\dfrac{\Delta t^{2}}{2}\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} & {\dfrac{\Delta t^{2}}{2}{\mathit{\boldsymbol{I}}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {-\Delta t [{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} \!-\!\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})}]_{\times}} & {-\Delta t\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} & {\Delta t{\mathit{\boldsymbol{I}}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} & {-\Delta t\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} \end{bmatrix} $

记$ t_{k-1} $时刻的状态估计值为$ \hat{\mathit{\boldsymbol{x}}}_{k-1|k-1} $、对应的误差估计值为$ \hat{\mathit{\boldsymbol{\varepsilon}}}_{k-1|k-1} $、误差估计的协方差矩阵为$ {\mathit{\boldsymbol{P}}}_{k-1|k-1} $，则根据卡尔曼滤波预测公式，时刻$ t_{k} $的状态预测估计值$ \hat{\mathit{\boldsymbol{x}}}_{k|k-1} $、误差预测估计值$ \hat{\mathit{\boldsymbol{\varepsilon}}}_{k|k-1} $、协方差的预测值$ {\mathit{\boldsymbol{P}}}_{k|k-1} $分别为

$ \begin{equation} \begin{aligned} & \hat{\mathit{\boldsymbol{x}}}_{k | k-1}=\mathscr{{\mathit{\boldsymbol{A}}}} (\hat{\mathit{\boldsymbol{x}}}_{k-1 | k-1}, \mathit{\boldsymbol{u}}_{\mathrm{m}, k-1}, \bf{0} ) \\ & \hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k-1}=\mathit{\boldsymbol{F}}_{k, k-1} \hat{\mathit{\boldsymbol{\varepsilon}}}_{k-1 | k-1} \\ & \mathit{\boldsymbol{P}}_{k | k-1}=\mathit{\boldsymbol{F}}_{k, k-1} \mathit{\boldsymbol{P}}_{k-1 | k-1} \mathit{\boldsymbol{F}}_{k, k-1}^{\rm T}+\mathit{\boldsymbol{\varSigma}}_{\mathrm{sys}, k} \end{aligned} \end{equation} $

(10)

3.3 状态更新

设$ t_{k} $时刻的误差状态观测的一般模型为

$ \begin{equation} \begin{aligned} & {\mathit{\boldsymbol{z}}}_{k} ={\mathit{\boldsymbol{H}}}_{k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +{\mathit{\boldsymbol{w}}}_{{\rm{meas}}, k} \\ & {\mathit{\boldsymbol{w}}}_{{\rm{meas}}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, {\mathit{\boldsymbol{\varSigma}}}_{{\rm{meas}}, k}) \end{aligned} \end{equation} $

(11)

其中，$ {\mathit{\boldsymbol{z}}}_{k} $、$ {\mathit{\boldsymbol{H}}}_{k} $、$ {\mathit{\boldsymbol{w}}}_{{\rm{meas}}, k} $和$ {\mathit{\boldsymbol{\varSigma}}}_{{\rm{meas}}, k} $分别表示$ t_{k} $时刻的观测值、观测矩阵、观测噪声和观测噪声协方差矩阵，具体形式详见第4节中的各观测模型。使用卡尔曼滤波更新方程对误差状态的预测值进行观测更新：

$ \begin{align} \mathit{\boldsymbol{K}}_{k} & =\mathit{\boldsymbol{P}}_{k | k-1} \mathit{\boldsymbol{H}}_{k}^{\mathrm{T}}\left(\mathit{\boldsymbol{H}}_{k} \mathit{\boldsymbol{P}}_{k | k-1} \mathit{\boldsymbol{H}}_{k}^{\mathrm{T}}+\mathit{\boldsymbol{\varSigma}}_{\rm {meas }, k}\right)^{-1} \\ \hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k} & =\hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k-1}+\mathit{\boldsymbol{K}}_{k}\left(\mathit{\boldsymbol{z}}_{k}-\mathit{\boldsymbol{H}}_{k} \hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k-1}\right) \\ \mathit{\boldsymbol{P}}_{k | k} & =\left(\mathit{\boldsymbol{I}}-\mathit{\boldsymbol{K}}_{k} \mathit{\boldsymbol{H}}_{k}\right) \mathit{\boldsymbol{P}}_{k | k-1} \end{align} $

(12)

根据式(6) 的误差定义进行逆向运算，使用误差估计值$ \hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k} $对状态预测值$ \hat{\mathit{\boldsymbol{x}}}_{k | k-1} $进行误差补偿，得到当前时刻状态的最优估计值$ \hat{\mathit{\boldsymbol{x}}}_{k | k} $，补偿过程记为

$ \begin{align} \hat{\mathit{\boldsymbol{x}}}_{k | k}=\hat{\mathit{\boldsymbol{x}}}_{k | k-1} \oplus \hat{\mathit{\boldsymbol{\varepsilon}}}_{k | k} \end{align} $

(13)

最后，对误差状态的最优估计值进行归零，为下一时刻的误差状态估计作准备，归零过程记为

$ \begin{align} \hat{\mathit{\boldsymbol{\varepsilon}}}_{k|k} \leftarrow \bm0 \end{align} $

(14)

4 标准观测模型库（Standard observation model library） 4.1 全局位置观测模型（GP）

北斗定位系统和全局视觉测量系统等导航技术通常输出信息为全局位置信息，状态更新时的观测模型记为GP。子系统的全局位置输出值记为$ \mathit{\boldsymbol{y}}_{{\rm GP}, k} \in \mathbb{R}^{3} $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{{\rm GP}, k} =\mathit{\boldsymbol{p}}_{k} +\mathit{\boldsymbol{w}}_{{\rm GP}, k} \end{align} $

(15)

其中，$ \mathit{\boldsymbol{w}}_{{\rm GP}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{w}}_{{\rm GP}, k} \sim {\mathcal N}( \mathit{\boldsymbol{0}}, $ $ \mathit{\boldsymbol{\varSigma}} _{{\rm GP}, k} ) $。则全局位置的观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{{\rm GP}, k} ={\mathit{\boldsymbol{H}}}_{{\rm GP, }k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{{\rm GP}, k} \end{align} $

(16)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{\mathrm{GP}, k} &\triangleq \mathit{\boldsymbol{y}}_{\mathrm{GP}, k}-\hat{\mathit{\boldsymbol{p}}}{}_{k | k-1}\\ \mathit{\boldsymbol{H}}_{\mathrm{GP}, k} &= [\mathit{\boldsymbol{I}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}}] \in \mathbb{R}^{3 \times 18} \end{align*} $

4.2 全局姿态观测模型（GA）

姿态航向参考系统（AHRS）和双天线GNSS定向系统等导航子系统的输出通常为全局姿态信息，观测更新时的观测模型记为GA。子系统的全局姿态输出值记为$ \mathit{\boldsymbol{y}}_{{\rm GA}, k} \in SO(3) $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{{\rm GA}, k} ={\rm Exp\, } (\mathit{\boldsymbol{w}}_{{\rm GA}, k})\mathit{\boldsymbol{R}}_{k} \end{align} $

(17)

其中，$ \mathit{\boldsymbol{w}}_{{\rm GA}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{w}}_{{\rm GA}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{{\rm GA}, k}) $，则全局姿态观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{{\rm GA}, k} ={\mathit{\boldsymbol{H}}}_{{\rm GA}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{{\rm GA}, k} \end{align} $

(18)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{\mathrm{GA}, k} & \triangleq {\rm Log\, } \left(\mathit{\boldsymbol{y}}_{\mathrm{GA}, k} \hat{\mathit{\boldsymbol{R}}}_{k | k-1}^{\mathrm{T}}\right)\\ \mathit{\boldsymbol{H}}_{\mathrm{GA}, k}& =[ \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{I}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} ] \in \mathbb{R}^{3 \times 18} \end{align*} $

4.3 全局位姿观测模型（GPA）

有地图辅助的视觉定位子系统和激光定位子系统等子导航系统通常同步输出全局位置和全局姿态（合称为全局位姿）信息，观测更新时的观测模型记为GPA。联合位置误差观测模型(16) 和姿态误差观测模型(18)，则全局位姿观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{\rm{GPA}, k} ={\mathit{\boldsymbol{H}}}_{\rm{GPA}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{\rm{GPA}, k} \end{align} $

(19)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{\mathrm{GA}, k} & \triangleq \begin{bmatrix} \mathit{\boldsymbol{y}}_{\mathrm{GP}, k}-\hat{\mathit{\boldsymbol{p}}}{}_{k | k-1} \\ {\rm Log\, } \left(\mathit{\boldsymbol{y}}_{\mathrm{GA}, k} \hat{\mathit{\boldsymbol{R}}}_{k | k-1}^{\rm T}\right) \end{bmatrix} \in \mathbb{R}^{6} \\ \mathit{\boldsymbol{w}}_{\mathrm{GPA}, k} & \triangleq \begin{bmatrix} \mathit{\boldsymbol{w}}_{\mathrm{GP}, k} \\ \mathit{\boldsymbol{w}}_{\mathrm{GA}, k} \end{bmatrix} \in \mathbb{R}^{6} \\ \mathit{\boldsymbol{H}}_{\mathrm{GPA}, k} &= \begin{bmatrix} \mathit{\boldsymbol{I}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} \\ \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{I}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} \end{bmatrix} \in \mathbb{R}^{6 \times 18} \end{align*} $

4.4 全局速度观测模型（GV）

卫星/惯性组合导航系统和视觉/惯性组合系统等子导航系统通常可输出全局速度信息，观测更新时的观测模型记为GV。子系统的全局速度输出值记为$ \mathit{\boldsymbol{y}}_{{\rm GV}, k} \in \mathbb{R}^{3} $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{{\rm GV}, k} =\mathit{\boldsymbol{v}}_{k} +\mathit{\boldsymbol{w}}_{{\rm GV}, k} \end{align} $

(20)

其中，$ \mathit{\boldsymbol{w}}_{{\rm GV}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{w}}_{{\rm GV}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{{\rm GV}, k}) $，则全局速度观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{{\rm GV}, k} ={\mathit{\boldsymbol{H}}}_{{\rm GV, }k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{{\rm GV}, k} \end{align} $

(21)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{{\rm GV}, k} & \triangleq \mathit{\boldsymbol{y}}_{{\rm GV}, k}-\hat{\mathit{\boldsymbol{v}}}_{k | k-1}\\ \mathit{\boldsymbol{H}}_{{\rm GV}, k} & = [\mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{I}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} ] \in \mathbb{R}^{3 \times 18} \end{align*} $

4.5 局部位置增量观测模型（LIP）

视觉里程计和激光雷达里程计等里程计通常输出局部位置增量和局部姿态增量信息。当融合里程计的局部位置增量时，所建立的观测模型记为LIP。在时间区间$ [{t_{k-1}, \; t_{k}}] $（$ t_{k} =t_{k-1} +{\Delta} t $）上，子系统输出的位置增量值记为$ \mathit{\boldsymbol{y}}_{\rm{LIP}, k} \in \mathbb{R}^{3} $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{\rm{LIP}, k} \mathit{\boldsymbol{=}}\mathit{\boldsymbol{R}}_{k-1}^{\rm T} ({\mathit{\boldsymbol{p}}_{k} -\mathit{\boldsymbol{p}}_{k-1}})+\mathit{\boldsymbol{w}}_{\rm{LIP}, k} \end{align} $

(22)

其中，$ \mathit{\boldsymbol{w}}_{\rm{LIP}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{w}}_{\rm{LIP}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{\rm{LIP}, k}) $。则局部位置增量观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{\rm{LIP}, k} ={\mathit{\boldsymbol{H}}}_{\rm{LIP}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{\rm{LIP}, {k}} \end{align} $

(23)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{\rm{LIP}, k} & \triangleq \mathit{\boldsymbol{y}}_{\rm{LIP}, k}-\hat{\mathit{\boldsymbol{R}}}{}^{\rm T}_{k-1 | k-1}\left(\hat{\mathit{\boldsymbol{p}}}{}_{k | k-1}-\hat{\mathit{\boldsymbol{p}}}{}_{k-1 | k-1}\right)\\ \mathit{\boldsymbol{H}}_{\rm{LIP}, k} & = [\hat{\mathit{\boldsymbol{R}}}{}_{k-1 | k-1}^{\rm T} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} ] \in \mathbb{R}^{3 \times 18} \end{align*} $

4.6 局部姿态增量观测模型（LIA）

当融合局部姿态增量时，所建立的观测模型记为LIA。子系统输出的姿态增量记为$ \mathit{\boldsymbol{y}}_{\rm{LIA}, k} \in SO(3) $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{\rm{LIA}, k} ={\rm E}{\rm{xp}}({\mathit{\boldsymbol{w}}_{\rm{LIA}, k}})\mathit{\boldsymbol{R}}_{k-1}^{\rm T} \mathit{\boldsymbol{R}}_{k} \end{align} $

(24)

其中，$ \mathit{\boldsymbol{w}}_{\rm{LIA}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{w}}_{\rm{LIA}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{\rm{LIA}, k}) $，则局部姿态增量观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{\rm{LIA}, k} ={\mathit{\boldsymbol{H}}}_{\rm{LIA}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{\rm{LIA}, k} \end{align} $

(25)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{{\rm LIA}, k} & \triangleq {\rm Log\, } \left(\mathit{\boldsymbol{y}}_{{\rm LIA}, k} \hat{\mathit{\boldsymbol{R}}}{}_{k | k-1}^{\rm T} \hat{\mathit{\boldsymbol{R}}}_{k-1 | k-1}\right)\\ \mathit{\boldsymbol{H}}_{{\rm LIA}, k} & = [\mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \hat{\mathit{\boldsymbol{R}}}{}_{k-1 | k-1}^{\rm T} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} \; \; \mathit{\boldsymbol{0}} ] \in \mathbb{R}^{3 \times 18} \end{align*} $

4.7 局部位姿增量观测模型（LIPA）

当同时融合局部位置增量和局部姿态增量时（合称为局部位姿增量），所建立的观测模型记为LIPA。联合LIP的观测模型(23) 和LIA的观测模型(25)，则局部位姿增量观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{\rm{LIPA}, k} ={\mathit{\boldsymbol{H}}}_{\rm{LIPA}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{\rm{LIPA}, k} \end{align} $

(26)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{\rm{LIPA}, k} & \triangleq \begin{bmatrix} \mathit{\boldsymbol{y}}_{{\rm LIP}, k}-\hat{\mathit{\boldsymbol{R}}}{}_{k-1 | k-1}^{\rm T}\left(\hat{\mathit{\boldsymbol{p}}}{}_{k | k-1}-\hat{\mathit{\boldsymbol{p}}}{}_{k-1 | k-1}\right) \\ {\rm Log\, } \left(\mathit{\boldsymbol{y}}_{{\rm LIA}, k} \hat{\mathit{\boldsymbol{R}}}{}_{k | k-1}^{\rm T} \hat{\mathit{\boldsymbol{R}}}_{k-1 | k-1}\right) \end{bmatrix} \\ &\quad \in \mathbb{R}^{6} \\ \mathit{\boldsymbol{w}}_{\rm{LIPA}, {\rm k}} & \triangleq \begin{bmatrix} \mathit{\boldsymbol{w}}_{\rm{LIP}, k} \\ \mathit{\boldsymbol{w}}_{\rm{LIA}, k} \end{bmatrix} \in \mathbb{R}^{6} \\ \mathit{\boldsymbol{H}}_{\rm{LIPA}, k} & = \begin{bmatrix} \hat{\mathit{\boldsymbol{R}}}{}_{k-1 | k-1}^{\rm T} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} \\ \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \hat{\mathit{\boldsymbol{R}}}{}_{k-1 | k-1}^{\rm T} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} & \mathit{\boldsymbol{0}} \end{bmatrix} \\ & \in \mathbb{R}^{6 \times 18} \end{align*} $

4.8 局部速度观测模型（LV）

超声波测速系统和编码器测速系统等测速子系统通常输出局部速度信息。融合局部速度所建立的观测模型记为LV。子系统输出的局部速度值记为$ \mathit{\boldsymbol{y}}_{{\rm LV}, k} \in \mathbb{R}^{3} $，输出模型设为

$ \begin{align} \mathit{\boldsymbol{y}}_{{\rm LV}, k} =\mathit{\boldsymbol{R}}_{k}^{\rm T} \mathit{\boldsymbol{v}}_{k} +\mathit{\boldsymbol{w}}_{{\rm LV}, k} \end{align} $

(27)

其中，$ \mathit{\boldsymbol{y}}_{{\rm LV}, k} \in \mathbb{R}^{3} $为输出噪声，且$ \mathit{\boldsymbol{y}}_{{\rm LV}, k} \sim {\mathcal N}(\mathit{\boldsymbol{0}}, $ $ {\mathit{\boldsymbol{\varSigma}}}_{{\rm LV}, k}) $，则局部速度观测模型为

$ \begin{align} \mathit{\boldsymbol{z}}_{{\rm LV}, k} ={\mathit{\boldsymbol{H}}}_{{\rm LV}, k} {\mathit{\boldsymbol{\varepsilon}}}_{k} +\mathit{\boldsymbol{w}}_{{\rm LV}, k} \end{align} $

(28)

其中，

$ \begin{align*} \mathit{\boldsymbol{z}}_{{\rm LV}, k} & \triangleq \mathit{\boldsymbol{y}}_{{\rm LV}, k}-\hat{\mathit{\boldsymbol{R}}}{}_{k | k-1}^{\rm T} \hat{\mathit{\boldsymbol{v}}}_{k | k-1} \in \mathbb{R}^{3}\\ {\mathit{\boldsymbol{H}}}_{{\rm LV}, k} & = [{0\mathit{\boldsymbol{}}} \; \; \hat{\mathit{\boldsymbol{R}}}{}_{k | k-1}^{\rm T} \; \; \hat{\mathit{\boldsymbol{R}}}{}_{k | k-1}^{\rm T}[\hat{\mathit{\boldsymbol{v}}}_{k | k-1}]_{\times} \; \; {0\mathit{\boldsymbol{}}} \; \; {0\mathit{\boldsymbol{}}} \; \; {0\mathit{\boldsymbol{}}}]\\ &\in \mathbb{R}^{3\times18} \end{align*} $

5 融合结构在线配置（Online configuration of the fusion structure）

融合结构配置是指在估计器内部选择哪些信息源及哪些类型的变量参与融合估计，包括2个层面：(1) 根据子系统失效报告，确定使用哪些子导航系统的输出进行融合估计，即寻找最优的子系统组合；(2) 根据所选子系统组合，确定使用各子系统的哪些输出量进行融合。配置模块的作用是在线地根据子系统失效报告和融合模式配置表为ES-EKF融合估计器选择出合适的融合模式，通过变更融合模式来实现融合算法结构的在线配置。

5.1 子系统失效报告

在本框架中，各子系统使用现有的失效检测方法独立地进行失效判别，例如，激光子系统可使用特征值分析法^[19]进行失效检测，视觉子系统常用的失效检测方法为特征点数目阈值法^[20]，卫星定位常用卡方检测法^[21]进行异常值诊断。各子系统周期性地完成失效检测并向配置模块发送失效报告，报告内容有4种可能：(1) 子系统发生失效，即由正常状态变为失效状态；(2) 子系统恢复正常，即由失效状态变为正常状态；(3) 子系统保持失效，即继续保持失效状态；(4) 子系统保持正常，即继续保持正常状态。

5.2 融合模式配置表及其制定

一个导航子系统可能会有多种类型的输出。例如，视觉定位子系统既可以输出位姿变量也可以输出速度变量，因此需要综合考虑各个子系统的特点为它们分配合适的观测模型。融合模式配置表是一个2维表格，每一种融合模式表示各子系统观测更新的一种组合方式，对应着一种融合结构。表 1制定了$ m $个子系统的$ s $种融合模式，第$ i $种融合模式记为

$ \begin{align} {{\mathit{\boldsymbol{M}}}}_{i} =[ c_{i1}, c_{i2}, \cdots, c_{im} ] \end{align} $

(29)

其中，$ c_{ij} $（$ i=1, \cdots, s $，$ j=1, \cdots, m $）表示第$ i $种融合模式中，子系统$ j $所分配到的观测模型，取值为标准观测模型集合的子集（空集表示该子系统的输出不参与融合）；$ w_{i} $（$ i=1, \cdots, s $）表示第$ i $种融合模式所分配到的优先级。

在状态更新阶段，ES-EKF融合估计器根据选定的融合模式自动从标准观测模型库调用相应的观测模型，从而实现子系统输出的有效融合。

在复杂环境中，固定使用一种融合模式通常无法获得稳定、可靠的定位结果，需要根据子系统的失效报告进行模式的变更。当某一个子系统发生失效时，很可能存在多种模式可供选择，因此在制定融合模式配置表时需要预先为每种融合模式分配一个优先级——表征该模式优先使用的级别。当有多个融合模式可供使用时，系统会选用优先级最高的模式进行多源融合。通常，需要对每一种融合模式进行定位精度测试，精度越高就分配越高的优先级。

5.3 融合模式在线变更流程

导航任务开始前，根据工作环境设置初始的融合模式，如果在导航任务中某些子系统出现失效，配置模块会根据融合模式配置表和失效报告在线变更ES-EKF融合估计器的融合模式，实现失效信息源的删除或新信息源的添加。例如，当移动机器人运动到非常昏暗的环境时，视觉子系统出现失效，为了保证导航定位功能正常，需要根据优先级删除视觉子系统，具体操作为：依据模式配置表找出不使用视觉子系统的融合模式，再将优先级最高的候选模式设定为新的融合模式；反之可实现视觉子系统的添加。图 2展示了ES-EKF融合估计器的融合模式在线变更的一般流程。

6 试验验证（Experimental verification）

为了验证该融合框架的有效性和可行性，基于本文融合框架实现一个惯性测量单元/磁力计/编码器/双目相机/激光雷达组合导航系统，分别进行固定融合模式试验、精度对比试验和在线变更融合模式试验。

6.1 试验平台

如图 3所示，试验的硬件平台由3部分构成，四轮独立驱动越野型移动机器人、工控机、传感器组。硬件平台的各产品及其型号如表 2所示，系统的软件架构如图 4所示。整个导航系统在ROS环境下使用C++ 语言实现，包含如下3个导航子系统：(1) 基于编码器和AHRS系统的位姿估计子系统，简称EO（encoder odometer）；(2) 基于IMU和双目视觉的位姿估计子系统，简称VO（visual odometer），改编自VINS-Fusion算法^[16]；(3) 基于激光雷达的位姿估计子系统，简称LO（laser odometer），改编自LeGO-LOAM算法^[22]。

6.2 试验场景

实验场地为野外土山，跑车时间为下午2时30分。以RTK定位数据为定位参考值（水平精度为10 mm $ + D\times 10^{-6} {\rm mm} $，竖直精度为20 mm $ +D\times $$ 10^{-6} {\rm mm} $，$ D $代表以千米计量的RTK接收机到基站的距离数值），跑车时长约600 s，路径长度约528 m，高度变化约35 m，测试场地的自然环境如图 3所示，测试区域的卫星地图如图 5所示，其中红色曲线表示跑车轨迹。

为了对同一跑车试验过程中不同融合模式的定位结果进行比较，本文采用ROS-BAG工具包记录试验过程中的各项传感器数据，再在ROS环境下进行数据集回放和融合测试，从而验证本文融合框架的有效性与可配置性。

6.3 固定融合模式试验

子系统EO输出位姿增量和全局速度数据，因此EO参与融合时可选择所使用的标准观测模型为LIPA或GV。子系统VO输出位姿增量和局部速度数据，对应的观测模型分别为LIPA和LV。子系统LO仅输出位姿增量，观测模型选为LIPA。针对图 4所示的导航系统，依据5.1节模式配置规则，制定出7种融合模式，详情见表 3。

所谓固定融合模式试验，即在整个系统运行过程中，固定使用一种融合模式进行子系统输出数据的融合，该试验的目的是为了确定每种融合模式的定位精度，以便为每一种模式分配合适的优先级。图 6为各融合模式下系统的定位曲线，表 4为各融合模式下导航系统的定位平均误差和相对误差。

对比7种融合模式下的定位误差情况，制定出各融合模式的优先级，定位误差越低，分配的优先级越高，详情见表 3第1列。

6.4 精度对比试验

为了验证本算法的定位精度，开展精度对比试验。试验内容：以robot-localization算法融合IMU、EO、VO和LO，计算其平均定位误差和相对误差，并与本文算法（模式7）进行精度对比。

选择robot-localization算法作为对比对象，主要是因为：(1) 与本文框架类似，它也是基于EKF的松耦合融合框架。(2) 它是ROS系统上非常流行、被广泛使用的一种定位算法包。图 7展示了本文算法（模式7）和robot-localization算法的定位轨迹，表 5统计了2种算法的平均误差和相对误差。量化结果表明，在融合相同信息源时，本文算法具有更高的定位精度。

6.5 在线变更融合模式试验

为了进一步验证本框架的在线配置融合结构的能力，进行半仿真试验，试验内容为：初始融合模式设置为模式7，第101 s时LO出现异常，算法根据表 3中的优先级在线变更融合模式为模式3，隔离子系统LO；第183 s时LO又恢复正常，算法再次在线变更融合模式为模式7，继续融合LO的输出信息。本试验的目的是验证该算法的在线配置能力，LO并未真正出现异常，因此该试验是一种半仿真试验。

图 8展示了动态配置测试的定位轨迹；定位误差量化值为：平均定位误差为2.788 6 m，相对平均误差为0.5282%。试验结果表明，当出现失效信息源时，本算法能够有效地进行融合模式变更，隔离开失效信息源，维持系统的定位性能；当被隔离的信息源恢复正常时，通过再次变更融合模式，添加该信息源，继续增强系统的定位性能。

7 结论与展望（Conclusion and prospect）

基于扩展卡尔曼滤波和标准观测模型库，提出一种不依赖特定平台、可在线配置结构的多源融合估计框架，赋予组合导航系统在线变更组合方式的能力，并通过试验验证了该算法的有效性和可行性。试验表明，本文提出的融合估计框架具有以下显著优势：(1) 不依赖于特定平台，可以方便地应用到无人车、无人机等各类运动平台上；(2) 在精度上优于robot-localization算法；(3) 无需复杂的系统设计便可快速地将现有的单一传感器算法集成在一块，配置简单、高效；(4) 具有动态配置功能，可以从软件层面实现信息源的动态添加和删除，为复杂场景下移动机器人的韧性定位提供一种可行的融合框架。

本文的融合框架尽管可以赋予组合导航系统动态删除或添加信息源的能力，但需要环境感知信息和健康检测报告才能发挥其价值。因此，将来的工作重点是：建立起工作场景和传感模态的关联，自适应地实现组合导航系统的结构调整，更可靠地完成融合估计和导航定位。

附录（Appendix）   误差状态运动模型推导

式(7) 为考虑噪声时全状态的运动模型；式(8) 为全状态的预测方程，即不考虑噪声时的全状态运动模型。具体公式如下：

$ \begin{align*} \mathit{\boldsymbol{x}}_{k} & =\mathscr{{\mathit{\boldsymbol{A}}}}\left(\mathit{\boldsymbol{x}}_{k-1}, \mathit{\boldsymbol{u}}_{{\rm m}, k-1}, \mathit{\boldsymbol{w}}_{{\rm sys}, k}\right) \Leftrightarrow \begin{cases} \mathit{\boldsymbol{p}}_{k}=\mathit{\boldsymbol{p}}_{k-1}+\mathit{\boldsymbol{v}}_{k-1} \Delta t+\dfrac{1}{2}\left[\mathit{\boldsymbol{R}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\mathit{\boldsymbol{b}}_{{\rm f}, k-1}\right)+\mathit{\boldsymbol{g}}_{k-1}\right] \Delta t^{2}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{p}}, k} \\ \mathit{\boldsymbol{v}}_{k}=\mathit{\boldsymbol{v}}_{k-1}+\left[\mathit{\boldsymbol{R}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\mathit{\boldsymbol{b}}_{{\rm f}, k-1}\right)+\mathit{\boldsymbol{g}}_{k-1}\right] \Delta t+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{v}}}}, k} \\ \mathit{\boldsymbol{R}}_{k}=\mathit{\boldsymbol{R}}_{k-1} {\rm Exp\, }\left(\left(\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1}-\mathit{\boldsymbol{b}}_{\omega, k-1}\right) \Delta t+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{a}}, k}\right) \\ \mathit{\boldsymbol{b}}_{{\rm f}, k}=\mathit{\boldsymbol{b}}_{{\rm f}, k-1}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}} {\rm f}, k} \\ \mathit{\boldsymbol{b}}_{\omega, k}=\mathit{\boldsymbol{b}}_{\omega, k-1}+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{b}}}} \omega, k} \\ \mathit{\boldsymbol{g}}_{k}=\mathit{\boldsymbol{g}}_{k-1}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{g}}, k} \end{cases}\\ \hat{\mathit{\boldsymbol{x}}}_{k} & =\mathscr{{\mathit{\boldsymbol{A}}}}\left(\hat{\mathit{\boldsymbol{x}}}_{k-1}, \mathit{\boldsymbol{u}}_{{\rm m}, k-1}, \mathit{\boldsymbol{0}}\right) \Leftrightarrow \begin{cases}\hat{\mathit{\boldsymbol{p}}}{}_{k}=\hat{\mathit{\boldsymbol{p}}}{}_{k-1}+\hat{\mathit{\boldsymbol{v}}}_{k-1} \Delta t+\dfrac{1}{2}\left[\hat{\mathit{\boldsymbol{R}}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}\right)+\hat{\mathit{\boldsymbol{g}}}_{k-1}\right] \Delta t^{2} \\ \hat{\mathit{\boldsymbol{v}}}_{k}=\hat{\mathit{\boldsymbol{v}}}_{k-1}+\left[\hat{\mathit{\boldsymbol{R}}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}\right) + \hat{\mathit{\boldsymbol{g}}}_{k-1}\right] \Delta t \\ \hat{\mathit{\boldsymbol{R}}}_{k}=\hat{\mathit{\boldsymbol{R}}}_{k-1} {\rm Exp\, }\left(\left(\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1}-\hat{\mathit{\boldsymbol{b}}}_{\omega, k-1}\right) \Delta t\right) \\ \hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k}=\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1} \\ \hat{\mathit{\boldsymbol{b}}}_{\omega, k}=\hat{\mathit{\boldsymbol{b}}}_{\omega, k-1} \\ \hat{\mathit{\boldsymbol{g}}}_{k}=\hat{\mathit{\boldsymbol{g}}}_{k-1} \end{cases} \end{align*} $

A.1 推导中用到的已知结论

结论(1)：$ {\rm Exp\, } (\mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}})\approx \mathit{\boldsymbol{I}}+[{\mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}}}]_{\times} $（$ \|\mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{a}}} \|\approx 0 $）

结论(2)：$ [{{\mathit{\boldsymbol{a}}}}]_{\times} {\mathit{\boldsymbol{b}}}=-[\mathit{\boldsymbol{b}}]_{\times} {\mathit{\boldsymbol{a}}} $，$ {\mathit{\boldsymbol{a}}}, {\mathit{\boldsymbol{b}}}\in \mathbb{R}^{3} $

结论(3)：$ {\mathit{\boldsymbol{R}}}[{{\mathit{\boldsymbol{a}}}}]_{\times} {\mathit{\boldsymbol{R}}}^{\rm T} =[{{\mathit{\boldsymbol{R}}}{\mathit{\boldsymbol{a}}}}]_{\times} $，$ \mathit{\boldsymbol{R}}\in SO(3) $，$ {\mathit{\boldsymbol{a}}}\in \mathbb{R}^{3} $

A.2 位置误差运动模型

根据位置误差的定义有：

$ \begin{align*} \mathit{\boldsymbol{\varDelta}}_{{{\mathit{\boldsymbol{p}}}}, k}& =\mathit{\boldsymbol{p}}_{k}-\hat{\mathit{\boldsymbol{p}}}{}_{k} \\ & =\left\{\mathit{\boldsymbol{p}}_{k-1}+\mathit{\boldsymbol{v}}_{k-1} \Delta t+\frac{1}{2}\left[\mathit{\boldsymbol{R}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\mathit{\boldsymbol{b}}_{{\rm f}, k-1}\right)+\mathit{\boldsymbol{g}}_{k-1}\right] \Delta t^{2}+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{p}}}}, k}\right\}- \\ &\quad \left\{\hat{\mathit{\boldsymbol{p}}}{}_{k-1}+\hat{\mathit{\boldsymbol{v}}}_{k-1} \Delta t+\frac{1}{2}\left[\hat{\mathit{\boldsymbol{R}}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}\right)+\hat{\mathit{\boldsymbol{g}}}_{k-1}\right] \Delta t^{2}\right\} \\ & =\left(\mathit{\boldsymbol{p}}_{k-1}-\hat{\mathit{\boldsymbol{p}}}{}_{k-1}\right)+\left(\mathit{\boldsymbol{v}}_{k-1}-\hat{\mathit{\boldsymbol{v}}}_{k-1}\right) \Delta t+\frac{1}{2}\left[\mathit{\boldsymbol{R}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\mathit{\boldsymbol{b}}_{{\rm f}, k-1}\right)-\hat{\mathit{\boldsymbol{R}}}_{k-1}\left(\mathit{\boldsymbol{f}}_{{\rm m}, k-1}-\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}\right)\right] \Delta t^{2}+\\ &\quad \frac{1}{2}\left(\mathit{\boldsymbol{g}}_{k-1}-\hat{\mathit{\boldsymbol{g}}}_{k-1}\right) \Delta t^{2}+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} \end{align*} $

对第3项进行展开，并忽略二次小量（需利用A.1中的已知结论(1)(2)），得到如下近似关系：

$ \begin{align*} & \; \mathit{\boldsymbol{R}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})-\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}}) \\ =&\; {\rm Exp\, } ({\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}})\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})-\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}}) \\ \overset{(1)}{\approx} &\; (\hat{\mathit{\boldsymbol{R}}}_{k-1} +[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1})({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})-\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}}) \\ =&\; \hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{b}}_{{\rm f}, k-1} +[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{f}}_{{\rm m}, k-1} -[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}} ]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{b}}_{{\rm f}, k-1} -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{f}}_{{\rm m}, k-1} +\hat{\mathit{\boldsymbol{R}}}_{k-1} \hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1} \\ =&\; -\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{b}}_{{\rm f}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})+[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{f}}_{{\rm m}, k-1} -[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}} ]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} \hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1} -[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} \\ \approx &\; -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} +[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times} \hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}}) \\ \overset{(2)}{=}&\; -[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})}]_{\times} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} \end{align*} $

整理可得：

$ \begin{align*} & \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{p}}}, k} \approx \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{p}}}, k-1} +\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k-1} {\Delta} t+\frac{1}{2} ({- [{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})} ]_{\times} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1}} ){\Delta} t^{2}+\frac{1}{2}\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{g}}}, k-1} {\Delta} t^{2}+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} \\ \Rightarrow \quad & \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{p}}} =\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{p}}}, k-1} +{\Delta} t\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k-1} -\frac{1}{2}{\Delta} t^{2} [{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})} ]_{\times} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -\frac{1}{2}{\Delta} t^{2}\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} +\frac{1}{2}{\Delta} t^{2}\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{g}}}, k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} \end{align*} $

A.3 速度误差运动模型

根据速度误差定义有：

$ \begin{align*} \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k} & =\mathit{\boldsymbol{v}}_{k} -\hat{\mathit{\boldsymbol{v}}}_{k} \\ & =\{{\mathit{\boldsymbol{v}}_{k-1} +[{\mathit{\boldsymbol{R}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})+\mathit{\boldsymbol{g}}_{k-1}}]{\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k}}\}-\{{\hat{\mathit{\boldsymbol{v}}}_{k-1} +[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})+\hat{\mathit{\boldsymbol{g}}}_{k-1}}]{\Delta} t}\} \\ & =({\mathit{\boldsymbol{v}}_{k-1} -\hat{\mathit{\boldsymbol{v}}}_{k-1}})+[{\mathit{\boldsymbol{R}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\rm f}, k-1}})-\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})}]{\Delta} t+({\mathit{\boldsymbol{g}}_{k-1} -\hat{\mathit{\boldsymbol{g}}}_{k-1}}){\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} \end{align*} $

忽略二次小量，整理可得：

$ \begin{align*} & \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k} \approx \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k-1} +({-[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})} ]_{\times} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1}}){\Delta} t+{\Delta} t\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{g}}}, k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} \\ \Rightarrow \quad & \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k} =\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k-1} -{\Delta} t[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})}]_{\times} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} +{\Delta} t\mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{g}}}, k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} \end{align*} $

A.4 姿态误差运动模型

使用姿态误差定义，并忽略二次小量（需利用A.1中的已知结论(1)(3)），可得：

$ \begin{align*} \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}, k} & ={\mathit{\boldsymbol{R}}}_{k} \hat{\mathit{\boldsymbol{R}}}{}_{k}^{\rm T} \\ &=[{\mathit{\boldsymbol{R}}_{k-1} {\rm E}{\rm{xp}}({({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\omega}, k-1}}){\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}})}][{\hat{\mathit{\boldsymbol{R}}}_{k-1} {\rm E}{\rm{xp}}({({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\omega}, k-1}}){\Delta} t})}]^{\rm T} \\ & =\mathit{\boldsymbol{R}}_{k-1} {\rm E}{\rm{xp}}({({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\omega}, k-1}}){\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}){\rm Exp\, } ({({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\omega}, k-1}}){\Delta} t})^{\rm T}\hat{\mathit{\boldsymbol{R}}}{}_{k-1}^{\rm T} \\ & \overset{(1)}{\approx} \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}, k-1} \hat{\mathit{\boldsymbol{R}}}_{k-1} ({{\mathit{\boldsymbol{I}}}+[{({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\mathit{\boldsymbol{b}}_{{\omega}, k-1}}){\Delta} t+\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}]_{\times}})({{\mathit{\boldsymbol{I}}}-[{({\mathit{\boldsymbol{\omega}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\omega}, k-1}}){\Delta} t} ]_{\times}})\hat{\mathit{\boldsymbol{R}}}{}_{k-1}^{\rm T} \\ & \approx \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}, k-1} \hat{\mathit{\boldsymbol{R}}}_{k-1} ({{\mathit{\boldsymbol{I}}}+[{-{\Delta} t\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}]_{\times}})\hat{\mathit{\boldsymbol{R}}}{}_{k-1}^{\rm T} \\ & \overset{(3)}{=} \mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}, k-1} ({{\mathit{\boldsymbol{I}}}+[{-{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}]_{\times}}) \\ & \overset{(1)}{\approx} ({\mathit{\boldsymbol{I}}+[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1}}]_{\times}})({{\mathit{\boldsymbol{I}}}+[{-{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}}]_{\times}}) \\ & \approx {\mathit{\boldsymbol{I}}}+[{\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k}} ]_{\times} \end{align*} $

假设姿态噪声协方差为数量矩阵，则姿态误差运动模型进一步表示为

$ \begin{align*} \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k} ={\rm Log\, } ({\mathit{\boldsymbol{\delta}}_{\mathit{\boldsymbol{R}}, k}})\approx \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k} \quad\Rightarrow \quad \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k} =\mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k-1} -{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1} \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k-1} +\mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k} \end{align*} $

A.5 其他误差运动模型

根据加速度计零偏误差、陀螺仪零偏误差和重力加速度误差的定义，可得：

$ \begin{align*} & \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k} =\mathit{\boldsymbol{b}}_{{\rm f}, k} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k} =\mathit{\boldsymbol{b}}_{{\rm f}, k-1} +\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1} =\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} +\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} \quad\Rightarrow \quad \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k} =\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k-1} +\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} \\ & \mathit{\boldsymbol{\varDelta}}_{{{\mathit{\boldsymbol{b}}}} \omega, k} =\mathit{\boldsymbol{b}}_{\omega, k}-\hat{\mathit{\boldsymbol{b}}}_{\omega, k}=\mathit{\boldsymbol{b}}_{\omega, k-1}+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{b}}}} \omega, k}-\hat{\mathit{\boldsymbol{b}}}_{\omega, k-1}=\mathit{\boldsymbol{\varDelta}}_{{{\mathit{\boldsymbol{b}}}} \omega, k-1}+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{b}}}} \omega, k} \quad\Rightarrow \quad \mathit{\boldsymbol{\varDelta}}_{{{\mathit{\boldsymbol{b}}}} \omega, k}=\mathit{\boldsymbol{\varDelta}}_{{{\mathit{\boldsymbol{b}}}} \omega, k-1}+\mathit{\boldsymbol{w}}_{{{\mathit{\boldsymbol{b}}}} \omega, k}\\ & \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}, k} =\mathit{\boldsymbol{g}}_{k}-\hat{\mathit{\boldsymbol{g}}}_{k}=\mathit{\boldsymbol{g}}_{k-1}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{g}}, k}-\hat{\mathit{\boldsymbol{g}}}_{k-1}=\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}, k-1}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{g}}, k} \quad\Rightarrow \quad \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}, k}=\mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{g}}, k-1}+\mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{g}}, k} \end{align*} $

A.6 误差运动模型的矩阵表示

误差运动模型的矩阵表示为$ \mathit{\boldsymbol{\varepsilon}}_{k} =\mathit{\boldsymbol{F}}_{k, k-1} \mathit{\boldsymbol{\varepsilon}}_{k-1} +\mathit{\boldsymbol{w}}_{{\rm{sys}}, k} $，其中，

$ \begin{align*} \mathit{\boldsymbol{\varepsilon}}_{k} &= \begin{bmatrix} \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{p}}}, k} \\ \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{v}}}, k} \\ \mathit{\boldsymbol{\delta}}_{{\mathit{\boldsymbol{a}}}, k} \\ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\rm f}, k} \\ \mathit{\boldsymbol{\varDelta}}_{\mathit{\boldsymbol{b}}{\omega}, k} \\ \mathit{\boldsymbol{\varDelta}}_{{\mathit{\boldsymbol{g}}}, k} \end{bmatrix} \mathit{\boldsymbol{F}}_{k, k-1}\\ &=\begin{bmatrix} {\mathit{\boldsymbol{I}}} & {{\Delta} t{\mathit{\boldsymbol{I}}}} & {-\dfrac{1}{2}{\Delta} t^{2}[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} \!\!-\!\!\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})} ]_{\times}} & {-\dfrac{1}{2}{\Delta} t^{2}\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} & {\dfrac{1}{2}{\Delta} t^{2}{\mathit{\boldsymbol{I}}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {-{\Delta} t[{\hat{\mathit{\boldsymbol{R}}}_{k-1} ({\mathit{\boldsymbol{f}}_{{\rm m}, k-1} -\hat{\mathit{\boldsymbol{b}}}_{{\rm f}, k-1}})}]_{\times}} & {-{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} & {{\Delta} t{\mathit{\boldsymbol{I}}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} & {-{\Delta} t\hat{\mathit{\boldsymbol{R}}}_{k-1}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{I}}} \end{bmatrix} \mathit{\boldsymbol{w}}_{{\rm{sys}}, k} = \begin{bmatrix} \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{p}}}, k} \\ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{v}}}, k} \\ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{a}}}, k} \\ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\rm f}, k} \\ \mathit{\boldsymbol{w}}_{\mathit{\boldsymbol{b}}{\omega}, k} \\ \mathit{\boldsymbol{w}}_{{\mathit{\boldsymbol{g}}}, k} \end{bmatrix} \end{align*} $