韓 天，郝 敏，李傳苗，趙雄偉，繆存孝

(北京科技大學(xué)機械工程學(xué)院，北京 100083)

0 引言

隨著通信技術(shù)的高速發(fā)展，室內(nèi)定位技術(shù)在監(jiān)控管理、無人駕駛等領(lǐng)域占據(jù)越來越重要的地位，可以為運動目標(biāo)提供準確位置信息的室內(nèi)定位技術(shù)越來越成為人們關(guān)注的焦點[1-5]。慣性導(dǎo)航技術(shù)是最早被應(yīng)用到室內(nèi)定位領(lǐng)域的導(dǎo)航技術(shù)，國內(nèi)外學(xué)者針對基于慣性導(dǎo)航的定位算法做了大量的研究，但是由于慣性導(dǎo)航存在累積誤差，所以并不適合長時間定位[6]。超寬帶(Ultra-wideband，UWB)是一種新型的無線通信技術(shù)，相比于傳統(tǒng)的無線通信技術(shù)，UWB具有功率譜低、保密性強、功耗低等優(yōu)點[7-8]。自2010年以來，已有幾種成熟的UWB無線電傳感器定位設(shè)備能夠提供厘米級的定位，例如DecaWave[9]的DWM1000和KickStarter的Pozyx。與其他室內(nèi)定位技術(shù)相比，UWB定位通過發(fā)送納秒(ns)或微秒(μs)非正弦窄脈沖可實現(xiàn)高精度解決方案，而不會產(chǎn)生誤差累積[10]。UWB信號容易被外界環(huán)境所干擾，而IMU的INS對這些干擾不敏感，在復(fù)雜環(huán)境中難以使用單一類型的傳感器滿足需求，所以為了克服多徑效應(yīng)，UWB通常需要與慣導(dǎo)融合。

M?kel?等[11]討論了傳感器融合技術(shù)，用于利用室內(nèi)導(dǎo)航安裝的IMU、氣壓計和UWB，它們可以在地板上獲得準確的位置，在現(xiàn)實的戰(zhàn)術(shù)測試場景中顯示。Sczyslo等使用了松散組合法，基于擴展Kalman濾波器來跟蹤行人的運動[12-16]。有學(xué)者提出了一種緊密耦合的方法，該方法結(jié)合了UWB范圍和INS的測量[17-19]。文獻[20]設(shè)計了一種用于無人機室內(nèi)導(dǎo)航的定位方法，該方法集成了3D激光掃描儀、UWB和INS的信息，該策略可改善定位效果，與僅INS和僅UWB的方法相比，準確性顯著提高。文獻[21]設(shè)計了一種使用松散耦合EKF算法的UWB和IMU集成系統(tǒng)，在NLOS和UWB信號不足或不可用的情況下，有效地提高了定位的精度和穩(wěn)健性。北京航空航天大學(xué)的儀玉杰等[22]針對在復(fù)雜室內(nèi)環(huán)境下單純依靠UWB技術(shù)定位結(jié)果會嚴重失真甚至缺失的問題，提出了一種將UWB與行人航跡推算方法(PDR)相結(jié)合的方法，解決了在復(fù)雜室內(nèi)環(huán)境下定位解算點可能缺失的問題，并且較顯著地提高了在復(fù)雜環(huán)境下定位系統(tǒng)的魯棒性和定位精度。文獻[23]利用安裝在移動節(jié)點上的UWB模塊產(chǎn)生位置的估計值，以校正慣導(dǎo)的航位推算法所得到的移動節(jié)點的位置坐標(biāo)校正慣導(dǎo)中由于陀螺漂移導(dǎo)致的累積定位誤差。文獻[24]基于UWB結(jié)合IMU的室內(nèi)定位設(shè)計方法提出了一種定位技術(shù)，可以有效抑制IMU的誤差積累，進一步提高了定位精度。文獻[25]針對UWB和IMU融合車輛的定位問題，提出了一種約束魯棒迭代擴展Kalman濾波算法，克服了針對非Gauss噪聲先天缺陷的擴展Kalman濾波器算法的缺點，定位精度達到0.21m。

