基于改進(jìn)SHKF 算法的UWB/IMU 組合定位方法

黃衛(wèi)華，梅宇恒，章 政，趙廣營(yíng)，劉思賢

（武漢科技大學(xué) 信息科學(xué)與工程學(xué)院，武漢 430081）

超寬帶（Ultra-Wide Band,UWB）定位技術(shù)因其定位精度相對(duì)較高、信號(hào)功耗相對(duì)低且抗多徑效應(yīng)較好等特性，被廣泛應(yīng)用于室內(nèi)定位、體育運(yùn)動(dòng)、智能制造等領(lǐng)域[1-3]。慣性測(cè)量單元（Inertial Measurement Unit,IMU）僅使用內(nèi)部慣性信息，具有不易受外部環(huán)境影響、可提供即時(shí)相對(duì)位置信息等特點(diǎn)[4-6]。在復(fù)雜的環(huán)境下，UWB/IMU 組合定位方法既可利用IMU 不易受環(huán)境干擾的特點(diǎn)，修正易受復(fù)雜環(huán)境干擾的UWB 信號(hào)；也可利用UWB 的絕對(duì)位置信息消除IMU 由于隨機(jī)游走與溫度漂移造成的累積誤差[7,8]。同時(shí)，UWB 定位系統(tǒng)不可避免地會(huì)遇到非視距（Non-line of Sight,NLOS）場(chǎng)景，且存在因信號(hào)波動(dòng)或擾動(dòng)而產(chǎn)生的隨機(jī)誤差，使得UWB/IMU 定位精度下降[9]。因此，研究鑒別NLOS 信號(hào)，以及確定UWB/IMU 傳感器數(shù)據(jù)的組合定位方法具有重要意義。

目前，許多學(xué)者采用機(jī)器學(xué)習(xí)的方法對(duì)UWB 采集的測(cè)距信號(hào)進(jìn)行分類(lèi)鑒別。文獻(xiàn)[10]同時(shí)分析UWB信號(hào)總功率與首徑功率，構(gòu)建深度學(xué)習(xí)模型，以鑒別出NLOS 環(huán)境下的UWB 基站測(cè)距數(shù)據(jù)。文獻(xiàn)[11]以UWB 的信號(hào)強(qiáng)度和距離估算值作為輸入特征，分別比較幾種不同機(jī)器學(xué)習(xí)算法對(duì)NLOS 信號(hào)鑒別的效果。文獻(xiàn)[12]以NLOS 環(huán)境下的誤差序列作為輸入，通過(guò)反向傳播神經(jīng)網(wǎng)絡(luò)建立誤差改進(jìn)模型，改正預(yù)測(cè)得到標(biāo)簽的誤差值。文獻(xiàn)[13]以UWB 波形信息作為特征，基于 python 的 Scikit-learn 庫(kù)，使用支持向量機(jī)（Support Vector Machine,SVM）、隨機(jī)森林與神經(jīng)網(wǎng)絡(luò)三種機(jī)器學(xué)習(xí)鑒別NLOS 信號(hào)。在實(shí)際場(chǎng)景中，UWB 測(cè)距信號(hào)與室內(nèi)障礙物的分布信息密切相關(guān)，測(cè)距信號(hào)的特征與標(biāo)簽映射關(guān)系的不確定性會(huì)影響機(jī)器學(xué)習(xí)的效率。此外，在UWB 測(cè)距信號(hào)采集過(guò)程中，不可避免會(huì)出現(xiàn)噪聲，直接對(duì)UWB 測(cè)距信號(hào)進(jìn)行鑒別，會(huì)影響鑒別NLOS 信號(hào)的準(zhǔn)確性。

