基于精化预积分的因子图GNSS/INS松组合定位

2023-11-07杨显赐乔书波肖国锐彭华东李松伟徐海龙

测绘工程 2023年6期

杨显赐,乔书波,肖国锐,彭华东,李松伟,徐海龙

(1.信息工程大学地理空间信息学院,郑州 450001:2.31121部队, 南京 210001)

GNSS/INS组合导航在军事、测绘领域一直都有广泛应用。惯性导航系统(inertial navigation system,INS)通过测量载体的角运动和线运动信息,在初始姿态、速度和位置已知的情况下,根据力学编排方程,依次递推出载体在各个时刻的姿态、速度和位置,具有自主性高、隐蔽性强、数据更新速率高[1],且不受外界环境干扰的优点,但由于加速度计和陀螺仪存在零偏等各种误差,定位精度随时间发散。全球导航卫星系统可以为用户提供全天候、连续、实时、精确的三维坐标信息[2],但卫星信号容易受到外界环境干扰发生中断。在GNSS/INS组合导航中,引入GNSS定位结果对INS进行校正,结合两种导航系统的优点,可以有效提高组合导航系统的稳定性与定位精度。

在GNSS/INS组合导航中,扩展卡尔曼滤波(extended kalman filter,EKF)是一种理论成熟、应用广泛的非线性滤波方法。文献[3]通过仿真实验验证了基于EKF的组合导航能有效跟踪载体轨迹的速度、姿态、位置,稳定性良好。文献[4]在EKF的基础上建立微机电系统(micro electro mechanical system,MEMS)INS/GNSS松组合(loosely coupled,LC)数学模型,可以明显提高组合导航的定位精度和稳定性。但是,EKF假设了马尔可夫性,认为当前时刻的状态只与上一时刻有关,忽略了上一时刻之前的信息,是增量递推过程,并且EKF的一次线性化不能做到近似整个函数,会导致精度损失。FGO利用非线性优化方法批量处理数据,求取一个滑动窗口内的最大后验概率估计(maximum a posteriori,MAP),使FGO算法可以追溯历史信息,获取全局最优解。文献[5]通过仿真实验验证了当多传感器可用性改变和产生异步时延时,因子图具有更好的容错性与扩展性。文献[6]通过仿真实验对比分析FGO算法与EKF算法,实验结果表明在模型非线性化特征明显情况下,FGO算法迭代求解能够减小误差,定位精度更高,误差曲线更加平滑。文献[7]使用香港地区城市峡谷中的实测数据进行实验分析,结果表明优化窗口的大小会影响因子图信息融合性能,并且通过将FGO优化器退化为“类EKF估计器”,分析验证FGO精度改进的原因之一是多次迭代。文献[8]在IMU预积分过程中考虑地球自转的影响,实验证明对于工业级MEMS模块,考虑地球自转时可以有效提高松组合的定位性能。

因此,本文基于FGO算法进行GNSS/INS组合导航的状态估计。考虑到因子图算法在同时定位与建图领域中使用的惯性测量单元(inertial measurement unit,IMU)精度较低,忽略了地球自转的影响,进一步设计精化预积分(refined pre-integration,RP)提高组合导航定位精度。采集三组包含导航级、MEMS级别的惯性导航系统进行实验验证,GNSS数据为精密单点定位(precise point positioning,PPP)产品。

1 基于因子图的GNSS/INS组合导航1.1 因子图算法原理

因子图是用来表达随机变量联合概率分布的二分图模型H=(F,X,E)[9-10],由因子节点f∈F和变量节点x∈X组成,边e∈E为存在于因子节点与变量节点之间的一条连接边。在贝叶斯网络中,所有变量联合的概率密度是由每个节点关联的条件概率密度相乘得到,类推到因子图,将联合概率密度表示为一系列因子的乘积,即:

(1)

随机系统状态空间模型,由一个运动方程和一个观测方程构成,即:

(2)

(3)

将式(1)、式(3)联合起来,即:

(4)

根据最大后验概率估计,由量测信息计算状态变量最优值,将式(4)转化为一个等价的最小二乘优化问题,即:

XMAP=argmax(P(Xk∣Zk))=

(5)

1.2 IMU精化预积分因子构建

基于上节理论基础,首先构建IMU预积分因子。在INS经典力学编排方程[11](mechanization equations,由惯性传感器输出求解导航参数的一套计算方案)中,姿态、速度、位置更新是逐历元进行的,在组合导航时,GNSS的采样频率远小于IMU,为了提高计算效率,使用IMU预积分理论对INS进行状态更新,预积分的思想常用在同时定位与建图领域中,但INS与相机等数据融合时,IMU的精度较低,没有考虑地球自转的影响,在GNSS/INS组合导航中,IMU陀螺仪的零偏稳定性小于或远小于地球自转角速率,容易受到地球自转的影响,因此本文进一步设计了精化预积分。

首先构建IMU测量模型:

(6)

根据INS运动学模型,可以得到位置、速度、姿态对时间的微分方程:

(7)

(8)

(9)

将式(9)的离散形式表示为:

(10)

