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

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

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

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

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

1 約束卡爾曼濾波模型

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

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

(1)

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

Dk=BXk+εk.

(2)

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

(3)

(4)

(5)

上式即為約束卡爾曼濾波模型。該模型由兩部分構(gòu)成，第一部分為標(biāo)準(zhǔn)卡爾曼濾波過程；第二部分為對(duì)標(biāo)準(zhǔn)卡爾曼濾波加以約束修正過程。

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

(6)

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

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

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

(7)

若Ak=1時(shí)為標(biāo)準(zhǔn)正交函數(shù)系，對(duì)于給定節(jié)點(diǎn){x0,x1,…,xm}求得離散正交多項(xiàng)式{P0(x),P1(x),…,Pn(x)}(n≤m)為

(8)

其中

(9)

因此，由最小二乘導(dǎo)出的方程組為：

(10)

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

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

(11)

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

(12)

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

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

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

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

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

圖1 周秒為103 010 s時(shí)星空?qǐng)D

圖2 平面定位偏差

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

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

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

3.2 解算方案設(shè)計(jì)

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

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

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

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

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

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

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

圖3 N方向定位偏差

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

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

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

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

圖6 行車路線

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

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

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

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

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

4 結(jié)束語

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

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

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

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

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