0 引言

    隨著科技的發(fā)展，作為智能制造與通用航空融合發(fā)展的四旋翼無人機(jī)受到了人們關(guān)注，其在民用與軍事領(lǐng)域應(yīng)用越來越廣泛，如高空航拍、電力系統(tǒng)巡查、抵近偵查等^[1]。

    本文視覺定位系統(tǒng)是指四旋翼無人機(jī)在一定高度上機(jī)載圖傳設(shè)備，通過攝像頭獲取光流數(shù)據(jù)計(jì)算四旋翼無人機(jī)相對(duì)地面移動(dòng)的位移，與高度、慣性數(shù)據(jù)融合后作出決策，自動(dòng)調(diào)整自身的姿態(tài)與位置，使得四旋翼無人機(jī)可以在一定區(qū)域內(nèi)懸停。

1 系統(tǒng)硬件組成

    本文四旋翼無人機(jī)控制器核心芯片采用意法半導(dǎo)體公司的STM32F407ZET6，該芯片內(nèi)部有1 MB大容量Flash，具有低功耗、多功能的外設(shè)接口，其最大的優(yōu)點(diǎn)在于具有浮點(diǎn)運(yùn)算單元^[2]，提高了數(shù)據(jù)處理能力。

    四旋翼無人機(jī)用于姿態(tài)控制的慣性傳感器采用MPU6050,其內(nèi)部集成了三軸MEMS加速度計(jì)和三軸MEMS陀螺儀，三軸磁場(chǎng)計(jì)AK8975經(jīng)過橢圓擬合后補(bǔ)償磁場(chǎng)數(shù)據(jù)用于定向，兩個(gè)傳感器使用IIC與主控制器通信，將得到九軸數(shù)據(jù)進(jìn)行姿態(tài)解算后得到橫滾角、俯仰角、偏航角^[3]。

    為了能夠?qū)崿F(xiàn)穩(wěn)定視覺定位，實(shí)驗(yàn)平臺(tái)增加定高模式，選用HC-SR04超聲波傳感器模塊，通過互補(bǔ)濾波融合慣性數(shù)據(jù)完成定高功能。

    由于需要用到許多復(fù)雜計(jì)算機(jī)視覺處理算法，而普通單片機(jī)處理數(shù)據(jù)能力有限，需要借助上位機(jī)來處理，所以選用了700線FPV攝像頭采集圖像，圖像信息經(jīng)5.8 G無線發(fā)射器傳輸?shù)浇邮掌?，接收器通過數(shù)據(jù)采集卡傳輸?shù)缴衔粰C(jī)。上位機(jī)圖像處理后將數(shù)據(jù)用無線數(shù)傳模塊傳輸?shù)剿男頍o人機(jī)控制器上進(jìn)行視覺區(qū)域定位任務(wù)。四旋翼無人機(jī)視覺區(qū)域定位系統(tǒng)結(jié)構(gòu)圖如圖1所示。

2 視覺區(qū)域定位算法

    本文采用了金字塔Lucas-Kanade稀疏光流的跟蹤方法，該算法最早起源于昆蟲運(yùn)動(dòng)引起視網(wǎng)膜變化^[4]。Lucas-Kanade稀疏光流基于以下3個(gè)假設(shè)：(1)亮度恒定；(2)運(yùn)動(dòng)是“小運(yùn)動(dòng)”；(3)空間一致^[5]。

    光流跟蹤角點(diǎn)采用Shi和Tomasi方法，通過光流得到角點(diǎn)移動(dòng)偏差量d存放在動(dòng)態(tài)數(shù)組中，然后除以檢測(cè)到角點(diǎn)數(shù)量得到一個(gè)平均值，動(dòng)態(tài)數(shù)組使用完后將其內(nèi)存釋放，提高運(yùn)行效率。

    在實(shí)際用光流跟蹤點(diǎn)中，有一些點(diǎn)在跟蹤過程中會(huì)發(fā)生很大變化或突然消失，這種情況會(huì)造成點(diǎn)的跟蹤失敗，前向與后向軌跡誤差能夠很好地去除誤匹配點(diǎn)的情況^[7]。

    將得到前向后向特征點(diǎn)軌跡誤差從小到大排列取中位值。當(dāng)特征點(diǎn)軌跡誤差大于中位值時(shí)，則把該特征點(diǎn)丟棄；當(dāng)特征點(diǎn)軌跡誤差小于等于中位值時(shí)，則保留該特征點(diǎn)。

    當(dāng)四旋翼無人機(jī)在某一水平方向發(fā)生漂移時(shí)，可以發(fā)現(xiàn)該方向的兩個(gè)電機(jī)轉(zhuǎn)速降低，使得產(chǎn)生一個(gè)傾角，傾角越大導(dǎo)致該方向慣性器件的加速度值就越大。將濾波后慣性器件的加速度值積分成速度，作為PID速度內(nèi)環(huán)反饋輸入：

其中，a_x是慣性器件x方向加速度值，v_x是加速度積分后得到的速度值，t表示運(yùn)行周期，為2 ms。y方向速度值同理得到。

    本文視覺定位算法結(jié)構(gòu)框架如圖2所示。期望水平偏差值d_{exp_x(y)}和期望水平速度值v_{exp_x(y)}都為0， PID控制算法全部采用雙閉環(huán)位置式PID，內(nèi)環(huán)與外環(huán)采用不同頻率控制，位置式PID控制方法如下：

式中k_p、T_i、T_d分別為控制器比例、積分和微分參數(shù)。e(k)為差值，u(k)為PID控制器輸出值。

    定位控制器融合多種數(shù)據(jù)，通過PWM(Pulse Width Modulation，PWM)輸出至電機(jī)，融合方式以式(5)形式給出：

