孫 偉，安晟均

基于Sage-Husa自適應(yīng)UKF的UWB/INS融合定位方法

孫 偉，安晟均

（遼寧工程技術(shù)大學(xué)測繪與地理科學(xué)學(xué)院，遼寧阜新 123000）

針對室內(nèi)環(huán)境中的非視距（NLOS）導(dǎo)致單一超寬帶（UWB）定位出現(xiàn)較大誤差的問題，提出基于無跡卡爾曼濾波（UKF）的超寬帶/慣性導(dǎo)航系統(tǒng)（INS）緊組合定位方法。優(yōu)化UWB量測方程，利用導(dǎo)航方程誤差狀態(tài)量簡化卡爾曼濾波的預(yù)測和校正環(huán)節(jié)，通過薩格-胡薩（Sage-Husa）自適應(yīng)估計并利用濾波器新息值對過程和測量噪聲進(jìn)行在線辨識，使得模型白噪聲更接近真實(shí)情況，利用野值處理方法降低非視距對定位精度的影響。仿真實(shí)驗(yàn)結(jié)果表明，與傳統(tǒng)擴(kuò)展卡爾曼濾波（EKF）和UKF濾波方案相比較，本文提出基于UKF的組合定位模型能有效抑制非視距誤差的影響并提高定位精度。

超寬帶；無跡卡爾曼濾波；自適應(yīng)濾波；室內(nèi)定位；非視距

0 引言

超寬帶（ultra-wide band，UWB）定位技術(shù)[1]中的無線信號由于多徑效應(yīng)和非視距（non-line-of-sight，NLOS）[2]干擾，在用于跟蹤定位移動的人或物時，其運(yùn)動軌跡會產(chǎn)生偏差。慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）在不需要任何額外附加條件下使用慣性測量單元（inertial measurement unit，IMU），以高頻更新率估計目標(biāo)位置并在短時間內(nèi)可保持較高定位精度，但隨時間推移系統(tǒng)定位誤差會累積[3]。將二者緊耦合，可促使定位信息融合更加徹底，定位精度更高，且能為UWB定位數(shù)據(jù)處理進(jìn)行假設(shè)性測驗(yàn)，有效處理NLOS和多徑效應(yīng)。

文獻(xiàn)[4]提出基于魯棒擴(kuò)展卡爾曼濾波的UWB融合步行者航跡推算定位方法，通過處理系統(tǒng)狀態(tài)和觀測值間的馬氏距離來抑制非視距對定位結(jié)果的影響，從而更新觀測值協(xié)方差。但UWB測距噪聲較強(qiáng)、信號中斷較多的情況下，如何提高算法性能還有待進(jìn)一步測試。文獻(xiàn)[5]提出一種基于UWB的室內(nèi)定位系統(tǒng)NLOS誤差補(bǔ)償方法。通過IMU固定標(biāo)簽的運(yùn)動趨勢得到虛擬慣性點(diǎn)坐標(biāo)，采用環(huán)境因子作為加權(quán)系數(shù)將虛擬慣性點(diǎn)坐標(biāo)與測量坐標(biāo)融合，得到NLOS環(huán)境下的補(bǔ)償坐標(biāo)，以減少NLOS的影響。但沒有考慮測量噪聲誤差和該方法在動態(tài)環(huán)境中下的表現(xiàn)。文獻(xiàn)[6]提出了一種新的NLOS識別方法，該方法根據(jù)UWB信號在信道中的傳播特征，結(jié)合支持向量機(jī)算法進(jìn)行NLOS識別。文獻(xiàn)[7]針對UWB定位樣本具有類不平衡的特征，使用基于矩的不平衡二元分類方法來識別NLOS信號。該方法利用視距信號樣本的2個第一矩的均值和協(xié)方差來表示其概率分布，然后利用概率分布和少量NLOS信號樣本的總和建立分類模型。該方法不依賴于視距信號個數(shù)，適用于處理視距和非視距信號個數(shù)之間的不平衡分類。文獻(xiàn)[8]提出使用距離測量的魯棒定位算法。針對混合環(huán)境，把預(yù)測為異常值的距離觀測值替換為使用最小中值算法獲得的估計距離。隨后，使用這些新的距離測量執(zhí)行二步加權(quán)最小二乘方法。通過仿真證明了所提出方法的定位精度高于傳統(tǒng)方法。文獻(xiàn)[9]研究基于擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）和無跡卡爾曼濾波（unscented Kalman filter，UKF）[10]的超寬帶結(jié)合IMU的算法。采用3次多項(xiàng)式擬合法來補(bǔ)償測距誤差，提出適用于非視距環(huán)境下室內(nèi)導(dǎo)航的誤差互補(bǔ)擴(kuò)展卡爾曼濾波算法。

