CN116380038A - 一种基于在线增量尺度因子图的多源导航信息融合方法 - Google Patents

Abstract

本发明公开了一种基于在线增量尺度因子图的多源导航信息融合方法，该方法以惯性导航系统为核心传感器构建因子图，针对因子图大规模运算时的效率问题，引入尺寸可自适应变化的动态滑动窗口方法来实现因子图的增量式更新，以融合时的协方差矩阵计算得到的圆概率误差为准，当预先设定的精度阈值高于或低于圆概率误差一定的比例，则减小或增大因子图优化时滑动窗口的尺寸，以实现根据导航精度自适应变化的滑动窗口方法。该方法相比现有的因子图多传感器信息融合方法，在保证优化结果的鲁棒性同时，提升传统因子图的优化效率。

Description

一种基于在线增量尺度因子图的多源导航信息融合方法

技术领域

本发明属于多源信息导航领域，具体而言，涉及一种基于在线增量尺度因子图的多源导航信息融合方法。

背景技术

目前，多传感器信息融合技术已经在导航领域中拥有广泛的应用。因子图的框架能够将不同更新率、不同误差的传感器合并起来进行应用。根据量测信息对导航状态变量的影响，可以将传感器抽象成量测因子。此外,基于因子图的多源信息融合导航方法，能方便地对来自异步异构传感器的数据进行处理，接收到传感器的输出数据后，扩充因子节点，根据系统的状态方程和量测方程快速有效的进行系统状态的更新，实现多传感器的数据综合处理，有效解决多源信息融合系统在优化过程中出现各种难以线性化的非线性化部分、无法获得准确的系统模型或多信息滤波时信息不同步等问题。

传统的因子图融合算法在如地下停车场、空旷的室外和卫星信号受到干扰等特殊场景下，传统因子图信息融合方法就会由于传感器失效造成漂移。且由于其自身结构，传统因子图的全局优化会造成计算量过大，计算效率较低。

本发明提供一种基于在线增量尺度因子图的多源导航信息融合方法，该方法相比于传统的因子图融合方法，其以惯性导航系统作为核心传感器，并加入其他辅助传感器以应对不同复杂场景下的应用环境。此外，该方法还针对传统因子图优化后期由于图规模增大导致优化效率降低的问题，引入了窗口尺寸可自适应变化的动态滑动窗口方法来实现因子图的增量式更新，以融合时的协方差矩阵计算得到的圆概率误差为准，当预先设定的精度阈值高于或低于圆概率误差一定的比例，则减小或增大因子图优化窗口的尺寸，以实现根据导航精度自适应变化的动态滑动窗口方法。该方法利用滑动窗口将传统因子图的结构尺度化，并用增量式的方法替代传统因子图的全局优化，在保证导航精度和鲁棒性的同时，显著提升因子图导航方法的优化效率。

发明内容

本发明提供一种基于在线增量尺度因子图的多源导航信息融合方法，其解决的技术问题在于：利用惯性导航和其他辅助传感器实现多场景下应用，利用动态滑动窗口方法实现因子图迭代优化，在保证算法的优化精度与鲁棒性时，显著提升因子图优化的计算效率。

本发明为达到上述目的采用以下技术方案：

一种基于在线增量尺度因子图的多源导航信息融合方法，采集惯性导航和其他辅助传感器的量测信息，并分别构建对应的传感器因子节点，通过动态滑动窗口方法实现上述组合导航系统的增量优化，然后利用因子图将传感器因子节点进行融合优化。利用该信息融合方法进行组合导航可以有效的达到缩减因子图优化计算量，并提高鲁棒性的目的。其步骤如下：

步骤1采集惯性导航系统的角速度和加速度输出数据；

步骤2采集辅助传感器的位置、速度与姿态数据；

步骤3由惯性导航系统与辅助传感器构建因子图；

步骤4引入可随估计精度窗口尺寸自适应变化的动态滑动窗口方法来实现因子图的增量式更新，以融合时的协方差矩阵计算得到的圆概率误差为准，当预先设定的精度阈值高于或低于圆概率误差一定的比例，则减小或增大因子图优化窗口的尺寸，以保证优化精度的同时，提升传统因子图的优化效率；

步骤5根据所确定的滑动窗口尺寸，进行因子图的增量式滑动优化，在贝叶斯网络上处理滑动时被丢弃的节点和新加入的节点，以实现根据导航精度窗口尺寸自适应变化的动态滑动窗口因子图优化方法。

优选地，所述步骤1包括以下步骤：

步骤1.1采集载体中惯性导航系统的加速度计测量数据，加速度计的输出数据如公式(1)所示：

其中，fⁿ为惯性导航系统的加速度计所输出的比力，

为载体相对于地球的加速度，ω_ie为地球自转角速度，ω_en为载体相对于地球的角速度，V_en为载体相对于地球的运动速度，g为载体处的地球的重力加速度。

步骤1.2采集载体中惯性导航系统的陀螺仪测量数据，陀螺仪的输出数据如公式(2)所示：

其中，

为载体系的陀螺仪相对于惯性系的角速度在载体系上的投影，/>

为地理系相对于惯性系的角速度在载体系上的投影，/>

