矩陣對角化變換魯棒QCKF在視覺和慣性融合姿態測量中的應用

郭肖亭, 孫長庫,2, 王 鵬,2

(1. 天津大學精密測試技術及儀器國家重點實驗室, 天津 300072; 2. 中航工業洛陽電光設備研究所光電控制技術重點實驗室, 河南 洛陽 471009)

0 引 言

姿態確定問題是頭盔姿態確定問題[1]、頭盔瞄準定位[2]、無人機飛行控制[3]、增強現實[4]等領域的關鍵性問題之一,在工程實踐和科學研究中有重要的理論和實踐意義。單一姿態測量方法在精度、速度等方面因自身技術的限制,難以滿足測量需求,多傳感器融合姿態測量技術[5]應用而生。將視覺和慣性融合進行姿態測量,可以發揮視覺方法重復性和穩定性好[6-7]以及慣性方法更新頻率高、不受環境光干擾的特點[8],實現各傳感器的優勢互補。

針對視覺、慣性融合視覺和慣性融合測量的相關問題,國內外學者進行了大量的研究和探索[9-13]。在實際應用中,融合姿態測量的系統模型通常是非線性或/和非高斯,因此通常使用擴展卡爾曼濾波(extended Kalman filter, EKF)[14]、無跡卡爾曼濾波(unscented Kalman filter, UKF)[15]、容積卡爾曼濾波(cubature Kalman filter, CKF)[16-18]等適合于計算機迭代處理的非線性濾波算法進行多傳感器融合。EKF是高斯白噪聲假設下典型的非線性濾波方法,但EKF存在理論上的局限性,其一階線性截斷誤差使得濾波精度低甚至導致濾波發散,尤其在系統非線性較強、存在未知干擾或狀態突變等情況下。UKF不對系統模型進行線性化處理, 將對非線性系統的函數近似轉換成對其后驗概率密度的近似,實現簡單,濾波效果優于EKF。CKF采用一組等權值的容積點集來計算后驗概率密度,相比EKF和UKF,具有更優的非線性逼近性能、數值精度。

上述非線性濾波算法都屬于高斯濾波器,即假設狀態的后驗分布和預測分布近似為高斯分布。而在實際應用中,往往存在著模型不確定性或/和噪聲統計特性不完全已知,這些不確定因素會造成傳統的非線性卡爾曼濾波算法失去最優性,使得估計精度降低甚至濾波發散。而基于魯棒控制理論的H∞濾波算法[19]與卡爾曼濾波器采用最小方差估計準則不同,該算法使用極大極小準則,不需要知道環境噪聲的統計特性等先驗信息,通過使噪聲能量和估計誤差能量的最大比值小于某一個正數,確保外界干擾對狀態估計結果的影響最小,因此該算法對系統模型誤差和外界干擾具有更強的魯棒性。將H∞魯棒濾波算法和CKF相結合,形成魯棒CKF濾波策略,可以克服系統非線性、非高斯特性以及突變型不良測量對濾波帶來的影響,提高測量系統的濾波精度以及魯棒性。針對計算誤差、舍入誤差、模型失準等原因造成協方差矩陣失去正定性而往往出現濾波中斷的問題,使用矩陣對角化變換代替標準CKF算法中的Cholesky分解[20],提高濾波穩定性。姿態參數采用計算量小、可全姿態工作且不存在奇異性的四元數,針對其在非線性濾波中易出現協方差矩陣奇異性的問題,使用乘性誤差四元數三維矢量進行預測值和方差的計算,以保持濾波算法的穩定性。

搭建了慣性和視覺多傳感器融合姿態測量系統,針對系統中各傳感器坐標系以及輔助坐標系不一致的問題,通過將各傳感器測量值轉換到目標坐標系中完成測量系統坐標系的統一,將提出的基于矩陣對角化變換的魯棒四元數容積卡爾曼濾波(quaternion cubature Kalman filter,QCKF)算法在實驗平臺上進行驗證,并與標準CKF進行對比,驗證本文算法的有效性。

