誤差狀態卡爾曼濾波的視覺慣性自適應融合定位方法研究

摘 要：頭盔瞄準具（HMS）是近年來新一代戰斗機飛行員的輔助瞄準設備，能夠幫助飛行員增強戰場態勢感知能力，對敵方目標進行快速、精準打擊。其能正常工作的關鍵是獲取飛行員頭部相對于運動飛機的姿態參數。本文結合頭盔瞄準具這一應用場景研究了視覺組合姿態測量關鍵技術。視覺慣性組合定位能夠實現目標位姿測量方法的優勢互補，而由于標稱噪聲矩陣無法絕對準確預測，融合算法的魯棒性、精度有待進一步提升。針對這一問題，本文提出一種誤差狀態卡爾曼濾波框架下基于變分貝葉斯推斷的視覺慣性自適應融合方法。首先，對于過程噪聲使用逆威沙特（Wishart）分布進行建模，之后通過引入隱變量分解一步預測協方差，并結合變分貝葉斯推斷實現了對過程噪聲協方差矩陣的在線估計。試驗證明，在復雜運動及標稱噪聲協方差矩陣偏移較大的測量條件下，所提位姿測量算法具有較高的精度與魯棒性，能夠完成對靶標的快速、高精度跟蹤。

關鍵詞：自適應； 誤差狀態卡爾曼濾波； 變分貝葉斯； 視覺慣性融合； 姿態測量

中圖分類號：TP212.9 文獻標識碼：A DOI：10.19452/j.issn1007-5453.2024.04.014

基金項目： 航空科學基金（201951048002）

準確、快速地獲取運動物體的姿態在航空航天和工業制造等領域都有著極為重要的意義[1-3]。頭盔瞄準具（HMS）是近年來新一代戰斗機飛行員的輔助瞄準設備，其安裝在飛行員頭盔上，并將作戰信息投射到頭部顯示器中。HMS能夠幫助飛行員增強戰場態勢感知能力，是當今智能化作戰中的重要裝備。在目標瞄準的過程中，火控計算機根據飛行員頭部的瞄準線指向控制軍械部分的武器線指向，從而完成武器的快速瞄準。在這個過程中，測量飛行員頭部相對于飛機坐標系的姿態角是確定瞄準線的關鍵[4]。為了更好地估計飛行員頭部的姿態，可以通過融合來自不同傳感器的數據，從而充分利用不同傳感器的優勢進行互補。目前常見的一種融合方案是將單目相機與慣性器件組成測量系統：視覺測量具有較高的精度以及長期的穩定性，但受視場及曝光時間等條件的限制，其跟蹤范圍較小且測量頻率較低；而慣性器件的測量頻率高且跟蹤范圍廣，但由于器件的硬件限制，其輸出存在零偏，積分運算會使測量誤差隨時間不斷增大。二者組成一個測量系統可以彌補相互的缺點，實現高精度、高頻率、廣范圍的物體跟蹤及位姿測量[5-8]。

對于包含慣性傳感器的測量系統，誤差狀態卡爾曼濾波器（ESKF）是一種流行的多源數據融合算法，具有許多獨特的優點[9-12]。首先，誤差狀態描述避免了過度參數化導致的問題。其次，由于誤差狀態通常較小，因此對參數的一階線性化帶來的線性化誤差較小。最后，由于誤差狀態變化緩慢，因此濾波校正可以以較低速率進行。

在誤差狀態卡爾曼濾波過程中，噪聲協方差矩陣的設定對濾波器性能至關重要，噪聲信息的不準確會導致算法精度下降甚至發散。由于視覺測量具有長期穩定性，因此測量噪聲協方差矩陣可以預設為標稱值，并假定其在測量過程中不發生變化。但由于慣性測量單元（IMU）的測量結果易受環境干擾，并考慮數據通信錯誤及傳感器自身突變等特殊情況，過程噪聲協方差矩陣需要根據實際情況進行自適應調整。

