【摘要】為提高無(wú)人車系統(tǒng)中微機(jī)電慣性測(cè)量單元（MEMS IMU）的姿態(tài)角解算精度，提出了一種基于粒子群優(yōu)化（PSO）算法和自適應(yīng)強(qiáng)跟蹤無(wú)跡卡爾曼濾波（STAUKF）算法的數(shù)據(jù)融合方法。首先，對(duì)兩種不同精度的IMU模塊通過(guò)STAUKF算法進(jìn)行濾波，然后，利用構(gòu)造的兩類誤差函數(shù)，引入PSO算法對(duì)兩種IMU的后驗(yàn)估計(jì)進(jìn)行融合，最后，在搭建的無(wú)人車平臺(tái)上進(jìn)行測(cè)試。試驗(yàn)結(jié)果表明，相較于兩種單一IMU解算數(shù)據(jù)，所提出的方法解算獲得的橫滾軸與俯仰軸角度均方根誤差分別減小了56.67%、58.94%，相較于冗余式雙IMU系統(tǒng)直接加權(quán)平均所解算的數(shù)據(jù)分減小了36.55%、52.15%，解算精度更高、魯棒性更強(qiáng)。

主題詞：冗余傳感器 數(shù)據(jù)融合 粒子群優(yōu)化 強(qiáng)跟蹤 卡爾曼濾波

中圖分類號(hào)：TP249" "文獻(xiàn)標(biāo)志碼：A" "DOI： 10.19620/j.cnki.1000-3703.20230744

Attitude Fusion Method of Unmanned Vehicle Dual IMU Based on PSO

【Abstract】In order to improve the attitude angle solving accuracy of Micro-Electro-Mechanical System Inertial Measurement Unit （MEMS IMU） in unmanned vehicle system， this paper proposed a Particle Swarm Optimization （PSO） based algorithm and a Strong Tracking Adaptive Unscented Kalman Filter （STAUKF） data fusion method. Firstly， two kinds of IMU modules with different precision were filtered by STAUKF algorithm. Secondly， two kinds of error functions were constructed and PSO algorithm was introduced to fuse the two kinds of IMU posterior estimation. Finally， the test was carried out on the built unmanned vehicle platform. Experimental results show that， compared with the data solved by two single IMU sensors， the root mean square error of the transverse roller shaft and pitch shaft angle solved by the proposed algorithm is reduced by 56.67% and 58.94%， respectively， and the data solved is reduced by 36.55% and 52.15% respectively compared with direct weighted average of the redundant dual IMU system. Therefore， the algorithm proposed in this paper is more accurate and robust.

Key words： Redundant sensor， Data fusion， Particle Swarm Optimization （PSO）， Strong tracking， Kalman filter

1 前言

無(wú)人車自動(dòng)導(dǎo)航技術(shù)的不斷發(fā)展對(duì)無(wú)人車姿態(tài)解算速度和精度提出了更高的要求[1]。目前，低成本微機(jī)電慣性測(cè)量單元（Micro-Electro-Mechanical System Inertial Measurement Unit，MEMS IMU）傳感器存在精度低和可靠性差等問(wèn)題，國(guó)內(nèi)外學(xué)者對(duì)此進(jìn)行了深入研究。文獻(xiàn)[2]、文獻(xiàn)[3]提出了利用改進(jìn)的擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）和簡(jiǎn)明誤差卡爾曼濾波（Error State Kalman Filter，ESKF）算法提高機(jī)器人定位精度。文獻(xiàn)[4]提出了利用自適應(yīng)強(qiáng)跟蹤誤差狀態(tài)擴(kuò)展卡爾曼濾波（Adaptive Strong Tacking-Error-State Kalman Filter，AST-ESKF）算法對(duì)慣性導(dǎo)航模塊與超寬帶模塊進(jìn)行融合，提高了軌跡估計(jì)的精度。文獻(xiàn)[5]、文獻(xiàn)[6]分別提出了李群擴(kuò)展卡爾曼濾波（Lie Group Extended Kalman Filter，LG-EKF）、無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法，提高了算法的估計(jì)精度。針對(duì)UKF無(wú)法滿足高精度且易受系統(tǒng)模型影響的問(wèn)題，文獻(xiàn)[7]、文獻(xiàn)[8]提出了自適應(yīng)魯棒無(wú)跡卡爾曼濾波（Adaptive Robust Unscented Kalman Filter，ARUKF）算法與改進(jìn)的強(qiáng)跟蹤自適應(yīng)無(wú)跡卡爾曼濾波（Strong Tracking Adaptive Unscented Kalman Filter，STAUKF）算法。文獻(xiàn)[9]提出了一種基于彎道自適應(yīng)強(qiáng)跟蹤卡爾曼濾波（Curve Adaptive Strong Tracking Kalman Filter，CASTKF）算法和側(cè)向加速度傳感器的可拓融合側(cè)向坡度的估計(jì)算法，提高了車輛的側(cè)向坡度估計(jì)精度。針對(duì)MEMS-IMU易受狀態(tài)干擾和量測(cè)噪聲影響的問(wèn)題，文獻(xiàn)[10]提出了模糊魯棒自適應(yīng)容積卡爾曼濾波（Fuzzy Robust Adaptive Cubature Kalman Filter，F(xiàn)RA-CKF）算法進(jìn)行姿態(tài)估計(jì)，并提高了濾波精度。文獻(xiàn)[11]提出了一種基于超寬帶技術(shù)的自適應(yīng)平方根容積卡爾曼濾波（Adaptive Square Root Cubature Kalman Filter，ASRCKF）算法，為無(wú)人車系統(tǒng)提供了更精準(zhǔn)的定位數(shù)據(jù)。針對(duì)輪式車輛里程計(jì)標(biāo)度變化較大的工況，文獻(xiàn)[12]提出利用強(qiáng)跟蹤容積卡爾曼濾波（Strong Tacking Cubature Kalman Filter，STCKF）對(duì)慣性導(dǎo)航模塊和里程計(jì)進(jìn)行信息融合，提高輪式車輛的定位精度。針對(duì)高超目標(biāo)運(yùn)動(dòng)狀態(tài)跟蹤時(shí)精度低和收斂慢的不足，文獻(xiàn)[13]提出自適應(yīng)交互多模型（Adaptive Interacting Multiple-Model，AIMM）與STCKF融合的方法，提高了算法的收斂速度和精度。

