姜 維，曹琢健，陸德彪，蔡伯根，上官偉，王 劍

(1．北京交通大學(xué) 電子信息工程學(xué)院，北京 100044；2．北京市軌道交通電磁兼容與衛(wèi)星導(dǎo)航工程技術(shù)研究中心，北京 100044)

目前，全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)仍然作為解決用戶定位導(dǎo)航的主要手段之一被廣泛使用。它能夠全天候、全時(shí)段地為全球用戶提供低成本、高精度的三維位置、速度和精確定時(shí)等導(dǎo)航信息。同時(shí)由于GNSS與慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)測量值的互補(bǔ)特性[1]，使得GNSS與INS的組合導(dǎo)航研究與應(yīng)用最為廣泛[2]。GNSS可提供長時(shí)間的高精度位置、速度信息，INS可提供短時(shí)間內(nèi)準(zhǔn)確的姿態(tài)測量信息，顯然，二者的組合導(dǎo)航系統(tǒng)性能遠(yuǎn)遠(yuǎn)高于單一系統(tǒng)。

針對GNSS/INS組合導(dǎo)航，很多學(xué)者展開了大量的研究工作。文獻(xiàn)[3]提出了一種新的自適應(yīng)過程噪聲調(diào)整算法應(yīng)用于GNSS/INS組合導(dǎo)航中，實(shí)驗(yàn)驗(yàn)證，這種自適應(yīng)調(diào)整機(jī)制提高了總體組合性能。文獻(xiàn)[4]提出一種將神經(jīng)網(wǎng)絡(luò)與低成本GPS/INS系統(tǒng)集成的新方法，實(shí)驗(yàn)結(jié)果表明，在GPS失效時(shí)，神經(jīng)網(wǎng)絡(luò)輔助INS的位置解在系統(tǒng)運(yùn)行270 s后的北向誤差為220 m，東向誤差為40 m，垂直方向誤差為29 m。文獻(xiàn)[5]利用低成本MEMS傳感器和GPS組合，并將研制出的自主導(dǎo)航系統(tǒng)在汽車和遙控飛機(jī)上測試，測試結(jié)果表明，面對模擬的GPS失效環(huán)境，能夠分別在7、6 s之內(nèi)維持可接受的導(dǎo)航精度。綜上所述，雖然GNSS和INS組合導(dǎo)航系統(tǒng)的性能遠(yuǎn)優(yōu)于單一導(dǎo)航系統(tǒng)，但在城市環(huán)境等多徑效應(yīng)嚴(yán)重、信號遮擋嚴(yán)重的區(qū)域，二者組合導(dǎo)航系統(tǒng)也難以保持較高精度的定位性能，定位結(jié)果無法使用。在一些GNSS衛(wèi)星信號失鎖的場景下，單獨(dú)依靠INS進(jìn)行定位，定位結(jié)果會迅速發(fā)散，在幾秒鐘之后就無法使用。

由于UWB信號的時(shí)域脈沖極窄，時(shí)間分辨率高，因此UWB系統(tǒng)具有很好的抗多徑干擾能力和厘米級別的距離分辨率。基于以上優(yōu)勢，UWB技術(shù)尤其適用于密集多徑場景的應(yīng)用中。所以，UWB技術(shù)可以極好地彌補(bǔ)GNSS受限環(huán)境下的劣勢，維持定位功能的可用性。

