肖春亮，張忠民

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

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

為了有效提高定位精度和魯棒性等因素，本文提出基于改進(jìn)自適應(yīng)H∞容積卡爾曼濾波器的UWB/INS 的室內(nèi)組合定位系統(tǒng)，該系統(tǒng)主要應(yīng)用于室內(nèi)無人車動態(tài)定位。在此UWB/INS 定位系統(tǒng)中，本文所提出的方案通過NLOS 識別與修正算法，可以有效地抑制NLOS 誤差，通過INS 修正算法有效抑制了INS 系統(tǒng)的累計誤差，最后通過改進(jìn)的自適應(yīng)H∞容積卡爾曼濾波器（improved adaptive H∞cubature Kalman filter，IAH∞CKF）進(jìn)行數(shù)據(jù)融合。實驗結(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)，通過廣義似然比檢測算法對行人的靜止態(tài)和運(yùn)動態(tài)進(jìn)行檢測，之后對于靜止態(tài)分別通過零速修正算法和零積分航向角速率修正算法對速度和航向角進(jìn)行修正；第2 部分是UWB 定位系統(tǒng)，通過模糊綜合評判的非視距識別算法對非視距信號識別，之后通過基于半定規(guī)劃的非視距修正算法對識別出的信號進(jìn)行修正；第3 部分是數(shù)據(jù)融合，當(dāng)定位標(biāo)簽值滿足UWB 更新條件時，通過改進(jìn)容積卡爾曼濾波器（improved cubature Kalman filter，ICKF）算法實現(xiàn)UWB/INS 的緊耦合融合。

1.1 INS 的定位算法

對于INS 導(dǎo)航，小車上安裝的慣性測量單元（inertial measurement unit，IMU）通常應(yīng)用低成本微機(jī)電系統(tǒng)（micro-electro mechanical system，MEMS）陀螺儀和加速計來實現(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)航過程為

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

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

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

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

圖2 INS 定位原理

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

1.2 UWB 的定位算法

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

1.2.1 UWB 估計算法TOA

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

圖3 UWB 定位原理

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

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

1.2.2 NLOS 信號的識別與修正

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

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

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

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

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

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

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

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

2.1 H∞濾波器和 H∞CKF

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

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

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

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

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

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

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

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

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

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

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

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

6）計算預(yù)測量測向量：

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

8）計算卡爾曼增益和更新狀態(tài)：

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

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

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

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

3 仿真與分析

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

3.1 實驗器材及環(huán)境

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

表1 模塊參數(shù)

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

表2 基站參數(shù)m

圖4 實驗環(huán)境平面

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

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

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

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

圖6 定位誤差

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

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

4 結(jié)束語

針對UWB 室內(nèi)定位算法容易受到NLOS 誤差影響的問題，本文提出了一種基于ICKF 的UWB/INS 室內(nèi)融合定位算法，通過改進(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)性，更適用于動態(tài)環(huán)境。之后進(jìn)行了對比實驗，實驗結(jié)果表明ICKF 相比于CKF 均方根誤差減小了2.32 cm（18.82%），這表明基于ICKF 的UWB/INS 融合定位算法具有更高的精度和更好的適應(yīng)性和魯棒性。