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

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

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

（湘潭大學自動化與電子信息學院，湖南湘潭 411100）

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

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

0 引言

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

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

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

1 UWB定位方法

室內(nèi)定位中，對位置進行解算，首先要從信號模塊上得到各個基站與標簽之間的距離，再使用距離信息代入定位算法進行計算。測距過程中，可以通過時間、角度和信號強度3種方法來得到距離信息。

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

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

圖1 TOA定位原理

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

4個及以上的基站則可進行三維定位，其方程組為

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

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

2 動量GD-PF算法

2.1 動量梯度下降算法

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

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

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

求出其均方誤差為

2.2 改進動量梯度下降法

2.3 粒子濾波

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

其具體步驟如下：

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

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

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

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

求出時刻的樣本均值，即

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

2.4 改進動量GD-PF算法

改進動量GD-PF的具體步驟：

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

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

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

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

3.1 UWB/MIMU融合定位方程

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

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

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

3.2 擴展卡爾曼濾波

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

擴展Kalman濾波的公式如下：

1）通過上一時刻的定位信息和狀態(tài)方程得到該時刻下的狀態(tài)值，以及通過UWB和MIMU在當前時刻得到的信息來取得觀測值。

2）將狀態(tài)方程和觀測方程進行微分和對協(xié)方差進行預測，即：

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

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

3.3 自適應(yīng)EKF

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

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

同樣，因為在室內(nèi)定位的過程中，目標的速度是有上限的。在連續(xù)2個時刻的UWB位置的距離若過大，UWB的定位就會在當前時刻受到非視距的影響。

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

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

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

5 實驗與結(jié)果分析

5.1 實驗環(huán)境

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

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

圖3 實驗場景和實際路徑

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

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

圖4 路徑1定位軌跡

圖5 路徑2定位軌跡

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

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

圖6 路徑1定位誤差比較

圖7 路徑2定位誤差比較

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

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

表1 GD和改進動量GD算法比較 次

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

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

表2 定位誤差對比表 cm

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

6 結(jié)束語

針對UWB室內(nèi)定位在非視距的環(huán)境下受環(huán)境影響較大的問題，本文提出UWB/MIMU自適應(yīng)動量GD-PF室內(nèi)協(xié)同定位算法。通過實驗和仿真可以看出，動量GD-PF算法與最小二乘法相比，其精度更高，使用改進的動量梯度下降也加快了其位置解算速度。然后EKF結(jié)合MIMU的數(shù)據(jù)后，其定位精度進一步提升，自適應(yīng)EKF也減少了異常觀測值的影響。從而可以得到結(jié)論：自適應(yīng)動量GD-PF算法進行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]. 電子學報, 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] 李天成, 范紅旗, 孫樹棟. 粒子濾波理論、方法及其在多目標跟蹤中的應(yīng)用[J]. 自動化學報, 2015, 41(12): 1981-2002.

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

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

[14] 沈凱, 管雪元, 李文勝. 擴展卡爾曼濾波在組合導航中的應(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)動量GD-PF室內(nèi)協(xié)同定位[J]. 導航定位學報, 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

國家重點研發(fā)計劃項目（2020YFA0713501）；湖南省自然科學基金項目（2021JJ50126）；湖南省教育廳重點項目（21A0083）。

李鵬（1978—），男，山東青島人，博士，教授，研究方向為室內(nèi)外導航定位等。

凌智?。?998—），男，湖南婁底人，碩士研究生，研究方向為室內(nèi)導航定位。