Sage-Husa 卡爾曼濾波（Sage-Husa Kalman Filter,SHKF）算法是一種用于狀態(tài)估計(jì)的濾波算法，具有應(yīng)用靈活性高、估計(jì)結(jié)果準(zhǔn)確性強(qiáng)等優(yōu)點(diǎn)。文獻(xiàn)[14]通過(guò)陀螺儀信息建立狀態(tài)方程，通過(guò)加速度計(jì)與磁力計(jì)的信息建立量測(cè)方程，采用SHKF 算法將陀螺儀、加速度計(jì)與磁力計(jì)組合以獲取無(wú)人機(jī)的航姿信息。文獻(xiàn)[15]以SHKF 為基礎(chǔ)，采用漸消因子進(jìn)行動(dòng)態(tài)調(diào)整，將捷聯(lián)慣導(dǎo)系統(tǒng)與多普勒計(jì)程儀組合，提升了多普勒計(jì)程儀量測(cè)信息異常情況下的導(dǎo)航精度。對(duì)于UWB/IMU組合定位而言，傳感器自身缺陷會(huì)引起如隨機(jī)誤差、溫度漂移等噪聲類(lèi)型與幅值多變的問(wèn)題，導(dǎo)致UWB/IMU 定位的新息誤差產(chǎn)生隨機(jī)性的異常值，從而SHKF 算法對(duì)UWB 與IMU 噪聲協(xié)方差估計(jì)值的準(zhǔn)確性降低。由于歷史噪聲的影響，當(dāng)兩者均產(chǎn)生較大誤差時(shí)，SHKF 算法無(wú)法將UWB/IMU 組合定位系統(tǒng)的定位估計(jì)值與實(shí)際值的偏差及時(shí)消除，甚至濾波器會(huì)逐漸失去估計(jì)作用。

鑒于上述情況，本文在實(shí)現(xiàn)基于概率密度提升樹(shù)的NLOS 信號(hào)鑒別方法的基礎(chǔ)上，提出了一種改進(jìn)SHKF 的 UWB/IMU 組合定位方法。首先，將UWB/IMU 特征數(shù)據(jù)的概率密度函數(shù)引入提升樹(shù)的損失函數(shù)中，對(duì)NLOS 信號(hào)進(jìn)行鑒別。然后，為了降低歷史噪聲對(duì)當(dāng)前定位的影響，在SHKF 算法中引入漸消因子，修改臨近量測(cè)數(shù)據(jù)的權(quán)值，并定義表示新息變化趨勢(shì)的自適應(yīng)因子，自動(dòng)調(diào)整用于補(bǔ)償新息誤差所采用的策略，從而提升SHKF 算法的準(zhǔn)確性和穩(wěn)定性。

1 基于概率密度提升樹(shù)的NLOS 信號(hào)鑒別方法

提升樹(shù)算法是以決策樹(shù)為基學(xué)習(xí)器，采用加法模型和前向分布算法的提升方法，具有模型簡(jiǎn)便、訓(xùn)練數(shù)據(jù)量少的優(yōu)點(diǎn)。在復(fù)雜環(huán)境下，UWB 測(cè)距存在的NLOS 問(wèn)題影響UWB/IMU 定位精度，采用提升樹(shù)對(duì)UWB 信號(hào)進(jìn)行鑒別，可以提升UWB 傳感器定位數(shù)據(jù)的質(zhì)量。同時(shí)，考慮到提升樹(shù)中訓(xùn)練數(shù)據(jù)特征屬性與訓(xùn)練效率的關(guān)聯(lián)性，為了細(xì)化UWB/IMU 特征數(shù)據(jù)屬性，提高提升樹(shù)的訓(xùn)練效率，將UWB/IMU 采集的特征數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析，把所得特征數(shù)據(jù)的分布密度引入提升樹(shù)中。基于概率密度提升樹(shù)的NLOS 信號(hào)鑒別方法如圖1 所示。

圖1 基于概率密度提升樹(shù)的NLOS 信號(hào)鑒別方法Fig.1 NLOS signal identification method based on probability density boosting tree

1.1 UWB/IMU 數(shù)據(jù)特征選取