對噪聲協方差矩陣自適應估計的傳統方法大致分為相關性法、協方差匹配法、最大似然估計法和貝葉斯法[13]4類。當前也有學者使用深度學習及深度神經網絡方法估計IMU的噪聲參數[14-16]，該方法需要大量的標記數據進行訓練并且需要更多的計算資源，相比傳統卡爾曼濾波明確的數學模型和簡單的遞歸方法，其工作機制的可理解性及模型的泛化性還有待驗證。

相關法利用系統輸出結果或更新值的自相關性來調整噪聲協方差，主要適用于常系數線性系統，其缺點是無法保障噪聲協方差矩陣的正定型并且計算量較大[17-18]。協方差匹配法使用更新值或殘差序列來計算更新值或殘差的協方差，并將獲得的協方差與其理論值進行匹配，以估計噪聲協方差，但該方法有不收斂的可能[19-20]。基于最大似然（ML）的自適應濾波器通過最大化似然函數估計協方差噪聲矩陣，如期望最大化（EM）方法[13]，但該方法需要足夠的先驗數據才能有較好的濾波效果。貝葉斯方法是最通用和最全面的自適應噪聲濾波方法[20]。Sarkka[21]為基于變分貝葉斯（VB）推理的自適應濾波奠定了基礎。通過將系統狀態和噪聲協方差的條件分布近似為高斯分布和逆威沙特（IW）分布的乘積，可以推斷噪聲協方差以及系統狀態的估算值。

VB方法由于其在估計未知噪聲統計方面的效率及完備的數學基礎而吸引了許多學者的興趣，并開發了許多基于VB的濾波器[20-25]。然而系統狀態和過程噪聲協方差的聯合后驗分布的概率模型是非共軛的[23]。因此，大多數基于VB的自適應算法只能在線估計未知的測量噪聲協方差，并假設過程噪聲協方差是已知并且恒定的[21-24]。為了減輕不準確的過程噪聲協方差的不利影響，黃玉龍等[22]提出了間接估計預測誤差協方差而不是過程噪聲協方差的算法。然而，在每個時間步長處利用不準確的標稱過程噪聲協方差，這可能導致精度下降。最近，黃玉龍等[22]提出了一種基于滑動窗口狀態矢量平滑后驗分布近似的自適應算法。然而由于慣性器件的輸出頻率遠高于視覺輸出頻率，該方法并不適合視覺慣性組合測量系統。

目前雖然已經有許多基于變分貝葉斯理論的自適應濾波器，但它們大多是基于傳統的卡爾曼濾波的框架導出的，很少有文獻討論ESKF中自適應方法的濾波性能。基于這樣的研究現狀，在綜合考慮濾波性能和計算代價的基礎上，本文采用誤差狀態卡爾曼濾波器對慣性和視覺數據進行融合，推導了測量系統的ESKF過程和測量模型，并采用VB理論提高了算法的自適應性。通過引入一個隱變量對一步預測誤差協方差矩陣進行分解，并利用變分貝葉斯理論將噪聲協方差和誤差狀態矢量一起推導，實現了過程噪聲協方差的直接估計，增強算法的精度及魯棒性。

1 視覺慣性組合測量系統

1.1 測量系統組成

本文所述融合定位方法所依托的測量系統為動基座上的視覺與雙慣性組合測量系統，采用兩個IMU與一個視覺傳感器進行組合測量，用于獲得非慣性系下待測目標相對于運動基座的姿態運動信息，測量系統結構如圖1所示。

測量系統由運動基座、高精度二維旋轉臺、主IMU、輔助IMU、單目視覺傳感器以及紅外特征點靶標組成。使用運動基座模擬載體的運動，使用高精度二維旋轉臺模擬目標物體的運動，主IMU用于獲取目標物體在自身坐標系下的運動，輔助IMU用于獲取載體在自身坐標系下的運動，單目視覺傳感器是一臺工業面陣相機，用于獲取目標物體在視覺傳感器坐標系下的運動，可以周期性地修正IMU由于零偏帶來的測量誤差。

