基于光流傳感器的位置信息融合方法

盧艷軍，張 前，張曉東，劉 冬

(1.沈陽航空航天大學(xué)自動(dòng)化學(xué)院，遼寧 沈陽 110136；2.遼寧通用航空研究院，遼寧 沈陽 110136)

0 引言

飛行器位置信息的獲取需要相關(guān)的傳感器測(cè)量和相關(guān)信息計(jì)算，光流傳感器和加速度計(jì)可以測(cè)量飛機(jī)的飛行速度，通過積分計(jì)算速度信息，得到飛機(jī)的位置信息[1]。超聲波傳感器垂直安裝在機(jī)身下方，可以直接測(cè)量到一個(gè)飛機(jī)高度，全球定位系統(tǒng)(GPS)是通過太空中的衛(wèi)星系統(tǒng)，直接給出飛行器的地理位置信息[2]。

目前，無人機(jī)主要是利用GPS傳感器獲取位置信息，但無人機(jī)在近地面空間飛行時(shí)，受建筑物、樹木對(duì)GPS信號(hào)的遮擋，且GPS傳感器本身誤差較大，無法獲得準(zhǔn)確的定位信息[3]。國內(nèi)外研究人員將四旋翼飛行器的GPS傳感器提供的數(shù)據(jù)與慣性測(cè)量單元(IMU)輸出的數(shù)據(jù)進(jìn)行融合，得到位置信息[4]，此位置信息相較于單一的GPS傳感器較精準(zhǔn)；但在GPS信號(hào)變?nèi)跎踔羴G失時(shí)，且在飛行過程中IMU會(huì)產(chǎn)生積分累積誤差，因此融合后的位置信息有局部震蕩，造成位置信息的不可靠，影響飛行器飛行效果[5]。文獻(xiàn)[6]拋棄GPS傳感器使用光流傳感器輔助慣性單元測(cè)得飛行器的速度，實(shí)現(xiàn)了飛行器定點(diǎn)懸停。文獻(xiàn)[7]通過光流傳感器采集數(shù)據(jù)設(shè)計(jì)了基于擴(kuò)展Kalman濾波(EKF)的導(dǎo)航算法，實(shí)現(xiàn)了飛行器在無GPS環(huán)境下的自主導(dǎo)航，但由于缺少GPS，無人機(jī)在高空機(jī)動(dòng)飛行的情況下，相關(guān)傳感器性能下降，導(dǎo)致飛行控制的精度降低[8]。

目前，基于GPS/IMU的位置信息融合方法存在積分累積誤差，且由外界因素導(dǎo)致飛行器上的GPS傳感器的信號(hào)變?nèi)跎踔羴G失時(shí)，產(chǎn)生位置信息誤差過大。本文針對(duì)此問題，提出基于光流傳感器的位置信息融合方法。

1 IMU與光流傳感器分析

1.1 IMU中加速度計(jì)的數(shù)據(jù)輸出模型

本文基于自主研發(fā)的IMU進(jìn)行相關(guān)研究。IMU由MPU6050(加速度計(jì)、陀螺儀)、HMC5883L(磁力計(jì))組成[9]。主要使用加速度計(jì)測(cè)得的速度信息。

加速度計(jì)數(shù)據(jù)輸出：

(1)

(2)

1.2 光流傳感器工作原理

采用的光流傳感器為PX4LOW型號(hào)，利用SAD塊匹配算法，其原理是以一定速率連續(xù)采集物體表面圖像，由于相鄰的兩幅圖像總會(huì)存在相同的特征，通過對(duì)比這些特征點(diǎn)的位置變化信息，便可以判斷出物體表面特征的平均運(yùn)動(dòng)，最終得到無人機(jī)相對(duì)于地面的水平速度(Vfx,Vfy)。

1) 采用SAD塊匹配算法[11]，并選擇連續(xù)2幀圖像中的最佳匹配塊

(3)

假設(shè)fk(m,n)為第k幅圖像中(m，n)處的像素灰度值，fk+1(m+i,n+j)為k+1圖像中(m+1，n+1)處的像素值，m×n為搜索范圍。當(dāng)尋找到兩塊像素灰度差異最小后，即可認(rèn)為t時(shí)刻(m，n)處像素點(diǎn)在t+Δt時(shí)刻運(yùn)動(dòng)(m+i，n+j)處，而(i，j) 即為Δt時(shí)間像素點(diǎn)的偏移值。

2) 旋轉(zhuǎn)補(bǔ)償

飛行器位置固定不變，飛行器做橫滾或俯仰運(yùn)動(dòng)時(shí)，繞機(jī)體坐標(biāo)系x軸或y軸的任何旋轉(zhuǎn)，導(dǎo)致姿態(tài)有小范圍的抖動(dòng)，光流檢測(cè)到數(shù)據(jù)的變化[12]。而飛行器繞z軸旋轉(zhuǎn)時(shí)，由于光流圍繞著圖像中心旋轉(zhuǎn)將被平均，導(dǎo)致光流為零，所以繞z軸旋轉(zhuǎn)不用進(jìn)行補(bǔ)償。

利用陀螺儀測(cè)得的角速度ωx，ωy，ωz和攝像頭的焦距f對(duì)光流傳感器進(jìn)行補(bǔ)償，分別得到圖像水平方向的光流分量ux,uy。

(4)

式(4)中，T=[Tx,Ty,Tz]為坐標(biāo)系變換矩陣。x，y分別為x和y方向的像素偏移量，z為高度。

3) 尺度放縮

根據(jù)投影公式計(jì)算得到飛行器的水平移動(dòng)速度(Vfx,Vfy)：

(5)

式(5)中，z為高度。

2 基于光流傳感器的位置信息融合方法

2.1 擴(kuò)展Kalman濾波器

擴(kuò)展Kalman濾波采用高斯系統(tǒng)的最優(yōu)無偏估計(jì)，該算法得到結(jié)果的方差是最小的，相較于梯度下降算法，互補(bǔ)濾波算法效果更佳[13]。擴(kuò)展Kalman濾波器是利用當(dāng)下時(shí)刻的幾個(gè)相關(guān)數(shù)據(jù)估計(jì)下一時(shí)刻系統(tǒng)的狀態(tài)。在本文中，需要信息融合的數(shù)據(jù)有光流傳感器的數(shù)據(jù)、慣性器件的輸出數(shù)據(jù)、GPS的數(shù)據(jù)和超聲波傳感器的數(shù)據(jù)。

擴(kuò)展Kalman濾波器系統(tǒng)模型：

(6)

上述表達(dá)式中，Xk與Xk-1表示k，k-1時(shí)的狀態(tài)向量，Φk,k-1是狀態(tài)Xk-1從k-1到k的轉(zhuǎn)換矩陣，Zk是觀測(cè)矩陣，觀測(cè)量為慣性單元的各傳感器輸出量，如GPS和光流傳感器和超聲波提供的輸出數(shù)據(jù)，式(6)中，Hk表示為量測(cè)轉(zhuǎn)移矩陣，測(cè)量噪聲矩陣Vk，系統(tǒng)狀態(tài)噪聲Wk-1，由各個(gè)參量組成的Kalman濾波器基本方程組[14]，如式(7)所示：

(7)

2.2 傳統(tǒng)GPS/IMU算法

建立GPS/IMU組合模型的狀態(tài)方程[15]，首先需要知道估計(jì)的IMU導(dǎo)參數(shù)系統(tǒng)的狀態(tài)方程：

Xt=[δVxδVyδVzφzφyφzδLδλδhεxbεyoεzbVa]，

(8)

式(8)中，δVx，δVy，δVz表示的是在三個(gè)不同方向上的速度誤差；φz，φy，φz是姿態(tài)角誤差；εxb，εyo，εzb是陀螺儀誤差；IMU解算的速度信息為Va=(Vx,Vy,Vz)。

GPS的實(shí)時(shí)帶有誤差觀測(cè)值：

(9)

式(9)中，LG來自GPS的接收到的緯度數(shù)據(jù)，λG是經(jīng)度信息，hG接收機(jī)得到實(shí)時(shí)高度數(shù)據(jù)，NG，NY，Nh是噪聲數(shù)據(jù)。其中的觀測(cè)值是利用GPS所測(cè)量的位置值，因此位置觀測(cè)方程為：

(10)

因此，系統(tǒng)測(cè)量位置方程為：

Zp(t)=Hp(t)X(t)+Vv(t)。

(11)

通過上述表達(dá)式可以推算出測(cè)量噪聲矩陣Vv(t)以及測(cè)量矩陣Hp(t)為：

(12)

將式(12)代入式(11)，即可得出飛行器飛行過程中位置信息Xt。

2.3 基于光流傳感器的位置信息融合改進(jìn)方法

2.3.1光流傳感器與加速度計(jì)融合

IMU中的加速度計(jì)測(cè)出的速度含有積分累計(jì)誤差，且加速度計(jì)動(dòng)態(tài)特性較差，在高頻段易受到干擾，產(chǎn)生高頻噪聲，光流傳感器的高頻段動(dòng)態(tài)響應(yīng)特性好，即短時(shí)間內(nèi)的數(shù)據(jù)穩(wěn)定可靠，利用光流測(cè)得的速度與IMU測(cè)得的速度進(jìn)行互補(bǔ)濾波融合，能有效地解決積分累計(jì)誤差的問題，融合前后效果如圖1和圖2所示。

圖1 加速度計(jì)測(cè)得的速度波形圖Fig.1 Velocity waveform measured by accelerometer

圖2 融合后的速度波形圖Fig.2 Velocity waveform after fusion

2.3.2基于光流傳感器的位置信息融合方法

本文把光流傳感器測(cè)得的速度信息和加速度計(jì)測(cè)得的速度信息先進(jìn)行融合得到一個(gè)融合速度，再將融合后的速度與超聲波傳感器所測(cè)得的高度信息作為擴(kuò)展Kalman濾波器中的預(yù)測(cè)量，然后選取GPS所提供的水平位置信息以及高度信息作為觀測(cè)量，最后通過擴(kuò)展Kalman濾波器融合得到水平位置信息和高度信息，從而估計(jì)無人機(jī)的位置信息。改進(jìn)方法原理如圖3 所示。

圖3 基于光流傳感器的位置信息融合改進(jìn)方法框圖Fig.3 Block diagram of improved method of position information fusion based on optical flow sensor

此系統(tǒng)的狀態(tài)方程和觀測(cè)方程可列為：

(13)

式(13)中，Bk為控制分配矩陣，Γk為噪聲分配矩陣，Wk為過程噪聲矩陣，Vk為測(cè)量噪聲，F(xiàn)為系統(tǒng)狀態(tài)函數(shù)，h為觀測(cè)函數(shù)。

此方法的狀態(tài)量設(shè)為：

(14)

式(14)中，Vfx和Vfy表示無人機(jī)的光流傳感器在x方向和在y方向上測(cè)得的速度信息；Zc為超聲波傳感器的讀數(shù)；加速度計(jì)速度信息Va=(Vx,Vy,Vz)。

由此觀測(cè)向量可設(shè)為：

(15)

觀測(cè)向量中的各個(gè)元素分別表示的是GPS所提供位置信息Xn,Yn,Zn。

將狀態(tài)方程和觀測(cè)方程代入擴(kuò)展的Kalman濾波器中可得：

(16)

式(16)中，

采用上述EKF法，可以求出X(t)=(Xt,Yt,Zt)，即可得無人機(jī)的位置信息。

3 實(shí)驗(yàn)仿真分析

