0 引言

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

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

1 系統硬件組成

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

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

    為了能夠實現穩定視覺定位，實驗平臺增加定高模式，選用HC-SR04超聲波傳感器模塊，通過互補濾波融合慣性數據完成定高功能。

    由于需要用到許多復雜計算機視覺處理算法，而普通單片機處理數據能力有限，需要借助上位機來處理，所以選用了700線FPV攝像頭采集圖像，圖像信息經5.8 G無線發射器傳輸到接收器，接收器通過數據采集卡傳輸到上位機。上位機圖像處理后將數據用無線數傳模塊傳輸到四旋翼無人機控制器上進行視覺區域定位任務。四旋翼無人機視覺區域定位系統結構圖如圖1所示。

2 視覺區域定位算法

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

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

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

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

    當四旋翼無人機在某一水平方向發生漂移時，可以發現該方向的兩個電機轉速降低，使得產生一個傾角，傾角越大導致該方向慣性器件的加速度值就越大。將濾波后慣性器件的加速度值積分成速度，作為PID速度內環反饋輸入：

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

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

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

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

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

3 實驗與分析

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

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

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

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

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

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

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

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

4 結論

    針對四旋翼無人機在無人控制時出現漂移情況，本文使用了金字塔Lucas-Kanade稀疏光流得到水平偏移量，將高度值和慣性數據融合進行視覺定位控制。實驗取得了一定效果。但由于慣性單元存在零點漂移造成累積誤差很難全部去除，圖傳和數傳無線傳輸過程產生干擾等原因導致本實驗平臺很難在非常小范圍內視覺定位，后續將優化狀態估計等算法以進一步減小定位誤差。

參考文獻

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

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

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

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

作者信息:

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

（南京工業大學 電氣工程與控制科學學院，江蘇 南京211816）