一般而言，可由UWB 的信道信息與量測(cè)信息鑒別出NLOS 信號(hào)。當(dāng)出現(xiàn)NLOS 情況時(shí)，UWB 信號(hào)功率會(huì)產(chǎn)生顯著變化，因此選取信號(hào)功率估計(jì)偏差作為特征一；此外，由于IMU 定位屬于無(wú)信標(biāo)定位，不易受NLOS 環(huán)境干擾，解算UWB 標(biāo)簽與UWB 基站的距離以及IMU 傳感器與UWB 基站的距離，通過(guò)比較這兩者的差值判斷UWB 測(cè)距數(shù)據(jù)的情況，并將其作為特征二。

特征一T1與特征二T2分別表示為：

其中，RxPower表示UWB 信號(hào)接收估計(jì)功率；FpPower表示UWB 首徑信號(hào)計(jì)算估計(jì)功率；DisIU表示IMU傳感器與UWB 基站的距離；DisUU表示UWB 標(biāo)簽與UWB 基站的距離。

定義特征向量T=[T1T2]T，則含有N組訓(xùn)練數(shù)據(jù)的集合Sd為：

其中，Ti、Bi分別表示第i組數(shù)據(jù)集的特征向量與其標(biāo)簽；Sdi表示第i組數(shù)據(jù)。設(shè)置標(biāo)簽為：

1.2 基于概率密度提升樹(shù)的構(gòu)建

1）融合概率密度的損失函數(shù)定義

當(dāng)UWB 傳感器采集的數(shù)據(jù)出現(xiàn)噪聲時(shí)，特征值方差大的數(shù)據(jù)難以與標(biāo)簽映射，導(dǎo)致采用提升樹(shù)對(duì)UWB 信號(hào)進(jìn)行特征分類(lèi)時(shí)，難以鑒別出NLOS 信號(hào)。通過(guò)在損失函數(shù)中融入U(xiǎn)WB/IMU 特征數(shù)據(jù)的分布密度，進(jìn)而細(xì)化特征數(shù)據(jù)屬性，提高提升樹(shù)的學(xué)習(xí)效率。

假設(shè)前m次迭代產(chǎn)生的決策樹(shù)所構(gòu)建的提升樹(shù)模型Trm為：

其中，σTik表示每個(gè)特征點(diǎn)的分離程度；σ1/N表示特征集的方差表示以特征值為切分點(diǎn)時(shí)，信號(hào)鑒別錯(cuò)誤的情況；表示特征集的平均值。

第m次迭代的累積概率密度函數(shù)為：

將式(6)引入提升樹(shù)的損失函數(shù)，則有：

2）決策樹(shù)的構(gòu)建

每一輪迭代都以求解損失函數(shù)的極小值為目標(biāo)產(chǎn)生決策樹(shù)，進(jìn)而不斷提升模型的準(zhǔn)確性，即解算出與αm：

聯(lián)立式(4)以及式(6)～式(8)，則有：

考慮到UWB 信號(hào)鑒別可以表示為鑒別正確情況與錯(cuò)誤情況的組合形式，則可將式(9)極小化的損失函數(shù)重新整理為：

根據(jù)極值判別式，式(11)對(duì)αm求一階導(dǎo)并令其等于0，則可以解算出αm：

為了方便算法迭代，采用分類(lèi)誤差率代替損失函數(shù)來(lái)選擇切分點(diǎn)，分類(lèi)誤差率的定義為：

選擇分類(lèi)誤差率最低的切分點(diǎn)Ti，得到第m次迭代產(chǎn)生的決策樹(shù)為：

將UWB 測(cè)距數(shù)據(jù)經(jīng)過(guò)基于概率密度提升樹(shù)分類(lèi)后，將鑒別為NLOS 的信號(hào)剔除，并利用鑒別為L(zhǎng)OS的信號(hào)構(gòu)建UWB 定位模型，進(jìn)而用于UWB/IMU 組合定位。

2 基于改進(jìn)SHKF 的UWB/IMU 組合定位算法