當(dāng)無(wú)人車運(yùn)行環(huán)境發(fā)生變化時(shí)，單一傳感器已無(wú)法保證導(dǎo)航系統(tǒng)的魯棒性和可靠性[14-16]。文獻(xiàn)[17]提出了一種基于卡爾曼濾波（Kalman Filter，KF）的兩級(jí)式冗余MEMS IMU信息融合方法，提高了姿態(tài)解算精度。考慮到不同類型傳感器的耦合方式和解算方法不同，文獻(xiàn)[18]、文獻(xiàn)[19]采用不同精度的冗余傳感器進(jìn)行信息融合，提升了系統(tǒng)的容錯(cuò)率和解算精度。文獻(xiàn)[20]、文獻(xiàn)[21]利用反向傳播（Back Propagation，BP）神經(jīng)網(wǎng)絡(luò)和多層感知機(jī)對(duì)冗余IMU陣列進(jìn)行信息融合，提升了MEMS IMU的數(shù)據(jù)解算精度。

在雙IMU傳感器數(shù)據(jù)融合中，還未出現(xiàn)計(jì)算成本低、精度高且信息利用率高的融合算法。為此，本文提出利用粒子群優(yōu)化（Particle Swarm Optimization，PSO）算法對(duì)STAUKF算法的后驗(yàn)估計(jì)及其輸出的后驗(yàn)誤差協(xié)方差進(jìn)行尋優(yōu)，從而對(duì)兩個(gè)IMU單元的數(shù)據(jù)進(jìn)行融合。

2 冗余式雙MEMS IMU系統(tǒng)結(jié)構(gòu)

冗余式雙MEMS IMU系統(tǒng)結(jié)構(gòu)如圖1所示，系統(tǒng)主要由IMU-STAUKF模塊Ⅰ、IMU-STAUKF模塊Ⅱ和PSO信息融合模塊組成。2個(gè)IMU-STAUKF模塊采用精度不同的六軸陀螺儀加速度計(jì)傳感器，分別通過(guò)STAUKF算法進(jìn)行濾波和姿態(tài)解算。數(shù)據(jù)融合模塊分別計(jì)算冗余式雙IMU傳感器的誤差函數(shù)，并利用PSO算法搜索最佳權(quán)重因子，實(shí)現(xiàn)冗余式雙傳感器數(shù)據(jù)融合。

3 自適應(yīng)強(qiáng)跟蹤無(wú)跡卡爾曼濾波算法

3.1 無(wú)跡卡爾曼濾波算法

根據(jù)慣性傳感器解算系統(tǒng)模型構(gòu)建的非線性7維狀態(tài)方程和10維量測(cè)方程為：

式中，XK=[q0，q1，q2，q3，ωX，ωY，ωZ]T為K時(shí)刻的系統(tǒng)狀態(tài)向量；ZK=[q0，q1，q2，q3，ωX，ωY，ωZ，aX，aY，aZ]T為K時(shí)刻的量測(cè)向量；q0、q1、q2、q3為四元數(shù)；ωX、ωY、ωZ分別為X、Y、Z軸角速度；aX、aY、aZ分別為X、Y、Z軸加速度；f（ ）、h（ ）分別為系統(tǒng)狀態(tài)向量、量測(cè)向量的非線性傳播函數(shù)；WK-1～N（0，QK）、VK～N（rK，RK）分別為（K-1）時(shí)刻系統(tǒng)噪聲和K時(shí)刻量測(cè)噪聲；QK為WK-1在K時(shí)刻更新后的方差矩陣；rK、RK分別為K時(shí)刻VK的期望矩陣與方差矩陣。

