附加正交函數(shù)擬合條件的導航濾波方法

劉國良，蔣廷臣，喻國榮，潘樹國，張 建2,

(1.淮海工學院 測繪與海洋信息學院，江蘇 連云港 222005; 2.南京康帕斯導航科技有限公司,江蘇 南京 210096; 3.東南大學 交通學院,江蘇 南京 210096; 4.東南大學 儀器科學與工程學院,江蘇 南京 210096)

隨著無人機、共享單車、滴滴打車、無人駕駛汽車等大眾導航新興產(chǎn)業(yè)的發(fā)展，對于利用全球衛(wèi)星導航定位技術實現(xiàn)米級至亞米級導航定位的需求正日趨旺盛。在大眾導航領域采用的GNSS接收機大都是價格低廉、功能單一、數(shù)據(jù)質(zhì)量不穩(wěn)定的單頻接收機。當這類接收機短暫經(jīng)過城市高樓、高架橋、高大綠化植物時，衛(wèi)星信號會被反射、遮擋或衰減，使得過街精度因子[1]快速惡化，導致接收機的定位結果往凈空條件好的一側偏移。

對于遮擋環(huán)境中，過街精度因子受到影響的問題，顏瑜軍[2]基于3D建筑模型提出SM 與GPS組合的定位算法。王勵揚[3]等利用車輪進動信息將DR系統(tǒng)與GPS系統(tǒng)組合，在信號遮擋時也能得到連續(xù)可靠的定位。歐敏輝[1]利用慣性導航不受凈空環(huán)境干擾的特點，驗證GPS/INS組合導航定位算法，使得過街平均誤差提高到1.49 m。劉志鋒[4]等通過電子羅盤和低精度IMU，融合航跡推算與合成孔徑等技術實現(xiàn)障礙物遮擋環(huán)境下的動態(tài)定位。但是此類組合算法加入其他測量設備，極大地增加硬件成本，難以滿足低成本的大眾導航現(xiàn)實需求。因此，一些學者提出通過附加約束條件的方法從軟件層面進行優(yōu)化，常有附加跡向約束[5,6]，附加基線約束[7]、附加速度約束[8]的卡爾曼濾波算法等。附加跡向約束是基于載體運動時一般不會偏離航向的假設，通過現(xiàn)有地圖或已知的道路航向構建約束條件，缺點是需要提前獲知準確地跡向信息。附加基線約束通常是利用兩個或多個天線間相對距離始終保持固定而建立約束方程，這種約束條件較為準確，且實用性較強，但也在一定程度上增加硬件成本。附加速度約束是通過載波或多普勒觀測值解算得到的速度及方向，從而建立下一歷元的運動約束條件。附加速度約束是目前比較經(jīng)濟可靠的方式之一，不需要通過外加硬件或先驗信息而增加成本，但其仍然容易受到觀測環(huán)境的影響而造成濾波不穩(wěn)定。

為此，本文提出在速度約束的基礎上增加狀態(tài)擬合約束的抗差自適應濾波方法。該方法充分利用載體穩(wěn)定的運動信息，通過采用正交函數(shù)最小二乘擬合的方式預測載體運動狀態(tài)，從而形成新的約束條件，以增強單一速度約束濾波的穩(wěn)定性。同時，為抑制不良觀測值和運動模型誤差的影響，加入抗差估計和自適應調(diào)節(jié)策略，利用速度條件和運動狀態(tài)擬合條件聯(lián)合建立抗差自適應卡爾曼濾波模型。

1 約束卡爾曼濾波模型

卡爾曼濾波是一種具有很高計算效率的實時參數(shù)估計方法，常用于導航定位算法當中。在實際的導航應用中，不僅速度，還有很多潛在的信息也能作為卡爾曼濾波的輸入，文獻[9]和[10]通過融合多個約束條件推導約束卡爾曼濾波模型。

假設卡爾曼濾波系統(tǒng)狀態(tài)方程和觀測方程為：

(1)

