高 嵩，宋佳鵬，房 穹，張熙為

抗差自適應(yīng)容積卡爾曼濾波在UWB室內(nèi)定位中的應(yīng)用

高 嵩，宋佳鵬，房 穹，張熙為

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

針對(duì)超寬帶（UWB）測(cè)距異常值、傳統(tǒng)濾波方法中動(dòng)力學(xué)模型不精準(zhǔn)、狀態(tài)向量誤差協(xié)方差陣非正定等問題，提出一種基于奇異值分解的抗差自適應(yīng)容積卡爾曼濾波算法，并將其應(yīng)用于UWB室內(nèi)定位中：以標(biāo)準(zhǔn)容積卡爾曼濾波（CKF）算法為基礎(chǔ)，利用殘差向量構(gòu)造抗差因子消除觀測(cè)異常值對(duì)定位解的影響；利用自適應(yīng)因子對(duì)整體模型誤差進(jìn)行實(shí)時(shí)調(diào)整和修正以提高濾波精度；同時(shí)用奇異值分解代替喬萊斯基（Cholesky）分解以進(jìn)一步提高濾波的穩(wěn)定性。實(shí)驗(yàn)結(jié)果表明，所提算法相比傳統(tǒng)的擴(kuò)展卡爾曼濾波（EKF）、無跡卡爾曼濾波（UKF）、CKF算法，能夠進(jìn)一步提高UWB系統(tǒng)的定位精度和抗干擾能力，定位最大誤差由1.5 m降至0.3 m，均方根誤差小于0.05 m。

超寬帶（UWB）定位；奇異值分解；容積卡爾曼濾波；測(cè)距異常值；系統(tǒng)噪聲

0 引言

隨著中國經(jīng)濟(jì)的轉(zhuǎn)型升級(jí)，特別是中國制造2025國家戰(zhàn)略的實(shí)施以來，目標(biāo)定位技術(shù)在諸多領(lǐng)域成為研究的熱點(diǎn)，人們對(duì)高精度定位技術(shù)的需求愈發(fā)強(qiáng)烈，希望其可以加快國民經(jīng)濟(jì)建設(shè)的步伐并助力產(chǎn)業(yè)結(jié)構(gòu)的升級(jí)。在良好的室外環(huán)境下，全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system，GNSS）可提供連續(xù)高精度定位結(jié)果；但在復(fù)雜的室內(nèi)環(huán)境下，由于GNSS信號(hào)受遮擋以及強(qiáng)烈的多路徑效應(yīng)影響，GNSS的室內(nèi)定位精度顯著下降甚至無法定位。因此需要考慮采用其他定位技術(shù)實(shí)現(xiàn)室內(nèi)定位。在眾多室內(nèi)定位技術(shù)中，超寬帶（ultra-wide band，UWB）由于其測(cè)距精度高、抗多路徑性能強(qiáng)等優(yōu)點(diǎn)，逐漸成為室內(nèi)定位技術(shù)的研究熱點(diǎn)。

