基于嵌入式Linux的農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法及應(yīng)用

韓 璞,王耀寬,胡四平

(1.南陽(yáng)理工學(xué)院 軟件學(xué)院,河南 南陽(yáng) 473000;2.湖北科技學(xué)院 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院,湖北 咸寧 437100)

0 引言

目前,美國(guó)、德國(guó)、日本等發(fā)達(dá)國(guó)家的農(nóng)用航空作業(yè)占耕地面積比例越來(lái)越高,無(wú)人機(jī)在農(nóng)業(yè)植保方面相對(duì)于傳統(tǒng)植保方式優(yōu)勢(shì)明顯,每分鐘可以完成0.07～0.13hm2的農(nóng)藥噴灑作業(yè),是人工的30～60倍,且農(nóng)用無(wú)人機(jī)還可以快速查看田間作物生產(chǎn)情況,具有傳統(tǒng)植保不可替代的優(yōu)勢(shì)。本文結(jié)合圖像處理和嵌入式控制技術(shù),設(shè)計(jì)了一套農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法,能夠?qū)崿F(xiàn)農(nóng)業(yè)無(wú)人機(jī)的避障和導(dǎo)航控制。

1 農(nóng)用無(wú)人機(jī)結(jié)構(gòu)與工作原理

1.1 農(nóng)用無(wú)人機(jī)機(jī)械結(jié)構(gòu)

本研究中農(nóng)用無(wú)人機(jī)為四旋翼無(wú)人機(jī),采用2.4G無(wú)線射頻收發(fā)模塊和藍(lán)牙4.0BLE無(wú)線控制模塊,通過(guò)這兩個(gè)模塊實(shí)現(xiàn)與手機(jī)端或PC端的無(wú)線通信,實(shí)時(shí)向控制端發(fā)送飛行器傳感器采集到的數(shù)據(jù)信息,同時(shí)接受控制端發(fā)送過(guò)來(lái)的控制指令。其主要的技術(shù)參數(shù)如表1所示。

表1 農(nóng)用無(wú)人機(jī)主要技術(shù)參數(shù)Table 1 The main technical parameters of agricultural UAV

本文研究的無(wú)人機(jī)主要包括嵌入式核心控制器、信號(hào)輸入和信號(hào)輸出等模塊。其中,嵌入式核心控制器采用意法半導(dǎo)體公司的STM32F103T8U6芯片,是一款采用72MHz 的32位Cortex-M3架構(gòu)處理器,其性價(jià)比和處理速度較高,能夠滿足無(wú)人機(jī)控制系統(tǒng)對(duì)硬件的要求;信號(hào)輸入由2.4G無(wú)線通信、藍(lán)牙透?jìng)?、陀螺儀加速度計(jì)和高精度氣壓計(jì)等構(gòu)成;信號(hào)輸出由電機(jī)及螺旋槳等構(gòu)成。四旋翼無(wú)人機(jī)實(shí)物如圖1所示。

圖1 四旋翼無(wú)人機(jī)實(shí)物圖Fig.1 The physical chart of four-rotor UAV

1.2 農(nóng)用無(wú)人機(jī)工作原理

采用四旋翼結(jié)構(gòu),結(jié)構(gòu)比較簡(jiǎn)單,由4個(gè)獨(dú)立的螺旋槳組成和4個(gè)控制電機(jī)組成,通過(guò)4個(gè)電機(jī)驅(qū)動(dòng)螺旋槳轉(zhuǎn)動(dòng)就能實(shí)現(xiàn)農(nóng)用無(wú)人機(jī)的飛行控制。農(nóng)用無(wú)人機(jī)的機(jī)械結(jié)構(gòu)如圖2所示。其中,M1、M2、M3和M4為4個(gè)螺旋槳,當(dāng)電機(jī)M1和電機(jī)M3做逆時(shí)針旋轉(zhuǎn),電機(jī)M2和電機(jī)M4做順時(shí)針旋轉(zhuǎn),4個(gè)螺旋槳均產(chǎn)生向上的升力。

