一種新型的INS與UWB融合的室內行人跟蹤方法*

吳翠先,周文敏,劉期烈,萬志鵬

(1.重慶郵電大學 a.通信與信息工程學院;b.通信新技術應用研究中心，重慶 400065;2.重慶信科設計有限公司，重慶 401121)

0 引 言

近年來，隨著無線技術的快速發展，基于室內場景的定位服務(如超市購物導航、火災緊急定位、住院病人位置追蹤等)已成為眾多物聯網應用的基本需求。為了解決室內定位問題，催生了如無線上網(WiFi)、慣性導航(Inertial Navigation System,INS)、射頻識別(Radio Frequency Identification，RFID)、藍牙、超寬帶(Ultra-wide Band，UWB)等一系列針對室內場景的定位技術[1-2]。

為了進一步提高室內定位系統(Indoor Position System,IPS)的魯棒性和定位精度，基于多源數據融合的組合定位系統成為研究的熱點[3]。文獻[4-5]研究了不同組合定位系統中的各種定位性能和可用性，具有優勢互補特性和高精度的INS與UWB組合定位成為組合導航的最佳選擇。

為了實現多種定位系統的融合，就必須考慮一種或多種濾波算法對多源數據融合。目前對濾波算法的研究已經取得了很多有價值的研究成果[6-14]。但這些研究中，一方面主要利用傳統的貝葉斯濾波算法來減輕外界噪聲，然而最接近貝葉斯估計的容積卡爾曼濾波算法很少應用于室內組合定位中；另一方面在實際復雜的定位環境中，外界噪聲的不確定性會造成定位系統的測量值異常，而且實際物體的無規律運動或狀態突變也會給定位系統的建模造成很大的影響。所以一種既能夠濾除測量噪聲又能適應無規律運動而造成的模型誤差的濾波算法，成為精確定位的關鍵。

本文提出了一種針對室內行人定位的有效的易于實現的UWB-INS組合定位框架和一種新的改進容積卡爾曼濾波算法。Matlab仿真結果表明，UWB-INS融合室內行人跟蹤算法在定位精度、軌跡效果明顯優于其他算法。

1 系統建模

對于UWB-INS組合定位問題，關鍵是如何根據運動目標的運動模型、測量模型、噪聲特征等先驗信息有效合理地融合兩套系統產生的數據，從而獲得更精確的位置信息。本文重點研究2D平面中的室內行人的位置跟蹤問題，由狀態變量xpose=(sT,vT)表示行人運動信息，其中s代表二維空間的目標載體的位置坐標，v代表二維空間的速度。

xk=f(xk-1,uk-1)+wk-1，

(1)

zk=h(xk)+vk。

(2)

式中：xk是與傳輸函數f(·)的相關的狀態向量，zk是通過測量函數h(·)得到的測量向量，u是控制輸入向量，w和v分別表示過程噪聲和測量噪聲。

1.1 運動模型

本文采用四元數法表示狀態向量中的姿態角。與歐拉角相比，四元數法不會受到萬向鎖的影響，并且與旋轉量有一一對應關系。此外，旋轉矩陣也被用到本文中，并以四元數的形式表示如下：

(3)

(4)

(5)

(6)

式中：g表示{N}坐標系中的重力加速度，am和ωm分別為加速度測量值和角速度測量值，an和ωn分別為加速度計和陀螺儀的測量噪聲，aω和ωω分別為加速計和陀螺儀的內部器件偏差。

1.2 測量模型

由于UWB在視距下具有很高的定位精度，所以在UWB-INS的組合定位系統中，UWB可緩解INS產生的累積誤差。為了提高UWB的定位精度，可以采用多個UWB基站進行定位。本文采用最小二乘法(Least Square,LS)處理多基站定位過程中出現的超定方程問題，從而獲得對標簽位置的精確估計。

假設在導航坐標系中UWB基站的位置向量為sn=[xn,yn,zn]T,n∈{1,2,3,…,N}表示基站的序號，N是基站總數。假設在k時刻UWB標簽的位置向量為sm,k=[xm,k,ym,k,zm,k]T，UWB標簽到n個UWB基站的距離可表示為

ln,k=‖sn-sm,k‖2=

(7)

類似地，如果已知st,k的真實位置，那么它到基站的真實距離可以表示為