在組合方式上，GNSS/INS組合導(dǎo)航主要有松組合和緊組合兩種組合模式[8]。松組合是利用GNSS接收機(jī)輸出的位置、速度信息和INS輸出的信息進(jìn)行組合，這種組合方式的優(yōu)點(diǎn)是系統(tǒng)靈活、運(yùn)算量低。但是其必須在GNSS可以輸出定位信息的情況下才可以使用。當(dāng)GNSS可見星數(shù)少于4顆時(shí)，松組合方式將無法使用。緊組合是利用GNSS接收機(jī)輸出的偽距、偽距率等觀測量，與INS結(jié)合星歷反算得到的偽距、偽距率進(jìn)行組合[9]，其最主要的優(yōu)勢是增強(qiáng)了系統(tǒng)的可用性，允許在GNSS可見星數(shù)量較少時(shí)獲取定位結(jié)果。因此，緊組合的GNSS/INS系統(tǒng)是目前應(yīng)用最廣泛的組合體系結(jié)構(gòu)。由于GNSS接收機(jī)跟蹤的衛(wèi)星少于4顆或UWB接收到的基站距離信息少于3個(gè)時(shí)，無法直接從GNSS或UWB計(jì)算待定位置，松組合系統(tǒng)將無法工作，同時(shí)為了更好地利用GNSS接收機(jī)的偽距、偽距率以及UWB距離測量值的原始信息，本文對GNSS、INS、UWB的三組合導(dǎo)航系統(tǒng)采用緊組合方式，確保在可用信號較少的情況下，系統(tǒng)仍可以工作。

卡爾曼濾波作為組合導(dǎo)航采用的最廣泛的濾波算法，能提供系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。本文針對多種傳感器組合，采用集中式結(jié)構(gòu)的卡爾曼濾波的融合方法，利用全部的、有限的量測數(shù)據(jù)，增強(qiáng)濾波的可靠性和系統(tǒng)的生存能力。集中式卡爾曼濾波在計(jì)算時(shí)，將各個(gè)子系統(tǒng)的數(shù)據(jù)統(tǒng)一輸入到信息融合中心，通過擴(kuò)展量測維度的方式，集中對多個(gè)數(shù)據(jù)源的數(shù)據(jù)進(jìn)行處理，保證了信息的充分利用。

1 GNSS/INS組合導(dǎo)航

1.1 慣性導(dǎo)航遞推算法

慣性導(dǎo)航系統(tǒng)建立在慣性原理基礎(chǔ)上，不需要任何外來信息，也不會向外輻射任何信息，僅靠慣性導(dǎo)航系統(tǒng)本身就能在全天候條件下，在全球范圍內(nèi)和任何介質(zhì)環(huán)境里自主地、隱蔽地進(jìn)行連續(xù)的三維定位和三維定向。慣性導(dǎo)航系統(tǒng)內(nèi)置三軸加速度計(jì)和三軸陀螺儀，分別用來測量運(yùn)載體的角運(yùn)動(dòng)信息和線運(yùn)動(dòng)信息，機(jī)載計(jì)算機(jī)根據(jù)這些測量信息解算出運(yùn)載體的航向、姿態(tài)、速度及位置。慣性導(dǎo)航計(jì)算過程包含姿態(tài)更新、速度更新、位置更新三部分，其微分方程[10]為

(1)

(2)

(3)

(4)

(5)

(6)

1.2 GNSS/INS松組合系統(tǒng)模型

GNSS/INS松組合導(dǎo)航方法流程[11]見圖1。GNSS輸出的位置和速度值，作為測量輸出給卡爾曼濾波器，組合卡爾曼濾波器用它與INS計(jì)算的位置、速度值的差來估計(jì)INS誤差。卡爾曼濾波器估計(jì)的INS誤差，對INS導(dǎo)航參數(shù)進(jìn)行校正，經(jīng)過校正后的INS導(dǎo)航參數(shù)構(gòu)成組合導(dǎo)航的輸出。

圖1 GNSS/INS松組合導(dǎo)航方法流程

松組合導(dǎo)航的2個(gè)主要優(yōu)點(diǎn)是簡單和冗余。在組合導(dǎo)航中，除了組合導(dǎo)航參數(shù)，通常還有獨(dú)立的GNSS導(dǎo)航輸出。但松組合方法有很多問題。盡管短時(shí)間內(nèi)可以使用3顆衛(wèi)星的信號輸出導(dǎo)航解，但通常GNSS導(dǎo)航輸出需要來自4顆不同衛(wèi)星的信號。如果只能跟蹤更少的衛(wèi)星，GNSS數(shù)據(jù)不能用于對INS進(jìn)行輔助，依靠INS單獨(dú)進(jìn)行解算的結(jié)果將會迅速發(fā)散。