圖2 農(nóng)用無(wú)人機(jī)的機(jī)械結(jié)構(gòu)Fig.2 The mechanical structure of agricultural UAV

設(shè)定Ω1、Ω2、Ω3和Ω4為4個(gè)螺旋槳,T1、T2、T3和T4為4個(gè)螺旋槳產(chǎn)生的升力,Q1、Q2、Q3和Q4為4個(gè)螺旋槳產(chǎn)生的反扭矩力。采用動(dòng)量守恒定理可以對(duì)懸浮的農(nóng)用無(wú)人機(jī)在空間產(chǎn)生的穩(wěn)態(tài)推力進(jìn)行數(shù)學(xué)建模,即

Ti：=CTρAriri2Ωi2

(1)

其中,CT為農(nóng)用無(wú)人機(jī)推力系數(shù),該值取決于無(wú)人機(jī)的機(jī)械結(jié)構(gòu);ρ為環(huán)境中大氣密度;Ari為第i個(gè)螺旋槳的面積;ri為第i個(gè)螺旋槳的半徑。式(1)可以簡(jiǎn)化為

Ti：=cTΩi2

(2)

其中,cT>0為一個(gè)常數(shù),其可在靜態(tài)推力測(cè)試中求出該值。

采用相同的分析方法,懸浮的農(nóng)用無(wú)人機(jī)產(chǎn)生的反扭矩為

Qi：=cQΩi2

(3)

其中,cQ>0為一個(gè)常數(shù),同樣可在靜態(tài)推力測(cè)試中求出該值。

由于農(nóng)用無(wú)人機(jī)設(shè)計(jì)完成后,其螺旋槳的結(jié)構(gòu)和尺寸不可能發(fā)生變化,因此其穩(wěn)態(tài)推力和反扭矩力只與電機(jī)的轉(zhuǎn)速有關(guān)。

2 雙目視覺(jué)理論

2.1 雙目視覺(jué)成像原理

雙目視覺(jué)理論是利用兩臺(tái)CCD攝像機(jī)從不同視覺(jué)獲取目標(biāo)物體的圖像信息,根據(jù)視差計(jì)算出目標(biāo)的三維坐標(biāo),整個(gè)過(guò)程包括構(gòu)建目標(biāo)物體三維模型和計(jì)算物體的三維坐標(biāo)信息等。單目和雙目視覺(jué)理論系統(tǒng)如圖3所示。

圖3 單目和雙目視覺(jué)理論系統(tǒng)Fig.3 The monocular and binocular vision theory system

從圖3(a)可以看出：采用單目視覺(jué)系統(tǒng)時(shí),射線CP上任一點(diǎn)Q在坐標(biāo)平面與空間P的成像坐標(biāo)重合。而在雙目視覺(jué)系統(tǒng)中,如圖3(b)所示,通過(guò)兩個(gè)攝像頭采集到的圖像信息,利用三角測(cè)量算法可以求出P的空間坐標(biāo)。

2.2 雙目視覺(jué)系統(tǒng)相機(jī)標(biāo)定

CCD攝像機(jī)將目標(biāo)物體的三維場(chǎng)景投射到二維平面,左右成像平面的圖像原點(diǎn)都與圖像的左上角重合,這個(gè)二維平面則可采用成像變換進(jìn)行表示。CCD攝像機(jī)包括線性模型和非線性模型兩種成像模型,本文研究的農(nóng)用無(wú)人機(jī)雙目攝像頭標(biāo)定模型中,采用線性模型進(jìn)行分析和研究。三維空間物體成像示意如圖4所示。三維空間物體成像主要包括以下4個(gè)坐標(biāo)系：①CCD攝像機(jī)坐標(biāo)系OCXCYCZC;②世界坐標(biāo)系(XW,YW,ZW);③成像坐標(biāo)系(x,y,z);④圖像坐標(biāo)系(u,v)。

圖4 三維空間物體成像示意圖Fig.4 Imaging sketch of three-dimensional space object

成像和圖像兩個(gè)坐標(biāo)系之間的齊次坐標(biāo)轉(zhuǎn)換為

