劉期烈,萬志鵬,周文敏,陳 澄

(重慶郵電大學(xué) 通信與信息工程學(xué)院，重慶 400065)

0 引 言

隨著物聯(lián)網(wǎng)、智慧城市的興起，基于位置服務(wù)(Location Based Service,LBS)的應(yīng)用在人們的生活中變得愈發(fā)重要[1]。LBS的關(guān)鍵是獲取移動設(shè)備的精確位置信息，從而實現(xiàn)導(dǎo)航服務(wù)，如目標(biāo)查找、路徑規(guī)劃等[2]。為了解決室外定位技術(shù)在室內(nèi)場景的局限性，研究人員提出了多種室內(nèi)定位技術(shù)，比如紅外線定位、超聲波定位、WiFi 定位、藍(lán)牙定位等[3]，但這些定位技術(shù)只能達(dá)到分米級定位精度且受環(huán)境影響較大。而超寬帶(Ultra-wideband,UWB)定位技術(shù)定位精度可以達(dá)到厘米級，慣性測量單元(Inertial Measurement Unit,IMU)定位技術(shù)利用內(nèi)部傳感原理不受外界環(huán)境干擾，因此UWB和IMU定位技術(shù)相比于上述定位技術(shù)有顯著的優(yōu)勢[4]。

UWB定位技術(shù)憑借其定位精度高、傳輸率高、功耗低等特點，十分適用于室內(nèi)定位領(lǐng)域。但是在室內(nèi)復(fù)雜環(huán)境下，UWB定位技術(shù)容易受到非視距(Non-line of Sight,NLOS)環(huán)境影響，信號通過障礙物時產(chǎn)生的附加延遲會降低定位精度。而IMU集成了加速度計、陀螺儀等傳感器，可以測量加速度、角速度等運動狀態(tài)信息。根據(jù)上述測量信息，就可以通過行人航跡推算(Pedestrian Dead Reckoning,PDR)算法[5-6]對位置進(jìn)行解算。但是IMU器件本身存在測量誤差，且由于IMU定位使用的是相對位移，會造成誤差的累積。

使用單一定位系統(tǒng)進(jìn)行定位仍然存在很大的局限性，無法實現(xiàn)復(fù)雜室內(nèi)環(huán)境下的高精度定位。文獻(xiàn)[7-10]對不同組合定位系統(tǒng)的定位性能進(jìn)行了研究，其中UWB與IMU組合定位由于其定位精度高和優(yōu)勢互補的特性，已經(jīng)成為了組合定位的較好選擇。上述融合定位方式可以分為兩種：第一種方式利用信息冗余完成對單一定位數(shù)據(jù)的修正，但是這樣信息融合度比較低，使得定位精度不夠高；第二種方式使用多種濾波算法對兩種定位數(shù)據(jù)進(jìn)行融合，但是當(dāng)其中一種定位數(shù)據(jù)誤差較大時會導(dǎo)致融合定位精度變低。然而，針對上述兩種融合定位的缺陷，還沒有文獻(xiàn)進(jìn)行深入、全面的研究。

因此，本文提出了一種將兩者結(jié)合的改進(jìn)融合定位模型。首先在UWB測量模型中引入了差分氣壓計，利用其高度信息對UWB定位結(jié)果進(jìn)行修正。然后在融合算法中引入IMU累積誤差判定門限，并使用卡爾曼濾波算法對UWB和IMU定位數(shù)據(jù)進(jìn)行融合，使得最終可以在復(fù)雜的室內(nèi)環(huán)境中獲得較高的定位精度。

本文的主要貢獻(xiàn)如下：一是在UWB測量模型中引入了差分氣壓計，并提出了一種差分加權(quán)UWB定位算法，降低NLOS環(huán)境對UWB定位的影響；二是在融合算法中引入IMU累積誤差門限，通過閾值判定提高融合定位精度；三是用UWB定位系統(tǒng)和IMU定位系統(tǒng)在NLOS環(huán)境中測試，獲得真實的測量數(shù)據(jù)，并在Matlab中使用所提的算法對測量數(shù)據(jù)進(jìn)行位置解算，以此來評估該算法的定位效果。在實際NLOS環(huán)境測試中，對于UWB測量模型，差分加權(quán)定位算法在定位精度上相比于三邊定位算法提高了38.09%；對于融合定位，在長時間進(jìn)行定位時，改進(jìn)融合定位模型在定位精度上相比于基礎(chǔ)融合定位模型提高了45.71%。

