一種多源融合室內(nèi)外無縫定位方法*

姚子揚(yáng)尚俊娜*孫建強(qiáng)肖江寧王奕騰

(1.杭州電子科技大學(xué)通信工程學(xué)院，浙江 杭州 310018；2.通信信息控制和安全技術(shù)重點(diǎn)實(shí)驗(yàn)室，浙江 嘉興 314033；3.中國電子科技集團(tuán)公司第三十六研究所，浙江 嘉興 314033)

隨著信息時代的來臨，基于位置信息的服務(wù)在各種應(yīng)用場景中扮演的角色愈加重要。室外場景下，全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)已經(jīng)可以提供實(shí)時的可靠穩(wěn)定的全球定位導(dǎo)航服務(wù)。然而在地下車庫、交通隧道以及城市峽谷等眾多類似室內(nèi)的場景下，衛(wèi)星信號丟失，由此也催生了諸多室內(nèi)定位無線技術(shù)，比如超寬帶、WIFI技術(shù)。如何借助現(xiàn)有的定位技術(shù)進(jìn)行準(zhǔn)確的定位愈加關(guān)鍵，多源融合定位技術(shù)也相應(yīng)被提出。它采用信息融合的方法，將衛(wèi)星導(dǎo)航定位、無線傳感器定位以及其他輔助定位技術(shù)進(jìn)行處理，最終獲得可靠準(zhǔn)確且穩(wěn)定的定位服務(wù)。

多源信息融合最早提出于1973年，美國系統(tǒng)科學(xué)家Bar-Shalom Y研究了一種數(shù)據(jù)融合處理濾波器[1]，初次引出了多源融合技術(shù)的概念。多源數(shù)據(jù)融合技術(shù)廣泛應(yīng)用在定位與組合導(dǎo)航領(lǐng)域。文獻(xiàn)[2]提出了一種具有普適性的多源融合定位系統(tǒng)架構(gòu)，從數(shù)據(jù)處理、定位算法、效能評估等方面對多源融合定位進(jìn)行了研究。文獻(xiàn)[3]研究了異構(gòu)環(huán)境下的融合定位，提出了一種慣性導(dǎo)航輔助的RSSI(received signal strength indicator，RSSI)快速建庫方法。文獻(xiàn)[4]針對室內(nèi)外無縫定位進(jìn)行了研究，在使用多個不同定位源的情況下，提出了新的異構(gòu)定位系統(tǒng)間切換算法，將不同定位系統(tǒng)的定位信息進(jìn)行融合處理。文獻(xiàn)[5]提出了一種在復(fù)雜室內(nèi)環(huán)境下的多源信息融合定位系統(tǒng)，該系統(tǒng)第一個階段為人群感知階段，第二個階段為定位導(dǎo)航階段，充分使用了藍(lán)牙、WiFi、地磁和圖像信息。文獻(xiàn)[6]在移動機(jī)器人同步定位與建圖中，對各類傳感器信息在不同層次進(jìn)行了融合，研究了一種慣導(dǎo)信息組合以及激光視覺信息處理方法。文獻(xiàn)[7]將GNSS定位與地磁技術(shù)進(jìn)行了融合處理，根據(jù)衛(wèi)星數(shù)以及精度因子實(shí)現(xiàn)切換算法設(shè)計(jì)；文獻(xiàn)[8]采用GPS、DR(dead reckoning，DR)和WiFi組合室內(nèi)外定位，根據(jù)信號類型自動切換定位；文獻(xiàn)[9]根據(jù)各誤差系數(shù)改進(jìn)了相關(guān)的發(fā)生器和濾波器，采用了WPS(wireless positioning system，WPS)、GPS、以及MIMU(miniature inertial measurement unit，MIMU)三種定位源進(jìn)行了定位實(shí)現(xiàn)。文獻(xiàn)[10]提出了一種基于計(jì)數(shù)以及閾值機(jī)制的智能切換算法，能在室內(nèi)外切換的定位場景下滿足低能耗高精度的定位。針對組合導(dǎo)航中GPS短時失效的問題，文獻(xiàn)[11]采用基于偏最小二乘的輔助高斯過程回歸預(yù)測設(shè)備的軌跡，能在短時間內(nèi)得到良好的估計(jì)精度。捷聯(lián)慣性導(dǎo)航作為多數(shù)多源融合方法中非常重要的組成部分，其計(jì)算方法對定位結(jié)果有一定的影響，文獻(xiàn)[12]比較了基于歐拉角和基于四元數(shù)的兩種非線性組合導(dǎo)航濾波的定位精度和計(jì)算量，實(shí)驗(yàn)證明前者精度較差但計(jì)算量小，后者則反之。文獻(xiàn)[13]利用預(yù)測殘差建立的統(tǒng)計(jì)量調(diào)節(jié)位置與速度向量，削弱狀態(tài)模型誤差和異常擾動導(dǎo)致的組合導(dǎo)航中卡爾曼濾波性能下降問題。針對精密單點(diǎn)定位動態(tài)定位精度低、收斂速度慢等問題，文獻(xiàn)[14]采用PPP/INS緊組合系統(tǒng)來改善動態(tài)定位性能。在GNSS信號受遮蔽的情況下，文獻(xiàn)[15]主要利用了地磁與慣性器件動態(tài)修正航向角，結(jié)合GNSS的定位結(jié)果該方法比未修正時提高了60%的準(zhǔn)確率。文獻(xiàn)[16]提出采用反距離加權(quán)內(nèi)插的多基準(zhǔn)站偽距差分法，修正北斗偽距方程減小空間偽距誤差，構(gòu)建了基于北斗偽距差分與WiFi的室內(nèi)外融合定位方法，有效解決了單一系統(tǒng)的定位缺陷和定位盲區(qū)問題。文獻(xiàn)[17]提出了一種結(jié)合UWB的GNSS組合導(dǎo)航方法，分別采用了集中式卡爾曼濾波和聯(lián)邦式卡爾曼濾波的多傳感器組合濾波結(jié)構(gòu)進(jìn)行融合定位，實(shí)驗(yàn)驗(yàn)證后者對于異常值具有更強(qiáng)的抗干擾能力。

