融合光流與MARG 傳感器的三維姿態測量研究*

石 琦，李 翔

(桂林電子科技大學電子工程與自動化學院，廣西 桂林 541004)

光流傳感器是一種能從連續圖像流中獲取運動信息的傳感器，近年來被廣泛用于無人機中，與加速度計、陀螺儀、磁強計(合稱MARG 傳感器，即磁強計magnetometer、加速度計accelerometer 及速率陀螺rate gyro 的縮寫[1]，亦可解釋為磁magnetic、角速度angular rate 和重力gravity 三種測量對象的縮寫[2-4])等傳感器相結合，以實現無人機的飛行控制。光流傳感器用于無人機飛控的一種典型方案是采用豎直向下安裝的單個光流傳感器，通過其測量值獲取無人機相對于地面的水平運動，可提高無人機定點懸停的位置精度[5-6]。但在此種方案中，光流傳感器的測量值實際上是無人機的平移、俯仰和橫滾三種運動共同作用的結果。因此，要想從光流測量值準確獲取無人機的平移運動，需要借助慣導(Inertial Navigation System，INS)提供的姿態信息或者陀螺儀提供的角速度測量值，將無人機姿態變化引起的光流加以消除，才能得到準確的水平速度(或位移)[7-11]。

如上所述，光流傳感器的測量值可分為平移分量與轉動分量兩部分，分別與載體的平移運動和旋轉運動相關。然而上述將光流傳感器用于無人機的方案中，只用到了平移運動形成的光流(平動光流)，而對于載體轉動造成的光流(轉動光流)則借助IMU 陀螺儀提供的角速度將其消去。此類用法的缺點在于:一是未能充分利用轉動光流包含的姿態信息；二是在消去轉動光流時易將慣導或陀螺儀的誤差代入到平移速度(或位移)的計算中。

在航天器控制領域，利用轉動光流獲取載體姿態信息的方法較為多見，如文獻[12]采用擴展卡爾曼濾波器(Extended Kalman Filter，EKF)由光流估計三軸角速度，文獻[13]和[14]采用多種不同的卡爾曼濾波算法由光流估計航天器的三軸角速度，文獻[15]對星空成像進行預處理后再提取光流數據以計算角速度。此類方法的問題是成像范圍內星體數目較多時，獲取光流所需計算量較大，且涉及非線性測量模型，需采用非線性的卡爾曼濾波算法(如EKF)。在無人機領域，文獻[16]嘗試采用光流傳感器測量無人機三維姿態，然而其所用算法也采用非線性測量模型，同樣存在求解復雜、計算量大等問題。

為充分利用光流傳感器測量值中包含的姿態信息，提出一種簡單的線性測量模型，可從光流測量值中提取角速度，并進而將獲得的角速度與陀螺儀測量值根據最小方差準則進行融合以減小角速度測量誤差，最后將融合后的角速度與加速度計及磁強計數據利用EKF 算法進行融合，以提高三維姿態的估計精度。通過實驗驗證了所提出的測量模型和算法的有效性。

1 光流傳感器測量模型

光流傳感器工作原理如圖1 所示，由于通常情況下物距d遠大于焦距f，故可認為像距近似等于f。由圖1 可見，凡是垂直于傳感器主光軸的運動均可產生光流，而旋轉運動與平移運動均可切割主光軸，故旋轉與平移均可產生相應的光流。因此，可將光流傳感器的測量值視為平移光流與轉動光流的疊加。

圖1 光流測量原理

首先考慮傳感器載體僅有平移運動的情況。設垂直于主光軸的平移速度為v，由平移造成的光流為δv，則由圖1 所示幾何關系可知δv正比于v，如式(1)所示，其中k為沿主光軸的單位矢量。可見平移光流δv與物距d成反比。

其次考慮傳感器載體轉動形成的光流。設載體角速度為ω，由其造成的光流為δω，經分析可知二者關系如式(2)所示。可見轉動光流δω與物距d無關。

