基于最優(yōu)基站選擇的超寬帶和慣導(dǎo)融合定位方法

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

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

0 引言

隨著通信技術(shù)的高速發(fā)展，室內(nèi)定位技術(shù)在監(jiān)控管理、無人駕駛等領(lǐng)域占據(jù)越來越重要的地位，可以為運(yùn)動(dòng)目標(biāo)提供準(zhǔn)確位置信息的室內(nèi)定位技術(shù)越來越成為人們關(guān)注的焦點(diǎn)[1-5]。慣性導(dǎo)航技術(shù)是最早被應(yīng)用到室內(nèi)定位領(lǐng)域的導(dǎo)航技術(shù)，國內(nèi)外學(xué)者針對基于慣性導(dǎo)航的定位算法做了大量的研究，但是由于慣性導(dǎo)航存在累積誤差，所以并不適合長時(shí)間定位[6]。超寬帶(Ultra-wideband，UWB)是一種新型的無線通信技術(shù)，相比于傳統(tǒng)的無線通信技術(shù)，UWB具有功率譜低、保密性強(qiáng)、功耗低等優(yōu)點(diǎn)[7-8]。自2010年以來，已有幾種成熟的UWB無線電傳感器定位設(shè)備能夠提供厘米級的定位，例如DecaWave[9]的DWM1000和KickStarter的Pozyx。與其他室內(nèi)定位技術(shù)相比，UWB定位通過發(fā)送納秒(ns)或微秒(μs)非正弦窄脈沖可實(shí)現(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、氣壓計(jì)和UWB，它們可以在地板上獲得準(zhǔn)確的位置，在現(xiàn)實(shí)的戰(zhàn)術(shù)測試場景中顯示。Sczyslo等使用了松散組合法，基于擴(kuò)展Kalman濾波器來跟蹤行人的運(yùn)動(dòng)[12-16]。有學(xué)者提出了一種緊密耦合的方法，該方法結(jié)合了UWB范圍和INS的測量[17-19]。文獻(xiàn)[20]設(shè)計(jì)了一種用于無人機(jī)室內(nèi)導(dǎo)航的定位方法，該方法集成了3D激光掃描儀、UWB和INS的信息，該策略可改善定位效果，與僅INS和僅UWB的方法相比，準(zhǔn)確性顯著提高。文獻(xiàn)[21]設(shè)計(jì)了一種使用松散耦合EKF算法的UWB和IMU集成系統(tǒng)，在NLOS和UWB信號不足或不可用的情況下，有效地提高了定位的精度和穩(wěn)健性。北京航空航天大學(xué)的儀玉杰等[22]針對在復(fù)雜室內(nèi)環(huán)境下單純依靠UWB技術(shù)定位結(jié)果會嚴(yán)重失真甚至缺失的問題，提出了一種將UWB與行人航跡推算方法(PDR)相結(jié)合的方法，解決了在復(fù)雜室內(nèi)環(huán)境下定位解算點(diǎn)可能缺失的問題，并且較顯著地提高了在復(fù)雜環(huán)境下定位系統(tǒng)的魯棒性和定位精度。文獻(xiàn)[23]利用安裝在移動(dòng)節(jié)點(diǎn)上的UWB模塊產(chǎn)生位置的估計(jì)值，以校正慣導(dǎo)的航位推算法所得到的移動(dòng)節(jié)點(diǎn)的位置坐標(biāo)校正慣導(dǎo)中由于陀螺漂移導(dǎo)致的累積定位誤差。文獻(xiàn)[24]基于UWB結(jié)合IMU的室內(nèi)定位設(shè)計(jì)方法提出了一種定位技術(shù)，可以有效抑制IMU的誤差積累，進(jìn)一步提高了定位精度。文獻(xiàn)[25]針對UWB和IMU融合車輛的定位問題，提出了一種約束魯棒迭代擴(kuò)展Kalman濾波算法，克服了針對非Gauss噪聲先天缺陷的擴(kuò)展Kalman濾波器算法的缺點(diǎn)，定位精度達(dá)到0.21m。

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

1 基于改進(jìn)馬氏距離判定的UWB測距

1.1 雙邊雙向測距原理

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

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

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

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

1.2 基于改進(jìn)馬氏距離判定的測距異常值檢測

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

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

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

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

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

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

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

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

2.1 IMU定位原理

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

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

2.2 UWB／IMU融合定位原理

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

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

在定位的過程中，用轉(zhuǎn)換矩陣R表示載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的變換，利用IMU和UWB對系統(tǒng)的12維狀態(tài)進(jìn)行估計(jì)，系統(tǒng)狀態(tài)取為位置、速度、方向。故在EKF計(jì)算的過程中，將狀態(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è)在離散時(shí)刻k到k+1的Δt內(nèi)，定位目標(biāo)相對載體坐標(biāo)系的位置 變化 ΔX＝[ΔxbΔybΔzb]， 則

將R表示為如下形式

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

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

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

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

δ的更新可由下式得到

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

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

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

(2)觀測方程

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

假設(shè)位于已知位置的UWB基站i的坐標(biāo)為(ρi，x，ρi，y，ρi，z)， 則在k時(shí)刻利用UWB標(biāo)簽解算得到的測距值為di，k。 假設(shè)此時(shí)定位目標(biāo)的實(shí)際位置坐標(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的時(shí)間更新和狀態(tài)更新可以得到定位目標(biāo)的狀態(tài)信息。時(shí)間更新方程為

狀態(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)的基站分別進(jìn)行通信。這種傳統(tǒng)的選擇方法沒有考慮到在每個(gè)時(shí)刻每個(gè)基站與定位目標(biāo)之間產(chǎn)生的測距值會對EKF解算過程中的協(xié)方差矩陣的更新產(chǎn)生的影響是不同的，這就導(dǎo)致傳統(tǒng)的基站輪詢選擇方法不能充分利用各個(gè)基站的位置信息。所以，為了進(jìn)一步提高融合定位的精度，本文提出了一種基于協(xié)方差矩陣跡的最優(yōu)基站選擇方法。

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

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

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

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

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

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

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

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

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

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

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

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

4.1 實(shí)驗(yàn)環(huán)境與硬件介紹

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

圖5 實(shí)驗(yàn)室平面簡圖Fig.5 Diagram of laboratory plane

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

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

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

UWB基站在實(shí)驗(yàn)環(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)進(jìn)行測量時(shí)，先將可由運(yùn)動(dòng)捕捉系統(tǒng)識別的反光標(biāo)記點(diǎn)放置在基站上，通過運(yùn)動(dòng)捕捉系統(tǒng)捕捉到的位置坐標(biāo)作為各個(gè)基站的具體位置坐標(biāo)，如表1所示。

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

4.2 測距異常值剔除實(shí)驗(yàn)

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

圖10為采用基于改進(jìn)馬氏距離的異常值檢測方法對異常值進(jìn)行剔除后的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可知，在整個(gè)測距的過程中沒有出現(xiàn)測距異常值，且測距的精度均在0.1m左右，上述的測距實(shí)驗(yàn)表明了本文采用的基于改進(jìn)馬氏距離的異常值檢測方法的有效性，能夠在保證測距實(shí)時(shí)性的同時(shí)，有效避免測距過程中異常值的出現(xiàn)。

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

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

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

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

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

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

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

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

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

5 結(jié)論

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