基于因子图优化PPP 的GNSS/INS 松组合导航

杨显赐，乔书波，肖国锐，贾晓雪，彭华东，李松伟

(信息工程大学地理空间信息学院,郑州 450001)

0 引言

全球导航卫星系统(GNSS)长期定位精度高、误差稳定不随时间累计，在室外开阔地带可以提供连续实时的高精度定位信息，但在室内、城市峡谷、丛林、洞穴等场景，极易受环境遮挡引起非视距误差以及多路径效应等影响发生信号失锁，严重时导致无法获取定位信息.惯性导航系统(INS)具有很好的抗干扰能力、短时精度高、隐蔽性强、自主性高，但误差累计快、长时间导航定位精度会迅速下降.因此，GNSS 和INS 之间具有很好的互补性，GNSS/INS 组合导航系统融合了两个系统的优点，可以在复杂环境下持续稳定输出平滑、高采样率、高精度的定位结果，已经被广泛应用在军事、测绘、智能交通、地质勘探等领域[1-2].

现在，在GNSS/INS 组合导航算法研究中，大多数以扩展卡尔曼滤波(EKF)算法为主.文献[3]研究表明，在城市复杂环境下，自适应滤波算法的定位精度相较于常规卡尔曼滤波提升30%.文献[4]重点研究了GNSS/INS 组合导航定位的滤波解算及系统的可靠性.但滤波方法服从一阶马尔可夫假设，增量式递推过程会导致在数据解算过程中无法充分利用历史信息.因子图优化(FGO)算法作为一种批量式数据处理过程，将一个时间段内的数据进行全局优化，并且当GNSS 信号失锁时，可以在GNSS 信号恢复后对失锁区间进行再处理.因此相较于EKF 算法，可以得到更可靠的导航定位结果[5].文献[6]面向城市复杂环境，基于FGO 算法进行组合导航定位，研究表明在GNSS 拒止情况下，相较于卡尔曼滤波算法可以将定位精度提高30%以上.文献[7]将FGO 与EKF相结合，结果表明FGO-EKF 算法具有更好的全局稳定性和航向精准度.文献[8]通过模拟数据和实际数据对模型进行验证表明，相比于卡尔曼滤波算法，FGO 模型有效降低了列车的位置误差和速度误差，且未出现误差发散现象.文献[9]在因子图优化过程中设置不同大小的滑动窗口，实验结果表明，滑动窗口的大小会影响FGO 信息融合性能.文献[10]在惯性测量单元(IMU)预积分过程中考虑地球自转的影响，实验证明即使对于消费级微机电系统(MEMS)模块，改进的IMU 预积分也可以有效提高松组合的定位性能.但上述方法均未将基于FGO 的精密单点定位(PPP)算法应用于GNSS/INS 组合导航.

针对卫星可见数不足、多复杂环境下GNSS 信号容易发生失锁的问题，本文提出了一种基于FGO的PPP 算法，进行GNSS/INS 组合导航.首先设置伪距、载波因子，搭建基于FGO 的PPP 框架，保证GNSS定位信息的可靠性；再进一步将解算出的PPP 位置信息设置为PPP 因子，和兼顾了地球自转的精化预积分因子一起搭建FGO PPP/INS 组合导航数据融合算法框架；最后通过车载实测实验进行算法验证，并和经典的EKF 算法进行对比实验，比较分析两种算法的数据融合定位效果.

1 FGO 算法原理

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

式中：f(X) 为作用于全体变量的函数为与变量节点相连的因子函数的乘积，通常将每一个因子fi(xi)表 示成一个误差函数即fi(xi)=d(erri(xi,zi))，其中d(·) 为 相应的代价函数，zi为实际量测值.

随机系统状态空间模型由运动方程xi和观测方程zi构成：

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

式中：f(x0) 为先验信息构造的误差函数，通常表示为观测值与均值之差，通过非线性优化使式(5)经过一系列的线性化不断逼近最小值，观测函数hi(xi) 通过泰勒展开公式得到，即HiΔi，雅可比矩阵Hi为线性化点处关于观测函数的偏微分.

