石 琦，李 翔

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

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

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

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

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

1 光流傳感器測量模型

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

圖1 光流測量原理

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

其次考慮傳感器載體轉(zhuǎn)動形成的光流。設(shè)載體角速度為ω，由其造成的光流為δω，經(jīng)分析可知二者關(guān)系如式(2)所示?？梢娹D(zhuǎn)動光流δω與物距d無關(guān)。

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

2 基于光流的角速度測量

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

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

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

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

3 光流與MARG 傳感器的融合

3.1 算法整體流程

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

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

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

③將融合后的角速度數(shù)據(jù)與加速度計及磁強(qiáng)計測量值一同代入EKF 估計姿態(tài)。

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

圖2 數(shù)據(jù)融合及姿態(tài)估計算法流程框圖

3.2 基于最小方差準(zhǔn)則的角速度融合

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

3.3 基于EKF 的姿態(tài)估計

選取姿態(tài)四元數(shù)q＝(q1q2q3q4)T作為狀態(tài)向量(q1為標(biāo)量部分，其余三個分量為矢量部分)，利用EKF 進(jìn)行姿態(tài)估計，遞推流程如下。

EKF 算法第一步(狀態(tài)預(yù)測):

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

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

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

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

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

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

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

4 實驗驗證

4.1 實驗條件

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

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

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

圖4 三維姿態(tài)角變化曲線

4.2 角速度測量及融合結(jié)果

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

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

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

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

4.3 EKF 姿態(tài)估計結(jié)果

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

在EKF 算法中分別采用兩種不同來源的角速度數(shù)據(jù):陀螺儀測量值ωg及其與光流傳感器融合后的角速度ωs。將EKF 輸出的姿態(tài)四元數(shù)轉(zhuǎn)化為歐拉角，與慣導(dǎo)模塊WTGAHRS1 提供的姿態(tài)角真值進(jìn)行比較，結(jié)果如表2 所示?？梢姡牍饬鹘馑愕慕撬俣群?，有助于提高姿態(tài)測量的精度。

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

5 結(jié)束語

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