盡管這些方法確實可以提高定位精度，但都沒有考慮基站信號的丟失對系統(tǒng)產(chǎn)生的影響。單一的定位系統(tǒng)或精度不夠、或應(yīng)用場景受限等，無法達到實際的應(yīng)用要求。為提高室內(nèi)定位精度、克服多徑效應(yīng)，本文首先開展了UWB和慣性導(dǎo)航融合定位方法研究。同時，為了解決基站信號丟失對系統(tǒng)產(chǎn)生的影響，先基于改進馬氏距離對測距異常值進行剔除，后引入了協(xié)方差矩陣跡作為基站選擇的度量信息，利用小車進行矩形運動實驗，實測數(shù)據(jù)結(jié)果表明，本文提出的最優(yōu)基站選擇算法的定位精度較輪詢算法有較大提升。

1 基于改進馬氏距離判定的UWB測距

1.1 雙邊雙向測距原理

UWB模塊進行測距的原理為：利用飛行時間(Time of Fly，TOF)來獲得兩個模塊之間的距離。測距采用的通信方式如圖1所示，在傳統(tǒng)的雙邊雙向測距(Double-sided Two-way Ranging，DS)協(xié)議的基礎(chǔ)上增加了一個REPORT消息幀，以便將所有的距離信息匯總到標(biāo)簽中進行下一步處理。

圖1 UWB測距的通信過程Fig.1 Diagram of UWB ranging communication process

UWB標(biāo)簽首先廣播一個UWB信號POLL，并記錄下發(fā)送時間Ta1，經(jīng)過時間tf后到達UWB基站；基站記錄下接收到POLL信號的時間Tb1，并對信號進行處理，經(jīng)過時間Treply1后，在Tb2(Tb2＝Tb1+Treply1)時刻基站向標(biāo)簽回應(yīng)一個ANSWER信號；標(biāo)簽在成功接收到基站的ANSWER后記錄下接收時間Ta2，并經(jīng)過時間Treply2后發(fā)送FINAL信號；基站接收到標(biāo)簽的FINAL信號后，根據(jù)各個接收時間節(jié)點的時間戳，估計出傳播時間

在已知UWB信號在空氣中傳播速度的前提下，可以很容易地計算出標(biāo)簽和基站之間的距離d

1.2 基于改進馬氏距離判定的測距異常值檢測

本文是在傳統(tǒng)馬氏距離異常值檢測方法的基礎(chǔ)上進行的改進，在檢測過程中使用的均值和協(xié)方差矩陣都是在最小協(xié)方差行列式(Minimum Covariance Determinant，MCD)估計中計算出的穩(wěn)定值，這樣計算的樣本中的異常值和正常值的馬氏距離有明顯的差值，從而可以實現(xiàn)對異常值的剔除。

傳統(tǒng)的馬氏距離可由如下計算公式得到

改進馬氏距離異常值檢測方法的核心思想是利用Rousseeuw提出來的快速MCD算法(FASTMCD)，再結(jié)合傳統(tǒng)馬氏距離的計算過程，得到一個穩(wěn)定的協(xié)方差矩陣和一個穩(wěn)定的均值向量，然后再根據(jù)傳統(tǒng)馬氏距離的計算公式計算出每個樣本數(shù)據(jù)的穩(wěn)定馬氏距離，再進行異常值的剔除。

首先從M個距離樣本中隨機選取h個距離數(shù)據(jù)，h＝Mα(0.5＜α＜1)，并計算隨機選取的這h個距離數(shù)據(jù)的均值T1和協(xié)方差矩陣S1，然后計算M個距離樣本與T1的馬氏距離d。接著從M個距離樣本中選出馬氏距離最小的h個數(shù)據(jù)樣本，再根據(jù)新選取的這h個樣本數(shù)據(jù)計算出新的均值T2和協(xié)方差矩陣S2，僅當(dāng)T1＝T2且detS1＝detS2時，S1＝S2才成立。 這個過程不斷迭代， 直到Ti＝Ti－1、Si＝Si－1時迭代停止，這時即認為得到的Ti和Si為穩(wěn)定的均值和協(xié)方差，最后根據(jù)Ti和Si計算距離樣本中每個數(shù)據(jù)的馬氏距離，完成對異常值的檢測。