1 視覺和慣性融合姿態測量原理

融合姿態測量原理如圖1所示。慣性測量單元(inertial measurement unit,IMU)和目標物體固定連接,攝像機正對與目標物體固定連接的立體靶標。在目標物體運動過程中,IMU以初始姿態為基準,測量其相對空間慣性坐標系姿態;攝像機拍攝立體靶標運動圖像,通過對特征點圖像坐標分析計算,完成視覺姿態測量。

圖1 姿態測量系統結構示意圖Fig.1 Schematic diagram of fusion measurement system

慣性測量和視覺測量在不同的坐標系中進行并且各自的測量姿態與目標物體運動姿態存在差異,因此對測量系統坐標系進行統一。定義ObXbYbZb、OcXcYcZc、OtXtYtZt、OnXnYnZn分別表示運動物體坐標系(b系)、攝像機坐標系(c系)、靶標坐標系(t系)以及IMU坐標系(n系),如圖2所示。測量中使用低精度的微機電系統(micro-electro-mechanical system,MEMS)-IMU,其無法感測地球自轉角速率,因此可選取和地球固定連接的坐標系為近似空間慣性坐標系OiXiYiZi,本文中選取c系。坐標系統一的目標可表述為將慣性測量和視覺測量均轉換到目標物體在c系中的旋轉姿態。

(1)

(2)

圖2 測量系統坐標系統一Fig.2 Coordinate system normalization

2 視覺和慣性融合姿態測量算法

2.1 視覺姿態測量

視覺姿態測量方法是使用攝像機拍攝運動物體上特征點,根據特征點在攝像機中成像坐標的變化,結合已知特征點在被測物體上的布局坐標,實現對被測物體姿態的測量[6-7]。本文視覺測量結構采用由外向內的方法,即特征點布置在運動體上,而攝像機固定在環境中并朝向被測物,在室內近距離高精度定姿尤其是頭部跟蹤應用場景中,此種測量結構與由內向外的視覺測量結構相比更實用。立體靶標由4個非共面紅外LEDs組成,為視覺測量的特征點,各個特征點之間的相對空間幾何關系已知。使用紅外特征點是為了避免環境中雜散光源在圖像處理過程中進行特征識別時帶來干擾。使用4個非共面特征點可以保證在較大的運動范圍內仍可以正確識別各個特征點。

在較大的運動范圍內使用較少的特征點進行視覺姿態測量,比例正交投影迭代變換(pose from orthography and scaling with iterations,POSIT)是比較經典的非線性迭代算法;而POSIT算法的測量精度與測量范圍有關,小范圍內可以達到較高的測量精度,隨測量范圍增加精度會相應地降低。針對此問題本文使用基于約束校準的姿態測量算法[6]。標準POSIT算法以初始位置為基準進行相對姿態測量,如圖3中A方案所示;而約束校準算法采用圖3中B方案,先標定后測量。首先對全空間進行標定,建立基準位置,空間基矢量,基準姿態的一一對應關系;測量階段尋找距離待測量位置最近的基準點,則待測位置姿態可表示為基準姿態與待測位置相對基準位置姿態的代數和為

θx=θbase+θbase-x

(3)

式中,θx,θbase,θbase-x分別表示待測量位置姿態、選定基準位置姿態以及待測位置相對基準位置姿態。

圖3 POSIT算法和約束校準算法對比Fig.3 Contrast of POSIT and restrain calibration algorithm

2.2 陀螺儀模型

慣性姿態測量借助IMU中三自由度陀螺儀完成。陀螺儀感測目標物體運動,輸出角速度,通過姿態解算算法實現慣性姿態測量。陀螺儀輸出中存在許多誤差項,需要對其進行標定補償。陀螺儀輸出可建模為

(4)