基于以上背景，為了在常用設(shè)備的基礎(chǔ)上進(jìn)一步探究如何實(shí)現(xiàn)室內(nèi)外無縫切換的高精度導(dǎo)航定位，本文主要對氣壓計(jì)、UWB定位、慣性導(dǎo)航定位以及GNSS定位的多源融合定位技術(shù)進(jìn)行研究，設(shè)計(jì)實(shí)現(xiàn)了相應(yīng)的組合導(dǎo)航定位算法，并最終完成多源融合定位。

1 多源融合室內(nèi)外定位方案

首先，本文在前人研究的基礎(chǔ)上提出了多源融合室內(nèi)外定位方案的整體設(shè)計(jì)框圖，如圖1所示，接下來著重詳細(xì)講述室外定位以及室內(nèi)外無縫定位部分。

圖1 多源融合室內(nèi)外定位設(shè)計(jì)框圖

因?yàn)橐瓿墒覂?nèi)外的融合定位設(shè)計(jì)，待選取定位場景較大，基于UWB的位置指紋定位方案前期要進(jìn)行較多的準(zhǔn)備工作，適用于小的定位場景，因此選用PDR輔助UWB的定位方案；室外定位階段，選擇GNSS組合INS的定位技術(shù)手段。使用GNSS信號進(jìn)行室外定位時，可以采用單點(diǎn)定位或者RTK差分定位。單點(diǎn)定位只需一根天線即可，成本低但定位精度不高，RTK采用了載波相位測量技術(shù)，差分定位時使用雙天線數(shù)據(jù)進(jìn)行差分處理，從算法上極大的改善了定位的精度與性能，接下來首先對GNSS的兩種定位模式的實(shí)現(xiàn)原理進(jìn)行講述，然后進(jìn)行實(shí)驗(yàn)研究測試。

1.1 GNSS定位技術(shù)

隨著全球衛(wèi)星數(shù)量的增加以及定位系統(tǒng)的完善，基本可以實(shí)現(xiàn)全天候、實(shí)時的室外定位。根據(jù)不同的劃分指標(biāo)，GNSS可以分為不同的定位方式，其中最為常用的是基于偽距的單點(diǎn)定位技術(shù)以及基于載波相位測量的RTK定位技術(shù)。

