基于双目eye in hand系统的多角度树干位姿估计方法

王欢 徐秀进 王红军 温志庆

摘要：针对采摘机器人自主行走导航过程中，难以准确定位其与果树之间的相对位置，难以准确估计果树树干姿态的问题，提出基于双目eye in hand系统的多角度树干位姿估计方法。利用YOLOv5深度学习方法与半全局块匹配算法识别树干并生成局部点云；利用半径滤波和体素滤波减少树干点云数据；利用闭环式手眼标定方法对双目eye in hand系统进行标定，并对同一树干多角度相机位置的点云数据进行拼接；利用随机抽样一致（RANSAC）算法与无约束最小二乘法估计并优化树干的位置和姿态，获取树干的圆柱体参数。通过对30幅标定板图像进行实验，闭环式手眼标定方法的平均欧式误差为3.717 7 mm；采用半径滤波和体素滤波可减少98.470%的点云数据；采用RANSAC算法、圆柱体估计算法拟合树干点云数据，得到圆柱体的半径r = 41.277 1 mm，R_MAE= 2.571 56 mm，R_RMSE= 2.989 36 mm；無约束最小二乘法优化后r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm。该文通过对双目eye in hand系统进行标定，建立坐标系转换关系，多角度采集环境信息，准确定位机器人与果树之间的相对位置，估计果树树干的姿态。

关键词：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 引言

在野外环境下，通过视觉方法对树干进行精准识别和定位是采摘机器人自主行走的关键^[1-2]。精确确定采摘机器人与树干的相对位置和距离，以及准确估计树干的位置和姿态，可提升导航和采摘的定位精確度，提高采摘机器人的作业效率，降低作业成本^[3-5]。

近年来，树干模型的三维重建和姿态估计一般采用激光雷达和图像2种方式。基于激光点云的树干模型三维重建以计算机视觉、计算机图形学为理论基础进行激光点云去噪以及树干骨架重建，但由于激光点云数据采样量较大，给分析和计算带来巨大挑战^[6-7]。通过图像进行树干模型的三维重建常采用双目视觉、多目视觉、深度视觉等方案。如邹湘军团队^[8-9]对荔枝视觉采摘机器人进行了大量地研究，利用双目视觉定位方法估计荔枝定位误差；LIN等^[10]提出基于深度相机与全卷积网络（fully convolutional networks，FCN）深度学习的番石榴识别与姿态估计方法，但深度相机受光照强度影响较大；CHEN等^[11-12]提出基于四目系统的香蕉果轴三维重建与定位方法，但仅从2个角度采集环境中的图像信息，图像信息少且位置固定。OLOFSSON等^[13]改进基于随机抽样一致（random sample consensus， RANSAC）模型对二维圆的拟合算法，提取树干胸径值，但在树干存在倾斜角度时，二维圆拟合误差较大。吴杭彬等^[14]结合布料模拟滤波（cloth simulation filtering， CSF）算法与RANSAC算法，有效滤除地面数据并拟合树干模型，实现树干胸径大规模提取自动化，但利用激光扫描仪采集图像数据，成本较高，无法适用于机器人行走过程中树干相对位置和姿态的估计。

本文提出基于双目eye in hand系统的多角度树干位姿估计方法，以机械臂和普通双目为基础，构建双目eye in hand系统进行双目标定、手眼标定；通过YOLOv5深度学习方法进行树干识别；利用经典立体匹配算法实现树干的三维重建；利用双目eye in hand系统进行多角度点云拼接；利用RANSAC算法进行树干姿态估计；利用无约束最小二乘法优化圆柱体参数，实现从多角度对同一树干进行重建，准确估计树干的相对位置和姿态。

1 方法

基于双目eye in hand系统的多角度树干位姿估计方法包含手眼标定、局部立体匹配、树干姿态估计等部分，如图1所示。

图1中，手眼标定部分建立各个坐标系之间的转换关系，并将相机坐标系下的图像信息转换到机器人坐标系；局部立体匹配部分生成深度学习检测目标的局部点云图；树干姿态估计部分获得优化后树干的近圆柱体模型。

1.1 闭环式手眼标定

手眼系统主要有2种：一种是eye to hand，相机固定，不随机器人运动；另一种是eye in hand，相机随机器人运动。手眼标定通过建立各个坐标系之间的转换关系，将双目相机拍摄的图像、三维重建后的三维点云数据转换到机器人坐标系下。

在双目eye in hand系统中，含有机器人基坐标系{R}、机器人末端坐标系{F}、工具坐标系{G}、标定板坐标系{B}和相机坐标系{C}等5个坐标系，如图2所示。

1.2 基于深度学习的局部点云图像

固定目标双目三维重建时，对每一个位置拍摄的双目图像，首先，根据双目标定参数进行图像矫正和极线对齐；然后，采用YOLOv5深度学习方法检测图像中的树干，去除树干以外的背景图像；接着，利用三角测量原理，对局部图像进行立体匹配，生成视差图，进而得到局部点云图；最后，先利用半径滤波法去除背景中的噪点，得到仅含有树干的点云数据；再进行体素下降，减少树干点云数据。

1.2.1 YOLO目标检测