零位偏差的標定可采用每次測量之前先標定的方法[21]。傳感器上電后先保持陀螺儀水平靜止測量幾組數據,對每組數據計算漂移角度,然后時間和漂移角度進行高階多項式曲線擬合獲取擬合系數,將每組數據擬合系數取均值得到標定擬合系數并保存。測量過程中根據標定擬合系數和采樣時間估計漂移角度,從漂移角度反算得到偏差值,陀螺儀輸出減去反算偏差對原始輸出中的零位偏差進行補償。具體過程如圖4所示。而對于與時間有關的隨機漂移的補償,可將其包含在濾波狀態量中,在每次濾波循環中對其不斷更新,從而進一步對陀螺儀輸出量進行補償。

圖4 陀螺儀零位偏差標定及補償Fig.4 Calibration and compensation of gyroscope bias

2.3 矩陣對角化變換

標準CKF算法中使用Cholesky分解進行協方差矩陣的分解,而Cholesky分解要求待分解矩陣具有正定性。當協方差矩陣失去正定性時可導致濾波立即中斷,對算法穩定性提出了極大的挑戰。濾波中斷多發生在濾波進行過多次之后由于模型失配、計算誤差等造成的協方差矩陣非正定。而基于矩陣對角化變換的方法對協方差矩陣進行分解,不需要矩陣具有正定性。

定理1設A為n階實對稱矩陣,則必有n階正交矩陣V,使得VTAV=V-1AV=D,即A=VTDV,其中D是以A的n個特征值為對角元素的對角陣。

而在卡爾曼濾波過程中協方差矩陣P是實對稱矩陣,由定理1立即可得P=VTDV,其中D是以P的n個特征值為對角元素的對角陣,V為n個特征值對應的兩兩正交的特征向量構成的正交矩陣陣,令

(5)

2.4 基于矩陣對角化變換的魯棒四元數容積卡爾曼濾波

不同于卡爾曼濾波基于使估計誤差的方差最小化的思想,H∞濾波是基于極大極小魯棒控制準則的濾波算法,即最小化可能存在最大誤差,具體指的是在對濾波初始狀態、系統噪聲、觀測噪聲等先驗信息未知的條件下獲取狀態量的最佳估計值,定義代價函數

(6)

引入約束條件γ,有

(7)

式(7)可改寫為

(8)

(9)

將H∞濾波應用到CKF中對H∞濾波方法中狀態估計協方差矩陣的遞推方程進行轉換

Pk+1=Pk+1|k-

(10)

如果對任意時刻γ取滿足條件pk>0的最小值,則可以得到最優H∞濾波。當γ→∞時,H∞濾波器退化為卡爾曼濾波器。當取最小值時濾波器的魯棒性最好,但估計方差不一定最小;當γ→∞時則可以得到最小方差估計,但魯棒性較差。因此通過適當選擇的值,調節系統在測量精度和系統魯棒性之間的平衡,以滿足測量需要。

CKF采用一組等權值的容積點集來計算后驗概率密度[14],對于模型精確系統的狀態估計精度可達到3階,但是當存在較大系統模型誤差或系統狀態存在突變的情況,仍會產生較大的估計誤差。在實際應用中,單一的CKF算法有時難以滿足測量需求,將QCKF和H∞魯棒濾波相結合,在CKF算法的框架中,將H∞濾波的魯棒特性加入到協方差矩陣更新計算中,并使用矩陣對角化變換代替標準CKF中的Cholesky分解,增加濾波算法的計算穩定性。具體過程如下。

步驟1計算容積點

(11)

(12)

四元數和陀螺儀漂移部分容積點相應為

(13)

式中,i=1,2,…,n。

步驟2計算經過狀態方程傳遞后的容積點

(14)

(15)

步驟3計算k+1時刻的狀態預測值

(16)

式中

(17)

(18)

(19)

步驟4計算k+1時刻狀態誤差協方差矩陣

(20)

式中

步驟5計算更新后的狀態容積點

(21)

(22)

步驟6計算經過測量方程的容積點

(23)

(24)

式中,i=1,2,…,n。

步驟7計算k+1時刻的測量預測值