2 PPP FGO 模型

PPP 技术一般采用单台GNSS 接收机，利用国际GNSS 服务(IGS)等提供的精密星历和卫星钟差产品，综合考虑各项误差模型的精确改正，基于伪距和载波相位观测值进行非差定位解算，开展大范围、高精度的动态导航定位测量.

2.1 PPP 数学模型

GNSS 定位的观测量主要为载波相位和伪距两种，基于原始观测值建立非差非组合PPP 观测方程：

式中：Pi、Li分别为第i个频率的伪距和载波相位观测值；ρ 为卫星至接收机之间的几何距离为第一个频率与第i个频率平方的比值；I1为第一个频率站星视线方向的电离层延迟；dT为倾斜对流层延迟；dtr和dts分别为接收机钟差和卫星钟差；Dr和Ds分别为第i个频率时接收机端和卫星端相位硬件延迟；λi和Ni分别为第i个频率载波相位波长和模糊度参数；和分别为伪距和载波相位的测量误差；Δ表示其他误差项改正，包括天线相位中心偏移和变化、相位缠绕、相对论效应、潮汐改正、地球自转等，这些误差项均可通过已有模型进行精确改正[13].本文假定上述误差项已在观测值上进行了改正.

双频无电离层模型是PPP 中最常见的函数模型，通过形成无电离层组合(IF)观测值，消除掉伪距和载波相位测量中的一阶电离层延迟，其模型的简化形式如下[14]：

式中：待估参数为Xs,Ys,Zs接收机三维位置坐标、接收机钟差、天顶对流层湿延迟和无电离层组合模糊度参数.

假设卫星坐标为 (Xs,Ys,Zs)，在测站近似坐标(X0,Y0,Z0)点进行线性化，得到误差方程：

2.2 PPP FGO 模型

双频无电离层模型中，基于伪距和载波的观测方程均为非线性的，经典的滤波算法通过泰勒级数展开近似线性化，在保留一阶项忽略高阶项的过程中会受到非线性误差的影响，因此我们引入SLAM 领域中广泛应用的FGO 算法，在近似点线性化时通过非线性优化的方法，通过多次迭代逼近真值，可以极大减少线性化过程中产生的非线性误差.

根据FGO 理论，定义接收机的位置 [x,y,z]、接收机钟差 dtr、天顶对流层湿延迟 dT、模糊度NIF为状态变量，伪距和载波观测信息为因子节点，状态变量和因子节点之间由对应的误差函数相连接.构建PPP FGO 框架如图1 所示.

图1 PPP FGO 框架

图1 中，双边因子表示前后历元间约束.状态变量为x=[x,y,z,dtr,dT] 和N，假设观测值均为相互独立且服从零均值的高斯正态分布.在未发生周跳的情况下，模糊度参数是不发生变化的，所以我们将状态变量模糊度单独列出.则由第一节FGO 算法原理可得，状态变量的最大后验概率估计为

3 基于FGO 的PPP GNSS/INS 组合导航

GNSS/INS 组合导航定位模型由第2 节PPP FGO 算法提供优化的位置信息、IMU 提供加速度和角速度信息.PPP 位置信息位于地心地固坐标系中，IMU 量测信息位于载体坐标系下，通过统一的坐标转换在“北-东-地(north-east-down)”导航坐标系下进行数据融合计算.

3.1 IMU 预积分因子

在经典的捷联INS 力学编排算法中，INS 的位置、速度、姿态更新是逐时刻遍历的，但IMU 的数据输出频率为200 Hz/s，要远大于GNSS 数据输出的1 Hz，因此为了消减冗余计算量，提高计算效率，引入FGO 算法的IMU 预积分技术.考虑到当IMU 中陀螺仪的零偏稳定性小于地球自转角速率时，此时传统IMU 预积分中忽略地球自转角速度，将影响IMU的解算精度，因此通过添加地球自转误差项，优化IMU 预积分.