1.3 GNSS/INS緊組合系統(tǒng)模型

GNSS/INS緊組合導(dǎo)航方法流程見圖2。源于GNSS的原始數(shù)據(jù)偽距和偽距率構(gòu)成卡爾曼濾波的量測輸入，濾波器采用這些測量信息來估計(jì)INS和GNSS系統(tǒng)的誤差。與松組合導(dǎo)航參數(shù)相同，組合導(dǎo)航參數(shù)是校正后的慣性導(dǎo)航參數(shù)。緊組合的優(yōu)勢在于不需要一個(gè)完整的GNSS解來輔助INS，即使只跟蹤了一個(gè)GNSS衛(wèi)星信號，GNSS的偽距數(shù)據(jù)仍然能夠作為組合系統(tǒng)的輸入[12-13]。

圖2 GNSS/INS緊組合導(dǎo)航方法流程

1.3.1 GNSS/INS緊組合導(dǎo)航狀態(tài)模型

GNSS/INS緊組合導(dǎo)航狀態(tài)模型的構(gòu)建基于INS的誤差方程，假設(shè)慣導(dǎo)系統(tǒng)姿態(tài)失準(zhǔn)角是一個(gè)小角度，并且不考慮地球重力模型誤差，經(jīng)過推導(dǎo)得出慣導(dǎo)系統(tǒng)誤差方程[10]如下。

姿態(tài)誤差方程為

(7)

速度誤差方程為

(8)

位置誤差方程為

(9)

度計(jì)誤差向量，在本文中采用常值來定義。

僅利用單臺GNSS接收機(jī)的量測信息進(jìn)行組合，狀態(tài)模型中包含GNSS接收機(jī)的鐘差和時(shí)鐘漂移率的誤差。用dt表示接收機(jī)的鐘差，則

(10)

綜合以上各式，INS誤差方程寫成矩陣形式為

(11)

式中：X為卡爾曼濾波的狀態(tài)向量，由17維向量組成，見式(12)；F為系統(tǒng)轉(zhuǎn)移矩陣，其定義了狀態(tài)向量隨時(shí)間變化的規(guī)律，在卡爾曼濾波系統(tǒng)模型中是慣性導(dǎo)航系統(tǒng)的誤差傳遞動(dòng)力學(xué)方程；W為系統(tǒng)過程噪聲，在這里建模成高斯白噪聲，對應(yīng)的協(xié)方差矩陣為Q。

(12)

1.3.2 GNSS/INS緊組合導(dǎo)航量測模型

在緊組合系統(tǒng)中，觀測值由GNSS偽距和偽距率與根據(jù)INS自身計(jì)算的位置、速度得到的偽距、偽距率作差構(gòu)造而成，定義為

(13)

卡爾曼濾波量測方程為

Z=HX+V

(14)

式中：V為系統(tǒng)量測噪聲，在這里建模成高斯白噪聲，對應(yīng)的協(xié)方差矩陣為R；H為量測矩陣，由式(16)確定。

(15)

(16)

2 GNSS/INS/UWB緊組合導(dǎo)航模型

2.1 UWB測距定位技術(shù)

UWB技術(shù)是一種不用載波，而利用納秒至微微秒級的非正弦波窄脈沖傳輸數(shù)據(jù)的無線通信技術(shù)，使用頻段為3.1～10.6 GHz和低于41 dBm·W的發(fā)射功率，其數(shù)據(jù)速率可達(dá)幾十Mbps到幾百M(fèi)bps。UWB技術(shù)具有抗干擾性強(qiáng)、低發(fā)射功率、可全數(shù)字化實(shí)現(xiàn)、保密性好的特點(diǎn)，近年來成為無線定位技術(shù)的熱點(diǎn)。UWB定位主要采用TOA算法[14]，其工作原理見圖3。

圖3 基于TOA算法的UWB定位原理

一旦取得了多個(gè)TOA測量值，就可得到未知標(biāo)簽和多個(gè)基站的距離，從而構(gòu)成圓周方程組，求解該方程組就能得到移動(dòng)標(biāo)簽的距離。

