融合UWB和IMU數據的新型室內定位算法

2021-08-24 06:53:20魏國亮王甘楠

小型微型計算機系統(tǒng) 2021年8期

周軍，魏國亮，田昕，王甘楠

1(上海理工大學光電信息與計算機工程學院，上海 200093)

2(上海理工大學理學院，上海 200093)

1 引言

在基于位置的服務及其應用中，位置信息起著至關重要的作用.在室外環(huán)境中，可以輕松地從全球定位系統(tǒng)(Global Positioning System，GPS)獲得準確的位置信息.但是，在室內環(huán)境中，GPS信號受到建筑阻擋[1]，導致GPS信號變弱甚至無法被檢測，因此GPS定位的結果無法滿足人們室內定位的要求.隨著技術的發(fā)展，目前已有許多技術可以代替GPS用于室內定位，如超寬帶、超聲波、藍牙等.其中大部分定位技術主要基于以下方式：到達角度(Angle of Arrival，AOA)[2]、到達時間差(Time Difference of Arrival，TDOA)[3]、到達時間(Time of Arrival，TOA)[4]、接收信號強度指引(Receive Signal Strength Index，RSSI)[5，6].由于基于TOA方法的定位精度較高[7]，可用于實現室內環(huán)境下的高精度定位.

超寬帶(Ultra-Wideband，UWB)具有許多理想的特性，包括低能耗、高分辨率、多徑免疫和某些障礙物穿透能力等，因此，UWB定位被認為是用于高精度室內定位中最有前途的技術之一[8].但是，當環(huán)境受到NLOS(Non-line-of-sight)影響時，UWB測距誤差增大，從而導致定位精度下降.因此，如何處理NLOS誤差已成為UWB定位領域的熱門研究課題[9].與UWB不同，INS使用慣性測量單元(Inertial Measurement Unit，IMU)在沒有其他任何輔助信息的情況下以較高的更新率估算物體的位置，短時間內的誤差較小，但具有累計誤差，與UWB系統(tǒng)形成良好的互補，因此有必要融合兩種系統(tǒng)達到更好的定位效果[10].

文獻[11]中，EKF基于IMU預測值對測量距離誤差進行檢測，并結合超寬帶的信道沖激響應來進一步提高檢測精度，忽略檢測到的不精確測量值.該方法不適用于復雜的室內環(huán)境且丟棄了許多有用信息.文獻[12]使用高斯低通濾波算法和主成分分析法進行預處理，并提取位置特征信息.結合最小二乘支持向量機對樣本集進行離線訓練和在線分類，獲取人員實時位置.文獻[13]提出了一種能夠確定特定的非視距信道的非視距識別與緩解方法，并根據識別出的信道信息進行定位估計，但保留了太多的離線數據，需要大量時間進行在線匹配篩選.為了能夠在復雜的室內環(huán)境下快速準確地識別出NLOS，并有效緩解NLOS影響，考慮UWB和INS各自的優(yōu)點，本文提出一種新的定位算法——利用PDR(Pedestrian Dead Reckoning)預測位置進行NLOS識別，并基于多信道NLOS誤差概率分布函數進行NLOS修正，最后利用KF(Kalman Filtering)融合UWB和IMU獲得準確的定位結果.主要貢獻歸納如下：

1)分信道采集數據與真實值比較獲得NLOS誤差，根據不同信道的誤差分布，擬合出對應于不同信道NLOS誤差的概率曲線.

2)利用PDR預測的位置進行NLOS識別，減少系統(tǒng)的NLOS識別時間.通過概率判斷NLOS誤差所屬的信道情況并對NLOS誤差進行估計，從而獲得更為準確的估計距離.

3)對UWB系統(tǒng)，利用改進的最小二乘法估計移動節(jié)點位置，提高UWB系統(tǒng)的定位精度.

2 系統(tǒng)模型

定位的過程主要分為兩個階段：離線階段和在線階段.

2.1 離線階段

基于TOA測距方法的NLOS誤差表達式如下[13]：

(1)

其中εnlos和attnlos分別表示與傳播通道中障礙物相關的介電常數和衰減因子，L是路徑的數量，ai和τi表示第i條路徑的幅值和TOA，aDP和τDP表示LOS(Line of sight)下的幅值和TOA.