为导航系相对于地理系的角速度在载体系上的投影，/>

为载体系相对于地理系的角速度在载体系上的投影；

优选地，所述步骤2包括以下步骤：

步骤2.1采集载体上的辅助传感器所输出的位置p_assi，速度v_assi，姿态

信息，具体采集信息如公式(3)所示：

其中，p_east,p_north,p_up分别表示辅助传感采集得到的东北天坐标系下的东向位置、北向位置与天向位置，v_east,v_north,v_up分别表示辅助传感采集得到的东北天坐标系下的东向速度、北向速度与天向速度，r,p,y分别表示辅助传感采集得到的横滚角、俯仰角与航向角。

优选地，所述步骤3包括以下步骤：

步骤3.1根据步骤1采集到的惯性导航系统的角速度和加速度测量值，构建IMU预积分因子，对一个预积分更新周期t_k～t_k+1内的测量数据在t_k时刻的载体系下进行预积分，得到位置、速度、姿态增量的计算公式如(4)所示：

其中，

和/>

分别表示当前时刻t的载体坐标系到t_k时刻载体坐标系的旋转矩阵与旋转速率矩阵，f_t ^b和/>

分别表示当前时刻t的加速度计和陀螺仪的测量值，/>

和/>

分别表示当前时刻t的加速度计和陀螺仪的偏差量。

根据INS的航位推算原理，得到t_k+1时刻的载体位置、速度和姿态如公式(5)所示：

其中，

和/>

分别表示t_k时刻的载体在导航坐标系下的位置、速度和姿态，gⁿ为t_k+1时刻导航坐标系下的重力矢量，/>

表示t_k时刻载体坐标系到导航坐标系的旋转矩阵。

由此得到IMU预积分因子如公式(6)所示：

其中，x_k+1和x_k分别表示t_k+1时刻与t_k时刻的位置、速度与姿态状态量，α_k表示惯性导航器件在t_k时刻的偏差变量，z_k表示t_k时刻的惯性器件测量值，h(x_k,α_k,z_k)表示由状态方程得到的t_k+1时刻的导航状态量预测值，其与当前估计值x_k+1的差值即为IMU预积分因子代表的需要进行的最小化误差函数，d(·)为代价函数。

步骤3.2根据上述的辅助传感器测量数据，利用其提供的速度、位置和姿态信息，构建辅助传感器因子，其量测方程

如公式(7)所示：

其中，h^assi(·)是t_k时刻辅助传感器的量测模型，n^assi为辅助传感器的测量噪声。则以辅助传感器构建的辅助传感器因子如公式(8)所示：

其中，d[·]为代价函数，err(·)为误差函数。

优选地，所述步骤4包括以下步骤：

步骤4.1首先为了平衡计算复杂度与导航精度，预先设定水平位置x和y方向上的期望精度为D＝(d_x,d_y)，令最大圆概率误差的偏差为(δ_x,δ_y)，而后计算以期望精度为圆心，设定偏差为半径的50％和95％圆概率误差，如公式(9)所示：

然后，以期望精度D为圆心，圆概率误差CEP和CEP95为半径作圆C1,C2，用于后续滑动窗口尺寸增加或减少的依据，如公式(10)所示：

步骤4.2为根据步骤3构建因子图，为获得较高的优化精度，先累积一定数量的因子节点进行优化，即将因子节点数量记作pri_accu；对因子节点数量pri_accu先进行一次优化，得到的值作为后续动态滑动窗口方法的先验值first_prior；

然后，使用先验值first_prior，把符合预设滑动窗口尺寸SW_first的因子节点数量加入到滑动窗口之中，进行多传感器因子图优化；

根据优化后所得到的协方差矩阵的逆Ω来获取权重矩阵以计算x,y方向上的偏差均值

Ω如公式(11)所示：

其中，θ是航向角，Ω_xx,Ω_yy,Ω_θθ分别是x,y,θ方向上的方差，Ω_xy,Ω_xθ,Ω_yθ分别是x和y、x和θ、y和θ的协方差，

分别是x和y、x和θ、y和θ的协方差的转置，两因子节点i,j之间的权重w_ij用公式(12)表示：

其中，det(·)为矩阵的行列式，使用公式(12)得到因子节点间的权重矩阵W，再通过公式(13)得到这一阶段的x和y二维平面的权重均值

其中，R是对角矩阵且其对角线为r_i＝∑_jw_ij，其对角线上的r₁和r₂即为偏差均值

z为当前的量测值，λ为设定的系数。

步骤4.3当偏差均值

在50％圆概率误差以内，说明优化后的结果精度较高；如果此时滑动窗口的尺寸大于预设的最小滑动窗口尺寸SW_min，则需要保持滑动窗口后面的传感器因子节点数量不变，而移除滑动窗口前面的传感器因子节点，如公式(14)所示：

其中，SW为当前滑动窗口的尺寸，考虑到若是精度过高时，需要重复计算公式(14)，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP的50％以上时，一次性减小滑动窗口尺寸SW_lose以降低计算复杂度，如公式(15)所示：

综合公式(14)和公式(15)，得到滑动窗口尺寸减小时的总公式如式(16)所示：