UWB/IMU 組合定位時(shí)，SHKF 算法通過(guò)指數(shù)衰減法修正UWB 的噪聲協(xié)方差矩陣，彌補(bǔ)模型估計(jì)結(jié)果與實(shí)際定位結(jié)果的誤差，但這一效果會(huì)隨時(shí)間逐漸減弱。考慮到傳感器受到的噪聲類(lèi)型與幅值多樣且多變，導(dǎo)致SHKF 算法易產(chǎn)生濾波發(fā)散的現(xiàn)象，本文設(shè)計(jì)了一種改進(jìn)的SHKF 算法。為了防止歷史UWB 和IMU 數(shù)據(jù)噪聲對(duì)當(dāng)前新息誤差的修正產(chǎn)生負(fù)面影響，將新息誤差分成兩部分：其一采用指數(shù)衰減法，降低當(dāng)前量測(cè)數(shù)據(jù)對(duì)噪聲協(xié)方差的影響；其二采用變窗口長(zhǎng)度的加權(quán)平均法，解算出漸消因子以增強(qiáng)臨近量測(cè)對(duì)預(yù)測(cè)協(xié)方差矩陣的影響。同時(shí)，設(shè)計(jì)表征新息誤差變化趨勢(shì)的自適應(yīng)因子，實(shí)時(shí)調(diào)整采用指數(shù)衰減法與加權(quán)平均法對(duì)新息誤差調(diào)整的占比，達(dá)到跟隨噪聲變化的目的。

2.1 UWB/IMU 定位模型

UWB 定位模型根據(jù)環(huán)境建立坐標(biāo)系，采用多個(gè)基站與單個(gè)標(biāo)簽得到UWB 標(biāo)簽與基站的絕對(duì)位置信息，稱(chēng)為全局定位[16]；IMU 定位模型根據(jù)自身運(yùn)動(dòng)建立坐標(biāo)系，基于加速度計(jì)與陀螺儀的數(shù)據(jù)得到IMU 相對(duì)于自動(dòng)導(dǎo)航車(chē)（Automated Guided Vehicle,AGV）的位置信息，稱(chēng)為局部定位。

令搭載UWB 與IMU 定位模塊的AGV 面向x方向行駛，其法線方向?yàn)閥方向。設(shè)k時(shí)刻系統(tǒng)的狀態(tài)向量為：

對(duì)于UWB 定位系統(tǒng)，采用雙向雙邊測(cè)距法得到UWB 三個(gè)基站與標(biāo)簽的距離在此基礎(chǔ)上，通過(guò)三邊定位法，求解以UWB 基站為圓心、三邊距離為半徑的定位方程組funUWBp為：

根據(jù)式(14)解算出k時(shí)刻的UWB 定位坐標(biāo)，并依據(jù)k-1 時(shí)刻UWB 的坐標(biāo)信息解算出速度信息，UWB的定位模型f1為：

其中，solve(·) 表示對(duì)函數(shù)求解表示k時(shí)刻UWB 的定位方程dt為相鄰時(shí)刻的時(shí)間間隔。

得到IMU 定位模型f2為：

2.2 改進(jìn)的SHKF 算法設(shè)計(jì)

1）SHKF 算法

將UWB 量測(cè)信息作為量測(cè)更新、IMU 量測(cè)信息作為預(yù)測(cè)更新，根據(jù)UWB 定位模型f1和IMU 定位模型f2，設(shè)k時(shí)刻 UWB 定位建模方程為IMU 定位建模方程為其中，rk與qk分別為k時(shí)刻UWB、IMU 傳感器的噪聲，表達(dá)式為：

其中，σdis為UWB 的定位誤差；σa為加速度計(jì)在x、y方向上偏移的矢量和；σω為偏航角角速度偏移；rk與qk的協(xié)方差分別為

由此可得系統(tǒng)的狀態(tài)方程與觀測(cè)方程分別為：