本文主要對單點(diǎn)定位以及載波相位定位進(jìn)行了初步研究。單點(diǎn)定位方法是利用一臺衛(wèi)星信號接收機(jī)根據(jù)偽距信息確定位置，載波相位定位方法則是利用載波相位測量技術(shù)，通過對相位差分確定位置[18]。

①偽距單點(diǎn)定位方法

目標(biāo)衛(wèi)星與響應(yīng)接收機(jī)之間的偽距等于衛(wèi)星信號從發(fā)射到到達(dá)對應(yīng)接收機(jī)的時間與光速的乘積，計(jì)算公式如式(1)所示。

式中:Δt為傳播時間，c為光速。

偽距單點(diǎn)定位首先獲取當(dāng)前時刻的廣播星歷，然后使用為衛(wèi)星信號解算算法確定相應(yīng)衛(wèi)星的位置[19]，接著計(jì)算得到衛(wèi)星信號接收機(jī)的到相應(yīng)衛(wèi)星之間的偽距值，最終確定目標(biāo)載體的位置坐標(biāo)。

②載波相位定位方法

將理想狀態(tài)的GPS時間作為標(biāo)準(zhǔn)，假設(shè)在歷元t j(gps)衛(wèi)星s j發(fā)射的載波信號相位為φj[t j(gps)]，此時在歷元t i(gps)接收機(jī)T i的參考載波信號為φi[t i(gps)]，那么兩者的載波相位差為:

考慮鐘差、大氣折射以及整周模糊度等因素，載波相位觀測方程最終可表示為:

由于目標(biāo)衛(wèi)星和對應(yīng)接收機(jī)之間的距離遠(yuǎn)大于接收站處兩個天線間的距離，可以忽略衛(wèi)星信號到對應(yīng)接收天線的干擾誤差。天線1和天線2可以同時測量到兩顆GPS衛(wèi)星k和GPS衛(wèi)星j的情況，如圖2所示，即載波相位測量采用雙差法，消除了接收機(jī)有關(guān)的載波相位及其鐘差。

圖2 GNSS雙差示意圖

沿載體的縱軸方向安裝兩根GPS天線1和2，其坐標(biāo)分別為(x1，y1，z1)和(x2，y2，z2)，利用載波相位測量方程可得天線1和2基線向量沿載體坐標(biāo)系的分量:

可以得載體的航向角和俯仰角為:

同時，通過對衛(wèi)星數(shù)據(jù)按照相應(yīng)的格式進(jìn)行解析，可以獲得物體運(yùn)動的經(jīng)緯度，在同一個度量基準(zhǔn)中，通過式(6)把大地坐標(biāo)系中的位置信息映射到空間直角坐標(biāo)系，映射關(guān)系如圖3所示。

圖3 空間直角坐標(biāo)系與大地坐標(biāo)

式中:e是橢球的第一偏心率，b是橢球的短半軸，b＝6 356.752 km。

1.2 GNSS定位模式對比實(shí)驗(yàn)

1.2.1 單點(diǎn)定位

使用中科微不同類型的單點(diǎn)定位模塊，包括5r、5s以及5p型號模塊，如圖4所示。

圖4 單點(diǎn)定位設(shè)備

進(jìn)行室外靜態(tài)定位精度測試，主要目的時觀測單點(diǎn)定位靜態(tài)情況下的定位誤差情況。測試地點(diǎn)為中科蘇州地理科學(xué)與技術(shù)研究院大廈5樓，測試環(huán)境如圖5所示，測試時長為2.5 h。

圖5 測試環(huán)境及靜態(tài)單點(diǎn)定位情況

測試結(jié)果顯示，單點(diǎn)定位東向最大偏差3.511 m，北向最大偏差4.442 m，東向平均偏差2.896 m，北向平均偏差2.448 m，北向平均偏差2.448 m，定位精度在米級，可見單點(diǎn)定位模式可用于對定位精度要求較低的場景，比如區(qū)分兩棟大樓的入口或者進(jìn)行米級模糊定位的場景。

