UWB/MIMU自適應(yīng)動(dòng)量GD-PF室內(nèi)協(xié)同定位

李 鵬，凌智琛，榮冬成，向宇翔

UWB/MIMU自適應(yīng)動(dòng)量GD-PF室內(nèi)協(xié)同定位

李 鵬，凌智琛，榮冬成，向宇翔

（湘潭大學(xué)自動(dòng)化與電子信息學(xué)院，湖南湘潭 411100）

針對(duì)超寬帶（UWB）室內(nèi)定位時(shí)容易受到非視距誤差影響，導(dǎo)致定位精度大大下降，甚至無(wú)法準(zhǔn)確定位的問(wèn)題，提出了一種UWB/MIMU自適應(yīng)動(dòng)量梯度下降-粒子濾波（GD-PF）室內(nèi)協(xié)同定位方法。首先在梯度下降算法中引入指數(shù)加權(quán)平均和變步長(zhǎng)策略，加快解算速度；然后采用粒子濾波對(duì)UWB解算數(shù)據(jù)進(jìn)行優(yōu)化，減小粗差對(duì)定位精度的影響；最后引入自適應(yīng)函數(shù)調(diào)整擴(kuò)展卡爾曼濾波（EKF）增益，對(duì)UWB和微型慣性測(cè)量單元（MIMU)的定位數(shù)據(jù)進(jìn)行聯(lián)合濾波。UWB/MIMU協(xié)同定位實(shí)驗(yàn)結(jié)果表明，自適應(yīng)動(dòng)量GD-PF協(xié)同定位算法與傳統(tǒng)定位算法相比，能有效消除非視距誤差的干擾，提高室內(nèi)定位精度和魯棒性。

超寬帶；微型慣性測(cè)量單元；梯度下降算法；粒子濾波；自適應(yīng)擴(kuò)展卡爾曼濾波

0 引言

隨著信息時(shí)代的飛速發(fā)展，對(duì)于位置服務(wù)精度的要求越來(lái)越高。在室外，有全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system, GNSS）能夠帶來(lái)很好的定位精度，但在室內(nèi)因有建筑物等遮擋物的影響，定位精度較差。據(jù)統(tǒng)計(jì)，80%的定位服務(wù)需求發(fā)生在室內(nèi)[1]，因此建立一個(gè)高精度、連續(xù)的室內(nèi)定位系統(tǒng)是非常有必要的。目前，室內(nèi)環(huán)境的無(wú)線定位技術(shù)得到了廣泛的研究，包括偽衛(wèi)星技術(shù)、藍(lán)牙、紫蜂（zigbee, ZB）、超寬帶（ultra wide band, UWB）等[2-3]。超寬帶技術(shù)作為近年來(lái)室內(nèi)定位中非常熱門的一種定位方式，由于具有時(shí)間分析率高、穿透性強(qiáng)、覆蓋范圍廣、系統(tǒng)復(fù)雜度低、抗多徑效應(yīng)良好等優(yōu)勢(shì)，越來(lái)越受到廣大公司和研究者的重視[4-5]。

傳統(tǒng)的定位算法如最小二乘法、文獻(xiàn)[6]提出的算法等，只有當(dāng)系統(tǒng)的測(cè)量誤差處于理想高斯分布時(shí)，才能達(dá)到較高的精度。然而室內(nèi)環(huán)境復(fù)雜多變，常常會(huì)處于非視距環(huán)境[7]（non line of sight, NLOS）。為了進(jìn)一步提高室內(nèi)定位的可靠性和準(zhǔn)確性，多傳感器融合定位技術(shù)引起了國(guó)內(nèi)外學(xué)者的關(guān)注，比如把射頻識(shí)別[8]或者微型慣性測(cè)量單元（miniature inertial measurement unit, MIMU）與UWB融合。擴(kuò)展卡爾曼濾波（extended Kalman filter, EKF）在多傳感器融合定位系統(tǒng)中得到廣泛應(yīng)用。