式中：下標k代表當前時刻；Wk為狀態(tài)輸入噪聲；Γk-1為系統(tǒng)噪聲驅(qū)動矩陣；Xk-1為前一時刻的狀態(tài)估值；Φk,k-1為狀態(tài)轉移矩陣；Xk為當前時刻的狀態(tài)向量；Lk為觀測向量；Ak為設計矩陣；ek為觀測噪聲。假定觀測值獨立且系統(tǒng)噪聲與觀測噪聲是互不相關的零均值高斯噪聲。若附加約束條件為：

Dk=BXk+εk.

(2)

式中：設計矩陣B為時不變的單位陣，有時為rk×m維時變矩陣Bk,Dk是約束值矩陣，其協(xié)因數(shù)陣為Rk；εk為約束條件的噪聲。當εk=0代表約束條件準確可靠為強約束，否則εk≠0。因此，由式(1)、(2)可建立拉格朗日最優(yōu)化條件：

(3)

(4)

(5)

上式即為約束卡爾曼濾波模型。該模型由兩部分構成，第一部分為標準卡爾曼濾波過程；第二部分為對標準卡爾曼濾波加以約束修正過程。

當Lk存在不良觀測值和系統(tǒng)狀態(tài)模型存在誤差時，可按等價權原理對不良觀測值作抗差估計和采用自適應因子調(diào)節(jié)模型誤差的影響。即式(4)和式(5)的第3式可改寫成附有約束條件的抗差自適應濾波模型。

(6)

2 正交函數(shù)的最小二乘擬合

在實際的導航定位中，當載體短暫經(jīng)過高大建筑物時，常規(guī)最小二乘估計得到的定位結果會大幅偏移，若僅利用上一時刻精度不高的位置及速度估值去約束下一時刻的運動狀態(tài)，一者容易造成基于速度建立的運動模型不準確，二者若在下一時刻也未觀測到良好的衛(wèi)星信息，未能及時對濾波模型做出準確地糾正，僅以速度約束的卡爾曼濾波模型的穩(wěn)定性會容易受到擾動。對于機動性不高的載體，其運動軌跡、速度及航向的變化一般為直線或規(guī)則曲線，即下一時刻的運動狀態(tài)，與之前若干時刻的運動狀態(tài)間能構成直線或規(guī)則曲線等模糊依存的關系，且這種關系是呈現(xiàn)倒逆衰減趨勢的。在數(shù)據(jù)處理中，建立這種模糊的依存關系常采用數(shù)據(jù)擬合的方法。在實際運用中，通過多項式進行擬合常采用正交多項式做最小二乘擬合[14]的方法。這種擬合方法采用遞推公式，是目前進行曲線擬合的最好方法[15]。

假設各多項式函數(shù){P0(x),P1(x),…,Pn(x)} (0,1,…,n為多項式次數(shù))是滿足：

(7)

若Ak=1時為標準正交函數(shù)系，對于給定節(jié)點{x0,x1,…,xm}求得離散正交多項式{P0(x),P1(x),…,Pn(x)}(n≤m)為

(8)

其中

(9)

因此，由最小二乘導出的方程組為：

(10)

解得系數(shù){a0,a1,…,an}，則擬合函數(shù)為：

φ(x)=a0P0(x)+a1P1(x)+…+anPn(x).

(11)

由狀態(tài)估值y求得擬合殘差平方和均值為：

(12)

3 實測數(shù)據(jù)分析

在市區(qū)道路動態(tài)定位中，高大構筑物間斷性地出現(xiàn)，因此信號遮擋亦為短暫性、間歇性的。此時若利用載波進行定位需要頻繁初始化固定模糊度，實用性不強，而碼偽距無須固定模糊度，且定位算法簡單，對接收機硬件要求也不高，較符合大眾導航領域米級至亞米級的定位需求。因此，本文采用GPS L1頻點和BDS B1頻點的碼偽距觀測值作為基礎定位數(shù)據(jù)源。

3.1 遮擋環(huán)境影響分析