(17)

(18)

式中：(δx，δy，δz)為位置偏移量；δri為第i個(gè)基站的距離偏移量。為方便起見，引進(jìn)axi、ayi、azi以簡化式(18)。axi、ayi、azi各項(xiàng)代表由近似位置指向第i號基站的單位矢量的方向余弦。

(19)

簡化后方程的矩陣形式為

(20)

2.2 GNSS/INS/UWB集中卡爾曼濾波模型

為了將GNSS、INS、UWB三者的測量數(shù)據(jù)充分利用，使用集中式卡爾曼濾波進(jìn)行數(shù)據(jù)融合。圖4給出了誤差狀態(tài)集中式卡爾曼濾波組合的結(jié)構(gòu)。集中式融合[15]就是將所有傳感器的量測數(shù)據(jù)都傳送到一個(gè)中心處理器進(jìn)行集中處理?？柭鼮V波需要對所有導(dǎo)航傳感器的系統(tǒng)誤差和噪聲源進(jìn)行建模，以確保所有相關(guān)的誤差都已經(jīng)考慮，所有的測量都已經(jīng)根據(jù)權(quán)值優(yōu)化，并且使用盡可能多的信息標(biāo)定每個(gè)誤差。從精度和魯棒性來說，集中式的組合結(jié)構(gòu)提供了最優(yōu)的導(dǎo)航參數(shù)。

圖4 集中式卡爾曼濾波組合結(jié)構(gòu)

集中濾波的狀態(tài)向量由17維組成，分別是3維位置誤差、3維速度誤差、3維姿態(tài)誤差、3維加速度誤差向量、3維陀螺儀誤差向量、GNSS接收機(jī)鐘差和鐘差漂移率，其表達(dá)式為

(21)

對應(yīng)的狀態(tài)轉(zhuǎn)移矩陣為

(22)

(23)

(24)

(25)

系統(tǒng)的量測模型見式(27)，包含了GNSS和UWB測量值的所有信息，為2m+n維向量，m為可見衛(wèi)星數(shù)，n為UWB基站個(gè)數(shù)。

(26)

量測轉(zhuǎn)移矩陣H為

(27)

(28)

(29)

(30)

慣性導(dǎo)航系統(tǒng)主要的噪聲源是由加速度計(jì)比力測量噪聲導(dǎo)致的速度誤差，以及由陀螺儀角速度測量噪聲帶來的姿態(tài)誤差。采用慣性導(dǎo)航器件的零偏來定義系統(tǒng)噪聲協(xié)方差矩陣為

(31)

式中：bf為加速度計(jì)零偏；bω為陀螺儀零偏。

(32)

集中卡爾曼濾波過程包含兩個(gè)更新過程，時(shí)間更新和量測更新。時(shí)間更新的兩個(gè)步驟預(yù)測了系統(tǒng)下一時(shí)刻的狀態(tài)值X和其協(xié)方差矩陣P。

后期編輯：除了前端錄制以外，錄播系統(tǒng)還提供了后期視頻編輯。教師可以在錄播主機(jī)管理后臺對錄制視頻編輯、剪輯、添加片頭、片尾和學(xué)院的LOGO。

(33)

Q(k-1)

(34)

在量測更新的流程里，首先是用最新觀測信息更新狀態(tài)估計(jì)和誤差協(xié)方差矩陣；其次是卡爾曼濾波的增益矩陣K的計(jì)算，此步可根據(jù)當(dāng)前狀態(tài)估計(jì)的不確定度和觀測噪聲情況來對狀態(tài)的修正量進(jìn)行優(yōu)化加權(quán)；然后是用卡爾曼增益加權(quán)的觀測數(shù)據(jù)更新狀態(tài)估計(jì)；最后是協(xié)方差矩陣的更新，用觀測數(shù)據(jù)的新信息更新誤差協(xié)方差矩陣。

HT(k)+R(k)]-1

(35)

(36)

(37)

3 實(shí)驗(yàn)與結(jié)果分析