3.1.1 UKF的狀態(tài)向量更新

UKF算法將先驗(yàn)誤差協(xié)方差矩陣進(jìn)行三角分解，對(duì)狀態(tài)變量進(jìn)行無(wú)跡變換（Unscented Transformation，UT），并通過(guò)f（ ）對(duì)Sigma點(diǎn)進(jìn)行非線性傳播。選取的Sigma點(diǎn)集和Sigma點(diǎn)非線性傳播模型可表示為：

式中，PX，K-1為（K-1）時(shí)刻狀態(tài)變量的后驗(yàn)誤差協(xié)方差矩陣；[χ*i，KK-1]為（K-1）時(shí)刻所計(jì)算的K時(shí)刻非線性傳播后的狀態(tài)變量的第i個(gè)Sigma點(diǎn)；χi，K-1為（K-1）時(shí)刻所選取的第i個(gè)Sigma點(diǎn)。

狀態(tài)預(yù)測(cè)值及均方差矩陣可以表示為：

3.1.2 UKF的量測(cè)向量更新

式中，ηi，K|K-1為量測(cè)向量的Sigma點(diǎn)集；h（ ）表示Sigma點(diǎn)的非線性傳播。

量測(cè)預(yù)測(cè)值、均方差矩陣及協(xié)方差矩陣可表示為：

因而，卡爾曼增益、后驗(yàn)估計(jì)值和先驗(yàn)誤差協(xié)方差矩陣可表示為：

3.2 自適應(yīng)強(qiáng)跟蹤濾波

強(qiáng)跟蹤濾波源于量測(cè)故障的檢測(cè)，可通過(guò)引入漸消因子λK對(duì)先驗(yàn)誤差協(xié)方差矩陣PX，K|K-1進(jìn)行修正，并實(shí)時(shí)調(diào)整卡爾曼增益。利用式（5）及當(dāng)前傳感器量測(cè)值ZK計(jì)算的新息陣εK及其方差陣ΓK可表示為：

式中，βK為指數(shù)漸消因子，且β0=1，β∞=1-b；b為漸消因子，通常取0.95～0.99。

由于量測(cè)噪聲矩陣RK在非線性系統(tǒng)中不易獲取，通過(guò)在STUKF算法中引入Sage-Husa自適應(yīng)方法對(duì)其進(jìn)行次優(yōu)估計(jì)，則量測(cè)噪聲可表示為：

由于強(qiáng)跟蹤濾波基于線性系統(tǒng)推導(dǎo)，將強(qiáng)跟蹤濾波引入U(xiǎn)KF算法[22]中，則單重漸消因子λK可表示為：

綜上，在雙冗余MEMS IMU數(shù)據(jù)解算系統(tǒng)中，UKF算法的量測(cè)噪聲不易建模，且估計(jì)精度低，可利用以下方法解決此問(wèn)題：

a. 對(duì)量測(cè)系統(tǒng)的噪聲進(jìn)行實(shí)時(shí)估計(jì)，引入漸消Sage-Husa自適應(yīng)，在濾波的同時(shí)對(duì)量測(cè)噪聲進(jìn)行估計(jì)；

b. 為提高濾波精度及對(duì)量測(cè)噪聲的識(shí)別精度，引入多重漸消因子，修正UKF中的先驗(yàn)誤差協(xié)方差矩陣。

4 基于粒子群優(yōu)化的姿態(tài)數(shù)據(jù)融合

4.1 姿態(tài)角后驗(yàn)估計(jì)融合

由于單一IMU傳感器的精度一定，可以通過(guò)增加傳感器的數(shù)量來(lái)提高量測(cè)系統(tǒng)的魯棒性和數(shù)據(jù)解算精度。而精度的提升即意味著輸出數(shù)據(jù)與真值間的均方差減小，故可利用求取均方差的極小值實(shí)現(xiàn)姿態(tài)的最優(yōu)估計(jì)。

4.1.1 基于誤差補(bǔ)償?shù)暮篁?yàn)估計(jì)方法

2個(gè)IMU-STAUKF模塊的量測(cè)相互獨(dú)立時(shí)，則后驗(yàn)估計(jì)可表示為：

4.1.2 基于信息融合后驗(yàn)估計(jì)方法

當(dāng)模塊Ⅰ與模塊Ⅱ的量測(cè)相互獨(dú)立時(shí)，依據(jù)信息融合的方法可得[23]：

式中，K1、K2為權(quán)重因子。

4.2 權(quán)重因子計(jì)算

粒子群優(yōu)化算法中提出以下假設(shè)：

a. 每個(gè)粒子只擁有自身速度和位置這2種屬性；

