基于因子圖優(yōu)化PPP 的GNSS/INS 松組合導(dǎo)航

楊顯賜，喬書波，肖國銳，賈曉雪，彭華東，李松偉

(信息工程大學(xué)地理空間信息學(xué)院,鄭州 450001)

0 引言

全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)長期定位精度高、誤差穩(wěn)定不隨時間累計，在室外開闊地帶可以提供連續(xù)實時的高精度定位信息，但在室內(nèi)、城市峽谷、叢林、洞穴等場景，極易受環(huán)境遮擋引起非視距誤差以及多路徑效應(yīng)等影響發(fā)生信號失鎖，嚴(yán)重時導(dǎo)致無法獲取定位信息.慣性導(dǎo)航系統(tǒng)(INS)具有很好的抗干擾能力、短時精度高、隱蔽性強、自主性高，但誤差累計快、長時間導(dǎo)航定位精度會迅速下降.因此，GNSS 和INS 之間具有很好的互補性，GNSS/INS 組合導(dǎo)航系統(tǒng)融合了兩個系統(tǒng)的優(yōu)點，可以在復(fù)雜環(huán)境下持續(xù)穩(wěn)定輸出平滑、高采樣率、高精度的定位結(jié)果，已經(jīng)被廣泛應(yīng)用在軍事、測繪、智能交通、地質(zhì)勘探等領(lǐng)域[1-2].

現(xiàn)在，在GNSS/INS 組合導(dǎo)航算法研究中，大多數(shù)以擴展卡爾曼濾波(EKF)算法為主.文獻[3]研究表明，在城市復(fù)雜環(huán)境下，自適應(yīng)濾波算法的定位精度相較于常規(guī)卡爾曼濾波提升30%.文獻[4]重點研究了GNSS/INS 組合導(dǎo)航定位的濾波解算及系統(tǒng)的可靠性.但濾波方法服從一階馬爾可夫假設(shè)，增量式遞推過程會導(dǎo)致在數(shù)據(jù)解算過程中無法充分利用歷史信息.因子圖優(yōu)化(FGO)算法作為一種批量式數(shù)據(jù)處理過程，將一個時間段內(nèi)的數(shù)據(jù)進行全局優(yōu)化，并且當(dāng)GNSS 信號失鎖時，可以在GNSS 信號恢復(fù)后對失鎖區(qū)間進行再處理.因此相較于EKF 算法，可以得到更可靠的導(dǎo)航定位結(jié)果[5].文獻[6]面向城市復(fù)雜環(huán)境，基于FGO 算法進行組合導(dǎo)航定位，研究表明在GNSS 拒止情況下，相較于卡爾曼濾波算法可以將定位精度提高30%以上.文獻[7]將FGO 與EKF相結(jié)合，結(jié)果表明FGO-EKF 算法具有更好的全局穩(wěn)定性和航向精準(zhǔn)度.文獻[8]通過模擬數(shù)據(jù)和實際數(shù)據(jù)對模型進行驗證表明，相比于卡爾曼濾波算法，F(xiàn)GO 模型有效降低了列車的位置誤差和速度誤差，且未出現(xiàn)誤差發(fā)散現(xiàn)象.文獻[9]在因子圖優(yōu)化過程中設(shè)置不同大小的滑動窗口，實驗結(jié)果表明，滑動窗口的大小會影響FGO 信息融合性能.文獻[10]在慣性測量單元(IMU)預(yù)積分過程中考慮地球自轉(zhuǎn)的影響，實驗證明即使對于消費級微機電系統(tǒng)(MEMS)模塊，改進的IMU 預(yù)積分也可以有效提高松組合的定位性能.但上述方法均未將基于FGO 的精密單點定位(PPP)算法應(yīng)用于GNSS/INS 組合導(dǎo)航.

針對衛(wèi)星可見數(shù)不足、多復(fù)雜環(huán)境下GNSS 信號容易發(fā)生失鎖的問題，本文提出了一種基于FGO的PPP 算法，進行GNSS/INS 組合導(dǎo)航.首先設(shè)置偽距、載波因子，搭建基于FGO 的PPP 框架，保證GNSS定位信息的可靠性；再進一步將解算出的PPP 位置信息設(shè)置為PPP 因子，和兼顧了地球自轉(zhuǎn)的精化預(yù)積分因子一起搭建FGO PPP/INS 組合導(dǎo)航數(shù)據(jù)融合算法框架；最后通過車載實測實驗進行算法驗證，并和經(jīng)典的EKF 算法進行對比實驗，比較分析兩種算法的數(shù)據(jù)融合定位效果.