根據式(1)可以看出不同的障礙物對NLOS誤差的影響是不同的.為了更加準確地估計NLOS誤差，在離線階段，用UWB對不同信道分別進行采樣，并將在不同障礙物下的測量值與真實值進行比較獲得準確的NLOS誤差值.將每一個信道測得的誤差值細分為w個區(qū)間.第k個區(qū)間上的中點和該區(qū)間上NLOS誤差值出現的頻率為ak和bk，將其代入正態(tài)分布曲線進行線性擬合，獲得每一個信道下誤差分布的概率曲線(假設每一信道下的NLOS誤差服從正態(tài)分布).本文對m個不同的信道進行采樣擬合：

(2)

其中：

擬牛頓法是求解非線性優(yōu)化問題最有效的方法之一，只要求每一步迭代時知道目標函數的梯度.通過擬牛頓法對式(2)求解，可以獲得不同信道下NLOS誤差的概率分布曲線.為了便于后期的比較，對每一條曲線進行相應的縮放，使得每一條NLOS誤差概率分布曲線的取值范圍在0-1之間，獲得的NLOS誤差概率曲線如圖1所示.可以看出木門和行人的NLOS誤差分布較為接近，而金屬障礙物鐵門和混凝土墻的分布較為接近.為了更好地進行NLOS識別修正，將行人分別與鐵門和混凝土墻的均值和標準差平均來表示鐵門與行人的情況和混泥土墻與行人的情況.

圖1 不同障礙物下的NLOS誤差分布

2.2 在線階段

考慮在一個室內環(huán)境下進行二維平面的目標定位與跟蹤.在該空間中的已知位置放置可以發(fā)送帶有時間戳的UWB發(fā)送裝置，目標攜帶有UWB無線接收裝置和INS的移動設備.

2.2.1 INS系統(tǒng)

INS通過IMU來估計位置，IMU的主要優(yōu)點是無需外部輸入即可估計其位置，并且可以在室內環(huán)境或沒有衛(wèi)星信號的地方使用，目前已經應用于室內定位.通常，IMU包括陀螺儀和加速度計.陀螺儀輸出物體的角速度，而加速度計輸出線性加速度.物體的三軸角速度通過一次積分可得姿態(tài)角信息，即偏航角、俯仰角和翻滾角.根據從IMU獲得的信息，可以將慣性參考坐標中的加速度轉換為導航參考坐標.然后，根據牛頓運動定律，在兩次積分后計算對象的位置[14].因此，IMU位置估計系統(tǒng)可以通過以下線性方程建模：

(3)

(4)

(5)

2.2.2 UWB測距模型

本文中，使用了基于TOA的測距方法，要求接收端和發(fā)送端時間同步，通過獲得信號傳播的時間t間接獲得錨節(jié)點和移動目標的距離[15]：

d=c×t

其中，c為信號傳播的速度(c=3×108m/s)，d為測得的發(fā)送端與接收端的距離.

假設有n個錨節(jié)點，測得移動目標接收到第i個錨節(jié)點發(fā)出的信號在空氣中的傳播時間為ti，由于有噪聲、障礙物等原因的存在，使得測量的距離存在測量誤差，表達式如下：

(6)

3 定位系統(tǒng)描述

3.1 步行者航跡推算

INS系統(tǒng)存在一個眾所周知的問題，即累積誤差[16]，位置估計誤差會隨著時間(迭代次數)而迅速增加.從式(3)-式(5)中可以看出，角速度測量中的誤差將引入到估計的姿態(tài)中，而線性加速度測量中引入的誤差將傳送到估計的速度和估計物體的位置.同時下一時刻的位置是基于當前的估計位置來獲得的，INS系統(tǒng)的當前估計位置的誤差將被會被累加到下一時刻的位置估計中.因此，在幾次位置更新后，累積的誤差會逐漸增大.為了較好地解決上述問題，采用了步行者航跡推算的方法(PDR)[17]，該方法包括3個過程：步檢測、步幅(SL)估計和方向估計.

3.1.1 步檢測