(25)

(26)

(27)

步驟8估計k+1時刻的測量誤差協方差和一步預測互協方差矩陣

(28)

(29)

步驟9計算k+1時刻的濾波增益矩陣

(30)

步驟10計算k+1時刻的狀態估計值

(31)

(32)

(33)

(34)

(35)

步驟11使用式(10)進行k+1時刻的狀態誤差協方差矩陣更新。

該方法將CKF和H∞濾波結合,發揮CKF算法的高精度和H∞濾波的魯棒性的特點,使用矩陣對角化變換改善數值計算的穩定性。視覺和慣性融合測量系統,在保持與慣性高頻率輸出的同時,在視覺和慣性數據重合時刻進行濾波,用視覺數據抑制融合姿態隨時間漂移,獲取高頻率、穩定、高精度的融合測量輸出。

3 實驗驗證及結果分析

視覺和慣性融合姿態測量系統裝置如圖5所示。實驗中轉臺為目標運動物體,IMU、立體靶標通過螺絲安裝在轉臺上,攝像機朝向靶標放置,計算機控制轉臺運動、操作傳感器進行測量并對測量數據進行分析計算。其中實驗中使用的三維立體靶標結構如圖6所示,各特征點之間的空間幾何關系在圖中標示,外圍3個LEDs在同一平面內為等邊三角形3個頂點,中心LED位于三角形中心且距離平面25 mm。

圖5 測量系統實驗平臺Fig.5 Measurement system experimental platform

圖6 立體靶標Fig.6 Three-dimensional target

表1 測量系統中固定旋轉矩陣標定結果

在測量過程中,轉臺根據設計路徑進行運動,固定在轉臺上的IMU和立體靶標隨轉臺一起轉動,運動的同時進行IMU測量數據采集以及攝像機拍攝立體靶標在不同姿態下的圖像如圖7所示;對拍攝圖像進行處理提取特征點,結合各個特征點的幾何空間坐標關系,使用約束校準方法求解視覺姿態測量值。視覺姿態測量值和IMU測量數據結合,使用不同的融合算法,完成融合姿態量的求解。其中慣性數據采集周期為0.01 s,視覺數據采集周期為1.8 s。

圖7 不同姿態拍攝圖像特征點提取Fig.7 Processed images of target from different attitudes

為了對基于矩陣對角化變換的魯棒CKF方法的有效性進行驗證,在不同的運動形式下將其與標準CKF、純慣性姿態測量以及視覺姿態測量進行對比。主要進行了轉臺繞不同的旋轉軸的單軸運動,如圖8～圖10所示,以及轉臺俯仰軸和方位軸交替運動的兩軸運動,兩軸交替運動的具體運動方式為

(1)Z=-36°,X=-24°～ +24°;

(2)Z=-33°,X=+24°～-24°;

?

(25)Z=+36°,X=-24°～ +24°。

其中,X指代俯仰軸,Z指代方位軸。方位角階梯式上升從負的最大值運動到正的最大值,在方位角每增加3°的運動過程中,俯仰角往返運動具體如圖11所示。

對轉臺單、雙軸運動形式下不同算法的姿態角、姿態角誤差輸出圖形(見圖8～圖12)以及對應的均方根誤差(root-mean-square error,RMSE)數據表2、表3進行分析可知,將慣性和視覺進行融合求解姿態,可抑制純慣性測量角度漂移現象,保持視覺測量的穩定性,使融合姿態更新速率與慣性保持一致;同時改進的魯棒CKF與標準CKF相比準確度較好。

圖8 俯仰角-36°～36°運動姿態曲線及對應誤差Fig.8 -36°～36°pitch axis attitude curve and its error curve

圖9 橫滾軸-33°～33°運動姿態曲線以及對應誤差Fig.9 -33°～33°roll axis attitude curve and its error curve

圖10 方位角-48°～48°運動姿態曲線以及對應誤差Fig.10 -48°～48°yaw axis attitude curve and its error curve