1 FGO 算法原理

因子圖是用來表達(dá)隨機變量聯(lián)合概率分布的二分圖模型H=(F,X,E)[11-12]，由因子節(jié)點f∈F和變量節(jié)點x∈X組成，邊e∈E為存在于因子節(jié)點與變量節(jié)點之間的一條連接邊.在貝葉斯網(wǎng)絡(luò)中，所有變量聯(lián)合的概率密度是由每個節(jié)點關(guān)聯(lián)的條件概率密度相乘得到，類推到因子圖，將聯(lián)合概率密度表示為一系列因子的乘積，即：

式中：f(X) 為作用于全體變量的函數(shù)為與變量節(jié)點相連的因子函數(shù)的乘積，通常將每一個因子fi(xi)表 示成一個誤差函數(shù)即fi(xi)=d(erri(xi,zi))，其中d(·) 為 相應(yīng)的代價函數(shù)，zi為實際量測值.

隨機系統(tǒng)狀態(tài)空間模型由運動方程xi和觀測方程zi構(gòu)成：

根據(jù)最大后驗概率估計，由量測信息計算狀態(tài)變量最優(yōu)值，將式(4)轉(zhuǎn)化為一個等價的最小二乘優(yōu)化問題，即：

式中：f(x0) 為先驗信息構(gòu)造的誤差函數(shù)，通常表示為觀測值與均值之差，通過非線性優(yōu)化使式(5)經(jīng)過一系列的線性化不斷逼近最小值，觀測函數(shù)hi(xi) 通過泰勒展開公式得到，即HiΔi，雅可比矩陣Hi為線性化點處關(guān)于觀測函數(shù)的偏微分.

2 PPP FGO 模型

PPP 技術(shù)一般采用單臺GNSS 接收機，利用國際GNSS 服務(wù)(IGS)等提供的精密星歷和衛(wèi)星鐘差產(chǎn)品，綜合考慮各項誤差模型的精確改正，基于偽距和載波相位觀測值進行非差定位解算，開展大范圍、高精度的動態(tài)導(dǎo)航定位測量.

2.1 PPP 數(shù)學(xué)模型

GNSS 定位的觀測量主要為載波相位和偽距兩種，基于原始觀測值建立非差非組合PPP 觀測方程：

式中：Pi、Li分別為第i個頻率的偽距和載波相位觀測值；ρ 為衛(wèi)星至接收機之間的幾何距離為第一個頻率與第i個頻率平方的比值；I1為第一個頻率站星視線方向的電離層延遲；dT為傾斜對流層延遲；dtr和dts分別為接收機鐘差和衛(wèi)星鐘差；Dr和Ds分別為第i個頻率時接收機端和衛(wèi)星端相位硬件延遲；λi和Ni分別為第i個頻率載波相位波長和模糊度參數(shù)；和分別為偽距和載波相位的測量誤差；Δ表示其他誤差項改正，包括天線相位中心偏移和變化、相位纏繞、相對論效應(yīng)、潮汐改正、地球自轉(zhuǎn)等，這些誤差項均可通過已有模型進行精確改正[13].本文假定上述誤差項已在觀測值上進行了改正.

雙頻無電離層模型是PPP 中最常見的函數(shù)模型，通過形成無電離層組合(IF)觀測值，消除掉偽距和載波相位測量中的一階電離層延遲，其模型的簡化形式如下[14]：

式中：待估參數(shù)為Xs,Ys,Zs接收機三維位置坐標(biāo)、接收機鐘差、天頂對流層濕延遲和無電離層組合模糊度參數(shù).

假設(shè)衛(wèi)星坐標(biāo)為 (Xs,Ys,Zs)，在測站近似坐標(biāo)(X0,Y0,Z0)點進行線性化，得到誤差方程：

2.2 PPP FGO 模型

雙頻無電離層模型中，基于偽距和載波的觀測方程均為非線性的，經(jīng)典的濾波算法通過泰勒級數(shù)展開近似線性化，在保留一階項忽略高階項的過程中會受到非線性誤差的影響，因此我們引入SLAM 領(lǐng)域中廣泛應(yīng)用的FGO 算法，在近似點線性化時通過非線性優(yōu)化的方法，通過多次迭代逼近真值，可以極大減少線性化過程中產(chǎn)生的非線性誤差.