其中:

这两项体现出考虑地球自转时对速度、位置的修正。

由式(9)减去式(10),就得到了IMU精化预积分的残差向量,由此构建IMU精化预积分因子:

(11)

1.3 GNSS PPP因子构建

(12)

式中:l为b系下IMU的几何中心相对于GNSS接收机天线相位中心的空间杆臂向量。

1.4 因子图模型递推方法

基于1.2、1.3节,构建IMU精化预积分因子和GNSS PPP因子,将对应的因子节点添加至因子图模型中,当接收到GNSS、IMU的测量信息时,更新因子图框架,进行变量节点更新。GNSS/INS组合导航因子图框架如图1所示,IMU精化预积分因子连接相邻两时刻的状态变量,各个时刻的GNSS PPP因子通过连接边与对应的状态变量相连接。

图1 GNSS/INS松组合因子图框架

图1中,先验因子包括运载体的初始位置等,定义为均值ux、方差∑x的高斯分布模型,即:

fprior(x)=x-ux.

(13)

综上,得到状态变量的最大后验概率估计:

(14)

式中:WZ表示优化窗口内预积分因子个数;M表示优化窗口内PPP因子个数;当GNSS未发生中断时,WZ=M。

当使用非线性优化方法批量处理数据时,观测值以一个时间序列到达,如果每次都进行完整的批量优化会导致计算效率低,无法保证计算的实时性,通过使用增量非线性最小二乘估计算法iSAM2[12],当新的因子添加进来时,只优化被新的因子影响的那一部分,从而提高解算效率。

2 基于FGO与EKF的融合过程对比分析

目前大多数导航系统状态估计的理论基础是卡尔曼滤波算法,在卫星导航、组合导航、多源数据融合中均有广泛应用[14]。松组合情况下,INS根据IMU输出信息解算出运载体的位置、速度、姿态,与PPP获取的定位产品在EKF滤波器中进行信息融合。GNSS/INS组合导航扩展卡尔曼滤波框架如图2所示。

图2 GNSS/INS组合导航扩展卡尔曼滤波框架

EKF通过预测、更新两个步骤,可以较为简洁地实现组合导航信息融合,在计算资源受限、待估量简单的情况下,是一种有效的信息融合手段,但其假设的马尔可夫性无法追溯历史信息,也存在线性化不足的缺点。

因子图使用非线性优化方法[13-15],可以获得全局最优解,充分利用一段时间内的全部信息,并通过高斯牛顿法或列文伯格-马夸尔特法做迭代优化,当迭代一次状态估计发生改变后,重新对新的估计点进行泰勒展开,而不是像EKF只在固定点做一次泰勒展开,这就使得FGO的优化范围更广、效果更佳。因此本文认为,相较于EKF,FGO的精度和可靠性都会更优。

3 车载实测实验与性能分析

车载实测实验利用加拿大诺瓦泰公司SPAN-ISA-100C型高精度光纤惯导、迈普时空研发的POS320光纤惯导和MP-M39型MEMS惯性测量单元进行惯导数据采集。如图3所示,将三组惯导和一台GNSS接收机安装在一个刚性平台上,通过跑车试验获取三组GNSS/INS组合导航数据。图4为组合导航实验轨迹图,实验时长约1.8 h,环境开阔。三组IMU的采样频率均为200 Hz,其标定参数如表1所示,表中最后一列数据为北东地坐标系下三组惯导的空间杆臂臂长,数据处理中GNSS定位结果为PPP高精度位置产品,GNSS的数据更新频率为1 Hz,经度、纬度、高程测量噪声的标准差为0.052 m、0.049 m、0.106 m,IMU预积分的频率为1 Hz。处理设备为ThinkPad P15v,FGO算法的实验环境为ubuntu20.04,编程语言为C++,EKF算法的实验环境为Window10,编程语言为Matlab。

表1 因子图在6种窗口大小下松组合位置误差

图3 GNSS/INS组合导航车载实验

图4 GNSS/INS组合导航实测轨迹

3.1 因子图定位性能分析

为验证因子图、IMU精化预积分的可行性和有效性,对因子图的定位性能进行实验分析。FGO作为一种批量处理数据的算法,需要有效控制解算规模,保证计算的实时性,通过滑动窗口将FGO固定在一个时间窗口内,随历元向前推进,不计算窗口之外的数据,获取全局最优解,在保证计算效率的同时得到最理想的定位结果。本文设置窗口大小为1 s、5 s、10 s、20 s、30 s、60 s,分析不同窗口下的定位结果得到最优的开窗大小,并与滤波解算的时间做对比,分析因子图在不同开窗大小下的时间效率,表1、表2分别为6种窗口大小下三维位置的均方根误差(root mean squared error,RMSE)和运行时间。

表2 因子图在6种窗口大小下松组合运行时间 s

表3 惯性导航元件标定参数及空间杠杆杆臂