(4)

其中,u0、v0是圖像坐標(biāo)系上一點(diǎn),du、dv是該點(diǎn)在x與y軸上映射值,s為射線光軸與坐標(biāo)的傾斜因子。

根據(jù)式(4),可以得到空間坐標(biāo)一點(diǎn)P與圖像像素坐標(biāo)系中像素點(diǎn)p的變換關(guān)系,即

(5)

其中,fx=fsx、fy=fsy為在x、y軸上的等效焦距;利用小孔成像,式(4)和式(5)可以轉(zhuǎn)換為

(6)

R=

(7)

T=(tx,ty,tz)T

(8)

其中,ψ為側(cè)傾角,θ為俯仰角,φ為旋轉(zhuǎn)角,向量R為正交旋轉(zhuǎn)矩陣,向量T=[tx,ty,tz]T為世界坐標(biāo)系原點(diǎn)在圖像坐標(biāo)系的映射點(diǎn)。

3 農(nóng)用無(wú)人機(jī)嵌入式系統(tǒng)

3.1 農(nóng)用無(wú)人機(jī)硬件設(shè)計(jì)

嵌入式系統(tǒng)硬件設(shè)計(jì)是整個(gè)農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航的基礎(chǔ),本文采用ARM+DSP的雙處理器架構(gòu)設(shè)計(jì)硬件平臺(tái),主要包括圖像采集、圖像處理和飛行控制等3個(gè)子系統(tǒng)。

圖像采集子系統(tǒng)主要負(fù)責(zé)采集飛行環(huán)境周邊的圖像數(shù)據(jù),并將采集到的圖像信息傳輸給圖像處理子系統(tǒng);圖像處理子系統(tǒng)是整個(gè)系統(tǒng)的核心,主要負(fù)責(zé)圖像處理、飛行規(guī)劃和控制指令的輸出。圖像處理系統(tǒng)反饋的飛行規(guī)劃和控制指令會(huì)傳遞給飛行控制模塊,農(nóng)用無(wú)人機(jī)會(huì)根據(jù)該結(jié)果調(diào)整4個(gè)電機(jī)旋轉(zhuǎn)方向和轉(zhuǎn)速,實(shí)現(xiàn)飛行姿態(tài)的轉(zhuǎn)變,從而順利抵達(dá)目標(biāo)地點(diǎn)。農(nóng)用無(wú)人機(jī)硬件框架如圖5所示。

圖5 農(nóng)用無(wú)人機(jī)硬件框架Fig.5 The hardware framework of agricultural UAV

1)圖像采集子系統(tǒng)。圖像采集子系統(tǒng)由CCD工業(yè)相機(jī)和視頻解碼器構(gòu)成：CCD工業(yè)相機(jī)主要負(fù)責(zé)獲取目標(biāo)物體的圖像信息;視頻解碼器負(fù)責(zé)將CCD工業(yè)相機(jī)采集到的圖像信息進(jìn)行解析,并傳送給DSP處理器。圖像采集子系統(tǒng)如圖6所示。

圖6 圖像采集子系統(tǒng)Fig.6 Image acquisition subsystem

2)圖像處理子系統(tǒng)。圖像處理子系統(tǒng)由DSP模塊和嵌入式控制器構(gòu)成：DSP模塊主要負(fù)責(zé)將獲取的目標(biāo)物體圖像信息進(jìn)行解碼和轉(zhuǎn)換,并將處理后的信息以控制指令的方式發(fā)送給嵌入式控制器;嵌入式控制器則將收到的指令信息轉(zhuǎn)換成PWM,驅(qū)動(dòng)4個(gè)電機(jī)的旋轉(zhuǎn)方向和轉(zhuǎn)速。圖像處理子系統(tǒng)如圖7所示。

圖7 圖像處理子系統(tǒng)Fig.7 Image Processing Subsystem

3.2 農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法設(shè)計(jì)