為直觀重現(xiàn)衛(wèi)星信號被短暫性、間歇性遮擋所造成的影響，采用某CORS站年積日為8d GPS 02:29:50～23:59:59的Trimble Net R9接收機采集的靜態(tài)觀測數(shù)據(jù)進行分析，在某觀測質(zhì)量較好的時段內(nèi)，隨機取120組連續(xù)10個歷元剔除方位角在0°～180°的衛(wèi)星數(shù)據(jù)，模擬實際間歇性遮擋環(huán)境。其中CORS站坐標參考值由GAMIT聯(lián)合同期IGS站數(shù)據(jù)獲得。周積秒為103 010 s時的全部可見衛(wèi)星視圖如圖1(a)所示，遮擋后如圖1(b)所示。

從圖1可見，遮擋前可視衛(wèi)星空間分布良好，遮擋后分布重心偏西南方向。采用單歷元偽距單點解算間歇性遮擋前后數(shù)據(jù)并與參考值作差，相對應的平面定位誤差分布如圖2(a)、2(b)所示。

圖1 周秒為103 010 s時星空圖

圖2 平面定位偏差

從圖2(a)中看到，間歇性遮擋前偽距單點定位偏差在±1.5 m且分布均勻，表明在此測段內(nèi)衛(wèi)星觀測數(shù)據(jù)質(zhì)量較好。從間歇性遮擋后的圖2(b)可見，未被遮擋的歷元定位結果正常，被遮擋的歷元有大幅度跳躍性偏移，N方向最大偏移量達10 m，偏移方向在衛(wèi)星密集分布的西南方。為更直觀顯示各歷元的實際偏移情況，繪制間歇性遮擋前后各歷元在E方向的定位偏差，如圖3所示。

從圖3(a)可以看出，間歇性遮擋前N方向的定位偏差在±1.5 m左右，而圖3(b)顯示所隨機選取的多組遮擋歷元N方向的定位出現(xiàn)大幅度偏移，最大偏移量將近10 m，而未被選中遮擋的歷元定位偏差仍在1.5 m之內(nèi)。由此說明，當可視衛(wèi)星被遮擋，導致空間分布不佳時會使解算結果出現(xiàn)大幅度的偏移。

除了遮擋之外，由高樓反射引起的多路徑效應也會使定位結果出現(xiàn)不同程度的偏移。但無論是可視衛(wèi)星分布不佳還是多路徑效應的影響，在低成本的大眾導航領域常是采用增加約束的方法予以抑制。

3.2 解算方案設計

為了驗證本文所提方法在大眾導航領域中的應用效果，采用集成Ublox-m8t芯片的單頻GNSS接收機，采集多組動態(tài)實測數(shù)據(jù)。通過采集的碼偽距觀測值作基礎定位，以3種解算方案進行解算分析：

方案1：偽距單點定位。采用經(jīng)典最小二乘的參數(shù)估計方法。

方案2：速度約束的偽距單點定位。在方案1的基礎上，采用多普勒求得的速度估值，建立勻速運動CV模型[9]，同時加入抗差等價權IGG-III方案，并采用兩段函數(shù)法確定自適應因子，聯(lián)合碼偽距建立抗差自適應卡爾曼濾波模型。

方案3：擬合約束的偽距單點定位。在方案2的基礎上，建立狀態(tài)擬合+速度約束的抗差自適應卡爾曼濾波定位方法。

3.3 動態(tài)測試分析

第1組：在推行的自行車上簡易安裝ZHD-H32接收機和單頻GNSS接收機。具體路線如圖4(a)所示，接近終點的遮擋路段定位結果如圖4(b)所示。

在圖4(a)中，路線起點為天橋，隨后轉入半邊有綠植遮擋的人行道，沿著地面近似直線的標識線推行至終點。圖4(b)為第1 166～1 239個歷元各方案的定位結果。從圖4(b)中可以看出，當接收機經(jīng)過遮擋信號的高樓時，偽距單點定位結果最大偏移約達30 m，過街精度極差，而基于速度約束的定位結果在一定程度上減少偏移量，但是擬合約束的改善效果更佳，定位結果更為平滑穩(wěn)定。為直觀表明擬合約束的改善效果，把速度約束和擬合約束的解算結果與參考值做差，繪制偏移明顯的E方向差值如圖5所示。

圖3 N方向定位偏差

圖4 第1組動態(tài)測試

圖5 第2組動態(tài)測試E方向偏差

