收稿日期：2023-04-10

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目（11875031）

作者簡(jiǎn)介：董璇（1998-），女，山西陽泉人，在讀碩士，研究方向?yàn)閿?shù)字通信與信號(hào)處理.E-mail：2390542709@qq.com.

*通信作者：姜恩華（1974-），男，安徽淮北人，副教授，碩士，研究方向?yàn)閿?shù)字通信與信號(hào)處理.E-mail：1251007451@qq.com.

文章編號(hào)：2095-6991（2024）04-0059-05

摘要：UWB定位技術(shù)定位精度高、系統(tǒng)結(jié)構(gòu)搭建簡(jiǎn)單，但在遮擋物影響下性能降低.IMU計(jì)算目標(biāo)位置不受周圍環(huán)境干擾，長(zhǎng)時(shí)間工作會(huì)存在誤差累積.本文提出一種基于平方根擴(kuò)展卡爾曼濾波法（Square Root-Extended Kalman Filter， SR-EKF）的慣性測(cè)量單元（Inertial Measurement Unit， IMU）/超寬帶（Ultra-Wideband， UWB）組合室內(nèi)定位技術(shù).平方根擴(kuò)展卡爾曼濾波融合IMU/UWB測(cè)量數(shù)據(jù)，通過提高描述狀態(tài)量的協(xié)方差矩陣精確度，進(jìn)而對(duì)狀態(tài)量校正.同時(shí)能夠補(bǔ)償IMU誤差累積，降低加速度計(jì)和陀螺儀零偏，克服復(fù)雜環(huán)境下未知影響，提高定位精度.實(shí)驗(yàn)結(jié)果顯示，定位算法能夠有效反饋IMU進(jìn)行誤差補(bǔ)償，并縮小算法估計(jì)位置與真實(shí)位置之間的誤差值，實(shí)現(xiàn)高精度目標(biāo)定位.

關(guān)鍵詞：超寬帶；IMU；室內(nèi)定位；平方根擴(kuò)展卡爾曼濾波

