趙鋮鑰，馬偉，蘇道畢力格，譚彧

(1.中國農(nóng)業(yè)大學(xué)工學(xué)院,北京市,100083;2.中國農(nóng)業(yè)科學(xué)院都市農(nóng)業(yè)研究所,成都市,610213)

0 引言

根據(jù)WCO最新統(tǒng)計數(shù)據(jù),2021年全球柑橘總產(chǎn)量達15 850萬噸,而我國達到了4 460萬噸,居世界首位,且近幾年產(chǎn)能還在逐步擴大[1-2]。然而,與之配套的柑橘收獲裝備短板問題突出。目前我國柑橘收獲時普遍采用人力勞動,但是大量農(nóng)村人員涌入城市,造成了柑橘采收獲季節(jié)的勞動力短缺,制約著我國柑橘產(chǎn)業(yè)的發(fā)展[3]。要夯實柑橘產(chǎn)業(yè)發(fā)展基礎(chǔ),就必須提高我國柑橘收獲機械化和自動化水平,而果園機械自動導(dǎo)航技術(shù)是實現(xiàn)機械化和自動化的關(guān)鍵,是目前研究的熱點。目前,果園機械自動導(dǎo)航技術(shù)的研究多集中在單一功能自動化裝備關(guān)鍵技術(shù)研究,主要導(dǎo)航方式有GPS導(dǎo)航、機器視覺導(dǎo)航、激光雷達導(dǎo)航以及多傳感器融合導(dǎo)航等[4-8]。但由于果園環(huán)境的復(fù)雜程度較高,GPS信號易受到高山、樹冠的遮擋產(chǎn)生誤差,激光雷達和視覺容易受到環(huán)境中雜草等因素的影響丟失道路特征信息,導(dǎo)致導(dǎo)航信息失效[9];而機器人引導(dǎo)與自主跟隨導(dǎo)航可以降低環(huán)境因素的影響,提高導(dǎo)航穩(wěn)定性、可靠性,還可以滿足運輸機器人跟隨采摘機器人前進的協(xié)同工作模式。目前國內(nèi)外農(nóng)業(yè)領(lǐng)域的跟隨導(dǎo)航研究主要集中在農(nóng)田環(huán)境中[10-13],在果園環(huán)境中的研究比較少。丁勇前等[14]設(shè)計一種基于紅外傳感器的車輛自主跟隨控制系統(tǒng),使用陣列紅外測距傳感器,獲取引導(dǎo)車和跟隨車之間的相對航向偏角,通過控制前輪轉(zhuǎn)向角跟隨前進,經(jīng)過試驗表明該系統(tǒng)能實現(xiàn)車輛的自主跟隨,系統(tǒng)運行穩(wěn)定可靠;畢偉平等[15]設(shè)計一種基于雙目視覺的果園作業(yè)車輛跟隨系統(tǒng),使用黑白棋格特征板作為引導(dǎo)特征,跟隨機器人通過雙目視覺識別黑白棋格獲取引導(dǎo)和跟隨車輛之間的相對位姿信息,最終可以實現(xiàn)車輛自主跟隨。這些為果園環(huán)境中的運輸機器人跟隨導(dǎo)航提供了研究思路。

視覺可以通過指定顏色將引導(dǎo)目標與環(huán)境進行明顯區(qū)分,二維激光雷達可以提取出目標距離、航向偏角等多種信息,且提取算法易于實現(xiàn),二者融合可以提高跟隨系統(tǒng)的精準性和穩(wěn)定性。針對柑橘采摘時的運輸工作,本文搭建兩個履帶式機器人,采摘機器人作為引導(dǎo)機器人,由操作員駕駛,在機器人后方固定一個紅色矩形特征板;運輸機器人作為跟隨機器人,通過搭載的二維激光雷達和視覺傳感器,獲取引導(dǎo)機器人特征板的二維信息,計算出二者的相對位姿,通過控制算法使跟隨機器人進行自主跟隨導(dǎo)航。在模擬環(huán)境中,對不同運行速度和軌跡情況,對建立的機器人跟隨系統(tǒng)進行跟隨穩(wěn)定性試驗和行間跟隨停車定位精準性試驗。

1 系統(tǒng)構(gòu)建

1.1 硬件構(gòu)建