光流傳感器的測量值應是上述兩部分光流的疊加，即

2 基于光流的角速度測量

由式(3)可知，當傳感器載體只進行旋轉運動時，光流測量值僅與角速度有關。光流與角速度的關系式即式(2)可改寫為矩陣形式，如式(4)所示。由于角速度ω為三維矢量，而轉動光流δω是像平面內的二維矢量，故式(4)中的Uω是一個2×3 常系數矩陣，它反映了光流傳感器在b系中的安裝方向。

將式(4)推廣到采用多個不共面光流傳感器進行測量的情形，則得到式(5):

式中:δωk表示第k個光流傳感器的測量值(其中k＝1，2，…，N)，Uωk為第k個光流傳感器對應的常系數矩陣。注意每個光流傳感器的測量值(即δωk)都是二維矢量(包含像平面內兩個正交方向的光流測量數據)，故Vω為2N×3 的常系數矩陣，由各光流傳感器在b系中的安裝角度確定。式(5)可視為關于角速度的超定方程組，可利用最小二乘法進行求解，如式(6)所示:

式中:ωof表示由光流測量值解算出的角速度。實際應用中，式(6)還可以進一步簡寫為式(7)，其中W＝是一個3×2N的常系數矩陣，只要能獲知角速度的真值，即可直接由光流測量值和相應的角速度真值求出W。

3 光流與MARG 傳感器的融合

3.1 算法整體流程

如上所述，光流傳感器可用于測量三維角速度，將其與MARG 傳感器進行數據融合有助于改善姿態估計精度。為此，設計了如下的數據融合與姿態估計流程:

①從光流傳感器測量數據中提取三維角速度；

②將光流傳感器獲取的角速度與陀螺儀測量得到的角速度進行融合；

③將融合后的角速度數據與加速度計及磁強計測量值一同代入EKF 估計姿態。

算法整體流程框圖如圖2 所示，其中:ωof表示由光流測量值提取的角速度，ωg為陀螺儀測得的角速度，ωs為二者融合后的角速度，g為加速度計測得的重力矢量，h為磁強計測得的地磁矢量。

圖2 數據融合及姿態估計算法流程框圖

3.2 基于最小方差準則的角速度融合

光流傳感器和陀螺儀均能測量載體的三維角速度，將兩者的測量值進行加權可提高測量精度，具體做法是按最小方差準則進行融合，如式(8)所示:

3.3 基于EKF 的姿態估計

選取姿態四元數q＝(q1q2q3q4)T作為狀態向量(q1為標量部分，其余三個分量為矢量部分)，利用EKF 進行姿態估計，遞推流程如下。

EKF 算法第一步(狀態預測):

式中:表示(k-1)時刻的驗后估計，表示k時刻的狀態預測值即驗前估計，Φk/k-1表示從(k-1)時刻到k時刻的狀態轉移矩陣，如式(10)所示，ωsx，k、ωsy，k和ωsz，k分別表示k時刻ωs的三軸分量，T為采樣周期。

式中:Pk-1/k-1為(k-1)時刻的驗后方差，Pk/k-1為k時刻的驗前方差，Qk為過程噪聲協方差陣。Qk由式(12)計算，其中Rω為角速度的協方差陣，Sk是由k時刻的姿態四元數按式(13)構造的雅可比矩陣，q1～q4依次為k時刻姿態四元數的四個分量。

EKF 算法第三步(狀態修正):

由k時刻的加速度計測量值gk與磁強計測量值hk構成EKF 的觀測量Zk，如式(14)所示:

式中:量測矩陣Hk可由式(16)求得。由于和分別為4 維和6 維向量，故Hk為6×4 矩陣。式(16)中:g0為本地重力加速度，hN及hU分別為地磁場的北向和天向分量。