其中，Zk為k時(shí)刻系統(tǒng)的量測(cè)向量。

卡爾曼濾波器的預(yù)測(cè)更新與量測(cè)更新為：

定義k時(shí)刻的新息誤差與新息誤差協(xié)方差分別為：

由式(25)可知，若能準(zhǔn)確估計(jì)新息協(xié)方差Ck，就能推算出量測(cè)噪聲協(xié)方差Rk。SHKF 算法通過(guò)指數(shù)衰減法估計(jì)當(dāng)前的新息誤差為：

根據(jù)式(25)和式(26)，可得SHKF 算法UWB 的噪聲協(xié)方差矩陣為：

2）漸消因子的解算

為了提升SHKF 算法的穩(wěn)定性，且為了解算方便，將漸消因子λk引入式(20)，通過(guò)λk修改臨近量測(cè)數(shù)據(jù)在位置估計(jì)中的占比，即：

當(dāng)λk=1 時(shí)，將SHKF 算法的新息協(xié)方差與噪聲協(xié)方差分別記為其協(xié)方差記為則有：

其中，tr(·)表示求矩陣的跡。

分別聯(lián)立式(25)、式(28)和式(29)，則λk為:

為了簡(jiǎn)化漸消因子的計(jì)算，令λk為：

通過(guò)實(shí)時(shí)改變窗口長(zhǎng)度，達(dá)到適應(yīng)復(fù)雜環(huán)境下UWB/IMU 傳感器噪聲變化的目的。設(shè)為k時(shí)刻可變窗口長(zhǎng)度，并令其與k-1時(shí)刻的新息成正比，考慮到不能過(guò)短，定義為：

其中，N0為常數(shù)，表示初始窗口長(zhǎng)度；KN代表窗口的比值常數(shù)；表示定位誤差，為k時(shí)刻的定位估計(jì)值與量測(cè)定位值的誤差。

當(dāng)新息誤差大時(shí)，適當(dāng)加寬窗口，削弱當(dāng)前噪聲的影響；當(dāng)新息誤差小時(shí)，適當(dāng)減小窗口，降低SHKF算法的計(jì)算量。

3）自適應(yīng)因子的設(shè)計(jì)

為了同時(shí)保證SHKF 算法的精度與穩(wěn)定性，根據(jù)相鄰時(shí)刻新息誤差變化趨勢(shì)，將噪聲的突變程度劃分為緩慢、中等、急速三種狀態(tài)。根據(jù)不同的狀態(tài)選擇不同的新息誤差修正策略，定義自適應(yīng)因子βk為：

結(jié)合式(25)與式(33)，將新息誤差分成自適應(yīng)的兩部分進(jìn)行補(bǔ)償，即新息協(xié)方差矩陣估計(jì)由組成，改進(jìn)SHKF 算法的新息誤差協(xié)方差矩陣為：

3 實(shí)測(cè)實(shí)驗(yàn)及結(jié)果分析

本文以載有UWB/IMU 傳感器的AGV 為運(yùn)動(dòng)載體，搭建了室內(nèi)環(huán)境下UWB/IMU 定位的實(shí)驗(yàn)平臺(tái)，驗(yàn)證本文所設(shè)計(jì)組合定位方法的可行性和有效性。實(shí)驗(yàn)場(chǎng)地尺寸為10 m × 8 m × 3.5 m，放置三個(gè)UWB 基站對(duì)UWB 標(biāo)簽測(cè)距，分別通過(guò)DW1000、MPU6050 模塊采集UWB、IMU 量測(cè)數(shù)據(jù)，采樣頻率分別為30 HZ與150 HZ。分別經(jīng)過(guò)到達(dá)時(shí)間補(bǔ)償（DW1000）、零偏修正（IMU6050）與滑動(dòng)均值濾波后，IMU 與UWB傳感器參數(shù)如表1 所示。

表1 傳感器參數(shù)Tab.1 Sensor parameters

3.1 NLOS 鑒別實(shí)驗(yàn)對(duì)比