b. 每個(gè)粒子在函數(shù)解中運(yùn)動(dòng)，只通過(guò)速度來(lái)改變方向和位置；

c. 每個(gè)粒子會(huì)跟蹤最優(yōu)粒子移動(dòng)，以通過(guò)較少的代數(shù)尋求最優(yōu)解。

值。每個(gè)粒子都會(huì)跟蹤個(gè)體歷史最優(yōu)位置和種群歷史最優(yōu)位置，其算法可表示為：

[xij（t+1）=xij（t）+vij（t+1）]" " " " " " " " （19）

[vij（t+1）=ωvij（t）+c1r1（t）pij（t）-xij（t）+c2r2（t）pgj（t）-xij（t）]（20）

式中，xij（t）為第t代（上一代）第j個(gè)維度的第i個(gè)粒子的位置；vij（t+1）為當(dāng)前代粒子移動(dòng)的速度；pij（t）為第t代粒子的個(gè)體極值（局部最優(yōu)）；pgj（t）為第t代粒子的全局極值（全局最優(yōu)）；ω=0.8為慣性權(quán)重；c1、c2為學(xué)習(xí)因子，取值均為0.5；r1（t）、r2（t）為（0，1）范圍內(nèi)的隨機(jī)數(shù)。

式（20）右側(cè)第1項(xiàng)為所保留的當(dāng)前粒子個(gè)體的運(yùn)動(dòng)慣性信息，第2項(xiàng)為當(dāng)前粒子的飛行經(jīng)驗(yàn)對(duì)其當(dāng)前速度的修正，第3項(xiàng)為種群所有粒子飛行經(jīng)驗(yàn)對(duì)當(dāng)前粒子速度的修正。

粒子群優(yōu)化算法具體步驟如下：

a. 設(shè)定粒子搜索的空間邊界、粒子飛行速度的上下限、最大迭代次數(shù)及c1、c2、ω，初始化粒子速度與位置；

b. 計(jì)算粒子的適應(yīng)度并保存?zhèn)€體和種群的歷史最優(yōu)位置；

c. 由式（20）更新每個(gè)粒子的速度與位置；

d. 重復(fù)步驟b并更新個(gè)體粒子的歷史最優(yōu)位置；

e. 對(duì)比每個(gè)粒子與種群的最優(yōu)位置和最佳適應(yīng)度，并對(duì)種群最優(yōu)位置和最佳適應(yīng)度進(jìn)行更新；

f. 判斷所得出極值是否滿足最大迭代次數(shù)或達(dá)到輸出精度要求，若滿足則輸出最佳權(quán)重因子，否則重復(fù)步驟c～步驟f。

PSO計(jì)算完成后，將權(quán)重因子分別代入式（15）和式（17），即可得到融合后誤差較小的姿態(tài)角數(shù)據(jù)。

綜上，基于冗余式雙IMU系統(tǒng)，首先利用STAUKF算法進(jìn)行數(shù)據(jù)濾波和姿態(tài)解算并提出以下方法進(jìn)行后驗(yàn)姿態(tài)融合：

a. 基于信息融合后驗(yàn)估計(jì)方法，依據(jù)PSO算法對(duì)兩種精度不同的傳感器信息進(jìn)行姿態(tài)融合，獲得系統(tǒng)的最優(yōu)估計(jì)；

b. 基于誤差補(bǔ)償后驗(yàn)估計(jì)方法，依據(jù)PSO算法對(duì)兩種精度不同的傳感器信息進(jìn)行姿態(tài)融合，獲得系統(tǒng)的最優(yōu)估計(jì)。

5 試驗(yàn)對(duì)比及數(shù)據(jù)分析

5.1 試驗(yàn)平臺(tái)

為驗(yàn)證所提出算法的正確性，利用STM32H743微控制器、六軸陀螺儀加速度計(jì)ICM20602及ICM20689搭建雙余度系統(tǒng)測(cè)試平臺(tái)，用于搭載冗余式雙IMU系統(tǒng)的無(wú)人車如圖2所示，車載硬件如表1所示。

其中，六軸陀螺加速度計(jì)ICM20602和ICM20689的參數(shù)如表2、表3所示。

5.2 算法對(duì)比

5.2.1 基于靜態(tài)測(cè)量時(shí)的數(shù)據(jù)對(duì)比

將搭載有冗余式雙IMU系統(tǒng)的無(wú)人車靜止于某一平面時(shí)，對(duì)單一傳感器ICM20602和ICM20689使用STAUKF算法解算，并利用數(shù)據(jù)平滑后加權(quán)平均的方式進(jìn)行數(shù)據(jù)融合。再利用3.1節(jié)中的兩種融合方法、PSO以及STAUKF進(jìn)行聯(lián)合解算。其中，橫滾軸、俯仰軸分別采樣2 000個(gè)采樣點(diǎn)。以多算法線程在同一組點(diǎn)處多次采樣后的均值作為基準(zhǔn)計(jì)算數(shù)據(jù)的均方根誤差，評(píng)判輸出數(shù)據(jù)的精度?；赟TAUKF算法及數(shù)據(jù)融合算法的橫滾軸的姿態(tài)角和角度誤差如圖3所示，俯仰軸的姿態(tài)角和角度誤差如圖4所示。