為了減少非視距環(huán)境的影響，提高室內(nèi)定位的精度，本文提出了一種UWB/MIMU自適應(yīng)動(dòng)量梯度下降-粒子濾波（gradient descent-particle filter, GD-PF）室內(nèi)協(xié)同定位方法。UWB的定位信息，首先通過(guò)改進(jìn)的動(dòng)量梯度下降算法解算，然后采用粒子濾波對(duì)估計(jì)位置進(jìn)行再次優(yōu)化，最后使用自適應(yīng)EKF將其與MIMU的數(shù)據(jù)融合，得到準(zhǔn)確的定位結(jié)果。

1 UWB定位方法

室內(nèi)定位中，對(duì)位置進(jìn)行解算，首先要從信號(hào)模塊上得到各個(gè)基站與標(biāo)簽之間的距離，再使用距離信息代入定位算法進(jìn)行計(jì)算。測(cè)距過(guò)程中，可以通過(guò)時(shí)間、角度和信號(hào)強(qiáng)度3種方法來(lái)得到距離信息。

本文中UWB的測(cè)距方法是信號(hào)到達(dá)時(shí)間[9]（time of arrival, TOA），測(cè)量定位過(guò)程中標(biāo)簽和基站之間信號(hào)傳播的時(shí)間，根據(jù)UWB的傳播速度，將時(shí)間轉(zhuǎn)換成距離信息。TOA進(jìn)行定位至少需要3個(gè)基站。3個(gè)基站可進(jìn)行二維定位，理想狀態(tài)下的TOA定位原理如圖1所示。

以基站位置為圓心，基站與標(biāo)簽的距離為半徑畫圓，其交點(diǎn)為標(biāo)簽的位置。圖中基站的數(shù)量和位置都是已知，令標(biāo)簽坐標(biāo)位置為（，），通過(guò)TOA算法得到標(biāo)簽到基站的時(shí)間分別為1、2、3，根據(jù)幾何原理和已知的數(shù)據(jù)，可得到的方程組為

圖1 TOA定位原理

式中為UWB信號(hào)傳播速度。按照方程組即可求出定位標(biāo)簽的二維位置。

4個(gè)及以上的基站則可進(jìn)行三維定位，其方程組為

式中為基站數(shù)量。該方程組可求出標(biāo)簽的三維位置（，，）。

在理想狀態(tài)時(shí)，可通過(guò)對(duì)式（1）、式（2）得到唯一的結(jié)果，但因?yàn)閷?shí)測(cè)距離存在誤差，可以通過(guò)方程組求出最優(yōu)解。

2 動(dòng)量GD-PF算法

2.1 動(dòng)量梯度下降算法

獲取UWB模塊得到的距離信息，解算結(jié)果的過(guò)程，用得較多的是最小二乘算法。最小二乘算法在視距環(huán)境下該方法可以得到不錯(cuò)的定位結(jié)果，但在NLOS環(huán)境下精度大大降低。本文采用動(dòng)量梯度下降算法對(duì)UWB得到的距離信息進(jìn)行優(yōu)化解算。

按照梯度下降法[10]，對(duì)于一個(gè)函數(shù)而言，從某個(gè)初始值開始，不斷往梯度方向進(jìn)行迭代，通過(guò)迭代，最后穩(wěn)定在最優(yōu)值左右。動(dòng)量梯度下降法在梯度下降算法基礎(chǔ)上對(duì)梯度進(jìn)行改進(jìn)，加快了位置解算速度，收斂也更加迅速。

從UWB模塊中獲得的距離信息，按照標(biāo)簽和基站之間距離，建立多元非線性模型為

求出其均方誤差為

2.2 改進(jìn)動(dòng)量梯度下降法

2.3 粒子濾波

粒子濾波[11]與傳統(tǒng)的濾波算法比較，在非線性上適用，并且也適用于非線性及非高斯噪聲環(huán)境的優(yōu)點(diǎn)。所以使用其對(duì)UWB解算數(shù)據(jù)進(jìn)行優(yōu)化。其數(shù)學(xué)模型為：

其具體步驟如下：