農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法采用人工勢(shì)場(chǎng)法,思路是：在視覺(jué)采集障礙物和目標(biāo)物體的基礎(chǔ)上,建立一個(gè)人工勢(shì)場(chǎng),使得無(wú)人機(jī)和障礙物之間的距離存在排斥力,與目標(biāo)物體存在吸引力,然后根據(jù)勢(shì)函數(shù)的下降方向進(jìn)行避障路徑的規(guī)劃。人工勢(shì)場(chǎng)的計(jì)算方程式為

(9)

(10)

人工勢(shì)場(chǎng)的負(fù)梯度值隨著農(nóng)用無(wú)人機(jī)和目標(biāo)間靠近而減小,直至0。

本文研究的內(nèi)容是農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法,目標(biāo)為動(dòng)態(tài),障礙物相對(duì)來(lái)說(shuō)也為動(dòng)態(tài)。農(nóng)用無(wú)人機(jī)勢(shì)場(chǎng)引力如圖8所示。

圖8 人工勢(shì)場(chǎng)的勢(shì)場(chǎng)引力Fig.8 The potential gravity of artificial potential field

因此,傳統(tǒng)的人工勢(shì)場(chǎng)需要進(jìn)行適當(dāng)改進(jìn),定義為

Uat(P,V)=ξp‖Pgoal(t)-P(t)‖m+
lv‖Vtar(t)-V(t)‖n

(11)

其中,Vtar(t)和V(t)分別為障礙物和農(nóng)用無(wú)人機(jī)在t時(shí)間點(diǎn)的移動(dòng)速度;ξp、lv為正參數(shù);m、n為人工勢(shì)場(chǎng)系數(shù)。人工勢(shì)場(chǎng)的負(fù)梯度函數(shù)表達(dá)式為

Fat(p,v)=-Uat(p,v)=

-pUat(p,v)-vUat(p,v)

(12)

聯(lián)立求解后可得

Fat(p,v)=Fat1(p)+Fat2(v)

(13)

其中

Fat1(p)=kξp‖Ptar(t)-p(t)‖mNprt

(14)

Fat2(v)=flv‖Vtar(t)-V(t)‖nNvrt

(15)

其中,Nprt為農(nóng)用無(wú)人機(jī)指向移動(dòng)障礙物的單位矢量,Nvrt為農(nóng)用無(wú)人機(jī)與移動(dòng)障礙物相對(duì)速度的單位矢量。

4 試驗(yàn)與分析

為了驗(yàn)證農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法的可行性和準(zhǔn)確性,采用MatLab仿真軟件對(duì)上述算法進(jìn)行了仿真,分別驗(yàn)證在無(wú)障礙物和有障礙物下,農(nóng)用無(wú)人機(jī)是否可以順利的達(dá)到目的地。這無(wú)人機(jī)的初始位置為(0,0),目標(biāo)地點(diǎn)為(50,50)。MatLab仿真結(jié)果如圖9所示。

圖9 MatLab仿真結(jié)果圖Fig.9 Simulation results by MatLab

由圖9可以看出：農(nóng)用無(wú)人機(jī)在無(wú)障礙物和有障礙物下,能夠利用視覺(jué)導(dǎo)航算法,準(zhǔn)確進(jìn)行避障和導(dǎo)航,可以安全達(dá)到目的地;仿真結(jié)果與預(yù)期效果一致,驗(yàn)證了農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法的可行性和有效性。

5 結(jié)論

結(jié)合圖像處理、DSP和嵌入式控制系統(tǒng),設(shè)計(jì)了一套農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法,實(shí)現(xiàn)了農(nóng)業(yè)無(wú)人機(jī)的避障與導(dǎo)航。MatLab仿真結(jié)果表明：農(nóng)用無(wú)人機(jī)在無(wú)障礙物和有障礙物下,能夠利用視覺(jué)導(dǎo)航算法,準(zhǔn)確進(jìn)行避障和導(dǎo)航,且仿真結(jié)果與預(yù)期效果一致,驗(yàn)證了農(nóng)用無(wú)人機(jī)視覺(jué)導(dǎo)航算法的可行性和有效性。