根據(jù)FGO 理論，定義接收機的位置 [x,y,z]、接收機鐘差 dtr、天頂對流層濕延遲 dT、模糊度NIF為狀態(tài)變量，偽距和載波觀測信息為因子節(jié)點，狀態(tài)變量和因子節(jié)點之間由對應(yīng)的誤差函數(shù)相連接.構(gòu)建PPP FGO 框架如圖1 所示.

圖1 PPP FGO 框架

圖1 中，雙邊因子表示前后歷元間約束.狀態(tài)變量為x=[x,y,z,dtr,dT] 和N，假設(shè)觀測值均為相互獨立且服從零均值的高斯正態(tài)分布.在未發(fā)生周跳的情況下，模糊度參數(shù)是不發(fā)生變化的，所以我們將狀態(tài)變量模糊度單獨列出.則由第一節(jié)FGO 算法原理可得，狀態(tài)變量的最大后驗概率估計為

3 基于FGO 的PPP GNSS/INS 組合導(dǎo)航

GNSS/INS 組合導(dǎo)航定位模型由第2 節(jié)PPP FGO 算法提供優(yōu)化的位置信息、IMU 提供加速度和角速度信息.PPP 位置信息位于地心地固坐標(biāo)系中，IMU 量測信息位于載體坐標(biāo)系下，通過統(tǒng)一的坐標(biāo)轉(zhuǎn)換在“北-東-地(north-east-down)”導(dǎo)航坐標(biāo)系下進行數(shù)據(jù)融合計算.

3.1 IMU 預(yù)積分因子

在經(jīng)典的捷聯(lián)INS 力學(xué)編排算法中，INS 的位置、速度、姿態(tài)更新是逐時刻遍歷的，但IMU 的數(shù)據(jù)輸出頻率為200 Hz/s，要遠(yuǎn)大于GNSS 數(shù)據(jù)輸出的1 Hz，因此為了消減冗余計算量，提高計算效率，引入FGO 算法的IMU 預(yù)積分技術(shù).考慮到當(dāng)IMU 中陀螺儀的零偏穩(wěn)定性小于地球自轉(zhuǎn)角速率時，此時傳統(tǒng)IMU 預(yù)積分中忽略地球自轉(zhuǎn)角速度，將影響IMU的解算精度，因此通過添加地球自轉(zhuǎn)誤差項，優(yōu)化IMU 預(yù)積分.

3.2 PPP 因子

由2.2 節(jié)構(gòu)建的PPP FGO 模型優(yōu)化解算，得到GNSS 接收機天線相位中心(APC)高精度的三維位置坐標(biāo)，設(shè)置GNSS 觀測值zGNSS(x)=pGNSS，同時得到相關(guān)協(xié)方差矩陣由IMU 精化預(yù)積分狀態(tài)遞推可得位置估計值由此構(gòu)建PPP 因子：

3.3 GNSS/INS FGO 模型

當(dāng)接收到GNSS 和INS 的量測信息時，定義因子節(jié)點并更新FGO 框架，進行狀態(tài)變量更新.GNSS/INS 組合導(dǎo)航FGO 框架如圖2 所示，由各個時刻GNSS 的偽距、載波相位觀測值計算PPP 因子，通過連接邊與對應(yīng)的狀態(tài)變量相關(guān)聯(lián)，由IMU 測量值構(gòu)建IMU 精化預(yù)積分因子連接前后兩時刻的狀態(tài)變量.

圖2 GNSS/INS 松 組合FGO 框架

考慮到PPP 直接定位產(chǎn)品只有三維位置，并且在只考慮位置時可以正常進行組合導(dǎo)航，因此我們在構(gòu)建PPP 因子時不考慮速度.

綜上，得到狀態(tài)變量的最大后驗概率估計：

式中：WZ為優(yōu)化窗口內(nèi)預(yù)積分因子個數(shù)；M為優(yōu)化窗口內(nèi)PPP 因子個數(shù)；當(dāng)GNSS 未發(fā)生中斷時，WZ=M.

