基于改進容積卡爾曼濾波器的超寬帶/慣性導航系統室內融合定位技術

肖春亮，張忠民

哈爾濱工程大學 信息與通信工程學院，黑龍江 哈爾濱 150001

目前，全球導航衛星系統特別是其中的北斗導航定位系統和全球定位系統，可以在室外環境中提供準確并可靠的實時定位數據[1]。然而，由于墻體、木質地板和家具等物品造成的信號阻擋、丟失和衰減，使得在室內環境下無線衛星信號無法有效使用[2]。因此，研發一個具有高魯棒性高精度的室內定位技術變得至關重要。近年來，實現室內定位的主流技術有藍牙[3]、WiFi、使用攝像機的視覺同時定位和映射[4]、光探測和測距[5]、慣性導航系統（inertial navigation system，INS）[6]等。但這些技術的定位效果都達不到室內定位的厘米級定位標準。超寬帶（ultra wide band，UWB）定位算法不但定位精度高，而且其成本低、功耗小，更適合用做室內定位。但是非視距（non line of sight，NLOS）[7]下，UWB 方法的定位精度會急劇降低。在現實中的定位環境下，為了抑制NLOS 對定位結果的不良影響，提高定位精度，單一UWB 算法已經很難滿足定位要求[8]，因此利用UWB 和其他定位技術實現多源數據融合定位成為該領域的研究重點[9]。INS 系統具有自主定位特性，將其與UWB 系統融合可以有效抑制NLOS 誤差，大幅度提升定位精度。因此，UWB/INS 融合定位算法是實現室內高精度定位的主流方案。而卡爾曼濾波器（Kalman filter，KF）由于其計算效率高并且易于實現，在數據融合中應用廣泛。為了在非線性且非高斯系統中能夠得到最優解，可以使用泰勒級數的一階來修正KF[10]，使其成為擴展卡爾曼濾波器（extended Kalman filter，EKF）[11]，但EKF 中的線性化誤差使得估計器在錯誤的答案中過于自信。無跡卡爾曼濾波器（unscented Kalman filter，UKF）使用無跡變換使KF 更適應非線性系統，與解析局部線性化相比，無跡變換擁有逼近輸出分布的性能。容積卡爾曼濾波器（cubature Kalman filter，CKF）基于三階球面徑向立方規則，使用一組體積點來逼近非線性系統的均值和協方差，其精度和效率明顯優于EKF 和UKF。

為了有效提高定位精度和魯棒性等因素，本文提出基于改進自適應H∞容積卡爾曼濾波器的UWB/INS 的室內組合定位系統，該系統主要應用于室內無人車動態定位。在此UWB/INS 定位系統中，本文所提出的方案通過NLOS 識別與修正算法，可以有效地抑制NLOS 誤差，通過INS 修正算法有效抑制了INS 系統的累計誤差，最后通過改進的自適應H∞容積卡爾曼濾波器（improved adaptive H∞cubature Kalman filter，IAH∞CKF）進行數據融合。實驗結果表明IAH∞CKF 的定位精度高于傳統的CKF 算法，該系統在室內環境中能提供魯棒性好和精度高的定位。

1 UWB/INS 定位系統

圖1是融合定位系統的總框圖。此系統主要由3 部分組成：第1 部分是INS 定位系統，通過廣義似然比檢測算法對行人的靜止態和運動態進行檢測，之后對于靜止態分別通過零速修正算法和零積分航向角速率修正算法對速度和航向角進行修正；第2 部分是UWB 定位系統，通過模糊綜合評判的非視距識別算法對非視距信號識別，之后通過基于半定規劃的非視距修正算法對識別出的信號進行修正；第3 部分是數據融合，當定位標簽值滿足UWB 更新條件時，通過改進容積卡爾曼濾波器（improved cubature Kalman filter，ICKF）算法實現UWB/INS 的緊耦合融合。

1.1 INS 的定位算法

