基于改進(jìn)容積卡爾曼濾波器的超寬帶/慣性導(dǎo)航系統(tǒng)室內(nèi)融合定位技術(shù)

肖春亮，張忠民

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

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

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

1 UWB/INS 定位系統(tǒng)

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

1.1 INS 的定位算法

對(duì)于INS 導(dǎo)航，小車上安裝的慣性測(cè)量單元（inertial measurement unit，IMU）通常應(yīng)用低成本微機(jī)電系統(tǒng)（micro-electro mechanical system，MEMS）陀螺儀和加速計(jì)來(lái)實(shí)現(xiàn)。由于MEMS 器件的精度較低，本文利用零速修正(zero velocity update，ZUPT)[12]和零角速度修正(zero augular rate update，ZARU)[13]算法校正目標(biāo)的加速度、角速度和姿態(tài)，以提高INS 定位的精度。捷聯(lián)慣導(dǎo)的導(dǎo)航過(guò)程為

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

卡爾曼濾波校正過(guò)程如下：

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

圖2是INS 定位原理示意，具體流程為角速度數(shù)據(jù)經(jīng)過(guò)1 次積分運(yùn)算后可以得到姿態(tài)數(shù)據(jù)，加速度數(shù)據(jù)經(jīng)過(guò)1 次積分運(yùn)算得到速度數(shù)據(jù)，然后再通過(guò)2 次積分運(yùn)算得到位置數(shù)據(jù)，最后通過(guò)姿態(tài)數(shù)據(jù)更新旋轉(zhuǎn)矩陣。

圖2 INS 定位原理

在現(xiàn)實(shí)環(huán)境中，由于MEMS-IMU 本身的精度在靜止時(shí)測(cè)量的加速度不是零、測(cè)量的角速度也不是零并且方向改變，如果在這一階段不修正這些數(shù)據(jù)，則慣性導(dǎo)航系統(tǒng)的累積誤差將隨著時(shí)間的推移而繼續(xù)增加。為了解決此誤差，本文通過(guò)ZUPT 和ZARU 優(yōu)化算法來(lái)處理，并通過(guò)卡爾曼濾波更新目標(biāo)的速度、位置和姿態(tài)信息。

1.2 UWB 的定位算法

對(duì)于2D 環(huán)境，只需3 個(gè)參考節(jié)點(diǎn)就可以進(jìn)行基本的室內(nèi)定位。目標(biāo)節(jié)點(diǎn)的定位有多種方法，如發(fā)射信號(hào)的到達(dá)時(shí)間差（time difference of arrival，TDOA）、發(fā)射信號(hào)的到達(dá)方向（direction of arrival，DOA）、發(fā)射信號(hào)的到達(dá)時(shí)間（time of arrival，TOA）和接收信號(hào)強(qiáng)度（received signal strength，RSS）等。但最適用于UWB 系統(tǒng)的是TOA，因?yàn)樗粌H利用了UWB 信號(hào)的巨大時(shí)間分辨率，而且成本低、便于布置。

1.2.1 UWB 估計(jì)算法TOA

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

圖3 UWB 定位原理

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

狀態(tài)誤差向量的觀測(cè)值由UWB 測(cè)距數(shù)據(jù)和INS 解算位置相對(duì)于UWB 基站解算位置的差值來(lái)表示：

1.2.2 NLOS 信號(hào)的識(shí)別與修正

嚴(yán)重的NLOS 會(huì)造成TOA 定位方法的測(cè)距誤差較大，減少NLOS 誤差對(duì)于室內(nèi)定位的影響是室內(nèi)定位技術(shù)的重大問(wèn)題。本文通過(guò)設(shè)定一個(gè)閾值來(lái)判斷NLOS 誤差，每一個(gè)時(shí)刻都可以得到UWB 和INS 這2 個(gè)系統(tǒng)的位置數(shù)據(jù)，通過(guò)將這2 個(gè)數(shù)據(jù)相減之后取絕對(duì)值，如果此絕對(duì)值比所設(shè)定的閾值大，那便認(rèn)為這時(shí)的UWB 信號(hào)受到了NLOS 誤差干擾。本文通過(guò)大量實(shí)驗(yàn)得出，在文中NLOS 實(shí)驗(yàn)環(huán)境下能夠造成的測(cè)距誤差值主要在0.24～0.33 m，因此本文將閾值設(shè)置為0.24 m。

