基于雙目視覺的農業機器人運動定位與避障研究

馬耀鋒,李紅麗

(鄭州工程技術學院,鄭州 450044)

0 引言

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

1 雙目視覺定位原理

1.1 雙目系統定位原理

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

圖1 雙目視覺系統成像示意圖

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

圖2 雙目系統視差測量示意圖

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

(1)

(2)

聯合式(1)和式(2)可得

(3)

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

(4)

雙目視覺系統三維成像坐標計算如圖3所示。

圖3 雙目視覺系統三維成像坐標計算示意圖

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

(5)

(6)

1.2 雙目視覺系統標定

本文研究的農業機器人將雙目攝像機安裝在車身本體,攝像機跟隨著機器人一起運動,可以較好地捕捉到工作場地周邊環境狀況,實現機器人的定位和避障。農業機器人結構簡單示意圖如圖4所示。

圖4 農業機器人結構簡單示意圖

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

(7)

(8)

(9)

(10)

其中,旋轉和平移矩陣分別為R和T,其分解后為

(11)

式(11)給出了兩個相機在方位上的關系,可以參考棋盤標定的方法,建立相機內外參數的方程,從而完成相機的標定。

2 農業機器人避障

2.1 障礙物特征提取

在對目標障礙物提供特征時,農業機器人視覺邊界采用直線模型,主要是采用Hough變換進行特征提取。Hough變換采用以下兩種方法實現障礙物的特征提取:

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

(12)

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

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

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

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

2)獲得P×P個邊界線交點;

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

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

2.2 求解避障最優路徑

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

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

(13)

其中,g(p)為農業機器人出發點到p點的代價,h(p)為當前預備節點到期望節點的代價,A*算法根據極小節點作為最優路徑節點。

A*算法求解避障最優路徑的主要流程為:

1)構建OPEN和CLOSE的節點表單。其中,OPEN為暫未進行代價估計的節點,CLOSE為已經完成代價估計的節點。

2)采用廣度搜索原理,設置農業機器人起點,并保存至OPEN節點。

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

4)重復步驟3),直到農業機器人達到目標地點。

3 農業機器人避障的實現

3.1 計算目標避障角度

根據雙目視覺系統獲取障礙物的位置后,便可以調整農業機器人的移動方向,從而實現避障。農業機器人避障模型如圖6所示。

圖6 農業機器人避障模型圖

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

(14)

農業機器人兩個運動輪子的運動學表達式為

(15)

因此,在避障過程中,農業機器人的目標角度為

(16)

其中,δe為農業機器人目標角度,αc為角度偏差值,dc為橫向位置偏差。確定這些參數以后,便可以計算出農業機器人方向盤轉動角度,從而可以實現對農業機器人轉向的控制。

3.2 農業機器人車輪控制

農業機器人車輪控制采用PID閉環控制,控制器根據目標路徑、避障角度和采樣周期T計算出偏差。離散化PID控制表達式為

(17)

其中,u(k)為農業機器人控制器輸出,Kp為比率系數,TD為微分時間常數,TI積分時間常數。PID控制增量為

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

(18)

農業機器人車輪PID控制程序如圖7所示。

圖7 農業機器人車輪PID控制程序

由圖7可以看出:農業機器人車輪PID控制程序主要包括PID增量計算、PID輸出計算和控制判斷。

4 實驗結果與分析

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

圖8 Matlab仿真結果圖

由圖8可以看出:農業機器人的上方障礙物較多,因此機器人向右轉彎,計算到障礙物外切圓的圓心位置后,農業機器人以起點、障礙物外切圓和終點的連接線作為農業機器人的避障路線。實驗結果表明:農業機器人從起點成功到達終點,算法路徑為最優避障路徑,證明了系統的準確性和可行性。

5結論

提出了一種基于雙目視覺的農業機器人運動定位和避障系統。該系統通過雙目視覺采集農業機器人周邊的環境信息,運用Hough變換提取障礙物特征信息,結合A*算法進行避障最優路徑的選擇,并采用PID閉環控制,實現對機器人方向盤的控制,實現了農業機器人運動定位和避障。實驗結果表明:農業機器人從起點成功到達終點,算法路徑為最優避障路徑,證明了系統的準確性和可行性。