為提高定位系統(tǒng)穩(wěn)固性及位置輸出平順性，本文提出基于UKF的UWB/INS緊組合濾波模型后，采用薩格-胡薩（Sage-Husa）自適應(yīng)濾波對其進(jìn)行優(yōu)化和野值剔除，完成在線對非視距干擾的有效辨別，提升緊組合定位方法在室內(nèi)環(huán)境下的精度及魯棒性。

1 組合濾波建模

1.1 狀態(tài)方程

常規(guī)的INS一般都是以十五階的狀態(tài)量來建立系統(tǒng)的狀態(tài)方程[11]。在室內(nèi)環(huán)境中，對象節(jié)點(diǎn)在相對較小的定位范圍內(nèi)低速運(yùn)動，可以不考慮地球曲率及自轉(zhuǎn)，從而得出慣導(dǎo)狀態(tài)方程為

1.2 量測方程

1.3 融合濾波

卡爾曼濾波通常對狀態(tài)誤差量來建模，一般不直接利用十五階狀態(tài)量。該方法可以分別進(jìn)行航跡推算及濾波運(yùn)算，即導(dǎo)航狀態(tài)由慣導(dǎo)狀態(tài)方程遞歸輸出。而濾波器僅在量測發(fā)生時估計狀態(tài)誤差，并及時將其反饋給系統(tǒng)狀態(tài)量。因此，濾波狀態(tài)變量可表示為

繼而得到先驗(yàn)誤差協(xié)方差矩陣為

對于量測更新階段，先對濾波器的增益進(jìn)行估計，進(jìn)而得到后驗(yàn)誤差協(xié)方差矩陣為

2 基于Sage-Husa的自適應(yīng)估計