基于GNSS/INS/UWB緊組合的定位實(shí)驗(yàn)，在北京交通大學(xué)主校區(qū)思源樓前空地進(jìn)行。思源樓高12層，由于樓體遮擋，思源樓正門前空地GNSS信號遮擋嚴(yán)重，因此選擇在此區(qū)域進(jìn)行定位實(shí)驗(yàn)，以驗(yàn)證算法的有效性。

實(shí)驗(yàn)裝置被安裝在測試車上，包含1臺UB370GNSS接收機(jī)，1個(gè)SPAN INS(由NovAtel OEM6 GNSS接收器和iMAR-FSAS IMU組成)，5個(gè)UWB基站和1個(gè)UWB標(biāo)簽。慣性傳感器的性能參數(shù)見表1。GNSS頻率與UWB測量值頻率均為10 Hz，INS頻率為200 Hz。UWB標(biāo)簽與GNSS天線處在同一垂線上。

表1 iMAR-FSAS IMU性能參數(shù)

與GNSS導(dǎo)航解算類似，UWB信號的幾何分布也影響著定位精度。引入精度因子的概念，精度因子取決于參與定位信號之間的幾何分布關(guān)系。給定測距信號，沿某軸向的定位精度與該軸向和用戶至基站信號矢量之間的夾角有關(guān)，夾角越小，該軸向上的定位精度越高。因此，用戶至基站的視線矢量在水平面內(nèi)均勻分布時(shí)定位精度較高。

實(shí)驗(yàn)環(huán)境和由SPAN輸出的實(shí)驗(yàn)軌跡見圖5。SPAN是商用高精度GPS/INS組合導(dǎo)航系統(tǒng)，但是在本實(shí)驗(yàn)惡劣的實(shí)驗(yàn)環(huán)境下，SPAN的定位結(jié)果精度不足，無法作為參考軌跡來驗(yàn)證本文算法，但可從圖5中看到大致的實(shí)驗(yàn)軌跡。

圖5 實(shí)驗(yàn)環(huán)境和SPAN定位軌跡

實(shí)驗(yàn)中選擇5個(gè)靜態(tài)測試點(diǎn)，便于進(jìn)行實(shí)驗(yàn)結(jié)果準(zhǔn)確性分析。采用上海華測公司研發(fā)的多模RTK GNSS接收機(jī)，接入千尋CORS服務(wù)，利用千尋平臺發(fā)布的差分?jǐn)?shù)據(jù)，進(jìn)行靜態(tài)RTK測量，得到靜態(tài)測試點(diǎn)的位置。測試點(diǎn)具有毫米級的位置精度，因此可作為參考位置來評估本文實(shí)驗(yàn)的定位精度。測試從點(diǎn)1開始，在每個(gè)點(diǎn)靜止2～4 min后，移動(dòng)至下一個(gè)點(diǎn)，且移動(dòng)過程基本近似為直線。測試點(diǎn)的靜止時(shí)長和測試周期見表2。

表2 測試點(diǎn)靜止時(shí)長和測試周期

圖6 實(shí)驗(yàn)軌跡對比

GNSS單點(diǎn)定位的可見衛(wèi)星數(shù)和定位精度(DOP)值見圖7,GNSS/INS/UWB組合定位的可見信號數(shù)和DOP值見圖8。對比兩圖可見，由于UWB的引入，信號數(shù)量的增加，三組合的信號具有更好的幾何分布特性，精度因子明顯低于GNSS單點(diǎn)定位的精度因子值。這為導(dǎo)航系統(tǒng)的精度性能提供了基礎(chǔ)。

圖7 GNSS單點(diǎn)定位可見衛(wèi)星數(shù)與DOP值

圖8 GNSS/INS/UWB組合定位可見信號數(shù)與DOP值