雖然UWB憑借其帶寬大、時(shí)間分辨率高的優(yōu)點(diǎn)可以獲得高達(dá)厘米級(jí)的測(cè)距精度，但在實(shí)際的應(yīng)用過程中將不可避免地產(chǎn)生測(cè)量誤差[1]。UWB測(cè)距誤差主要為標(biāo)準(zhǔn)時(shí)間偏差和非視距（non-line of sight，NLOS）誤差。標(biāo)準(zhǔn)時(shí)間偏差一般容易改正，可通過改正函數(shù)對(duì)其進(jìn)行校正[2]。針對(duì)NLOS誤差的處理，主要分為抗差濾波處理方法和根據(jù)數(shù)據(jù)信號(hào)特征鑒別方法2類[3]。經(jīng)過標(biāo)準(zhǔn)時(shí)間偏差改正和NLOS誤差削弱后的UWB測(cè)距值的精度得到很大的提高；但UWB測(cè)距值中還會(huì)存在一些測(cè)量值誤差，這種誤差在濾波定位解算過程中表現(xiàn)為觀測(cè)值異常，會(huì)嚴(yán)重影響濾波解算結(jié)果的精度與穩(wěn)定。針對(duì)觀測(cè)值異常的問題，有學(xué)者將抗差估計(jì)理論與濾波算法相結(jié)合，提出了抗差卡爾曼濾波、抗差擴(kuò)展卡爾曼濾波、抗差無跡卡爾曼濾波等濾波算法[4-5]，這些算法通過對(duì)異常值降權(quán)處理抑制其對(duì)濾波解算結(jié)果的影響，能大幅度提高濾波解算結(jié)果的精度。此外在實(shí)際的運(yùn)動(dòng)過程中，載體的運(yùn)動(dòng)模型一般難以用精確的數(shù)學(xué)模型構(gòu)建，導(dǎo)致動(dòng)力學(xué)模型與載體的真實(shí)運(yùn)動(dòng)模型之間存在誤差，這會(huì)使濾波器的動(dòng)態(tài)性能受到較大影響[6]。為了有效控制動(dòng)力學(xué)模型誤差的影響，有學(xué)者提出了自適應(yīng)無跡卡爾曼濾波和自適應(yīng)容積卡爾曼濾波[7-8]，這些濾波算法通過自適應(yīng)因子控制動(dòng)態(tài)擾動(dòng)異常，能提高濾波算法的精度和穩(wěn)定性。本文在充分吸收抗差濾波、自適應(yīng)濾波和容積卡爾曼濾波各自優(yōu)點(diǎn)的基礎(chǔ)上，同時(shí)顧及在實(shí)際運(yùn)算過程中，由于計(jì)算舍入誤差、系統(tǒng)噪聲的先驗(yàn)統(tǒng)計(jì)信息不準(zhǔn)確等原因?qū)е聽顟B(tài)向量的誤差協(xié)方差陣失去對(duì)稱正定性的問題[9-10]，利用基于奇異值分解的抗差自適應(yīng)容積卡爾曼濾波（singular value decomposition adaptively robust cubature Kalman filter，SVD-ARCKF）算法進(jìn)行UWB定位解算。

1 UWB定位系統(tǒng)的動(dòng)力學(xué)模型和量測(cè)模型

UWB定位模式主要有基于接收信號(hào)強(qiáng)度、基于信號(hào)到達(dá)時(shí)間、基于信號(hào)到達(dá)時(shí)間差、基于信號(hào)到達(dá)角度和信號(hào)雙向傳播時(shí)間（round-trip time，RTT）模式[11-12]。RTT測(cè)距不需要基準(zhǔn)站和流動(dòng)站時(shí)間同步[13]，因此本文選用RTT測(cè)距模式。

在UWB的定位中，一般以UWB流動(dòng)站天線的相位中心點(diǎn)的位置和速度信息作為濾波的狀態(tài)向量。則UWB定位系統(tǒng)的動(dòng)力學(xué)模型為

UWB的量測(cè)模型為

2 基于SVD-ARCKF的UWB定位算法

該算法以標(biāo)準(zhǔn)容積卡爾曼濾波（cubature Kalman filter，CKF）算法為基礎(chǔ)，利用殘差向量構(gòu)造抗差因子，消除測(cè)距異常值對(duì)定位解的影響，利用自適應(yīng)因子對(duì)整體模型誤差進(jìn)行實(shí)時(shí)調(diào)整和修正以提高濾波精度，同時(shí)用奇異值分解（singular value decomposition，SVD）代替Cholesky分解以進(jìn)一步提高濾波的穩(wěn)定性，具體實(shí)現(xiàn)步驟為：

3）時(shí)間更新。通過UWB定位系統(tǒng)的動(dòng)力學(xué)模型計(jì)算傳播容積點(diǎn)

5）量測(cè)更新。由UWB的量測(cè)模型計(jì)算容積點(diǎn)

計(jì)算量測(cè)估計(jì)值

計(jì)算預(yù)測(cè)殘差

常用的誤差判別統(tǒng)計(jì)量有方差分量統(tǒng)計(jì)量和預(yù)測(cè)殘差統(tǒng)計(jì)量等，常用的抗差矩陣構(gòu)建法有Huber法及Andrew法等[15]。針對(duì)UWB定位系統(tǒng)模型中可能出現(xiàn)觀測(cè)量個(gè)數(shù)小于狀態(tài)向量維數(shù)的情況，本文選擇利用預(yù)測(cè)殘差值構(gòu)建統(tǒng)計(jì)量，并利用Huber法建立等價(jià)權(quán)矩陣以確保等價(jià)權(quán)陣對(duì)角線不為零。

基于預(yù)測(cè)殘差構(gòu)建的統(tǒng)計(jì)量為

抗差因子構(gòu)造的等式為