本文的坐標系定義如下：OcXcYcZc為單目視覺傳感器坐標系，OmXmYmZm為主IMU坐標系，OsXsYsZs為輔助IMU坐標系。本文使用CBA表示由A坐標系轉換到B坐標系的方向余弦矩陣。

1.2 卡爾曼濾波器狀態量定義

定義標稱狀態矢量及誤差狀態矢量為

在物體位姿測量試驗中，二維轉臺進行多軸旋轉來模擬物體自身的運動，其運動軌跡如圖3和圖4所示。運動平臺的三個軸均以幅值為10°、周期為25s的正弦軌跡進行運動。

3.2 算法效果評估

為了充分驗證算法的濾波性能，使用不同的標稱噪聲協方差矩陣對誤差狀態卡爾曼濾波（ESKF）、容積卡爾曼濾波（CKF）、自適應誤差狀態卡爾曼濾波（AESKF）、基于期望最大化的自適應誤差狀態卡爾曼濾波（AESKF-EM）、基于變分貝葉斯推斷的自適應誤差狀態卡爾曼濾波（AESKF-VB）和基于協方差匹配的自適應誤差狀態卡爾曼濾波（AESKF-CM）進行測試驗證，考慮以下三種應用場景。場景1：標稱過程噪聲協方差矩陣遠大于參考過程噪聲協方差矩陣：-Qk=100Q？k。場景2：標稱過程噪聲協方差矩陣等于參考過程噪聲協方差矩陣：-Qk=Q？k。場景3：標稱過程噪聲協方差矩陣遠小于參考過程噪聲協方差矩陣：-Qk=0.01Q？k。

測試結果如圖5～圖7所示。位姿估算結果誤差的均方根誤差（RMSE）見表1。在場景1和場景3中，由于標稱過程噪聲協方差矩陣與參考過程噪聲協方差矩陣相差極大，而ESKF和CKF算法無自適應調節功能，因此其濾波效果遠比AESKF的濾波效果差。由于-Qk和Q？k相差極大，AESKF-CM未必能夠正確收斂；而AESKF-EM和AESKF-VB需要依賴標稱值進行一步預測，所以其預測精度也并不高。相比之下，在該場景中本文提出的濾波算法在降低RMSE方面表現效果十分顯著。這證明當過程噪聲協方差矩陣與真值相差十分大時，本文所提出的濾波算法十分有效。

在場景2中，標稱過程噪聲協方差矩陣與參考過程噪聲協方差矩陣相同，6種濾波算法的估計誤差都有所降低。其中濾波效果提升最多的是AESKF-VB，因為其在估計噪聲協方差矩陣時，仍要用到標稱值。盡管如此，AESKF算法仍與其精度持平，有效地降低了姿態估計誤差，這從RMSE結果可以得知。

在實際測量中，過程噪聲協方差矩陣精確值往往不能事先精確得知，將其設定為固定值會降低姿態估計精度。綜上，本文提出的濾波算法在多種場景下均能實現良好的濾波效果，具有較好的普適性。

4 結論

本文提出了一種基于誤差狀態卡爾曼濾波框架的視覺慣性自適應融合算法，通過引入隱變量并結合貝葉斯推斷的方法實現了過程噪聲協方差的在線估計，從而實現更精確的位姿估計。算法無須提前獲得較精確的過程噪聲協方差矩陣，具有更高的魯棒性與測量環境普適性。經過試驗測試，本文算法實現了在標稱過程噪聲協方差矩陣過大、過小及適中的全測量場景下的高精度姿態測量。尤其是在標稱過程噪聲協方差偏離真值較大的測量條件下，所提算法能夠顯著改善測量效果。綜上所述，本文提出的算法具有較高的精度及魯棒性，可以實現視覺慣性自適應融合的物體位姿精準估計。

參考文獻

[1]Eckenhoff K， Geneva P， Huang G Q. MIMC-VINS：A versatile and resilient multi-imu multi-camera visual-inertial navigation system[J]. IEEE Transactions on Robotics， 2021， 37（5）： 1360-1380.