1.2.2 RTK差分定位

使用RTK差分定位設(shè)備進(jìn)行室外靜態(tài)定位精度測試，測試設(shè)備如圖6所示，設(shè)備集成了GPS衛(wèi)星信號模塊、4G模塊，可以通過網(wǎng)絡(luò)RTK技術(shù)實(shí)現(xiàn)數(shù)據(jù)差分定位，獲得RTK定位數(shù)據(jù)。

圖6 RTK差分定位設(shè)備

測試地點(diǎn)為中科蘇州地理科學(xué)與技術(shù)研究院大廈6樓樓頂，采用上述RTK差分定位設(shè)備針對靜態(tài)情形下的RTK定位進(jìn)行定位精度的測試，測試時長為1 h，具體測試情況如圖7所示。

圖7 測試環(huán)境及靜態(tài)RTK定位情況

由圖7(b)可以看到，RTK靜態(tài)差分定位精度在mm級，當(dāng)動態(tài)定位時，雖然定位精度會有一定的下降，但厘米級定位已經(jīng)可以完成高精度定位的目的，進(jìn)一步驗(yàn)證了理論的準(zhǔn)確性。因此后續(xù)定位方案GNSS定位模式采用RTK差分定位，并輔以慣性導(dǎo)航進(jìn)行修正，并結(jié)合其他無線傳感器，通過多源融合濾波算法，最終完成室內(nèi)外無縫定位方案設(shè)計(jì)。

1.3 自身差分氣壓測高技術(shù)

大氣壓強(qiáng)是大氣狀態(tài)的一項(xiàng)關(guān)鍵指標(biāo)，氣壓能在一定程度上反應(yīng)對應(yīng)壓強(qiáng)下的高度，但氣壓具有周期性的日變化和年變化，以及非周期性的與天氣密切相關(guān)的變化，為了避免這種變化削弱氣壓測高的準(zhǔn)確性和穩(wěn)定性，本文采用自身差分氣壓測高技術(shù)獲得高度變化量，從而增強(qiáng)在信號遮擋沒有固定解的情況下的定位魯棒性。高度變化量為:

式中:T m為當(dāng)前氣溫，P i－1與P i分別是相鄰兩個時刻的氣壓值。

2 基于卡爾曼濾波的航向角修正算法設(shè)計(jì)

空曠的室外環(huán)境下，可以取得良好的GNSS信號，借助于RTK定位技術(shù)可以實(shí)現(xiàn)高精度的定位，但其信號的質(zhì)量會因定位環(huán)境的改變而發(fā)生改變，因此采用GNSS與慣導(dǎo)組合的方式實(shí)現(xiàn)室外的連續(xù)的定位。慣性導(dǎo)航長時間定位時會產(chǎn)生誤差的累積，且其定位的精確性主要依賴于航向角以及算法的設(shè)計(jì)，因此，為了改善GNSS組合INS的定位性能，提高定位的準(zhǔn)確性，提出了一種基于卡爾曼濾波的航向角修正算法，將RTK定位的航向角α與慣導(dǎo)定位航向角Δα用進(jìn)行組合處理，通過數(shù)據(jù)融合解算，降低航向角測定誤差。

2.1 GNSS/INS航向角修正算法實(shí)現(xiàn)

具體算法處理設(shè)計(jì)過程如下:

首先，令系統(tǒng)的狀態(tài)量X＝[αΔα]T，以線性運(yùn)動的載體為研究對象，建立線性狀態(tài)方程模型:

即

式中:A是狀態(tài)轉(zhuǎn)移矩陣，且是狀態(tài)噪聲。

取觀測量Y＝[αΔα]T，則觀測方程模型為:

式中:H是觀測矩陣，是觀測噪聲。

當(dāng)前RTK定位的絕對角度等于上一時刻RTK的絕對角度與慣導(dǎo)變化的相對角度之和，經(jīng)濾波算法處理后，即可得到組合航向角。

接下來利用組合航向角以及RTK定位信息、慣導(dǎo)定位信息進(jìn)行組合導(dǎo)航航向角修正算法設(shè)計(jì)?；贕NNS和INS兩種定位源，以卡爾曼濾波為例，令觀測矩陣和狀態(tài)轉(zhuǎn)移矩陣分別如式(12)和(13)所示。