基于改進馬氏距離的異常值檢測方法的流程圖如圖2所示。

圖2 基于改進馬氏距離的異常值檢測方法流程圖Fig.2 Flowchart of outlier detection method based on improved Mahalanobis distance

2 UWB和慣導(dǎo)融合定位方法

由于UWB信號具有抗干擾能力強、分辨率高等一系列的優(yōu)點，使得基于UWB信號的無線定位技術(shù)在無線定位領(lǐng)域中脫穎而出。但是，由于室內(nèi)條件下環(huán)境復(fù)雜度高和障礙物多等問題，導(dǎo)致信號在傳播過程中會遇到非視距和多路徑等問題，定位精度會隨環(huán)境的復(fù)雜度提升而大幅下降，故可以采用UWB和慣導(dǎo)融合的方法來提高室內(nèi)定位精度。慣性導(dǎo)航系統(tǒng)利用IMU加速度計和陀螺對定位目標(biāo)的位姿進行解算，UWB標(biāo)簽與基站通過圖1所示過程獲得距離，然后利用EKF將IMU與UWB信息進行融合，最終完成對定位目標(biāo)的位置信息解算。

2.1 IMU定位原理

慣性導(dǎo)航系統(tǒng)是一種完全自主且不受外部環(huán)境干擾的獨立式導(dǎo)航系統(tǒng)，具有隱蔽性好和獨立性高等優(yōu)勢。如圖3所示，慣性導(dǎo)航系統(tǒng)的基本工作原理是以IMU作為系統(tǒng)硬件部分的核心傳感器，以牛頓定律作為姿態(tài)解算的理論基礎(chǔ)。IMU主要包括敏感線運動的加速度計傳感器和敏感角運動的陀螺傳感器。其中，利用加速度計可以獲得載體的加速度，利用陀螺可以獲得載體的角速度。加速度對時間進行一次積分計算后即為載體的速度，對時間進行再次積分可以得到載體的位移，角速度對時間進行一次積分可以得到載體的姿態(tài)信息。

圖3 IMU定位原理Fig.3 Positioning principle of IMU

2.2 UWB／IMU融合定位原理

由于Kalman濾波只能用于線性系統(tǒng)，故使用以Kalman濾波算法為基礎(chǔ)的EKF算法進行融合。

(1)狀態(tài)方程

在定位的過程中，用轉(zhuǎn)換矩陣R表示載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的變換，利用IMU和UWB對系統(tǒng)的12維狀態(tài)進行估計，系統(tǒng)狀態(tài)取為位置、速度、方向。故在EKF計算的過程中，將狀態(tài)向量ξ(k)取為

式(4)中，X為定位目標(biāo)在導(dǎo)航坐標(biāo)系下的位置坐標(biāo)，ρ＝R－1˙X為定位目標(biāo)在載體系下的速度，三維矢量δ為定位目標(biāo)的姿態(tài)誤差。

定位目標(biāo)的方向這里采用了參考姿態(tài)Rref和旋轉(zhuǎn)矢量δ來表示，δ的大小表示旋轉(zhuǎn)角，δ方向上的單位矢量表示旋轉(zhuǎn)軸，則其對應(yīng)的旋轉(zhuǎn)矩陣可表示為Dδ。

將Dδ一階展開，有

式(5)中，I為大小為3×3的單位矩陣。

假設(shè)在離散時刻k到k+1的Δt內(nèi)，定位目標(biāo)相對載體坐標(biāo)系的位置 變化 ΔX＝[ΔxbΔybΔzb]， 則

將R表示為如下形式

則k+1時刻定位目標(biāo)在導(dǎo)航坐標(biāo)系下的位置為

k+1時刻定位目標(biāo)在載體坐標(biāo)系下的速度可由如下方程式得到

d為由Rodriguez參數(shù)表示的姿態(tài)誤差，有

式(11)中，ωx、ωy、ωz分別為陀螺在離散時間間隔內(nèi)相對載體坐標(biāo)系三個軸上的角速度，在采樣時間內(nèi)視為常數(shù)。