1 系統(tǒng)建模

本節(jié)對UWB/IM融合定位系統(tǒng)進(jìn)行建模，該系統(tǒng)首先使用UWB定位技術(shù)、IMU定位技術(shù)進(jìn)行單一定位，然后將兩者數(shù)據(jù)通過卡爾曼濾波器進(jìn)行融合，可以有效地將兩者的優(yōu)勢進(jìn)行互補，最終可以得到魯棒性高且更準(zhǔn)確的定位結(jié)果。

1.1 UWB測量模型

信號到達(dá)時間(Time of Arrival,TOA)是UWB定位常用的定位方法，該方法是基于測距完成定位的。而基于測距完成的定位算法中，最常用的有三邊定位算法和最小二乘定位算法。

在理想環(huán)境下的二維平面內(nèi)，根據(jù)三邊定位算法，以三個基站作為圓心，以基于TOA測得的基站與標(biāo)簽距離作為半徑，可以畫出3個圓并交于一點，交點就是標(biāo)簽所在位置，如圖1所示。

圖1 理想情況下的TOA方法

(1)

但是實際上室內(nèi)環(huán)境比較復(fù)雜，當(dāng)對室內(nèi)人員或者物品進(jìn)行定位時，常常是LOS與NLOS混合的環(huán)境而不是單一的LOS或者NLOS環(huán)境。受NLOS影響，TOA測距值比真實值大，使得由測距值得到的3個圓不能相交于一個點，而是相交于一個區(qū)域，如圖2所示。這時可使用最小二乘法求解目標(biāo)最優(yōu)估計位置。

圖2 NLOS影響下的TOA方法

將方程組(1)擴展到N個基站，并用第2～N個等式分別減去第1個等式，可得方程組(2)：

(2)

可轉(zhuǎn)換成矩陣形式：

AX=b，

(3)

(4)

(5)

(6)

利用最小二乘法求解，可得到位置的估計值

(7)

1.2 IMU測量模型

IMU定位技術(shù)把牛頓力學(xué)當(dāng)作基礎(chǔ)，IMU通過集成的陀螺儀獲取當(dāng)前采樣點下載體的角速度信息，并通過該值來解算加速度計與導(dǎo)航坐標(biāo)系的相對方向角，進(jìn)而得到當(dāng)前采樣點下載體的運動姿態(tài)和航向角等信息。同時，通過加速度計可以獲取在載體坐標(biāo)系下當(dāng)前采樣點的加速度信息，再使用姿態(tài)矩陣進(jìn)行坐標(biāo)系的轉(zhuǎn)換并去除重力加速度的影響，即可獲得在導(dǎo)航坐標(biāo)系下當(dāng)前采樣點的載體加速度。然后對加速度進(jìn)行一次積分和二次積分，分別得到當(dāng)前采樣點相對于上一采樣點的速度、位移，再以上一采樣點的速度和位置為基礎(chǔ)，最終得到當(dāng)前采樣點下的速度和位置，如下式所示：

(8)

式中：a為導(dǎo)航坐標(biāo)系下的加速度，ω為陀螺儀測得的角速度，t為運動時間，v0、s0和θ0分別表示目標(biāo)在上一采樣點的速度、位移和運動方位角，v1、s1和θ1分別表示目標(biāo)在當(dāng)前采樣點的速度、位移和運動方位角。

IMU的定位精度取決于其加速度計和陀螺儀等傳感器件的測量精度，測量加速度和角速度而產(chǎn)生的微小誤差，會分別在一次積分、二次積分獲得速度、位置的過程中累積。同時因為IMU積分計算得到的是相對位移，即當(dāng)前采樣點的位置信息是由基于上一采樣點的位置信息計算得到的，所以位置誤差會隨時間累積，長時間下來會因為該誤差過大而導(dǎo)致位置信息無法使用。

1.3 數(shù)據(jù)融合算法