2）預(yù)測(cè)。根據(jù)狀態(tài)方程，得到預(yù)測(cè)粒子為

3）校正。進(jìn)行權(quán)值計(jì)算，進(jìn)行權(quán)值計(jì)算，越接近于觀測(cè)狀態(tài)的粒子，其權(quán)重越大，粒子的權(quán)值為

4）重采樣。在重采樣階段，選擇隨機(jī)重采樣，將[0,1]均勻分成相互獨(dú)立的層，令為[0,1]均勻分布的隨機(jī)變量，產(chǎn)生一個(gè)隨機(jī)數(shù)，根據(jù)其落在的區(qū)間，所對(duì)應(yīng)的隨機(jī)變量就是所需要的輸出量。本質(zhì)是根據(jù)權(quán)重的大小進(jìn)行篩選。篩選過(guò)程中，既要大量保留權(quán)值大的粒子，又要有一小部分權(quán)值小的粒子，得到新的粒子群，繼續(xù)進(jìn)行濾波。

進(jìn)行權(quán)值歸一化，為

求出時(shí)刻的樣本均值，即

將重采樣后的粒子結(jié)合速度信息通過(guò)狀態(tài)方程得到新的預(yù)測(cè)粒子，即回到第2）步。

2.4 改進(jìn)動(dòng)量GD-PF算法

改進(jìn)動(dòng)量GD-PF的具體步驟：

4）依照公式得到梯度和步長(zhǎng)，更新下一步的位置，轉(zhuǎn)步驟2）。

5）使用改進(jìn)動(dòng)量梯度下降算法解算出該時(shí)刻的位置，再進(jìn)行粒子濾波，得到該時(shí)刻使用改進(jìn)動(dòng)量GD-PF算法得到結(jié)果。

6）返回步驟1），繼續(xù)對(duì)后續(xù)時(shí)刻的位置進(jìn)行解算。

3 自適應(yīng)EKF算法

3.1 UWB/MIMU融合定位方程

本文將UWB的數(shù)據(jù)和MIMU的數(shù)據(jù)結(jié)合，進(jìn)行融合定位，可以補(bǔ)償各自的缺點(diǎn)，能夠更好地改善受到非視距影響的定位精度。在組合導(dǎo)航系統(tǒng)中，濾波算法對(duì)導(dǎo)航定位的解算精度有重要的影響[12]。

使用自適應(yīng)EKF將UWB的定位數(shù)據(jù)和MIMU取得的信息進(jìn)行融合，能夠結(jié)合二者的優(yōu)點(diǎn)，使定位更加準(zhǔn)確。

UWB的定位數(shù)據(jù)是通過(guò)改進(jìn)動(dòng)量GD-PF算法解算的結(jié)果，根據(jù)MIMU的陀螺儀、加速度計(jì)得到的數(shù)據(jù)，從陀螺儀得到的數(shù)據(jù)是載體坐標(biāo)系下、、軸方向的加速度大小情況以及俯仰角、滾轉(zhuǎn)角、航偏角。首先需要將MIMU的信息從載體坐標(biāo)系轉(zhuǎn)換成導(dǎo)航坐標(biāo)系下的加速度矢量。將得到的加速度矢量和UWB得到的定位坐標(biāo)為該系統(tǒng)的觀測(cè)值，該系統(tǒng)的方程組為

3.2 擴(kuò)展卡爾曼濾波

擴(kuò)展卡爾曼濾波建立在卡爾曼濾波的基礎(chǔ)上，其核心思想是，對(duì)非線性系統(tǒng)，通過(guò)對(duì)狀態(tài)方程和觀測(cè)方程展開Taylor級(jí)數(shù)并忽略二階及以上項(xiàng)，得到近似的線性化模型，最后在應(yīng)用Kalman濾波完成對(duì)目標(biāo)估計(jì)等處理[13-14]。

擴(kuò)展Kalman濾波的公式如下：

1）通過(guò)上一時(shí)刻的定位信息和狀態(tài)方程得到該時(shí)刻下的狀態(tài)值，以及通過(guò)UWB和MIMU在當(dāng)前時(shí)刻得到的信息來(lái)取得觀測(cè)值。

