馬耀鋒,李紅麗

(鄭州工程技術(shù)學(xué)院,鄭州 450044)

0 引言

隨著社會經(jīng)濟(jì)和無線通信、嵌入式、機(jī)械等技術(shù)的快速發(fā)展,研究人員研究出了各類機(jī)器人替代人類完成繁瑣和危險(xiǎn)的工作,不僅降低了人類作業(yè)風(fēng)險(xiǎn),還大大提高了工作效率。農(nóng)業(yè)機(jī)器人是一種集外界環(huán)境感知、定位和行走規(guī)劃于一體的綜合系統(tǒng),被廣泛應(yīng)用于各種農(nóng)業(yè)生產(chǎn)中。農(nóng)業(yè)機(jī)器人的工作環(huán)境復(fù)雜、多變,因此需要其具備較高的環(huán)境識別、定位和避障的能力。農(nóng)業(yè)機(jī)器人定位和避障往往是研究中的重難點(diǎn)問題,本文提出了一種基于雙目視覺的農(nóng)業(yè)機(jī)器人運(yùn)動定位和避障系統(tǒng),可通過雙目視覺采集農(nóng)業(yè)機(jī)器人周邊的環(huán)境信息,實(shí)現(xiàn)農(nóng)業(yè)機(jī)器人運(yùn)動定位和避障。

1 雙目視覺定位原理

1.1 雙目系統(tǒng)定位原理

雙目視覺系統(tǒng)根據(jù)人眼立體成像原理,利用兩個攝像機(jī)從不同的角度形成兩幅圖像,而同一點(diǎn)在兩幅圖像中會形成非常明顯的視覺差異,根據(jù)這些差異,利用三角測量法,可以計(jì)算出目標(biāo)點(diǎn)的三維立體信息。雙目視覺系統(tǒng)成像示意圖如圖1所示。

圖1 雙目視覺系統(tǒng)成像示意圖

從圖1可以看出:兩個攝像機(jī)距離為B,水平放置,相機(jī)焦距都為f,空間上P點(diǎn)在左右相機(jī)成像分別為P1(Xleft,Yleft)和P2(Xright,Yright)。由于兩個相機(jī)水平放置,目標(biāo)物體在兩幅圖像中的Y坐標(biāo)相同,因此只需要求出X坐標(biāo),便可以得到P點(diǎn)在空間上的三維坐標(biāo)。雙目系統(tǒng)視差測量示意如圖2所示。

圖2 雙目系統(tǒng)視差測量示意圖

圖2中,CL和CR分別為兩個攝像機(jī)的光心,相機(jī)焦距為f,P點(diǎn)在左右相機(jī)成像分別為PL(xL,yL)和PR(xR,yR),ALPL=XL,ARPR=XR,PRB=a,由幾何中三角相似定理可知

(1)

(2)

聯(lián)合式(1)和式(2)可得

(3)

另外,可以得出d的值,即

(4)

雙目視覺系統(tǒng)三維成像坐標(biāo)計(jì)算如圖3所示。

圖3 雙目視覺系統(tǒng)三維成像坐標(biāo)計(jì)算示意圖

假設(shè)P(X,Y,Z),在左右圖像平面坐標(biāo)中成像分別為(xL,y)和(xR,y),利用前面介紹的幾何三角相似定理,可以計(jì)算出P在空間坐標(biāo)系中的坐標(biāo),即

(5)

(6)

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

本文研究的農(nóng)業(yè)機(jī)器人將雙目攝像機(jī)安裝在車身本體,攝像機(jī)跟隨著機(jī)器人一起運(yùn)動,可以較好地捕捉到工作場地周邊環(huán)境狀況,實(shí)現(xiàn)機(jī)器人的定位和避障。農(nóng)業(yè)機(jī)器人結(jié)構(gòu)簡單示意圖如圖4所示。

圖4 農(nóng)業(yè)機(jī)器人結(jié)構(gòu)簡單示意圖

雙目視覺系統(tǒng)標(biāo)定是實(shí)現(xiàn)目標(biāo)物體三維坐標(biāo)計(jì)算的前提,其準(zhǔn)確率直接影響立體視覺精度。本文研究的雙目視覺系統(tǒng)兩個攝像機(jī)位置和方向跟隨著采摘機(jī)器人變化,視覺參數(shù)相對穩(wěn)定。根據(jù)前文雙目系統(tǒng)測距原理介紹及圖2,空間P(Xw,Yw,Zw)點(diǎn)在兩幅圖像中的坐標(biāo)分別為P1(u1,v1)和P2(u1,v1),可以得到左右相機(jī)獲取P點(diǎn)圖像的模型為

