一種多源融合室內外無縫定位方法*

姚子揚尚俊娜*孫建強肖江寧王奕騰

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

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

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

基于以上背景，為了在常用設備的基礎上進一步探究如何實現室內外無縫切換的高精度導航定位，本文主要對氣壓計、UWB定位、慣性導航定位以及GNSS定位的多源融合定位技術進行研究，設計實現了相應的組合導航定位算法，并最終完成多源融合定位。

1 多源融合室內外定位方案

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

圖1 多源融合室內外定位設計框圖

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

1.1 GNSS定位技術

隨著全球衛星數量的增加以及定位系統的完善，基本可以實現全天候、實時的室外定位。根據不同的劃分指標，GNSS可以分為不同的定位方式，其中最為常用的是基于偽距的單點定位技術以及基于載波相位測量的RTK定位技術。

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

①偽距單點定位方法

目標衛星與響應接收機之間的偽距等于衛星信號從發射到到達對應接收機的時間與光速的乘積，計算公式如式(1)所示。

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

偽距單點定位首先獲取當前時刻的廣播星歷，然后使用為衛星信號解算算法確定相應衛星的位置[19]，接著計算得到衛星信號接收機的到相應衛星之間的偽距值，最終確定目標載體的位置坐標。

②載波相位定位方法

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

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

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

圖2 GNSS雙差示意圖

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

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

同時，通過對衛星數據按照相應的格式進行解析，可以獲得物體運動的經緯度，在同一個度量基準中，通過式(6)把大地坐標系中的位置信息映射到空間直角坐標系，映射關系如圖3所示。

圖3 空間直角坐標系與大地坐標

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

1.2 GNSS定位模式對比實驗

1.2.1 單點定位

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

圖4 單點定位設備

進行室外靜態定位精度測試，主要目的時觀測單點定位靜態情況下的定位誤差情況。測試地點為中科蘇州地理科學與技術研究院大廈5樓，測試環境如圖5所示，測試時長為2.5 h。

圖5 測試環境及靜態單點定位情況

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

1.2.2 RTK差分定位

使用RTK差分定位設備進行室外靜態定位精度測試，測試設備如圖6所示，設備集成了GPS衛星信號模塊、4G模塊，可以通過網絡RTK技術實現數據差分定位，獲得RTK定位數據。

圖6 RTK差分定位設備

測試地點為中科蘇州地理科學與技術研究院大廈6樓樓頂，采用上述RTK差分定位設備針對靜態情形下的RTK定位進行定位精度的測試，測試時長為1 h，具體測試情況如圖7所示。

圖7 測試環境及靜態RTK定位情況

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

1.3 自身差分氣壓測高技術

大氣壓強是大氣狀態的一項關鍵指標，氣壓能在一定程度上反應對應壓強下的高度，但氣壓具有周期性的日變化和年變化，以及非周期性的與天氣密切相關的變化，為了避免這種變化削弱氣壓測高的準確性和穩定性，本文采用自身差分氣壓測高技術獲得高度變化量，從而增強在信號遮擋沒有固定解的情況下的定位魯棒性。高度變化量為:

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

2 基于卡爾曼濾波的航向角修正算法設計

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

2.1 GNSS/INS航向角修正算法實現

具體算法處理設計過程如下:

首先，令系統的狀態量X＝[αΔα]T，以線性運動的載體為研究對象，建立線性狀態方程模型:

即

式中:A是狀態轉移矩陣，且是狀態噪聲。

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

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

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

接下來利用組合航向角以及RTK定位信息、慣導定位信息進行組合導航航向角修正算法設計。基于GNNS和INS兩種定位源，以卡爾曼濾波為例，令觀測矩陣和狀態轉移矩陣分別如式(12)和(13)所示。

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

令系統狀態量X＝[x，v x，a x，y，v y，a y]，觀測量Y＝[x，y，v·cosα，v·sinα]，代入卡爾曼濾波方程，即可得到組合導航定位結果。

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

圖8 組合導航位置預測更新框圖

組合導航整體處理框圖如圖9所示。

圖9 組合導航框圖