当滑动窗口向前滑动，并移除滑动窗口末端边缘的因子节点时，为防止移除边缘因子节点时造成的信息丢失，通过边缘化将边缘的因子节点转化为先验信息；从因子图中移除一个因子节点θ_i，等价于从联合概率密度函数中边缘化因子节点θ_i。从概率密度的角度，边缘化因子节点θ_i又等价于对因子节点θ_i进行积分，计算公式为式(17)所示：

其中，Θ表示状态变量集合，p(·)表示联合概率密度函数。公式(17)表示边缘化过程，对于末尾状态节点θ_n的积分仅需从联合概率分布的乘积末尾将θ_n舍去，计算公式为公式(18)所示：

步骤4.4当偏差均值

在95％圆概率误差以外，说明优化后的结果精度较低；如果此时滑动窗口尺寸小于预设的最大滑动窗口尺寸SW_max，则滑动窗口前面的节点不需移除，而只需要向滑动窗口中加入新的传感器因子节点，如公式(19)所示：

考虑到若是精度过低时，需要重复计算上式，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP95的50％以上时，一次性增大滑动窗口尺寸SW_increase以降低计算复杂度，如公式(20)所示：

综合公式(19)和公式(20)，得到滑动窗口尺寸增加时的总公式如式(21)所示：

步骤4.5当上述得到的偏差均值

在50％圆概率误差以外，在95％圆概率误差以内，或是当滑动窗口尺寸达到预设的最小尺寸SW_min或最大尺寸SW_max时就不再减少或者增加，如公式(22)所示：

优选地，所述步骤5包括以下步骤：

步骤5.1根据上述方程得到滑动窗口的具体尺寸SW，通过增量推理的方式完成因子图模型的更新：滑动窗口内因子图的代价函数分为两种类型，若为先验因子，则因子图的代价函数如式(23)所示，

其中，

为位置、速度、姿态先验因子信息，/>

为陀螺和加速度即的零偏漂移先验因子信息；

若不是先验因子，则因子图的代价函数如式(24)：

其中，

为滑动窗口内每一个IMU预积分因子在一定时间间隔内所得到的位置、速度、姿态增量Δp_i,Δv_i,Δr_i和对应陀螺仪与加速度计零偏增量/>

SW为滑动窗口的尺寸，N为在对应的时间内，含有的除惯导外其他辅助传感器数量，/>

为其他辅助传感器对应的代价函数；

步骤5.2根据利用因子图模型对代价函数进行分解，以使得每一个因子节点都跟传感器对应，由此得到简化后的导航状态最大后验估计如公式(25)所示：

其中，f_i(·)为因子节点对应的局部函数，X_i为对应传感器因子节点的状态量集合，X为导航系统所有状态变量节点的集合；

导航状态量和各传感量测值的后验概率密度函数p(X|Z)与所有因子节点乘积成正比，则有公式(26)：

其中，X是状态变量的集合，Z是传感器量测值集合，

为马氏距离的平方，Σ_i为对应的协方差矩阵，z_i是因子节点θ_i处的量测值。由此，各传感器的融合问题就是求解最大后验估计/>

而根据公式(26)看出系统状态量的最大后验估计本质上就是求解下列非线性最小二乘公式，如公式(27)所示：

而滑动窗口内的因子节点融合问题根据雅可比矩阵将公式(27)改为公式(28)：

其中，ΔX＝[ΔX₁,...,ΔX_SW]为滑动窗口内的导航状态量增量，

为当前优化估计值/>

下观测得到的残差，/>

为包含滑窗内所有因子节点的雅可比矩阵，如公式(29)所示：

其中，

表示状态变量X_i的雅可比矩阵对X_j的偏导数，/>

表示量测信息Z_i的雅可比矩阵对X_j的偏导数。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明在实时性和鲁棒性方面较传统算法拥有优越性。由于本发明以窗口尺寸可自适应变化的动态滑动窗口因子图增量平滑优化方法代替传统的因子图全局优化，大大提升了图优化的计算效率，故在因子图处理多种传感器的大规模数据时也能保证算法的实时性。此外，本发明用辅助传感器与惯性导航构成组合导航系统进行因子图融合，提高了在多种复杂场景下该算法的鲁棒性。

附图说明

图1为本发明的方法流程图；

图2为在相同数据下使用不同滑窗尺寸因子图优化的运行时间；

图3为在相同数据下使用不同滑窗尺寸因子图优化与真值三维对比结果；

图4为在相同数据下使用不同滑窗尺寸因子图优化与真值二维对比结果；

图5为在相同数据下使用不同滑窗尺寸因子图优化与真值三维误差对比结果；

图6为东北天坐标系下本算法优化结果、GNSS和真值数据的航迹对比；

图7为东北天坐标系下本算法优化结果与GNSS的三维位置误差对比；

图8为本算法优化过程中滑窗尺寸动态变化曲线图；

图9为使用本算法与传统isam优化算法在运行时间、东北天三轴的位置RMSE误差对比。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明，所述实施方式的示例在附图中示出，下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明是一种基于在线增量尺度因子图的多源导航信息融合方法，其方法流程如图1所示，包括以下几个步骤：