跟隨機器人系統(tǒng)包括激光雷達視覺系統(tǒng)與機器人平臺,激光雷達視覺系統(tǒng)主要由2D LiDAR、攝像頭、裝有Ubuntu18.04操作系統(tǒng)的工控機以及機器人平臺組成。2D LiDAR為一款單線二維激光雷達,安裝在移動機器人正中心,其掃描角度為360°,在幀率為8 Hz時水平角分辨率為0.96°,點頻率為8 kHz,最大測距為12 m,測距精度為0.02 m;視覺傳感器為高清免驅(qū)攝像頭,安裝在激光雷達正前下方,其分辨率設(shè)置為640×480,幀率為30幀/s,視場角為90°。

移動機器人以STM32F1為主控板,主進程通過UART總線接收激光雷達視覺系統(tǒng)傳輸?shù)乃俣瓤刂菩畔?從進程通過讀取編碼器獲得兩側(cè)履帶的實際前進速度,并輸出兩側(cè)電機的控制信號,實現(xiàn)機器人前進與差速轉(zhuǎn)向。圖1為跟隨機器人系統(tǒng)硬件平臺。

圖1 跟隨機器人系統(tǒng)硬件平臺Fig.1 Hardware platform of following robot system1.移動機器人 2.顯示器 3.工控機 4.2D LiDAR 5.攝像頭

引導(dǎo)機器人的標志物為一紅色矩形特征板,垂直放置在引導(dǎo)機器人后方,用于引導(dǎo)跟隨機器人,引導(dǎo)機器人由操作者遙控前進,引導(dǎo)機器人與跟隨機器人構(gòu)成的系統(tǒng)如圖2所示。

圖2 引導(dǎo)跟隨機器人系統(tǒng)Fig.2 Guided-following robot system

1.2 軟件構(gòu)建

1.2.1 特征識別方法

本文選取紅色矩形板作為引導(dǎo)目標,如圖3(a)所示。通過與環(huán)境顏色進行區(qū)分,來確定目標在視覺圖像中的位置,獲取引導(dǎo)機器人在跟隨機器人坐標系下的角度范圍。本文將彩色圖像從RGB(Red,Green and Blue)空間轉(zhuǎn)換到HSV(Hue,Saturation and Value)顏色空間中,分別調(diào)節(jié)H、S、V三個分量的閾值,獲得二值圖像,如圖3(b)所示。其中目標的白色像素點最多,記錄相連區(qū)域的白色像素點,數(shù)量最多的為興趣區(qū)域(ROI),并使用最大矩形在圖像中框選出興趣區(qū)域(ROI),如圖3(c)所示。矩形框含有左右兩側(cè)邊框像素橫坐標信息x1,x2,然后通過計算獲取目標方向,其方向計算示意圖如圖3(d)所示。

(a) 輸入圖像

(1)

1.2.2 點云提取方法

目標點云提取的示意圖如圖4所示。設(shè)置跟隨機器人局部坐標系的原點與激光雷達中心重合,x軸為前進方向,y軸垂直前進方向向左。獲取與圖像相同時刻的激光雷達數(shù)據(jù)并選取正前方180°的數(shù)據(jù)點,根據(jù)圖像識別角度范圍提取激光點云。該范圍的點云中包含目標點云和無效點云,需要進行聚類以及特征提取,獲得目標點云信息。

圖4 目標點云示意圖Fig.4 Schematic diagram of the target point cloud

由于二維激光雷達是單線掃描式,激光數(shù)據(jù)點Pi以極坐標形式(li,θi)存儲,對點云遍歷并計數(shù),通過距離公式計算相鄰兩點距離di,如式(2)所示。

(2)

當相鄰點間距離超過設(shè)置閾值時,判斷點集計數(shù)值k,當大于指定數(shù)量時就保存到點云類列表G,直至遍歷完成。

通過坐標轉(zhuǎn)換公式將極坐標轉(zhuǎn)換到笛卡爾坐標系下,如式(3)所示。

(3)

根據(jù)每個點云類的兩側(cè)端點坐標(xl,yl)(xr,yr)求解出直線方程Ax+By+C=0的各個系數(shù),并根據(jù)式(2)計算出點云邊界長度li,然后根據(jù)點到直線距離公式,計算點云類中每個點到該直線的距離Di,如式(4)所示。

(4)

然后求出平均距離,由于本文特征為直線,平均距離趨近于0,故選擇小于閾值且與實際目標長度最接近的點云類,最終提取出目標點云集Gt。