不同算法RMSE/(°)俯仰角橫滾角方位角視覺0.07580.04390.0943CKF0.06870.06120.0955魯棒CKF0.05230.06530.0873

表3 兩軸運動不同算法的姿態角RMSE

對圖8～圖11姿態角誤差輸出圖形進一步分析可知,視覺姿態誤差角隨時間變化平穩,沒有與時間相關的趨勢項;但視覺姿態中會存在尖峰噪聲,而尖峰噪聲的出現是隨機的,沒有明顯的規律。

魯棒CKF保持與慣性同頻率的高頻輸出且能抑制慣性隨時間漂移帶來的誤差而跟隨穩定的視覺姿態。標準CKF也具有高頻輸出、抑制漂移的特點,但其輸出噪聲較大無論是視覺的尖峰噪聲或慣性的抖動噪聲均在標準CKF融合結果中有比較明顯的體現,魯棒CKF在弱化融合結果跟隨測量噪聲和系統噪聲變化方面優于CKF。單軸運動,每個軸的運動時間均不到60 s,其效果表明短時間內魯棒CKF數據融合算法的有效性;而兩軸交替運動,運動時間達到760多秒且運動形式復雜,其效果表明長時間魯棒CKF數據融合算法的有效性。

為直觀說明數據融合后相比純視覺姿態高頻輸出的特點,以圖8俯仰角單軸運動為例給出融合后數據更新速率與視覺、慣性更新速率的對比,如圖13所示。由圖13可知,融合姿態可抑制慣性姿態角度漂移,能跟隨穩定的視覺姿態,保持慣性數據高頻輸出的特點。

圖11 俯仰軸和方位軸交替運動姿態角誤差曲線Fig.11 Pitch axis and yaw axis alternating motion attitude angle error curves

圖12 俯仰軸和方位軸交替運動姿態角曲線Fig.12 Pitch axis and yaw axis alternating motion attitude angle curves

圖13 融合前后數據更新頻率對比Fig.13 Contrast of update frequency before and after data fusion

4 結 論

基于視覺和慣性融合姿態測量的目的,搭建測量系統平臺,對測量涉及的坐標系之間的關系進行了梳理和標定,目的是將不同的傳感器測量值統一到相同的坐標系中,為進一步的融合計算做準備。運動物體在單軸短時間運動以及雙軸長時間運動的不同運動形式中,使用視覺法、慣性法、CKF、矩陣對角化魯棒CKF法對姿態角進行計算并對姿態誤差角進行對比。實驗結果表明,魯棒CKF算法有效地利用視覺測量穩定性好的特性來抑制慣性測量誤差隨時間無約束發散并結合慣性測量更新頻率高的特點,極大地提高融合姿態的輸出頻率。改進的魯棒CKF在抑制視覺測量的尖峰噪聲、慣性測量的抖動噪聲均明顯優于CKF濾波算法。使用矩陣對角化變換代替標準CKF算法中的Cholesky分解,可以避免在狀態誤差協方差矩陣非正定時濾波中斷的情況,增加融合濾波算法數值計算的穩定性。

[1] GROVES P D. Principles of GNSS, inertial, and multisensor integrated navigation systems[M]. London:Artech House, 2008.

[2] FOXLIN E. Head-tracking relative to a moving vehicle or simulator platform using differential inertial sensors[C]∥Proc.of the SPIE-International Society for Optical Engineering,2002:133-144.

[3] CARRILLO L R G, LPEZ A E D, LOZANO R, et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV[J].Journal of Intelligent & Robotic Systems,2012,65(1): 373-387.

[4] CHAI L, HOFF W A, VINCENT T. 3-D motion and structure estimation using inertial sensors and computer vision for augmented reality[J]. Presence: Teleoperators and Virtual Environments, 2002, 11(5):474-492.

[5] GEBRE-EGZIABHER D, HAYWARD R C, POWELL J D. Design of multi-sensor attitude determination systems[J]. IEEE Trans.on Aerospace & Electronic Systems,2004,40(2):627-649.