步骤1，采集惯性导航系统的角速度和加速度输出数据。所述步骤1的具体步骤包括以下步骤：

步骤1.1，采集载体中惯性导航系统的加速度计测量数据，加速度计的输出数据如如公式：

其中，fⁿ为惯性导航系统的加速度计所输出的比力，

为载体相对于地球的加速度，ω_ie为地球自转角速度，ω_en为载体相对于地球的角速度，V_en为载体相对于地球的运动速度，g为载体处的地球的重力加速度。

步骤1.2，采集载体中惯性导航系统的陀螺仪测量数据，陀螺仪的输出数据如公式所示：

其中，

为载体系的陀螺仪相对于惯性系的角速度在载体系上的投影，/>

为地理系相对于惯性系的角速度在载体系上的投影，/>

为导航系相对于地理系的角速度在载体系上的投影，/>

为载体系相对于地理系的角速度在载体系上的投影；

步骤2，采集辅助传感器的位置、速度与姿态数据，所述步骤2的具体步骤包括以下步骤：

步骤2.1，采集载体上的辅助传感器所输出的位置p_assi，速度v_assi，姿态

信息，具体采集信息如公式所示：

p_assi＝[p_east,p_north,p_up]

v_assi＝[v_east,v_north,v_up]

其中，p_east,p_north,p_up分别表示辅助传感采集得到的东北天坐标系下的东向位置、北向位置与天向位置，v_east,v_north,v_up分别表示辅助传感采集得到的东北天坐标系下的东向速度、北向速度与天向速度，r,p,y分别表示辅助传感采集得到的横滚角、俯仰角与航向角。

步骤3，由惯性导航系统与辅助传感器构建因子图，所述步骤3的具体步骤包括以下步骤：

步骤3.1，根据步骤1采集到的惯性导航系统的角速度和加速度测量值，构建IMU预积分因子，对一个预积分更新周期t_k～t_k+1内的测量数据在t_k时刻的载体系下进行预积分，得到位置、速度、姿态增量的计算公式如所示：

其中，

和/>

分别表示当前时刻t的载体坐标系到t_k时刻载体坐标系的旋转矩阵与旋转速率矩阵，f_t ^b和/>

分别表示当前时刻t的加速度计和陀螺仪的测量值，/>

和/>

分别表示当前时刻t的加速度计和陀螺仪的偏差量。

根据INS的航位推算原理，得到t_k+1时刻的载体位置、速度和姿态如公式所示：

其中，

和/>

分别表示t_k时刻的载体在导航坐标系下的位置、速度和姿态，gⁿ为t_k+1时刻导航坐标系下的重力矢量，/>

表示t_k时刻载体坐标系到导航坐标系的旋转矩阵。

由此得到IMU预积分因子如公式所示：

其中，x_k+1和x_k分别表示t_k+1时刻与t_k时刻的位置、速度与姿态状态量，α_k表示惯性导航器件在t_k时刻的偏差变量，z_k表示t_k时刻的惯性器件测量值，h(x_k,α_k,z_k)表示由状态方程得到的t_k+1时刻的导航状态量预测值，其与当前估计值x_k+1的差值即为IMU预积分因子代表的需要进行的最小化误差函数，d(·)为代价函数。

步骤3.2，根据上述的辅助传感器测量数据，利用其提供的速度、位置和姿态信息，构建辅助传感器因子，其量测方程

如公式所示：

其中，h^assi(·)是t_k时刻辅助传感器的量测模型，n^assi为辅助传感器的测量噪声。则以辅助传感器构建的辅助传感器因子如公式所示：

其中，d[·]为代价函数，err(·)为误差函数。

步骤4，引入随估计精度窗口尺寸自适应变化的动态滑动窗口方法来实现因子图的增量式更新，以融合时的协方差矩阵计算得到的圆概率误差为准，当预先设定的精度阈值高于或低于圆概率误差一定的比例，则减小或增大因子图优化窗口的尺寸，以保证优化精度的同时，提升传统因子图的优化效率。所述步骤4的具体步骤包括以下步骤：

步骤4.1，首先为了平衡计算复杂度与导航精度，预先设定水平位置x和y方向上的期望精度为D＝(d_x,d_y)，令最大圆概率误差的偏差为(δ_x,δ_y)，而后计算以期望精度为圆心，设定偏差为半径的50％和95％圆概率误差，如公式所示：

CEP＝0.589(δ_x+δ_y)

CEP95＝1.2272(δ_x+δ_y)

然后，以期望精度D为圆心，圆概率误差CEP和CEP95为半径作圆C1,C2，用于后续滑动窗口尺寸增加或减少的依据，如公式(10)所示：

步骤4.2，根据步骤3构建因子图，为获得较高的优化精度，先累积一定数量的因子节点进行优化，即将因子节点数量记作pri_accu；对因子节点数量pri_accu先进行一次优化，得到的值作为后续动态滑动窗口方法的先验值first_prior；

然后，使用先验值first_prior，把符合预设滑动窗口尺寸SW_first的因子节点数量加入到滑动窗口之中，进行多传感器因子图优化；

根据优化后所得到的协方差矩阵的逆Ω来获取权重矩阵以计算x,y方向上的偏差均值