1.2.3 導(dǎo)航計算方法

如圖5所示,引導(dǎo)機器人中心O為原點,跟隨機器人中心O′為原點,與激光雷達中心重合。通過獲取的目標點云集Gt提取出兩側(cè)邊緣點的距離值lA、lB,角度值θA、θB,然后通過計算得到兩機器人的縱向距離d、橫向偏差f以及航向角偏差θ三個導(dǎo)航參數(shù)。

圖5 機器人相對位置示意圖Fig.5 Schematic diagram of the robots relative position

由于激光雷達無法精準測得矩形板的兩側(cè)端點,故其長度l通過激光雷達獲得的端點計算獲得,激光雷達相鄰點間隔角度較小,其產(chǎn)生的誤差在本文中可以進行忽略,得到l的計算公式,如式(5)所示。

(5)

跟隨機器人與引導(dǎo)機器人的縱向距離d的計算公式,如式(6)、式(7)所示。

d=h+lAsinα

(6)

(7)

式中：α——點云Gt左側(cè)邊緣點線束與矩形板夾角,(°);

h——引導(dǎo)機器人中心到矩形板的距離,m。

跟隨機器人與引導(dǎo)機器人的橫向偏差f的計算公式,如式(8)所示。

(8)

跟隨機器人與引導(dǎo)機器人的航向角偏差θ的計算公式,如式(9)、式(10)所示。

(9)

(10)

式中：β——點云Gt右側(cè)邊緣點線束與矩形板夾角,(°)。

1.2.4 跟隨控制方法

跟隨機器人的控制框圖如圖6所示,其前進控制使用測得的縱向間距d和期望間距之間的誤差作為輸入,輸出跟隨線速度v,控制機器人前進;轉(zhuǎn)向控制根據(jù)測得的航向角偏差θ和橫向偏差f與期望偏差之間的誤差作為輸入,輸出跟隨機器人的角速度ωc,控制機器人轉(zhuǎn)向;然后進行差速模型運動學(xué)逆向運算,獲取兩側(cè)履帶的目標線速度vl和vr,以編碼器返回的線速度作為反饋,控制輸出兩側(cè)履帶電機的PWM電壓信號,最終實現(xiàn)機器人的運動調(diào)整。

圖6 跟隨機器人控制框圖Fig.6 Block diagram of follow-control rob

2 試驗結(jié)果與分析

2.1 檢測標定試驗

將引導(dǎo)機器人放置在跟隨機器人前方0°,±20°航向,兩者直線距離為1.2 m,然后對引導(dǎo)機器人的航向進行0°,±20°旋轉(zhuǎn),產(chǎn)生9種相對位置情況,每種情況下采集10次數(shù)據(jù),通過檢測識別得到跟隨標志的激光數(shù)據(jù)。

圖7為跟隨機器人±20°航向的檢測結(jié)果,根據(jù)式(6)、式(8)、式(9)獲得兩者的縱橫向偏差和航向角偏差,通過與擺放位置實際值相減計算三者的平均偏差,其中平均縱向偏差為0.012 m,標準差0.031 m,平均橫向偏差0.015 m,標準差0.022 m,平均航向角偏差0.18°,標準差0.54°,因此本文提出的檢測方法具有較高精度,可以滿足跟隨要求。

(a) -20°航向、-20°姿態(tài) (b) -20°航向、+20°姿態(tài)

2.2 算法對比試驗

對比引導(dǎo)機器人在0.3 m/s的運行速度下分別以直線和90°圓弧軌跡前進時,跟隨機器人應(yīng)用幾種傳統(tǒng)控制算法的跟隨性能。設(shè)定軌跡長度為5 m,目標縱向距離1.2 m,目標橫向偏差0 m,目標航向角偏差0°,跟隨機器人以8 Hz的頻率返回獲取的導(dǎo)航參數(shù),并計算偏差結(jié)果,試驗結(jié)果見表1。

表1 不同控制算法對比Tab.1 Comparison of different control algorithms

從表1可以看出相對于單獨的模糊控制,PID算法和模糊PID算法對跟蹤性能均有提升,而PID算法和模糊PID算法在縱向、橫向以及航向角三者的平均偏差雖互有不同,但趨于近似,算法并未有顯著提升,由于PID算法參數(shù)的調(diào)節(jié)難度更低、使用更加方便,且控制結(jié)果基本滿足了跟隨要求,故選用PID算法作為跟隨系統(tǒng)的控制算法。