將AGV 放置在距離UWB 基站5 m 處，以無(wú)障礙物遮擋的情況模擬LOS 場(chǎng)景，如圖2 所示。在距離UWB 基站2.5 m 處放置障礙物木板模擬NLOS 場(chǎng)景，如圖3 所示。

圖2 LOS 場(chǎng)景模擬Fig.2 LOS scenario simulation

圖3 NLOS 場(chǎng)景模擬Fig.3 NLOS scenario simulation

分別在上述NLOS 與LOS 場(chǎng)景下各采樣60 s，采集數(shù)據(jù)特征分布如圖4 所示。隨機(jī)選取75%的數(shù)據(jù)作為訓(xùn)練數(shù)據(jù)，剩下25%的數(shù)據(jù)作為測(cè)試數(shù)據(jù)。分別采用提升樹(shù)與本文所設(shè)計(jì)的基于概率密度的提升樹(shù)對(duì)UWB 信號(hào)進(jìn)行鑒別，設(shè)置迭代次數(shù)為50 次，兩種算法的鑒別結(jié)果如圖5 和圖6 所示。

圖4 采集數(shù)據(jù)特征分布圖Fig.4 Collect data feature distribution map

圖5 提升樹(shù)信號(hào)鑒別結(jié)果Fig.5 Result of signal identification by boosting tree

圖5 和圖6 中藍(lán)色圓圈區(qū)域表示特征值交界處。由藍(lán)色圓圈內(nèi)信號(hào)的鑒別結(jié)果可知，提升樹(shù)算法在NLOS 與LOS 數(shù)據(jù)特征值交界處的數(shù)據(jù)難以鑒別，而基于概率密度的提升樹(shù)算法提高了這部分?jǐn)?shù)據(jù)鑒別的準(zhǔn)確率。采用NLOS 信號(hào)鑒別的準(zhǔn)確率PNN、誤識(shí)別率PNL作為性能指標(biāo)，鑒別效果如表2 所示。

表2 NLOS 信號(hào)鑒別效果Tab.2 Performance of NLOS signal identification

3.2 UWB/IMU 組合定位實(shí)驗(yàn)

以UWB 基站1 為原點(diǎn)建立坐標(biāo)系，坐標(biāo)系單位為cm，UWB 基站2 與基站3 的坐標(biāo)分別為(700,0)、(0,500)。在(150,250)處，通過(guò)放置障礙物模擬NLOS環(huán)境，實(shí)驗(yàn)現(xiàn)場(chǎng)實(shí)際環(huán)境如圖7 所示。

圖7 UWB/IMU 組合定位實(shí)驗(yàn)現(xiàn)場(chǎng)Fig.7 Site of UWB/IMU combined positioning experiment

AGV 從(50,50)處出發(fā)，以0.5 m/s 的速度勻速繞類(lèi)矩形路線行駛一周。人為去除15 s～17 s之間的UWB量測(cè)數(shù)據(jù)，模擬兩個(gè)以上UWB 基站的測(cè)距數(shù)據(jù)被鑒別為NLOS 信號(hào)的情況。UWB 與IMU 采集的數(shù)據(jù)獨(dú)立解算后，AGV 的定位軌跡如圖8 所示。

圖8 UWB 與IMU 獨(dú)立的AGV 定位軌跡Fig.8 Independent AGV positioning trajectory of the UWB and IMU

圖8 中，粉色區(qū)域分別表示1 s～7 s、25 s～30 s、33.5 s～35.5 s 的AGV 行駛區(qū)間，在此區(qū)間障礙物擋住了一個(gè)UWB 基站的測(cè)距路線，導(dǎo)致UWB 的定位隨機(jī)誤差增大；藍(lán)色區(qū)域分別表示從15 s 和32 s 開(kāi)始IMU 定位數(shù)據(jù)誤差增大；黃色區(qū)域表示從15 s 至17 s出現(xiàn)UWB 數(shù)據(jù)缺失情況。

