抗差自適應容積卡爾曼濾波在UWB室內定位中的應用

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

抗差自適應容積卡爾曼濾波在UWB室內定位中的應用

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

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

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

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

0 引言

隨著中國經濟的轉型升級，特別是中國制造2025國家戰略的實施以來，目標定位技術在諸多領域成為研究的熱點，人們對高精度定位技術的需求愈發強烈，希望其可以加快國民經濟建設的步伐并助力產業結構的升級。在良好的室外環境下，全球衛星導航系統（global navigation satellite system，GNSS）可提供連續高精度定位結果；但在復雜的室內環境下，由于GNSS信號受遮擋以及強烈的多路徑效應影響，GNSS的室內定位精度顯著下降甚至無法定位。因此需要考慮采用其他定位技術實現室內定位。在眾多室內定位技術中，超寬帶（ultra-wide band，UWB）由于其測距精度高、抗多路徑性能強等優點，逐漸成為室內定位技術的研究熱點。

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

1 UWB定位系統的動力學模型和量測模型

UWB定位模式主要有基于接收信號強度、基于信號到達時間、基于信號到達時間差、基于信號到達角度和信號雙向傳播時間（round-trip time，RTT）模式[11-12]。RTT測距不需要基準站和流動站時間同步[13]，因此本文選用RTT測距模式。

在UWB的定位中，一般以UWB流動站天線的相位中心點的位置和速度信息作為濾波的狀態向量。則UWB定位系統的動力學模型為

UWB的量測模型為

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

該算法以標準容積卡爾曼濾波（cubature Kalman filter，CKF）算法為基礎，利用殘差向量構造抗差因子，消除測距異常值對定位解的影響，利用自適應因子對整體模型誤差進行實時調整和修正以提高濾波精度，同時用奇異值分解（singular value decomposition，SVD）代替Cholesky分解以進一步提高濾波的穩定性，具體實現步驟為：

3）時間更新。通過UWB定位系統的動力學模型計算傳播容積點

5）量測更新。由UWB的量測模型計算容積點

計算量測估計值

計算預測殘差

常用的誤差判別統計量有方差分量統計量和預測殘差統計量等，常用的抗差矩陣構建法有Huber法及Andrew法等[15]。針對UWB定位系統模型中可能出現觀測量個數小于狀態向量維數的情況，本文選擇利用預測殘差值構建統計量，并利用Huber法建立等價權矩陣以確保等價權陣對角線不為零。

基于預測殘差構建的統計量為

抗差因子構造的等式為

7）對狀態向量預測值協方差進行修正。由于建立精準的UWB定位系統模型比較困難，且系統噪聲的先驗統計信息也常是未知或不準確的，這些因素都會導致動力學模型出現誤差。動力學模型誤差通常影響整體的參數估計效果[16]，因此本文構造一個自適應因子對UWB的定位系統模型進行整體的調節和修正。具體實現步驟為

8）狀態更新。濾波增益矩陣計算如下式

3 實驗與結果分析

為了驗證所提的SVD-ARCKF算法在UWB定位解算中的有效性，并評估其性能，本文在室內場景下對UWB定位算法進行了實驗驗證，并和現有的擴展卡爾曼濾波（extended Kalman filter，EKF）、無跡卡爾曼濾波（unscented kalman filter，UKF）、CKF定位算法進行對比分析。

圖1 定位場景

圖2 實驗平臺

圖3 EKF定位軌跡

圖4 UKF定位軌跡

圖5 CKF定位軌跡

圖6 SVD-ARCKF定位軌跡

圖8 y方向誤差

表1 室內實驗中4種濾波方法的誤差統計信息 m

從上述結果分析可以得到：

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

2）EKF方法定位的精度較差，穩定性難以得到保證，當UWB信號受到NLOS誤差影響時，其定位精度迅速降低。由于本文只采用15個參考檢核點對不同方法的定位精度進行檢驗，部分誤差較大區域沒有設置參考檢核點，因此EKF方法的實際定位精度要低于表1中的統計結果。

3）UKF方法與CKF方法的定位精度大致相同，但都比EKF方法的定位精度高，這是因為利用UKF和CKF方法進行定位時不需要對UWB的量測模型進行線性化，不會產生截斷誤差。

4 結束語

UWB以其距離分辨率高和抗干擾能力強等特點，在室內定位中優勢明顯，很多研究團隊已取得了一定的研究成果，但在UWB測距異常值、傳統濾波方法中動力學模型不精準、狀態向量誤差協方差陣非正定等問題仍沒有得到很好的解決；因此本文提出SVD-ARCKF算法，并將其應用于UWB室內定位中。利用實驗對所提算法進行驗證，實驗結果表明本文所提算法相比傳統的EKF、UKF、CKF算法更能提高UWB系統的定位精度和抗干擾能力。

[1] 閆大禹, 宋偉, 王旭丹, 等. 國內室內定位技術發展現狀綜述[J]. 導航定位學報, 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] 盛坤鵬, 王堅, 李晨輝, 等. 非視距誤差改正的超寬帶定位模型研究[J]. 測繪科學, 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] 張且且, 趙龍, 周建華. 抗差自適應分步濾波算法在PPP/INS組合導航中的應用[J]. 導航定位與授時, 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] 高社生, 王建超, 焦雅林. 自適應SVD-UKF算法及在組合導航的應用[J]. 中國慣性技術學報, 2010, 18(6): 737-742.

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

[11] 王卿, 蔚保國. 基于非合作室內環境的超寬帶無中心組網定位[J]. 河北科技大學學報, 2021, 42(1) : 8-14.

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

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

[14] 謝強, 唐和生, 邸元. SVD-Unscented卡爾曼濾波的非線性結構系統識別[J]. 應用力學學報, 2008(1) : 57-61, 181.

[15] 楊元喜, 任夏, 許艷. 自適應抗差濾波理論及應用的主要進展[J]. 導航定位學報, 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

高嵩，宋佳鵬，房穹，等. 抗差自適應容積卡爾曼濾波在UWB室內定位中的應用[J].導航定位學報, 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

國家自然科學基金項目（42074012）；遼寧省重點研發計劃項目（2020JH2/10100044）；遼寧省自然科學基金計劃指導計劃項目（2019-ZD-0051）；遼寧省教育廳基礎研究項目（LJ2020JCL016）。

高嵩（2000—），女，遼寧沈陽人，本科生，研究方向為室內外一體化定位。

張熙為（1998—），男，遼寧阜新人，碩士研究生，研究方向為室內外導航定位。