本文采用峰值檢測的方法[18，19]來判定是否發(fā)生行走行為.首先，需要定義一個峰值檢測的參數，減少噪聲的影響.在實驗中發(fā)現，由于個人行走的習慣及噪聲的影響，檢測到的步數通常高于實際步數.考慮到常人的步頻不會太快，因此對峰值檢測設置相應的時間間隔，如果在一段時間內出現多個峰值，則當出現第一個峰值時即認為行走了一步，并忽略該時間段內對其他峰值的檢測.如圖2所示，當設置間隔時間為0.5s時，會出現偶然的誤檢測，而當時間閾值設為0.6s則不會出現誤檢測，可以看出設置合適的時間閾值可以有效提高步數檢測的準確性.

圖2 時間閾值對步檢測的影響

3.1.2 步幅估計

正常行走時行人的步長不會發(fā)生太大的變化，所以步長可以假設成一個固定值，也可以通過加速度信號來進行計算，本文應用文獻[20]中的經驗公式，通過獲得的Z軸加速度信息對行人步長進行估計：

SL=K×(Azmax-Azmin)(1/4)

其中，Azmax和Azmin分別是在單步中Z軸方向上最大垂直加速度和最小垂直加速度，K是可以通過對步長訓練得到的常數.

3.1.3 方向估計

通常有兩種主要方法可估計行人在第t階段的方向：陀螺儀方法和加速度計方法[1].而本文中首先使用偏航角θt+1表示行人的方向，初步估計行人位置，用于NLOS初步識別和修正.

根據以上3步過程，首先可以獲得預測位置：

xt+1，pre=xt+SLt+1×cosθt+1

(7)

yt+1，pre=yt+SLt+1×sinθt+1

(8)

其中，xt，yt分別為第t時刻行人所在坐標系的橫縱坐標，xt+1，pre，yt+1，pre分別為預測行人在第t+1時刻的橫縱坐標.

3.2 UWB定位系統(tǒng)

通過式(7)和式(8)得到IMU的預測位置，根據距離公式計算預測位置與錨節(jié)點之間的距離：

(9)

將UWB距離測量值與式(9)獲得的距離預測值做差來獲得誤差的估計值，如式(10)所示：

(10)

設置閾值φ進行NLOS識別判斷，當：

Δdi≤φ

(11)

滿足時，則認為對應測量處于LOS，不對其進行處理，否則認為其受到NLOS干擾，將式(10)獲得的NLOS的估計誤差代入不同信道下的NLOS誤差的概率分布函數，得到屬于各個信道的概率ωj，j=1，2，…，m.

通過NLOS誤差屬于每一個信道的概率來判別該測量值所受NLOS的情況.由于難以表示所有障礙物對應的NLOS誤差概率分布函數，因此設置閾值α以解決無法識別修正的情況.當：

ωj<α，?j，j=1，2，…，m

(12)

滿足時，則認為該測量值超出了該方法的識別修正能力，因此將該測量值丟棄不用于最終的UWB系統(tǒng)的最小二乘定位.此外設置一個大于α的閾值β，如果：

max(ωj)>β，j=1，2，…，m

(13)

則認為NLOS誤差的分布對應其中概率最大值所對應信道的誤差分布，假設最大概率對應信道的誤差分布為N(μ，σ2)，所以該NLOS誤差對應的大小及標準差為：

(14)

(15)

當式(12)和式(13)兩式都不滿足時，則將傳播信道視為多個信道的組合，定義：

W={j|α<ωj<β，j=1，2，…，m}

則修正的NLOS誤差及其對應的標準差分別為：

(16)

(17)

根據式(14)和式(16)從而獲得UWB系統(tǒng)測量距離的修正值：

得到修正距離后，使用最小二乘定位法對移動目標位置進行估計，假設行人的坐標為(x，y).因此，第i個錨節(jié)點與移動節(jié)點之間的距離為：

假設修正值的誤差服從同一分布εi，即：

兩邊取平方:

(18)

將式(18)寫成矩陣形式：

HX=b

其中：

從而獲得UWB系統(tǒng)最小二乘的解：

XLS，t+1=(HTH)-1HTb

(19)