對于INS 導航，小車上安裝的慣性測量單元（inertial measurement unit，IMU）通常應用低成本微機電系統（micro-electro mechanical system，MEMS）陀螺儀和加速計來實現。由于MEMS 器件的精度較低，本文利用零速修正(zero velocity update，ZUPT)[12]和零角速度修正(zero augular rate update，ZARU)[13]算法校正目標的加速度、角速度和姿態，以提高INS 定位的精度。捷聯慣導的導航過程為

式中：k為時間索引，tk為采樣間隔，xk為位置，vk為速度，qk為導航坐標系的載波系統，fk為加速度測量值，g為重力加速度， ωk為陀螺儀測量值，Ω(·)為四元數更新矩陣。

卡爾曼濾波校正過程如下：

式中：Kk為卡爾曼濾波增益矩陣， θk為修正后的姿態角。

圖2是INS 定位原理示意，具體流程為角速度數據經過1 次積分運算后可以得到姿態數據，加速度數據經過1 次積分運算得到速度數據，然后再通過2 次積分運算得到位置數據，最后通過姿態數據更新旋轉矩陣。

圖2 INS 定位原理

在現實環境中，由于MEMS-IMU 本身的精度在靜止時測量的加速度不是零、測量的角速度也不是零并且方向改變，如果在這一階段不修正這些數據，則慣性導航系統的累積誤差將隨著時間的推移而繼續增加。為了解決此誤差，本文通過ZUPT 和ZARU 優化算法來處理，并通過卡爾曼濾波更新目標的速度、位置和姿態信息。

1.2 UWB 的定位算法

對于2D 環境，只需3 個參考節點就可以進行基本的室內定位。目標節點的定位有多種方法，如發射信號的到達時間差（time difference of arrival，TDOA）、發射信號的到達方向（direction of arrival，DOA）、發射信號的到達時間（time of arrival，TOA）和接收信號強度（received signal strength，RSS）等。但最適用于UWB 系統的是TOA，因為它不僅利用了UWB 信號的巨大時間分辨率，而且成本低、便于布置。

1.2.1 UWB 估計算法TOA

TOA 算法是通過測量基站與待測標簽之間的時間差來進行測距。為了解決TOA 算法中因器件精度問題導致的UWB 基站和定位標簽時鐘不同步問題，本文擬使用雙邊雙向測距(double-sided two-way ranging，DS-TWR)方法進行TOA 定位。如圖3 所示，2D 環境下以UWB 各個基站為圓心，通過TOA 方法測得的距離值為半徑畫圓，理想條件下，3 個圓相交于一點，這個點就是待測定位目標的實際位置，其具體計算過程為

圖3 UWB 定位原理

式中：(x0,y0)是標簽的坐標，(xi,yi)是第i個基站的坐標，ri為標簽和第i個基站之間的距離。

狀態誤差向量的觀測值由UWB 測距數據和INS 解算位置相對于UWB 基站解算位置的差值來表示：

1.2.2 NLOS 信號的識別與修正

嚴重的NLOS 會造成TOA 定位方法的測距誤差較大，減少NLOS 誤差對于室內定位的影響是室內定位技術的重大問題。本文通過設定一個閾值來判斷NLOS 誤差，每一個時刻都可以得到UWB 和INS 這2 個系統的位置數據，通過將這2 個數據相減之后取絕對值，如果此絕對值比所設定的閾值大，那便認為這時的UWB 信號受到了NLOS 誤差干擾。本文通過大量實驗得出，在文中NLOS 實驗環境下能夠造成的測距誤差值主要在0.24～0.33 m，因此本文將閾值設置為0.24 m。

具有不確定性的真實測量噪聲協方差矩陣建模如下：式中：對角線元素對應UWB 移動標簽和每個UWB 信標之間的測量噪聲，非對角元素為UWB 移動標簽和每個UWB 信標之間的相關噪聲。由于定位精度受到NLOS 的影響，系統的定位精度可能會受到嚴重影響，甚至可能崩潰。因此，必須引入一些參數來調整R。本文調整R的方法是使用自適應魯棒函數引入魯棒性因子：