狀態(tài)噪聲W和觀測噪聲V如式(14)及式(15)所示。

令系統(tǒng)狀態(tài)量X＝[x，v x，a x，y，v y，a y]，觀測量Y＝[x，y，v·cosα，v·sinα]，代入卡爾曼濾波方程，即可得到組合導(dǎo)航定位結(jié)果。

組合導(dǎo)航工作過程如圖8所示，首先終端處理設(shè)備會集成慣性測量單元以及衛(wèi)星信號接收機(jī)。使用GPS衛(wèi)星信號，采用慣性測量單元測量載體的姿態(tài)信息如加速度及角速度，通過衛(wèi)星信號接收機(jī)獲取當(dāng)前的經(jīng)度、緯度、高度以及速度、加速度等信息，然后通過終端設(shè)備上的數(shù)據(jù)融合處理單元進(jìn)行位置的預(yù)測和更新，獲得載體的運(yùn)動軌跡，實(shí)現(xiàn)定位。

圖8 組合導(dǎo)航位置預(yù)測更新框圖

組合導(dǎo)航整體處理框圖如圖9所示。

圖9 組合導(dǎo)航框圖

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

2.2.1 實(shí)驗(yàn)設(shè)備及解析數(shù)據(jù)格式

使用DHNAV－S300組合導(dǎo)航端機(jī)(內(nèi)置凌思陀螺儀、衛(wèi)星信號接收模塊)，進(jìn)行室內(nèi)外的無縫定位的算法設(shè)計(jì)及驗(yàn)證。DHNAV－S300可接收GNSS全系統(tǒng)多頻段信號，用來實(shí)現(xiàn)室外RTK定位。本文通過濾波算法將GNSS數(shù)據(jù)與慣導(dǎo)數(shù)據(jù)融合處理，可在GNSS信號失鎖情況下提供高精度的位置、航向、姿態(tài)、時間、信息。通過雙天線解算出載體相對正北的朝向夾角，同時可通過載波相位差分解算提供厘米級位置信息。具體實(shí)驗(yàn)設(shè)備如圖10所示，包括測試筆記本，實(shí)驗(yàn)車，組合導(dǎo)航端機(jī)、雙天線等。

圖10 測試設(shè)備

通過RS232轉(zhuǎn)UWB串口，將端機(jī)采集的數(shù)據(jù)保存到本地計(jì)算機(jī)上，主要處理的數(shù)據(jù)格式包括0183標(biāo)準(zhǔn)協(xié)議格式GPGGA、慣導(dǎo)信息GTIMU以及組合導(dǎo)航信息GPFPD。

2.2.2 航向角測試實(shí)驗(yàn)

測試地點(diǎn)在中科蘇州地理科學(xué)與技術(shù)研究院大廈四周及其地下車庫，測試路徑包括了室外部分以及室內(nèi)部分，室外情況下采用GNSS信號與IMU組合定位，室內(nèi)情況下GNSS信號完全失鎖，采用慣導(dǎo)和輪速計(jì)進(jìn)行軌跡外推定位。組合導(dǎo)航的航向角初始值由天線A指向天線B的絕對方向確定，本文按照雙天線安裝位置，根據(jù)安裝角度補(bǔ)償270°作為小車前進(jìn)的航向角。

首先對本小節(jié)提出的組合導(dǎo)航航向角修正算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)設(shè)備的定位軌跡如圖11所示，光纖陀螺所測的標(biāo)準(zhǔn)航向角如圖12所示，衛(wèi)導(dǎo)模塊所測航向角如圖13所示。為驗(yàn)證航向角測量精度，將衛(wèi)導(dǎo)模塊測試所得航向角與光纖陀螺測試結(jié)果進(jìn)行了對比，如圖14所示。

圖11 定位軌跡

圖12 光纖陀螺所測航向角

圖13 組合導(dǎo)航模塊測量航向角圖

圖14 航向角對比圖

分析定位及定向結(jié)果，由定位軌跡圖可以看出，定位軌跡非常平滑，沒有較大(大于1 m)跳點(diǎn)，在信號較好處均有RTK定位，由RTK定位精度(厘米量級)可推算出全程定位誤差均小于1 m。