測試點(diǎn)水平分量位置誤差的散點(diǎn)圖見圖9。其中圖9(a)～9(e)分別是測試點(diǎn)1～5的誤差散點(diǎn)，從圖中可以看出，GNSS單點(diǎn)定位結(jié)果和GNSS/INS定位結(jié)果誤差基本都在1 m以上，且準(zhǔn)確度不足，軌跡跳動(dòng)劇烈；而GNSS/INS/UWB組合定位結(jié)果基本收斂在測試點(diǎn)0.5 m的范圍內(nèi)。需要說明的一點(diǎn)是，由于在實(shí)驗(yàn)開始階段，三組合卡爾曼濾波需要收斂時(shí)間，所以圖9(a)中的測試點(diǎn)1定位結(jié)果有一個(gè)收斂的過程，收斂后基本穩(wěn)定在測試點(diǎn)附近。圖9(f)為所有測試點(diǎn)的GNSS/INS/UWB組合定位結(jié)果誤差散點(diǎn)圖，從圖中可以看出，穩(wěn)定后的GNSS/INS/UWB定位結(jié)果具有很好的精密度。

圖9 測試點(diǎn)水平分量誤差散點(diǎn)

圖10是測試點(diǎn)的東向、北向、天向誤差，對其進(jìn)行量化評估，3個(gè)方向的RMS值柱狀圖見圖11。從圖中可以看出，東向、北向的均方根誤差均在0.2 m以下，而垂直方向的均方根誤差也均在0.4 m以下。

圖10 測試點(diǎn)東向、北向、天向誤差

圖11 測試點(diǎn)東向、北向、天向RMS值

水平面的距離均方根誤差(DRMS)描述了水平面的位置誤差，而平均徑向球面誤差(MRSE)描述了三維位置誤差。5個(gè)測試點(diǎn)的DRMS值和MRSE值見表3。針對所有測試點(diǎn)的DRMS值為0.1 236 m，MRSE值為0.2 727 m。因此，實(shí)驗(yàn)數(shù)據(jù)表明在GNSS多徑效應(yīng)嚴(yán)重、定位困難的環(huán)境下，依靠本文提出的UWB輔助增強(qiáng)GNSS/INS組合導(dǎo)航的方法能夠獲得較高的定位精度。水平方向定位精度能達(dá)到2 dm以下，而三維球面的定位精度能達(dá)到5 dm以下。且系統(tǒng)具有較好的穩(wěn)定性，基本消除了GNSS信號造成的位置跳動(dòng)的影響。

表3 測試點(diǎn)的DRMS和MRSE值

4 結(jié)論

UWB技術(shù)作為近年來備受關(guān)注的無線電技術(shù)，在抗多徑效應(yīng)、定位精度等方面具有強(qiáng)大的優(yōu)勢。為了克服城市區(qū)域內(nèi)的嚴(yán)重多徑效應(yīng)、信號衰落，本文提出了將UWB技術(shù)引入GNSS/INS組合的組合導(dǎo)航模型。由于在城市環(huán)境下，GNSS信號遮擋嚴(yán)重，面對可能的GNSS可見星數(shù)不足的情況，本文采用了緊組合的組合模式，保證了對有限信息的充分利用，提高了系統(tǒng)的可用性。集中式卡爾曼濾波結(jié)構(gòu)用于融合GNSS、INS、UWB的測量信息，保證系統(tǒng)能獲得全局最優(yōu)估計(jì)。

為了對本文提出的組合定位方法進(jìn)行評估，在GNSS信號建筑物遮擋嚴(yán)重的場所進(jìn)行了實(shí)驗(yàn)測試。實(shí)驗(yàn)過程中分別在5個(gè)位置已知的測試點(diǎn)靜止了幾分鐘。實(shí)驗(yàn)結(jié)果表明：

(1)在GNSS單點(diǎn)定位可靠性嚴(yán)重下降的情況下，本文所提出的導(dǎo)航系統(tǒng)能夠提供水平面精度優(yōu)于0.2 m，3D球面精度優(yōu)于0.4 m的定位精度。

(2)系統(tǒng)所采用的組合結(jié)構(gòu)和濾波方法能夠提供較快的收斂速度和穩(wěn)定性，基本消除了GNSS信號造成的位置跳動(dòng)的影響。