具有不確定性的真實(shí)測(cè)量噪聲協(xié)方差矩陣建模如下：式中：對(duì)角線元素對(duì)應(yīng)UWB 移動(dòng)標(biāo)簽和每個(gè)UWB 信標(biāo)之間的測(cè)量噪聲，非對(duì)角元素為UWB 移動(dòng)標(biāo)簽和每個(gè)UWB 信標(biāo)之間的相關(guān)噪聲。由于定位精度受到NLOS 的影響，系統(tǒng)的定位精度可能會(huì)受到嚴(yán)重影響，甚至可能崩潰。因此，必須引入一些參數(shù)來(lái)調(diào)整R。本文調(diào)整R的方法是使用自適應(yīng)魯棒函數(shù)引入魯棒性因子：

式中：k0和k1是經(jīng)驗(yàn)常數(shù)， 通常為2.0～3.0 和是時(shí)間索引k的第i個(gè)歸一化更新測(cè)量，可以定義為

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

i是時(shí)間索引k的第i次更新測(cè)量。

式中 γij是第i行和第j列中協(xié)方差的穩(wěn)健性因子。

2 改進(jìn)的自適應(yīng)H∞容積卡爾曼濾波器

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

2.1 H∞濾波器和 H∞CKF

H∞濾波器是卡爾曼濾波器的一種特殊形式。H∞濾波器是在干擾最大化的條件下最小化估計(jì)誤差。狀態(tài)轉(zhuǎn)移方程和測(cè)量方程表示如下：

式中：k為時(shí)間單元；xk為k時(shí)刻的狀態(tài)向量；yk為k時(shí)刻的觀測(cè)向量；f和h分別為狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)；wk和vk分別為統(tǒng)計(jì)特性未知的系統(tǒng)噪聲項(xiàng)和測(cè)量噪聲，噪聲項(xiàng)wk和vk的統(tǒng)計(jì)可能是未知的或確定的。

代替直接估計(jì)狀態(tài)，可以估計(jì)狀態(tài)的線性組合：

式中：zk為要估計(jì)的信號(hào)；Lk為已知的矩陣，并且通常被設(shè)置為單位矩陣，可以直接估計(jì)狀態(tài)向量。

H∞濾波器的設(shè)計(jì)思想是找到當(dāng)前P0、Qk和Rk達(dá)到上限時(shí)使成本函數(shù)最小化的方法，其中Qk和Rk分別為wk和vk的協(xié)方差矩陣，P0為根據(jù)特定問(wèn)題預(yù)設(shè)的初始估計(jì)誤差協(xié)方差矩陣，其為x0與初始估計(jì)x?0的接近程度，代價(jià)函數(shù)為

其中 γ為誤差衰減參數(shù)。

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

1）計(jì)算體積點(diǎn)：

2）容積點(diǎn)轉(zhuǎn)移方程為

3）計(jì)算預(yù)測(cè)狀態(tài)向量和預(yù)測(cè)誤差協(xié)方差：

4）計(jì)算容積點(diǎn)：

5）容積點(diǎn)轉(zhuǎn)移方程為

6）計(jì)算預(yù)測(cè)量測(cè)向量：

7）計(jì)算更新協(xié)方差矩陣和互協(xié)方差矩陣：

8）計(jì)算卡爾曼增益和更新?tīng)顟B(tài)：

9）計(jì)算相應(yīng)的誤差協(xié)方差：

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

2.2 自適應(yīng)H∞CKF 濾波算法