比較定向結(jié)果與高精度光纖所測航向角，兩者變化趨勢基本一致，由于二者測量起始時間，輸出頻率，時鐘不一致，難以給出具體的誤差曲線，但比較各個拐點(diǎn)角度值(減去一個大約8°的安裝偏差角)及變化趨勢分析可知，平均定向精度小于1°。

同時，針對不同場景下航向角算法性能進(jìn)行了驗(yàn)證，室外航向角測試以及室外→室內(nèi)→室外航向角測試分別如圖15和16所示。

圖15 室外航向角測試

圖15的室外航向角測試中，由平緩以及拐彎變化的局部放大圖可以看到，室外衛(wèi)星信號良好的情況下，組合后的航向角以GNSS的航向角所在數(shù)據(jù)為中心，航向角變化更加平滑且變化趨勢也與慣導(dǎo)的航向角一致。

在圖16的室外→室內(nèi)→室外航向角測試中，由室外到室內(nèi)的局部放大圖可以看到，當(dāng)GNSS信號不穩(wěn)定或失鎖情況下，組合后的航向角以慣導(dǎo)的航向角為準(zhǔn)，由室內(nèi)到室外的局部放大圖可以看到，當(dāng)重新接收到良好的GNSS衛(wèi)星信號后，組合航向角能夠根據(jù)GNSS信號快速修正，回到準(zhǔn)確的位置，在室外室內(nèi)情況下都得到連續(xù)且準(zhǔn)確的運(yùn)動姿態(tài)信息。

圖16 室外→室內(nèi)→室外航向角測試

3 基于擴(kuò)展卡爾曼濾波的多源融合算法設(shè)計(jì)

目前，本文已使用的定位源包括UWB、INS、GNSS以及氣壓計(jì)，分別在不同場景下實(shí)現(xiàn)了定位或者參數(shù)修正的目的，接下來如何充分使用上述定位源，設(shè)計(jì)一個具有穩(wěn)定性高、定位精度好的融合算法是主要問題。

3.1 多源融合濾波算法實(shí)現(xiàn)

由融合濾波算法理論及仿真可得，擴(kuò)展卡爾曼濾波在處理非線性運(yùn)動問題與卡爾曼有較好的性能，并且與廣義延拓濾波相比，理論更加成熟。因此，本小節(jié)以擴(kuò)展卡爾曼濾波為基礎(chǔ)對融合定位算法設(shè)計(jì)研究。

基于擴(kuò)展卡爾曼濾波的信息融合示意圖如圖17所示。

圖17 擴(kuò)展卡爾曼濾波信息融合示意圖

多源信息融合中的擴(kuò)展卡爾曼狀態(tài)空間模型與觀測空間模型，如式(16)所示，式中，Hik表示不同的融合源在k時刻的系統(tǒng)狀態(tài)觀測系數(shù)。

本文基于UWB、GNSS、INS以及氣壓計(jì)融合定位源，通過擴(kuò)展卡爾曼濾波的完成位置更新處理。令設(shè)狀態(tài)向量X＝[x，y，xpdr，ypdr，xgnss，ygnss，xuwb，yuwb，Δh]T，(x，y)表示多源融合定位得到的位置坐標(biāo)，(xpdr，ypdr)為PDR定位解算的位置，(xgnss，ygnss)為GNSS定位解算的位置坐標(biāo)，(xuwb，yuwb)為超寬帶定位得到的位置坐標(biāo)，Δh為差分氣壓計(jì)解算的高度值。令觀測量Y＝[x，y，xpdr，ypdr，xgnss，ygnss，xuwb，yuwb，Δh]T。狀態(tài)方程如式(17)所示，觀測方程如式(18)所示，式中w i表示不同定位源對融合定位結(jié)果(x，y)的權(quán)值。

