基于最優基站選擇的超寬帶和慣導融合定位方法

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

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

0 引言

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

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

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

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

1.1 雙邊雙向測距原理

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

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

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

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

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

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

傳統的馬氏距離可由如下計算公式得到

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

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

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

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

2 UWB和慣導融合定位方法

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

2.1 IMU定位原理

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

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

2.2 UWB／IMU融合定位原理

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

(1)狀態方程

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

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

定位目標的方向這里采用了參考姿態Rref和旋轉矢量δ來表示，δ的大小表示旋轉角，δ方向上的單位矢量表示旋轉軸，則其對應的旋轉矩陣可表示為Dδ。

將Dδ一階展開，有

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

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

將R表示為如下形式

則k+1時刻定位目標在導航坐標系下的位置為

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

d為由Rodriguez參數表示的姿態誤差，有

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

δ的更新可由下式得到

式(12)中，Dd為d對應的旋轉矩陣。

系統的狀態模型可表示為

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

(2)觀測方程

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

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

則觀測矩陣為

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

式(16)中，

(3)EKF融合

根據狀態方程和觀測方程，通過EKF的時間更新和狀態更新可以得到定位目標的狀態信息。時間更新方程為

狀態更新方程為

綜上，基于UWB和慣性導航融合的定位系統整體流程如圖4所示。

3 最優基站選擇方法

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

不同于輪詢方法，本文提出的UWB基站選擇方法使定位標簽可以自由選擇區域內參與下一時刻EKF解算的最優基站。

根據定位目標的運動軌跡具有連續性這一特點，通過已知的當前時刻定位目標的位置坐標預測下一時刻在當前區域內的所有基站所能產生的測距值，根據測距值進一步計算出所能引起的協方差矩陣跡的變化，并以此作為在下一時刻進行基站選擇的度量信息，目的是在每一時刻都以協方差矩陣跡的變化作為基準，選擇當前區域內能使系統狀態量的估計誤差最小的基站作為下一時刻的通信基站，即最優基站。

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

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

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

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

根據式(25)，可在任一時刻計算定位目標所在區域內所有基站能引起的協方差的變化。

矩陣的跡定義為矩陣主對角元素之和，由協方差矩陣的特殊性質可以知道，協方差矩陣的跡即為系統所有狀態量方差之和，即

ξ為系統的狀態量，所以本文提出了一種利用協方差矩陣跡作為對下一時刻基站進行選擇的度量指標，在每個時刻都選擇定位目標所在區域內能使協方差矩陣跡最小，即能使系統的狀態估計誤差最小的基站作為下一時刻的最優通信基站。

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

在每個時刻計算定位目標當前所在區域內每個基站的跡，然后選擇值最小的基站作為下一時刻參與EKF解算的最優通信基站。

4 實驗與結果分析

4.1 實驗環境與硬件介紹

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

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

圖6 實驗環境Fig.6 Diagram of experiment environment

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

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

UWB基站在實驗環境中的布置和本文中使用的導航坐標系示意圖如圖8所示。

圖8 UWB的基站布置和導航坐標系的建立Fig.8 UWB base station layout and establishment of navigation coordinate system

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

表1 UWB基站坐標Table 1 Coordinates of UWB base station

4.2 測距異常值剔除實驗

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

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

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

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

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

4.3 最優基站選擇融合定位實驗

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

圖11 定位標簽與小車Fig.11 Diagram of locating tag and trolley

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

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

圖12 定位結果對比Fig.12 Comparison of positioning results

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

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

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

5 結論

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