Ω如公式所示：

其中，θ是航向角，Ω_xx,Ω_yy,Ω_θθ分别是x,y,θ方向上的方差，Ω_xy,Ω_xθ,Ω_yθ分别是x和y、x和θ、y和θ的协方差，

分别是x和y、x和θ、y和θ的协方差的转置，两因子节点i,j之间的权重w_ij用公式表示：

其中，det(·)为矩阵的行列式，使用上式得到因子节点间的权重矩阵W，再通过下式得到这一阶段的x和y二维平面的权重均值

其中，R是对角矩阵且其对角线为r_i＝∑_jw_ij，其对角线上的r₁和r₂即为偏差均值

z为当前的量测值，λ为设定的系数。

步骤4.3，当偏差均值

在50％圆概率误差以内，说明优化后的结果精度较高；如果此时滑动窗口的尺寸大于预设的最小滑动窗口尺寸SW_min，则需要保持滑动窗口后面的传感器因子节点数量不变，而移除滑动窗口前面的传感器因子节点，如公式：

其中，SW为当前滑动窗口的尺寸，考虑到若是精度过高时，需要重复计算公式(14)，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP的50％以上时，一次性减小滑动窗口尺寸SW_lose以降低计算复杂度，如公式所示：

综合上述两个式子，可以得到滑动窗口尺寸减小时的总公式如下式：

当滑动窗口向前滑动，并移除滑动窗口末端边缘的因子节点时，为防止移除边缘因子节点时造成的信息丢失，通过边缘化将边缘的因子节点转化为先验信息；从因子图中移除一个因子节点θ_i，等价于从联合概率密度函数中边缘化因子节点θ_i。从概率密度的角度，边缘化因子节点θ_i又等价于对因子节点θ_i进行积分，计算公式如下所示：

其中，Θ表示状态变量集合，p(·)表示联合概率密度函数。公式(17)表示边缘化过程，对于末尾状态节点θ_n的积分仅需从联合概率分布的乘积末尾将θ_n舍去，计算公式为下式所示：

步骤4.4，当偏差均值

在95％圆概率误差以外，说明优化后的结果精度较低；如果此时滑动窗口尺寸小于预设的最大滑动窗口尺寸SW_max，则滑动窗口前面的节点不需移除，而只需要向滑动窗口中加入新的传感器因子节点，即公式：

考虑到若是精度过低时，需要重复计算上式，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP95的50％以上时，一次性增大滑动窗口尺寸SW_increase以降低计算复杂度，即公式：

综合上述两个式子，可以得到滑动窗口尺寸增加时的总公式如下式：

/>

步骤4.5，当上述得到的偏差均值

在50％圆概率误差以外，在95％圆概率误差以内，或是当滑动窗口尺寸达到预设的最小尺寸SW_min或最大尺寸SW_max时就不再减少或者增加，即公式：

SW不变

步骤5，根据所确定的滑动窗口尺寸，进行因子图的增量式滑动优化，在贝叶斯网络上处理滑动时被丢弃的因子节点和新加入的因子节点，以实现根据导航精度窗口尺寸自适应变化的动态滑动窗口因子图优化方法。所述步骤5的具体步骤包括以下步骤：

步骤5.1，根据上述方程得到滑动窗口的具体尺寸SW，通过增量推理的方式完成因子图模型的更新：滑动窗口内因子图的代价函数分为两种类型，若为先验因子，则因子图的代价函数如式所示：

其中，

为位置、速度、姿态先验因子信息，/>

为陀螺和加速度即的零偏漂移先验因子信息；

若不是先验因子，则因子图的代价函数如式：

其中，

为滑动窗口内每一个IMU预积分因子在一定时间间隔内所得到的位置、速度、姿态增量Δp_i,Δv_i,Δr_i和对应陀螺仪与加速度计零偏增量/>

SW为滑动窗口的尺寸，N为在对应的时间内，含有的除惯导外其他辅助传感器数量，/>

为其他辅助传感器对应的代价函数。

步骤5.2，根据利用因子图模型对代价函数进行分解，以使得每一个因子节点都跟传感器对应，由此得到简化后的导航状态最大后验估计如公式：

其中，f_i(·)为因子节点对应的局部函数，X_i为对应传感器因子节点的状态量集合，X为导航系统所有状态变量节点的集合；

导航状态量和各传感量测值的后验概率密度函数p(X|Z)与所有因子节点乘积成正比，即有公式：

其中，X是状态变量的集合，Z是传感器量测值集合，

为马氏距离的平方，Σ_i为对应的协方差矩阵，z_i是因子节点θ_i处的量测值。由此，各传感器的融合问题就是求解最大后验估计/>

而根据公式(26)看出系统状态量的最大后验估计本质上就是求解下列非线性最小二乘公式，如公式：

而滑动窗口内的因子节点融合问题根据雅可比矩阵将上式改为如下公式：

其中，ΔX＝[ΔX₁,...,ΔX_SW]为滑动窗口内的导航状态量增量，

为当前优化估计值/>

下观测得到的残差，/>

为包含滑窗内所有因子节点的雅可比矩阵，如公式所示：

其中，