(8)

(9)

注意：N需要大于未知向量的維數。

從等式(6)可以得到

(10)

將式(10)的第2～n行分別減去第一行并進行線性化得到以下等式：

2Gsm,k=bk，

(11)

(12)

(13)

利用最小二乘法對式(11)求解得

(14)

最終可得UWB標簽的速度為

(15)

假設在k時刻INS的速度和位置分別為sINS和vINS，于是融合導航系統的測量誤差方程為

(16)

2 基于動態魯棒容積卡爾曼濾波的INS/UWB融合導航系統

本文提出了一種UWB-INS融合導航的系統框架，融合了UWB系統和INS系統產生的位置數據來實現精確定位,其主要系統框架如圖1所示,α、ω、ψ分別為慣性測量單元測得的加速度、角速度、磁場強度，并作為INS的輸入。利用三軸磁力計的測量值來對INS的方向進行初始化，使用遞歸積分方法來獲得載體的位置、速度、姿態角，利用基于UWB標簽和UWB基站的距離的TOA算法來獲得導航坐標的測量值。在UWB-INS融合導航系統中，利用UWB的測量值來矯正INS的輸出。然而室內環境通常比較復雜，UWB測量結果容易受到多徑效應和非視距因素的影響。這會帶來異常值，進而造成定位性能下降甚至是濾波發散。另外，行人的不確定性運動會給系統建模帶來模型誤差，進而導致傳統濾波算法預測精度低、穩定性差。

為了解決上述問題，本文利用強跟蹤算法、Huber M估計理論和增強策略，提出了一種動態增強魯棒容積卡爾曼算法(Dynamic Robust Cubature Kalman Filtering,DRCKF)。標準容積卡曼算法因為不需要計算雅可比矩陣和海森矩陣，易于建立且復雜度較低，滿足實時快速狀態估計的要求。但是該濾波器只是適用于確定模型的濾波，在非確定性模型下，該濾波器的性能嚴重下降。為了克服上述問題，在容積卡爾曼濾波(Cubature Kalman Filter，CKF)的預測階段的預測誤差協方差矩陣中引入了一個動態增強因子，來減少在非線性不確定模型的系統中CKF算法所產生的模型誤差。但是CKF算法中除了存在模型誤差外，還存在由于外界噪聲或測量異常值造成的系統異常。本算法在引入動態增強因子基礎上，又引入了Huber M估計來檢測并校正外界噪聲誤差造成的異常值。動態增強魯棒容積卡爾曼算法的具體實現如圖1所示,其中zz表示模型誤差。

圖1 超寬帶與慣性導航系統融合導航的系統框架

2.1 標準容積卡爾曼濾波

(1)時間預測

(17)

(18)

(19)

(20)

(21)

(2)測量更新

(22)

(23)

Zi,k+1|k=h(Xi,k+1|k,uk)，

(24)

(25)

(26)

(27)

(28)

(29)

(30)

2.2 動態魯棒容積卡爾曼濾波算法

在復雜的室內環境中，傳統的容積卡爾曼濾波算法需要準確的系統模型和測量噪聲特性才能很好地工作，而實際運動模型的不確定性和噪聲特性的多變性可能會導致標準的容積卡爾曼濾波算法精度低和穩定性差。因此，本文提出一種新型的動態魯棒容積卡爾曼濾波算法：首先，追蹤目標的運動模型并進行實時調整，緩解模型的不確定誤差；然后，識別測量值中的異常值并加以矯正，以此來獲得精確穩定的估計。

2.2.1 運動模型誤差的處理

為了緩解行人運動模型的不確定誤差，所提算法在傳統標準卡爾曼算法的基礎上引入了一種強跟蹤算法。通過引入一種增強因子λk，從先前的狀態估計中提取有用的信息并且根據系統模型的不同情況實時調整增益矩陣。

根據強跟蹤算法，在式(21)中構建一個新的協方差矩陣為

(31)

從而λk可通過以下等式求得：

(32)

式(32)第一行是濾波器的性能指數,第二行表示新舊的狀態變量差值的正交關系，這能夠確保有效信息被提取出來，并且確定濾波器性能是否最優。因篇幅所限，λk的具體推導過程略，可通過微信掃描本文開放科學(資源服務)標識碼(OSID)查看。