當(dāng)使用非線性優(yōu)化方法批量處理數(shù)據(jù)時，觀測值以一個時間序列到達(dá)，如果每次都進行完整的批量優(yōu)化會導(dǎo)致計算效率低，無法保證計算的實時性，通過使用增量非線性最小二乘估計算法iSAM2[16]，當(dāng)新的因子添加進來時，只優(yōu)化被新的因子影響的那一部分，從而提高了解算效率.

4 車載實測實驗與性能分析

為了驗證本文所述基于FGO 的GNSS/INS 組合導(dǎo)航算法在實際工程應(yīng)用中的有效性和可靠性，在鄭州市設(shè)計一組車載實驗.跑車測試平臺如圖3 所示，序號1 表示GNSS 接收機天線，INS 元件和GNSS 接收機集成在設(shè)備2 中，跑車測試軌跡如圖4 所示，其中，綠色地標(biāo)為起點，紅色地標(biāo)為終點.本實驗使用非線性優(yōu)化方法對PPP 和組合導(dǎo)航數(shù)據(jù)進行處理對PPP 和組合導(dǎo)航數(shù)據(jù)進行非線性優(yōu)化處理.滑動窗口大小為20 s，當(dāng)窗口中待估參數(shù)達(dá)到窗口大小上限時，窗口向前推進，對窗口外的歷史數(shù)據(jù)采用邊緣化操作，構(gòu)建到先驗因子中[6].由霍尼韋爾的高性能的微機電系統(tǒng)HGuidei300 提供IMU 數(shù)據(jù)，采樣頻率為200 Hz，其標(biāo)稱參數(shù)如表1 所示.GNSS 數(shù)據(jù)為本文自行解算的高精度PPP 定位結(jié)果，數(shù)據(jù)更新頻率為1 Hz，IMU 預(yù)積分的頻率為1 Hz，數(shù)據(jù)處理設(shè)備為ThinkPad P15v，F(xiàn)GO 算法的實驗環(huán)境為ubuntu16.04，編程語言為C++，EKF 的實驗環(huán)境為Window10，編程語言為MATLAB.

表1 慣性導(dǎo)航元件標(biāo)定參數(shù)及空間杠桿桿臂

圖3 跑車測試平臺

圖4 跑車測試軌跡

4.1 PPP 定位性能分析

為了驗證FGO 算法對于PPP 解算的有效性，通過車載實測實驗對該PPP 算法的定位精度進行評估.實驗采集了2022 年7 月6 日約1.6 h 的GPS 衛(wèi)星動態(tài)觀測數(shù)據(jù)，數(shù)據(jù)處理采用IGS GBM 分析中心的精密星歷和鐘差產(chǎn)品，衛(wèi)星截止高度角選為10°，接收機類型為Septentrio，對比實驗采用目前成熟的RTKLIB 程序[17]進行PPP 動態(tài)定位解算，動態(tài)定位運動軌跡“真值”基于NovAtel 公司開發(fā)的后處理軟件Inertial Explorer，利用緊組合模式計算得到，其精度為厘米級.在進行FGO 解算時，先通過RTKLIB將相關(guān)誤差項進行精確改正，再傳遞到模型中對狀態(tài)變量進行非線性優(yōu)化求解.圖5 給出了基于FGO 算法、EFK 算法進行PPP 動態(tài)定位后，N、E、D 方向三維坐標(biāo)分量的FGO 估值、EFK 估值與參考坐標(biāo)值之間的差異.可以看出，基于FGO 估計時，模糊度收斂之后的數(shù)據(jù)將會同步帶入未收斂前，進行全局優(yōu)化，因此曲線整體相對平滑.進一步結(jié)合表2 中PPP 定位的三維位置均方根誤差(RMSE)可以初步得到如下結(jié)論：使用EFK 算法時，PPP 定位具有一定的收斂時間；使用FGO 算法時，由于其全局優(yōu)化的功能，可以很好地將過去時間段和現(xiàn)在時刻的定位信息進行統(tǒng)一優(yōu)化，因此定位結(jié)果在初始時刻就實現(xiàn)了收斂.FGP 算法的批處理模式相當(dāng)于雙向濾波平滑器，但是不同的是，F(xiàn)GO 算法通過采用滑動窗口和邊緣化策略，并且利用了雅可比矩陣的稀疏性，與傳統(tǒng)批量平滑相比估計效率提升了35 倍，顯著提高了導(dǎo)航系統(tǒng)的實時性[18-19].除此之外，F(xiàn)GO 算法即插即用的特點，可有效處理組合導(dǎo)航等多源異類異步信息[20].特別當(dāng)應(yīng)用在SLAM 中時，F(xiàn)GO 算法可通過回環(huán)檢測實現(xiàn)載體運動的重新定位，保證系統(tǒng)長時間運行、全局一致性軌跡和地圖構(gòu)建[21].分析三維位置的RMSE值，相較于EFK 算法，F(xiàn)GO 算法在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 定位結(jié)果設(shè)置為GNSS PPP 因子，構(gòu)建到GNSS/INS 松組合導(dǎo)航定位FGO框架中.為了驗證FGO 算法對GNSS/INS 松組合導(dǎo)航信息融合的有效性，采用目前應(yīng)用成熟的基于EFK 算法的組合導(dǎo)航作為對比實驗，實驗基于嚴(yán)恭敏[22]開發(fā)的PSINS 工具箱進行.考慮到實驗的充分性，設(shè)置4 組實驗進行對比分析，實驗一(EKF PPPEKF)使用RTKLIB 處理的PPP 位置信息與INS 進行基于EKF 的組合導(dǎo)航，實驗二(FGO PPP-EKF)使用RTKLIB 處理的PPP 位置信息與INS 進行基于FGO 的組合導(dǎo)航，實驗三(EKF PPP-FGO)使用FGO處理的PPP 位置信息與INS 進行基于EKF 的組合導(dǎo)航，實驗四(FGO PPP-FGO)使用FGO 處理的PPP位置信息與INS 進行基于FGO 的組合導(dǎo)航，通過充分分析不同精度的PPP 定位結(jié)果在兩種算法下與慣導(dǎo)融合的定位性能，評價兩種算法的優(yōu)劣.動態(tài)定位運動軌跡“真值”同4.1 節(jié)得到.表3 為實測數(shù)據(jù)在4 組實驗下組松合導(dǎo)航的三維位置RMSE，位置誤差曲線如圖6 所示.

