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

韓 璞,王耀寬,胡四平

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

0 引言

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

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

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

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

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

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

圖1 四旋翼無人機實物圖Fig.1 The physical chart of four-rotor UAV

1.2 農(nóng)用無人機工作原理

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

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

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

Ti：=CTρAriri2Ωi2

(1)

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

Ti：=cTΩi2

(2)

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

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

Qi：=cQΩi2

(3)

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

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

2 雙目視覺理論

2.1 雙目視覺成像原理

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

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

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

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

CCD攝像機將目標(biāo)物體的三維場景投射到二維平面,左右成像平面的圖像原點都與圖像的左上角重合,這個二維平面則可采用成像變換進(jìn)行表示。CCD攝像機包括線性模型和非線性模型兩種成像模型,本文研究的農(nóng)用無人機雙目攝像頭標(biāo)定模型中,采用線性模型進(jìn)行分析和研究。三維空間物體成像示意如圖4所示。三維空間物體成像主要包括以下4個坐標(biāo)系：①CCD攝像機坐標(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

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

(4)

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

根據(jù)式(4),可以得到空間坐標(biāo)一點P與圖像像素坐標(biāo)系中像素點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)系原點在圖像坐標(biāo)系的映射點。

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

3.1 農(nóng)用無人機硬件設(shè)計

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

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

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

1)圖像采集子系統(tǒng)。圖像采集子系統(tǒng)由CCD工業(yè)相機和視頻解碼器構(gòu)成：CCD工業(yè)相機主要負(fù)責(zé)獲取目標(biāo)物體的圖像信息;視頻解碼器負(fù)責(zé)將CCD工業(yè)相機采集到的圖像信息進(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ū)動4個電機的旋轉(zhuǎn)方向和轉(zhuǎn)速。圖像處理子系統(tǒng)如圖7所示。

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

3.2 農(nóng)用無人機視覺導(dǎo)航算法設(shè)計

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

(9)

(10)

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

本文研究的內(nèi)容是農(nóng)用無人機視覺導(dǎo)航算法,目標(biāo)為動態(tài),障礙物相對來說也為動態(tài)。農(nóng)用無人機勢場引力如圖8所示。

圖8 人工勢場的勢場引力Fig.8 The potential gravity of artificial potential field

因此,傳統(tǒ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)用無人機在t時間點的移動速度;ξp、lv為正參數(shù);m、n為人工勢場系數(shù)。人工勢場的負(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)用無人機指向移動障礙物的單位矢量,Nvrt為農(nóng)用無人機與移動障礙物相對速度的單位矢量。

4 試驗與分析

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

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

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

5 結(jié)論

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