常見的UWB/IMU融合定位模型如圖3所示。分別通過UWB與IMU測量模型得到單一定位值，兩者差值即卡爾曼濾波器觀測向量。將觀測向量輸入卡爾曼濾波器得到IMU定位誤差估計值，再用來對IMU測得定位值進(jìn)行補償，即可獲取最終定位結(jié)果。

圖3 UWB/IMU融合定位模型

下面對上述融合定位模型中的融合算法進(jìn)行推導(dǎo)，主要對其中的狀態(tài)向量、狀態(tài)方程、觀測向量以及觀測方程進(jìn)行描述。由于本文UWB與IMU僅在二維平面內(nèi)進(jìn)行信息融合，不涉及高度，因此各參數(shù)只考慮兩個元素(x和y)。

狀態(tài)向量是IMU定位時的速度誤差和位置誤差，即

Xk=[δpk,xδpk,yδvk,xδvk,y]T。

(9)

式中：(δpk,x,δpk,y) 是位置誤差，(δvk,x,δvk,y) 是速度誤差。

狀態(tài)方程為

Xk+1=FXk+wk。

(10)

式中：wk是系統(tǒng)噪聲，且wk～N(0,Qk)，Qk是狀態(tài)轉(zhuǎn)移噪聲協(xié)方差矩陣；F是狀態(tài)轉(zhuǎn)移矩陣，

(11)

式中：Δt是采樣周期。

觀測向量是IMU解算的坐標(biāo)與UWB解算的坐標(biāo)的差值，即

(12)

觀測方程為

Zk=HXk+nk。

(13)

式中：nk是測量噪聲，且nk～N(0,Rk)，Rk是觀測噪聲協(xié)方差矩陣；H是測量矩陣，

(14)

基于狀態(tài)方程和觀測方程，即可使用卡爾曼濾波算法進(jìn)行時間更新和觀測更新，完成數(shù)據(jù)融合。具體步驟如下：

首先對狀態(tài)向量和誤差協(xié)方差進(jìn)行預(yù)測：

(15)

Pk+1|k=FPkFT+Qk。

(16)

然后計算卡爾曼增益：

Kk+1=Pk+1|kHT(HPk+1|kHT+Rk)-1。

(17)

最后對狀態(tài)向量和誤差協(xié)方差進(jìn)行更新：

(18)

Pk+1=(I-Kk+1H)Pk+1|k。

(19)

2 改進(jìn)的融合定位模型

本文提出的改進(jìn)融合定位模型如圖4所示，在圖3融合定位模型上進(jìn)行了兩步改進(jìn)：首先在模型中引入了差分氣壓計，使用其在高度信息上的冗余對UWB測量模型進(jìn)行優(yōu)化；然后在融合算法中引入了IMU累積誤差判定門限，對IMU所受累積誤差進(jìn)行判定，對融合算法進(jìn)行優(yōu)化。

圖4 改進(jìn)的融合定位模型

2.1 UWB測量模型優(yōu)化

上述使用最小二乘法解算位置，其實是默認(rèn)為所有測距值賦予相同的權(quán)重，而顯然每個測距值受NLOS誤差影響程度不一樣。應(yīng)該對每個測距值所受誤差大小進(jìn)行判定，對誤差小的測距值賦予較大的權(quán)重，對誤差大的測距值賦予較小的權(quán)重。

本文提出了一種通過差分氣壓計測高的方法來對測距值所受誤差大小進(jìn)行判斷，然后即可根據(jù)所受誤差大小對不同測距值賦予不同權(quán)重，再使用加權(quán)最小二乘法來求解最優(yōu)位置估計值，即采取NLOS鑒別及誤差削弱的方法削弱NLOS對測距的影響。

2.1.1 NLOS鑒別

圖5 標(biāo)簽兩個解關(guān)于基站平面對稱

(20)

再將這些四面體解算出的標(biāo)簽高度hm與差分氣壓計測得標(biāo)簽高度hbar進(jìn)行比較，得到每個組合的高度差值δhm，以此作為每個基站組合測距受NLOS誤差影響情況。

2.1.2 降低NLOS誤差影響

|δh(k)|=|h(k)UWB-h(k)bar|，

(21)

(22)

式中：λ1表示經(jīng)驗閾值；h表示氣壓計測高精度，本文使用的氣壓計模塊為SPL06-001，該模塊在高線性模式下的測高精度可達(dá)5 cm[13]。

