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

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

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

0 引言

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

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

目前，基于GPS/IMU的位置信息融合方法存在積分累積誤差，且由外界因素導致飛行器上的GPS傳感器的信號變弱甚至丟失時，產生位置信息誤差過大。本文針對此問題，提出基于光流傳感器的位置信息融合方法。

1 IMU與光流傳感器分析

1.1 IMU中加速度計的數據輸出模型

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

加速度計數據輸出：

(1)

(2)

1.2 光流傳感器工作原理

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

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

(3)

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

2) 旋轉補償

飛行器位置固定不變，飛行器做橫滾或俯仰運動時，繞機體坐標系x軸或y軸的任何旋轉，導致姿態有小范圍的抖動，光流檢測到數據的變化[12]。而飛行器繞z軸旋轉時，由于光流圍繞著圖像中心旋轉將被平均，導致光流為零，所以繞z軸旋轉不用進行補償。

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

(4)

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

3) 尺度放縮

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

(5)

式(5)中，z為高度。

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

2.1 擴展Kalman濾波器

擴展Kalman濾波采用高斯系統的最優無偏估計，該算法得到結果的方差是最小的，相較于梯度下降算法，互補濾波算法效果更佳[13]。擴展Kalman濾波器是利用當下時刻的幾個相關數據估計下一時刻系統的狀態。在本文中，需要信息融合的數據有光流傳感器的數據、慣性器件的輸出數據、GPS的數據和超聲波傳感器的數據。

擴展Kalman濾波器系統模型：

(6)

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

(7)

2.2 傳統GPS/IMU算法

建立GPS/IMU組合模型的狀態方程[15]，首先需要知道估計的IMU導參數系統的狀態方程：

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

(8)

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

GPS的實時帶有誤差觀測值：

(9)

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

(10)

因此，系統測量位置方程為：

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

(11)

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

(12)

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

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

2.3.1光流傳感器與加速度計融合

IMU中的加速度計測出的速度含有積分累計誤差，且加速度計動態特性較差，在高頻段易受到干擾，產生高頻噪聲，光流傳感器的高頻段動態響應特性好，即短時間內的數據穩定可靠，利用光流測得的速度與IMU測得的速度進行互補濾波融合，能有效地解決積分累計誤差的問題，融合前后效果如圖1和圖2所示。

圖1 加速度計測得的速度波形圖Fig.1 Velocity waveform measured by accelerometer

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

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

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

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

此系統的狀態方程和觀測方程可列為：

(13)

式(13)中，Bk為控制分配矩陣，Γk為噪聲分配矩陣，Wk為過程噪聲矩陣，Vk為測量噪聲，F為系統狀態函數，h為觀測函數。

此方法的狀態量設為：

(14)

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

由此觀測向量可設為：

(15)

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

將狀態方程和觀測方程代入擴展的Kalman濾波器中可得：

(16)

式(16)中，

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

3 實驗仿真分析

3.1 實驗平臺簡介

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

圖4 四旋翼飛行器平臺Fig.4 Quadrotor aircraft platform

3.2 實驗結果及分析

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

圖5 軌跡圖Fig.5 Trajectories

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

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

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

表1 算法對比Tab.1 Algorithm comparison

從表1可知，光流/超聲波/IMU/GPS組合模型改進算法的實驗結果在x，y，z三個方向的誤差和位置偏差幅度均優于傳統GPS/IMU組合模型算法，所以表明基于擴展Kalman濾波的光流/超聲波/IMU/GPS組合模型改進算法明顯誤差更小，精度更高。

4 結論

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