表示状态变量X_i的雅可比矩阵对X_j的偏导数，/>

表示量测信息Z_i的雅可比矩阵对X_j的偏导数。

实施例：

实施例基于以下的仿真环境：以惯导为核心传感器，GNSS为辅助传感器，使用实际传感器和仿真数据两种途径得到的惯导数据与GNSS数据进行惯导与GNSS组合导航下的动态滑动窗口因子图优化算法的验证。

使用本算法进行动态滑动窗口下的因子图优化结果作为实例说明。使用实际传感器数据的验证结果如图2，3，4，5所示。使用不同滑窗尺寸因子图优化的运行时间如图2所示，使用不同滑窗尺寸因子图优化与真值三维对比结果如图3所示，使用不同滑窗尺寸因子图优化与真值二维对比结果如图4所示，使用不同滑窗尺寸因子图优化与真值三维误差对比结果如图5所示。

使用本算法进行动态滑动窗口下的因子图优化结果作为实例说明。使用仿真数据的验证结果如图6，7，8，9所示。仿真数据采用一条设定的300秒车载轨迹，并在51-110秒中设定GNSS的东北天位置误差为10米，10米，2米；在161-200秒中设定GNSS的东北天位置误差为30米，30米，4米；在241-270秒中设定GNSS的东北天位置误差为50米，50米，5米。东北天坐标系下本算法、GNSS和真值数据的航迹对比如图6所示，东北天坐标系下本算法、GNSS的三轴误差对比如图7所示，在算法优化过程中滑窗尺寸变化如图8所示，使用本算法与传统isam算法对比结果如图9所示。

算法首先累积一定的因子节点用于第一次优化以增加其初值的准确性，而后通过一个的滑动窗口来控制后续优化过程中所加入的因子图节点数量。从图2中可以看到，在相同数据下使用不同滑动窗口的尺寸，其优化时间是不同的，且滑动窗口的尺寸越大，其所花费的时间也越长。从图3和图4中可以看到，无论是三维坐标系还是二维坐标系，滑窗尺寸越小，其估计的轨迹与GNSS真值的误差就越大。从上述图2，3，4中可以看到，滑窗尺寸越大，所耗费的优化时间越长，优化得到的精度越准确，与本专利的理论部分相符。图5则分别从x,y,z三个方向清晰展示了滑窗尺寸与因子图优化精度间的关系，进一步佐证了上述的结论。从图6和图7可以看出，本算法在面对仿真数据中设定的GNSS噪音可以自适应的调整滑动窗口的大小以得到算法优化时间与优化精度的平衡。图8展示了不同时间段滑动窗口尺寸的自适应变化情况。图9使用本算法与传统isam算法进行比对，可以看出，与不使用滑窗算法的传统方法相比，本算法运行时间更短，且东北天三轴的均方根误差与传统方法相当甚至更低，说明了本专利理论的有效性。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的任一单元和全部组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

本技术领域技术人员可以理解的是，本发明可以涉及用于执行本申请中所述操作中的一项或多项操作的设备。所述设备可以为所需的目的而专门设计和制造，或者也可以包括通用计算机中的已知设备，所述通用计算机有存储在其内的程序选择性地激活或重构。这样的计算机程序可以被存储在设备(例如，计算机)可读介质中或者存储在适于存储电子指令并分别耦联到总线的任何类型的介质中，所述计算机可读介质包括但不限于任何类型的盘(包括软盘、硬盘、光盘、CD-ROM、和磁光盘)、随机存储器(RAM)、只读存储器(ROM)、电可编程ROM、电可擦ROM(EPROM)、电可擦除可编程ROM(EEPROM)、闪存、磁性卡片或光线卡片。可读介质包括用于以由设备(例如，计算机)可读的形式存储或传输信息的任何机构。例如，可读介质包括随机存储器(RAM)、只读存储器(ROM)、磁盘存储介质、光学存储介质、闪存装置、以电的、光的、声的或其他的形式传播的信号(例如载波、红外信号、数字信号)等。

本技术领域技术人员可以理解的是，可以用计算机程序指令来实现这些结构图和/或框图和/或流图中的每个框以及这些结构图和/或框图和/或流图中的框的组合。可以将这些计算机程序指令提供给通用计算机、专业计算机或其他可编程数据处理方法的处理器来生成机器，从而通过计算机或其他可编程数据处理方法的处理器来执行的指令创建了用于实现结构图和/或框图和/或流图的框或多个框中指定的方法。

本技术领域技术人员可以理解的是，本发明中已经讨论过的各种操作、方法、流程中的步骤、措施、方案可以被交替、更改、组合或删除。进一步地，具有本发明中已经讨论过的各种操作、方法、流程中的其他步骤、措施、方案也可以被交替、更改、重排、分解、组合或删除。进一步地，现有技术中的具有与本发明中公开的各种操作、方法、流程中的步骤、措施、方案也可以被交替、更改、重排、分解、组合或删除。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于，方法包括以下步骤：

步骤1：采集载体中惯性导航系统的角速度和加速度输出数据；

步骤2：采集载体中辅助传感器的位置、速度与姿态数据；

步骤3：根据惯性导航系统与辅助传感器构建因子图；

