王歡 徐秀進 王紅軍 溫志慶

摘要：針對采摘機器人自主行走導航過程中，難以準確定位其與果樹之間的相對位置，難以準確估計果樹樹干姿態(tài)的問題，提出基于雙目eye in hand系統(tǒng)的多角度樹干位姿估計方法。利用YOLOv5深度學習方法與半全局塊匹配算法識別樹干并生成局部點云；利用半徑濾波和體素濾波減少樹干點云數(shù)據(jù)；利用閉環(huán)式手眼標定方法對雙目eye in hand系統(tǒng)進行標定，并對同一樹干多角度相機位置的點云數(shù)據(jù)進行拼接；利用隨機抽樣一致（RANSAC）算法與無約束最小二乘法估計并優(yōu)化樹干的位置和姿態(tài)，獲取樹干的圓柱體參數(shù)。通過對30幅標定板圖像進行實驗，閉環(huán)式手眼標定方法的平均歐式誤差為3.717 7 mm；采用半徑濾波和體素濾波可減少98.470%的點云數(shù)據(jù)；采用RANSAC算法、圓柱體估計算法擬合樹干點云數(shù)據(jù)，得到圓柱體的半徑r = 41.277 1 mm，R_MAE= 2.571 56 mm，R_RMSE= 2.989 36 mm；無約束最小二乘法優(yōu)化后r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm。該文通過對雙目eye in hand系統(tǒng)進行標定，建立坐標系轉換關系，多角度采集環(huán)境信息，準確定位機器人與果樹之間的相對位置，估計果樹樹干的姿態(tài)。

關鍵詞：eye?in?hand手眼標定；局部立體匹配；樹干圓柱體估計；多角度點云拼接

中圖分類號：S225; TP391.41?????????文獻標志碼：A ???????文章編號：1674-2605（2023）05-0006-08

DOI：10.3969/j.issn.1674-2605.2023.05.006

Multi-angle Tree Trunk Pose Estimation Method Based on Binocular ??????Eye in Hand System

WANG Huan¹??XU Xiujin¹??WANG Hongjun¹??WEN Zhiqing²

（1.College of Engineering， South China Agricultural University， Guangzhou 510642， China

2.Intelligent Robot Engineering Research Center，?Ji Hua Laboratory， Foshan 528200， China）

Abstract：?In the process of autonomous walking and navigation for harvesting robots， it is difficult to accurately locate the relative position between them and the fruit tree， as well as to accurately estimate the posture of the fruit tree trunk. Based on the binocular eye in hand system， YOLOv5 deep learning method and classical stereo matching algorithm are applied to identify tree trunks and generate local point clouds. Radial filtering and voxel filtering are used to reduce the number of point clouds. A closed-loop hand eye calibration method is proposed to calibrate the system and concatenate point cloud data from multi angle camera positions on the same tree trunk， using the random sampling consensus （RANSAC） algorithm and unconstrained least squares method to estimate and optimize the trunk posture， and obtain the trunk cylinder parameters. Through repeated experiments on 30 sets of calibration board images， the average Euclidean error of the closed-loop hand eye calibration method is 3.717 7 mm; Reduce 98.470% of point cloud data by using radius filtering and voxel filtering; The RANSAC cylinder estimation algorithm fitting tree trunk point cloud data yields a cylinder radius of r = 41.277 1 mm，?R_MAE?= 2.571 56 mm，?R_RMSE?= 2.989 36 mm;?After unconstrained least squares optimization，?r = 39.402 8 mm，?R_MAE?= 1.989 55 mm，?R_RMSE?= 2.465 88 mm.?The closed-loop hand eye calibration method proposed in this article calibrates the eye in hand system， establishes coordinate system conversion relationships， collects environmental information from multiple angles， effectively and accurately locates the relative position between the robot and the fruit tree， and estimates the posture of the fruit tree trunk.

Keywords：eye in hand calibration; local stereo matching; estimation of tree trunk cylinder;?multi angle point cloud stitching

0 引言

在野外環(huán)境下，通過視覺方法對樹干進行精準識別和定位是采摘機器人自主行走的關鍵^[1-2]。精確確定采摘機器人與樹干的相對位置和距離，以及準確估計樹干的位置和姿態(tài)，可提升導航和采摘的定位精確度，提高采摘機器人的作業(yè)效率，降低作業(yè)成本^[3-5]。