在標(biāo)準(zhǔn)CKF 算法中，假設(shè)噪聲的平均值為零，并且噪聲的統(tǒng)計(jì)特性是準(zhǔn)確已知的；但在實(shí)踐中，統(tǒng)計(jì)特征通常是時(shí)變的和未知的，這會(huì)使得濾波精度下降，甚至發(fā)散。為了解決上述問(wèn)題，可以在濾波過(guò)程中通過(guò)自適應(yīng)算法估計(jì)和修改噪聲的統(tǒng)計(jì)特性。根據(jù)Sage-Husa 最大后驗(yàn)估計(jì)算法，得到了次優(yōu)的常噪聲統(tǒng)計(jì)估計(jì)。結(jié)合體積卡爾曼濾波器的估計(jì)器顯示為

3 仿真與分析

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

3.1 實(shí)驗(yàn)器材及環(huán)境

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

表1 模塊參數(shù)

實(shí)驗(yàn)場(chǎng)地為邊長(zhǎng)為10 m 的正方形室內(nèi)區(qū)域；障礙物為長(zhǎng)2.0 m、寬2 cm、高1.5m 的鐵板，當(dāng)將鐵板放在正方形區(qū)域中心時(shí)，就是處于NLOS 環(huán)境。基站參數(shù)設(shè)置如表2 所示，實(shí)驗(yàn)環(huán)境平面圖如圖4 所示。

表2 基站參數(shù)m

圖4 實(shí)驗(yàn)環(huán)境平面

3.2 UWB/INS 融合定位算法結(jié)果與分析

本文進(jìn)行了數(shù)據(jù)融合算法對(duì)比實(shí)驗(yàn)，分別對(duì)比了基于聯(lián)邦EKF、CKF 和IAH∞CKF 的融合定位實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果如圖5 所示。從圖5 中不難看出基于IAH∞CKF 和CKF 的UWB 組合定位算法相比于基于聯(lián)邦EKF 的組合定位算法定位效果更好，但是基于IAH∞CKF 和CKF 的融合定位算法由于定位精度都比較高，所以較難通過(guò)肉眼直接分辨定位結(jié)果。

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

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

圖6 定位誤差

圖7 累計(jì)分布函數(shù)

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

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

由表3 數(shù)據(jù)可知，ICKF 算法相較于聯(lián)邦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%）。通過(guò)CKF 算法與ISRUKF 算法的對(duì)比實(shí)驗(yàn)結(jié)果可知，CKF 算法融合了INS 修正框架與UWB 系統(tǒng)的NLOS 抑制算法，有效提高了CKF 的定位精度，并通過(guò)基于三階球面徑向立方規(guī)則，使用一組體積點(diǎn)來(lái)逼近非線性系統(tǒng)的均值和協(xié)方差，使其精度和效率明顯優(yōu)于ISRUKF 算法。但在估計(jì)最壞的情況下，誤差較大，會(huì)影響定位魯棒性。通過(guò)ICKF 算法與CKF 算法的實(shí)驗(yàn)結(jié)果對(duì)比可知，ICKF 算法通過(guò)加入H∞模塊和自適應(yīng)濾波器模塊有效地解決了CKF 算法在估計(jì)最壞的情況下，誤差較大、魯棒性不好的問(wèn)題，有效地提高了定位精度，達(dá)到了厘米級(jí)室內(nèi)精度的定位要求。

4 結(jié)束語(yǔ)

針對(duì)UWB 室內(nèi)定位算法容易受到NLOS 誤差影響的問(wèn)題，本文提出了一種基于ICKF 的UWB/INS 室內(nèi)融合定位算法，通過(guò)改進(jìn)的自適應(yīng)H∞容積卡爾曼濾波器將UWB 定位數(shù)據(jù)和INS 定位數(shù)據(jù)進(jìn)行融合。其中ICKF 算法是在標(biāo)準(zhǔn)CKF算法的基礎(chǔ)上增加了H∞濾波器、Sage-Husa 濾波器，這使其不但能有效抑制NLOS 誤差而且具有自適應(yīng)性，更適用于動(dòng)態(tài)環(huán)境。之后進(jìn)行了對(duì)比實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明ICKF 相比于CKF 均方根誤差減小了2.32 cm（18.82%），這表明基于ICKF 的UWB/INS 融合定位算法具有更高的精度和更好的適應(yīng)性和魯棒性。