(7)

(8)

(9)

(10)

其中,旋轉(zhuǎn)和平移矩陣分別為R和T,其分解后為

(11)

式(11)給出了兩個相機(jī)在方位上的關(guān)系,可以參考棋盤標(biāo)定的方法,建立相機(jī)內(nèi)外參數(shù)的方程,從而完成相機(jī)的標(biāo)定。

2 農(nóng)業(yè)機(jī)器人避障

2.1 障礙物特征提取

在對目標(biāo)障礙物提供特征時,農(nóng)業(yè)機(jī)器人視覺邊界采用直線模型,主要是采用Hough變換進(jìn)行特征提取。Hough變換采用以下兩種方法實(shí)現(xiàn)障礙物的特征提取:

1)角度約束關(guān)系。Hough變換會根據(jù)障礙物特征分布情況對其進(jìn)行檢測,該方法在判定障礙物特征時會對其最大特征進(jìn)行查詢,其一般要求臨近的邊界滿足以下角度約束關(guān)系,即

(12)

其中,σleft和σright為障礙物特征左右界限角度值,使用該式可以除去沒有作用的特征。

2)障礙物邊界特征提取。本文采用消失點(diǎn)分析hough算法檢測到的結(jié)果,從而避免Hough算法的“虛峰”和“漏檢”狀況的出現(xiàn),消失點(diǎn)指檢測到障礙物特征兩側(cè)邊界相交處。

障礙物邊界特征提取的主要步驟為:

1)結(jié)合角度約束表達(dá)式,利用Hough變換在圖像左右邊界獲取P組的邊界線;

2)獲得P×P個邊界線交點(diǎn);

3)根據(jù)交點(diǎn)位置,計(jì)算出障礙物邊界線,防止“虛峰”和“漏檢”狀況的出現(xiàn),提交障礙物計(jì)算精度。障礙物邊界特征提取簡圖如圖5所示。

圖5 障礙物邊界特征提取簡圖

2.2 求解避障最優(yōu)路徑

根據(jù)Hough算法提取到障礙物特征后,可以引入約束條件,融入A*(A-Star)算法可以讓農(nóng)業(yè)機(jī)器人在避障過程中獲取最優(yōu)路徑。A*算法是結(jié)合Dijkstra算法和估計(jì)函數(shù),根據(jù)計(jì)算出的估計(jì)值確定最優(yōu)路徑預(yù)備節(jié)點(diǎn),可提高求解效率。算法的估計(jì)函數(shù)為

f(p)=g(p)+h(p)

(13)

其中,g(p)為農(nóng)業(yè)機(jī)器人出發(fā)點(diǎn)到p點(diǎn)的代價,h(p)為當(dāng)前預(yù)備節(jié)點(diǎn)到期望節(jié)點(diǎn)的代價,A*算法根據(jù)極小節(jié)點(diǎn)作為最優(yōu)路徑節(jié)點(diǎn)。

A*算法求解避障最優(yōu)路徑的主要流程為:

1)構(gòu)建OPEN和CLOSE的節(jié)點(diǎn)表單。其中,OPEN為暫未進(jìn)行代價估計(jì)的節(jié)點(diǎn),CLOSE為已經(jīng)完成代價估計(jì)的節(jié)點(diǎn)。

2)采用廣度搜索原理,設(shè)置農(nóng)業(yè)機(jī)器人起點(diǎn),并保存至OPEN節(jié)點(diǎn)。

3)將OPEN節(jié)點(diǎn)中的第一個節(jié)點(diǎn),保存到CLOSE節(jié)點(diǎn)表單中,根據(jù)估計(jì)值,查詢和確定所有的預(yù)備節(jié)點(diǎn),保存于OPEN節(jié)點(diǎn)表單,并利用估計(jì)函數(shù)對所有節(jié)點(diǎn)進(jìn)行代價計(jì)算和排序。

4)重復(fù)步驟3),直到農(nóng)業(yè)機(jī)器人達(dá)到目標(biāo)地點(diǎn)。

3 農(nóng)業(yè)機(jī)器人避障的實(shí)現(xiàn)

3.1 計(jì)算目標(biāo)避障角度

