無GPS導(dǎo)航下基于光流的小型無人機(jī)導(dǎo)航算法

曾幼涵， 姜香菊

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

0 引 言

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

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

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

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

1 PX4FLOW光流傳感器檢測算法

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

(1)

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

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

(2)

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

3)尺度縮放

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

(3)

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

2 融合策略

加速度計(jì)、陀螺儀、光流傳感器有不同的頻率響應(yīng)特性，從頻域?qū)鞲衅鲾?shù)據(jù)進(jìn)行融合處理，可以提升測量精度、減小估計(jì)誤差。加速度計(jì)動(dòng)態(tài)特性較差，在高頻段易受干擾，產(chǎn)生高頻噪聲，陀螺儀和光流傳感器在低頻段動(dòng)態(tài)響應(yīng)特性較差，而高頻段動(dòng)態(tài)響應(yīng)特性好，即短時(shí)間內(nèi)的數(shù)據(jù)穩(wěn)定可靠，但長時(shí)間數(shù)據(jù)易受感染，雖然陀螺儀加速度計(jì)和光流傳感器都存干擾和誤差，但在頻域上的響應(yīng)特性互補(bǔ)，通過3只傳感器的在不同頻域下動(dòng)態(tài)響應(yīng)的互相補(bǔ)充，可以得到整個(gè)頻域范圍內(nèi)優(yōu)秀的動(dòng)態(tài)響應(yīng)。

2.1 加速度計(jì)與陀螺儀的互補(bǔ)濾波器設(shè)計(jì)

通過加速度計(jì)與陀螺儀互補(bǔ)濾波，初步得到加速度與陀螺儀讀數(shù)。具體流程如圖1。

圖1 互補(bǔ)濾波器的算法流程

加速度計(jì)信號(hào)首先經(jīng)過低通濾波器消除高頻噪聲，然后，對(duì)得到的結(jié)果進(jìn)行歸一化，由重力向量與加速度計(jì)向量垂直分量叉乘，得到姿態(tài)誤差值。

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

2.2 光流傳感器對(duì)加速度校正

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

圖2 光流校正結(jié)構(gòu)

該算法的數(shù)學(xué)表達(dá)式可以表達(dá)為

(4)

綜上，設(shè)計(jì)的濾波器可以得到飛行器的姿態(tài)、速度、位移，通過這些信息可以對(duì)飛行器進(jìn)行完全控制。

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

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

圖3 光流傳感器安裝位置

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

使用SD卡記錄傳感器數(shù)據(jù)，將光流融合算法結(jié)果與慣性導(dǎo)航算法結(jié)果進(jìn)行對(duì)比。

讀取慣性導(dǎo)航算法的速度信息與光流融合算法得到的速度信息，如圖4所示?？梢钥闯觯瑧彝?shí)驗(yàn)條件下，隨著時(shí)間的推移通過慣性導(dǎo)航算法得到的速度估計(jì)曲線已經(jīng)出現(xiàn)嚴(yán)重的發(fā)散現(xiàn)象，而通過光流融合算法得出的速度估計(jì)曲線依然可以得到高精確度的飛行器速度數(shù)據(jù)。

圖4 慣性導(dǎo)航與光流融合的速度估計(jì)

因?yàn)閼T性導(dǎo)航算法速度估計(jì)產(chǎn)生嚴(yán)重的誤差，導(dǎo)致位置估計(jì)誤差發(fā)散的更快，因此,未給出慣性導(dǎo)航的位置估計(jì)圖形。融合算法得到的飛行器位置與目標(biāo)位置如圖5所示，可以看出當(dāng)飛行器進(jìn)入懸停狀態(tài)，飛行器一直穩(wěn)定在期望位置附近，可以實(shí)現(xiàn)0.5 m以內(nèi)的精確的定點(diǎn)飛行效果。

圖5 水平位置估計(jì)值與期望值對(duì)比

4 結(jié)束語

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