δ的更新可由下式得到

式(12)中，Dd為d對應(yīng)的旋轉(zhuǎn)矩陣。

系統(tǒng)的狀態(tài)模型可表示為

式(13)中，f(·)為對系統(tǒng)進行一階Taylor展開后的線性化函數(shù)模型。

(2)觀測方程

在慣導(dǎo)和UWB融合定位中，由于UWB測距不存在累積誤差，且測距誤差僅在0.1m左右，屬于一種較高精度的無線測距技術(shù)，所以UWB的測距值可作為EKF中的觀測值。

假設(shè)位于已知位置的UWB基站i的坐標(biāo)為(ρi，x，ρi，y，ρi，z)， 則在k時刻利用UWB標(biāo)簽解算得到的測距值為di，k。 假設(shè)此時定位目標(biāo)的實際位置坐標(biāo)為Xk＝(xk，yk，zk)， 則觀測方程可以建模為

則觀測矩陣為

圖4 UWB和IMU融合定位的整體流程圖Fig.4 Flowchart of UWB and IMU fusion positioning

式(16)中，

(3)EKF融合

根據(jù)狀態(tài)方程和觀測方程，通過EKF的時間更新和狀態(tài)更新可以得到定位目標(biāo)的狀態(tài)信息。時間更新方程為

狀態(tài)更新方程為

綜上，基于UWB和慣性導(dǎo)航融合的定位系統(tǒng)整體流程如圖4所示。

3 最優(yōu)基站選擇方法

經(jīng)過UWB和慣導(dǎo)的融合定位后，對于定位區(qū)域內(nèi)的基站，傳統(tǒng)的選擇方法采用的是輪詢通信的方式，即按照固定規(guī)則的通信順序與區(qū)域內(nèi)的基站分別進行通信。這種傳統(tǒng)的選擇方法沒有考慮到在每個時刻每個基站與定位目標(biāo)之間產(chǎn)生的測距值會對EKF解算過程中的協(xié)方差矩陣的更新產(chǎn)生的影響是不同的，這就導(dǎo)致傳統(tǒng)的基站輪詢選擇方法不能充分利用各個基站的位置信息。所以，為了進一步提高融合定位的精度，本文提出了一種基于協(xié)方差矩陣跡的最優(yōu)基站選擇方法。

不同于輪詢方法，本文提出的UWB基站選擇方法使定位標(biāo)簽可以自由選擇區(qū)域內(nèi)參與下一時刻EKF解算的最優(yōu)基站。

根據(jù)定位目標(biāo)的運動軌跡具有連續(xù)性這一特點，通過已知的當(dāng)前時刻定位目標(biāo)的位置坐標(biāo)預(yù)測下一時刻在當(dāng)前區(qū)域內(nèi)的所有基站所能產(chǎn)生的測距值，根據(jù)測距值進一步計算出所能引起的協(xié)方差矩陣跡的變化，并以此作為在下一時刻進行基站選擇的度量信息，目的是在每一時刻都以協(xié)方差矩陣跡的變化作為基準，選擇當(dāng)前區(qū)域內(nèi)能使系統(tǒng)狀態(tài)量的估計誤差最小的基站作為下一時刻的通信基站，即最優(yōu)基站。

在EKF融合解算過程中的協(xié)方差矩陣可以表示為

根據(jù)來自基站i的測距值di， 利用本文上節(jié)中的EKF觀測方程的建立過程，帶入式(16)可以得到對應(yīng)的觀測矩陣Hi， 并且可以進一步計算出對應(yīng)的Kalman濾波增益Ki

式(22)中，R為測距噪聲的方差陣。利用Kalman濾波增益Ki對協(xié)方差矩陣進行更新，有

由于觀測矩陣具有稀疏性的特點，所以可以將式(24)簡化為

根據(jù)式(25)，可在任一時刻計算定位目標(biāo)所在區(qū)域內(nèi)所有基站能引起的協(xié)方差的變化。

矩陣的跡定義為矩陣主對角元素之和，由協(xié)方差矩陣的特殊性質(zhì)可以知道，協(xié)方差矩陣的跡即為系統(tǒng)所有狀態(tài)量方差之和，即

