基于Sage-Husa自適應UKF的UWB/INS融合定位方法

孫 偉，安晟均

基于Sage-Husa自適應UKF的UWB/INS融合定位方法

孫 偉，安晟均

（遼寧工程技術大學測繪與地理科學學院，遼寧阜新 123000）

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

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

0 引言

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

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

為提高定位系統(tǒng)穩(wěn)固性及位置輸出平順性，本文提出基于UKF的UWB/INS緊組合濾波模型后，采用薩格-胡薩（Sage-Husa）自適應濾波對其進行優(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é)點在相對較小的定位范圍內(nèi)低速運動，可以不考慮地球曲率及自轉，從而得出慣導狀態(tài)方程為

1.2 量測方程

1.3 融合濾波

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

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

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

2 基于Sage-Husa的自適應估計

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

3 非視距處理

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

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

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

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

4 仿真及結果分析

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

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

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

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

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

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

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

圖1 仿真實驗場景

表2 UWB基站點號及坐標 m

4.2 結果及分析

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結束語

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

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

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

[3] 姚子揚, 尚俊娜, 孫建強, 等. 一種多源融合室內(nèi)外無縫定位方法[J]. 傳感技術學報, 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]. 信息工程大學學報, 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]. 北京大學學報(自然科學版), 2017, 53(6): 1003-1010.

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

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

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

[14] 黃劍雄, 劉小雄, 章衛(wèi)國, 等. 基于視覺/慣導的無人機組合導航算法研究[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] 盛坤鵬, 王堅, 李晨輝, 等. 非視距誤差改正的超寬帶定位模型研究[J]. 測繪科學, 2021, 46(3): 40-47.

[19] 尹燁佳, 華驚宇, 李楓, 等. 非視距環(huán)境中的無線網(wǎng)絡殘差加權定位算法[J]. 傳感技術學報, 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自適應UKF的UWB/INS融合定位方法[J]. 導航定位學報, 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遼寧省“興遼英才計劃”青年拔尖人才項目（XLYC1907064）；遼寧工程技術大學學科創(chuàng)新團隊資助項目（LNTU20TD-06）；2018年度遼寧省“百千萬人才工程”人選科技活動資助項目（遼百千萬立項[2019]45號）。

孫偉（1984—），男，黑龍江蘿北縣人，博士，教授，研究方向為慣性及多源信息融合導航技術。

安晟均（1998—），男，遼寧營口人，碩士研究生，研究方向為慣性導航與組合導航方法。