式中：k0和k1是經驗常數， 通常為2.0～3.0 和是時間索引k的第i個歸一化更新測量，可以定義為

其中(Zk-HkXk/(k-1))

i是時間索引k的第i次更新測量。

式中 γij是第i行和第j列中協方差的穩健性因子。

2 改進的自適應H∞容積卡爾曼濾波器

目前常用的融合估計方法是利用KF 算法對系統進行濾波處理，但KF 只適用于線性系統，而現實中的環境一般都是非線性系統，針對此問題，提出了EKF。但EKF 在線性化變換過程中會產生系統誤差，這些誤差會導致累積誤差甚至導致發散，為了解決這一缺陷，提出了一種以無跡變換作為傳播均值和協方差信息的方法。然而，當模型尺寸增加時，UKF 表現更差。Arasaratnam和Haykin 通過球面徑向體積規則提出了CKF，即使模型尺寸增加，其在精度方面也表現出良好的性能。但CKF 的動態適應性不好，為了解決CKF的自適應能力，本文提出改進的自適應H∞CKF。

2.1 H∞濾波器和 H∞CKF

H∞濾波器是卡爾曼濾波器的一種特殊形式。H∞濾波器是在干擾最大化的條件下最小化估計誤差。狀態轉移方程和測量方程表示如下：

式中：k為時間單元；xk為k時刻的狀態向量；yk為k時刻的觀測向量；f和h分別為狀態轉移函數和觀測函數；wk和vk分別為統計特性未知的系統噪聲項和測量噪聲，噪聲項wk和vk的統計可能是未知的或確定的。

代替直接估計狀態，可以估計狀態的線性組合：

式中：zk為要估計的信號；Lk為已知的矩陣，并且通常被設置為單位矩陣，可以直接估計狀態向量。

H∞濾波器的設計思想是找到當前P0、Qk和Rk達到上限時使成本函數最小化的方法，其中Qk和Rk分別為wk和vk的協方差矩陣，P0為根據特定問題預設的初始估計誤差協方差矩陣，其為x0與初始估計x?0的接近程度，代價函數為

其中 γ為誤差衰減參數。

結合H∞濾波器，提出了H∞容積卡爾曼濾波器（H∞CKF），此算法總結如下：

1）計算體積點：

2）容積點轉移方程為

3）計算預測狀態向量和預測誤差協方差：

4）計算容積點：

5）容積點轉移方程為

6）計算預測量測向量：

7）計算更新協方差矩陣和互協方差矩陣：

8）計算卡爾曼增益和更新狀態：

9）計算相應的誤差協方差：

式中In為單位矩陣。 γ對于體積H∞的存在非常重要。隨著 γ的增大，對系統模型的噪聲統計特性和誤差的變化濾波方法變得越來越不敏感，其魯棒性逐漸降低。然而，估計狀態的方差也被減小，并且估計精度被提高。同時，c的最小值必須保證濾波器的存在。H∞CKF 在最壞的情況下使估計誤差最小化，這使得它比體積卡爾曼濾波器更魯棒。

2.2 自適應H∞CKF 濾波算法

在標準CKF 算法中，假設噪聲的平均值為零，并且噪聲的統計特性是準確已知的；但在實踐中，統計特征通常是時變的和未知的，這會使得濾波精度下降，甚至發散。為了解決上述問題，可以在濾波過程中通過自適應算法估計和修改噪聲的統計特性。根據Sage-Husa 最大后驗估計算法，得到了次優的常噪聲統計估計。結合體積卡爾曼濾波器的估計器顯示為

3 仿真與分析

本文在NLOS 環境下進行了融合定位對比實驗，其中重點比較了聯邦EKF 算法、CKF 算法、ISRUKF 算法和ICKF 算法的融合性能，通實驗軌跡圖、定位誤差對比圖和累計分布函數圖可以清晰地看出本文所提出的ICKF 融合定位算法具有更好的定位效果。