7）對(duì)狀態(tài)向量預(yù)測(cè)值協(xié)方差進(jìn)行修正。由于建立精準(zhǔn)的UWB定位系統(tǒng)模型比較困難，且系統(tǒng)噪聲的先驗(yàn)統(tǒng)計(jì)信息也常是未知或不準(zhǔn)確的，這些因素都會(huì)導(dǎo)致動(dòng)力學(xué)模型出現(xiàn)誤差。動(dòng)力學(xué)模型誤差通常影響整體的參數(shù)估計(jì)效果[16]，因此本文構(gòu)造一個(gè)自適應(yīng)因子對(duì)UWB的定位系統(tǒng)模型進(jìn)行整體的調(diào)節(jié)和修正。具體實(shí)現(xiàn)步驟為

8）狀態(tài)更新。濾波增益矩陣計(jì)算如下式

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

為了驗(yàn)證所提的SVD-ARCKF算法在UWB定位解算中的有效性，并評(píng)估其性能，本文在室內(nèi)場(chǎng)景下對(duì)UWB定位算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，并和現(xiàn)有的擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）、無跡卡爾曼濾波（unscented kalman filter，UKF）、CKF定位算法進(jìn)行對(duì)比分析。

圖1 定位場(chǎng)景

圖2 實(shí)驗(yàn)平臺(tái)

圖3 EKF定位軌跡

圖4 UKF定位軌跡

圖5 CKF定位軌跡

圖6 SVD-ARCKF定位軌跡

圖8 y方向誤差

表1 室內(nèi)實(shí)驗(yàn)中4種濾波方法的誤差統(tǒng)計(jì)信息 m

從上述結(jié)果分析可以得到：

1）4種濾波定位方法的整體軌跡都比較符合流動(dòng)站的運(yùn)動(dòng)軌跡，但在實(shí)驗(yàn)過程中，小車有一小段時(shí)間在房間的柱子中間穿過，此時(shí)UWB脈沖信號(hào)受到柱子的遮擋產(chǎn)生了NLOS誤差，平面軌跡在拐角處出現(xiàn)了持續(xù)較大的偏差。

2）EKF方法定位的精度較差，穩(wěn)定性難以得到保證，當(dāng)UWB信號(hào)受到NLOS誤差影響時(shí)，其定位精度迅速降低。由于本文只采用15個(gè)參考檢核點(diǎn)對(duì)不同方法的定位精度進(jìn)行檢驗(yàn)，部分誤差較大區(qū)域沒有設(shè)置參考檢核點(diǎn)，因此EKF方法的實(shí)際定位精度要低于表1中的統(tǒng)計(jì)結(jié)果。