近年來，樹干模型的三維重建和姿態(tài)估計一般采用激光雷達和圖像2種方式。基于激光點云的樹干模型三維重建以計算機視覺、計算機圖形學為理論基礎進行激光點云去噪以及樹干骨架重建，但由于激光點云數(shù)據(jù)采樣量較大，給分析和計算帶來巨大挑戰(zhàn)^[6-7]。通過圖像進行樹干模型的三維重建常采用雙目視覺、多目視覺、深度視覺等方案。如鄒湘軍團隊^[8-9]對荔枝視覺采摘機器人進行了大量地研究，利用雙目視覺定位方法估計荔枝定位誤差；LIN等^[10]提出基于深度相機與全卷積網(wǎng)絡（fully convolutional networks，F(xiàn)CN）深度學習的番石榴識別與姿態(tài)估計方法，但深度相機受光照強度影響較大；CHEN等^[11-12]提出基于四目系統(tǒng)的香蕉果軸三維重建與定位方法，但僅從2個角度采集環(huán)境中的圖像信息，圖像信息少且位置固定。OLOFSSON等^[13]改進基于隨機抽樣一致（random sample consensus， RANSAC）模型對二維圓的擬合算法，提取樹干胸徑值，但在樹干存在傾斜角度時，二維圓擬合誤差較大。吳杭彬等^[14]結合布料模擬濾波（cloth simulation filtering，?CSF）算法與RANSAC算法，有效濾除地面數(shù)據(jù)并擬合樹干模型，實現(xiàn)樹干胸徑大規(guī)模提取自動化，但利用激光掃描儀采集圖像數(shù)據(jù)，成本較高，無法適用于機器人行走過程中樹干相對位置和姿態(tài)的估計。

本文提出基于雙目eye in hand系統(tǒng)的多角度樹干位姿估計方法，以機械臂和普通雙目為基礎，構建雙目eye in hand系統(tǒng)進行雙目標定、手眼標定；通過YOLOv5深度學習方法進行樹干識別；利用經(jīng)典立體匹配算法實現(xiàn)樹干的三維重建；利用雙目eye in hand系統(tǒng)進行多角度點云拼接；利用RANSAC算法進行樹干姿態(tài)估計；利用無約束最小二乘法優(yōu)化圓柱體參數(shù)，實現(xiàn)從多角度對同一樹干進行重建，準確估計樹干的相對位置和姿態(tài)。

1 方法

基于雙目eye in hand系統(tǒng)的多角度樹干位姿估計方法包含手眼標定、局部立體匹配、樹干姿態(tài)估計等部分，如圖1所示。

圖1中，手眼標定部分建立各個坐標系之間的轉換關系，并將相機坐標系下的圖像信息轉換到機器人坐標系；局部立體匹配部分生成深度學習檢測目標的局部點云圖；樹干姿態(tài)估計部分獲得優(yōu)化后樹干的近圓柱體模型。

1.1 閉環(huán)式手眼標定

手眼系統(tǒng)主要有2種：一種是eye?to?hand，相機固定，不隨機器人運動；另一種是eye?in?hand，相機隨機器人運動。手眼標定通過建立各個坐標系之間的轉換關系，將雙目相機拍攝的圖像、三維重建后的三維點云數(shù)據(jù)轉換到機器人坐標系下。

在雙目eye in hand系統(tǒng)中，含有機器人基坐標系{R}、機器人末端坐標系{F}、工具坐標系{G}、標定板坐標系{B}和相機坐標系{C}等5個坐標系，如圖2所示。

1.2 基于深度學習的局部點云圖像

固定目標雙目三維重建時，對每一個位置拍攝的雙目圖像，首先，根據(jù)雙目標定參數(shù)進行圖像矯正和極線對齊；然后，采用YOLOv5深度學習方法檢測圖像中的樹干，去除樹干以外的背景圖像；接著，利用三角測量原理，對局部圖像進行立體匹配，生成視差圖，進而得到局部點云圖；最后，先利用半徑濾波法去除背景中的噪點，得到僅含有樹干的點云數(shù)據(jù)；再進行體素下降，減少樹干點云數(shù)據(jù)。

1.2.1 YOLO目標檢測

為減小計算量，以及無關環(huán)境信息對三維重建的影響，采用深度學習方法提取指定目標。傳統(tǒng)有監(jiān)督的深度學習方法主要分為目標檢測和語義分割2種；相對于語義分割而言，YOLO目標檢測算法數(shù)據(jù)集制作較為方便，且YOLO模型識別速度快、精度高、魯棒性強，適用于工程領域。對于樹干識別，先利用YOLOv5目標檢測算法，獲取雙目左右圖像中樹干的位置矩陣，再進行局部立體匹配。

1.2.2 局部點云圖像

1.2.3 半徑濾波與體素濾波

在立體匹配時，會生成一部分誤匹配點，影響樹干模型的計算。采用濾波的方法去除立體匹配產生的噪聲點，提取樹干相關的點云數(shù)據(jù)。樹干的點云數(shù)據(jù)相對噪聲，數(shù)據(jù)量多且密集，半徑濾波通過設置濾波半徑與閾值點云數(shù)去除噪聲點。當某一點濾波半徑內的點云數(shù)大于閾值點云數(shù)，則將該點視為數(shù)據(jù)點；若不滿足該條件，則為噪聲點。經(jīng)半徑濾波后只剩下樹干點云數(shù)據(jù)，由于數(shù)據(jù)密度較高，采用體素濾波減少單位面積內的點云數(shù)據(jù)，對點云數(shù)據(jù)進行均勻下采樣。

1.3 點云拼接與樹干姿態(tài)估計

1.3.1 點云拼接

1.3.3 ?圓柱體參數(shù)優(yōu)化

RANSAC算法擬合得到的圓柱體參數(shù)并非最優(yōu)解，采用最小二乘法進行非線性優(yōu)化，求解最優(yōu)圓柱體參數(shù)。將RANSAC算法擬合圓柱體得到的參數(shù)作為初始參數(shù)，RANSAC算法擬合后篩選的點云數(shù)據(jù)作為非線性優(yōu)化的輸入數(shù)據(jù)，進行非線性無約束優(yōu)化。

對于有約束的最小二乘法參數(shù)優(yōu)化問題，可以描述為

2 實驗與結果分析

2.1 實驗環(huán)境

2.2 手眼系統(tǒng)標定實驗

每個角點的平均誤差計算結果如圖6所示，平均歐式誤差為3.717 7 mm。

2.3 局部圖像點云實驗

在生成局部圖像時，首先，通過深度學習檢測得到待匹配的指定區(qū)域；然后，對指定區(qū)域匹配時，經(jīng)半徑濾波后，得到樹干表面的局部點云圖；最后，在保留樹干姿態(tài)信息的前提下，盡量減少點云數(shù)量，以減少計算量。

實驗時，機械臂采用圓形的軌跡規(guī)劃，多角度拍攝同一棵樹的雙目圖像，計算不同拍攝位置的點云數(shù)量和處理后的點云數(shù)量。樹干點云數(shù)量變換圖如圖7所示。

2.4 點云拼接、位姿估計與參數(shù)優(yōu)化實驗

對不同角度拍攝的雙目圖像，通過局部立體匹配得到該位置的點云圖像。通過將點云圖像轉換到同一機器人坐標系下進行點云拼接。對3個位置的點云圖像拼接后，進行圓柱體位置姿態(tài)估計。

對拼接后的點云進行姿態(tài)估計和參數(shù)優(yōu)化，計算軸線的最小值點和最大值點。半徑誤差圖值如圖9所示，優(yōu)化前后圓柱體參數(shù)如表1所示。

由圖9可知，優(yōu)化后圓柱體半徑與真實半徑的差值減小。

由表1可知，利用最小二乘法進行非線性優(yōu)化后圓柱體r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm。

3 ?結論

本文提出基于雙目eye in hand系統(tǒng)的多角度樹干位姿估計方法，通過計算雙目eye in hand系統(tǒng)各坐標系的轉換關系對標定板進行三維重建，運用標定板角點定量計算手眼標定的誤差，平均歐式誤差為3.717 7 mm；運用半全局塊匹配算法，結合YOLOv5目標檢測算法對樹干進行局部三維重建，采用半徑濾波和體素濾波減少98.470%的點云數(shù)據(jù)；采用RANSAC圓柱體估計算法對樹干點云數(shù)據(jù)進行圓柱體擬合，擬合后圓柱體的半徑r = 41.277 1 mm，R_MAE= 2.571 56 mm，R_RMSE= 2.989 36 mm。利用最小二乘法進行非線性優(yōu)化求解最優(yōu)圓柱體參數(shù)，優(yōu)化后r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm，實現(xiàn)了樹干精準位姿估計。

本文僅從多角度對單一目標進行三維重建，在后續(xù)研究中，通過手眼系統(tǒng)可以根據(jù)多角度點云圖像拼接的特點，對周圍環(huán)境進行大規(guī)模的三維重建，建立多角度多目標的三維重建系統(tǒng)。

參考文獻

[1] 容秀嬋，鄒湘軍，李承恩，等.基于數(shù)據(jù)驅動的虛擬場景搭建及模型檢索優(yōu)化方法[J].中國農機化學報，2022，43（8）：128-135.

[2] 李秋潔，丁旭東，鄧賢.基于激光雷達的果園行間路徑提取與導航[J].農業(yè)機械學報，2020，51（S2）：344-350.

[3] 鄭太雄，江明哲，馮明馳.基于視覺的采摘機器人目標識別與定位方法研究綜述[J].儀器儀表學報，2021，42（9）：28-51.

[4] 王輝，毛文華，劉剛，等.基于視覺組合的蘋果作業(yè)機器人識別與定位[J].農業(yè)機械學報，2012，43（12）：165-170.

[5] 方建軍.移動式采摘機器人研究現(xiàn)狀與進展[J].農業(yè)工程學報，2004，20（2）：273-278.

[6] 鄭似青，熊偉，方陸明，等.立木胸徑近真值測量與計算模型的研究[J].西北林學院學報，2021，36（5）：125-132.

[7] 劉魯霞，龐勇，李增元.基于地基激光雷達的亞熱帶森林單木胸徑與樹高提取[J].林業(yè)科學，2016，52（2）：26-37.

[8] ZOU X， ZOU H， LU J. Virtual manipulator-based binocular stereo vision positioning system and errors modelling[J]. Ma-chine Vision and Applications， 2012，23：43-63.

[9] 葉敏，鄒湘軍，羅陸鋒，等.荔枝采摘機器人雙目視覺的動態(tài)定位誤差分析[J].農業(yè)工程學報，2016，32（5）：50-56.

[10] LIN G， TANG Y， ZOU X， et al. Guava detection and pose estimation using a low-cost RGB-D sensor in the field[J]. Sensors， 2019，19（2）：428.

[11] CHEN M， TANG Y， ZOU X， et al. Three-dimensional perception of orchard banana central stock enhanced by adaptive multi-vision technology[J]. Computers and Electro-nics in Agriculture， 2020，174：105508.

[12] CHEN M， TANG Y， ZOU X， et al. 3D global mapping of large-scale unstructured orchard integrating eye-in-hand stereo vision and SLAM[J]. Computers and Electronics in Agricu-lture， 2021，187：106237.

[13] OLOFSSON K， HOLMGREN J， OLSSON H. Tree stem and height measurements using terrestrial laser scanning and the RANSAC algorithm[J]. Remote Sensing， 2014，6（5）：4323-4344.

[14] 吳杭彬，王旭飛，劉春.基于林地激光點云的樹木胸徑自動提取方法[J].同濟大學學報（自然科學版），2022，50（7）：947-954.

作者簡介：

王歡，男，1998 年生，碩士研究生，主要研究方向：機器視覺。E-mail： 1306357145@qq.com

徐秀進，男，1998年生，碩士研究生，主要研究方向：機器視覺。E-mail：?xxj17306690937@163.com

王紅軍（通信作者），女，1966年生，博士，教授，主要研究方向：機器視覺、農業(yè)采摘機器人。E-mail：?xtwhj@scau.edu.cn

溫志慶，男，1964年生，博士，教授級高工，主要研究方向：機器視覺與人工智能。E-mail：?j.wen1234@outlook.com