ξ為系統(tǒng)的狀態(tài)量，所以本文提出了一種利用協(xié)方差矩陣跡作為對下一時刻基站進行選擇的度量指標(biāo)，在每個時刻都選擇定位目標(biāo)所在區(qū)域內(nèi)能使協(xié)方差矩陣跡最小，即能使系統(tǒng)的狀態(tài)估計誤差最小的基站作為下一時刻的最優(yōu)通信基站。

再次利用觀測矩陣的稀疏性質(zhì)，對式(27)進一步簡化

在每個時刻計算定位目標(biāo)當(dāng)前所在區(qū)域內(nèi)每個基站的跡，然后選擇值最小的基站作為下一時刻參與EKF解算的最優(yōu)通信基站。

4 實驗與結(jié)果分析

4.1 實驗環(huán)境與硬件介紹

本文的實驗測試環(huán)境為北京科技大學(xué)機電信息樓814實驗室，實驗室環(huán)境布局的平面簡圖如圖5所示。房間大小為5.0m×8.0m，在房間內(nèi)4.0m×4.2m大小的場地上方四周設(shè)有運動捕捉系統(tǒng)Optitrack定位攝像頭，本文的實驗場地位于運動捕捉系統(tǒng)捕捉范圍內(nèi)一個2.4m×2.4m的正方形區(qū)域。實際的實驗環(huán)境和上方的運動捕捉系統(tǒng)的部分定位攝像頭如圖6所示。運動捕捉系統(tǒng)屬于高精度的室內(nèi)定位系統(tǒng)，采用紅外攝像頭覆蓋室內(nèi)的定位空間。在被定位的物體上放置反光標(biāo)記點，通過獲取這些反光標(biāo)記點反射在定位攝像頭上的圖像，計算被定位物體的三維位置信息，且Optitrack定位攝像頭的定位精度能達到0.1mm。在本文的實驗中均使用了運動捕捉系統(tǒng)解算的運動軌跡作為定位目標(biāo)的實際運動軌跡，與本文中所提出的定位方法形成定位精度的對比。

圖5 實驗室平面簡圖Fig.5 Diagram of laboratory plane

圖6 實驗環(huán)境Fig.6 Diagram of experiment environment

在實驗過程中實時采集的UWB和IMU數(shù)據(jù)通過SPI和I2C的串行通信方式與STM32F405RG處理器進行通信，并對數(shù)據(jù)進行解算得到定位坐標(biāo)，最后將最終的定位結(jié)果、UWB測距值和IMU數(shù)據(jù)通過藍牙發(fā)送至上位機端保存成csv格式，并導(dǎo)入Matlab進行解算和繪圖。定位標(biāo)簽和上位機端的藍牙接收裝置如圖7所示。

圖7 定位標(biāo)簽和藍牙接收器Fig.7 Diagram of location tag and bluetooth receiver

UWB基站在實驗環(huán)境中的布置和本文中使用的導(dǎo)航坐標(biāo)系示意圖如圖8所示。

圖8 UWB的基站布置和導(dǎo)航坐標(biāo)系的建立Fig.8 UWB base station layout and establishment of navigation coordinate system

在對基站坐標(biāo)進行測量時，先將可由運動捕捉系統(tǒng)識別的反光標(biāo)記點放置在基站上，通過運動捕捉系統(tǒng)捕捉到的位置坐標(biāo)作為各個基站的具體位置坐標(biāo)，如表1所示。

表1 UWB基站坐標(biāo)Table 1 Coordinates of UWB base station

4.2 測距異常值剔除實驗

將基于馬氏距離的異常值檢測算法程序加入到測距程序中，仍讓標(biāo)簽分別位于基站的1m和2.5m處，測距實驗共進行1000次，測距頻率為100Hz，實驗結(jié)果如圖9所示。

圖10為采用基于改進馬氏距離的異常值檢測方法對異常值進行剔除后的1.0m和2.5m處的測距結(jié)果。

圖9 測距異常值檢測結(jié)果Fig.9 Diagram of ranging results using outlier detection