采用改進(jìn)的SHKF 算法分別與βk≡ 0時(shí)的算法（SHKF0 算法）以及βk≡ 1時(shí)的算法（SHKF1 算法）對(duì)比。上述算法的遺忘因子b均設(shè)為0.99，改進(jìn)SHKF算法的初始窗口長(zhǎng)度N0為10，比值常數(shù)KN為10，自適應(yīng)因子βk中閾值c0、c1分別為1 和3.5。三種算法的AGV 定位軌跡對(duì)比如圖9 和圖10 所示，對(duì)應(yīng)的實(shí)時(shí)定位誤差結(jié)果如圖11 所示，定位均方根誤差（Root Mean Square Error,RMSE）的統(tǒng)計(jì)結(jié)果見(jiàn)表3。

表3 定位RMSE 對(duì)比Tab.3 Comparison of RMSE of positioning

圖9 改進(jìn)SHKF 與SHKF0 算法的UWB/IMU 組合定位軌跡對(duì)比Fig.9 UWB/IMU combined location trajectory comparison between improved SHKF and SHKF0

圖10 改進(jìn)SHKF 與SHKF1 算法的UWB/IMU 組合定位軌跡對(duì)比Fig.10 UWB/IMU combined location trajectory comparison between improved SHKF and SHKF1

圖11 SHKF0、SHKF1 與改進(jìn)SHKF 算法的實(shí)時(shí)定位誤差Fig.11 Real-time location error of SHKF0,SHKF1 and improved SHKF algorithm

對(duì)應(yīng)圖8中UWB與IMU獨(dú)立的AGV定位軌跡，從圖9 可知，粉色區(qū)域處由于UWB 與IMU 兩個(gè)傳感器的定位誤差同時(shí)增大，通過(guò)SHKF0 算法所估算出AGV 的位置出現(xiàn)了較大偏差，且短時(shí)間內(nèi)無(wú)法修正，而改進(jìn)的SHKF 算法引入了漸消因子，并通過(guò)自適應(yīng)因子提升了漸消因子的作用，因此迅速修正了當(dāng)前噪聲，提高了AGV 的定位精度。從圖11 可知，SHKF1算法引入漸消因子雖然提升了算法的穩(wěn)定性，但自適應(yīng)因子并未對(duì)漸消因子實(shí)時(shí)調(diào)整，所以SHKF1 算法的定位軌跡平滑度較差，定位精度相對(duì)較低。

由表3 可得，與SHKF0 和SHKF1 算法相比，本文算法的x軸定位RMSE 分別提升了5.97%、50.62%，y軸定位RMSE 分別提升了26.49%、39.48%，定位RMSE 分別提升了18.25%、44.58%。

4 總結(jié)

針對(duì)復(fù)雜環(huán)境下UWB 的NLOS 信號(hào)鑒別以及隨機(jī)誤差問(wèn)題，本文提出一種基于改進(jìn)SHKF 算法的UWB/IMU 組合定位方法。基于室內(nèi)環(huán)境下UWB/IMU組合定位的AGV 實(shí)驗(yàn)平臺(tái)開(kāi)展實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果表明：本文算法對(duì)NLOS 信號(hào)鑒別的準(zhǔn)確率為99.12%，定位均方根誤差為4.30 cm。本文的主要結(jié)論如下：

1）將UWB/IMU 特征數(shù)據(jù)的分布密度融入提升樹(shù)的損失函數(shù)中，細(xì)化了特征值的屬性，提升了NLOS信號(hào)鑒別的準(zhǔn)確率。

2）根據(jù)新息誤差的變化趨勢(shì)提出一種具有自適應(yīng)因子的改進(jìn)SHKF 算法，自適應(yīng)調(diào)整漸消因子與權(quán)重系數(shù)對(duì)新息誤差的修正，跟蹤噪聲類(lèi)型與幅值的變化，進(jìn)而提升UWB/IMU 組合定位的精度。