試驗(yàn)算法的均方根誤差如表4所示，兩種后驗(yàn)估計(jì)融合算法所得到的姿態(tài)角均方根誤差最小?；谡`差補(bǔ)償?shù)暮篁?yàn)估計(jì)融合算法以及基于信息融合的后驗(yàn)估計(jì)融合算法所解算的橫滾軸與俯仰軸角度均方根誤差，相較于兩種單一傳感器數(shù)據(jù)解算分別平均減小了79.92%、82.09%，相較于冗余式雙IMU系統(tǒng)直接加權(quán)平均所解算的數(shù)據(jù)分別平均減小了68.29%、50.00%。

由于此時(shí)的角度基準(zhǔn)為均值基準(zhǔn)，故引入數(shù)據(jù)方差再次評(píng)判數(shù)據(jù)精度，各算法所解算數(shù)據(jù)方差如表5所示。

由表5可以看出，無(wú)論是橫滾軸或俯仰軸，在靜態(tài)時(shí)各算法輸出數(shù)據(jù)中基于信息融合后驗(yàn)估計(jì)方法方差最小，基于誤差補(bǔ)償后驗(yàn)估計(jì)方法次之，直接對(duì)兩個(gè)模塊加權(quán)平均后所得數(shù)據(jù)方差較大，單一傳感器利用STAUKF直接濾波輸出后的方差最大。綜上，由表4、表5綜合評(píng)判可知：在靜態(tài)測(cè)量時(shí)冗余式雙IMU系統(tǒng)測(cè)量數(shù)據(jù)誤差小于單一IMU傳感器測(cè)量數(shù)據(jù)的誤差，應(yīng)用PSO與STAUKF聯(lián)合解算的基于誤差補(bǔ)償?shù)暮篁?yàn)估計(jì)方法、基于信息融合的后驗(yàn)估計(jì)方法的測(cè)量數(shù)據(jù)誤差小于直接對(duì)雙IMU模塊進(jìn)行加權(quán)平均后所得到的數(shù)據(jù)，表明在靜態(tài)正常測(cè)量時(shí)本文方法更具優(yōu)勢(shì)。

5.2.2 基于無(wú)人車運(yùn)動(dòng)時(shí)的測(cè)量數(shù)據(jù)對(duì)比

在搭載有冗余式雙IMU系統(tǒng)的無(wú)人車平面運(yùn)動(dòng)時(shí)，對(duì)單一傳感器ICM20602和ICM20689使用STAUKF算法解算，并利用數(shù)據(jù)平滑后加權(quán)平均的方式進(jìn)行數(shù)據(jù)融合。再利用3.1節(jié)中的兩種融合方法、PSO以及STAUKF進(jìn)行聯(lián)合解算。其中，橫滾軸、俯仰軸分別采樣2 000個(gè)采樣點(diǎn)。以多算法線程在同一組點(diǎn)處多次采樣后的均值作為基準(zhǔn)計(jì)算數(shù)據(jù)的均方根誤差，評(píng)判輸出數(shù)據(jù)的精度?；赟TAUKF算法及數(shù)據(jù)融合算法的橫滾軸的姿態(tài)角和角度誤差如圖5所示，俯仰軸的姿態(tài)角和角度誤差如圖6所示。

由圖5、圖6可以看出，動(dòng)態(tài)條件下數(shù)據(jù)離散程度較大，在某些點(diǎn)處可能會(huì)出現(xiàn)“野值”，但從圖中可以直觀地看到，本文所提出的方法很好地抑制了“野值”的出現(xiàn)，即便在較為復(fù)雜的運(yùn)動(dòng)環(huán)境中也可保持精度較高的姿態(tài)數(shù)據(jù)輸出。

試驗(yàn)算法的均方根誤差如表6所示，通過(guò)兩種后驗(yàn)估計(jì)融合算法所得到的角度均方根誤差最小。相較于兩種單一IMU傳感器解算數(shù)據(jù)，基于誤差補(bǔ)償?shù)暮篁?yàn)估計(jì)融合算法以及基于信息融合的后驗(yàn)估計(jì)融合算法所解算的橫滾軸與俯仰軸角度均方根誤差分別平均減小了56.67%、58.94%，相較于冗余式雙IMU系統(tǒng)直接加權(quán)平均所解算的數(shù)據(jù)分別平均減小了36.55%、52.15%。

由于此時(shí)的角度基準(zhǔn)為均值基準(zhǔn)，故引入數(shù)據(jù)方差再次評(píng)判數(shù)據(jù)精度，各算法所解算數(shù)據(jù)方差如表7所示。

由表7可以看出，在無(wú)人車正常行駛狀態(tài)下，各算法輸出橫滾軸與俯仰軸數(shù)據(jù)中基于信息融合后驗(yàn)估計(jì)方法與基于誤差補(bǔ)償后驗(yàn)估計(jì)方法的方差最小，直接對(duì)兩個(gè)模塊加權(quán)平均后所得數(shù)據(jù)方差較大，單一傳感器利用STAUKF直接濾波輸出后的方差最大。綜上，由表6與表7綜合評(píng)判可知：在無(wú)人車正常行駛時(shí)，冗余式雙IMU系統(tǒng)測(cè)量數(shù)據(jù)誤差小于單一IMU傳感器測(cè)量數(shù)據(jù)的誤差；應(yīng)用PSO與STAUKF聯(lián)合解算的基于誤差補(bǔ)償后驗(yàn)估計(jì)方法、基于信息融合后驗(yàn)估計(jì)方法的測(cè)量數(shù)據(jù)誤差小于直接對(duì)雙IMU模塊進(jìn)行加權(quán)平均后所得到的數(shù)據(jù)誤差。試驗(yàn)數(shù)據(jù)說(shuō)明冗余式雙IMU系統(tǒng)滿足輔助無(wú)人車建圖導(dǎo)航需求，本文方法提高了雙IMU系統(tǒng)的解算精度，并充分利用雙IMU數(shù)據(jù)輸出，增加了系統(tǒng)的魯棒性。

5.2.3 無(wú)人車二維建圖數(shù)據(jù)對(duì)比

利用無(wú)人車搭載冗余式雙IMU系統(tǒng)并以Karto算法在實(shí)驗(yàn)室走廊進(jìn)行二維建圖測(cè)試，試驗(yàn)場(chǎng)景如圖7所示，無(wú)人車建圖測(cè)試結(jié)果對(duì)比如圖8所示。

在路面不平整的情況下，采用冗余式雙IMU系統(tǒng)對(duì)激光雷達(dá)所掃描的數(shù)據(jù)進(jìn)行校正。由圖8對(duì)比可知：在建圖算法中引入IMU數(shù)據(jù)后所建地圖的邊界更為清晰；由冗余式雙IMU系統(tǒng)應(yīng)用信息融合后驗(yàn)估計(jì)方法處理后的數(shù)據(jù)所建地圖相較直接將其加權(quán)平均后的地圖邊界更為平滑，少見(jiàn)重影與偏轉(zhuǎn)；應(yīng)用誤差補(bǔ)償后驗(yàn)估計(jì)方法處理后的數(shù)據(jù)所建地圖效果相較于直接加權(quán)平均的方法優(yōu)勢(shì)明顯，但還存在少量重影。試驗(yàn)結(jié)果證實(shí)了所提出的兩種融合方法滿足無(wú)人車建圖需要，且在實(shí)際場(chǎng)景應(yīng)用中基于信息融合后驗(yàn)估計(jì)方法更具優(yōu)勢(shì)。

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

本文利用PSO算法對(duì)STAUKF算法所解算出的角度以誤差尋優(yōu)的方式進(jìn)行融合，從而提升冗余式雙IMU系統(tǒng)的數(shù)據(jù)精度與利用率，并通過(guò)無(wú)人車硬件平臺(tái)對(duì)所提出的算法進(jìn)行建圖試驗(yàn)。通過(guò)多種算法的比較，可以看出本文所提出的融合方法較加權(quán)平均或單一傳感器更具優(yōu)勢(shì)，且通過(guò)實(shí)測(cè)建圖，可以證實(shí)所提出的方法滿足低成本冗余MEMS IMU系統(tǒng)在無(wú)人車定位與導(dǎo)航系統(tǒng)中應(yīng)用，并且兩種方法精度差距不大，都提高了系統(tǒng)信息利用率以及系統(tǒng)魯棒性，但在實(shí)測(cè)中建議使用基于信息融合后驗(yàn)估計(jì)方法，其精度最高。

參 考 文 獻(xiàn)

[1] 李娜， 賀海育， 賈偉， 等. 基于2D建圖算法的導(dǎo)航方法[J]. 無(wú)線電工程， 2022， 52（4）： 612-616.

LI N， HE H Y， JIA W， et al. Navigation Method Based on 2D Mapping Algorithm[J]. Radio Engineering， 2022， 52（4）： 612-616.

[2] 仇海濤， 陳曉東， 張峰. 基于改進(jìn)ESKF濾波和自適應(yīng)濾波的組合導(dǎo)航算法研究[J]. 電光與控制， 2023， 30（5）： 1-5.

QIU H T， CHEN X D， ZHANG F. Research on Integrated Navigation Algorithm Based on Improved ESKF Filter and Adaptive Filter[J]. Electronics Optics and Control， 2023， 30（5）： 1-6.

[3] 周萌萌， 張冰， 趙強(qiáng)， 等. 基于自適應(yīng)漸消Sage-Husa擴(kuò)展卡爾曼濾波的協(xié)同定位算法[J]. 中國(guó)艦船研究， 2022， 17（4）： 92-97.

ZHOU M M， ZHANG B， ZHAO Q， et al. Cooperative" " " " "Localization Algorithm Based on Adaptive Fading" "Sage-Husa Extended Kalman Filter[J]. China Ship Research， 2022， 17（4）： 92-97.

[4] 張?chǎng)?吳飛， 朱海. 自適應(yīng)強(qiáng)跟蹤AST-ESKF無(wú)人車室內(nèi)導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào)， 2022， 10（4）： 34-42.

ZHANG W T， WU F， ZHU H. Indoor Navigation Algorithm for Unmanned Vehicle Based on Adaptive Strong Tracking AST-ESKF[J]. Journal of Navigation and Positioning， 2022， 10（4）： 34-42.

[5] CUI J R， WANG M S， WU W Q， et al. Lie Group Based" " Nonlinear State Errors for MEMS-IMU/GNSS/Magnetometer Integrated Navigation[J]. Journal of Navigation， 2021， 74（4）： 887-900.

[6] JULIER S， UHLMANN J， DURRANT-WHYTE H F. A New Method for the Nonlinear Transformation of Means and" " " Covariances in Filters and Estimators[J]. IEEE Transactions on Automatic Control， 2000， 45（3）： 477-482.

[7] 李明， 柴洪洲， 靳凱迪， 等. 改進(jìn)的強(qiáng)跟蹤自適應(yīng)UKF算法及其在大方位失準(zhǔn)角對(duì)準(zhǔn)中的應(yīng)用[J]. 導(dǎo)航定位學(xué)報(bào)， 2022， 10（6）： 165-172.

LI M， CHAI H Z， JIN K D， et al. Improved Strong Tracking Adaptive UKF Algorithm and Its Application in Large" " " " Azimuth Misalignment[J]. Journal of Navigation and" " " " " Positioning， 2022， 10（6）： 165-172.

[8] 許萬(wàn)， 程兆， 夏瑞東， 等. 一種基于動(dòng)態(tài)殘差的自適應(yīng)魯棒無(wú)跡卡爾曼濾波器定位算法[J]. 中國(guó)機(jī)械工程， 2023， 34（21）： 2607-2614.

XU W， CHENG Z， XIA R D， et al. An Adaptive Robust" " Unscented Kalman Filter Localization Algorithm Based on Dynamic Residual[J]. China Mechanical Engineering， 2023， 34（21）： 2607-2614.

[9] 劉軼材， 范志先， 王翔宇， 等. 基于彎道自適應(yīng)強(qiáng)跟蹤卡爾曼濾波的側(cè)向坡度估計(jì)算法[J]. 同濟(jì)大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2021， 49（增刊1）： 148-154.

LIU Y C， FAN Z X， WANG X Y， et al. Road Bank" " " " " " "Estimation Based on Curve Adaptive Strong Tracking" " " " Kalman Filter[J]. Journal of Tongji University （Natural" " " Science Edition）， 2021， 49（S1）： 148-154.

[10] 喬美英， 高翼飛， 李宛妮， 等. 基于模糊魯棒自適應(yīng)CKF算法的MEMS-IMU姿態(tài)估計(jì)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2022， 30（3）： 296-303.

QIAO M Y， GAO Y F， LI W N， et al. Attitude Estimation of MEMS-IMU Based on Fuzzy Robust Adaptive CKF" " " " "Algorithm[J]. Chinese Journal of Inertial Technology， 2022， 30（3）： 296-303.

[11] 趙宏強(qiáng)， 鄧文斌， 辛濤， 等. 基于UWB和KF的無(wú)人車目標(biāo)跟蹤方法[J]. 傳感器與微系統(tǒng)， 2022， 41（10）： 34-38.

ZHAO H Q， DENG W B， XIN T， et al. Target Tracking Method for Unmanned Ground Vehicle Based on UWB and KF[J]. Sensors and Microsystems， 2022， 41（10）： 34-38.

[12] 李士心， 黃鳳榮， 邱時(shí)前， 等. 基于自適應(yīng)強(qiáng)跟蹤濾波的捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2018， 26（2）： 156-161.

LI S X， HUANG F R， QIU S Q， et al. SINS/Odometer" " " "Integrated Navigation Method Based on Adaptive Strong Tracking Filter[J]. Chinese Journal of Inertial Technology， 2018， 26（2）： 156-161.

[13] 羅亞倫， 廖育榮， 李兆銘， 等. 高超目標(biāo)強(qiáng)跟蹤C(jī)KF自適應(yīng)交互多模型跟蹤算法[J]. 北京航空航天大學(xué)學(xué)報(bào)（2022-09-28）[2023-10-24]. https：//doi.org/10.13700/j.bh.1001-5965.2022.0587.

LUO Y L， LIAO Y R， LI Z M， et al. Strong Tracking CKF Adaptive Interacting Multiple-Model Algorithm Based on Maneuvering Hypersonic-Target Tracking[J]. Journal of Beijing University of Aeronautics and Astronautics（2022-09-28）[2023-10-24]. https：//doi.org/10.13700/j.bh.1001-5965.2022.0587.

[14] 蔡英鳳， 陸子恒， 李祎承， 等. 基于多傳感器融合的緊耦合SLAM系統(tǒng)[J]. 汽車工程， 2022， 44（3）： 350-361.

CAI Y F， LU Z H， LI Y C， et al. Tightly Coupled SLAM" "System Based on Multi-Sensor Fusion[J]. Automotive" " " " Engineering， 2022， 44（3）： 350-361.

[15] 許智賓， 李宏偉， 張斌， 等. 雙目視覺(jué)與慣導(dǎo)融合的移動(dòng)機(jī)器人定位方法[J]. 測(cè)繪學(xué)報(bào)， 2021， 50（11）： 1512-1521.

XU Z B， LI H W， ZHANG B， et al. Localization Method of Mobile Robot Based on Binocular Vision and Inertial" " " " Navigation[J]. Journal of Surveying and Mapping， 2021， 50（11）： 1512-1521.

[16] 許萬(wàn)， 朱力， 張宇豪， 等. 基于自適應(yīng)UKF的移動(dòng)機(jī)器人室內(nèi)定位算法研究[J]. 傳感器與微系統(tǒng)， 2022， 41（11）： 44-47.

XU W， ZHU L， ZHANG Y H， et al. Research on Indoor" " Localization Algorithm for Mobile Robot Based on" " " " " " Adaptive UKF[J]. Sensors and Microsystems， 2022， 41（11）： 44-47.

[17] XUE L， YANG B， YANG X G， et al. A Redundant Fused MIMU Attitude System Algorithm Based on Two-Stage" " " "Data Fusion of MEMS Gyro Clusters Array[J]." " " " " " " " "Measurement， 2021， 184.

[18] WANG L， WU W Q， WEI G， et al. Navigation Information Fusion in a Redundant Marine Rotational Inertial" " " " " " "Navigation System Configuration[J]. Journal of Navigation， 2018， 71（6）： 1531-1552.

[19] XING L， TU X W， QIAN W X， et al. Performance Enhancement Method for Angular Rate Measurement Based on" " Redundant MEMS IMUs[J]. Micromachines， 2019， 10（8）： 514.

[20] 喬文超， 王紅雨， 王鴻東. 基于BP神經(jīng)網(wǎng)絡(luò)的無(wú)人機(jī)IMU多傳感器冗余的補(bǔ)償算法[J]. 電子測(cè)量與儀器學(xué)報(bào)， 2020， 34（12）： 19-28.

QIAO W C， WANG H Y， WANG H D. Compensation" " " "Algorithm for UAV IMU Multi-Sensor Redundancy Based on BP Neural Network[J]. Journal of Electronic Measurement and Instrumentation， 2020， 34（12）： 19-28.

[21] 廖亞楨， 劉昱， 張立強(qiáng)， 等. 面向PDR定位系統(tǒng)的多層感知機(jī)IMU陣列數(shù)據(jù)融合方法[J]. 電子測(cè)量與儀器學(xué)報(bào)， 2022， 36（8）： 35-42.

LIAO Y Z， LIU Y， ZHANG L Q， et al. Multi-Layer Sensor IMU Array Data Fusion Method for PDR Positioning System[J]. Journal of Electronic Measurement and" " " " " " " " " " " " Instrumentation， 2022， 36（8）： 35-42.

[22] 蔣凱， 周建江， 呂瑞廣， 等. 基于IMM-JPDA-ISTUKF的車載毫米波雷達(dá)多目標(biāo)跟蹤算法[J]. 現(xiàn)代雷達(dá)（2022-10-14）[2023-10-24]. http：//kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html.

JIANG K， ZHOU J J， Lü R G， et al. Multi-Target Tracking Algorithm of Vehicle-Mounted Millimeter-Wave Radar Based on IMM-JPDA-ISTUKF[J]. Modern Radar（2022-10-14）[2023-10-24]. http：//kns.cnki.net/kcms/detail/32.1353.TN.20221014.1057.002.html.

[23] 彥恭敏， 翁浚. 捷聯(lián)慣導(dǎo)算法于組合導(dǎo)航原理[M]. 西安： 西北工業(yè)大學(xué)出版社， 2019.

YAN G M， WENG J. Strapdown Inertial Navigation" " " " " Algorithm and Integrated Navigation Principle[M]. Xi’an： Northwestern Polytechnical University Press， 2019.