其中，λ在0和1之間，并利用更新后的偏航角對式(7)和式(8)進行更新，從而獲得更新后的估計位置：

(20)

3.3 KF融合IMU和UWB

卡爾曼濾波器廣泛用于數據融合和狀態(tài)估計，并且提出了許多使用卡爾曼濾波器來解決不同的問題.這些不同形式的卡爾曼濾波器遵循相同的兩個通用步驟：預測步驟和更新步驟[21，22].本節(jié)中，利用卡爾曼濾波融合上述UWB和INS系統(tǒng)的定位結果，實現更加準確的行人位置估計.

預測階段：

(21)

Pk|k-1=Pk-1|k-1+Qk

(22)

更新階段：

Kk=Pk|k-1C(CPk|k-1CT+Rk)-1

(23)

Xk|k=Xk|k-1+Kk(Z-CXk|k-1)

(24)

Pk|k=(I-KkC)Pk|k-1

(25)

其中：

Zk由INS和UWB兩個系統(tǒng)的估計值組成，兩個子系統(tǒng)的估計值由式(19)和式(20)獲得，Kk是卡爾曼增益，Rk是測量誤差的協(xié)方差矩陣.其中Rk元素的表達式如下：

Qk/PDR=Pk|k-1

在獲得更為準確的估計結果之后，重新計算更新后的NLOS誤差值對距離進行更新，再次獲得最小二乘的估計位置，更新偏航角，卡爾曼濾波融合兩個系統(tǒng)的結果，反復執(zhí)行以上過程，直至估計位置變化在一定的范圍之內(微小的變化可能由測量所受的噪聲引起)，結束循環(huán)，獲得最終的估計位置，并以此結果作為INS下一時刻的起始位置，從而來降低INS的累計誤差，減小UWB的NLOS誤差，降低整個系統(tǒng)的估計位置的誤差，獲得好的定位結果.

4 仿真設計和結果分析

為了驗證定位系統(tǒng)的性能，模擬了一個室內定位的場景.假設在60m×50m 的房間內，左下角為原點，在固定的地點放置5個參考節(jié)點，行人帶有移動節(jié)點和慣性導航模塊在5個參考節(jié)點的覆蓋范圍內，沿著以點(10，5)、(50，5)、(50，45)和(10，45)為頂點的矩形逆時針行走一圈.假設PDR算法的步長為0.8米，誤差服從N(0，0.042)，共走了200步，角度噪聲設為N(0，π/50).UWB測距系統(tǒng)在LOS下的測距誤差服從N(0，0.0262).仿真中假設有5種不同的NLOS情況，它們的分布分別為N(0.72，0.452)、N(0.92，0.132)、N(0.36，0.082)、N(0.59，0.092)和N(0.45，0.112).

4.1 選擇合適的參考節(jié)點

從上文分析可以看出選擇合適的參考節(jié)點對定位精度有明顯的影響，由于在NLOS的識別和緩解過程中，NLOS誤差被修正，殘留的NLOS的誤差值很小，為了便于觀察選擇合適的錨節(jié)點作為參考節(jié)點對定位精度的影響，首先假設5個錨節(jié)點都處于LOS情況下，誤差服從N(0，0.0262).仿真效果圖如圖3所示，可以看出相比較于LOS情況下始終選擇同一個錨節(jié)點作為參考節(jié)點的情況，選擇合適的錨節(jié)點作為參考節(jié)點能夠有效減少定位誤差，進行了50次仿真.同一個錨節(jié)點作為參考節(jié)點的仿真中，50次仿真的總誤差286.8361米，而每一次定位過程中都選擇合適的錨節(jié)點作為參考節(jié)點的總誤差為246.8173米，定位精度提高了13.95%.

圖3 LOS下參考節(jié)點選擇對誤差的影響