中圖分類號(hào)：TN911""" 文獻(xiàn)標(biāo)志碼：A

UWB/IMU Combined Indoor Positioning System Based on SR-EKF

DONG Xuan， JIANG En-hua*

（School of Physics and Electronic Information， Huaibei Normal University， Huaibei 235000， Anhui， China）

Abstract：UWB positioning technology has high positioning accuracy and simple system structure construction， but its performance is reduced under the influence of obstructions. The IMU calculates the target position without interference from the surrounding environment， and errors will accumulate when working for a long time. An inertial measurement unit （IMU）/Ultra Wideband （UWB） integrated indoor positioning technology based on square root extended Kalman Filter （SR-EKF） is proposed. The square root extended Kalman filter fuses the IMU/UWB measurement data， and then corrects the state quantity by improving the accuracy of the covariance matrix describing the state quantity. At the same time， it can compensate for the accumulation of IMU errors， reduce the zero bias of the accelerometer and gyroscope， overcome unknown influences in complex environments， and improve positioning accuracy. With the help of MATLAB platform simulation experiments， the simulation results show that this positioning algorithm can effectively feedback the IMU for error compensation， and reduce the error value between the algorithm estimated position and the real position， and realize high-precision target positioning.

Key words：ultra-wideband; IMU; indoor positioning; SR-EKF

UWB室內(nèi)定位精度較已有技術(shù)提高了很多，能精確到厘米級(jí)的誤差范圍，實(shí)現(xiàn)目標(biāo)精準(zhǔn)定位滿足人們需求，適用性更高.從精度、覆蓋范圍和部署成本方面來講，UWB技術(shù)用于高精度室內(nèi)定位是最有前途的技術(shù)之一［1］.發(fā)送占用高帶寬的窄脈沖波傳輸數(shù)據(jù)，降低信道衰落，實(shí)現(xiàn)小功耗且安全的傳輸信號(hào)，缺點(diǎn)是傳輸過程中未知的非視線（non line of sight，NLOS）效應(yīng)導(dǎo)致定位誤差變大，在這種情況下，僅使用UWB定位技術(shù)將無法滿足實(shí)際需求.

借助慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）的組成部分IMU，輔助UWB定位技術(shù)，IMU測(cè)量原理是根據(jù)目標(biāo)原始位置以及速度信息和姿態(tài)，推測(cè)下一時(shí)刻狀態(tài)下的物體位置，IMU能較為精準(zhǔn)地測(cè)量運(yùn)動(dòng)載體的加速度值，檢測(cè)方向角運(yùn)動(dòng)，同時(shí)不受周圍環(huán)境的干擾.由于慣導(dǎo)誤差隨著時(shí)間不斷積累［2］，無法長(zhǎng)期提供較高的定位精度，本文基于平方根擴(kuò)展卡爾曼濾波法融合UWB/IMU數(shù)據(jù)，提高定位精度.

1" UWB定位系統(tǒng)

UWB系統(tǒng)結(jié)構(gòu)簡(jiǎn)單，分為發(fā)送和接收兩部分，把基站和標(biāo)簽作為收發(fā)設(shè)備，進(jìn)行信號(hào)傳輸.標(biāo)簽身上攜帶著IMU，同時(shí)獲取UWB與IMU的測(cè)量數(shù)據(jù).

根據(jù)基站與標(biāo)簽之間的信號(hào)飛行時(shí)間計(jì)算距離，然后由三邊測(cè)距定位算法求解目標(biāo)位置.常用的算法有基于到達(dá)時(shí)間差（TDOA）［3］的經(jīng)典算法，如Chan算法和Taylor迭代算法，基于到達(dá)時(shí)間（TOA）［4］定位方法，如最小二乘法 （Least Square， LS）等.本文采用TOA定位方法，實(shí)現(xiàn)簡(jiǎn)單，但要求節(jié)點(diǎn)間精確保持時(shí)間同步，采用雙邊雙向測(cè)距法消除時(shí)鐘同步要求，進(jìn)行通信測(cè)距的設(shè)備獨(dú)立工作［5］，測(cè)距原理如圖1所示.

（1） UWB技術(shù)獲取初始定位坐標(biāo)

該技術(shù)包括以下步驟.

步驟1：標(biāo)記傳輸信號(hào)的飛行時(shí)間，根據(jù)飛行時(shí)間獲取標(biāo)簽、基站之間的距離.計(jì)算信號(hào)傳輸時(shí)間Tprop，其計(jì)算公式為：

Tround1=Treply1+2Tprop，

Tround2=Treply2+2Tprop，（1）

其中，Tround1、Tround2為基站與標(biāo)簽之間傳輸信號(hào)的飛行時(shí)間.將上式進(jìn)行整理得：

Tprop=

Tround1×Tround2－Treply1×Treply2Tround1+Tround2+Treply1+Treply2.（2）

步驟2：計(jì)算標(biāo)簽與基站之間的偽距離.信號(hào)傳播速度與公式（2）得到的Tprop相乘，可得：

di=c×（Tprop）i（i=1，2，…，n），（3）

其中，di表示基站i與標(biāo)簽的距離，c是信號(hào)傳播速度.

（2） 三邊測(cè)距［6-7］定位算法確定目標(biāo)位置

以基站坐標(biāo)xi，yii=1，2，3為圓心，基站與標(biāo)簽之間的距離di為半徑做圓交于一點(diǎn)，為目標(biāo)位置x，y，算法原理如圖2所示.

根據(jù)目標(biāo)與基站i坐標(biāo)計(jì)算兩者間距離，可得：

di=xi－x2+yi－y2，i=1，2，3.（4）

將公式展開變換得

x2i+y2i+x2+y2－2xix－2yiy=d2i，（5）

令Ki=x2i+y2i，R=x2+y2，得

d2i－Ki=－2xix－2yiy+R.（6）

將公式（6）用矩陣形式可表示為：

d21－K1d22－K2d2i－Ki=

－2x1－2y11－2x2－2y21－2xi－2yi1xyR. （7）

Y=AX.（8）

求解公式（8）中矩陣X便可得到標(biāo)簽位置x，y，利用最小二乘法得：

X=ATA－1ATY.（9）

通過式（9）求解得到的位置坐標(biāo)x，y，將其作為IMU模塊預(yù)測(cè)下一時(shí)刻位置的初始值.

2" IMU慣導(dǎo)系統(tǒng)

2.1" IMU工作原理

慣性測(cè)量單元測(cè)量數(shù)據(jù)不受所處環(huán)境干擾，其傳感器獲取相關(guān)數(shù)據(jù)信息，核心器件為加速度計(jì)和陀螺儀.通過航向推算下一時(shí)刻目標(biāo)位置，只需要當(dāng)前測(cè)量數(shù)據(jù)與上一時(shí)刻的位置坐標(biāo)，IMU位姿解算如圖3所示.

圖3中，傳感器測(cè)量得到的數(shù)據(jù)信息相對(duì)于慣性參考系下，加速度計(jì)輸出值稱比力f，f實(shí)際上是物體加速度加上重力加速度；應(yīng)用

陀螺儀測(cè)量得到相互正交的3個(gè)方向角速度值.加速度計(jì)和陀螺儀采集到的所有數(shù)據(jù)不能直接用來參與計(jì)算，通過坐標(biāo)系轉(zhuǎn)換到導(dǎo)航系上，進(jìn)而得到目標(biāo)物體的速度、位置等導(dǎo)航信息.

2.2" IMU姿態(tài)算法

本文以東-北-天（ENU）為軸的地理坐標(biāo)系作為導(dǎo)航坐標(biāo)對(duì)定位過程中導(dǎo)航參數(shù)的積分解算［8］.構(gòu)造旋轉(zhuǎn)矩陣的方法有多種，本文選用四元數(shù)法，四元數(shù)由1個(gè)實(shí)數(shù)和3個(gè)虛數(shù)構(gòu)成［9］，在值為1（規(guī)范化）條件下，只需要保存3個(gè)元素，具體公式為：

q20+q21+q22+q23=1，（10）

q=q0q1q2q3.（11）

由文獻(xiàn)［10］可知，四元數(shù)表示載體坐標(biāo)系變換到導(dǎo)航坐標(biāo)系的旋轉(zhuǎn)矩陣為：

Cnb=q20+q21-q22－q232（q1q2－q0q3）2（q1q3+q0q2）2（q1q2+q0q3）q20－q21+q22－q232（q2q3－q0q1）2（q1q3－q0q2）2（q2q3+q0q1）q20－q21-q22+q23，（12）

其中，陀螺儀輸出的角速度值與采樣時(shí)間相乘構(gòu)成等效旋轉(zhuǎn)矢量，即姿態(tài)變換量，轉(zhuǎn)換為變換四元數(shù)構(gòu)造旋轉(zhuǎn)矩陣，加速度向量通過四元數(shù)做3D旋轉(zhuǎn)，比力變換后對(duì)其二重積分，根據(jù)上一時(shí)刻的位置向量得到下一時(shí)刻的目標(biāo)估計(jì)位置.

3" 擴(kuò)展卡爾曼濾波

3.1" 擴(kuò)展卡爾曼濾波原理

擴(kuò)展卡爾曼濾波［12］算法的狀態(tài)和觀測(cè)方程分別為：

xk=f（xk－1，uk－1）+wk，yk=h（xk，nk）+vk，（13）

其中，在離散時(shí)間k時(shí)刻的狀態(tài)量xk，yk為觀測(cè)量，wk和vk是均值為零、方差分別為R、Q的高斯白噪聲，f、h分別表示狀態(tài)轉(zhuǎn)移過程和觀測(cè)過程.

信號(hào)預(yù)測(cè)可表示為：

x∧kk－1=f（x∧k－1k－1） .（14）

Pkk-1=Fk－1Pk－1k－1FTk－1+Γk－1Qk－1ΓTk－1.（15）

其中，x∧k|k－1表示k時(shí)刻的系統(tǒng)狀態(tài)量預(yù)測(cè)向量，F(xiàn)k－1表示k-1時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，Pk|k－1表示k時(shí)刻的協(xié)方差預(yù)測(cè)矩陣，Qk－1表示過程噪聲的協(xié)方差矩陣，Γk－1表示噪聲增益矩陣.

狀態(tài)更新過程可表示為：

Sk=HkPkk－1HTk+Rk.（16）

Kk=Pkk－1HTkS－1k.（17）

x∧k|k=x∧k|k－1+KkZ∧k=

x∧k|k－1+Kk（yk－h(huán)（x∧k|k－1））.（18）

協(xié)方差更新過程可表示為：

Pk|k=（1－KkHk）Pk|k－1.（19）

式（16）至（19）中，Kk為卡爾曼增益矩陣，Hk為量測(cè)矩陣，h（x∧k|k－1）表示預(yù)測(cè)量值，x∧k|k表示修正后的狀態(tài)量估計(jì)值.

3.2" 平方根擴(kuò)展卡爾曼濾波

本文濾波方法采用平方根擴(kuò)展卡爾曼濾波法，通過直接計(jì)算協(xié)方差矩陣的平方根［13］，代替協(xié)方差矩陣提高定位精度.

狀態(tài)量x的協(xié)方差預(yù)測(cè)矩陣用來描述x的精確度.x中含有各種數(shù)據(jù)值，其精度值有高有低，就有可能導(dǎo)致Pk矩陣內(nèi)部元素之間的數(shù)量級(jí)差別大，在運(yùn)行過程中由于計(jì)算機(jī)存儲(chǔ)位數(shù)有限的情況下，會(huì)導(dǎo)致誤差越來越大.故引入對(duì)矩陣元素開平方的方法降低誤差.如果Pk的平方根是Ak，那么可表示為：

Pk=AkATk.（20）

根據(jù)喬萊斯基分解法，得到形式為上三角陣的Ak.

更新濾波增益，由預(yù)測(cè)方程Pk|k－1=Ak|k－1ATk|k－1，得到：

ak=ATk|k－1HTk.（21）

bk=（aTkak+Rk）－1.（22）

濾波增益可表示為：

Kk=bkAk|k－1ak.（23）

狀態(tài)量更新可表示為：

x∧k=x∧k|k－1+KkZ∧k=

x∧k|k－1+Kk（Zk－Ηkx∧k|k－1）.（24）

4" 基于SR-EKF的組合室內(nèi)定位實(shí)驗(yàn)設(shè)計(jì)

4.1" 組合定位系統(tǒng)

實(shí)際生活中，僅用一種定位技術(shù)在未知室內(nèi)環(huán)境下測(cè)量得到的位置坐標(biāo)精度可能不高.為滿足實(shí)際需求構(gòu)建基于SR-EKF的IMU系統(tǒng)與UWB技術(shù)組合定位，組織定位原理如圖4所示.基站發(fā)送信號(hào)過程中受到遮擋物干擾導(dǎo)致測(cè)距不準(zhǔn)，IMU輔助定位修正位置變量.加速度計(jì)和陀螺儀產(chǎn)生的偏置分別用于校正原始加速度計(jì)和陀螺儀的值，通過補(bǔ)償各單元的零偏.

在圖4中，將位置s、速度v、四元數(shù)q、加速度偏差ba、角速度偏差bω作為狀態(tài)量進(jìn)行建模［14］，微分方程可表示為：

x·k－1=（s·v·q·ba·bω·）T，（25）

x∧k|k－1=xk－1+x·k－1Δt，（26）

其中，x∧k|k－1表示IMU對(duì)狀態(tài)量位置的預(yù)測(cè)值，Δt是IMU的采樣時(shí)間.

計(jì)算狀態(tài)量的位置預(yù)測(cè)值和UWB各基站位置之間的距離li（i是基站個(gè)數(shù)）.由式（26）求得狀態(tài)量位置預(yù)測(cè)值，通過兩點(diǎn)間距離計(jì)算公式（4）求得li.將UWB系統(tǒng)輸出的偽距di（真實(shí)距離加噪聲距離）與預(yù)測(cè)距離li相減作為濾波方程的量測(cè)向量Z∧k優(yōu)化狀態(tài)量，同時(shí)更新量測(cè)矩陣.狀態(tài)量更新公式可表示為：

x∧k|k=x∧k|k－1+KkZ∧k，（27）

其中，Z∧k=（d1－l1，….dn－ln）T.

4.2" 實(shí)驗(yàn)及結(jié)果分析

在室內(nèi)環(huán)境下搭建一個(gè)圓形跑道模擬實(shí)際場(chǎng)景，驗(yàn)證算法準(zhǔn)確性.將遙控小車作為移動(dòng)標(biāo)簽，與基站進(jìn)行信號(hào)傳送，并將IMU模塊固定在小車上，通過加速度計(jì)與陀螺儀測(cè)量數(shù)據(jù).搭建好環(huán)境后控制小車圍繞跑道運(yùn)動(dòng)，分別獲得17200幀相關(guān)數(shù)據(jù).其中，IMU及UWB測(cè)量的采樣頻率設(shè)置為200 Hz，采樣時(shí)間為0.05 s.在本次實(shí)驗(yàn)中共搭建4個(gè)基站，呈包圍狀放置.人為隨機(jī)擺放擋板、玻璃，模擬實(shí)際生活中遇到的障礙物.本文借助MATLAB平臺(tái)運(yùn)行實(shí)驗(yàn)，得到的IMU加速度計(jì)與陀螺儀測(cè)量值零偏補(bǔ)償圖如圖5和圖6所示.

分析圖中數(shù)據(jù)，其中加速度值在X軸上的誤差在（－0.012，0）（單位：m）之間，Y軸上的誤差在（－0.12，0）之間；角速度在X軸上，誤差為（0，0.07）之間，Y軸誤差在（－0.01，－0.008）之間.由此可知各軸上測(cè)量值誤差較小，算法有效地減小IMU誤差發(fā)散.

組合定位算法中計(jì)算得到的量測(cè)向量Z∧k，經(jīng)過SR-EKF濾波處理后得到置信度S，該值越小則可信度越高，組合定位結(jié)果準(zhǔn)確性高.量測(cè)信息的置信度曲線如圖7所示.

圖7" 量測(cè)信息的置信度

根據(jù)兩種算法計(jì)算得到的位置坐標(biāo)，分別在X、Y軸上繪制出的誤差值對(duì)比結(jié)果如圖8和圖9所示，從圖中觀測(cè)實(shí)驗(yàn)數(shù)據(jù)，本文定位算法解算出的X軸位置誤差在0.016 m左右，Y軸位置誤差在0.038 m左右，且與EKF算法相比，本文算法定位精確度明顯優(yōu)于該算法.可知本文算法定位結(jié)果更加貼近真實(shí)軌跡，可實(shí)現(xiàn)較精準(zhǔn)的室內(nèi)定位.

5" 結(jié)語

針對(duì)組合定位技術(shù)提出了一種基于平方根擴(kuò)展卡爾曼濾波的UWB與IMU組合定位算法，由實(shí)驗(yàn)結(jié)論可知，本文算法計(jì)算得到的位置坐標(biāo)與物體實(shí)際位置坐標(biāo)之間的誤差值小，驗(yàn)證了平方根濾波的有效性.其中協(xié)方差矩陣描述狀態(tài)量x的精確度，對(duì)協(xié)方差矩陣開平方，從數(shù)值上降低矩陣誤差，進(jìn)而提高其對(duì)狀態(tài)量的表示精度.此外，選用位置、速度、四元數(shù)、加速度值及角速度偏差參與微分方程，較傳統(tǒng)方法增加了信息量，預(yù)測(cè)當(dāng)前時(shí)刻位置的同時(shí)對(duì)IMU測(cè)量值進(jìn)行誤差補(bǔ)償，提高了下一時(shí)刻預(yù)測(cè)狀態(tài)量的準(zhǔn)確度.

參考文獻(xiàn)：

［1］ YIN Z，JIANG X，YANG Z，et al.WUB-IP：a high-precision UWB positioning scheme for indoor multiuser applications［J］.IEEE Systems Journal，2019，13（1）：279-288．

［2］ 嚴(yán)恭敏，秦永元，馬建萍.慣導(dǎo)/里程儀組合導(dǎo)航系統(tǒng)算法研究［J］.計(jì)算機(jī)測(cè)量與控制，2006（8）：1087-1089.

［3］ ELWISCHGER B P B，SAUTER T.Efficient ambiguity resolution in wireless localization systems［J］.IEEE Transactions on Industrial Informatics，2017，13（2）：888-897.

［4］ WU S，ZHANG S，HUANG D.A TOA-based localization algorithm with simultaneous NLOS mitigation and synchronization error elimination［J］.IEEE Sensors Letters，2019，3（4）：1-4．

［5］ 張忠超，高同躍，丁昭，等.基于UWB與IMU的多傳感器融合室內(nèi)定位技術(shù)研究［J］.工業(yè)控制計(jì)算機(jī)，2022，35（4）：45-48.

［6］ 徐慶坤，宋中越.基于UWB的自主跟隨機(jī)器人定位系統(tǒng)的設(shè)計(jì)［J］.計(jì)算機(jī)工程與設(shè)計(jì)，2019，40（11）：3337-3341.

［7］ LUO R C，HSIAO T J.Dynamic wireless indoor localization incorporating with an autonomous mobile robot based on an adaptive signal model fingerprinting approach［J］.IEEE Trans.Industrial Electronics，2019，66（3）：1940-1951.

［8］ 王春琦，孔祥琦，丁曉歡，等.基于無跡卡爾曼濾波的IMU和UWB融合定位算法研究［J］.南昌航空大學(xué)學(xué)報(bào)（自然科學(xué)版），2020，34（3）：8-17.

［9］ 張艷國(guó)，李擎.基于慣性測(cè)量單元的激光雷達(dá)點(diǎn)云融合方法［J］.系統(tǒng)仿真學(xué)報(bào)，2018，30（11）：4334-4339.

［10］ 嚴(yán)恭敏.捷聯(lián)慣導(dǎo)算法及車載組合導(dǎo)航系統(tǒng)研究［D］.西安：西北工業(yè)大學(xué)，2004.

［11］ 鄭偉雄，沈陽，馬浚清，等.基于捷聯(lián)慣導(dǎo)的懸臂式掘進(jìn)機(jī)位姿解算算法研究［J］.煤炭工程，2020，52（9）：170-176.

［12］ 王嫣，常曉鵬，張建平.基于EKF算法的UWB和ZigBee測(cè)量技術(shù)的混合運(yùn)動(dòng)目標(biāo)定位［J］.應(yīng)用科學(xué)學(xué)報(bào)，2019，37（6）：815-824.

［13］ 李春輝，馬健，楊永建，等.基于修正的自適應(yīng)平方根容積卡爾曼濾波算法［J］.系統(tǒng)工程與電子技術(shù)，2021，43（7）：1824-1830.

［14］ 張斌飛，靳伍銀.基于UWB和IMU緊組合的室內(nèi)定位導(dǎo)航算法［J］.電子測(cè)量技術(shù)，2022，45（10）：67-73.

［責(zé)任編輯：李" 嵐］