基于雙目eye in hand系統(tǒng)的多角度樹干位姿估計(jì)方法

王歡 徐秀進(jìn) 王紅軍 溫志慶

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

關(guān)鍵詞：eye?in?hand手眼標(biāo)定；局部立體匹配；樹干圓柱體估計(jì)；多角度點(diǎn)云拼接

中圖分類號(hào)：S225; TP391.41?????????文獻(xiàn)標(biāo)志碼：A ???????文章編號(hào)：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)境下，通過視覺方法對(duì)樹干進(jìn)行精準(zhǔn)識(shí)別和定位是采摘機(jī)器人自主行走的關(guān)鍵^[1-2]。精確確定采摘機(jī)器人與樹干的相對(duì)位置和距離，以及準(zhǔn)確估計(jì)樹干的位置和姿態(tài)，可提升導(dǎo)航和采摘的定位精確度，提高采摘機(jī)器人的作業(yè)效率，降低作業(yè)成本^[3-5]。

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

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

1 方法

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

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

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

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

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

1.2 基于深度學(xué)習(xí)的局部點(diǎn)云圖像

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

1.2.1 YOLO目標(biāo)檢測(cè)

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

1.2.2 局部點(diǎn)云圖像

1.2.3 半徑濾波與體素濾波

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

1.3 點(diǎn)云拼接與樹干姿態(tài)估計(jì)

1.3.1 點(diǎn)云拼接

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

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

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

2 實(shí)驗(yàn)與結(jié)果分析

2.1 實(shí)驗(yàn)環(huán)境

2.2 手眼系統(tǒng)標(biāo)定實(shí)驗(yàn)

每個(gè)角點(diǎn)的平均誤差計(jì)算結(jié)果如圖6所示，平均歐式誤差為3.717 7 mm。

2.3 局部圖像點(diǎn)云實(shí)驗(yàn)

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

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

2.4 點(diǎn)云拼接、位姿估計(jì)與參數(shù)優(yōu)化實(shí)驗(yàn)

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

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

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

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

3 ?結(jié)論

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

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

參考文獻(xiàn)

[1] 容秀嬋，鄒湘軍，李承恩，等.基于數(shù)據(jù)驅(qū)動(dòng)的虛擬場(chǎng)景搭建及模型檢索優(yōu)化方法[J].中國(guó)農(nóng)機(jī)化學(xué)報(bào)，2022，43（8）：128-135.

[2] 李秋潔，丁旭東，鄧賢.基于激光雷達(dá)的果園行間路徑提取與導(dǎo)航[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2020，51（S2）：344-350.

[3] 鄭太雄，江明哲，馮明馳.基于視覺的采摘機(jī)器人目標(biāo)識(shí)別與定位方法研究綜述[J].儀器儀表學(xué)報(bào)，2021，42（9）：28-51.

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

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

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

[7] 劉魯霞，龐勇，李增元.基于地基激光雷達(dá)的亞熱帶森林單木胸徑與樹高提取[J].林業(yè)科學(xué)，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] 葉敏，鄒湘軍，羅陸鋒，等.荔枝采摘機(jī)器人雙目視覺的動(dòng)態(tài)定位誤差分析[J].農(nóng)業(yè)工程學(xué)報(bào)，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] 吳杭彬，王旭飛，劉春.基于林地激光點(diǎn)云的樹木胸徑自動(dòng)提取方法[J].同濟(jì)大學(xué)學(xué)報(bào)（自然科學(xué)版），2022，50（7）：947-954.

作者簡(jiǎn)介：

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

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

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

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