2.2.2 測量異常值的處理

UWB或INS的測量值中出現異常值時，式(26)中測量噪聲協方差Rk+1將與實際誤差不一致，導致式(26)中協方差矩陣Pzz,k+1|k無法反映預測值的偏差。這將最終導致估計的不準確，限制了標準容積卡曼算法在實際應用中的實用性。為了對測量值的異常值進行檢測和矯正，采用Huber的M估計方法對以下目標函數進行最小化，可以得到去除異常值的狀態估計結果：

(33)

(34)

式中：參數c通常設為1.5來獲得高統計效率。

為了使目標函數(34)取得最小值，令式(33)關于xk的一階偏導等于0，于是可以求得最優解的充分必要條件：

(35)

式中：Ψ(rsi)=?ρ(rsi)/?rsi，xk,i表示狀態向量的第i個分量。

定義gi=Ψ(rsi)/rsi，則有

(36)

基于上式，同等權重矩陣可以表示為

(37)

(38)

(39)

2.2.3 增強策略

當測量模型存在不確定性時，引入增強因子，改進式(21)中的誤差協方差矩陣，可以確保更準確的估計結果。因此，我們提出了一個增強策略(不確定性檢測算法)，在適當的時候調整狀態變量的誤差協方差矩陣:

(40)

式中：Yk+1為新的更新值，因篇幅所限具體表達式未給出，可通過微信掃描本文OSID碼查看。

本文定義以下兩種測量場景：

情景1 C0:估計系統正常工作。

情景2 C1:估計系統包含模型不確定性。

(41)

(42)

3 仿真與分析

3.1 硬件設計

本文使用的慣導模塊是由荷蘭Xses公司開發的MTI300。該模塊集成了三軸加速計、三軸陀螺儀和三軸磁力計，可以測得三維空間下物體的姿態角、加速度值以及所在的磁場強度,其數據刷新率最高可達400 Hz。該模塊體積小、精度高，非常適合行人定位。該模塊具體參數如表1所示。UWB定位系統采用的是由意法半導體公司開發的UWB MINI3s。該定位模塊采用STM32F105RCT6單片機為主控芯片，通過SPI讀寫DWM1000天線模塊間的測距通信信息。該系統主要有1個標簽和4個基站組成，采用TOA(Time of Arrival)方法來進行測距，數據刷新率最高可達10 Hz。在該系統上擴展了無線WiFi模塊來實現標簽與上位機的無線通信。

表1 INS的性能參數

測試環境如圖2所示，將慣導模塊安裝在行人的腳尖，而UWB的模塊由行人手持或者安裝在頭部。當行人行走時，UWB定位系統和慣導模塊的定位數據分別通過WiFi和藍牙傳入電腦上的上位機，由上位機解算出位置坐標并顯示行人的軌跡。

圖2 測試環境

3.2 測量與仿真結果

分別在視距環境(Line-of-Sight，LOS)和在非視距環境(Non-line-of-sight,NLOS)下，通過測試評估和仿真對所提出算法的性能進行驗證。首先，將UWB-INS組合系統分別與單一UWB和INS系統的定位性能進行比較；然后，將所提出的數據融合算法與傳統的擴展卡爾曼濾波(Extended Kalman Filter,EKF)、CKF算法的性能進行比較。在仿真中，假設標簽以位置(0.6，0.6)m作為起始位置，并以水平和垂直速度都為0.30 m/s在二維平面x-y上運動。采樣周期為T=0.28 s，總的運行時間為24 s。

3.2.1 LOS 下定位結果

在LOS下，UWB、INS和UWB-INS組合定位系統的定位軌跡和均方根誤差分別如圖3和圖4所示。圖3中顯示UWB會出現定位點缺失，個別點偏離嚴重，導致總體軌跡偏差較嚴重。而從圖4中可得知，UWB的均方根誤差較為不穩定，最大可達41 cm,而最小為10 cm。INS定位效果明顯比UWB定位效果要好，但是與真實軌跡相比還有一定的偏差，而且隨著時間的推移，INS與真實軌跡的偏差程度逐漸增大。而UWB-INS組合定位系統的軌跡效果明顯優于單一定位系統，定位精度在10 cm左右，長期定位效果更加穩定，更加接近真實軌跡。