在復(fù)雜的室內(nèi)環(huán)境中，噪聲的概率統(tǒng)計特性是有變化趨勢的。比如：超寬帶測距存在多路徑干擾的情況；IMU容易受到溫度變化的干擾，測量值會發(fā)生偏移。雖然不能準(zhǔn)確地給出這樣的隨機(jī)噪聲模型，但是可以通過對噪聲的方差和平均進(jìn)行適當(dāng)?shù)恼{(diào)節(jié)從而使模型近似的白噪聲更加逼近實(shí)際的情況?？柭鼮V波的自適應(yīng)估計具有較好的在線辨識能力，能較好地區(qū)分出噪聲的均值及方差，從而更好地適應(yīng)各種噪聲。由于IMU數(shù)據(jù)的噪聲幅值變化較慢，所以只考慮了在線辨識噪聲的均值及方差，從而使計算過程更簡單[17]。

3 非視距處理

在超寬帶定位系統(tǒng)中，NLOS誤差影響非常明顯。非視距誤差與實(shí)際環(huán)境有密切關(guān)系，在電磁信號傳播過程中若有障礙或多徑效應(yīng)影響，會導(dǎo)致測距不準(zhǔn)確。上述情況是由于超寬帶硬件接收到多徑電磁信號，且把第一個到達(dá)的有時延信號判定為直路信號，這種情況在視距條件下基本無影響[19]。

在實(shí)際情況下判定超寬帶硬件接收電磁信號是否為NLOS信號，可以依據(jù)接收非視距信道的信號功率會有很大的衰減這個特征，具體方法為

此外，緊耦合定位模型通過概率一致性檢驗(yàn)證明數(shù)據(jù)合理性，卡爾曼濾波中的新息為白噪聲序列，并且正交、時間不相關(guān)，則歸一化新息方差為

以上2種方式配合，從物理特性和概率模型出發(fā)，提出了NLOS檢測的基本原理，可有效地減少NLOS對定位系統(tǒng)的影響。在遇到非視距情況時，該系統(tǒng)會自動地選取慣性導(dǎo)航進(jìn)行位置信息的解算，使其具有較好的穩(wěn)定性。

4 仿真及結(jié)果分析

4.1 測試環(huán)境建立

將馬爾可夫過程一階模型對慣性傳感器誤差進(jìn)行建模，而慣性部件的噪聲包括高斯白噪聲及高斯馬爾可夫噪聲。公式為：

表1 IMU參數(shù)配置表

視距情況下，用白噪聲模型對超寬帶測距偽距值噪聲進(jìn)行建模為

對于有NLOS干擾的場景，本文提出了以下的假定：

1）設(shè)時刻UWB測距存在NLOS誤差的概率均勻分布，則表明在仿真環(huán)境下，各時刻有非視距干擾的可能性是相同的。

圖1為模擬的室內(nèi)定位場景示意圖。仿真環(huán)境選擇在矩陣實(shí)驗(yàn)室（matrix laboratory, MATLAB）R2020b下進(jìn)行，實(shí)驗(yàn)場景設(shè)置在三軸為12 m×8 m×3 m的室內(nèi)環(huán)境中，設(shè)定星號點(diǎn)表示基站（坐標(biāo)如表2所示），圓圈點(diǎn)表示原點(diǎn)。設(shè)定對象歷經(jīng)勻速運(yùn)動、加速運(yùn)動、減速運(yùn)動、直線運(yùn)動、曲線運(yùn)動等多種狀態(tài)。并將目標(biāo)節(jié)點(diǎn)的速度控制在1 m/s以內(nèi)，采樣的總時間為300 s。

圖1 仿真實(shí)驗(yàn)場景

表2 UWB基站點(diǎn)號及坐標(biāo) m

4.2 結(jié)果及分析

為驗(yàn)證論文所提方案可行性，對比分析開展單一UWB定位、UWB/IMU組合定位測試；基于EKF和UKF濾波的組合定位對比測試。

4.2.1 單一UWB定位及其與IMU組合定位對比

如圖2所示，待測目標(biāo)點(diǎn)分別在單一UWB測距定位和組合定位EKF濾波方案下進(jìn)行仿真實(shí)驗(yàn)。通過對比軌跡可以看出，單一UWB系統(tǒng)在運(yùn)動中的目標(biāo)節(jié)點(diǎn)跟隨上不穩(wěn)定，空間分辨率較低導(dǎo)致軸波動大。而組合定位EKF算法下的估計運(yùn)動軌跡更接近于實(shí)際軌跡。

圖2 UWB測距與EKF組合定位軌跡對比

如圖3所示，單一UWB定位系統(tǒng)只使用測距值來解算位置。在非視距測距誤差產(chǎn)生后，僅測距UWB系統(tǒng)輸出不穩(wěn)定，與實(shí)際的位置偏差較大。然而運(yùn)用EKF算法的組合定位模型精度維持在0.3 m左右，表現(xiàn)出較好的效果。結(jié)果表明，利用EKF算法建立的模型可以很好地追蹤到目標(biāo)的位置，從而提高了單一UWB系統(tǒng)在NLOS干擾下的性能。

此外組合模型也能為物體的姿態(tài)、速度等精確地給出定位信息。如圖4及圖5所示，基于EKF算法實(shí)現(xiàn)組合定位速度與姿態(tài)跟蹤曲線。從圖4、圖5中可明顯看出，基于EKF算法的組合定位速度曲線跟隨較好，姿態(tài)曲線略有起伏，但誤差不大。

圖3 UWB測距與EKF組合定位誤差對比

圖4 基于EKF的組合定位速度曲線

圖5 基于EKF的組合定位姿態(tài)曲線

4.2.2 組合定位中的EKF和UKF濾波性能對比結(jié)果

由圖6濾波器位置誤差曲線可知，相比較之下，UKF算法的定位誤差浮動范圍維持在0.1 m之內(nèi)，可以快速收斂到真實(shí)值，非線性近似精度更高。

圖6 組合定位EKF與UKF誤差對比圖

EKF算法定位誤差偏大的原因是：當(dāng)UWB測距存在非視距誤差時，濾波器不能識別這種情況，并對位置量進(jìn)行了錯誤修正，從而導(dǎo)致其與實(shí)際值偏差；在新的視距偽距值產(chǎn)生后，由于測量更新過程中的非線性處理步長過大，使濾波產(chǎn)生了暫時性的波動。

5 結(jié)束語

本文提出基于UKF濾波的UWB/INS緊組合定位模型，可有效解決室內(nèi)定位中由于NLOS干擾而影響定位精度的問題。利用Sage-Husa自適應(yīng)估計分辨過程及測量噪聲，并利用野值處理技術(shù)對NLOS干擾完成在線識別，使優(yōu)化后模型的動態(tài)性能得以提升。測試結(jié)果表明，優(yōu)于僅測距的UWB系統(tǒng)以及運(yùn)用EKF算法的組合導(dǎo)航系統(tǒng)，誤差可控制在10 cm以內(nèi)，能有效提高非視距條件下的定位精度及穩(wěn)定性。

[1] 李景文, 韋晶閃, 周俊芬, 等. 融合UWB+PDR的室內(nèi)定位方法改進(jìn)[J]. 測繪通報, 2022(3): 36-40.

[2] 高端陽, 李安, 傅軍. 復(fù)雜環(huán)境下抗NLOS干擾的定位算法[J]. 導(dǎo)航定位學(xué)報, 2018, 6(1): 40-44,119.

[3] 姚子揚(yáng), 尚俊娜, 孫建強(qiáng), 等. 一種多源融合室內(nèi)外無縫定位方法[J]. 傳感技術(shù)學(xué)報, 2022, 35(1): 72-83.

[4] LI X, WANG Y, KHOSHELHAM K. UWB/PDR tightly coupled navigation with robust extended kalman filter for NLOS environments[J]. Mobile Information Systems, 2018(3): 1-14.

[5] YANG X, WANG J, SONG D, et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system[J]. IEEE Sensors Journal, 2021, 21(9): 11203-11212.

[6] KOLAKOWSKI M, MODELSKI J. Detection of direct path component absence in NLOS UWB channel[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 201822nd International Microwave and Radar Conference (MIKION). Poznan Poland: IEEE, 2018: 247-250[2022-05-26].

[7] 宋波, 李生林, 劉恩啟. 基于類別不平衡分類的超寬帶非視距信號識別方法研究[J]. 信息工程大學(xué)學(xué)報, 2019, 20(2): 147-153.

[8] PARK C H, CHANG J H. Robust LMedS-based WLS and Tukey-based EKF algorithms under LOS/NLOS mixture conditions[J]. IEEE Access, 2019, 7: 148198-148207.

[9] WANG G, QIN S, QING L, et al. UWB and IMU system fusion for indoor navigation[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 201837nd Chinese Control Conference (CCC). Piscataway: IEEE, 2018: 4946-4950[2022-05-26].

[10] 余懷瑾, 依那, 蔣偉, 等. 基于UKF算法的高機(jī)動機(jī)載平臺多普勒頻移估計[J]. 北京大學(xué)學(xué)報(自然科學(xué)版), 2017, 53(6): 1003-1010.

[11] 徐愛功, 劉韜, 隋心, 等. UWB/INS緊組合的室內(nèi)定位定姿方法[J]. 導(dǎo)航定位學(xué)報, 2017, 5(2): 14-19.

[12] 李月. UWB/INS聯(lián)合室內(nèi)定位方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2019.

[13] 孫偉, 李亞丹, 黃恒, 等. 基于級聯(lián)濾波的建筑結(jié)構(gòu)信息/慣導(dǎo)室內(nèi)定位方法[J]. 儀器儀表學(xué)報, 2021, 42(3): 10-16.

[14] 黃劍雄, 劉小雄, 章衛(wèi)國, 等. 基于視覺/慣導(dǎo)的無人機(jī)組合導(dǎo)航算法研究[J]. 計算機(jī)測量與控制, 2021, 29(2): 137-143, 149.

[15] MOGHTADAIEE V, ALIKHANI N, GHORASHI S A. Low-cost localisation considering LOS/NLOS impacts in challenging indoor environments[J]. International Journal of Sensor Networks, 2020, 32(1): 15-24.

[16] SUN W, LIU J Z. Design of robust cubature fission particle filter algorithm in multi-source cooperative navigation[J]. Scientific Reports, 2022, 12(1): 1-13.

[17] SUN W, WU J J, DING W, et al. A robust indirect Kalman filter based on the gradient descent algorithm for attitude estimation during dynamic conditions[EB/OL]. [2022-05-26]. https: //ieeexplore. ieee. org/stamp/stamp. jsp?tp=&arnumber=9099275. DOI: 10. 1109/ACCESS. 2020. 2997250.

[18] 盛坤鵬, 王堅(jiān), 李晨輝, 等. 非視距誤差改正的超寬帶定位模型研究[J]. 測繪科學(xué), 2021, 46(3): 40-47.

[19] 尹燁佳, 華驚宇, 李楓, 等. 非視距環(huán)境中的無線網(wǎng)絡(luò)殘差加權(quán)定位算法[J]. 傳感技術(shù)學(xué)報, 2019, 32(5): 749-755.

Fusion localization method of UWB/INS based on Sage-Husa adaptive UKF

SUN Wei, AN Shengjun

（School of Geomatics, Liaoning Technical University, Fuxin, Liaoning 123000, China）

An ultra-wide band/inertial navigation system (INS) tight integrated positioning method based on unscented Kalman filter (UKF) was proposed to solve the problem of significant error of single ultra-wide band (UWB) localization caused by non-line-of-sight (NLOS) in the indoor environment．The UWB measurement equation was optimized，the error state of the navigation equation was used to simplify the prediction and correction of the Kalman filtering，and the process and measurement noise were identified online through the Sage-Husa adaptive estimation and the filter’s innovation value，so that the white noise of the model was closer to a real condition，and the outlier processing method was used to reduce the influence of non-line-of-sight on the positional accuracy．The results of the emulation experiment indicate that contrast with the conventional extended Kalman filter (EKF) and UKF filtering schemes，the integrated positioning model based on UKF can effectively suppress the influence of non-line-of-sight error and improve the positional accuracy．

ultra-wide band；unscented Kalman filter；adaptive filtering；indoor localization；non-line-of-sight

P228

A

2095-4999(2022)06-0068-07

孫偉，安晟均. 基于Sage-Husa自適應(yīng)UKF的UWB/INS融合定位方法[J]. 導(dǎo)航定位學(xué)報, 2022, 10(6): 68-74.（SUN Wei，AN Shengjun. Fusion localization method of UWB/INS based on Sage-Husa adaptive UKF[J]. Journal of Navigation and Positioning, 2022, 10(6): 68-74.）

10.16547/j.cnki.10-1096. 20220609.

2022-07-08

2019遼寧省“興遼英才計劃”青年拔尖人才項(xiàng)目（XLYC1907064）；遼寧工程技術(shù)大學(xué)學(xué)科創(chuàng)新團(tuán)隊(duì)資助項(xiàng)目（LNTU20TD-06）；2018年度遼寧省“百千萬人才工程”人選科技活動資助項(xiàng)目（遼百千萬立項(xiàng)[2019]45號）。

孫偉（1984—），男，黑龍江蘿北縣人，博士，教授，研究方向?yàn)閼T性及多源信息融合導(dǎo)航技術(shù)。

安晟均（1998—），男，遼寧營口人，碩士研究生，研究方向?yàn)閼T性導(dǎo)航與組合導(dǎo)航方法。