此時分為兩種工作模式，下面分別介紹。

(1)F1(k)=1

若F1(k) 的值為“1”，判定UWB定位較準(zhǔn)，此時直接選擇此種定位結(jié)果所得到的二維坐標(biāo)(x,y)，加上差分氣壓計所得高度hbar，得到三維定位結(jié)果(x,y,hbar)。

(2)F1(k)！=1

通過測距解算出的高度確定每個測距值的權(quán)值，并通過加權(quán)最小二乘法得到二維定位坐標(biāo)(x,y) ，加上差分氣壓計所得高度hbar，得到三維定位結(jié)果(x,y,hbar)。具體過程如下：

首先，求每個基站參與的組合的高度差值之和，再除以個數(shù)，即可得到每個基站參與造成的平均高度差δhi，然后根據(jù)該差值即可確定該基站測距值的可信度βi：

βi=1/δhi。

(23)

然后，已知標(biāo)簽與每個基站三維測距值di3d，使用勾股定理(式(24))即可得到標(biāo)簽與每個基站測得的水平距離di2d：

(24)

最后，類比式(7)，可以求解加權(quán)最小二乘的標(biāo)簽位置估計值

(25)

式中：

(26)

2.2 融合算法優(yōu)化

IMU在長時間進(jìn)行定位時漂移誤差會導(dǎo)致較大的累積誤差。UWB和IMU分別完成定位后，將這兩個定位結(jié)果相減，將該差值δP(k)作為觀測量輸入卡爾曼濾波之前，可求其絕對值并對該值進(jìn)行判斷:

|δP(k)|=|P(k)IMU-P(k)UWB|，

(27)

(28)

式中：λ2表示經(jīng)驗閾值；p表示UWB與IMU平均定位誤差之和，在NLOS環(huán)境下短時間內(nèi)，UWB與IMU的平均定位誤差分別為26 cm、22 cm。

若F2(k)的值為“1”，判定此時UWB與IMU定位結(jié)果相差較大。若同時有F1(k)的值為“1”，則說明此時UWB定位結(jié)果比較準(zhǔn)確，IMU定位結(jié)果累積誤差較大。可引入矯正因子δJ，其大小為k時刻IMU與UWB定位差值：

δJ=P(k)IMU-P(k)UWB。

(29)

因此，在k+1時刻及之后的IMU測量值都要引入該矯正因子：

P(k+1)IMU=P(k+1)IMU-δJ。

(30)

對應(yīng)地，要在k+1時刻及之后的觀測向量中引入矯正因子：

(31)

在融合算法中引入IMU累積誤差判定門限可以對IMU測量誤差進(jìn)行判別，進(jìn)而在IMU測量值以及融合算法的觀測向量中引入矯正因子，最終得到精確的融合定位結(jié)果。

3 仿真與分析

本文使用的是基于DW1000芯片的Decawave套件，該套件主要由4個基站1個標(biāo)簽組成，采用雙向測距(Two Way Ranging，TWR)方法來進(jìn)行測距，數(shù)據(jù)刷新率可達(dá)10 Hz。另外，主基站上擴展了無線WiFi模塊來實現(xiàn)主基站與上位機的無線通信，可在上位機顯示實時軌跡。

使用的慣導(dǎo)模塊是由荷蘭Xsens公司開發(fā)的MTI600，該模塊集成了三軸加速計，測量范圍±10g，可以測得三維加速度值；同時集成了三軸陀螺儀，測量范圍±2 000 °/s，可以測得三維姿態(tài)角，其數(shù)據(jù)刷新率可達(dá)1 000 Hz。另外，該模塊體積小、精度高，非常適合與UWB模塊進(jìn)行融合定位。該模塊具體參數(shù)如表1所示。

表1 IMU的性能參數(shù)

同時，在UWB模塊上集成了氣壓計模塊SPL06-001，該模塊測高分辨率很高，在高線性模式下可以達(dá)到0.006 hPa(5 cm)。同時可以通過差分氣壓法，即在標(biāo)簽和主基站上都集成了該氣壓計模塊，這樣可以對氣壓計偏差進(jìn)行校準(zhǔn)，獲得更為準(zhǔn)確的測壓信息，進(jìn)而得到較為準(zhǔn)確的標(biāo)簽高度信息。