圖3 LOS下行人定位軌跡

圖4 LOS下的均方根誤差對比圖

將標簽放置在頭頂，慣導安裝在腳尖，在圖2所示的環境下行走。在這里每一個小格長寬都為0.6 m，行人從(0.6,0.6)m出發，以0.3 m/s的速度依次通過(0.6,4.8)m、(3,4.8)m、(3,0.6)m，最終到達起始點(0.6,0.6)m。經過不同濾波算法優化的行人動態軌跡如圖5所示,可以看出在LOS下經過濾波算法的行人軌跡明顯更接近真實軌跡。LOS下不同濾波算法的軌跡估計誤差如圖6所示，可見CKF估計效果較差，最大誤差能達到23 cm,而且該算法的定位效果并不穩定，出現的異常值點較為嚴重。雖然EKF在起始階段的估計效果比較好，但從(3,4.8)m到(3,0.6)m時存在著較大偏差。相比前兩種算法，所提的DRCKF算法的優化效果更加明顯，更加接近真實軌跡。表2也顯示，DRCKF算法最大估計誤差只有10 cm，相比其他濾波算法定位效果有了很大提升。

圖5 LOS下優化后的行人軌跡

圖6 LOS下的定位估計誤差

表2 LOS下不同濾波算法的估計誤差對比

3.2.2 NLOS下定位結果

為了體現所提出算法的魯棒性和精確性，需要在NLOS環境下做進一步測試。通過在行人的真實路線中設置一些障礙物或者人為遮擋基站來制造NLOS環境。在NLOS下，UWB、INS和UWB-INS組合定位系統的定位軌跡和均方根誤差分別如圖7和圖8所示。從圖7可知，當基站受到遮擋或用人體對基站進行遮擋時，單一的UWB定位效果比較差。從圖8可知，UWB的最大均方誤差可達50 cm以上，移動軌跡中的定位點跳動性比較大，與實際路線的吻合度變差；單一的INS雖然有一定的累積誤差，但定位誤差基本上在20 cm以下。

圖7 NLOS下行人定位軌跡

圖8 NLOS下均方根誤差對比圖

不同濾波算法的下優化后的行人軌跡如圖9所示。從圖中可知，CKF的估計效果較差，最大估計誤差可達50 cm,而經過EKF優化的路線軌跡雖然優于CKF，并且對定位過程中產生的異常值有一定的魯棒性，與實際軌跡仍有較大偏差。從表3也可以看出，DRCKF對于異常值具有更好地魯棒性，對于UWB系統與慣導系統之間的偏差估計更加準確，估計誤差在15 cm以下，最終優化后的軌跡與實際軌跡更加貼合，進一步證明了該算法能夠適應模型誤差和噪聲較大的環境。

表3 非視距下不同濾波算法的估計誤差對比

為了驗證本文所提的動態魯棒容積卡爾曼濾波算法在復雜環境下的定位性能，我們采用了標準容積卡爾曼算法、擴展卡爾曼濾波算法與本文所提濾波算法進行對比。本文所提濾波算法的復雜度主要與迭代次數有關,考慮到實時性的效果，一般迭代50～100次可以達到比較好的定位效果。在實際測試中，DRCKF算法基本能到達到實時估計的效果，算法復雜度基本滿足實時定位需求。

4 結束語

為了滿足某些復雜場景(醫院病患位置追蹤、監獄犯人位置監控、礦工位置跟蹤等)下的定位需求，本文通過分析單一的UWB或INS定位系統的優缺點，闡明了融合定位系統的突出定位優勢，在此基礎上提出了一種融合定位框架來克服單一定位系統的定位缺陷。為了實現兩套系統基礎數據的融合，減少測量噪聲、系統模型不確定性誤差對融合系統的狀態估計的不利影響，提出了一種動態增強魯棒容積卡爾曼濾波算法。利用UWB-INS硬件定位系統，分別在視距和非視距環境下獲取行人的運動軌跡數據。Matlab仿真和實驗結果證明了融合定位系統的定位性能明顯優于單一定位系統，而且所提出的數據融合算法在定位精度和魯棒性上優于傳統的濾波算法。另外，兩套系統融合必然會增加系統的能耗，所以降低融合定位系統測能耗和硬件成本將成為未來融合定位研究的一個重要熱點。