[2]閻峰，劉震.基于立體視覺的無人機位姿測量方法[J]. 航空科學技術，2023，34（4）：72-78. Yan Feng， Liu Zhen. UAV pose measurement method based on stereo vision[J]. Aeronautical Science Technology， 2023， 34（4）： 72-78.（in Chinese）

[3]張西林，何亞坤，張恪易，等.面向自主空中加油任務的目標檢測技術研究[J]. 航空科學技術，2023，34（2）：64-71. Zhang Xilin， He Yakun， Zhang Keyi， et al. Object detection technology for autonomous air to air refueling[J]. Aeronautical Science Technology， 2023， 34（2）： 64-71.（in Chinese）

[4]張立民，姜杰，方偉，等.頭位瞄準技術應用與研究[J].艦船電子工程，2021，41（8）：93-96. Zhang Limin， Jiang Jie， Fang Wei， et al. Application and research of head position aiming technology[J]. Ship Electronic Engineering， 2021， 41（8）： 93-96.（in Chinese）

[5]Assa A， Janabi-Sharifi F.A Kalman filter-based framework for enhanced sensor fusion[J]. IEEE Sensors， 2015， 15（6）： 3281-3292.

[6]Eckenhoff K， Geneva P， Bloecker J， et al. Multi-camera visualinertial navigation with online intrinsic and extrinsic calibration[C]. 2019 International Conference on Robotics and Automation（ICRA）， 2019.

[7]Guo Xiaoting， Sun Changku， Wang Peng， et al. Vision sensor and dual MEMS gyroscope integrated system for attitude deter‐mination on moving base [J]. Review of Scientific Instruments， 2018， 89（1）： 015002.

[8]Gao Xitian， Li Baoquan， Shi Wuxi， et al. Visual-inertial odometry system with simultaneous extrinsic parameters optimization[C]. 2020 IEEE/ASME International Conference on Advanced Intelligent Mechatronics（AIM）， 2020： 1977-1982.

[9]崔曉珍，周琪，武東杰，等. 可融合里程計的INS/GNSS組合導航系統設計與試驗分析[J]. 航空科學技術，2022，33（2）：84-89. Cui Xiaozhen， Zhou Qi， Wu Dongjie， et al. Design and experi‐mental analysis of INS/GNSS integrated navigation system with odometer[J]. Aeronautical Science Technology， 2022， 33（2）： 84-89.（in Chinese）

[10]Vitali R V， McGinnis R S， Perkins N C. Robust error-state Kalman filter for estimating IMU orientation[J]. IEEE Sensors Journal， 2020， 21（3）： 3561-3569.

[11]Roumeliotis S I， Sukhatme G S， Bekey G A. Circumventing dynamic modeling： Evaluation of the error-state kalman filter applied to mobile robot localization[C]//Proceedings of 1999 IEEE International Conference on Robotics and Automation. IEEE， 1999.

[12]Del Rosario M B， Khamis H， Ngo P， et al. Computationally efficient adaptive error-state Kalman filter for attitude estimation[J]. IEEE Sensors Journal， 2018， 18（22）： 9332-9342.

[13]Duník J， Straka O， Kost O， et al. Noise covariance matrices in state‐space models： A survey and comparison of estimation methods： Part I[J]. International Journal of Adaptive Control and Signal Processing， 2017， 31（11）： 1505-1543.

[14]Brossard M， Barrau A， Bonnabel S. AI-IMU dead-reckoning[J]. IEEE Transactions on Intelligent Vehicles， 2020， 5（4）： 585-595.

[15]Brossard M， Barrau A， Bonnabel S. RINS-W： Robust inertial navigation system on wheels[C]. IEEE/RSJ International Con‐ference on Intelligent Robots and Systems（IROS）， 2019： 2068-2075.

[16]蘭德智. 基于DNN-Kalman濾波算法的移動機器人軌跡估計[D]. 哈爾濱： 哈爾濱工業大學，2022. Lan Dezhi. Mobile robot trajectory estimation based on DNNKalman filtering algorithm[D]. Harbin： Harbin Institute of Technology， 2022.（in Chinese）

