0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，a_x是慣性器件x方向加速度值，v_x是加速度積分后得到的速度值，t表示運行周期，為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)輸出至電機，融合方式以式(5)形式給出：

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

3 實驗與分析

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

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

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

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

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

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

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

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

4 結(jié)論

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

參考文獻

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

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

[3] 呂印新，肖前貴，胡壽松.基于四元數(shù)互補濾波的無人機姿態(tài)解算[J].燕山大學(xué)學(xué)報，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é)報(信息科學(xué)版)，2007，32(8)：680-683.

[6] 鮮斌，劉洋，張旭，等.基于視覺的小型四旋翼無人機自主飛行控制[J].機械工程學(xué)報，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）