2）將狀態(tài)方程和觀測(cè)方程進(jìn)行微分和對(duì)協(xié)方差進(jìn)行預(yù)測(cè)，即：

3）更新估計(jì)值和協(xié)方差為：

以上為一個(gè)計(jì)算周期，各個(gè)時(shí)刻對(duì)其進(jìn)行EKF處理，就是在這個(gè)計(jì)算周期內(nèi)進(jìn)行循環(huán)。

3.3 自適應(yīng)EKF

本文中自適應(yīng)的作用是修正當(dāng)目標(biāo)狀態(tài)受到環(huán)境影響，有可能產(chǎn)生異常觀測(cè)值情況下的定位結(jié)果，從而減少異常值的影響，提高定位精度。

狀態(tài)誤差和觀測(cè)誤差是有限的，同一時(shí)刻UWB/MIMU得到的狀態(tài)值和觀測(cè)值之間的距離若過(guò)大，說(shuō)明該時(shí)刻受到了異常值的影響，從而使得誤差較大。

同樣，因?yàn)樵谑覂?nèi)定位的過(guò)程中，目標(biāo)的速度是有上限的。在連續(xù)2個(gè)時(shí)刻的UWB位置的距離若過(guò)大，UWB的定位就會(huì)在當(dāng)前時(shí)刻受到非視距的影響。

4 自適應(yīng)動(dòng)量GD-PF協(xié)同定位算法

本文使用了自適應(yīng)動(dòng)量GD-PF協(xié)同定位算法。首先通過(guò)TOA方法從UWB模塊上得到距離信息，使用動(dòng)量GD-PF算法處理其距離信息，得到UWB的定位結(jié)果；然后通過(guò)自適應(yīng)EKF算法來(lái)融合MIMU模塊中加速度計(jì)和陀螺儀的數(shù)據(jù)；最終得到融合定位結(jié)果。其算法結(jié)構(gòu)如圖2所示。

圖2 自適應(yīng)動(dòng)量GD-PF算法

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

5.1 實(shí)驗(yàn)環(huán)境

為了對(duì)自適應(yīng)動(dòng)量GD-PF算法進(jìn)行性能評(píng)估，通過(guò)UWB/MIMU室內(nèi)融合定位系統(tǒng)來(lái)驗(yàn)證。該系統(tǒng)中有4個(gè)基站1、2、3、4，通過(guò)障礙物來(lái)制造NLOS環(huán)境。

在實(shí)驗(yàn)環(huán)境中，進(jìn)行了多種路徑的實(shí)驗(yàn)，從中選擇2種路徑來(lái)進(jìn)行數(shù)據(jù)分析，其實(shí)際路徑和基站分布如圖3所示。

圖3 實(shí)驗(yàn)場(chǎng)景和實(shí)際路徑

5.2 數(shù)據(jù)分析

在上述實(shí)驗(yàn)環(huán)境中，通過(guò)定位系統(tǒng)得到信息，使用最小二乘算法和自適應(yīng)動(dòng)量GD-PF算法得到定位軌跡。

圖4 路徑1定位軌跡

圖5 路徑2定位軌跡

由圖4及圖5可知，當(dāng)前環(huán)境下自適應(yīng)動(dòng)量GD-PF算法與最小二乘法的定位結(jié)果比較，該算法的定位更加接近實(shí)際軌跡，也更加平滑，在非視距的環(huán)境下的定位更加準(zhǔn)確、穩(wěn)定。

室內(nèi)定位系統(tǒng)使用不同的算法得到結(jié)果的誤差。圖6和圖7為2種路徑在最小二乘算法、改進(jìn)動(dòng)量GD-PF算法和自適應(yīng)動(dòng)量GD-PF算法3種算法下的定位誤差比較。

圖6 路徑1定位誤差比較

圖7 路徑2定位誤差比較