由文獻(xiàn)[2]可知，多元融合定位中，如果兩個及以上的定位源定位效果較好，則融合系統(tǒng)的性能就可優(yōu)于單一定位源；而如果只有一個定位源的定位效果遠(yuǎn)優(yōu)于其他定位源，則融合系統(tǒng)的性能就無法得到提升。根據(jù)定位源的置信度來判斷其在融合系統(tǒng)中的權(quán)值，可以削弱不理想定位源對整體定位效果的影響。另一方面，為了降低計(jì)算量從而提高導(dǎo)航定位的實(shí)時性，本文多源融合方法采用了基于多源置信度的滑動窗權(quán)重計(jì)算方法，置信度矩陣如式(19)所示。

式中:p i與ˉp分別為第i個定位源的定位結(jié)果與全部k個定位源定位結(jié)果平均值，對上述置信度矩陣進(jìn)行列歸一化處理，然后對每行求和后再進(jìn)行一次歸一化，得到置信度向量S＝[S1…S i…S k]T，S i即為第i個定位源在當(dāng)前位置的置信度，設(shè)滑動窗窗口和步長均為n，該值主要取決于慣性導(dǎo)航定位軌跡發(fā)散時間，誤差累積越慢，窗口長度越長。第i個定位源在一個滑動窗時間段內(nèi)的權(quán)重如式(20)所示。

狀態(tài)轉(zhuǎn)換矩陣A和觀測矩陣H分別如式(21)和式(22)所示。

狀態(tài)向量和測量向量的噪聲協(xié)方差矩陣Q和R分別如式(23)和式(24)所示。

當(dāng)各個參數(shù)的初始值確定后，代入到擴(kuò)展卡爾曼濾波的更新方程中，獲得相應(yīng)的估計(jì)值，并與測量值相結(jié)合，得到最優(yōu)估計(jì)值，從而獲取最終的融合定位結(jié)果。

在開闊的室外環(huán)境下，RTK的定位精度能夠達(dá)到厘米級，而在室內(nèi)環(huán)境中，UWB能夠也能夠?qū)崿F(xiàn)厘米級的定位。為了充分地合理地利用高精度定位源，我們設(shè)置了重置時間間隔，其值與上述權(quán)重計(jì)算中滑動窗窗長一致，在超出重置時間間隔后，如果存在高信噪比的GNSS定位結(jié)果或高信號強(qiáng)度的UWB定位結(jié)果，我們會以此為初始值開始新一輪的融合定位。

3.2 多源融合算法對比

在本小節(jié)中，將采集獲得的實(shí)驗(yàn)數(shù)據(jù)通過MATLAB進(jìn)行對其他多源融合算法的分析對比。在文獻(xiàn)[17]通過實(shí)驗(yàn)驗(yàn)證了聯(lián)邦式卡爾曼濾波比集中式卡爾曼濾波具有更強(qiáng)的抗干擾能力，故實(shí)驗(yàn)中我們采用聯(lián)邦式卡爾曼濾波進(jìn)行對比，此外我們還對比了信號質(zhì)量加權(quán)融合方法[16]。

我們在建筑物內(nèi)設(shè)置了4個基站提供UWB信號，配備有組合導(dǎo)航終端的實(shí)驗(yàn)車的運(yùn)行軌跡為繞建筑物順時針一周，并在中途駛?cè)虢ㄖ飪?nèi)以驗(yàn)證室內(nèi)外切換的定位性能，其起始點(diǎn)位置在圖18中坐標(biāo)為(5，5)。

圖18 多源融合算法對比軌跡圖

由圖18分析對比軌跡可知，三種算法的在室內(nèi)外切換的情況下均可以做到軌跡連貫平滑不出現(xiàn)較大的跳變，另一方面，由于室外GNSS信號不穩(wěn)定，信號質(zhì)量加權(quán)融合方法的定位軌跡受其影響較大，而本文算法與聯(lián)邦式卡爾曼濾波則能從一定程度上避免受其影響。由表1中定位誤差的平均值與標(biāo)準(zhǔn)差可得，本文算法在定位精度與穩(wěn)定性均優(yōu)于其他兩種算法。表中平均運(yùn)行時間是由整體運(yùn)行時間除于定位點(diǎn)個數(shù)計(jì)算得到的，可以得知，信號質(zhì)量加權(quán)融合用時最少，而其余兩者的用時比較接近，信號質(zhì)量加權(quán)融合的方法雖然有最低的運(yùn)行時間，但其最大定位誤差有1.225 m，定位軌跡跳變明顯。本文提出的多源融合室內(nèi)外無縫定位方法能夠在較低的時延下獲得平均定位誤差0.299 m的定位效果。