2.3 跟隨性能試驗

本試驗主要研究在不同運行速度和軌跡的情況下,運輸機器人以指定距離和航向角跟隨引導(dǎo)機器人的跟隨性能。引導(dǎo)機器人在0.3 m/s、0.4 m/s、0.5 m/s的三種運行速度下,分別以直線和90°圓弧(圓弧半徑1.5 m)軌跡行進,設(shè)定軌跡長度為5 m,運輸機器人自動跟隨引導(dǎo)機器人,目標縱向距離1.2 m,目標橫向偏差0 m,目標航向角偏差0°,跟隨機器人以8 Hz的頻率返回獲取的導(dǎo)航參數(shù),并計算偏差結(jié)果,表2為試驗的縱向偏差統(tǒng)計結(jié)果,表3為試驗的橫向偏差統(tǒng)計結(jié)果,表4位試驗的航向角偏差統(tǒng)計結(jié)果。

表2 縱向偏差試驗結(jié)果Tab.2 Test results of longitudinal deviation

表3 橫向偏差試驗結(jié)果Tab.3 Test results of lateral deviation

表4 航向角偏差試驗結(jié)果Tab.4 Test results of yaw deviation

由表2～表4可知,運行速度對跟隨性能的影響較小,可以通過PID算法的參數(shù)進行修正;跟隨機器人對于直線軌跡行駛跟隨性能較好,平均縱向偏差小于0.015 m,標準差小于0.08 m,平均橫向偏差小于0.01 m,標準差小于0.018 m,平均航向偏差小于1.107°,標準差小于1.665°;而跟隨90°弧線軌跡行駛時,縱向偏差小于0.078 m,平均橫向偏差在0.3 m左右,最大橫向偏差達到了0.56 m,平均航向角偏差在21°左右,最大航向角偏差達到了43°,雖然圓弧軌跡跟隨時橫向偏差和航向角偏差較大,但是并未丟失跟隨目標,滿足跟隨任務(wù)要求。

2.4 停車定位試驗

由于柑橘運輸機器人跟隨采摘機器人在行間工作時,采摘機器人需要停下進行采摘并把柑橘存放到運輸機器人的儲存?zhèn)}內(nèi),所以運輸機器人需要停止在采摘機器人機械臂投放范圍內(nèi)。本文設(shè)置引導(dǎo)機器人在0.3 m/s、0.4 m/s、0.5 m/s的速度下直線行駛,進行跟隨機器人停車定位試驗,試驗結(jié)果如表5所示?？梢钥闯鲭S著速度的上升,縱向偏差和橫向偏差的值都有所上升,在最大運行速度0.5 m/s下,其最大縱向偏差為0.205 m,最大橫向偏差為0.053 m,可以滿足實際使用。

表5 停車定位試驗結(jié)果Tab.5 Test results of position when stopping

3 結(jié)論

本文提出了一種柑橘運輸機器人目標檢測定位及協(xié)同導(dǎo)航方法,并在模擬環(huán)境下對該方法的工作質(zhì)量進行了驗證,該方法為柑橘自主運輸機械提供了技術(shù)支撐,為實際果園工作奠定基礎(chǔ)。

1) 通過視覺圖像HSV閾值分割融合激光點云聚類特征識別方法,提取目標引導(dǎo)標志物信息,實現(xiàn)相對位姿獲取。并對該方法進行了識別定位精度試驗,平均橫、縱向偏差均小于0.015 m,平均航向角偏差0.018°,證明該檢測方法具有較高精度。

2) 通過試驗對比不同跟隨控制算法,選擇了適合本系統(tǒng)的PID控制算法,該方法參數(shù)調(diào)節(jié)方便且滿足工作要求。并在不同速度、軌跡條件下對跟隨性能進行了試驗,試驗表明速度的升高對跟隨精度影響不大。在最大0.5 m/s的跟隨速度下,直線軌跡的平均橫、縱向偏差小于0.015 m,平均航向偏差1.107°,跟隨性能良好;弧線軌跡跟蹤偏差較大,平均橫向偏差在0.3 m左右,但能滿足跟隨任務(wù)要求。

3) 通過跟隨停車定位試驗,縱向偏差小于0.205 m,證明本系統(tǒng)在運輸時滿足存儲工作條件。