窗口增大时将会增加解算规模,加重非线性优化迭代的负担。由表2可得,随滑动窗口变大,程序运行时间增加,结合表1可得,在窗口大小为20 s时,三维位置误差均优于其他方向或呈基本相当的状态。因此,结合定位精度与计算效率,本文认为在窗口大小为20 s时,FGO的导航定位结果最优。下文均以窗口大小20 s的情况开展相关实验。

其次,分析精化预积分对因子图定位性能的影响。精化预积分主要考虑地球自转对陀螺的影响,通过表1数据可得,实验所采用三组惯导元件陀螺仪的零偏稳定性均小于地球自转角速率数值为15.041 067 178 6 °/h,即在其工作过程中均会受到地球自转的影响,且当陀螺的零偏稳定性越高时,越容易受到其干扰。表4为地球自转项改正前后松组合位置误差,位置误差曲线如图5所示。下文图表中将SPAN-ISA-100C简称为100C,将MP-M39简称为M39。

表4 地球自转项改正前后松组合位置误差 m

图5 地球自转项改正前后松组合位置误差曲线

由表4分析,SPAN-ISA-100C、POS320使用精化预积分时组合导航定位精度在北、东、地三个方向上分别提升了56.13%、93.7%、39.7%和16.5%、68.9%、42.3%,但对于MP-M39,由于其陀螺仪的零偏稳定性较大,地球自转的影响有限,在北方向、东方向分别提升了8%、7.9%,地方向持相当状态。由此可见,对于导航级惯导,地球自转的影响不可忽略,对于战术级惯导,精化预积分对GNSS/INS的定位精度也有一定程度的提升。

3.2 FGO与EKF定位性能比较

通过3.1实验分析可得,在窗口大小为20 s时,基于精化预积分的因子图GNSS/INS松组合定位结果最优。在组合导航领域,EKF形式简洁、应用广泛,是一种成熟的用来估计不确定量的方法。将FGO与EKF进行对比,进一步验证FGO的可行性与有效性。表5为三组惯导在FGO和EKF下松组合的均方根位置误差,位置误差曲线如图6所示。

表5 FGO和EKF松组合位置误差 m

图6 GNSS/INS FGO和EKF松组合位置误差曲线

由图6可得,FGO相较于EKF的误差曲线更加平滑,由表5分析三组惯导松组合时的RMSE值可得,FGO相对于EKF,SPAN-ISA-100C、POS320、MP-M39的定位精度在北、东、地三个方向上分别提升了53.03%、52.35%、64.59%,60.14%、22.62%、50.06%,22.2%、40%、67.16%。

针对于图5(a)与图6(a)出现的误差偏移现象,结合图5(b)与图6(b)、图5(c)与图6(c)可得,在5 600s左右高程位置均发生了抖动,分析是因为当时车载路况原因,导致平台发生了较大抖动,一方面由于光纤级惯导SPAN-ISA-100C精度高,对细微变化感受敏感,一方面由于SPAN-ISA-100C与平台发生微小松动,导致高程方向发生位置误差偏移。

为了更好验证两种算法的适用性,本文选取一段数据,每间隔120 s,设置30 s、60 s一次的GNSS中断,共计13次,表6为三组惯导松组合时在三维位置上的最大误差,表7为中断时段姿态三维均方根误差,图7为三组惯导松组合GNSS中断60s时在三维位置上的最大误差曲线图,图8为GNSS中断60 s时姿态误差曲线图。

表6 松组合时EKF、FGO三维位置最大误差 m

表7 松组合时EKF、FGO姿态误差 (°)

图7 GNSS中断60 s时EKF和FGO位置误差曲线

图8 GNSS中断60 s时EKF和FGO姿态误差曲线

由式(5)分析可得,图优化算法求取的是全局最优解,因此在发生GNSS信号中断时,优化区域通过与前一时段GNSS信号正常时的定位信息做全局最优化,即图优化算法在GNSS信号恢复后,对GNSS信号失锁时间段内的状态进行再优化,形成一个整体的最优结果,可以极大提高GNSS发生信号失锁时导航定位精度,与EKF状态递推方法相比可以更有效地减小组合导航位置发散,其次图优化算法采用非线性优化方法,优化时进行了多次迭代,与EKF只进行一阶泰勒展开相比,多次迭代也可以有效减小误差。如图7、图8所示,当发生GNSS中断时,FGO的定位效果要明显优于EKF,位置误差发散慢,姿态误差也更加稳定。三组实验位置最大误差如表6所示,分析可得,对于导航级惯导,使用FGO算法时,SPAN-ISA-100C和POS320在GNSS中断30 s时最大误差均在1.3 m以内,中断60 s时最大误差均在4.7 m以内,使用EKF算法时,SPAN-ISA-100C和POS320在GNSS中断30 s时最大误差达到7.11 m,中断60 s时最大误差达到40.5 m,地方向误差发散较慢,但FGO的定位效果也优于EKF;对于战术级惯导,由于其本身陀螺仪的零偏稳定性较差,当发生GNSS信号中断时,位置误差发散快,GNSS中断时段较快的误差发散会影响优化区间整体的精度,因此MP-M39在与GNSS松组合时定位误差较大,但在GNSS中断30 s和60 s时,FGO的定位效果也均优于EKF。