技术领域Technical field
本发明涉及果实采摘技术领域，具体涉及一种七自由度水果采摘机器人及其采摘方法。The invention relates to the technical field of fruit picking, and in particular to a seven-degree-of-freedom fruit picking robot and a picking method thereof.
背景技术Background technique
乔木类水果，如苹果、梨、番石榴、柑橘等不仅可以补充人体所需的各类维生素，还对预防疾病、减缓衰老、美容养颜具有一定的效果。由于乔木类水果的需求量与日俱增、主要以新鲜水果为主，因此，乔木类水果成熟后需在短时间内完成采摘、运输等，以保证其新鲜程度，从而让满足人们的日常需求。然而，现有的农业劳动力匮乏，且人工采摘的单日采摘效率低，导致采摘成本高，进而增加水果的生产成本；同时，乔木类水果又具有一定的采摘时限要求，超过时限会导致水果卖相、口感等变差，从而导致产品滞销。Arbor fruits, such as apples, pears, guavas, citrus, etc., can not only supplement various vitamins needed by the human body, but also have certain effects on preventing diseases, slowing down aging, and beautifying the skin. Since the demand for arbor fruits is increasing day by day, mainly fresh fruits, the arbor fruits need to be picked and transported within a short time after maturity to ensure their freshness and meet people's daily needs. However, the existing agricultural labor force is scarce, and the single-day picking efficiency of manual picking is low, resulting in high picking costs, thereby increasing the production cost of fruits. At the same time, tree fruits have certain picking time limits, and exceeding the time limit will cause the fruit to be sold. The appearance and taste will deteriorate, resulting in unsaleable products.
现阶段，机械化采摘多为拉拽装置辅助人工进行采摘；在拉拽过程中，采摘头会挤压水果，对果实、果树枝条等造成伤害，进而影响果实的运输、出售，影响果树的二次结果等。此外，乔木类水果在果树的枝条上呈现不规则的分布，果实周围常伴随障碍物遮挡，现阶段我国乔木类果树基本都为三维种植，由于种植面广、种植量大，几乎很少有进行剪枝的情况，从而会导致乔木类水果被树枝、树叶等遮挡情况频发，现有的采摘装置无法有效规避树枝、树叶等障碍物，极易造成错采、漏采、误采等问题，降低水果采摘的成功率，还易对果实、果树枝条造成不可逆的损伤，造成果树减产、水果滞销等问题。At present, mechanized picking mostly uses pulling devices to assist manual picking; during the pulling process, the picking heads will squeeze the fruits, causing damage to the fruits, fruit tree branches, etc., which will affect the transportation and sales of the fruits, and affect the secondary health of the fruit trees. Results etc. In addition, arbor fruits are irregularly distributed on the branches of fruit trees, and the fruits are often surrounded by obstacles. At this stage, arbor fruit trees in my country are basically three-dimensionally planted. Due to the wide planting area and large planting volume, they are rarely used. Pruning will frequently cause tree fruits to be blocked by branches, leaves, etc. Existing picking devices cannot effectively avoid obstacles such as branches, leaves, etc., and can easily cause problems such as wrong picking, missing picking, and mistaken picking. Reducing the success rate of fruit picking can also easily cause irreversible damage to fruits and fruit tree branches, resulting in problems such as reduced fruit tree yields and unsalable fruits.
发明内容Contents of the invention
针对以上现有技术存在的问题，本发明的目的在于提供一种七自由度水果采摘机器人，该采摘机器人能够自动定位识别乔木类水果，同时完成采摘路径的规划、自动避开树枝或树叶等障碍物，完成乔木类水果的自动化、机械化采摘，操作简便、采摘效率高，从而有效节省采摘的劳动力成本，确保果树的产量与果实的品质。In view of the problems existing in the above existing technologies, the purpose of the present invention is to provide a seven-degree-of-freedom fruit picking robot that can automatically locate and identify tree fruits, complete the planning of the picking path, and automatically avoid obstacles such as branches or leaves. It can complete the automated and mechanized picking of tree fruits with easy operation and high picking efficiency, thus effectively saving the labor cost of picking and ensuring the yield of fruit trees and the quality of fruits.
本发明的另一个目的在于提供一种上述七自由度水果采摘机器人的采摘方法，从而完成乔木类水果的智能化采摘。Another object of the present invention is to provide a picking method for the above-mentioned seven-degree-of-freedom fruit picking robot, thereby completing intelligent picking of tree fruits.
本发明的目的通过以下技术方案实现：The object of the present invention is achieved through the following technical solutions:
一种七自由度水果采摘机器人，其特征在于：包括履带式底盘、七自由度机械臂组件、末端执行器、视觉感知系统及避障路径规划系统；七自由度机械臂组件由一自由度机械臂与六自由度机械臂组成，一自由度机械臂安装在履带式底盘上，六自由度机械臂安装在一自由度机械臂上；末端执行器安装在六自由度机械臂远离一自由度机械臂的一端端部；视觉感知系统包括深度相机与定位模块，深度相机安装在末端执行器上、跟随末端执行器一同运动；避障路径规划系统与定位模块集成在中控装置中，中控装置安装在履带式底盘上且分别与履带式底盘、七自由度机械臂组件、末端执行器、深度相机电连接。A seven-degree-of-freedom fruit picking robot is characterized by: including a crawler chassis, a seven-degree-of-freedom robotic arm assembly, an end effector, a visual perception system and an obstacle avoidance path planning system; the seven-degree-of-freedom robotic arm assembly is composed of a one-degree-of-freedom mechanical arm. The arm is composed of a six-degree-of-freedom robotic arm. The one-degree-of-freedom robotic arm is installed on the crawler chassis. The six-degree-of-freedom robotic arm is installed on the one-degree-of-freedom robotic arm. The end effector is installed on the six-degree-of-freedom robotic arm away from the one-degree-of-freedom machine. One end of the arm; the visual perception system includes a depth camera and a positioning module. The depth camera is installed on the end effector and moves with the end effector; the obstacle avoidance path planning system and the positioning module are integrated in the central control device. It is installed on the crawler chassis and is electrically connected to the crawler chassis, seven-degree-of-freedom manipulator assembly, end effector, and depth camera respectively.
作进一步优化，所述履带式底盘上设置用于采摘后水果存放的水果篮。For further optimization, a fruit basket is provided on the crawler chassis for storing fruits after picking.
作进一步优化，所述末端执行器包括底座、曲柄滑块机构及四杆机构，曲柄滑块机构设置在底座上，包括转动盘、连接杆与滑块，转动盘转动设置在底座端面上且底座对应端面滑动设置滑块，连接杆一端与转动盘外圈转动连接、另一端与滑块顶面转动连接，从而通过转动盘的转动控制滑块的平移；四杆机构为对称设置的两组，包括第一连杆、“L”形连杆、“7”字形连杆与第二连杆，滑块远离连接杆一端且对应两组四杆机构设置安装块，两组四杆机构的第一连杆分别与安装块转动连接，“L”形连杆的一端分别与对应的第一连杆转动连接、另一端分别与对应的“7”字形连杆一端转动连接，且“L”形连杆的拐角与底座转动连接，两根第二连杆设置在两根“L”形连杆之间且第二连杆一端分别与底座端面转动连接、另一端分别与对应的“7”字形连杆拐角转动连接。For further optimization, the end effector includes a base, a crank slider mechanism and a four-bar mechanism. The crank slider mechanism is arranged on the base, including a rotating disk, a connecting rod and a slider. The rotating disk is rotated on the end face of the base and the base A slider is provided corresponding to the end face sliding. One end of the connecting rod is rotationally connected to the outer ring of the rotating disk, and the other end is rotationally connected to the top surface of the slider, so that the translation of the slider is controlled by the rotation of the rotating disk; the four-bar mechanism is two groups of symmetrical arrangements. It includes a first connecting rod, an "L" shaped connecting rod, a "7" shaped connecting rod and a second connecting rod. The slider is far away from one end of the connecting rod and is provided with a mounting block corresponding to the two sets of four-bar mechanisms. The first link of the two sets of four-bar mechanisms The connecting rods are respectively rotatably connected with the mounting blocks, one end of the "L"-shaped connecting rod is rotatably connected with the corresponding first connecting rod, and the other end is rotatably connected with one end of the corresponding "7"-shaped connecting rod, and the "L"-shaped connecting rod is rotatably connected with the corresponding first connecting rod. The corners of the rods are rotationally connected to the base, and the two second connecting rods are arranged between the two "L" shaped connecting rods. One end of the second connecting rod is rotationally connected to the end face of the base, and the other end is connected to the corresponding "7" shape. Rod corner rotation connection.
作进一步优化，所述底座的底面固定设置舵机，舵机输出轴与转动盘共轴线且转动盘固定套接在舵机输出轴外壁。For further optimization, the bottom surface of the base is fixed with a steering gear, the steering gear output shaft and the rotating disk are coaxial, and the rotating disk is fixedly sleeved on the outer wall of the steering gear output shaft.
作进一步优化，所述底座上且对应滑块设置滑移导轨，滑块卡在滑移导轨上且滑动连接。For further optimization, a sliding guide rail is provided on the base and corresponding to the slide block, and the slide block is stuck on the sliding guide rail and is slidingly connected.
作进一步优化，所述“7”字形连杆相对侧面分别设置夹紧胶条且“7”字形连杆位于夹紧胶条上侧的端面设置刀片。For further optimization, the opposite sides of the "7"-shaped connecting rod are equipped with clamping strips respectively, and the end face of the "7"-shaped connecting rod located on the upper side of the clamping strip is equipped with a blade.
作进一步优化，所述深度相机采用RealSense D435i相机。For further optimization, the depth camera uses RealSense D435i camera.
一种七自由度水果采摘机器人的采摘方法，采用上述采摘机器人，其特征在于：包括采摘点与障碍物定位算法、手眼标定算法与路径规划算法；A picking method for a seven-degree-of-freedom fruit picking robot, using the above-mentioned picking robot, is characterized by: including a picking point and obstacle positioning algorithm, a hand-eye calibration algorithm and a path planning algorithm;
所述采摘点与障碍物定位算法具体为：The picking point and obstacle positioning algorithm is specifically:
首先，使用深度相机采集RGB图像与深度图像，并应用语义分割网络对RGB图像进行分割；First, use a depth camera to collect RGB images and depth images, and apply a semantic segmentation network to segment the RGB images;
之后，对图像中的每一个像素均打上标签，即背景为0，连通域分别打上1、2、…、N-1、N的标签，同一个连通域的像素打上同样的标签；若物体存在重叠、会把不同物体的多个连通域计为一个连通域，从而提取果实连通域；再结合深度图像，将各个果实连通域转换为三维点云、同时将障碍物二值化区域也转换为三维点云，应用统计分析法移除三维点云中的噪声；然后，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值；最终，以果实中心点铅垂向上一个半径值的位置作为果实采摘点；After that, each pixel in the image is labeled, that is, the background is 0, the connected domains are labeled 1, 2,..., N-1, N respectively, and the pixels in the same connected domain are labeled with the same label; if the object exists Overlap, multiple connected domains of different objects will be counted as one connected domain to extract the fruit connected domain; combined with the depth image, each fruit connected domain will be converted into a three-dimensional point cloud, and the binary area of the obstacle will also be converted into For the three-dimensional point cloud, apply statistical analysis method to remove the noise in the three-dimensional point cloud; then, use the least squares method to perform sphere fitting on the fruit connected domain in the three-dimensional point cloud to obtain the fruit center point position and radius value; finally, use the fruit The position with a radius value vertically upward from the center point is used as the fruit picking point;
之后，应用体素栅格化的点云简化法对上述获得的三维点云进行体素化处理，实现使用大量立方体逼近障碍物和果实的空间分布；After that, the point cloud simplification method of voxel rasterization is applied to voxelize the three-dimensional point cloud obtained above to achieve the use of a large number of cubes to approximate the spatial distribution of obstacles and fruits;
所述手眼标定算法用于求解相机与末端执行器之间的位姿转换关系、从而获得末端执行器相对于采摘目标之间的坐标转换关系；The hand-eye calibration algorithm is used to solve the pose transformation relationship between the camera and the end effector, thereby obtaining the coordinate transformation relationship between the end effector and the picking target;
所述路径规划算法具体为：首先，在初始点与目标点之间随机选取2～3个点，对选择的点位进行判断、看其是否处于障碍物上，若处于，则更换对应的随机点；确定随机点Qr后，将随机点与初始点、目标点形成多棵搜索树，同时沿随机点Qr方向以单位步长进行扩展，寻找到新节点Qn，再以新节点重复上述步骤，期间同样要考虑与障碍物有无碰撞、步长等，直至多棵搜索树同时存在相互连接(多棵搜索树同时存在相互连接说明找到路径、完成规划)；The specific path planning algorithm is: first, randomly select 2 to 3 points between the initial point and the target point, judge the selected points to see if they are on obstacles, and if so, replace the corresponding random points. point; after determining the random point Qr , form multiple search trees with the random point, the initial point, and the target point, and at the same time expand with a unit step in the direction of the random point Qr , find the new node Qn , and then repeat with the new node During the above steps, whether there is a collision with obstacles, step length, etc. must also be considered until multiple search trees are connected to each other at the same time (the existence of multiple search trees to be connected to each other at the same time means that the path is found and the planning is completed);
上述路径规划算法基于RRT的双树对向交替扩展探索方式来选择代价小的树进行扩展，一定程度上拥有了方向性，避免采样时过于随机，在面对狭窄通道搜索采样时、成功率更高，搜索速度和搜索效率上有明显提升。The above path planning algorithm is based on RRT's dual-tree alternate expansion exploration method to select trees with the lowest cost for expansion. It has directionality to a certain extent and avoids overly random sampling. When faced with narrow channel search sampling, the success rate is higher. High, search speed and search efficiency have been significantly improved.
作进一步优化，所述使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值具体步骤为：For further optimization, the least squares method is used to perform sphere fitting on the fruit connected domain in the three-dimensional point cloud, and the specific steps to obtain the fruit center point position and radius value are:
首先建立球体方程：First establish the equation of the sphere:
然后，假设第i个果实的三维点云坐标为(xi,yi,zi)，带入球体方程中，可得：Then, assuming that the three-dimensional point cloud coordinates of the i-th fruit are (xi , yi , zi ), and put them into the sphere equation, we can get:
令：make:
则：but:
经上述各点的计算能够获得球心坐标(x0,y0,z0)与球体半径r0；Through the calculation of the above points, the coordinates of the center of the sphere (x0 , y0 , z0 ) and the radius of the sphere r0 can be obtained;
之后，预设合理范围值H，计算三维点云中每个点到球心坐标(x0,y0,z0)的距离h0，若|h0-r0|＜H，则该点在拟合球体内，依次记录整个拟合球体内的点的数量；After that, a reasonable range value H is preset, and the distance h0 from each point in the three-dimensional point cloud to the sphere center coordinates (x0 , y0 , z0 ) is calculated. If |h0 -r0 |＜H, then the point In the fitting sphere, record the number of points in the entire fitting sphere in sequence;
重复多次进行球体拟合，选取模型内点的数量最多的球体对应的模型参数为最佳拟合参数，输出最佳拟合参数的球心坐标(x,y,z)及半径r作为果实的三维拟合结果。Repeat the sphere fitting multiple times, select the model parameters corresponding to the sphere with the largest number of internal points in the model as the best fitting parameters, and output the sphere center coordinates (x, y, z) and radius r of the best fitting parameters as the fruit. The three-dimensional fitting results.
作进一步优化，所述合理范围值H为0～0.5，通过大量前期实验数据获得。For further optimization, the reasonable range value H is 0 to 0.5, which is obtained through a large amount of preliminary experimental data.
作进一步优化，所述体素栅格化的点云简化法对上述获得的三维点云进行体素化处理具体步骤为：For further optimization, the voxel rasterized point cloud simplification method performs voxelization on the three-dimensional point cloud obtained above. The specific steps are:
首先，根据点云创建一个最小三维体素长方体，其体积为V：First, create a minimum three-dimensional voxel cuboid based on the point cloud, with a volume V:
V＝Lx·Ly·Lz；V=Lx ·Ly ·Lz ;
式中：Lx表示点云X轴方向最大范围；Ly表示点云Y轴方向最大范围；Lz表示点云Z轴方向最大范围；In the formula: Lx represents the maximum range of the point cloud in the X-axis direction; Ly represents the maximum range of the point cloud in the Y-axis direction; Lz represents the maximum range of the point cloud in the Z-axis direction;
然后，计算需要划分的小立方栅格的边长L，根据L的大小将最小三维体素长方体分解成个小立方体格栅；栅格划分完毕后，将点云数据放到相应的小格栅中，同时删除那些不包含数据点的小格栅；在每个小栅格中，将离小格栅中心最近的数据点保留下来，代表本小格栅中的所有点，删除其余点。Then, calculate the side length L of the small cubic grid that needs to be divided, and decompose the minimum three-dimensional voxel cuboid into small cubic grids; after the grid is divided, place the point cloud data into the corresponding small grids, and delete those small grids that do not contain data points; in each small grid, separate the small grids The data point closest to the center is retained, representing all points in this small grid, and the remaining points are deleted.
作进一步优化，所述手眼标定算法具体为：For further optimization, the hand-eye calibration algorithm is specifically:
首先，根据标定板棋盘格的所有角点的像素值、深度值和深度相机的参数，通过像素坐标系与世界坐标系之间的转换公式进行转换，从而获得标定板棋盘格上所有角点的空间坐标，转换为对应的点的坐标数组，同时将其与所拍照的图片中所对应的所有角点、在图像坐标系下的成像点的坐标数组记录为多组控制点；转换公式为：First, according to the pixel values, depth values and depth camera parameters of all corner points of the calibration board checkerboard, the conversion formula between the pixel coordinate system and the world coordinate system is used to obtain the values of all corner points on the calibration board checkerboard. The spatial coordinates are converted into the coordinate array of the corresponding point, and at the same time, the coordinate arrays of all the corner points corresponding to the photographed picture and the imaging point in the image coordinate system are recorded as multiple sets of control points; the conversion formula is:
式中：X、Y、Z表示世界坐标系下的坐标；f表示焦距；R表示3x3的正交旋转矩阵；t表示三维平移向量；u0、v0表示图像坐标系原点在像素坐标系统的坐标；dx、dy表示每个像素在图像平面x方向、y方向的物理尺寸；u、v表示像素坐标；ZC表示相机坐标中Z轴的向量；In theformula: Coordinates; dx and dy represent the physical size of each pixel in the x and y directions of the image plane; u and v represent the pixel coordinates; ZC represents the vector of the Z axis in the camera coordinates;
然后，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标，获得摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，从而获得多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T中的旋转和平移量；Then, using the coordinates of multiple control points in the three-dimensional scene and their perspective projection coordinates in the image, the absolute pose relationship between the camera coordinate system and the world coordinate system representing the three-dimensional scene structure is obtained, including the absolute translation vector t and Rotate the matrix R to obtain the rotation and translation amounts in the transformation matrix T from the spatial coordinates of all corner points on the multiple sets of calibration plate checkerboards to the camera coordinate system;
之后确定机械臂DH参数表；Then determine the robot arm DH parameter table;
再然后，通过网口通信读出平移位置，计算出x、y、z三轴上的单位矢量a：Then, read the translation position through network port communication, and calculate the unit vector a on the x, y, and z axes:
由于相邻两根连杆Ti与Ti-1的变换关系式为：Since the transformation relationship between two adjacent connecting rodsTi and Ti-1 is:
式中：ai表示公共法线间的距离与垂直于所在平面内两轴的夹角；di表示两根连杆的相对位置；θi表示两根连杆的相对位置di和两根连杆公垂线的夹角；c表示三角函数中的cos()函数；s表示三角函数中的sin()函数；In the formula: ai represents the distance between the common normals and the angle between the two axes perpendicular to the plane; di represents the relative position of the two connecting rods; θi represents the relative position of the two connecting rods di and the two The angle between the common vertical lines of the connecting rod; c represents the cos() function in trigonometric functions; s represents the sin() function in trigonometric functions;
多次移动七自由度机械臂组件，获得多组末端执行器相对于基坐标的旋转和平移量；将末端执行器相对于基坐标的多组旋转和平移参数记作多组姿态矩阵B，将上述多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T视为外参矩阵A，之后，得到组A、B，利用AX＝XB计算得到相机坐标系到七自由度机械臂组件末端坐标系的旋转和平移变换，从而获得相机坐标系到七自由度机械臂组件末端坐标系的变换矩阵Te；Move the seven-degree-of-freedom manipulator component multiple times to obtain multiple sets of rotation and translation amounts of the end effector relative to the base coordinates; record the multiple sets of rotation and translation parameters of the end effector relative to the base coordinates as multiple sets of attitude matrices B, and The transformation matrix T from the spatial coordinates of all corner points on the above multiple sets of calibration board checkerboards to the camera coordinate system is regarded as the external parameter matrix A. After that, we get For groups A and B, use AX=XB to calculate the rotation and translation transformation from the camera coordinate system to the end coordinate system of the seven-degree-of-freedom robotic arm assembly, thereby obtaining the transformation matrix Te from the camera coordinate system to the end-coordinate system of the seven-degree-of-freedom robotic arm assembly. ;
最后，移动七自由度机械臂组件、并使用深度相机对采摘目标进行拍照，像素坐标系与世界坐标系之间的转换公式获得采摘目标的空间坐标；再利用变换矩阵Te以及标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T，获得采摘目标相对于末端执行器坐标系下的位姿。Finally, move the seven-degree-of-freedom robotic arm assembly and use a depth camera to take pictures of the picking target. The conversion formula between the pixel coordinate system and the world coordinate system obtains the spatial coordinates of the picking target; then use the transformation matrix Te and the calibration board checkerboard The transformation matrix T from the spatial coordinates of all corner points on the camera coordinate system to the camera coordinate system is used to obtain the pose of the picking target relative to the end effector coordinate system.
作进一步优化，为了优化路径较少不必要的转折，所述路径规划算法中，在搜索树集合Tree中的节点中任取两点Qi与Qj，其中i,j∈[1,2,3,…,n]；对Qi与Qj间的路径进行碰撞检测，若无碰撞在，则删除Qi与Qj之间的所有节点。For further optimization, in order to optimize the path with fewer unnecessary turns, in the path planning algorithm, any two points Qi and Qj are selected from the nodes in the search tree set Tree , where i, j∈[1,2 ,3,…,n]; Perform collision detection on the path between Qi and Qj . If there is no collision, delete all nodes between Qi and Qj .
作进一步优化，所述碰撞检测为：采用向空间坐标系的三个坐标轴上进行投影的方式、对立方体和圆柱体之间进行碰撞检测，若投影后立方体和圆柱体连接中心点的线段的投影长度大于各自中心点投影后到各自最长边界的距离和，且三个轴同时满足下，则视为立方体和圆柱体不会发生碰撞；For further optimization, the collision detection is: using the method of projecting onto the three coordinate axes of the spatial coordinate system to detect the collision between the cube and the cylinder. If after projection, the line segment connecting the center point of the cube and the cylinder is If the projection length is greater than the sum of the distances from each center point to the longest boundary after projection, and if the three axes are satisfied at the same time, the cube and cylinder will be considered to not collide;
具体为：Specifically:
首先，找到立方体的四个顶点、并通过对其坐标求平均的方法获得立方体为投影时的中心点Plm；再找到圆柱体的上下底面圆心坐标、并通过对其求平均的方法获得圆柱体未投影时的中心点Pym；然后，计算获得中心点Plm与中心点Pym之间的连线ld、其在x轴上的投影为lD，同时，分别获得中心点Plm与中心点Pym在x轴上的投影PlM与PyM；之后，计算PlM与PyM到各自物体(即立方体、圆柱体)投影后的边界的距离ra与rb：First, find the four vertices of the cube and average their coordinates to obtain the center point Plm when the cube is projected; then find the coordinates of the center points of the upper and lower bases of the cylinder and average them to obtain the cylinder. The center point Pym when not projected; then, calculate and obtain the connection line ld between the center point Plm and the center point Pym , and its projection on the x-axis is lD. At the same time, obtain the center point Plm and the center point P ym respectively. The projections PlM and PyM of the center point Pym on the x-axis; then, calculate the distances r a andr bfrom PlM and PyM to the projected boundaries of their respective objects (i.e., cube, cylinder):
若|lD|＞ra+rb，且在y轴、z轴上也满足获得的|lD|＞ra+rb，则立方体与圆柱体无碰撞；否则，立方体与圆柱体存在碰撞。If |lD |＞ra +rb , and the obtained |lD |＞ra +rb is also satisfied on the y-axis and z-axis, then there is no collision between the cube and the cylinder; otherwise, the cube and the cylinder exist collision.
本发明具有如下技术效果：The invention has the following technical effects:
本申请履带式底盘、七自由度机械臂组件与末端执行器的配合，实现了对于乔木类水果的自动化采摘，有效降低人为采摘过程中的劳动力成本，提高采摘效率。The cooperation of the crawler chassis, the seven-degree-of-freedom robotic arm assembly and the end effector in this application realizes the automated picking of arbor fruits, effectively reducing labor costs in the manual picking process and improving picking efficiency.
同时，本申请通过RGB图像与深度图像的配合、利用语义分割网络进行分割，之后通过标签与连通域的配合，获得果实和障碍物感兴趣区域，鲁棒性与实时性高；通过结合深度图像的数据，将各个果实连通域转换为三维点云，应用统计分析法移除三维点云中的噪声，使用最小二乘法对三维点云进行球体拟合、获得果实中心点位置和半径值，从而获得果实的采摘点，定位准确且精度高、定位误差小，能够有效实现乔木类水果的精确采摘；通过路径规划算法，完成采摘路径的规划，进而有效完成采摘过程中的障碍躲避，避免采摘过程中对于果树造成的损伤、确保采摘的有效性，避免采摘过程中由于障碍物的影响而出现错采、漏采等问题，造成果实或果树的损伤、影响果实的品质或果树的再次结果。此外，本申请通过底座、曲柄滑块机构及四杆机构的协同配合，实现对于乔木类水果的剪切，采摘连贯性好，同时能够保证采摘后对果实的收集、采摘的时效性强，能够有效保证采摘效率、节省人力物力与采摘成本。At the same time, this application uses the combination of RGB images and depth images to perform segmentation using a semantic segmentation network, and then uses the combination of labels and connected domains to obtain areas of interest for fruits and obstacles, with high robustness and real-time performance; by combining depth images data, convert each fruit connected domain into a three-dimensional point cloud, apply statistical analysis method to remove the noise in the three-dimensional point cloud, use the least squares method to perform sphere fitting on the three-dimensional point cloud, and obtain the fruit center point position and radius value, thereby Obtain the picking point of the fruit with accurate positioning, high precision and small positioning error, which can effectively realize the precise picking of tree fruits. Through the path planning algorithm, the planning of the picking path is completed, thereby effectively completing the obstacle avoidance during the picking process and avoiding the picking process. It prevents damage to fruit trees, ensures the effectiveness of picking, and avoids problems such as wrong picking and missing picking due to obstacles during the picking process, causing damage to the fruit or fruit trees, affecting the quality of the fruit or the re-fruiting of the fruit trees. In addition, this application uses the collaborative cooperation of the base, crank slider mechanism and four-bar mechanism to achieve shearing of arbor fruits and good picking consistency. At the same time, it can ensure the collection of fruits after picking and the timeliness of picking, and can Effectively ensure picking efficiency, save manpower, material resources and picking costs.
附图说明Description of drawings
图1为本申请实施例中水果采摘机器人的结构示意图。Figure 1 is a schematic structural diagram of a fruit picking robot in an embodiment of the present application.
图2为本申请实施例中水果采摘机器人的末端执行器的结构示意图。Figure 2 is a schematic structural diagram of the end effector of the fruit picking robot in the embodiment of the present application.
图3为本申请实施例中果实和障碍物的语义分割效果图。Figure 3 is a semantic segmentation rendering of fruits and obstacles in the embodiment of the present application.
图4为本申请实施例中障碍物的三维重构效果图。Figure 4 is a three-dimensional reconstruction rendering of the obstacle in the embodiment of the present application.
其中，10、履带式底盘；11、水果篮；20、七自由度机械臂组件；21、一自由度机械臂；22、六自由度机械臂；30、末端执行器；31、底座；310、舵机；321、转动盘；322、连接杆；323、滑块；3230、安装块；331、第一连杆；332、“L”形连杆；333、“7”字形连杆；3331、刀片；3332、夹紧胶条；334、第二连杆；40、深度相机；50、中控装置。Among them, 10. Crawler chassis; 11. Fruit basket; 20. Seven-degree-of-freedom robotic arm assembly; 21. One-degree-of-freedom robotic arm; 22. Six-degree-of-freedom robotic arm; 30. End effector; 31. Base; 310. Steering gear; 321, rotating disk; 322, connecting rod; 323, slider; 3230, mounting block; 331, first connecting rod; 332, "L" shaped connecting rod; 333, "7" shaped connecting rod; 3331. Blade; 3332, clamping strip; 334, second link; 40, depth camera; 50, central control device.
具体实施方式Detailed ways
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, rather than all the embodiments.
实施例1：Example 1:
如图1～2所示：一种七自由度水果采摘机器人，其特征在于：包括履带式底盘10、七自由度机械臂组件20、末端执行器30、视觉感知系统及避障路径规划系统；履带式底盘10采用松灵机器人(深圳)有限公司的履带底盘，该底盘顶部具有一套支撑装置，由铝合金平板、8080铝型材等组成。七自由度机械臂组件20由一自由度机械臂21与六自由度机械臂22组成，一自由度机械臂21安装在履带式底盘10上，六自由度机械臂22安装在一自由度机械臂21上；一自由度机械臂21采用24V步进电机作为原动机，应用滚珠丝杆机构实现直线运动；六自由度机械臂22采用睿尔曼RML63系列机器人，该机械臂自重10.2kg、工作半径为900mm的球体。As shown in Figures 1 and 2: a seven-degree-of-freedom fruit picking robot, which is characterized by: including a crawler chassis 10, a seven-degree-of-freedom robotic arm assembly 20, an end effector 30, a visual perception system and an obstacle avoidance path planning system; The crawler chassis 10 adopts the crawler chassis of Songling Robot (Shenzhen) Co., Ltd. The top of the chassis has a set of support devices, which is composed of aluminum alloy flat plates, 8080 aluminum profiles, etc. The seven-degree-of-freedom robotic arm assembly 20 is composed of a one-degree-of-freedom robotic arm 21 and a six-degree-of-freedom robotic arm 22. The one-degree-of-freedom robotic arm 21 is installed on the crawler chassis 10, and the six-degree-of-freedom robotic arm 22 is installed on the one-degree-of-freedom robotic arm. 21 on; the one-degree-of-freedom robotic arm 21 uses a 24V stepper motor as the prime mover, and uses a ball screw mechanism to achieve linear motion; the six-degree-of-freedom robotic arm 22 uses a RML63 series robot. The robotic arm has a dead weight of 10.2kg and a working radius of 10.2kg. It is a 900mm sphere.
末端执行器30安装在六自由度机械臂22远离一自由度机械臂21的一端端部；包括底座31、曲柄滑块机构及四杆机构，曲柄滑块机构设置在底座31上，包括转动盘321、连接杆322与滑块323，转动盘321转动设置在底座31端面上且底座31对应端面滑动设置滑块323(如图2所示，即底座31上端面)，连接杆322一端与转动盘321外圈转动连接、另一端与滑块323顶面转动连接，从而通过转动盘321的转动控制滑块323的平移；底座31的底面固定设置舵机310，舵机310输出轴与转动盘321共轴线且转动盘321固定套接在舵机310输出轴外壁；底座31上且对应滑块323设置滑移导轨，滑块323卡在滑移导轨上且滑动连接，从而对滑块323的滑动方向形成硬限位。四杆机构为对称设置的两组，包括第一连杆331、“L”形连杆332、“7”字形连杆333与第二连杆334，滑块323远离连接杆322一端且对应两组四杆机构设置安装块3230，两组四杆机构的第一连杆331分别与安装块3230转动连接，“L”形连杆332的一端分别与对应的第一连杆331转动连接、另一端分别与对应的“7”字形连杆333一端转动连接，且“L”形连杆332的拐角通过设置在底座31上的第一转轴与底座31实现转动连接(即如图2所示：第一转轴转动设置在底座31端面，“L”形连杆332的拐角固定套接在对应的第一转轴外壁)，两根第二连杆334设置在两根“L”形连杆332之间且第二连杆334一端分别通过设置在底座31上的第二转轴与底座31端面转动连接(即如图2所示：第二转轴转动设置在底座31端面，第二连杆334一端固定套接在第二转轴外壁)、另一端分别与对应的“7”字形连杆333拐角转动连接。“7”字形连杆333相对侧面分别设置夹紧胶条3332且“7”字形连杆333位于夹紧胶条3332上侧的端面设置刀片3331(如图2所示)。The end effector 30 is installed at one end of the six-degree-of-freedom mechanical arm 22 away from the one-degree-of-freedom mechanical arm 21; it includes a base 31, a crank slider mechanism and a four-bar mechanism. The crank slider mechanism is arranged on the base 31 and includes a rotating disk. 321. Connecting rod 322 and slider 323. The rotating disk 321 is rotatably installed on the end face of the base 31 and the slider 323 is provided slidingly on the corresponding end face of the base 31 (as shown in Figure 2, that is, the upper end face of the base 31). One end of the connecting rod 322 is connected to the rotating The outer ring of the disk 321 is rotationally connected, and the other end is rotationally connected to the top surface of the slider 323, so that the rotation of the slider 323 is controlled by the rotation of the rotating disk 321; a steering gear 310 is fixedly provided on the bottom surface of the base 31, and the output shaft of the steering gear 310 is connected to the rotating disk 321 is coaxial and the rotating disk 321 is fixedly sleeved on the outer wall of the output shaft of the steering gear 310; a sliding guide rail is provided on the base 31 corresponding to the slider 323, and the slider 323 is stuck on the slide guide rail and is slidingly connected, thereby ensuring the stability of the slider 323. The sliding direction forms a hard limit. The four-bar mechanism is a symmetrically arranged two groups, including a first connecting rod 331, an "L" shaped connecting rod 332, a "7" shaped connecting rod 333 and a second connecting rod 334. The slider 323 has one end away from the connecting rod 322 and corresponds to two The four-bar mechanism is provided with a mounting block 3230. The first connecting rods 331 of the two groups of four-bar mechanisms are respectively rotatably connected to the mounting block 3230. One end of the "L" shaped connecting rod 332 is rotatably connected to the corresponding first link 331, and the other end is rotatably connected to the corresponding first link 331. One end is rotatably connected to one end of the corresponding "7"-shaped link 333, and the corner of the "L"-shaped link 332 is rotatably connected to the base 31 through the first rotating shaft provided on the base 31 (i.e., as shown in Figure 2: The first rotating shaft is rotatably arranged on the end surface of the base 31, the corners of the "L" shaped connecting rods 332 are fixedly connected to the corresponding outer walls of the first rotating shaft), and the two second connecting rods 334 are arranged between the two "L" shaped connecting rods 332. In addition, one end of the second connecting rod 334 is rotatably connected to the end surface of the base 31 through a second rotating shaft provided on the base 31 (that is, as shown in Figure 2: the second rotating shaft is rotatably provided on the end surface of the base 31, and one end of the second connecting rod 334 is fixed. sleeved on the outer wall of the second rotating shaft), and the other end is connected to the corresponding "7" shaped connecting rod 333 in a corner rotation. The opposite sides of the "7" shaped connecting rod 333 are respectively provided with clamping strips 3332, and the end surface of the "7" shaped connecting rod 333 located above the clamping strip 3332 is provided with a blade 3331 (as shown in Figure 2).
视觉感知系统包括深度相机40与定位模块，深度相机40采用RealSense D435i相机，其安装在末端执行器30上(具体可为底座31端面)、跟随末端执行器30一同运动；避障路径规划系统与定位模块集成在中控装置50中，中控装置50安装在履带式底盘10上且分别与履带式底盘10、七自由度机械臂组件20、末端执行器30、深度相机40电连接。The visual perception system includes a depth camera 40 and a positioning module. The depth camera 40 uses a RealSense D435i camera, which is installed on the end effector 30 (specifically, it can be the end surface of the base 31) and moves together with the end effector 30; the obstacle avoidance path planning system and The positioning module is integrated in the central control device 50. The central control device 50 is installed on the crawler chassis 10 and is electrically connected to the crawler chassis 10, the seven-degree-of-freedom robotic arm assembly 20, the end effector 30, and the depth camera 40 respectively.
履带式底盘10上设置用于采摘后水果存放的水果篮11。The crawler chassis 10 is provided with a fruit basket 11 for storing fruits after picking.
实施例2：Example 2:
一种七自由度水果采摘机器人的采摘方法，采用如实施例1所示的水果采摘机器人，其特征在于：包括采摘点与障碍物定位算法、手眼标定算法与路径规划算法；采摘点与障碍物定位算法与手眼标定算法由定位模块进行运算，路径规划算法由避障路径规划系统进行运算。A picking method for a seven-degree-of-freedom fruit picking robot, using the fruit picking robot shown in Embodiment 1, which is characterized by: including a picking point and obstacle positioning algorithm, a hand-eye calibration algorithm and a path planning algorithm; picking points and obstacles The positioning algorithm and hand-eye calibration algorithm are calculated by the positioning module, and the path planning algorithm is calculated by the obstacle avoidance path planning system.
采摘点与障碍物定位算法具体为：The picking point and obstacle positioning algorithm is specifically:
首先，使用深度相机采集分辨率为640*480像素的RGB图像与深度图像，并应用语义分割网络DeepLabV3+对RGB图像进行分割；First, use a depth camera to collect RGB images and depth images with a resolution of 640*480 pixels, and apply the semantic segmentation network DeepLabV3+ to segment the RGB images;
之后，对图像中的每一个像素均打上标签，即背景为0，连通域分别打上1、2、…、N-1、N的标签，同一个连通域的像素打上同样的标签；若物体存在重叠、会把不同物体的多个连通域计为一个连通域，从而提取果实连通域；After that, each pixel in the image is labeled, that is, the background is 0, the connected domains are labeled 1, 2,..., N-1, N respectively, and the pixels in the same connected domain are labeled with the same label; if the object exists Overlapping will count multiple connected domains of different objects as one connected domain, thereby extracting the fruit connected domain;
再结合深度图像，将各个果实连通域转换为三维点云、同时将障碍物二值化区域转换为三维点云，具体为：Combined with the depth image, each fruit connected domain is converted into a three-dimensional point cloud, and the obstacle binarized area is converted into a three-dimensional point cloud, specifically as follows:
式中，(xi,yi,zi)表示像素点i的三维点云坐标；(ui,vi)表示乔木类水果果实区域中像素点i的图像坐标；(u0,v0)表示深度相机的光心位置，通过前期图像标定获得；(fx,fy)表示深度相机的焦距，通过前期图像标定获得；Id表示获得的深度图像。In the formula, (xi , yi , zi ) represents the three-dimensional point cloud coordinates of pixel point i; (ui , vi) represents the image coordinates of pixel point i in the fruit area of tree fruits; (u0 , v0 ) represents the optical center position of the depth camera, which is obtained through preliminary image calibration; (fx , fy ) represents the focal length of the depth camera, which is obtained through preliminary image calibration; Id represents the obtained depth image.
应用统计分析法移除三维点云中的噪声，具体为：获得三维点云后，计算三维点云中每个点到其所有临近点的平均距离接着计算每个点到其所有临近点的距离、将其和平均距离作差/>然后求平方，再求平均数得到方差，用方差开根号得到标准偏差，然后利用统计分析法中的拉依达检验法：即如果可疑数据xp与实验数据的算术平均值/>之间的偏差的绝对值大于三倍的标准偏差，则认为是离群点、进行删除；若不是(即与上述情况相反)，则保留。Apply statistical analysis methods to remove noise in the three-dimensional point cloud, specifically: after obtaining the three-dimensional point cloud, calculate the average distance from each point in the three-dimensional point cloud to all its adjacent points Then calculate the distance between each point and all its adjacent points, and compare it with the average distance/> Then find the square, then find the average to get the variance, use the square root of the variance to get the standard deviation, and then use the Raida test method in statistical analysis: that is, if the suspicious data xp is the arithmetic mean of the experimental data/> If the absolute value of the deviation is greater than three times the standard deviation, it is considered an outlier and deleted; if not (that is, contrary to the above situation), it is retained.
然后，使用最小二乘法对三维点云中的果实连通域进行球体拟合，获得果实中心点位置和半径值；具体步骤为：Then, use the least squares method to perform sphere fitting on the fruit connected domain in the three-dimensional point cloud to obtain the fruit center point position and radius value; the specific steps are:
首先建立球体方程：First establish the equation of the sphere:
然后，假设第i个果实的三维点云坐标为(xi,yi,zi)，带入球体方程中，可得：Then, assuming that the three-dimensional point cloud coordinates of the i-th fruit are (xi , yi , zi ), and put them into the sphere equation, we can get:
令：make:
则：but:
(由于(MT·M)-1＝M-1·(MT)-1,(MT)-1·MT＝1)；(Since (MT ·M)-1 =M-1 ·(MT )-1 , (MT )-1 ·MT =1);
经上述各点的计算能够获得球心坐标(x0,y0,z0)与球体半径r0；Through the calculation of the above points, the coordinates of the center of the sphere (x0 , y0 , z0 ) and the radius of the sphere r0 can be obtained;
之后，预设合理范围值H，其中H为0～0.5、通过大量前期实验数据获得，计算三维点云中每个点到球心坐标(x0,y0,z0)的距离h0，若|h0-r0|＜H，则该点在拟合球体内，依次记录整个拟合球体内的点的数量；After that, a reasonable range value H is preset, where H is 0 to 0.5. It is obtained through a large amount of preliminary experimental data, and the distance h0 from each point in the three-dimensional point cloud to the sphere center coordinates (x0 , y0 , z0 ) is calculated. If |h0 -r0 |<H, then the point is in the fitting sphere, and the number of points in the entire fitting sphere is recorded in sequence;
重复多次进行球体拟合，选取模型内点的数量最多的球体对应的模型参数为最佳拟合参数，输出最佳拟合参数的球心坐标(x,y,z)及半径r作为果实的三维拟合结果。Repeat the sphere fitting multiple times, select the model parameters corresponding to the sphere with the largest number of internal points in the model as the best fitting parameters, and output the sphere center coordinates (x, y, z) and radius r of the best fitting parameters as the fruit. The three-dimensional fitting results.
最终，以果实中心点铅垂向上一个半径值的位置作为果实采摘点；Finally, the position of a radius value vertically upward from the center point of the fruit is used as the fruit picking point;
之后，应用体素栅格化的点云简化法对上述获得的三维点云进行体素化处理，实现使用大量立方体逼近障碍物和果实的空间分布；具体为：After that, the point cloud simplification method of voxel rasterization is applied to voxelize the three-dimensional point cloud obtained above, so as to use a large number of cubes to approximate the spatial distribution of obstacles and fruits; specifically:
首先，根据点云创建一个最小三维体素长方体，其体积为V：First, create a minimum three-dimensional voxel cuboid based on the point cloud, with a volume V:
V＝Lx·Ly·Lz；V=Lx ·Ly ·Lz ;
式中：Lx表示点云X轴方向最大范围；Ly表示点云Y轴方向最大范围；Lz表示点云Z轴方向最大范围；In the formula: Lx represents the maximum range of the point cloud in the X-axis direction; Ly represents the maximum range of the point cloud in the Y-axis direction; Lz represents the maximum range of the point cloud in the Z-axis direction;
然后，计算需要划分的小立方栅格的边长L，根据L的大小将最小三维体素长方体分解成个小立方体格栅；栅格划分完毕后，将点云数据放到相应的小格栅中，同时删除那些不包含数据点的小格栅；在每个小栅格中，将离小格栅中心最近的数据点保留下来，代表本小格栅中的所有点，删除其余点。Then, calculate the side length L of the small cubic grid that needs to be divided, and decompose the minimum three-dimensional voxel cuboid into small cubic grids; after the grid is divided, place the point cloud data into the corresponding small grids, and delete those small grids that do not contain data points; in each small grid, separate the small grids The data point closest to the center is retained, representing all points in this small grid, and the remaining points are deleted.
手眼标定算法用于求解相机与末端执行器之间的位姿转换关系、从而获得末端执行器相对于采摘目标之间的坐标转换关系；其具体步骤为：The hand-eye calibration algorithm is used to solve the pose transformation relationship between the camera and the end effector, thereby obtaining the coordinate transformation relationship between the end effector and the picking target; the specific steps are:
首先，根据标定板棋盘格的所有角点的像素值、深度值和深度相机的参数，通过像素坐标系与世界坐标系之间的转换公式进行转换，从而获得标定板棋盘格上所有角点的空间坐标，转换为对应的点的坐标数组，同时将其与所拍照的图片中所对应的所有角点、在图像坐标系下的成像点的坐标数组记录为多组控制点；转换公式为：First, according to the pixel values, depth values and depth camera parameters of all corner points of the calibration board checkerboard, the conversion formula between the pixel coordinate system and the world coordinate system is used to obtain the values of all corner points on the calibration board checkerboard. The spatial coordinates are converted into the coordinate array of the corresponding point, and at the same time, the coordinate arrays of all the corner points corresponding to the photographed picture and the imaging point in the image coordinate system are recorded as multiple sets of control points; the conversion formula is:
式中：X、Y、Z表示世界坐标系下的坐标；f表示焦距；R表示3x3的正交旋转矩阵；t表示三维平移向量；u0、v0表示图像坐标系原点在像素坐标系统的坐标；dx、dy表示每个像素在图像平面x方向、y方向的物理尺寸；u、v表示像素坐标；ZC表示相机坐标中Z轴的向量；In theformula: Coordinates; dx and dy represent the physical size of each pixel in the x and y directions of the image plane; u and v represent the pixel coordinates; ZC represents the vector of the Z axis in the camera coordinates;
然后，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标，获得摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，从而获得多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T中的旋转和平移量；Then, using the coordinates of multiple control points in the three-dimensional scene and their perspective projection coordinates in the image, the absolute pose relationship between the camera coordinate system and the world coordinate system representing the three-dimensional scene structure is obtained, including the absolute translation vector t and Rotate the matrix R to obtain the rotation and translation amounts in the transformation matrix T from the spatial coordinates of all corner points on the multiple sets of calibration plate checkerboards to the camera coordinate system;
之后确定机械臂DH参数表，如下表1所示：Then determine the robot arm DH parameter table, as shown in Table 1 below:
表中：ai(mm)表示公共法线间的距离；ai(°)表示公共法线间的距离ai(mm)和垂直于ai(mm)所在平面内两轴的夹角；di(mm)表示两根连杆之间的相对位置；θi(°)表示两根连杆的相对位置di di(mm)和两根连杆公垂线之间的夹角。In the table: ai (mm) represents the distance between common normals; ai (°) represents the distance between common normals ai (mm) and the angle between the two axes in the plane perpendicular to ai (mm); di (mm) represents the relative position between the two connecting rods; θi (°) represents the angle between the relative position di di (mm) of the two connecting rods and the common vertical line of the two connecting rods.
再然后，通过网口通信读出平移位置，计算出x、y、z三轴上的单位矢量a：Then, read the translation position through network port communication, and calculate the unit vector a on the x, y, and z axes:
同时也可以读出每个机械臂此时的DH参数，由于相邻两根连杆Ti与Ti-1的变换关系式为：At the same time, the DH parameters of each robotic arm at this time can also be read, because the transformation relationship between the two adjacent linksTi and Ti-1 is:
式中：ai表示公共法线间的距离与垂直于所在平面内两轴的夹角；di表示两根连杆的相对位置；θi表示两根连杆的相对位置di和两根连杆公垂线的夹角；c表示三角函数中的cos()函数；s表示三角函数中的sin()函数；In the formula: ai represents the distance between the common normals and the angle between the two axes perpendicular to the plane; di represents the relative position of the two connecting rods; θi represents the relative position of the two connecting rods di and the two The angle between the common vertical lines of the connecting rod; c represents the cos() function in trigonometric functions; s represents the sin() function in trigonometric functions;
多次移动七自由度机械臂组件，获得多组末端执行器相对于基坐标的旋转和平移量；将末端执行器相对于基坐标的多组旋转和平移参数记作多组姿态矩阵B，将上述多组标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T视为外参矩阵A，之后，得到组A、B，利用AX＝XB计算得到相机坐标系到七自由度机械臂组件末端坐标系的旋转和平移变换，从而获得相机坐标系到七自由度机械臂组件末端坐标系的变换矩阵Te；Move the seven-degree-of-freedom manipulator component multiple times to obtain multiple sets of rotation and translation amounts of the end effector relative to the base coordinates; record the multiple sets of rotation and translation parameters of the end effector relative to the base coordinates as multiple sets of attitude matrices B, and The transformation matrix T from the spatial coordinates of all corner points on the above multiple sets of calibration board checkerboards to the camera coordinate system is regarded as the external parameter matrix A. After that, we get For groups A and B, use AX=XB to calculate the rotation and translation transformation from the camera coordinate system to the end coordinate system of the seven-degree-of-freedom robotic arm assembly, thereby obtaining the transformation matrix Te from the camera coordinate system to the end-coordinate system of the seven-degree-of-freedom robotic arm assembly. ;
最后，移动七自由度机械臂组件、并使用深度相机对采摘目标进行拍照，像素坐标系与世界坐标系之间的转换公式获得采摘目标的空间坐标；再利用变换矩阵Te以及标定板棋盘格上所有角点的空间坐标到相机坐标系的变换矩阵T，获得采摘目标相对于末端执行器坐标系下的位姿。最后还可将采摘目标相对于末端执行器坐标系下的位姿转换为相对于机械臂基坐标系坐标下的位姿，从而让机械臂能更好的向采摘目标靠近。Finally, move the seven-degree-of-freedom robotic arm assembly and use a depth camera to take pictures of the picking target. The conversion formula between the pixel coordinate system and the world coordinate system obtains the spatial coordinates of the picking target; then use the transformation matrix Te and the calibration board checkerboard The transformation matrix T from the spatial coordinates of all corner points on the camera coordinate system to the camera coordinate system is used to obtain the pose of the picking target relative to the end effector coordinate system. Finally, the position and posture of the picking target relative to the end-effector coordinate system can be converted into a position and posture relative to the robot arm's base coordinate system, so that the robot arm can better approach the picking target.
路径规划算法具体为：首先，在初始点与目标点之间随机选取2～3个点，对选择的点位进行判断、看其是否处于障碍物上，若处于，则更换对应的随机点；确定随机点Qr后，将随机点与初始点、目标点形成多棵搜索树，同时沿随机点Qr方向以单位步长进行扩展，寻找到新节点Qn，再以新节点重复上述步骤，期间同样要考虑与障碍物有无碰撞、步长等，直至多棵搜索树同时存在相互连接(多棵搜索树同时存在相互连接说明找到路径、完成规划)；The specific path planning algorithm is: first, randomly select 2 to 3 points between the initial point and the target point, judge the selected point to see if it is on an obstacle, and if it is, replace the corresponding random point; After determining the random point Qr , the random point, the initial point, and the target point are formed into multiple search trees. At the same time, the random point Qr is expanded with a unit step size in the direction of the random point Q r to find the new node Qn, and then the above steps are repeated with the new node. During this period, we must also consider whether there is a collision with obstacles, step length, etc., until multiple search trees are connected to each other at the same time (the existence of multiple search trees to be connected to each other at the same time means that the path is found and the planning is completed);
为了优化路径较少不必要的转折，路径规划算法中，在搜索树集合Tree中的节点中任取两点Qi与Qj，其中i,j∈[1,2,3,…,n]；对Qi与Qj间的路径进行碰撞检测，若无碰撞在，则删除Qi与Qj之间的所有节点。In order to optimize the path with fewer unnecessary turns, in the path planning algorithm, two points Qi and Qj are randomly selected from the nodes in the search tree set Tree , where i, j∈[1,2,3,…,n ]; Perform collision detection on the path between Qi and Qj . If there is no collision, delete all nodes between Qi and Qj .
其中，碰撞检测为：采用向空间坐标系的三个坐标轴上进行投影的方式、对立方体和圆柱体之间进行碰撞检测，若投影后立方体和圆柱体连接中心点的线段的投影长度大于各自中心点投影后到各自最长边界的距离和，且三个轴同时满足下，则视为立方体和圆柱体不会发生碰撞；Among them, the collision detection is: using the method of projecting onto the three coordinate axes of the spatial coordinate system to detect the collision between the cube and the cylinder. If the projection length of the line segment connecting the center points of the cube and cylinder after projection is greater than their respective The sum of the distances from the center point to the longest boundary after projection, and if the three axes are satisfied at the same time, the cube and cylinder will be considered to not collide;
具体为：Specifically:
首先，找到立方体的四个顶点、并通过对其坐标求平均的方法获得立方体为投影时的中心点Plm；再找到圆柱体的上下底面圆心坐标、并通过对其求平均的方法获得圆柱体未投影时的中心点Pym；然后，计算获得中心点Plm与中心点Pym之间的连线ld、其在x轴上的投影为lD，同时，分别获得中心点Plm与中心点Pym在x轴上的投影PlM与PyM；之后，计算PlM与PyM到各自物体(即立方体、圆柱体)投影后的边界的距离ra与rb：First, find the four vertices of the cube and average their coordinates to obtain the center point Plm when the cube is projected; then find the coordinates of the center points of the upper and lower bases of the cylinder and average them to obtain the cylinder. The center point Pym when not projected; then, calculate and obtain the connection line ld between the center point Plm and the center point Pym , and its projection on the x-axis is lD. At the same time, obtain the center point Plm and the center point P ym respectively. The projections PlM and PyM of the center point Pym on the x-axis; then, calculate the distances r a andr bfrom PlM and PyM to the projected boundaries of their respective objects (i.e., cube, cylinder):
若|lD|＞ra+rb，且在y轴、z轴上也满足获得的|lD|＞ra+rb，则立方体与圆柱体无碰撞；否则，立方体与圆柱体存在碰撞。If |lD |＞ra +rb , and the obtained |lD |＞ra +rb is also satisfied on the y-axis and z-axis, then there is no collision between the cube and the cylinder; otherwise, the cube and the cylinder exist collision.
采摘机器人进行果实采摘的具体方法为：The specific method for fruit picking by picking robots is:
首先，整个乔木类水果采摘机器人由履带式底盘10驱动、向前移动到某一采摘位置，同时中控装置50对末端执行器30进行位置初始化，即末端执行器30三维空间坐标的初始化；且七自由度机械臂组件20进行归零操作，根据单片机算法，获得乔木类水果果实在水平、竖直、前后方向上相对于末端执行器30(初始化位置)的数据；之后，中控装置50通过视觉感知系统获取果实的采摘点、并利用避障路径规划系统获得末端执行器30的采摘路径First, the entire tree-type fruit picking robot is driven by the crawler chassis 10 and moves forward to a certain picking position. At the same time, the central control device 50 initializes the position of the end effector 30, that is, the three-dimensional spatial coordinates of the end effector 30 are initialized; and The seven-degree-of-freedom robotic arm assembly 20 performs a zeroing operation, and according to the single-chip computer algorithm, obtains the data of the arbor fruits relative to the end effector 30 (initialization position) in the horizontal, vertical, and front-to-back directions; after that, the central control device 50 passes The visual perception system obtains the picking point of the fruit, and uses the obstacle avoidance path planning system to obtain the picking path of the end effector 30
末端执行器30经过一自由度机械臂21与六自由度机械臂22的水平与竖直的调整、到达一定采摘位置，即使得果实茎部位于末端执行器30的两个刀片3331之下剪切空间内；此时，中控装置50在控制舵机310的运转，舵机310通过转动盘321与连接杆322拉动滑块323向靠近舵机310的一侧运动(即从远点向近点运动)，滑块323通过安装块3230拉动第一连杆331运动、进而使得“L”形连杆332在此机构中为旋转副进行旋转运动，“L”形连杆332推动“7”字形连杆333运动、且“7”字形连杆333收到第二连杆334的位移限制，进而使得两根“7”字形连杆333向相互靠近的方向移动，实现对于果实茎部的剪切。切断完成中，刀片3331下侧的夹紧胶条3332处于夹紧状态，对果实的果梗进行夹持，避免果实切断后掉落。之后，启动七自由度机械臂组件20使得末端执行器30位于水果篮11上方(此处由于最终的位置是确定的，因此控制可通过限位传感器实现、也可通过距离控制实现，为本领域的常规技术，本申请具体实施方式不做过多论述)，继续启动舵机310运行，滑块323向远离舵机310的一侧运动(即从近点向远点运动)，实现“7”字形连杆333的打开，果实掉落在水果篮11中、实现收集；如此循环完成水果的采集工作。The end effector 30 is adjusted horizontally and vertically by the one-degree-of-freedom mechanical arm 21 and the six-degree-of-freedom mechanical arm 22 to reach a certain picking position, that is, the fruit stem is sheared under the two blades 3331 of the end effector 30 In the space; at this time, the central control device 50 is controlling the operation of the steering gear 310. The steering gear 310 pulls the slider 323 through the rotating disk 321 and the connecting rod 322 to move to the side closer to the steering gear 310 (that is, from the far point to the near point). movement), the slider 323 pulls the first link 331 to move through the mounting block 3230, thereby causing the "L"-shaped link 332 to perform rotational movement for the rotating pair in this mechanism, and the "L"-shaped link 332 pushes the "7" shape The connecting rod 333 moves, and the "7"-shaped connecting rod 333 is restricted by the displacement of the second connecting rod 334, thereby causing the two "7"-shaped connecting rods 333 to move in a direction closer to each other, thereby achieving shearing of the fruit stem. . When the cutting is completed, the clamping strip 3332 on the lower side of the blade 3331 is in a clamping state to clamp the stem of the fruit to prevent the fruit from falling after cutting. After that, the seven-degree-of-freedom robotic arm assembly 20 is started so that the end effector 30 is located above the fruit basket 11 (since the final position is determined here, the control can be achieved through a limit sensor or distance control, which is a common problem in this field. Conventional technology, the specific implementation of this application will not be discussed too much), continue to start the operation of the steering gear 310, and the slider 323 moves to the side away from the steering gear 310 (that is, moves from the near point to the far point), realizing "7" When the glyph connecting rod 333 is opened, the fruits fall into the fruit basket 11 and are collected; in this way, the fruit collection work is completed in a cycle.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310493761.4ACN116439018B (en) | 2023-05-05 | 2023-05-05 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310493761.4ACN116439018B (en) | 2023-05-05 | 2023-05-05 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| Publication Number | Publication Date |
|---|---|
| CN116439018A CN116439018A (en) | 2023-07-18 |
| CN116439018Btrue CN116439018B (en) | 2024-01-02 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310493761.4AActiveCN116439018B (en) | 2023-05-05 | 2023-05-05 | Seven-degree-of-freedom fruit picking robot and picking method thereof |
| Country | Link |
|---|---|
| CN (1) | CN116439018B (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US12084296B2 (en)* | 2020-09-14 | 2024-09-10 | Yamaha Hatsudoki Kabushiki Kaisha | Movable harvesting apparatus and harvesting unit |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116810800A (en)* | 2023-08-18 | 2023-09-29 | 苏州博田自动化技术有限公司 | Agricultural picking robot training method and system based on vision fusion |
| CN117021059B (en)* | 2023-10-09 | 2024-02-06 | 北京市农林科学院智能装备技术研究中心 | Picking robot and fruit positioning method, device, electronic equipment and medium |
| CN117397462A (en)* | 2023-10-19 | 2024-01-16 | 农业农村部南京农业机械化研究所 | Fruit picking method, device, medium and electronic equipment |
| CN117765085B (en)* | 2024-02-22 | 2024-06-07 | 华南农业大学 | Tea bud leaf picking far and near alternate positioning method |
| CN119238470A (en)* | 2024-10-21 | 2025-01-03 | 江苏大学 | A method and device for safe motion positioning of rigid-flexible hybrid robotic arm with visual-tactile fusion in canopy |
| CN119924087B (en)* | 2025-02-11 | 2025-09-16 | 仲恺农业工程学院 | Fruit picking robot navigation system based on fuzzy immune-PID control |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN205305450U (en)* | 2016-01-13 | 2016-06-15 | 浙江大学 | A end effector of robot for tomato bud picking |
| CN208305031U (en)* | 2018-05-02 | 2019-01-01 | 浙江工贸职业技术学院 | Fruit picking machine hand |
| CN109839110A (en)* | 2019-01-09 | 2019-06-04 | 浙江大学 | A kind of multiple target point path planning method based on quick random search tree |
| CN209755248U (en)* | 2018-11-28 | 2019-12-10 | 东莞理工学院 | An end effector of a picking manipulator |
| CN112132894A (en)* | 2020-09-08 | 2020-12-25 | 大连理工大学 | A real-time tracking method of robotic arm based on binocular vision guidance |
| CN113733088A (en)* | 2021-09-07 | 2021-12-03 | 河南大学 | Mechanical arm kinematics self-calibration method based on binocular vision |
| CN114067206A (en)* | 2021-11-16 | 2022-02-18 | 哈尔滨理工大学 | Spherical fruit identification and positioning method based on depth image |
| CN114260895A (en)* | 2021-12-22 | 2022-04-01 | 江苏大学 | Method and system for determining grabbing obstacle avoidance direction of mechanical arm of picking machine |
| WO2022212830A2 (en)* | 2021-04-02 | 2022-10-06 | Appharvest Technology, Inc. | Robotic fruit harvesting system |
| CN115250745A (en)* | 2022-08-05 | 2022-11-01 | 华南农业大学 | Full-automatic fruit picking robot and picking method based on vision technology |
| CN115958590A (en)* | 2021-12-24 | 2023-04-14 | 合肥哈工图南智控机器人有限公司 | A RRT-based method and device for robot arm deep frame obstacle avoidance motion planning |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DK3537867T3 (en)* | 2016-11-08 | 2023-11-06 | Dogtooth Tech Limited | ROBOT FRUIT PICKING SYSTEM |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN205305450U (en)* | 2016-01-13 | 2016-06-15 | 浙江大学 | A end effector of robot for tomato bud picking |
| CN208305031U (en)* | 2018-05-02 | 2019-01-01 | 浙江工贸职业技术学院 | Fruit picking machine hand |
| CN209755248U (en)* | 2018-11-28 | 2019-12-10 | 东莞理工学院 | An end effector of a picking manipulator |
| CN109839110A (en)* | 2019-01-09 | 2019-06-04 | 浙江大学 | A kind of multiple target point path planning method based on quick random search tree |
| CN112132894A (en)* | 2020-09-08 | 2020-12-25 | 大连理工大学 | A real-time tracking method of robotic arm based on binocular vision guidance |
| WO2022212830A2 (en)* | 2021-04-02 | 2022-10-06 | Appharvest Technology, Inc. | Robotic fruit harvesting system |
| CN113733088A (en)* | 2021-09-07 | 2021-12-03 | 河南大学 | Mechanical arm kinematics self-calibration method based on binocular vision |
| CN114067206A (en)* | 2021-11-16 | 2022-02-18 | 哈尔滨理工大学 | Spherical fruit identification and positioning method based on depth image |
| CN114260895A (en)* | 2021-12-22 | 2022-04-01 | 江苏大学 | Method and system for determining grabbing obstacle avoidance direction of mechanical arm of picking machine |
| CN115958590A (en)* | 2021-12-24 | 2023-04-14 | 合肥哈工图南智控机器人有限公司 | A RRT-based method and device for robot arm deep frame obstacle avoidance motion planning |
| CN115250745A (en)* | 2022-08-05 | 2022-11-01 | 华南农业大学 | Full-automatic fruit picking robot and picking method based on vision technology |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US12084296B2 (en)* | 2020-09-14 | 2024-09-10 | Yamaha Hatsudoki Kabushiki Kaisha | Movable harvesting apparatus and harvesting unit |
| Publication number | Publication date |
|---|---|
| CN116439018A (en) | 2023-07-18 |
| Publication | Publication Date | Title |
|---|---|---|
| CN116439018B (en) | Seven-degree-of-freedom fruit picking robot and picking method thereof | |
| Luo et al. | Vision-based extraction of spatial information in grape clusters for harvesting robots | |
| Nielsen et al. | Vision-based 3D peach tree reconstruction for automated blossom thinning | |
| US20240165807A1 (en) | Visual servoing of a robot | |
| CN114830915B (en) | Lychee visual picking robot based on lidar navigation and its implementation method | |
| CN108648274B (en) | A cognitive point cloud map creation system for visual SLAM | |
| CN114067309B (en) | Sweet pepper identification and picking sequence determination method based on multi-view three-dimensional reconstruction | |
| CN104331924B (en) | Three-dimensional rebuilding method based on single camera SFS algorithms | |
| CN114260895B (en) | A method and system for determining the direction of grabbing and avoiding obstacles by the mechanical arm of a picking robot | |
| CN106826833A (en) | Independent navigation robot system based on 3D solid cognition technologies | |
| CN112529948A (en) | Mature pomegranate positioning method based on Mask R-CNN and 3-dimensional sphere fitting | |
| CN117021059B (en) | Picking robot and fruit positioning method, device, electronic equipment and medium | |
| CN110433467A (en) | Picking up table tennis ball robot operation method and equipment based on binocular vision and ant group algorithm | |
| Zou et al. | Density estimation method of mature wheat based on point cloud segmentation and clustering | |
| CN115439746A (en) | Tomato stem identification and three-dimensional reconstruction method based on deep learning | |
| Choi et al. | NeRF-based 3D reconstruction pipeline for acquisition and analysis of tomato crop morphology | |
| CN110060304A (en) | A kind of organism three-dimensional information acquisition method | |
| Burusa et al. | Attention-driven next-best-view planning for efficient reconstruction of plants and targeted plant parts | |
| Li et al. | Multi-vision-based localization and pose estimation of occluded apple fruits for harvesting robots | |
| CN117011843A (en) | Image recognition and posture assessment method for automatic picking of dragon fruits | |
| Wei et al. | Fast Multi-View 3D reconstruction of seedlings based on automatic viewpoint planning | |
| Paturkar et al. | Overview of image-based 3D vision systems for agricultural applications | |
| CN115829924B (en) | 3D pose detection algorithm for oblate spheroid fruits based on key point detection | |
| CN113313741B (en) | Point cloud self-registration method based on calibration sphere | |
| Karty et al. | 3d data collection for individual plant farming |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |