激光雷達與視覺融合的跟隨運輸機器人設計*

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

(1.中國農業大學工學院,北京市,100083;2.中國農業科學院都市農業研究所,成都市,610213)

0 引言

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

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

1 系統構建

1.1 硬件構建

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

移動機器人以STM32F1為主控板,主進程通過UART總線接收激光雷達視覺系統傳輸的速度控制信息;從進程通過讀取編碼器獲得兩側履帶的實際前進速度,并輸出兩側電機的控制信號,實現機器人前進與差速轉向。圖1為跟隨機器人系統硬件平臺。

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

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

圖2 引導跟隨機器人系統Fig.2 Guided-following robot system

1.2 軟件構建

1.2.1 特征識別方法

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

(a) 輸入圖像

(1)

1.2.2 點云提取方法

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

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

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

(2)

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

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

(3)

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

(4)

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

1.2.3 導航計算方法

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

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

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

(5)

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

d=h+lAsinα

(6)

(7)

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

h——引導機器人中心到矩形板的距離,m。

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

(8)

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

(9)

(10)

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

1.2.4 跟隨控制方法

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

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

2 試驗結果與分析

2.1 檢測標定試驗

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

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

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

2.2 算法對比試驗

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

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

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

2.3 跟隨性能試驗

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

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

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

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

由表2～表4可知,運行速度對跟隨性能的影響較小,可以通過PID算法的參數進行修正;跟隨機器人對于直線軌跡行駛跟隨性能較好,平均縱向偏差小于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°,雖然圓弧軌跡跟隨時橫向偏差和航向角偏差較大,但是并未丟失跟隨目標,滿足跟隨任務要求。

2.4 停車定位試驗

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

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

3 結論

本文提出了一種柑橘運輸機器人目標檢測定位及協同導航方法,并在模擬環境下對該方法的工作質量進行了驗證,該方法為柑橘自主運輸機械提供了技術支撐,為實際果園工作奠定基礎。

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

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

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