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

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

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

0 引言

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

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

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

1 FGO 算法原理

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

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

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

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

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

2 PPP FGO 模型

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

2.1 PPP 數學模型

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

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

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

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

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

2.2 PPP FGO 模型

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

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

圖1 PPP FGO 框架

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

3 基于FGO 的PPP GNSS/INS 組合導航

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

3.1 IMU 預積分因子

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

3.2 PPP 因子

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

3.3 GNSS/INS FGO 模型

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

圖2 GNSS/INS 松 組合FGO 框架

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

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

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

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

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

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

表1 慣性導航元件標定參數及空間杠桿桿臂

圖3 跑車測試平臺

圖4 跑車測試軌跡

4.1 PPP 定位性能分析

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

表3 GNSS/INS 三維位置RMSE m

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

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

5 結束語

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

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

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