本文定位實驗選在重慶郵電大學(xué)信科大廈2樓大廳，場地大小為3.6 m×5.4 m，如圖6所示。為了驗證本文提出的優(yōu)化方法，分別對UWB定位、融合定位進(jìn)行測試實驗。

圖6 測試環(huán)境

3.1 UWB測量模型對比實驗

由上述分析可知，UWB定位受NLOS測距影響較大，因此這里在NLOS環(huán)境下對UWB分別進(jìn)行靜態(tài)和動態(tài)定位實驗。

3.1.1 靜態(tài)實驗

此處實驗將待測標(biāo)簽放在圖6所示環(huán)境的水平地面上，并分別在如表2所示8個點放置10 s進(jìn)行數(shù)據(jù)采集，開展NLOS環(huán)境下的UWB靜態(tài)定位實驗。表2所示為NLOS環(huán)境下靜態(tài)測試點性能統(tǒng)計結(jié)果。

表2 UWB在NLOS環(huán)境下靜態(tài)定位

由表2可知，在NLOS環(huán)境下進(jìn)行靜態(tài)點定位，三邊定位算法的均方根誤差最小值為19.82 cm，最大值為61.73 cm，平均定位誤差為39.97 cm；差分加權(quán)定位算法的均方根誤差最小值為12.72 cm，最大值為35.63 cm，平均定位誤差為23.79 cm。可以發(fā)現(xiàn)，UWB定位精度受NLOS環(huán)境影響較大，而本文提出的差分加權(quán)定位算法相比于三邊定位算法定位精度提高了40.04%，具有較好的抗NLOS能力。

3.1.2 動態(tài)實驗

此處實驗讓行人手持UWB模塊放在身前，然后在圖6所示測試環(huán)境下按預(yù)定軌跡行走，開展NLOS環(huán)境下的UWB動態(tài)定位實驗。

圖7所示為NLOS環(huán)境下兩種定位方法的UWB定位軌跡，表3所示為UWB動態(tài)定位誤差。

圖7 NLOS環(huán)境下UWB動態(tài)定位

表3 UWB動態(tài)定位誤差

結(jié)合圖7和表3可知，在NLOS環(huán)境下，使用三邊定位算法的X、Y軸最大誤差分別為50.27 cm和69.08 cm，X、Y軸平均誤差分別為20.51 cm和32.49 cm，平均定位誤差為42 cm；使用差分加權(quán)定位算法X、Y軸最大誤差分別為34.48 cm和44.84 cm，X、Y軸平均誤差分別為13.17 cm和17.29 cm，平均定位誤差為26 cm。差分加權(quán)定位算法相比于三邊定位算法在定位精度上提高了38.09%，可以對UWB受到的NLOS誤差進(jìn)行有效削弱，得到與真實路線更吻合的軌跡。

3.2 融合定位對比實驗

由3.1節(jié)可知，UWB定位受NLOS環(huán)境影響較大，此處使用UWB與IMU進(jìn)行數(shù)據(jù)融合，進(jìn)一步降低UWB受NLOS環(huán)境的影響。由于本文進(jìn)行三維定位時的高度信息由集成在UWB模塊上的氣壓計模塊提供，而UWB模塊與IMU模塊僅在二維平面內(nèi)進(jìn)行信息融合，因此可以忽略UWB模塊與IMU模塊在高度上的差異。讓實驗人員手持UWB模塊放在身前，并將IMU模塊放置在腳尖，然后在圖6所示測試環(huán)境下按預(yù)定軌跡行走，開展NLOS環(huán)境下的短時間和長時間融合定位實驗。

3.2.1 短時間融合定位實驗

圖8所示為與圖7相同條件下的短時間融合定位軌跡。結(jié)合圖7可知，在NLOS環(huán)境下進(jìn)行短時間定位時，IMU平均定位誤差為22 cm，此時三邊定位算法的平均定位誤差為42 cm，融合定位平均定位誤差為28 cm；差分加權(quán)定位算法的平均定位誤差為26 cm，改進(jìn)融合定位平均定位誤差為19 cm?？梢钥闯鲈贜LOS情況下，融合定位比單一UWB定位方法的定位精度更高，更符合實際行走軌跡。