表3 GNSS/INS 三維位置RMSE m

圖6 GNSS/INS 松組合三維位置RMSE

圖6 和表3 實驗結(jié)果表明，高精度PPP 定位結(jié)果可以極大地提高GNSS/INS 松組合導(dǎo)航的定位性能.對實驗一、三和實驗二、四進行分組對比，可以看出，當(dāng)PPP 定位精度相同時，EKF 的馬爾可夫理論導(dǎo)致載體運動狀態(tài)的傳遞只能是向前推進的，但FGO算法可以在接收到后一階段的PPP 定位信息時，對整個時間段內(nèi)的狀態(tài)進行再優(yōu)化，形成一個整體的最優(yōu)結(jié)果，可以極大提高組合導(dǎo)航的定位精度.結(jié)合圖6 分析FGO 算法相較于EFK 的誤差曲線更加平滑，定位精度更高.對實驗一和實驗四進行對比分析，F(xiàn)GO 算法相較于EFK 算法，可以極大地提高組合導(dǎo)航的定位精度，為松組合提供更優(yōu)的PPP 定位結(jié)果的同時，實現(xiàn)了信息融合的更優(yōu)化.結(jié)合表3，相較于EFK 算法，基于FGO 的組合導(dǎo)航在N、E、D 方向上定位精度分別提升了49.08%、41.22%、71.86%.

5 結(jié)束語

本文針對衛(wèi)星可見數(shù)不足、多復(fù)雜環(huán)境下GNSS信號容易發(fā)生失鎖的問題，提出了一種基于FGO 的PPP 算法，進行GNSS/INS 松組合導(dǎo)航，極大地提高了組合導(dǎo)航定位精度.實驗結(jié)果表明：

1)在進行PPP 時，F(xiàn)GO 算法由于其全局優(yōu)化的特性，可以在模糊度收斂后對收斂前時間段的數(shù)據(jù)進行再優(yōu)化，使PPP 定位結(jié)果在初始時刻就體現(xiàn)出模糊度收斂后的定位精度，極大提高了PPP 的可靠性和有效性.

2)本文提出的基于FGO 的PPP 算法，可以為GNSS/INS 松組合提供可靠的導(dǎo)航定位結(jié)果，相較于EKF 算法，在N、E、D 方向上的定位精度分別提升了49.08%、41.22%、71.86%.