[17]Mehra R. On the identification of variances and adaptive Kalman filtering[J]. IEEE Transactions on Automatic Control， 1970， 15（2）： 175-184.

[18]Davari N， Gholami A. Variational Bayesian adaptive Kalman filter for asynchronous multirate multi-sensor integrated navigation system[J]. Ocean Engineering， 2019， 174： 108-116.

[19]Liu Yahui， Fan Xiaoqian， Lyu Chen， et al. An innovative information fusion method with adaptive Kalman filter for integrated INS/GPS navigation of autonomous vehicles[J]. Mechanical Systems and Signal Processing， 2018， 100： 605-616.

[20]Huang Yulong， Zhang Yonggang， Wu Zhemin， et al. A novel adaptive Kalman filter with inaccurate process and measurement noise covariance matrices[J]. IEEE Transactions on Automatic Control， 2017， 63（2）： 594-601.

[21]Sarkka S， Nummenmaa A. Recursive noise adaptive Kalman filtering by variational Bayesian approximations[J]. IEEE Transactions on Automatic Control， 2009， 54（3）： 596-600.

[22]Huang Yulong， Zhang Yonggang， Xu Bo， et al. A new adaptive extended Kalman filter for cooperative localization[J]. IEEE Transactions on Aerospace and Electronic Systems， 2017， 54（1）： 353-368.

[23]Xu Hong， Duan Keqing， Yuan Huadong， et al. Black box variational inference to adaptive kalman filter with unknown process noise covariance matrix[J]. Signal Processing， 2020， 169： 107413.

[24]Huang Yulong， Zhang Yonggang， Li Ning， et al. A robust Gaussian approximate filter for nonlinear systems with heavy tailed measurement noises[C]. 2016 IEEE International Conference on Acoustics， Speech and Signal Processing（ICASSP）. IEEE， 2016： 4209-4213.

[25]He Jingjing， Sun Changku， Zhang Baoshang， et al. Variational Bayesian-based maximum correntropy cubature Kalman filter with both adaptivity and robustness[J]. IEEE Sensors Journal， 2020， 21（2）： 1982-1992.

Vision Inertia Adaptive Fusion Method for Attitude Determination Based on Error State Kalman Filter

Wang Peng1，2， Wang Dawei1， He Jingjing1

1. State Key Lab of Precision Measuring TechnologyInstruments，Tianjin University，Tianjin 300072，China

2. Key Laboratory of Electro-Optical Control Technology，Luoyang Institute of Electrooptic Equipment， Luoyang 471000，China

Abstract： Helmet Mounted Sights（HMS） are auxiliary sighting equipment for new generation fighter pilots in recent years. They can help pilots enhance battlefield situational awareness and conduct rapid and precise strikes against enemy targets. The key to its normal operation is to obtain the attitude parameter of the pilot’s head relative to the moving aircraft helmet-mounted sight. This paper investigates the key techniques for visual fusion and posture measurement in the context of helmet mounted sight. The visual inertial fusion method can realize the complementary advantages of these two target position measurement methods. However， the robustness and accuracy of the fusion algorithm need to be further improved， because the nominal noise matrix cannot be predicted absolutely and accurately. To address this problem， this paper proposes a visual inertial adaptive fusion method based on variational Bayesian inference in the error-state Kalman filter framework. First， the process noise is modeled using the inverse Wishart distribution. Then， the covariance is predicted in one step by introducing a latent variable， and the online estimation of the process noise covariance matrix is achieved by combining the variational Bayesian inference. Experimental findings unequivocally demonstrate that the proposed pose measurement algorithm exhibits remarkable accuracy and robustness in the face of complex motion and substantial deviations in the nominal noise covariance matrix. The proposed algorithm can complete fast and high-precision tracking of the target.

Key Words： adaptive； Error-State Kalman filter （ESKF）； Variational Bayesian （VB）； vision inertia fusion； attitude measuremen