曾幼涵, 姜香菊
(蘭州交通大學(xué) 自動(dòng)化與電氣工程學(xué)院,甘肅 蘭州 730070)
目前飛行器的位置與速度的估計(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)采用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為飛行器的飛行高度。
加速度計(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)。
通過加速度計(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)。
加速度通過積分獲得的速度可以用光流測得的速度來校正[5,6]。校正后的加速度通過積分可以得到飛行器的位移。光流校正結(jié)構(gòu)如圖2所示。
圖2 光流校正結(jié)構(gòu)
該算法的數(shù)學(xué)表達(dá)式可以表達(dá)為
(4)
綜上,設(shè)計(jì)的濾波器可以得到飛行器的姿態(tài)、速度、位移,通過這些信息可以對(duì)飛行器進(jìn)行完全控制。
本文采用自主搭建的四旋翼飛行器硬件平臺(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ì)比
本文提出一種四旋翼飛行器水平方向上速度與位置信息的測量方法,該算法可以在無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)重依賴。