式中，M_i是電機(jī)輸出PWM值，u_ultra是超聲波定高PWM值，u_yaw是偏航角姿態(tài)控制PWM值，u_{x_out}是x方向位姿控制器PWM值，u_{y_out}是y方向位姿控制器PWM值。

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

    根據(jù)攝像機(jī)成像原理，可以推導(dǎo)出相似三角形公式為：當(dāng)焦距f為定值，四旋翼無人機(jī)處在一定高度值h，則圖像坐標(biāo)系上角點(diǎn)移動(dòng)距離x(y)與四旋翼無人機(jī)運(yùn)動(dòng)水平距離X(Y)是成正比關(guān)系的。

    圖3為四旋翼無人機(jī)速度和高度值曲線圖。將加速度積分后速度與超聲波速度互補(bǔ)濾波后得到融合速度，作為PID內(nèi)環(huán)反饋輸入，2 ms控制一次，如圖3(a)所示；超聲波高度值作為PID外環(huán)反饋輸入，50 ms控制一次，如圖3(b)所示，四旋翼無人機(jī)高度誤差在±5 cm范圍內(nèi)。

    為了減少定位過程中的誤差，使用前向后向誤差減少誤匹配點(diǎn)，將四旋翼以一個(gè)y方向上下移動(dòng)仿真飛行狀態(tài)，如圖4所示，使用前向后向誤差校準(zhǔn)后去除一些誤匹配點(diǎn)。

    對(duì)使用前向后向誤差的圖4進(jìn)行分析，發(fā)現(xiàn)圖中有一些零點(diǎn)，表現(xiàn)為目標(biāo)跟蹤點(diǎn)丟失。使用卡爾曼濾波對(duì)光流數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)，估計(jì)后數(shù)據(jù)變得非常平滑，如圖5所示。

    四旋翼無人機(jī)飛到0.6 m左右高度切換到定高模式，然后調(diào)飛至目標(biāo)物上方，如圖6所示，飛穩(wěn)后切換到視覺定位模式。

    圖7為前后方向速度和光流偏差值曲線圖。將機(jī)體坐標(biāo)系上的x方向加速度積分后得到的速度值取負(fù)，作為PID內(nèi)環(huán)反饋輸入，2 ms控制1次，如圖7(a)所示；圖像坐標(biāo)系上y方向光流偏移量經(jīng)過卡爾曼濾波后，作為PID內(nèi)環(huán)反饋輸入，80 ms控制1次，如圖7(b)所示。兩者變化關(guān)系相反且共同控制前后方向。

    圖8為左右方向速度和光流偏差值曲線圖。將機(jī)體坐標(biāo)系上y方向加速度積分后得到速度值取負(fù)，作為PID內(nèi)環(huán)反饋輸入，2 ms控制1次，如圖8(a)所示；圖像坐標(biāo)系上x方向光流偏移量經(jīng)過卡爾曼濾波后，作為PID外環(huán)反饋輸入，80 ms控制1次，如圖8(b)所示。兩者變化關(guān)系相反且共同控制左右方向。

    由于用機(jī)載攝像頭測(cè)出的是一種局部變化量，實(shí)驗(yàn)定位結(jié)果誤差不能以本身實(shí)驗(yàn)測(cè)量值給出，所以本文選擇在一塊寬1 m、長(zhǎng)1.5 m黑白棋布上飛行定位，且視覺定位區(qū)域在1 m2左右。

4 結(jié)論

    針對(duì)四旋翼無人機(jī)在無人控制時(shí)出現(xiàn)漂移情況，本文使用了金字塔Lucas-Kanade稀疏光流得到水平偏移量，將高度值和慣性數(shù)據(jù)融合進(jìn)行視覺定位控制。實(shí)驗(yàn)取得了一定效果。但由于慣性單元存在零點(diǎn)漂移造成累積誤差很難全部去除，圖傳和數(shù)傳無線傳輸過程產(chǎn)生干擾等原因?qū)е卤緦?shí)驗(yàn)平臺(tái)很難在非常小范圍內(nèi)視覺定位，后續(xù)將優(yōu)化狀態(tài)估計(jì)等算法以進(jìn)一步減小定位誤差。

參考文獻(xiàn)

[1] 段渭超.面向小型無人機(jī)的視覺高度測(cè)量和目標(biāo)識(shí)別技術(shù)研究與FPGA優(yōu)化[D].長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2013.

[2] 江斌.小型無人低空四旋翼飛行器綜合設(shè)計(jì)[D].杭州：浙江大學(xué)，2013.

[3] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補(bǔ)濾波的無人機(jī)姿態(tài)解算[J].燕山大學(xué)學(xué)報(bào)，2014，38(2)；175-180.

[4] Fumiya Iida．Biologically inspired visual odometer for navigation of a flying robot[J]．Robotics and Autonomous Systems，2003，44(3)：201-208．

[5] 江志軍，易華蓉.一種基于圖像金字塔光流的特征跟蹤方法[J].武漢大學(xué)學(xué)報(bào)(信息科學(xué)版)，2007，32(8)：680-683.

[6] 鮮斌，劉洋，張旭，等.基于視覺的小型四旋翼無人機(jī)自主飛行控制[J].機(jī)械工程學(xué)報(bào)，2015，51(9)：58-63.

[7] KALAL Z，MIKOLAJCZYK K，MATAS J.Forward-backward error：Automatic detection of tracking failures[C].Pattern Recognition(ICPR)，2010 20th International Conference on.IEEE，2010：2756-2759.

作者信息:

張  鵬，沈  捷，王  莉，任重庚

（南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院，江蘇 南京211816）