[6] SUN C, SUN P, WANG P. An improvement of pose measurement method using global control points calibration[J]. Plos One, 2015, 10(7): e0133905.

[7] WANG P, XIAO X, ZHANG Z, et al. Study on the position and orientation measurement method with monocular vision system[J]. Chinese Optics Letters, 2010, 8(1): 55-58.

[8] GROVES P D. Navigation using inertial sensors[J]. IEEE Aerospace & Electronic Systems Magazine, 2015, 30(2):42-69.

[9] ENAYATI N. A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer-assisted surgery[J]. IEEE Trans.on Instrumentation & Measurement, 2015, 64(8): 2291-2301.

[10] ZHOU S, FEI F, ZHANG G, et al. 2D human gesture tracking and recognition by the fusion of MEMS inertial and vision sensors[J]. Sensors Journal IEEE, 2014, 14(4):1160-1170.

[11] YANG Z, SHEN S. Monocular visual-inertial state estimation with online initialization and camera-IMU extrinsic calibration[J]. IEEE Trans.on Automation Science & Engineering, 2017,14(1): 39-51.

[12] LIU Y, XIONG R, WANG Y, et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble[J]. IEEE Trans.on Industrial Electronics, 2016, 63(10): 6205-6216.

[13] ARAAR O, AOUF N, VITANOV I. Vision based autonomous landing of multirotor UAV on moving platform[J]. Journal of Intelligent & Robotic Systems, 2016, 85:1-16.

[14] 趙琳,張勝宗,李亮,等.基于自適應EKF的BDS/GPS精密單點定位方法[J].系統工程與電子技術,2016,38(9):2142-2148.

ZHAO L, ZHANG S Z, LI L, et al. BDS/GPS integrated precise point positioning based on adaptive extended Kalamn filter[J]. Systems Engineering and Electronics,2016,38(9):2142-2148.

[15] 喬相偉,周衛東,吉宇人.用四元數狀態切換無跡卡爾曼濾波器估計的飛行器姿態[J].控制理論與應用,2012,29(1):97-103.

QIAO X W, ZHOU W D, JI Y R. Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29(1): 97-103.

[16] 黃湘遠,湯霞清,武萌.基于降維CKF和平滑的SINS/OD動基座對準[J].系統工程與電子技術,2016,38(9):2135-2141.

HUANG X Y, TANG X Q, WU M. Research on moving base initial alignment of SINS/OD with reduced dimension CKF smoo-ther[J]. Systems Engineering and Electronics, 2016, 38(9):2135-2141.

[17] ARASARATNAM I, HAYKIN S. Cubature kalman filters[J]. IEEE Trans.on Automatic Control, 2009, 54(6): 1254-1269.

[18] 張秋昭, 張書畢, 鄭南山,等. GPS/INS組合系統的多重漸消魯棒容積卡爾曼濾波[J]. 中國礦業大學學報, 2014, 43(1):162-168.

ZHANG Q Z, ZHANG S B, ZHENG N S, et al. Multiple fading robust cubature Kalman filter for DPS/INS integrated navigation[J]. Journal of China University of Mining and Technology, 2014, 43(1):162-168.

[19] LIM J. A tutorial-game theory-based extended H infinity filtering approach to nonlinear problems in signal processing[J]. Digital Signal Processing, 2014, 34(1): 1-15.

[20] 趙利強, 陳坤云, 王建林, 等. 基于矩陣對角化變換的高階容積卡爾曼濾波[J]. 控制與決策, 2016, 31(6): 1080-1086.

ZHAO L Q, CHEN K Y, WANG J L, et al. High-degree cubature Kalman filter based on diagonalization of matrix[J]. Control and Decision,2016, 31(6): 1080-1086.

[21] ZHANG Y, YANG X, XING X, et al. The standing calibration method of MEMS gyro bias for autonomous pedestrian navigation system[J].Journal of Navigation,2016,70(3):607-617.