步骤4：引入随估计精度窗口尺寸自适应变化的动态滑动窗口方法来实现因子图的增量式更新，以融合时的协方差矩阵计算得到的圆概率误差为准，当预先设定的精度阈值高于或低于圆概率误差一定的比例，则减小或增大因子图滑动窗口的尺寸；

步骤5：根据所确定的滑动窗口尺寸，进行因子图的增量式滑动优化，在贝叶斯网络上处理滑动时被丢弃的因子节点和新加入的因子节点。

2.根据权利要求1所述的一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于,步骤1的实现过程为：

步骤1.1：采集载体中惯性导航系统的加速度计测量数据，加速度计的输出数据如公式(1)所示：

其中，fⁿ为惯性导航系统的加速度计所输出的比力，

为载体相对于地球的加速度，ω_ie为地球自转角速度，ω_en为载体相对于地球的角速度，V_en为载体相对于地球的运动速度，g为载体处的地球的重力加速度；

步骤1.2：采集载体中惯性导航系统的陀螺仪测量数据，陀螺仪的输出数据如公式(2)所示：

其中，

为载体系的陀螺仪相对于惯性系的角速度在载体系上的投影，/>

为地理系相对于惯性系的角速度在载体系上的投影，/>

为导航系相对于地理系的角速度在载体系上的投影，/>

为载体系相对于地理系的角速度在载体系上的投影。

3.根据权利要求2所述的一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于,步骤2的实现过程为：

步骤2.1：采集载体上的辅助传感器所输出的位置p_assi，速度v_assi，姿态

信息，具体采集信息如公式(3)所示：

其中，p_east,p_north,p_up分别表示辅助传感采集得到的东北天坐标系下的东向位置、北向位置与天向位置，v_east,v_north,v_up分别表示辅助传感采集得到的东北天坐标系下的东向速度、北向速度与天向速度，r,p,y分别表示辅助传感采集得到的横滚角、俯仰角与航向角。

4.根据权利要求3所述的一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于,步骤3的实现过程为：

步骤3.1：根据步骤1采集到的惯性导航系统的角速度和加速度测量值，构建IMU预积分因子，对一个预积分更新周期t_k～t_k+1内的测量数据在t_k时刻的载体系下进行预积分，得到位置增量

速度增量/>

姿态增量/>

的计算公式如(4)所示：

其中，

和/>

分别表示当前时刻t的载体坐标系到t_k时刻载体坐标系的旋转矩阵与旋转速率矩阵，f_t ^b和/>

分别表示当前时刻t的加速度计和陀螺仪的测量值，/>

和/>

分别表示当前时刻t的加速度计和陀螺仪的偏差量；

根据INS的航位推算原理，得到t_k+1时刻的载体位置、速度和姿态如公式(5)所示：

其中，

和/>

分别表示t_k时刻的载体在导航坐标系下的位置、速度和姿态，gⁿ为t_k+1时刻导航坐标系下的重力矢量，/>

表示t_k时刻载体坐标系到导航坐标系的旋转矩阵；

由此得到IMU预积分因子如公式(6)所示：

其中，x_k+1和x_k分别表示t_k+1时刻与t_k时刻的位置、速度与姿态状态量，α_k表示惯性导航器件在t_k时刻的偏差变量，z_k表示t_k时刻的惯性器件测量值，h(x_k,α_k,z_k)表示由状态方程得到的t_k+1时刻的导航状态量预测值，其与当前估计值x_k+1的差值即为IMU预积分因子代表的需要进行的最小化误差函数，d(·)为代价函数；

步骤3.2：构建辅助传感器因子，其量测方程

如公式(7)所示：

其中，h^assi(·)是t_k时刻辅助传感器的量测模型，n^assi为辅助传感器的测量噪声；则以辅助传感器构建的辅助传感器因子如公式(8)所示：

其中，d[·]为代价函数，err(·)为误差函数。

5.根据权利要求4所述的一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于,步骤4的实现过程为：

步骤4.1：首先为了平衡计算复杂度与导航精度，预先设定水平位置x和y方向上的期望精度为D＝(d_x,d_y)，令最大圆概率误差的偏差为(δ_x,δ_y)，而后计算以期望精度为圆心，设定偏差为半径的50％和95％圆概率误差，如公式(9)所示：

然后，以期望精度D为圆心，圆概率误差CEP和CEP95为半径作圆C1,C2，用于后续滑动窗口尺寸增加或减少的依据，如公式(10)所示：

步骤4.2：根据步骤3构建因子图，为提高优化精度，先累积一定数量的因子节点进行优化，即将因子节点数量记作pri_accu；对因子节点数量pri_accu先进行一次优化，得到的值作为后续动态滑动窗口方法的先验值first_prior；

然后，使用先验值first_prior，把符合预设滑动窗口尺寸SW_first的因子节点数量加入到滑动窗口之中，进行多传感器因子图优化；

根据优化后所得到的协方差矩阵的逆Ω来获取权重矩阵以计算x,y方向上的偏差均值

Ω如公式(11)所示：

其中，θ是航向角，Ω_xx,Ω_yy,Ω_θθ分别是x,y,θ方向上的方差，Ω_xy,Ω_xθ,Ω_yθ分别是x和y、x和θ、y和θ的协方差，

