融合光流與MARG 傳感器的三維姿態(tài)測(cè)量研究*

石 琦，李 翔

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

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

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

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

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

1 光流傳感器測(cè)量模型

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

圖1 光流測(cè)量原理

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

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

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

2 基于光流的角速度測(cè)量

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

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

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

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

3 光流與MARG 傳感器的融合

3.1 算法整體流程

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

①?gòu)墓饬鱾鞲衅鳒y(cè)量數(shù)據(jù)中提取三維角速度；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)條件

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

圖3 實(shí)驗(yàn)所用光流傳感器模塊

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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