3.2 PPP 因子

由2.2 节构建的PPP FGO 模型优化解算，得到GNSS 接收机天线相位中心(APC)高精度的三维位置坐标，设置GNSS 观测值zGNSS(x)=pGNSS，同时得到相关协方差矩阵由IMU 精化预积分状态递推可得位置估计值由此构建PPP 因子：

3.3 GNSS/INS FGO 模型

当接收到GNSS 和INS 的量测信息时，定义因子节点并更新FGO 框架，进行状态变量更新.GNSS/INS 组合导航FGO 框架如图2 所示，由各个时刻GNSS 的伪距、载波相位观测值计算PPP 因子，通过连接边与对应的状态变量相关联，由IMU 测量值构建IMU 精化预积分因子连接前后两时刻的状态变量.

图2 GNSS/INS 松 组合FGO 框架

考虑到PPP 直接定位产品只有三维位置，并且在只考虑位置时可以正常进行组合导航，因此我们在构建PPP 因子时不考虑速度.

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

式中：WZ为优化窗口内预积分因子个数；M为优化窗口内PPP 因子个数；当GNSS 未发生中断时，WZ=M.

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

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

为了验证本文所述基于FGO 的GNSS/INS 组合导航算法在实际工程应用中的有效性和可靠性，在郑州市设计一组车载实验.跑车测试平台如图3 所示，序号1 表示GNSS 接收机天线，INS 元件和GNSS 接收机集成在设备2 中，跑车测试轨迹如图4 所示，其中，绿色地标为起点，红色地标为终点.本实验使用非线性优化方法对PPP 和组合导航数据进行处理对PPP 和组合导航数据进行非线性优化处理.滑动窗口大小为20 s，当窗口中待估参数达到窗口大小上限时，窗口向前推进，对窗口外的历史数据采用边缘化操作，构建到先验因子中[6].由霍尼韦尔的高性能的微机电系统HGuidei300 提供IMU 数据，采样频率为200 Hz，其标称参数如表1 所示.GNSS 数据为本文自行解算的高精度PPP 定位结果，数据更新频率为1 Hz，IMU 预积分的频率为1 Hz，数据处理设备为ThinkPad P15v，FGO 算法的实验环境为ubuntu16.04，编程语言为C++，EKF 的实验环境为Window10，编程语言为MATLAB.

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

图3 跑车测试平台

图4 跑车测试轨迹

4.1 PPP 定位性能分析

为了验证FGO 算法对于PPP 解算的有效性，通过车载实测实验对该PPP 算法的定位精度进行评估.实验采集了2022 年7 月6 日约1.6 h 的GPS 卫星动态观测数据，数据处理采用IGS GBM 分析中心的精密星历和钟差产品，卫星截止高度角选为10°，接收机类型为Septentrio，对比实验采用目前成熟的RTKLIB 程序[17]进行PPP 动态定位解算，动态定位运动轨迹“真值”基于NovAtel 公司开发的后处理软件Inertial Explorer，利用紧组合模式计算得到，其精度为厘米级.在进行FGO 解算时，先通过RTKLIB将相关误差项进行精确改正，再传递到模型中对状态变量进行非线性优化求解.图5 给出了基于FGO 算法、EFK 算法进行PPP 动态定位后，N、E、D 方向三维坐标分量的FGO 估值、EFK 估值与参考坐标值之间的差异.可以看出，基于FGO 估计时，模糊度收敛之后的数据将会同步带入未收敛前，进行全局优化，因此曲线整体相对平滑.进一步结合表2 中PPP 定位的三维位置均方根误差(RMSE)可以初步得到如下结论：使用EFK 算法时，PPP 定位具有一定的收敛时间；使用FGO 算法时，由于其全局优化的功能，可以很好地将过去时间段和现在时刻的定位信息进行统一优化，因此定位结果在初始时刻就实现了收敛.FGP 算法的批处理模式相当于双向滤波平滑器，但是不同的是，FGO 算法通过采用滑动窗口和边缘化策略，并且利用了雅可比矩阵的稀疏性，与传统批量平滑相比估计效率提升了35 倍，显著提高了导航系统的实时性[18-19].除此之外，FGO 算法即插即用的特点，可有效处理组合导航等多源异类异步信息[20].特别当应用在SLAM 中时，FGO 算法可通过回环检测实现载体运动的重新定位，保证系统长时间运行、全局一致性轨迹和地图构建[21].分析三维位置的RMSE值，相较于EFK 算法，FGO 算法在N、E、D 方向上的定位精度分别提升了37.09%、28.79%、64.59%.