考慮到算法的識別修正能力是有限的，有可能存在未完全消除NLOS誤差的情況，為進一步說明選擇合適參考節(jié)點對定位精度的影響，因此假設有一個錨節(jié)點始終處于NLOS情況下未被識別和修正，并且該NLOS誤差服從N(0.33，0.082).當兩種方法都不將受NLOS影響的錨節(jié)點作為參考節(jié)點時，其定位效果和LOS下相似，定位精度提高了11.61%.而當受NLOS影響的錨節(jié)點始終作為參考節(jié)點時，實時選擇合適的錨節(jié)點作為參考節(jié)點能夠明顯的提高定位精度，仿真效果如圖4所示.選取同一個錨節(jié)點作為參考節(jié)點的50次仿真總誤差為2.6297×103米，而每次定位都選擇合適的錨節(jié)點作為參考節(jié)點的總誤差僅為1.7259×103米，定位精度提高了34.37%.

圖4 NLOS下參考節(jié)點選擇對誤差的影響

4.2 NLOS誤差均值的動態(tài)變化

室內環(huán)境的變化如溫度、濕度等會導致NLOS誤差的變化，為了驗證該算法能夠有效解決該類問題，在仿真中假設NLOS誤差均值從0.40m-0.80m變化，同時方差不變.其定位誤差(單位m)如表1所示，從表中可以看出，當由于室內環(huán)境的變化導致的NLOS誤差變化時，使用本文提出的算法可以獲得較為精準的定位結果.

表1 不同NLOS誤差均值下的定位誤差比較

4.3 NLOS下的定位結果

如圖5所示，比較了不同的方法之間的定位誤差，從圖中可以看出不經過NLOS識別和修正直接融合UWB和IMU的定位誤差和直接使用LS獲得估計位置的定位誤差比較接近.對于不分信道，只判別LOS和NLOS在短時間內的定位誤差較小，但是，隨著時間的增加，定位誤差也逐漸增加.而本文提出的方法定位誤差較小且比較穩(wěn)定.

圖5 不同定位方式的定位誤差比較

5 實驗設計與結果分析

為了進一步驗證本文提出的定位算法的定位性能，自主搭建了融合INS和UWB的定位系統(tǒng)平臺，該定位系統(tǒng)包括慣性導航模塊(BWT901CL)和UWB模塊(UA-100).實驗中，慣性導航模塊和UWB移動節(jié)點安裝在行人腳部，UWB參考節(jié)點安裝在已知位置，為了確保會出現不同的NLOS信道，將一個錨節(jié)點置于木門后面，另一個錨節(jié)點放在走廊與移動節(jié)點之間始終受墻阻擋.移動節(jié)點將采集到的錨節(jié)點的ID和對應的距離值傳輸給PC端，慣性導航模塊也將數據傳送給PC端，PC端將兩個模塊的數據進行融合.利用上述系統(tǒng)在一個6.4m×8m的空間內進行實驗.行人攜帶測量裝置繞桌椅沿著以3.6m和4.8m為邊長的長方形逆時針行走一圈.

如圖6所示，○表示LS估計為位置時選擇固定的參考節(jié)點，將UWB和INS數據融合后的誤差，×表示在LS估計中每次都選擇合適的錨節(jié)點作為參考節(jié)點的定位誤差，可以看出通過選擇合適的參考節(jié)點能夠有效的降低最終的定位誤差，提高定位精度.

圖6 LS與改進的LS的定位誤差比較

從圖7中可以看出PDR的方法起初可以獲得較為準確的定位結果，而隨著步數的增加定位誤差越來越大.而不分信道識別修正NLOS誤差，剛開始能夠獲得較為準確的定位結果，隨著時間的推移誤差有增大的趨勢.然而本文所提出的算法對于不同的障礙物引起的NLOS有很好的識別修正能力，能夠獲得較好的定位結果，定位誤差較小且穩(wěn)定，該方法下的最大定位誤差為0.25m.

圖7 不同方法的定位誤差比較

6 結論

本文研究了室內定位系統(tǒng)在非視距下的精確定位問題.首先，在離線階段建立了不同障礙物下的NLOS誤差分布.利用PDR的預測位置和NLOS誤差的概率曲線進行非視距誤差識別和緩解，利用改進的最小二乘法來提高UWB系統(tǒng)的定位精度，并對偏航角進行修正，最后利用KF融合UWB和IMU的估計位置提高定位精度.該方法充分利用了兩個系統(tǒng)的優(yōu)點，在不同的NLOS誤差情況下也能取得較好的定位結果.