從圖6、圖7可以看出，使用改進(jìn)動(dòng)量GD-PF算法與最小二乘算法相比，精度都有較大的提升。自適應(yīng)動(dòng)量GD-PF算法使用自適應(yīng)EKF結(jié)合MIMU的數(shù)據(jù)，實(shí)現(xiàn)協(xié)同定位，該算法得到的定位誤差比只使用UWB定位的誤差更小，精度更高，異常定位結(jié)果明顯減少。

表1記錄的是采用改進(jìn)動(dòng)量梯度下降算法與梯度下降算法的迭代次數(shù)的比較。

表1 GD和改進(jìn)動(dòng)量GD算法比較 次

路徑1需要解算343個(gè)位置，路徑2需要解算283個(gè)位置，由表1可以看出，使用改進(jìn)動(dòng)量梯度下降算法的迭代次數(shù)比原梯度的下降算法更少。

表2記錄的是UWB/MIMU定位在使用不同定位算法的情況下定位結(jié)果的誤差信息。

表2 定位誤差對(duì)比表 cm

從表2中可以發(fā)現(xiàn)，梯度下降算法得到定位結(jié)果比最小二乘法的更好，使用改進(jìn)動(dòng)量GD-PF的誤差相較于梯度下降法進(jìn)一步減小。使用EKF結(jié)合MIMU的數(shù)據(jù)，定位誤差又有相應(yīng)的下降，將EKF加上自適應(yīng)后，從最大誤差值可以發(fā)現(xiàn)，相較于EKF，自適應(yīng)動(dòng)量GD-PF能夠更好地減少異常值的影響，使其定位精度更準(zhǔn)確。

6 結(jié)束語(yǔ)

針對(duì)UWB室內(nèi)定位在非視距的環(huán)境下受環(huán)境影響較大的問(wèn)題，本文提出UWB/MIMU自適應(yīng)動(dòng)量GD-PF室內(nèi)協(xié)同定位算法。通過(guò)實(shí)驗(yàn)和仿真可以看出，動(dòng)量GD-PF算法與最小二乘法相比，其精度更高，使用改進(jìn)的動(dòng)量梯度下降也加快了其位置解算速度。然后EKF結(jié)合MIMU的數(shù)據(jù)后，其定位精度進(jìn)一步提升，自適應(yīng)EKF也減少了異常觀測(cè)值的影響。從而可以得到結(jié)論：自適應(yīng)動(dòng)量GD-PF算法進(jìn)行UWB和MIMU協(xié)同定位，可以改善非視距情況下的定位效果。

[1] LI Z K, LIU C Y, GAO J X, et al. An improved WiFi/PDR integrated system using an adaptive and robust filter for indoor localization[J]. Isprs International Journal of Geo-Information, 2016, 5(12): 224.

[2] BIANCHI V, CIAMPOLINI P, MUNARI I D. RSSI-based indoor localization and identification for zigbee wireless sensor networks in smart homes[J]. IEEE Trans Instrumentation and Measurement, 2019, 68(2): 566-575.

[3] LI X, WANG J, LIU C. A bluetooth/PDR integration algorithm for an indoor positioning system[J]. Sensors, 2015, 15(10): 24862-24885.

[4] RAMON J R A, FERNANDO S G. Comparing Ubisense, BeSpoon, and DecaWave UWB location systems: indoor performance analysis[J]. IEEE Transactions on Instrumentation and Measurement, 2017, 66(8): 2106-2117.

[5] HU D, HUANG Z, ZHANG S, et al. Joint TDOA, FDOA and differential Doppler rate estimation: method and its performance analysis[J]. Chinese Journal of Aeronautics, 2018, 31(1): 137-147.

[6] 楊天池, 金梁, 程娟. 一種基于TOA定位的CHAN改進(jìn)算法[J]. 電子學(xué)報(bào), 2009, 37(4): 819-822.

[7] BARRAL V, ESCUDERO C J, GARCíA -NAYA J A, et al. Environmental cross-validation of NLOS machine learning classification/mitigation with low-cost UWB positioning systems[J]. Sensors (Basel, Switzerland), 2019, 19(24): 5438.