分别是x和y、x和θ、y和θ的协方差的转置，两因子节点i,j之间的权重w_ij用公式(12)表示：

其中，det(·)为矩阵的行列式，使用公式(12)得到因子节点间的权重矩阵W，再通过公式(13)得到这一阶段的x和y二维平面的权重均值(μ_x,μ_y)：

其中，R是对角矩阵且其对角线为r_i＝∑_jw_ij，其对角线上的r₁和r₂即为偏差均值

z为当前的量测值，λ为设定的系数；

步骤4.3：当偏差均值

在50％圆概率误差以内，说明优化后的结果精度高；如果此时滑动窗口的尺寸大于预设的最小滑动窗口尺寸SW_min，则需要保持滑动窗口后面的因子节点数量不变，而移除滑动窗口前面的因子节点，如公式(14)所示：

其中，SW为当前滑动窗口的尺寸，考虑到若是精度过高时，需要重复计算公式(14)，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP的50％以上时，一次性减小滑动窗口尺寸SW_lose以降低计算复杂度，如公式(15)所示：

综合公式(14)和公式(15)，得到滑动窗口尺寸减小时的总公式如式(16)所示：

当滑动窗口向前滑动，并移除滑动窗口末端边缘的因子节点时，为防止移除边缘因子节点时造成的信息丢失，通过边缘化将边缘的因子节点转化为先验信息；从因子图中移除一个因子节点θ_i，等价于从联合概率密度函数中边缘化因子节点θ_i；从概率密度的角度，边缘化因子节点θ_i又等价于对因子节点θ_i进行积分，计算公式为式(17)所示：

其中，Θ表示状态变量集合，p(·)表示联合概率密度函数；公式(17)表示边缘化过程，对于末尾状态节点θ_n的积分仅需从联合概率分布的乘积末尾将θ_n舍去，计算公式为公式(18)所示：

步骤4.4：当偏差均值

在95％圆概率误差以外，说明优化后的结果精度低；如果此时滑动窗口尺寸小于预设的最大滑动窗口尺寸SW_max，则滑动窗口前面的节点不需移除，而只需要向滑动窗口中加入新的因子节点，如公式(19)所示：

考虑到若是精度过低时，需要重复计算上式，导致算法的计算复杂度增加；因此，设定当精度阈值高于CEP95的50％以上时，一次性增大滑动窗口尺寸SW_increase以降低计算复杂度，如公式(20)所示：

综合公式(19)和公式(20)，得到滑动窗口尺寸增加时的总公式如式(21)所示：

步骤4.5：当上述得到的偏差均值

在50％圆概率误差以外，在95％圆概率误差以内，或是当滑动窗口尺寸达到预设的最小尺寸SW_min或最大尺寸SW_max时就不再减少或者增加，如公式(22)所示：

6.根据权利要求5所述的一种基于在线增量尺度因子图的多源导航信息融合方法，其特征在于,步骤5的实现过程为：

步骤5.1：根据上述方程得到滑动窗口的具体尺寸SW，通过增量推理的方式完成因子图模型的更新：滑动窗口内因子图的代价函数分为两种类型，若为先验因子，则因子图的代价函数如式(23)所示，

其中，

为位置、速度、姿态先验因子信息，/>

为陀螺和加速度即的零偏漂移先验因子信息；

若不是先验因子，则因子图的代价函数如式(24)：

其中，

为滑动窗口内每一个IMU预积分因子在一定时间间隔内所得到的位置、速度、姿态增量Δp_i,Δv_i,Δr_i和对应陀螺仪与加速度计零偏增量

SW为滑动窗口的尺寸，N为在对应的时间内，含有的除惯导外其他辅助传感器数量，/>

为其他辅助传感器对应的代价函数；

步骤5.2根据利用因子图对代价函数进行分解，以使得每一个因子节点都跟传感器对应，由此得到简化后的导航状态最大后验估计如公式(25)所示：

其中，f_i(·)为因子节点对应的局部函数，X_i为对应传感器因子节点的状态量集合，X为导航系统所有状态变量节点的集合；

导航状态量和各传感量测值的后验概率密度函数p(X|Z)与所有因子节点乘积成正比，则有公式(26)：

其中，X是状态变量的集合，Z是传感器量测值集合，

为马氏距离的平方，Σ_i为对应的协方差矩阵，z_i是因子节点θ_i处的量测值；由此，各传感器的融合问题就是求解最大后验估计/>

而根据公式(26)看出系统状态量的最大后验估计本质上就是求解下列非线性最小二乘公式，如公式(27)所示：

而滑动窗口内的因子节点融合问题根据雅可比矩阵将公式(27)改为公式(28)：

其中，ΔX＝[ΔX₁,...,ΔX_SW]为滑动窗口内的导航状态量增量，

为当前优化估计值/>

下观测得到的残差，/>

为包含滑窗内所有因子节点的雅可比矩阵，如公式(29)所示：

其中，

表示状态变量X_i的雅可比矩阵对X_j的偏导数，/>

表示量测信息Z_i的雅可比矩阵对X_j的偏导数。

Images