基于粒子群優化的無人車雙慣性測量單元姿態融合方法

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

主題詞：冗余傳感器 數據融合 粒子群優化 強跟蹤 卡爾曼濾波

中圖分類號：TP249" "文獻標志碼：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 前言

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

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

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

2 冗余式雙MEMS IMU系統結構

冗余式雙MEMS IMU系統結構如圖1所示，系統主要由IMU-STAUKF模塊Ⅰ、IMU-STAUKF模塊Ⅱ和PSO信息融合模塊組成。2個IMU-STAUKF模塊采用精度不同的六軸陀螺儀加速度計傳感器，分別通過STAUKF算法進行濾波和姿態解算。數據融合模塊分別計算冗余式雙IMU傳感器的誤差函數，并利用PSO算法搜索最佳權重因子，實現冗余式雙傳感器數據融合。

3 自適應強跟蹤無跡卡爾曼濾波算法

3.1 無跡卡爾曼濾波算法

根據慣性傳感器解算系統模型構建的非線性7維狀態方程和10維量測方程為：

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

3.1.1 UKF的狀態向量更新

UKF算法將先驗誤差協方差矩陣進行三角分解，對狀態變量進行無跡變換（Unscented Transformation，UT），并通過f（ ）對Sigma點進行非線性傳播。選取的Sigma點集和Sigma點非線性傳播模型可表示為：

式中，PX，K-1為（K-1）時刻狀態變量的后驗誤差協方差矩陣；[χ*i，KK-1]為（K-1）時刻所計算的K時刻非線性傳播后的狀態變量的第i個Sigma點；χi，K-1為（K-1）時刻所選取的第i個Sigma點。

狀態預測值及均方差矩陣可以表示為：

3.1.2 UKF的量測向量更新

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

量測預測值、均方差矩陣及協方差矩陣可表示為：

因而，卡爾曼增益、后驗估計值和先驗誤差協方差矩陣可表示為：

3.2 自適應強跟蹤濾波

強跟蹤濾波源于量測故障的檢測，可通過引入漸消因子λK對先驗誤差協方差矩陣PX，K|K-1進行修正，并實時調整卡爾曼增益。利用式（5）及當前傳感器量測值ZK計算的新息陣εK及其方差陣ΓK可表示為：

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

由于量測噪聲矩陣RK在非線性系統中不易獲取，通過在STUKF算法中引入Sage-Husa自適應方法對其進行次優估計，則量測噪聲可表示為：

由于強跟蹤濾波基于線性系統推導，將強跟蹤濾波引入UKF算法[22]中，則單重漸消因子λK可表示為：

綜上，在雙冗余MEMS IMU數據解算系統中，UKF算法的量測噪聲不易建模，且估計精度低，可利用以下方法解決此問題：

a. 對量測系統的噪聲進行實時估計，引入漸消Sage-Husa自適應，在濾波的同時對量測噪聲進行估計；

b. 為提高濾波精度及對量測噪聲的識別精度，引入多重漸消因子，修正UKF中的先驗誤差協方差矩陣。

4 基于粒子群優化的姿態數據融合

4.1 姿態角后驗估計融合

由于單一IMU傳感器的精度一定，可以通過增加傳感器的數量來提高量測系統的魯棒性和數據解算精度。而精度的提升即意味著輸出數據與真值間的均方差減小，故可利用求取均方差的極小值實現姿態的最優估計。

4.1.1 基于誤差補償的后驗估計方法

2個IMU-STAUKF模塊的量測相互獨立時，則后驗估計可表示為：

4.1.2 基于信息融合后驗估計方法

當模塊Ⅰ與模塊Ⅱ的量測相互獨立時，依據信息融合的方法可得[23]：

式中，K1、K2為權重因子。

4.2 權重因子計算

粒子群優化算法中提出以下假設：

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

b. 每個粒子在函數解中運動，只通過速度來改變方向和位置；

c. 每個粒子會跟蹤最優粒子移動，以通過較少的代數尋求最優解。

值。每個粒子都會跟蹤個體歷史最優位置和種群歷史最優位置，其算法可表示為：

