《電子技術(shù)應(yīng)用》
您所在的位置：首頁 > 嵌入式技術(shù) > 設(shè)計應(yīng)用 > 四旋翼視覺區(qū)域定位系統(tǒng)設(shè)計
四旋翼視覺區(qū)域定位系統(tǒng)設(shè)計
2017年電子技術(shù)應(yīng)用第2期
張 鵬,，沈 捷,，王 莉，任重庚
南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院,，江蘇 南京211816
摘要： 提出四旋翼無人機(jī)利用機(jī)載攝像頭實現(xiàn)視覺定位的方案,。采用金字塔Lucas-Kanade稀疏光流方法來獲取圖像特征點(diǎn)的水平偏差。將高度,、慣性和水平偏差進(jìn)行數(shù)據(jù)融合,，融合的數(shù)據(jù)作為位置式PID控制器的反饋輸入。為了縮減視覺定位過程中軌跡檢測的誤差,，使用前向后向誤差去除誤匹配點(diǎn),；對某時刻跟蹤點(diǎn)丟失問題，使用離散卡爾曼濾波對該時刻的狀態(tài)進(jìn)行估計,。仿真和實驗表明,，該方案能使得四旋翼無人機(jī)在一定高度下具有良好的定位效果。
中圖分類號： TP273
文獻(xiàn)標(biāo)識碼： A
DOI：10.16157/j.issn.0258-7998.2017.02.009
中文引用格式： 張鵬,，沈捷,，王莉，等. 四旋翼視覺區(qū)域定位系統(tǒng)設(shè)計[J].電子技術(shù)應(yīng)用,，2017,，43(2)：44-46，50.
英文引用格式： Zhang Peng,，Shen Jie,，Wang Li，et al. A design of visual area localization system for quadrotor[J].Application of Electronic Technique,，2017,，43(2)：44-46，50.
A design of visual area localization system for quadrotor
Zhang Peng,，Shen Jie,，Wang Li，Ren Chonggeng
College of Automation and Electrical Engineering,，Nanjing Tech University,，Nanjing 211816，China
Abstract： This paper proposes a visual localization strategy based on quadrotor with micro camera. The pyramid Lucas-Kanade sparse optical flow is applied to obtain the horizontal deviation of feature points. The data fusion of height, inertia, and horizontal deviation is used as the input of positional PID controller. To reduce the error of tracking trajectory, the forward-backward error algorithm is employed to eliminate the mismatching points. Kalman filtering algorithm is adopted to estimate the current state when feature points are failed to be tracked. The simulation and experiment show the effectiveness of proposed method within a certain range.
Key words : optical flow,；data fusion,；forward-backward error；PID

0 引言

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

    本文視覺定位系統(tǒng)是指四旋翼無人機(jī)在一定高度上機(jī)載圖傳設(shè)備，通過攝像頭獲取光流數(shù)據(jù)計算四旋翼無人機(jī)相對地面移動的位移,，與高度,、慣性數(shù)據(jù)融合后作出決策，自動調(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加速度計和三軸MEMS陀螺儀,，三軸磁場計AK8975經(jīng)過橢圓擬合后補(bǔ)償磁場數(shù)據(jù)用于定向,，兩個傳感器使用IIC與主控制器通信，將得到九軸數(shù)據(jù)進(jìn)行姿態(tài)解算后得到橫滾角,、俯仰角,、偏航角[3]。

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

    由于需要用到許多復(fù)雜計算機(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所示。

qrs1-t1.gif

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

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

qrs1-gs1.gif

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

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

qrs1-gs2.gif

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

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

     qrs1-gs3.gif

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

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

    qrs1-gs4.gif

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

qrs1-t2.gif

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

    qrs1-gs5.gif

式中，Mi是電機(jī)輸出PWM值,，uultra是超聲波定高PWM值,，uyaw是偏航角姿態(tài)控制PWM值，ux_out是x方向位姿控制器PWM值,，uy_out是y方向位姿控制器PWM值,。

3 實驗與分析

    根據(jù)攝像機(jī)成像原理，可以推導(dǎo)出相似三角形公式為：qrs1-3-x1.gif當(dāng)焦距f為定值,，四旋翼無人機(jī)處在一定高度值h,，則圖像坐標(biāo)系上角點(diǎn)移動距離x(y)與四旋翼無人機(jī)運(yùn)動水平距離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),。

qrs1-t3.gif

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

qrs1-t4.gif

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

qrs1-t5.gif

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

qrs1-t6.gif

    圖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)系相反且共同控制前后方向,。

qrs1-t7.gif

    圖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)系相反且共同控制左右方向,。

qrs1-t8.gif

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

4 結(jié)論

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

參考文獻(xiàn)

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

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

[3] 呂印新,，肖前貴，胡壽松.基于四元數(shù)互補(bǔ)濾波的無人機(jī)姿態(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ī)自主飛行控制[J].機(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）

此內(nèi)容為AET網(wǎng)站原創(chuàng)，未經(jīng)授權(quán)禁止轉(zhuǎn)載,。