無GPS導航下基于光流的小型無人機導航算法

曾幼涵， 姜香菊

(蘭州交通大學 自動化與電氣工程學院，甘肅 蘭州 730070)

0 引 言

目前飛行器的位置與速度的估計廣泛采用全球定位系統(global positioning system,GPS)和慣性導航系統實現[1]，然而在建筑物內等無GPS信號情況下，很難實現無人飛行器的定位[7]。因此,進行飛行器無GPS信號的導航技術研究對于飛行器自主飛行有重要意義。

呂強等人[2]使用光流傳感器輔助高精度慣性單元測得飛行器的速度，實現了飛行器懸停效果。方家豪等人[3]使用地面微基站系統實現了無人機在無GPS環境下的定位。楊天雨等人[4]通過光流傳感器采集數據設計了基于無跡卡爾曼濾波(unscented Kalman filtering,UKF)的導航算法，實現了飛行器無GPS環境下的自主導航。

以上研究實現了在無GPS導航下的飛行器定位，但也存在相應不足：只能估計飛行器的速度無法得到飛行器的位置信息、需要已知飛行環境并提前在地面布置外部導航系統以及需要建立精確的系統模型，對處理器運算能力要求高，而小型飛行器由于受自身載荷有限，板載計算能力較弱，無法使用過于復雜算法。

本文引入一種基于光流與慣性測量單元(inertial measurement unit,IMU)的算法，將傳感器采集的數據進行融合，在不需要已知飛行環境和外部定位系統的情況下，不僅可以獲得飛行器的姿態、速度而且還可以估計位置信息。實驗表明該方法可以在無GPS導航下實現對小型無人機進行有效控制，且具有精度高、運算量小、迭代簡單的優點。

1 PX4FLOW光流傳感器檢測算法

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

(1)

假設fk(m,n)為第k幅圖像中(m,n)處的像素灰度值，fk(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)旋轉補償

為去除由于攝像頭的安裝誤差以及自旋產生的干擾，利用光流傳感器測得的角速度ω和攝像頭的焦距f對旋轉的器件進行補償，分別得到圖像水平方向的光流分量vx，vy

(2)

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

3)尺度縮放

根據投影公式計算得到飛行器的水平移動速度vfx，vfy

(3)

式中z為飛行器的飛行高度。

2 融合策略

加速度計、陀螺儀、光流傳感器有不同的頻率響應特性，從頻域對傳感器數據進行融合處理，可以提升測量精度、減小估計誤差。加速度計動態特性較差，在高頻段易受干擾，產生高頻噪聲，陀螺儀和光流傳感器在低頻段動態響應特性較差，而高頻段動態響應特性好，即短時間內的數據穩定可靠，但長時間數據易受感染，雖然陀螺儀加速度計和光流傳感器都存干擾和誤差，但在頻域上的響應特性互補，通過3只傳感器的在不同頻域下動態響應的互相補充，可以得到整個頻域范圍內優秀的動態響應。

2.1 加速度計與陀螺儀的互補濾波器設計

通過加速度計與陀螺儀互補濾波，初步得到加速度與陀螺儀讀數。具體流程如圖1。

圖1 互補濾波器的算法流程

加速度計信號首先經過低通濾波器消除高頻噪聲，然后，對得到的結果進行歸一化，由重力向量與加速度計向量垂直分量叉乘，得到姿態誤差值。

誤差值經過比例—積分(proportional integral,PI)控制器，得到角速度修正值，與陀螺儀測量值經過高通濾波器后的預測值進行疊加，得到修正后的角速度值，根據一階龍格庫塔方法求解一階微分方程，可以求出四元數的值，從而得到飛行器姿態。

2.2 光流傳感器對加速度校正

加速度通過積分獲得的速度可以用光流測得的速度來校正[5,6]。校正后的加速度通過積分可以得到飛行器的位移。光流校正結構如圖2所示。

圖2 光流校正結構

該算法的數學表達式可以表達為

(4)

綜上，設計的濾波器可以得到飛行器的姿態、速度、位移，通過這些信息可以對飛行器進行完全控制。

3 實驗結果分析

本文采用自主搭建的四旋翼飛行器硬件平臺進行測試，傳感器安裝位置如圖3所示，用于對比光流和慣性單元融合的數據算法的精確度。

圖3 光流傳感器安裝位置

在室外環境光線良好的條件下進行定點懸停實驗，實驗中首先用遙控器控制飛行器起飛并抵達指定位置，然后發送指令，飛行器進入自主懸停程序，懸停高度1 m,時間2 min。測試和對比慣性導航算法和光流融合算法的速度估計數據和位置估計信息。

使用SD卡記錄傳感器數據，將光流融合算法結果與慣性導航算法結果進行對比。

讀取慣性導航算法的速度信息與光流融合算法得到的速度信息，如圖4所示?？梢钥闯?，懸停實驗條件下，隨著時間的推移通過慣性導航算法得到的速度估計曲線已經出現嚴重的發散現象，而通過光流融合算法得出的速度估計曲線依然可以得到高精確度的飛行器速度數據。

圖4 慣性導航與光流融合的速度估計

因為慣性導航算法速度估計產生嚴重的誤差，導致位置估計誤差發散的更快，因此,未給出慣性導航的位置估計圖形。融合算法得到的飛行器位置與目標位置如圖5所示，可以看出當飛行器進入懸停狀態，飛行器一直穩定在期望位置附近，可以實現0.5 m以內的精確的定點飛行效果。

圖5 水平位置估計值與期望值對比

4 結束語

本文提出一種四旋翼飛行器水平方向上速度與位置信息的測量方法，該算法可以在無GPS條件下，不需要外部輔助定位系統，不需要已知飛行環境，不需要添加激光雷達等昂貴的傳感器，僅需要光流傳感器和IMU模塊的數據就可以實現飛行器速度和位置的精確估計，該算法迭代簡單，很適合搭載微型處理器，自身載荷有限的小型無人機平臺。實驗結果表明：光流與IMU的數據算法,能夠得到精確的位置和速度信息，為小型飛行器的自主控制提供精確的導航參數，克服了傳統微慣性導航系統對GPS的嚴重依賴。