3）UKF方法與CKF方法的定位精度大致相同，但都比EKF方法的定位精度高，這是因?yàn)槔肬KF和CKF方法進(jìn)行定位時(shí)不需要對(duì)UWB的量測(cè)模型進(jìn)行線性化，不會(huì)產(chǎn)生截?cái)嗾`差。

4 結(jié)束語

UWB以其距離分辨率高和抗干擾能力強(qiáng)等特點(diǎn)，在室內(nèi)定位中優(yōu)勢(shì)明顯，很多研究團(tuán)隊(duì)已取得了一定的研究成果，但在UWB測(cè)距異常值、傳統(tǒng)濾波方法中動(dòng)力學(xué)模型不精準(zhǔn)、狀態(tài)向量誤差協(xié)方差陣非正定等問題仍沒有得到很好的解決；因此本文提出SVD-ARCKF算法，并將其應(yīng)用于UWB室內(nèi)定位中。利用實(shí)驗(yàn)對(duì)所提算法進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果表明本文所提算法相比傳統(tǒng)的EKF、UKF、CKF算法更能提高UWB系統(tǒng)的定位精度和抗干擾能力。

[1] 閆大禹, 宋偉, 王旭丹, 等. 國內(nèi)室內(nèi)定位技術(shù)發(fā)展現(xiàn)狀綜述[J]. 導(dǎo)航定位學(xué)報(bào), 2019, 7(4): 5-12.

[2] CAZZORLA A, DE A G, MOSCHITTA A, et al. A 5. 6-GHz UWB position measurement system[J]. IEEE Transactions on Instrumentation and Measurement, 2013, 62(3): 675-683.

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

[4] EINICKE G A, WHITE L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[5] 張且且, 趙龍, 周建華. 抗差自適應(yīng)分步濾波算法在PPP/INS組合導(dǎo)航中的應(yīng)用[J]. 導(dǎo)航定位與授時(shí), 2020, 7(5): 73-81.

[6] YANG Y, GAO W. An optimal adaptive Kalman filter[J]. Journal of Geodesy, 2006 , 80(4): 177-183.

[7] GAO Z, MU D, GAO S, et al. Adaptive unscented Kalman filter based on maximum posterior and random weighting[J]. Aerospace Science and Technology, 2017, 71: 12-24.

[8] WANG Z, QIU H, QIAN G. Adaptive robust cubature Kalman filtering for satellite attitude estimation[J]. Chinese Journal of Aeronautics, 2018, 31(4): 806-819.

[9] 高社生, 王建超, 焦雅林. 自適應(yīng)SVD-UKF算法及在組合導(dǎo)航的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào), 2010, 18(6): 737-742.

[10] 李兆銘, 楊文革, 丁丹, 等. 基于SVD的多終端實(shí)時(shí)定軌自適應(yīng)魯棒CKF算法[J]. 儀器儀表學(xué)報(bào), 2016 , 37(3): 490-496.

[11] 王卿, 蔚保國. 基于非合作室內(nèi)環(huán)境的超寬帶無中心組網(wǎng)定位[J]. 河北科技大學(xué)學(xué)報(bào), 2021, 42(1) : 8-14.

[12] 董家志. 基于UWB的室內(nèi)定位與跟蹤算法研究[D]. 成都：電子科技大學(xué), 2015: 9-12.

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

[14] 謝強(qiáng), 唐和生, 邸元. SVD-Unscented卡爾曼濾波的非線性結(jié)構(gòu)系統(tǒng)識(shí)別[J]. 應(yīng)用力學(xué)學(xué)報(bào), 2008(1) : 57-61, 181.

[15] 楊元喜, 任夏, 許艷. 自適應(yīng)抗差濾波理論及應(yīng)用的主要進(jìn)展[J]. 導(dǎo)航定位學(xué)報(bào), 2013, 1(1) : 9-15.

[16] YANG Y, GAO W. An optimal adaptive Kalman filter[J]. Journal of Geodesy, 2006, 80(4): 177-183.

[17] ER M J, WU S, LU J, et al. Face recognition with radial basis function (RBF) neural networks[J]. IEEE Transactions on Neural Networks, 2002, 13(3): 697-710.

Application of adaptively robust cubature Kalman filter in UWB indoor location

GAO Song, SONG Jiapeng, FANG Qiong, ZHANG Xiwei

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

Aiming at the problems of ultra-wide band (UWB) ranging outliers, the inaccuracy of dynamic model in traditional filtering methods, and the non-positive definite state vector error covariance matrix, a robust adaptive volume Kalman filter algorithm based on singular value decomposition was proposed and applied to UWB indoor positioning. Based on the standard cubature Kalman filter (CKF) algorithm, the residual vector is used to construct the robust factor to eliminate the influence of the observed outliers on the positioning solution, and the adaptive factor is used to adjust and correct the overall model error in real time to improve the filtering accuracy. At the same time, the singular value decomposition is used to replace Cholesky decomposition to further improve the filtering stability. Experimental results show that compared with traditional extended Kalman filter (EKF), unscented Kalman filter (UKF) and CKF algorithms, the proposed algorithm can improve the positioning accuracy and anti-jamming ability of UWB system better. The maximum positioning error is reduced from 1.5 m to 0.3 m, and the root mean square error is less than 0.05 m.

ultra-wide band (UWB) positioning; singular value decomposition; cubature Kalman filter (CKF); ranging outliers; system noise

P228

A

2095-4999(2023)01-0142-06

高嵩，宋佳鵬，房穹，等. 抗差自適應(yīng)容積卡爾曼濾波在UWB室內(nèi)定位中的應(yīng)用[J].導(dǎo)航定位學(xué)報(bào), 2023, 11(1): 142-147.（GAO Song, SONG Jiapeng, FANG Qiong,et al. Application of adaptively robust cubature Kalman filter in UWB indoor location[J]. Journal of Navigation and Positioning, 2023, 11(1): 142-147.）DOI:10.16547/j.cnki.10-1096.20230121.

2021-08-05

國家自然科學(xué)基金項(xiàng)目（42074012）；遼寧省重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2020JH2/10100044）；遼寧省自然科學(xué)基金計(jì)劃指導(dǎo)計(jì)劃項(xiàng)目（2019-ZD-0051）；遼寧省教育廳基礎(chǔ)研究項(xiàng)目（LJ2020JCL016）。

高嵩（2000—），女，遼寧沈陽人，本科生，研究方向?yàn)槭覂?nèi)外一體化定位。

張熙為（1998—），男，遼寧阜新人，碩士研究生，研究方向?yàn)槭覂?nèi)外導(dǎo)航定位。