從圖5(a)中可以看出，僅附加速度約束的濾波結果通過抗差處理和自適應調(diào)節(jié)，在一定程度上削弱不良觀測值的影響，使偽距單點約30 m的偏移量縮小到8.75 m；在圖5(b)中，本文所提的擬合與速度聯(lián)合約束濾波方法，把最大偏移量縮小到4.34 m左右，相較于方案2提升了50.4%，進一步增強濾波的穩(wěn)定性。

第2組：在SUV越野車上裝載毫米級 IMU/GNSS融合定位的GNSS終端動態(tài)檢測設備和單頻GNSS接收機，兩者共用同一接收天線。通過Inertial Explorer軟件解算IMU/GNSS采集的數(shù)據(jù)，以其解算結果作為參考值。由檢測設備的定位結果繪制的行車路線如圖6所示。

圖6 行車路線

從圖6可見，路線經(jīng)過高架橋底、山邊公路、隧道以及市區(qū)樓房等，基本穿越城市各類遮擋物邊沿。截取其中一處高樓遮擋嚴重的Google Earth圖像，如圖7所示。

圖7 第2組動態(tài)測試遮擋路段定位對比

在圖7中，道路南側有遮擋高樓，北側有遮擋樓房，北側影響較南側嚴重。此時，偽距單點定位結果往南側最大偏移達22.27 m，僅加速度約束的定位結果最大偏移量縮小到8.27 m，擬合與速度聯(lián)合約束的進一步縮小，最大偏移量為5.78 m，較偽距單點提升74.0%，較速度約束提升30.1%。另外，分析全程(除隧道外)擬合與速度聯(lián)合約束定位結果與參考值的偏差，統(tǒng)計結果如表1所示。

表1 擬合約束結果偏差統(tǒng)計表 m

從表1可以看出，即使在城市復雜道路環(huán)境下，本文所提方法在N，E方向仍能達到97%以內(nèi)的定位精度。另外，需要注意的是，測試發(fā)現(xiàn)當接收機長時間處于可視衛(wèi)星空間分布不佳的遮擋環(huán)境中時，若仍做機動性拐彎或小半徑變道，附加擬合約束的定位結果會存在一定程度上的偏差。

4 結束語

本文通過實測數(shù)據(jù)分析在可視衛(wèi)星空間分布不佳的遮擋環(huán)境中，定位結果會出現(xiàn)大幅度的偏移，通過采用增加新約束條件的方式以縮小定位的偏移，利用最小二乘正交擬合的方法，利用的擬合約束條件以增強卡爾曼濾波的穩(wěn)定性，改善短時不良觀測環(huán)境對定位所造成的偏移影響。從上述分析結果可以看出：

1)在高樓林立的城市道路觀測環(huán)境中，衛(wèi)星信號常受遮擋，可視衛(wèi)星空間分布不佳，是使定位結果出現(xiàn)大幅度偏移的主要因素。此時若采用單歷元偽距定位的方法會出現(xiàn)跳躍性的偏移，但通過增加約束的方法能夠在一定程度上削弱遮擋環(huán)境的影響。

2)從各解算方案可以看出，附加擬合約束的定位效果最好，僅以速度約束的定位效果次之。從實測數(shù)據(jù)解算的最大偏移量看，附加擬合約束的定位效果比速度約束的提升30.1%以上，表明本文所提方法具有一定程度上削弱和大幅度偏移，增強濾波穩(wěn)定性。

3)在實踐中，通過擬合來建立約束條件也存在一些困難，如擬合窗口的寬度一般難以預先準確設定。在測試中試過m=5,10,15,20,35，其中m=10時效果較好。另外，通過擬合建立的約束條件并不能總保證其準確性，在觀測環(huán)境良好時可不啟用，擬合的多項式次數(shù)可按實際情況設定，文中采用的是二次多項式。

同時，本文所述的方法并未能完全消除遮擋、多路徑和不良觀測值的影響，僅是在一定程度上抑制偏移量，提高過街精度。當載體在遮擋環(huán)境下，還需采用慣導、激光、雷達以及視覺SLAM等輔助測量手段以進一步提升定位的穩(wěn)定性及可靠性。