3.1 實(shí)驗(yàn)平臺(tái)簡介

基于自主研發(fā)的四旋翼飛行器平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，如圖4所示。飛控處理器選用Pixhawk樂迪版，光流傳感器選開源的，型號(hào)為PX4FLOW，IMU型號(hào)選的是MPU6050 ，GPS選用M8N，超聲波傳感器型號(hào)選的是US-100。通過型號(hào)為HC-12的無線串口模塊，將飛行器的各項(xiàng)數(shù)據(jù)實(shí)時(shí)傳回到電腦終端。

圖4 四旋翼飛行器平臺(tái)Fig.4 Quadrotor aircraft platform

3.2 實(shí)驗(yàn)結(jié)果及分析

在實(shí)驗(yàn)中，光流傳感器通過處理圖像信息獲得飛行器的速度信息，GPS可以直接提供飛行器的位置信息，IMU可以獲取飛行器在飛行過程中的速度信息和姿態(tài)信息，超聲波垂直地面安裝在飛行器上可獲得飛行高度的測(cè)量值，做勻速螺旋上升運(yùn)動(dòng)，軌跡如圖5所示。本文基于自研的實(shí)驗(yàn)平臺(tái)做了位置信息融合算法實(shí)驗(yàn)，并對(duì)比了傳統(tǒng)GPS/IMU算法和光流/超聲波/IMU/GPS組合模型改進(jìn)算法，最終結(jié)果如圖6、圖7所示。

圖5 軌跡圖Fig.5 Trajectories

圖6 x，y，z 方向的位置變化對(duì)比圖Fig.6 Comparison chart of position change in x，y，z directions

圖6為飛行器在兩種算法下x，y，z三個(gè)方向的位置變化對(duì)比圖，可以很明顯地看出，通過傳統(tǒng)GPS/IMU組合模型算法的擴(kuò)展Kalman濾波器得到的位置信息與實(shí)際位置偏差幅度很大，局部存在著震蕩情況，通過光流/超聲波/IMU/GPS組合模型改進(jìn)算法的擴(kuò)展Kalman濾波器得到的位置信息與實(shí)際位置幾乎重合，位置偏差幅度很小。圖7為飛行器在兩種算法下x，y，z三個(gè)方向的誤差變化對(duì)比圖，可以明顯地看出，通過傳統(tǒng)GPS/IMU組合模型算法的擴(kuò)展Kalman濾波器得到的位置信息誤差比較大，通過光流/超聲波/IMU/GPS組合模型改進(jìn)算法的擴(kuò)展Kalman濾波器得到的位置信息誤差很小。兩種算法的具體的對(duì)比信息如表1所示。

圖7 x，y，z方向的誤差變化對(duì)比圖Fig.7 Comparison chart of position error in x，y，z directions

表1 算法對(duì)比Tab.1 Algorithm comparison

從表1可知，光流/超聲波/IMU/GPS組合模型改進(jìn)算法的實(shí)驗(yàn)結(jié)果在x，y，z三個(gè)方向的誤差和位置偏差幅度均優(yōu)于傳統(tǒng)GPS/IMU組合模型算法，所以表明基于擴(kuò)展Kalman濾波的光流/超聲波/IMU/GPS組合模型改進(jìn)算法明顯誤差更小，精度更高。

4 結(jié)論

本文提出基于光流傳感器的位置信息融合方法。該方法引入了光流傳感器，將其與IMU融合后的速度信息和超聲波傳感器的高度信息作為擴(kuò)展Kalman濾波中的預(yù)測(cè)量，觀測(cè)量為GPS提供的位置信息和高度信息，最后融合得到精準(zhǔn)的位置信息。仿真實(shí)驗(yàn)結(jié)果表明，該算法能更精準(zhǔn)地獲取四軸飛行器的位置信息，有效地解決了位置信息誤差大的問題，幫助飛行器更好地完成飛行任務(wù)，具有極高的工程應(yīng)用價(jià)值。