接下來由式(17)求出k時刻的卡爾曼增益Kk，其中Rk為量測噪聲協方差陣，由磁強計的噪聲協方差陣Rh及加速度計的協方差陣Rg根據式(18)構造而成。

由卡爾曼增益可得k時刻的驗后估計(狀態修正)如式(19)所示:

4 實驗驗證

4.1 實驗條件

實驗采用的光流模塊型號為LC302，將三個光流模塊布置于兩兩正交的三個平面上，如圖3 所示。此外，實驗中還采用型號為IMU901 的MARG 傳感器模塊與圖3 所示光流模塊進行數據融合與姿態解算。

圖3 實驗所用光流傳感器模塊

采用型號為WTGAHRS1 的慣導模塊提供三維姿態基準，其姿態角精度可達靜態0.05°、動態0.1°，可作為三維姿態的真值，實驗中將其與光流模塊及航姿模塊互相固定，使得三者能夠同步轉動。數據采集時，手持模塊進行無規則轉動，使用串口接收數據進行統一處理，并將數據導入MATLAB 中進行解算。由WTGAHRS1 模塊測得的三維姿態變化如圖4 所示。

圖4 三維姿態角變化曲線

4.2 角速度測量及融合結果

以慣導模塊WTGAHRS1 提供的三維角速度數據作為真值，首先求出式(7)中的常系數矩陣W，如式(21)所示:

得到矩陣W后，再將其代入式(7)，即可從光流傳感器測量值解算出角速度ωof。

接下來按式(8)將ωof與陀螺儀測量的角速度ωg進行融合得到ωs。由實測數據分別計算ωof和ωg的均方根誤差(Root Mean Square Error，RMSE)，結果如表1 所示。將各軸角速度的RMSE 取平均后代入式(8)即可得到加權系數，從而由ωof和ωg加權后得到ωs，其RMSE 亦列于表1 中。由表1 可見，式(7)所給出的線性測量模型能夠有效地從多個不共面安裝的光流傳感器測量值中提取三維角速度信息，且測量精度與低成本MEMS 陀螺儀相當。將光流傳感器獲取的角速度信息與陀螺儀測量值進行融合后，可進一步提高精度，使三軸角速度的誤差限制在1°/s 以內。

表1 三維角速度誤差(RMSE) 單位:(°)/s

4.3 EKF 姿態估計結果

采用3.3 小節中的EKF 算法進行三維姿態估計，各傳感器的噪聲協方差陣取值為:Rω＝2.5×10-4I3×3，Rh＝4×10-4I3×3，Rg＝10-6I3×3，其中角速度的噪聲協方差參考表1 所列RMSE，重力及地磁矢量的噪聲協方差則由加速度計和磁強計噪聲分別經過矢量單位化處理后得到。此外，對重力及地磁矢量均作單位化處理后，式(16)所示量測矩陣Hk中的g0相當于變為1，而hN及hU則直接由本地的地磁傾角γ確定，實驗中測得γ≈39°，即hN≈cos39°，hU≈-sin39°。

在EKF 算法中分別采用兩種不同來源的角速度數據:陀螺儀測量值ωg及其與光流傳感器融合后的角速度ωs。將EKF 輸出的姿態四元數轉化為歐拉角，與慣導模塊WTGAHRS1 提供的姿態角真值進行比較，結果如表2 所示。可見，引入光流解算的角速度后，有助于提高姿態測量的精度。

表2 三維姿態角誤差(RMSE) 單位:(°)

5 結束語

提出一種利用光流傳感器解算角速度并與陀螺儀進行融合的方法，采用線性測量模型由光流測量值解算出三維角速度，并將其與MARG 傳感器中的陀螺儀按最小方差準則進行融合，再以融合后的角速度構建EKF 的狀態方程，以加速度計與磁強計數據構建EKF 量測方程。實驗結果表明，所提出的方法改善了基于MARG 傳感器的姿態估計精度，提供了一種姿態測量的新途徑，有助于提升姿態測量系統的冗余度。