为减小计算量，以及无关环境信息对三维重建的影响，采用深度学习方法提取指定目标。传统有监督的深度学习方法主要分为目标检测和语义分割2种；相对于语义分割而言，YOLO目标检测算法数据集制作较为方便，且YOLO模型识别速度快、精度高、鲁棒性强，适用于工程领域。对于树干识别，先利用YOLOv5目标检测算法，获取双目左右图像中树干的位置矩阵，再进行局部立体匹配。

1.2.2 局部点云图像

1.2.3 半径滤波与体素滤波

在立体匹配时，会生成一部分误匹配点，影响树干模型的计算。采用滤波的方法去除立体匹配产生的噪声点，提取树干相关的点云数据。树干的点云数据相对噪声，数据量多且密集，半径滤波通过设置滤波半径与阈值点云数去除噪声点。当某一点滤波半径内的点云数大于阈值点云数，则将该点视为数据点；若不满足该条件，则为噪声点。经半径滤波后只剩下树干点云数据，由于数据密度较高，采用体素滤波减少单位面积内的点云数据，对点云数据进行均匀下采样。

1.3 点云拼接与树干姿态估计

1.3.1 点云拼接

1.3.3  圆柱体参数优化

RANSAC算法拟合得到的圆柱体参数并非最优解，采用最小二乘法进行非线性优化，求解最优圆柱体参数。将RANSAC算法拟合圆柱体得到的参数作为初始参数，RANSAC算法拟合后筛选的点云数据作为非线性优化的输入数据，进行非线性无约束优化。

对于有约束的最小二乘法参数优化问题，可以描述为

2 实验与结果分析

2.1 实验环境

2.2 手眼系统标定实验

每个角点的平均误差计算结果如图6所示，平均欧式误差为3.717 7 mm。

2.3 局部图像点云实验

在生成局部图像时，首先，通过深度学习检测得到待匹配的指定区域；然后，对指定区域匹配时，经半径滤波后，得到树干表面的局部点云图；最后，在保留树干姿态信息的前提下，尽量减少点云数量，以减少计算量。

实验时，机械臂采用圆形的轨迹规划，多角度拍摄同一棵树的双目图像，计算不同拍摄位置的点云数量和处理后的点云数量。树干点云数量变换图如图7所示。

2.4 点云拼接、位姿估计与参数优化实验

对不同角度拍摄的双目图像，通过局部立体匹配得到该位置的点云图像。通过将点云图像转换到同一机器人坐标系下进行点云拼接。对3个位置的点云图像拼接后，进行圆柱体位置姿态估计。

对拼接后的点云进行姿态估计和参数优化，计算轴线的最小值点和最大值点。半径误差图值如图9所示，优化前后圆柱体参数如表1所示。

由图9可知，优化后圆柱体半径与真实半径的差值减小。

由表1可知，利用最小二乘法进行非线性优化后圆柱体r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm。

3  结论

本文提出基于双目eye in hand系统的多角度树干位姿估计方法，通过计算双目eye in hand系统各坐标系的转换关系对标定板进行三维重建，运用标定板角点定量计算手眼标定的误差，平均欧式误差为3.717 7 mm；运用半全局块匹配算法，结合YOLOv5目标检测算法对树干进行局部三维重建，采用半径滤波和体素滤波减少98.470%的点云数据；采用RANSAC圆柱体估计算法对树干点云数据进行圆柱体拟合，拟合后圆柱体的半径r = 41.277 1 mm，R_MAE= 2.571 56 mm，R_RMSE= 2.989 36 mm。利用最小二乘法进行非线性优化求解最优圆柱体参数，优化后r = 39.402 8 mm，R_MAE= 1.989 55 mm，R_RMSE= 2.465 88 mm，实现了树干精准位姿估计。

本文仅从多角度对单一目标进行三维重建，在后续研究中，通过手眼系统可以根据多角度点云图像拼接的特点，对周围环境进行大规模的三维重建，建立多角度多目标的三维重建系统。

参考文献

[1] 容秀婵，邹湘军，李承恩，等.基于数据驱动的虚拟场景搭建及模型检索优化方法[J].中国农机化学报，2022，43（8）：128-135.

[2] 李秋洁，丁旭东，邓贤.基于激光雷达的果园行间路径提取与导航[J].农业机械学报，2020，51（S2）：344-350.

[3] 郑太雄，江明哲，冯明驰.基于视觉的采摘机器人目标识别与定位方法研究综述[J].仪器仪表学报，2021，42（9）：28-51.

[4] 王辉，毛文华，刘刚，等.基于视觉组合的苹果作业机器人识别与定位[J].农业机械学报，2012，43（12）：165-170.

[5] 方建军.移动式采摘机器人研究现状与进展[J].农业工程学报，2004，20（2）：273-278.

[6] 郑似青，熊伟，方陆明，等.立木胸径近真值测量与计算模型的研究[J].西北林学院学报，2021，36（5）：125-132.

[7] 刘鲁霞，庞勇，李增元.基于地基激光雷达的亚热带森林单木胸径与树高提取[J].林业科学，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].农业工程学报，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年生，博士，教授，主要研究方向：机器视觉、农业采摘机器人。E-mail： xtwhj@scau.edu.cn

温志庆，男，1964年生，博士，教授级高工，主要研究方向：机器视觉与人工智能。E-mail： j.wen1234@outlook.com