[8] ZHAIC, ZOU Z, ZHOU Q, et al. A 2. 4-GHz ISM RF and UWB hybrid RFID real-time locating system for industrial enterprise internet of things[J]. Enterprise Information Systems, 2017, 11(6): 909-926.

[9] KAUNE R. Accuracy studies for TDOA and TOA localization [C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of International Conference on Information Fusion. Singapore: IEEE, 2012: 408-415[2021-05-01].

[10] RUDER S. An overview of gradient descent optimization algorithms[EB/OL]. (2017-6-15)[2021-05-01]. https: //arxiv. org/pdf/1609. 04747. pdf.

[11] 李天成, 范紅旗, 孫樹棟. 粒子濾波理論、方法及其在多目標(biāo)跟蹤中的應(yīng)用[J]. 自動(dòng)化學(xué)報(bào), 2015, 41(12): 1981-2002.

[12] 曾慶化, 王敬賢, 孟騫, 等. 基于UWB優(yōu)化配置的室內(nèi)行人導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2017, 25(2): 186-191.

[13] 王學(xué)斌, 徐建宏, 張章. 卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究[J]. 計(jì)算機(jī)應(yīng)用與軟件, 2012, 29(6): 212-215.

[14] 沈凱, 管雪元, 李文勝. 擴(kuò)展卡爾曼濾波在組合導(dǎo)航中的應(yīng)用[J]. 傳感器與微系統(tǒng), 2017, 36(8): 158-160.

[15] TONG X, LI Z, HAN G, et al. Adaptive EKF based on HMM recognizer for attitude estimation using MEMS MARG sensors[J]. IEEE Sensors Journal, 2017, 18(8): 3299-3310.

UWB/MIMU adaptive momentum GD-PF indoor cooperative positioning

LI Peng, LING Zhichen, RONG Dongcheng, XIANG Yuxiang

（School of automation and electronic information,Xiangtan University, Xiangtan, Hunan 411100, China）

To address the problem that ultra wide band (UWB) indoor positioning is easily affected by non line of sight (NLOS) errors, which leads to a significant decrease in positioning accuracy or even inability to locate accurately. In this paper, an UWB/MIMU adaptive momentum gradient descent-particle filter (GD-PF) indoor co-localization method is proposed. Firstly, exponential weighted average and variable step size strategies are introduced in the gradient descent (GD) algorithm to speed up the gradient solving speed. Then the particle filtering is used to optimize the UWB solved data to reduce the gross errors on the localization accuracy. Finally, an adaptive function is introduced to adjust the extended Kalman filter (EKF) gain to jointly filter the localization data of UWB and miniature inertial measurement unit (MIMU). The experimental results of UWB/MIMU co-localization show that the adaptive momentum GD-PF co-localization algorithm can effectively eliminate the interference of NLOS errors and improve the indoor positioning accuracy and robustness compared with the traditional positioning algorithm.

ultra wide band; miniature inertial measurement unit; gradient descent; particle filter; adaptive extended Kalman filter

TP212；TN925

A

2095-4999(2022)06-0122-07

李鵬，凌智琛，榮冬成，等. UWB/MIMU自適應(yīng)動(dòng)量GD-PF室內(nèi)協(xié)同定位[J]. 導(dǎo)航定位學(xué)報(bào), 2022, 10(6): 122-128.（LI Peng, LING Zhichen, RONG Dongcheng, et al. UWB/MIMU adaptive momentum GD-PF indoor cooperative positioning[J]. Journal of Navigation and Positioning, 2022, 10(6): 122-128.）

10.16547/j.cnki.10-1096.20220616.

2022-04-11

國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2020YFA0713501）；湖南省自然科學(xué)基金項(xiàng)目（2021JJ50126）；湖南省教育廳重點(diǎn)項(xiàng)目（21A0083）。

李鵬（1978—），男，山東青島人，博士，教授，研究方向?yàn)槭覂?nèi)外導(dǎo)航定位等。

凌智?。?998—），男，湖南婁底人，碩士研究生，研究方向?yàn)槭覂?nèi)導(dǎo)航定位。