圖8 短時間融合定位

另外可以發(fā)現(xiàn)，在短時間NLOS環(huán)境下進(jìn)行融合定位，UWB受到NLOS距離的影響導(dǎo)致定位誤差較大，而IMU不受環(huán)境影響且在短時間內(nèi)定位精度較高，因此在這種情況下，UWB不僅不能對IMU起到修正作用，基礎(chǔ)融合定位反而會取得比單一IMU定位更差的定位效果。而對于改進(jìn)融合定位，由于降低了UWB受到NLOS距離的影響，可以得到更高的UWB定位精度，因此改進(jìn)融合定位可以對短時間內(nèi)IMU定位起到一定的修正效果；相比于基礎(chǔ)融合定位，改進(jìn)融合定位可以在定位精度上提高32.15%。

3.2.2 長時間融合定位實驗

如圖9所示為繞預(yù)定軌跡行走多圈即長時間行走之后，兩種融合定位方法軌跡對比。表4所示為兩種融合定位方法的定位誤差。

圖9 長時間融合定位

表4 融合定位誤差

由圖9可知，在長時間行走后，差分加權(quán)定位算法得到的UWB平均定位誤差為27 cm，定位效果與之前相差不大；而IMU平均定位誤差為60 cm，可以看出該誤差遠(yuǎn)大于短時間行走時的IMU平均定位誤差。這是因為IMU定位存在累積誤差，也說明了對IMU累積誤差進(jìn)行削弱的必要性。

結(jié)合圖9和表4可知，對于基礎(chǔ)融合定位，X、Y軸最大誤差分別為46.71 cm和41.23 cm，X、Y軸平均誤差分別為26.35 cm和23.78 cm；對于改進(jìn)融合定位，X、Y軸最大誤差分別為26.48 cm和31.74 cm，X、Y軸平均誤差分別12.25 cm和16.52 cm。

另外可以發(fā)現(xiàn)，對于基礎(chǔ)融合定位，不僅UWB定位精度受NLOS測距影響大大降低，IMU定位產(chǎn)生的累積誤差也很大，導(dǎo)致基礎(chǔ)融合定位方法定位效果較差，平均定位誤差為35 cm。因此對于改進(jìn)融合定位方法，可以在優(yōu)化UWB測量模型的基礎(chǔ)上，在融合算法中引入矯正因子，動態(tài)糾正IMU累積誤差，就能獲得更好的融合定位效果，平均定位誤差為19 cm。改進(jìn)的融合定位模型相比于基礎(chǔ)融合定位模型，定位精度提高了45.71%。

另外，在融合定位模型中引入矯正因子，只需要進(jìn)行常數(shù)次運算，時間復(fù)雜度為O(1)，故改進(jìn)的融合定位模型可以滿足定位實時性要求。

4 結(jié)束語

單一UWB定位系統(tǒng)存在很大的局限性，無法適用于大多數(shù)場景，如隧道定位、超市導(dǎo)航等，因此本文提出了一種UWB/IMU融合定位的優(yōu)化方法。首先，針對UWB定位時受NLOS誤差影響大，提出了一種差分權(quán)值定位算法，并在NLOS環(huán)境中進(jìn)行靜態(tài)和動態(tài)定位對比實驗，驗證了該算法比三邊定位算法具有更高的精確度。然后，針對IMU定位時受累積誤差影響大，在融合算法中引入IMU測量值矯正因子，并在動態(tài)NLOS環(huán)境下進(jìn)行融合定位對比實驗，驗證了在融合算法中引入矯正因子可以進(jìn)一步提高融合定位的精度，使得最終可以在復(fù)雜的室內(nèi)環(huán)境中，獲取一個較為準(zhǔn)確的融合定位結(jié)果。

但本文使用的方法存在兩點需優(yōu)化之處：一是在融合定位中進(jìn)行的改進(jìn)，主要針對于單一定位精度及融合模型中的測量值，可以進(jìn)一步對融合時使用的卡爾曼濾波算法進(jìn)行改進(jìn)；二是使用多源信息進(jìn)行融合定位，功耗會比單一系統(tǒng)更高，可以考慮利用UWB、IMU的測量信息實現(xiàn)睡眠機制，來保證整個系統(tǒng)的續(xù)航能力。