表1 多源融合算法定位誤差及復(fù)雜度對比

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

接下來進(jìn)行整體的定位測試，首先在測試定位環(huán)境中部署足夠的UWB基站，然后按照一定的實(shí)驗(yàn)路徑行駛，定位終端設(shè)備包括計(jì)算機(jī)、UWB標(biāo)簽以及差分氣壓測高模塊移動站，將定位終端搭載在無人車上，控制在中科院地理信息產(chǎn)業(yè)樓周圍繞行，周圍有灌木樓宇遮擋，并進(jìn)入地下車庫測試信號完全失鎖情況下定位情況，最后使用計(jì)算機(jī)將各組定位源數(shù)據(jù)通過融合算法進(jìn)行融合處理。實(shí)驗(yàn)路徑及定位場景如圖19和圖20所示，實(shí)驗(yàn)路徑包括室外和室內(nèi)部分，分別用不同的線型進(jìn)行了表示，整體測試路線為室外→室內(nèi)→室外。

圖19 實(shí)驗(yàn)場景平面示意圖

圖20 地下車庫測試場景

定位數(shù)據(jù)通過RS232轉(zhuǎn)USB串口輸出到計(jì)算機(jī)上進(jìn)行同步的定位解算。獲取到的定位信息包括試驗(yàn)車的運(yùn)動的經(jīng)緯度、位置坐標(biāo)、速度、航向角、姿態(tài)信息等等，將獲取的經(jīng)緯度數(shù)據(jù)導(dǎo)入谷歌地圖觀察多源融合導(dǎo)航定位以及單一的GNSS定位情況，如圖21所示?？梢钥吹?，當(dāng)實(shí)驗(yàn)車進(jìn)入地下車庫后，GNSS定位消失，多源融合導(dǎo)航依舊有高精度的定位信息，當(dāng)實(shí)驗(yàn)車行駛出車庫后，GNSS信號重新捕獲，GNSS定位從單點(diǎn)定位逐漸過渡到高精度RTK定位，將融合導(dǎo)航定位重新拉回到準(zhǔn)確值，實(shí)現(xiàn)連續(xù)平穩(wěn)且準(zhǔn)確的定位輸出。軌跡曲線平滑無跳點(diǎn)，并且能清晰分辨出進(jìn)出車庫口軌跡。在車庫內(nèi)信號完全丟失情況下依然能提供高精度的定位數(shù)據(jù)，信號重新捕獲后能與定位軌跡有較好的銜接。

圖21 樓宇遮擋及車庫環(huán)境測試

4 結(jié)論

本文為整體上實(shí)現(xiàn)室內(nèi)外無縫定位的目的，首先提出了一種多源融合室內(nèi)外定位方案，并對GNSS定位模式進(jìn)行了實(shí)驗(yàn)驗(yàn)證。然后，基于不同場景對定位算法進(jìn)行了研究:室外情況下，首先采用GNSS組合慣性導(dǎo)航進(jìn)行定位解算，并提出了一種基于濾波方法的航向角修正算法。進(jìn)入到室內(nèi)后，采用作者提出的PDR輔助UWB的多源融合室內(nèi)定位方法，并結(jié)合氣壓計(jì)進(jìn)行定位高度方向的修正。同時，提出了一種新的融合定位濾波算法將不同定位源的數(shù)據(jù)相融合，實(shí)現(xiàn)多源融合定位。測試顯示，該方法在室外情況下能夠?qū)崿F(xiàn)厘米級定位，在室內(nèi)外過渡階段，依舊可以保持連續(xù)準(zhǔn)確的定位，在室內(nèi)GNSS衛(wèi)星信號丟失的場景中，仍然能夠?qū)崿F(xiàn)分米級定位。在接下來的工作中，我們會探究不同場景下多源融合定位的定位表現(xiàn)，以期實(shí)現(xiàn)智能識別多場景的高精度多源融合定位。