圖10 測距異常值剔除后的檢測結(jié)果Fig.10 Diagram of ranging results after removing the outliers

由圖10可知，在整個測距的過程中沒有出現(xiàn)測距異常值，且測距的精度均在0.1m左右，上述的測距實驗表明了本文采用的基于改進馬氏距離的異常值檢測方法的有效性，能夠在保證測距實時性的同時，有效避免測距過程中異常值的出現(xiàn)。

4.3 最優(yōu)基站選擇融合定位實驗

為了驗證本文提出的UWB和IMU融合定位方法的精度，在實際場景下進行實驗并計算其估計軌跡誤差。如圖11所示，實驗中將定位標(biāo)簽固定在可由運動捕捉系統(tǒng)反饋控制運動的定位小車上，定位標(biāo)簽由充電寶通過USB口進行供電，并在定位標(biāo)簽中心位置上布設(shè)一個可由運動捕捉系統(tǒng)識別的反光標(biāo)記點。

圖11 定位標(biāo)簽與小車Fig.11 Diagram of locating tag and trolley

控制小車在實驗場地中按照一個1.2m×1.2m的矩形軌跡進行運動，實驗結(jié)果中以由運動捕捉系統(tǒng)獲得的定位標(biāo)簽中心的反光標(biāo)記點的軌跡作為定位標(biāo)簽的實際運動軌跡。

移動小車按照矩形軌跡運行兩次，分別采用本文的最優(yōu)基站選擇方法和傳統(tǒng)的輪詢方法估計小車的位置軌跡。實驗中的真實軌跡均采用的為運動捕捉系統(tǒng)的輸出結(jié)果，最后的真實軌跡取為兩次運動捕捉系統(tǒng)數(shù)據(jù)的平均值，實驗對比結(jié)果如圖12示。

圖12 定位結(jié)果對比Fig.12 Comparison of positioning results

由圖12可知，在x方向和y方向上雖然某些位置處會出現(xiàn)采用傳統(tǒng)選擇方法的精度高于本文的最優(yōu)基站選擇方法的情況，但是整體軌跡中明顯能看出最優(yōu)基站選擇方法對精度的改進。在z方向上，當(dāng)使用傳統(tǒng)的基站輪詢選擇方法進行定位時，定位結(jié)果具有較大的波動性，而使用了最優(yōu)基站選擇的定位結(jié)果中，z方向數(shù)據(jù)的平穩(wěn)性有了很大提升。圖13給出了利用兩種不同基站選擇方法進行定位時整體誤差的比較。

圖13 兩種不同基站選擇方法的誤差對比Fig.13 Error comparison of two different base station selection methods

由圖13明顯可知，最優(yōu)基站選擇方法對定位誤差波動幅度有一定的改善，定位精度得到提升。在整個運動過程中，采用最優(yōu)基站選擇方法的最大定位誤差值為0.258m，絕對誤差的平均值為0.226m，均方根誤差為0.227m。采用傳統(tǒng)輪詢方法時，在整個過程中的最大定位誤差值為0.342m，絕對誤差的平均值為0.267m，均方根誤差為0.269m。相比采用輪詢方法的定位結(jié)果，使用本文提出的最優(yōu)基站選擇方法時，平均精度提高了15.3%，均方根誤差減小了15.6%。

5 結(jié)論

本文對UWB和慣性導(dǎo)航的融合定位算法進行了研究，通過采用基于改進馬氏距離的異常值檢測算法，有效地對測距過程中的異常值進行了剔除。然后提出了一種基于協(xié)方差矩陣跡的最優(yōu)基站選擇算法，不同于按照給定順序進行通信的輪詢算法，所提出的算法能夠?qū)崟r計算所在區(qū)域內(nèi)每個基站在下一時刻能對當(dāng)前時刻協(xié)方差的跡產(chǎn)生的變化量，并選擇能使當(dāng)前協(xié)方差的跡變化最大的基站作為下一時刻的通信基站。實驗結(jié)果表明，本文提出的基站選擇算法具有計算量小、實時性高等優(yōu)點，與傳統(tǒng)的輪詢算法相比，平均定位精度提高了15.3%，是一種高精度的實時室內(nèi)定位方法。