3.1 實驗器材及環境

本文選取4 個UWB 基站作為本實驗的基站數量，這樣不但相比于5 基站能節約成本，而且相比于3 基站，4 個基站還可以增加冗余度，提升定位精度；其他實驗器材還包括1 個UWB 標簽、1 個INS 定位模塊和1 塊鐵板。各模塊參數如表1所示。

表1 模塊參數

實驗場地為邊長為10 m 的正方形室內區域；障礙物為長2.0 m、寬2 cm、高1.5m 的鐵板，當將鐵板放在正方形區域中心時，就是處于NLOS 環境?；緟翟O置如表2 所示，實驗環境平面圖如圖4 所示。

表2 基站參數m

圖4 實驗環境平面

3.2 UWB/INS 融合定位算法結果與分析

本文進行了數據融合算法對比實驗，分別對比了基于聯邦EKF、CKF 和IAH∞CKF 的融合定位實驗。實驗結果如圖5 所示。從圖5 中不難看出基于IAH∞CKF 和CKF 的UWB 組合定位算法相比于基于聯邦EKF 的組合定位算法定位效果更好，但是基于IAH∞CKF 和CKF 的融合定位算法由于定位精度都比較高，所以較難通過肉眼直接分辨定位結果。

圖5 不同算法融合定位軌跡

通過定位誤差圖6 與累積分布函數（cumulative distribution function，CDF）圖7 可更直觀地對比不同融合定位算法的定位效果。圖7 展示了根據圖6 定位結果誤差求得的誤差累積分布函數圖，通過圖7 可以看出，CKF 和IAH∞CKF 中80%的定位誤差分別在0.149 和0.124 m 以下。這說明在NLOS 環境下，基于CKF 的UWB/INS 融合定位方案具有更突出的性能，可以滿足室內厘米級定位要求。

圖6 定位誤差

圖7 累計分布函數

計算4 組定位算法的最大誤差、平均誤差和均方根誤差，結果如表3 所示。

表3 不同融合算法的定位誤差cm

由表3 數據可知，ICKF 算法相較于聯邦EKF算法、ISRUKF 算法和CKF 算法最大誤差分別減小了8.61 cm（31.96%）、8.22 cm（30.96%）、5.91 cm（24.38%），平均誤差分別減小了5.34 cm（36.55%）、3.72 cm（28.64%）、2.21 cm（19.25%），均方根誤差分別減小了5.48 cm（35.38%）、4.04 cm（28.75%）、2.32 cm（18.82%）。通過CKF 算法與ISRUKF 算法的對比實驗結果可知，CKF 算法融合了INS 修正框架與UWB 系統的NLOS 抑制算法，有效提高了CKF 的定位精度，并通過基于三階球面徑向立方規則，使用一組體積點來逼近非線性系統的均值和協方差，使其精度和效率明顯優于ISRUKF 算法。但在估計最壞的情況下，誤差較大，會影響定位魯棒性。通過ICKF 算法與CKF 算法的實驗結果對比可知，ICKF 算法通過加入H∞模塊和自適應濾波器模塊有效地解決了CKF 算法在估計最壞的情況下，誤差較大、魯棒性不好的問題，有效地提高了定位精度，達到了厘米級室內精度的定位要求。

4 結束語

針對UWB 室內定位算法容易受到NLOS 誤差影響的問題，本文提出了一種基于ICKF 的UWB/INS 室內融合定位算法，通過改進的自適應H∞容積卡爾曼濾波器將UWB 定位數據和INS 定位數據進行融合。其中ICKF 算法是在標準CKF算法的基礎上增加了H∞濾波器、Sage-Husa 濾波器，這使其不但能有效抑制NLOS 誤差而且具有自適應性，更適用于動態環境。之后進行了對比實驗，實驗結果表明ICKF 相比于CKF 均方根誤差減小了2.32 cm（18.82%），這表明基于ICKF 的UWB/INS 融合定位算法具有更高的精度和更好的適應性和魯棒性。