2.2 實驗驗證與結果分析

2.2.1 實驗設備及解析數據格式

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

圖10 測試設備

通過RS232轉UWB串口，將端機采集的數據保存到本地計算機上，主要處理的數據格式包括0183標準協議格式GPGGA、慣導信息GTIMU以及組合導航信息GPFPD。

2.2.2 航向角測試實驗

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

首先對本小節提出的組合導航航向角修正算法進行了實驗驗證，實驗設備的定位軌跡如圖11所示，光纖陀螺所測的標準航向角如圖12所示，衛導模塊所測航向角如圖13所示。為驗證航向角測量精度，將衛導模塊測試所得航向角與光纖陀螺測試結果進行了對比，如圖14所示。

圖11 定位軌跡

圖12 光纖陀螺所測航向角

圖13 組合導航模塊測量航向角圖

圖14 航向角對比圖

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

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

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

圖15 室外航向角測試

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

在圖16的室外→室內→室外航向角測試中，由室外到室內的局部放大圖可以看到，當GNSS信號不穩定或失鎖情況下，組合后的航向角以慣導的航向角為準，由室內到室外的局部放大圖可以看到，當重新接收到良好的GNSS衛星信號后，組合航向角能夠根據GNSS信號快速修正，回到準確的位置，在室外室內情況下都得到連續且準確的運動姿態信息。

圖16 室外→室內→室外航向角測試

3 基于擴展卡爾曼濾波的多源融合算法設計

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

3.1 多源融合濾波算法實現

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

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

圖17 擴展卡爾曼濾波信息融合示意圖

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

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

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

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

狀態轉換矩陣A和觀測矩陣H分別如式(21)和式(22)所示。

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

當各個參數的初始值確定后，代入到擴展卡爾曼濾波的更新方程中，獲得相應的估計值，并與測量值相結合，得到最優估計值，從而獲取最終的融合定位結果。

在開闊的室外環境下，RTK的定位精度能夠達到厘米級，而在室內環境中，UWB能夠也能夠實現厘米級的定位。為了充分地合理地利用高精度定位源，我們設置了重置時間間隔，其值與上述權重計算中滑動窗窗長一致，在超出重置時間間隔后，如果存在高信噪比的GNSS定位結果或高信號強度的UWB定位結果，我們會以此為初始值開始新一輪的融合定位。

3.2 多源融合算法對比

在本小節中，將采集獲得的實驗數據通過MATLAB進行對其他多源融合算法的分析對比。在文獻[17]通過實驗驗證了聯邦式卡爾曼濾波比集中式卡爾曼濾波具有更強的抗干擾能力，故實驗中我們采用聯邦式卡爾曼濾波進行對比，此外我們還對比了信號質量加權融合方法[16]。

我們在建筑物內設置了4個基站提供UWB信號，配備有組合導航終端的實驗車的運行軌跡為繞建筑物順時針一周，并在中途駛入建筑物內以驗證室內外切換的定位性能，其起始點位置在圖18中坐標為(5，5)。

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

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

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

3.3 實驗驗證與結果分析

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

圖19 實驗場景平面示意圖

圖20 地下車庫測試場景

定位數據通過RS232轉USB串口輸出到計算機上進行同步的定位解算。獲取到的定位信息包括試驗車的運動的經緯度、位置坐標、速度、航向角、姿態信息等等，將獲取的經緯度數據導入谷歌地圖觀察多源融合導航定位以及單一的GNSS定位情況，如圖21所示。可以看到，當實驗車進入地下車庫后，GNSS定位消失，多源融合導航依舊有高精度的定位信息，當實驗車行駛出車庫后，GNSS信號重新捕獲，GNSS定位從單點定位逐漸過渡到高精度RTK定位，將融合導航定位重新拉回到準確值，實現連續平穩且準確的定位輸出。軌跡曲線平滑無跳點，并且能清晰分辨出進出車庫口軌跡。在車庫內信號完全丟失情況下依然能提供高精度的定位數據，信號重新捕獲后能與定位軌跡有較好的銜接。

圖21 樓宇遮擋及車庫環境測試

4 結論

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