根據(jù)雙目視覺系統(tǒng)獲取障礙物的位置后,便可以調(diào)整農(nóng)業(yè)機(jī)器人的移動方向,從而實(shí)現(xiàn)避障。農(nóng)業(yè)機(jī)器人避障模型如圖6所示。

圖6 農(nóng)業(yè)機(jī)器人避障模型圖

圖6中,XOY為大地坐標(biāo)系,P(x,y)為目標(biāo)路徑上一點(diǎn),R為軌道半徑,L為農(nóng)業(yè)機(jī)器人前輪距離。避障模型目標(biāo)角度會根據(jù)運(yùn)動學(xué)方程和農(nóng)業(yè)機(jī)器人的幾何關(guān)系確定,重點(diǎn)是確定在當(dāng)前路徑需要前進(jìn)多少。由此得到農(nóng)業(yè)機(jī)器人避障模型表達(dá)式,即

(14)

農(nóng)業(yè)機(jī)器人兩個運(yùn)動輪子的運(yùn)動學(xué)表達(dá)式為

(15)

因此,在避障過程中,農(nóng)業(yè)機(jī)器人的目標(biāo)角度為

(16)

其中,δe為農(nóng)業(yè)機(jī)器人目標(biāo)角度,αc為角度偏差值,dc為橫向位置偏差。確定這些參數(shù)以后,便可以計(jì)算出農(nóng)業(yè)機(jī)器人方向盤轉(zhuǎn)動角度,從而可以實(shí)現(xiàn)對農(nóng)業(yè)機(jī)器人轉(zhuǎn)向的控制。

3.2 農(nóng)業(yè)機(jī)器人車輪控制

農(nóng)業(yè)機(jī)器人車輪控制采用PID閉環(huán)控制,控制器根據(jù)目標(biāo)路徑、避障角度和采樣周期T計(jì)算出偏差。離散化PID控制表達(dá)式為

(17)

其中,u(k)為農(nóng)業(yè)機(jī)器人控制器輸出,Kp為比率系數(shù),TD為微分時間常數(shù),TI積分時間常數(shù)。PID控制增量為

Δu(k)=u(k)-u(k-1)

(18)

農(nóng)業(yè)機(jī)器人車輪PID控制程序如圖7所示。

圖7 農(nóng)業(yè)機(jī)器人車輪PID控制程序

由圖7可以看出:農(nóng)業(yè)機(jī)器人車輪PID控制程序主要包括PID增量計(jì)算、PID輸出計(jì)算和控制判斷。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證農(nóng)業(yè)機(jī)器人運(yùn)動定位和避障的可行性,采用MatLab仿真軟件進(jìn)行了仿真與分析。農(nóng)業(yè)機(jī)器人在實(shí)際作業(yè)過程中,對自身定位和障礙物檢測是實(shí)時的,所以本文以前面5m范圍的障礙物對避障方位。試驗(yàn)仿真結(jié)果如圖8所示。

圖8 Matlab仿真結(jié)果圖

由圖8可以看出:農(nóng)業(yè)機(jī)器人的上方障礙物較多,因此機(jī)器人向右轉(zhuǎn)彎,計(jì)算到障礙物外切圓的圓心位置后,農(nóng)業(yè)機(jī)器人以起點(diǎn)、障礙物外切圓和終點(diǎn)的連接線作為農(nóng)業(yè)機(jī)器人的避障路線。實(shí)驗(yàn)結(jié)果表明:農(nóng)業(yè)機(jī)器人從起點(diǎn)成功到達(dá)終點(diǎn),算法路徑為最優(yōu)避障路徑,證明了系統(tǒng)的準(zhǔn)確性和可行性。

5結(jié)論

提出了一種基于雙目視覺的農(nóng)業(yè)機(jī)器人運(yùn)動定位和避障系統(tǒng)。該系統(tǒng)通過雙目視覺采集農(nóng)業(yè)機(jī)器人周邊的環(huán)境信息,運(yùn)用Hough變換提取障礙物特征信息,結(jié)合A*算法進(jìn)行避障最優(yōu)路徑的選擇,并采用PID閉環(huán)控制,實(shí)現(xiàn)對機(jī)器人方向盤的控制,實(shí)現(xiàn)了農(nóng)業(yè)機(jī)器人運(yùn)動定位和避障。實(shí)驗(yàn)結(jié)果表明:農(nóng)業(yè)機(jī)器人從起點(diǎn)成功到達(dá)終點(diǎn),算法路徑為最優(yōu)避障路徑,證明了系統(tǒng)的準(zhǔn)確性和可行性。