表2 基于EKF 和FGO 的PPP 的三维位置RMSE m

图5 基于EKF 和FGO 的PPP 三维位 置RMSE

4.2 GNSS/INS 定位性能分析

将基于FGO 的PPP 定位结果设置为GNSS PPP 因子，构建到GNSS/INS 松组合导航定位FGO框架中.为了验证FGO 算法对GNSS/INS 松组合导航信息融合的有效性，采用目前应用成熟的基于EFK 算法的组合导航作为对比实验，实验基于严恭敏[22]开发的PSINS 工具箱进行.考虑到实验的充分性，设置4 组实验进行对比分析，实验一(EKF PPPEKF)使用RTKLIB 处理的PPP 位置信息与INS 进行基于EKF 的组合导航，实验二(FGO PPP-EKF)使用RTKLIB 处理的PPP 位置信息与INS 进行基于FGO 的组合导航，实验三(EKF PPP-FGO)使用FGO处理的PPP 位置信息与INS 进行基于EKF 的组合导航，实验四(FGO PPP-FGO)使用FGO 处理的PPP位置信息与INS 进行基于FGO 的组合导航，通过充分分析不同精度的PPP 定位结果在两种算法下与惯导融合的定位性能，评价两种算法的优劣.动态定位运动轨迹“真值”同4.1 节得到.表3 为实测数据在4 组实验下组松合导航的三维位置RMSE，位置误差曲线如图6 所示.

表3 GNSS/INS 三维位置RMSE m

图6 GNSS/INS 松组合三维位置RMSE

图6 和表3 实验结果表明，高精度PPP 定位结果可以极大地提高GNSS/INS 松组合导航的定位性能.对实验一、三和实验二、四进行分组对比，可以看出，当PPP 定位精度相同时，EKF 的马尔可夫理论导致载体运动状态的传递只能是向前推进的，但FGO算法可以在接收到后一阶段的PPP 定位信息时，对整个时间段内的状态进行再优化，形成一个整体的最优结果，可以极大提高组合导航的定位精度.结合图6 分析FGO 算法相较于EFK 的误差曲线更加平滑，定位精度更高.对实验一和实验四进行对比分析，FGO 算法相较于EFK 算法，可以极大地提高组合导航的定位精度，为松组合提供更优的PPP 定位结果的同时，实现了信息融合的更优化.结合表3，相较于EFK 算法，基于FGO 的组合导航在N、E、D 方向上定位精度分别提升了49.08%、41.22%、71.86%.

5 结束语

本文针对卫星可见数不足、多复杂环境下GNSS信号容易发生失锁的问题，提出了一种基于FGO 的PPP 算法，进行GNSS/INS 松组合导航，极大地提高了组合导航定位精度.实验结果表明：

1)在进行PPP 时，FGO 算法由于其全局优化的特性，可以在模糊度收敛后对收敛前时间段的数据进行再优化，使PPP 定位结果在初始时刻就体现出模糊度收敛后的定位精度，极大提高了PPP 的可靠性和有效性.

2)本文提出的基于FGO 的PPP 算法，可以为GNSS/INS 松组合提供可靠的导航定位结果，相较于EKF 算法，在N、E、D 方向上的定位精度分别提升了49.08%、41.22%、71.86%.