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

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

0 引言

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

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

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

1.1 硬件構(gòu)建

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

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

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

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

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

1.2 軟件構(gòu)建

1.2.1 特征識(shí)別方法

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

(a) 輸入圖像

(1)

1.2.2 點(diǎn)云提取方法

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

圖4 目標(biāo)點(diǎn)云示意圖Fig.4 Schematic diagram of the target point cloud

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

(2)

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

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

(3)

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

(4)

然后求出平均距離,由于本文特征為直線,平均距離趨近于0,故選擇小于閾值且與實(shí)際目標(biāo)長(zhǎng)度最接近的點(diǎn)云類,最終提取出目標(biāo)點(diǎn)云集Gt。

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

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

圖5 機(jī)器人相對(duì)位置示意圖Fig.5 Schematic diagram of the robots relative position

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

(5)

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

d=h+lAsinα

(6)

(7)

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

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

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

(8)

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

(9)

(10)

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

1.2.4 跟隨控制方法

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

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

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

2.1 檢測(cè)標(biāo)定試驗(yàn)

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

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

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

2.2 算法對(duì)比試驗(yàn)

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

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

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

2.3 跟隨性能試驗(yàn)

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

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

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

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

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

2.4 停車定位試驗(yàn)

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

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

3 結(jié)論

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

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

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

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