[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個維度的第i個粒子的位置；vij（t+1）為當前代粒子移動的速度；pij（t）為第t代粒子的個體極值（局部最優）；pgj（t）為第t代粒子的全局極值（全局最優）；ω=0.8為慣性權重；c1、c2為學習因子，取值均為0.5；r1（t）、r2（t）為（0，1）范圍內的隨機數。

式（20）右側第1項為所保留的當前粒子個體的運動慣性信息，第2項為當前粒子的飛行經驗對其當前速度的修正，第3項為種群所有粒子飛行經驗對當前粒子速度的修正。

粒子群優化算法具體步驟如下：

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

b. 計算粒子的適應度并保存個體和種群的歷史最優位置；

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

d. 重復步驟b并更新個體粒子的歷史最優位置；

e. 對比每個粒子與種群的最優位置和最佳適應度，并對種群最優位置和最佳適應度進行更新；

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

PSO計算完成后，將權重因子分別代入式（15）和式（17），即可得到融合后誤差較小的姿態角數據。

綜上，基于冗余式雙IMU系統，首先利用STAUKF算法進行數據濾波和姿態解算并提出以下方法進行后驗姿態融合：

a. 基于信息融合后驗估計方法，依據PSO算法對兩種精度不同的傳感器信息進行姿態融合，獲得系統的最優估計；

b. 基于誤差補償后驗估計方法，依據PSO算法對兩種精度不同的傳感器信息進行姿態融合，獲得系統的最優估計。

5 試驗對比及數據分析

5.1 試驗平臺

為驗證所提出算法的正確性，利用STM32H743微控制器、六軸陀螺儀加速度計ICM20602及ICM20689搭建雙余度系統測試平臺，用于搭載冗余式雙IMU系統的無人車如圖2所示，車載硬件如表1所示。

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

5.2 算法對比

5.2.1 基于靜態測量時的數據對比

將搭載有冗余式雙IMU系統的無人車靜止于某一平面時，對單一傳感器ICM20602和ICM20689使用STAUKF算法解算，并利用數據平滑后加權平均的方式進行數據融合。再利用3.1節中的兩種融合方法、PSO以及STAUKF進行聯合解算。其中，橫滾軸、俯仰軸分別采樣2 000個采樣點。以多算法線程在同一組點處多次采樣后的均值作為基準計算數據的均方根誤差，評判輸出數據的精度。基于STAUKF算法及數據融合算法的橫滾軸的姿態角和角度誤差如圖3所示，俯仰軸的姿態角和角度誤差如圖4所示。

試驗算法的均方根誤差如表4所示，兩種后驗估計融合算法所得到的姿態角均方根誤差最小。基于誤差補償的后驗估計融合算法以及基于信息融合的后驗估計融合算法所解算的橫滾軸與俯仰軸角度均方根誤差，相較于兩種單一傳感器數據解算分別平均減小了79.92%、82.09%，相較于冗余式雙IMU系統直接加權平均所解算的數據分別平均減小了68.29%、50.00%。

由于此時的角度基準為均值基準，故引入數據方差再次評判數據精度，各算法所解算數據方差如表5所示。

由表5可以看出，無論是橫滾軸或俯仰軸，在靜態時各算法輸出數據中基于信息融合后驗估計方法方差最小，基于誤差補償后驗估計方法次之，直接對兩個模塊加權平均后所得數據方差較大，單一傳感器利用STAUKF直接濾波輸出后的方差最大。綜上，由表4、表5綜合評判可知：在靜態測量時冗余式雙IMU系統測量數據誤差小于單一IMU傳感器測量數據的誤差，應用PSO與STAUKF聯合解算的基于誤差補償的后驗估計方法、基于信息融合的后驗估計方法的測量數據誤差小于直接對雙IMU模塊進行加權平均后所得到的數據，表明在靜態正常測量時本文方法更具優勢。

5.2.2 基于無人車運動時的測量數據對比

在搭載有冗余式雙IMU系統的無人車平面運動時，對單一傳感器ICM20602和ICM20689使用STAUKF算法解算，并利用數據平滑后加權平均的方式進行數據融合。再利用3.1節中的兩種融合方法、PSO以及STAUKF進行聯合解算。其中，橫滾軸、俯仰軸分別采樣2 000個采樣點。以多算法線程在同一組點處多次采樣后的均值作為基準計算數據的均方根誤差，評判輸出數據的精度。基于STAUKF算法及數據融合算法的橫滾軸的姿態角和角度誤差如圖5所示，俯仰軸的姿態角和角度誤差如圖6所示。

由圖5、圖6可以看出，動態條件下數據離散程度較大，在某些點處可能會出現“野值”，但從圖中可以直觀地看到，本文所提出的方法很好地抑制了“野值”的出現，即便在較為復雜的運動環境中也可保持精度較高的姿態數據輸出。

試驗算法的均方根誤差如表6所示，通過兩種后驗估計融合算法所得到的角度均方根誤差最小。相較于兩種單一IMU傳感器解算數據，基于誤差補償的后驗估計融合算法以及基于信息融合的后驗估計融合算法所解算的橫滾軸與俯仰軸角度均方根誤差分別平均減小了56.67%、58.94%，相較于冗余式雙IMU系統直接加權平均所解算的數據分別平均減小了36.55%、52.15%。

由于此時的角度基準為均值基準，故引入數據方差再次評判數據精度，各算法所解算數據方差如表7所示。

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

5.2.3 無人車二維建圖數據對比

利用無人車搭載冗余式雙IMU系統并以Karto算法在實驗室走廊進行二維建圖測試，試驗場景如圖7所示，無人車建圖測試結果對比如圖8所示。

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

6 結束語

本文利用PSO算法對STAUKF算法所解算出的角度以誤差尋優的方式進行融合，從而提升冗余式雙IMU系統的數據精度與利用率，并通過無人車硬件平臺對所提出的算法進行建圖試驗。通過多種算法的比較，可以看出本文所提出的融合方法較加權平均或單一傳感器更具優勢，且通過實測建圖，可以證實所提出的方法滿足低成本冗余MEMS IMU系統在無人車定位與導航系統中應用，并且兩種方法精度差距不大，都提高了系統信息利用率以及系統魯棒性，但在實測中建議使用基于信息融合后驗估計方法，其精度最高。

參 考 文 獻

[1] 李娜， 賀海育， 賈偉， 等. 基于2D建圖算法的導航方法[J]. 無線電工程， 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] 仇海濤， 陳曉東， 張峰. 基于改進ESKF濾波和自適應濾波的組合導航算法研究[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] 周萌萌， 張冰， 趙強， 等. 基于自適應漸消Sage-Husa擴展卡爾曼濾波的協同定位算法[J]. 中國艦船研究， 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] 張雯濤， 吳飛， 朱海. 自適應強跟蹤AST-ESKF無人車室內導航算法[J]. 導航定位學報， 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] 李明， 柴洪洲， 靳凱迪， 等. 改進的強跟蹤自適應UKF算法及其在大方位失準角對準中的應用[J]. 導航定位學報， 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] 許萬， 程兆， 夏瑞東， 等. 一種基于動態殘差的自適應魯棒無跡卡爾曼濾波器定位算法[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] 劉軼材， 范志先， 王翔宇， 等. 基于彎道自適應強跟蹤卡爾曼濾波的側向坡度估計算法[J]. 同濟大學學報（自然科學版）， 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] 喬美英， 高翼飛， 李宛妮， 等. 基于模糊魯棒自適應CKF算法的MEMS-IMU姿態估計[J]. 中國慣性技術學報， 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] 趙宏強， 鄧文斌， 辛濤， 等. 基于UWB和KF的無人車目標跟蹤方法[J]. 傳感器與微系統， 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] 李士心， 黃鳳榮， 邱時前， 等. 基于自適應強跟蹤濾波的捷聯慣導/里程計組合導航方法[J]. 中國慣性技術學報， 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] 羅亞倫， 廖育榮， 李兆銘， 等. 高超目標強跟蹤CKF自適應交互多模型跟蹤算法[J]. 北京航空航天大學學報（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系統[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] 許智賓， 李宏偉， 張斌， 等. 雙目視覺與慣導融合的移動機器人定位方法[J]. 測繪學報， 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] 許萬， 朱力， 張宇豪， 等. 基于自適應UKF的移動機器人室內定位算法研究[J]. 傳感器與微系統， 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神經網絡的無人機IMU多傳感器冗余的補償算法[J]. 電子測量與儀器學報， 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] 廖亞楨， 劉昱， 張立強， 等. 面向PDR定位系統的多層感知機IMU陣列數據融合方法[J]. 電子測量與儀器學報， 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的車載毫米波雷達多目標跟蹤算法[J]. 現代雷達（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] 彥恭敏， 翁浚. 捷聯慣導算法于組合導航原理[M]. 西安： 西北工業大學出版社， 2019.

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