Spatial heterogeneous visual positioning system of tomato picking robot

Abstract

The invention provides a spatial heterogeneous visual positioning system of a tomato picking robot, and relates to the field of deep learning, and the spatial heterogeneous visual positioning system comprises the following steps: S1, calibrating the positioning of the tomato picking robot; s2, acquiring a tomato image in a non-closed environment through a fixed camera; step S3, obtaining fruit space coordinate information under the field of view of the fixed camera; s4, judging the reachable space of the picking mechanical arm, and moving the picking mechanical arm; s5, carrying out secondary spatial information perception on the target by adopting a palm center camera, and further obtaining spatial coordinate information in a relatively closed environment; s6, performing approximate ball fitting to obtain an optimal picking object; and S7, tomato number judgment and picking pose estimation are carried out. According to the system, the fixed camera and the palm center camera are combined, the characteristics of tomato growth can be effectively handled, the positioning precision and the real-time performance are improved, the higher picking success rate and efficiency are achieved, and damage occurring in the picking process is avoided.

Description

Technical Field

[0001] The present invention relates to the field of deep learning technology, and in particular to a spatial heterogeneous vision positioning system for a tomato picking robot. Background Art

[0002] In modern agriculture, the application of automation and intelligent technology has gradually become an important means to improve production efficiency and reduce labor costs; especially in the field of fruit and vegetable picking. With the increasing demand for fresh fruits and vegetables, traditional manual picking methods face many problems such as low efficiency, high labor intensity, easy damage to fruits, and easy to miss the best picking period.

[0003] In the prior art, in the field of tomato picking, there are already some picking robots that rely on visual positioning technology and robotic arm control systems to achieve automatic recognition and grasping of tomatoes; for example: Chinese patent document CN118941960A discloses a composite picking method for bunched tomatoes based on deep learning, and Chinese patent CN118887402A discloses a tomato segmentation method based on an improved YOLOv8 model. These systems use cameras to take pictures and capture picking targets in the orchard, and use deep learning algorithms (mainly YOLO target detection networks) to complete the recognition of tomato positions, thereby completing the control of the picking robotic arm and realizing the picking of tomato fruits; however, these solutions mainly focus on the positioning and grasping of fruits, and fail to fully consider the growth characteristics of tomatoes and the complexity of their cluster growth. Specifically: the existing visual system is easily restricted in the acquisition of depth information due to the complexity of the tomato growing environment (such as light changes, obstructions, etc.), which leads to insufficient positioning accuracy of tomatoes; at the same time, in the scenario of picking multiple fruits in a tomato bunch, the existing picking system lacks real-time statistics and dynamic adjustment capabilities, and cannot flexibly adjust the picking strategy according to environmental changes and fruit status, resulting in collisions and damage to the fruit during the picking process, which in turn affects the efficiency and effectiveness of the overall picking operation and increases the defective rate of picked fruits. Summary of the invention

[0004] In view of the problems existing in the above-mentioned prior art, the purpose of the present invention is to provide a spatial heterogeneous visual positioning system for a tomato picking robot. The system combines a fixed camera with a palm camera, which can effectively cope with the structural characteristics of tomato growth, improve positioning accuracy and real-time performance, thereby achieving a higher picking success rate and picking efficiency, and avoiding damage during the picking process.

[0005] The purpose of the present invention is achieved through the following technical solutions: A spatially heterogeneous visual positioning system for a tomato picking robot, comprising: Step S1, designing a tomato picking robot, including a mobile chassis, a lifting column, a picking robot arm, a fixed camera, a palm camera, a gripper and a control system; at the same time, calibrating the positioning of the tomato picking robot, including hand-eye calibration of the picking robot arm and conversion between coordinate systems; Step S2, obtaining a tomato RGB image and a depth image in a non-enclosed environment by using a fixed camera; Step S3, using the improved yolov10 to detect the ripe tomato fruit, and obtaining the spatial coordinate information of the fruit under the field of view of the fixed camera; Step S4, optimizing the detected fruit depth distance, determining the accessible space of the picking robot arm, and moving the picking robot arm to the vicinity of the optimal target area; Step S5: Use a palm camera to perform secondary spatial information perception on the target, and further obtain spatial coordinate information in a relatively closed environment; Step S6, performing approximate sphere fitting on the tomato point cloud in the picking area to obtain the optimal picking object; Step S7: determine the number of tomatoes and estimate the picking posture.

[0006] Based on the further optimization of the above scheme, in the step S1, when the positioning of the tomato picking robot is calibrated, the positioning system includes the coordinate system {L} of the lifting column base, the coordinate system {R} of the picking robot arm, the coordinate system {Fc} of the fixed camera, the coordinate system {Pc} of the palm camera, the flange coordinate system {F}, the gripper coordinate system {G} and the given calibration plate coordinate system {C}, and the hand-eye calibration of the picking robot arm and the conversion between the coordinate systems include the hand-eye calibration of the fixed camera and the hand-eye calibration of the palm camera; Among them, the hand-eye calibration of the fixed camera is: Fix the calibration plate at the end of the picking robot arm and move the picking robot arm at multiple positions. In this process, keep the position relationship between the calibration plate and the end of the picking robot arm unchanged, and ensure that the relative position between the base of the picking robot arm and the fixed camera remains unchanged. The transformation relationship is:

[0007] Where: , Respectively represent i Group, j In group data a Coordinate system to b Homogeneous transformation matrix of coordinate system; By constructing a nonlinear equation system, the transformation matrix from the picking robot coordinate system {R} to the fixed camera coordinate system {Fc} is obtained. :

[0008] The hand-eye calibration of the palm camera is: Since the relative posture between the end of the picking robot arm and the calibration plate is always kept unchanged during the movement of the picking robot arm, the transformation relationship during the movement is:

[0009]

[0010] Similarly, by constructing a nonlinear equation system, the transformation matrix from the palm coordinate system {Pc} to the picking robot arm coordinate system {R} is obtained; In addition, for any picking robot arm posture, the transformation relationship between the fixed camera coordinate system {Fc}, the palm camera coordinate system {Pc} and the world coordinate system {L} is:

[0011] Where: is the constant transformation matrix of the coordinate system {L} relative to the coordinate system {R} determined by the assembly relationship between the picking robot arm and the lifting column; is the transformation matrix of coordinate system {R} relative to coordinate system {F} obtained by inverse kinematics.

[0012] Based on the further optimization of the above scheme, in step S3, the improved yolov10 includes using Dysample upsampling technology to improve the Neck part in the yolov10 network and introducing RMTBlock to improve the Bottleneck structure in the C2F module; Among them, the Dysample upsampling technology is used to improve the Neck part of the yolov10 network. First, the input feature map F in Apply a small-scale convolution and K d (usually 3x3), obtain the local feature response map R d :

[0013] Then, according to the local feature response map R d , get the dynamic upsampling scale factor :

[0014] Where: β represents the regulating factor; mean(R d ) Represents the local feature response map Rd Perform global average pooling to obtain the overall feature density; Finally, apply the dynamic upsampling scale factor , through bilinear interpolation method to input feature map F in Upsample to obtain the final upsampled output F up :

[0015] The Bottleneck structure in the C2F module is improved by introducing RMTBlock as follows: For the input feature map F in Use convolutions of different sizes and K d1 、K d3 、K d5 (1x1, 3x3, 5x5 respectively), and obtain the feature map of the response scale F d1 、F d3 、F d5 ; Then, the feature maps of different scales are weighted fused to obtain the fused feature map F merge :

[0016] Where: Represents the feature maps F d1 、F d3 、F d5 The weights are obtained through learning; Finally, use 1x1 convolution and fusion feature map F merge Compress and output refined features; The improved yolov10 includes collecting and organizing a large number of annotated tomato image datasets, which include ripe and unripe tomatoes under different environments and lighting conditions; after collecting and organizing the dataset, the dataset is enhanced (such as random cropping, rotation, scaling and color transformation) to improve the robustness of the model; model training is performed based on the improved yolov10 architecture, using pre-trained weights as the initial model, and fine-tuning on a custom tomato dataset to adapt to specific target detection tasks, selecting appropriate loss functions and optimizers, and setting learning rates and batch sizes to ensure that the model can converge effectively; finally, the trained model is applied to image recognition.

[0017] Based on the further optimization of the above scheme, the "determining the accessible space of the picking robot arm" in step S4 is specifically: detecting the coordinates of the center point of each tomato fruit and determining whether it is located in the working space of the picking robot arm, including: First, combine the fruit bounding box and obtain the center point of the corresponding bounding box ( x c ,y c ):

[0018] Where: x min 、x max Respectively represent the minimum and maximum horizontal coordinates in the bounding box; y min 、y max Respectively represent the minimum and maximum vertical coordinates in the bounding box; Then, a mathematical model of the picking robot arm workspace is established: Assume that the position that the picking robot's gripper can reach is represented by the vector PX Indicates that the joint angle of the picking robot arm is The position of the gripper is obtained by the forward kinematics formula:

[0019] Where: f(·) It is a function designed according to the kinematics of the picking robot arm, usually containing trigonometric functions and transformation matrices, which are used to describe the mapping from joint angles to the end effector positions; Define the picking robot arm workspace W:

[0020] Where: represents the possible range of all joint angles, R 3represents three-dimensional real number space; Afterwards, the center point of the bounding box ( x c ,y c ) Transform the coordinate system of the picking robot arm and obtain the corresponding point in the coordinate system of the picking robot arm :

[0021] Where: M represents the 4x4 homogeneous transformation matrix determined by the camera positioning parameters, installation angle and relative position; Finally, check the transformed coordinate points Is it located in the workspace W, that is, Whether it belongs to W; if the tomato fruit is not in the working space of the picking robot arm, replace the next target tomato.

[0022] Based on the further optimization of the above solution, the step S5 of "further acquiring spatial coordinate information in a relatively closed environment" includes segmenting the tomato fruit point cloud information through mask information, specifically: First, the corresponding tomato mask is obtained based on the tomato fruit frame detected by the palm camera; Then, the point cloud and image are registered:

[0023] Where: Represents a 2D pixel point in homogeneous coordinate form [ x,y,1 ] T ; X Indicates the corresponding 3D world coordinate point; K represents the intrinsic parameter matrix of the camera:

[0024] Where: f x , f y Indicates the focal length of the camera (i.e., pixel unit); c x , c y Represents the coordinates of the center of the image; [ R | t ] represents the external parameter matrix of the camera:

[0025] Where: R represents the rotation matrix, t Represents translation vectors, which describe the transformation from the world coordinate system to the camera coordinate system;r xy When the camera coordinate system is transformed to the world coordinate system, the rotation matrix is Axis and The rotational component about the axis; t x , t y , t z Represents the world coordinate system x, y, z Displacement of the axis; Finally, determine whether the point is in the mask foreground area based on the projection coordinates:

[0026] Where: Represents the coordinates of the 3D world coordinate point X projected onto the 2D image; like If it falls in the foreground area of ​​the 2D mask, the corresponding 3D world coordinate point is retained; otherwise, the corresponding 3D world coordinate point is deleted; After completing the segmentation of the tomato point cloud information, denoising, clustering and other operations are performed to obtain the point cloud information of the target fruit.

[0027] Based on the further optimization of the above solution, the step S6 of "approximate sphere fitting of the tomato point cloud in the area to be picked" is specifically as follows: First, for each point in the tomato discrete point cloud ( x i ,y i ,z i ) to fit the approximate spherical equation:

[0028] Where: x 0 ,y 0 ,z 0) represents the coordinates of the center of an approximate sphere; r represents the radius of the approximate sphere; After that, define the objective function S(x) :

[0029] right At the current iteration point x (k) Expand and keep only the first-order terms:

[0030] Where: express exist x(k) The i-th row of the Jacobian matrix at ; Then the objective function S(x) Approximately:

[0031] make J k Is J i,k Composition m x n The Vjacob matrix is:

[0032] Then the objective function is approximated Taking the derivative and equalizing it to zero, we get the iterative formula:

[0033] The approximate sphere equation is iteratively optimized through the above iterative formula, and finally the coordinates of the sphere center and the sphere radius are obtained.

[0034] Based on the further optimization of the above solution, the “picking posture estimation” in step S7 is specifically as follows: For the pose of a single tomato fruit: the spatial coordinates of the centroid of the tomato fruit and the stalk are obtained by point cloud segmentation and approximate sphere fitting using a palm camera. C 0. C 1 (where the centroid coordinates of the fruit are the coordinates of the approximate sphere center, and the centroid coordinates of the stalk are the center of the identification frame), C 0. C 1 and the origin of the initial gripper coordinate system O G Forming the growth reference surface, vector Indicates the direction of the fruit; According to the direction of the fruit, the Y-axis direction of the gripper coordinate system during picking is obtained:

[0035] After that, the X-axis direction basis of the gripper coordinate system is obtained:

[0036] Get the Z-axis direction basis based on the X-axis direction basis through the Y-axis direction:

[0037] At this time, the position of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} is:

[0038] Where: P 0 represents the three-dimensional spatial coordinates of the picking point; For multi-fruit tomato posture: Different from the pose estimation of a single-fruit tomato, when using the gripper to pick multiple-fruit tomatoes, if the picking pose is estimated by the fruit stalk and the tomato centroid, it is easy to collide with the fruit, causing damage or picking failure due to the direction of the Y-axis of the gripper coordinate system. Therefore, when picking multiple-fruit tomatoes, the single-fruit centroid is used to estimate the picking pose. C 0. Multi-point cloud sphere centroid C 2 and the origin of the initial gripper coordinate system O G Form the picking reference surface, the centroid of the multi-fruit point cloud sphere ( X,Y,Z ) Specifically:

[0039] Where: x i ,y i ,z i ) represents the centroid coordinates of each single fruit in a multi-fruit tomato. n represents the total number of fruits in multi-fruited tomatoes; At this time, the vector Indicates the Z-axis direction of the parallel gripper n Z2 , i.e. the gripping direction of the gripper, and obtain the direction bases of the gripper on the X-axis and Y-axis respectively n X2 , n Y2 :

[0040] Finally, the position and posture of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} are obtained: .

[0041] The following are the technical effects of the solution of the present invention: The present invention effectively solves the problem of insufficient positioning accuracy in the prior art by pre-positioning and calibrating the tomato picking robot, thereby improving the efficiency and success rate in the tomato picking process; the asynchronous positioning method combining a fixed camera with a palm camera significantly improves the target recognition capability of the picking robot in complex environments, which not only improves the recognition accuracy, but also effectively reduces the impact of external factors on positioning, ensuring the real-time statistics and dynamic adjustment capabilities of the picking robot. In addition, the present application comprehensively considers single-fruit tomato fruits and multi-fruit tomato fruits, and overcomes the problems of fruit damage and picking failure caused by mutual occlusion between fruits during the picking process by estimating the different picking postures of single-fruit tomato fruits and multi-fruit tomato fruits, thereby ensuring the coordination of multi-fruit picking and the safety of the fruits, further improving the picking efficiency and ensuring the quality of the picked fruits. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Flow chart of the spatial heterogeneous visual positioning system in an embodiment of the present invention.

[0043] Figure 2 It is a network structure diagram of the improved yolov10 in the embodiment of the present invention.

[0044] Figure 3 Schematic diagram of the key coordinate system and calibration relationship of the picking robot in an embodiment of the present invention.

[0045] Figure 4 The figure is a flow chart of obtaining the three-dimensional coordinates of the center of mass in an embodiment of the present invention.

[0046] Figure 5 Schematic diagram of picking posture in an embodiment of the present invention; wherein, Figure 5 (a) is a schematic diagram of the posture for picking a single tomato. Figure 5 (b) Schematic diagram of the picking posture of multi-fruit tomatoes.

[0047] Figure 6 Schematic diagram of picking postures of tomatoes at different growth heights in the implementation of the present invention. DETAILED DESCRIPTION

[0048] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0049] Embodiment 1: A spatially heterogeneous visual positioning system for a tomato picking robot, comprising: Step S1, designing a tomato picking robot, including a mobile chassis, a lifting column, a picking mechanical arm, a fixed camera, a palm camera, a gripper and a control system (wherein the lifting column and the fixed camera are both arranged on the end surface of the mobile chassis, the picking mechanical arm is arranged at the end of the lifting column, and the end of the picking mechanical arm away from the lifting column is provided with a gripper, a palm motor is arranged at the gripper, and the control system is arranged inside the mobile chassis; the fixed camera and the palm camera are both existing common RGB-D depth cameras); At the same time, the positioning of the tomato picking robot is calibrated, including the hand-eye calibration of the picking robot arm and the conversion between coordinate systems; Figure 3 As shown, specifically: when calibrating the positioning of the tomato picking robot, the positioning system includes the coordinate system of the lifting column base {L}, the coordinate system of the picking robot arm {R}, the coordinate system of the fixed camera {Fc}, the coordinate system of the palm camera {Pc}, the flange coordinate system {F}, the gripper coordinate system {G} and the given calibration plate coordinate system {C}, and the hand-eye calibration of the picking robot arm and the conversion between coordinate systems include the hand-eye calibration of the fixed camera and the hand-eye calibration of the palm camera; Among them, the hand-eye calibration of the fixed camera is: Fix the calibration plate at the end of the picking robot arm and move the picking robot arm at multiple positions. In this process, keep the position relationship between the calibration plate and the end of the picking robot arm unchanged, and ensure that the relative position between the base of the picking robot arm and the fixed camera remains unchanged. The transformation relationship is:

[0050] Where: , Respectively represent i Group, j In group data a Coordinate system to b Homogeneous transformation matrix of coordinate system; By constructing a nonlinear equation system, the transformation matrix from the picking robot coordinate system {R} to the fixed camera coordinate system {Fc} is obtained. :

[0051] The hand-eye calibration of the palm camera is: Since the relative posture between the end of the picking robot arm and the calibration plate is always kept unchanged during the movement of the picking robot arm, the transformation relationship during the movement is:

[0052]

[0053] Similarly, by constructing a nonlinear equation system, the transformation matrix from the palm coordinate system {Pc} to the picking robot arm coordinate system {R} is obtained; In addition, for any picking robot arm posture, the transformation relationship between the fixed camera coordinate system {Fc}, the palm camera coordinate system {Pc} and the world coordinate system {L} is:

[0054] Where: is the constant transformation matrix of the coordinate system {L} relative to the coordinate system {R} determined by the assembly relationship between the picking robot arm and the lifting column; is the transformation matrix of coordinate system {R} relative to coordinate system {F} obtained by inverse kinematics.

[0055] Step S2, obtaining a tomato RGB image and a depth image in a non-enclosed environment by using a fixed camera; Step S3: Use the improved yolov10 to detect ripe tomato fruits (the improved yolov10 network structure is shown in the figure below) Figure 2 As shown in the figure), the spatial coordinate information of the fruit under the fixed camera field of view is obtained; the improved yolov10 includes using Dysample upsampling technology to improve the Neck part in the yolov10 network and introducing RMTBlock to improve the Bottleneck structure in the C2F module; Among them, the Dysample upsampling technology is used to improve the Neck part of the yolov10 network. First, the input feature map F in Apply a small-scale convolution and K d (usually 3x3), obtain the local feature response map R d :

[0056] Then, according to the local feature response map R d , get the dynamic upsampling scale factor :

[0057] Where: β represents the regulating factor; mean(R d ) Represents the local feature response map R d Perform global average pooling to obtain the overall feature density; Finally, apply the dynamic upsampling scale factor , through bilinear interpolation method to input feature map F inUpsample to obtain the final upsampled output F up :

[0058] The Bottleneck structure in the C2F module is improved by introducing RMTBlock as follows: For the input feature map F in Use convolutions of different sizes and K d1 、K d3 、K d5 (1x1, 3x3, 5x5 respectively), and obtain the feature map of the response scale F d1 、F d3 、F d5 ; Then, the feature maps of different scales are weighted fused to obtain the fused feature map F merge :

[0059] Where: Represents the feature maps F d1 、F d3 、F d5 The weights are obtained through learning; Finally, use 1x1 convolution and fusion feature map F merge Compress and output refined features; The improved yolov10 includes collecting and organizing a large number of annotated tomato image datasets, which include ripe and unripe tomatoes under different environments and lighting conditions; after collecting and organizing the dataset, the dataset is enhanced (such as random cropping, rotation, scaling and color transformation) to improve the robustness of the model; model training is performed based on the improved yolov10 architecture, using pre-trained weights as the initial model, and fine-tuning on a custom tomato dataset to adapt to specific target detection tasks, selecting appropriate loss functions and optimizers, and setting learning rates and batch sizes to ensure that the model can converge effectively; finally, the trained model is applied to image recognition.

[0060] Among them, yolov10 uses its unique feature extraction network to quickly analyze images and generate multiple candidate frames. At the same time, it eliminates duplicate detections through the non-maximum suppression (NMS) algorithm, outputs the accurate tomato target position and its confidence, and post-processes the target detection results, including filtering out low-confidence detection frames and adjusting the coordinates of the detection frames to improve detection accuracy. In addition, by calculating the distance and position relationship between targets, it provides a reference for subsequent positioning and picking tasks. According to the results of target detection and maturity assessment, feedback information is collected, and the model is continuously iterated and optimized to improve detection accuracy and response speed, ensuring stability and reliability in practical applications.

[0061] Step S4, optimizing the depth distance of the detected fruit, specifically: using a fixed camera or a palm camera to obtain the three-dimensional coordinates of the center of the sphere of the monitored tomato fruit ( X i ,Y i ,Z i ),in Z i Indicates the depth of the corresponding tomato fruit. The detected fruit is sorted according to its depth. Z i Sort the fruits. Fruits with smaller depth values ​​are closer to the camera and may be picked first. Then, the coordinates of the center point of each tomato fruit are detected to determine whether it is located in the working space of the picking robot arm, that is, the accessible space of the picking robot arm is determined, including: First, combine the fruit bounding box and obtain the center point of the corresponding bounding box ( x c ,y c ):

[0062] Where: x min 、x max Respectively represent the minimum and maximum horizontal coordinates in the bounding box; y min 、y max Respectively represent the minimum and maximum vertical coordinates in the bounding box; Then, a mathematical model of the picking robot arm workspace is established: Assume that the position that the picking robot's gripper can reach is represented by the vector PX Indicates that the joint angle of the picking robot arm is The position of the gripper is obtained by the forward kinematics formula:

[0063] Where: f(·) It is a function designed according to the kinematics of the picking robot arm, usually containing trigonometric functions and transformation matrices, which are used to describe the mapping from joint angles to the end effector positions; Define the picking robot arm workspace W:

[0064] Where: represents the possible range of all joint angles, R 3 represents three-dimensional real number space; Afterwards, the center point of the bounding box ( x c ,y c ) Transform the coordinate system of the picking robot arm and obtain the corresponding point in the coordinate system of the picking robot arm :

[0065] Where: M represents the 4x4 homogeneous transformation matrix determined by the camera positioning parameters, installation angle and relative position; Finally, check the transformed coordinate points Is it located in the workspace W, that is, Whether it belongs to W; if the tomato fruit is not in the working space of the picking robot arm, replace the next target tomato.

[0066] Move the picking robot arm to the vicinity of the optimal target area.

[0067] Step S5, using the palm camera to perform secondary spatial information perception of the target (i.e., the palm camera repeats steps S2 and S3 to achieve secondary detection of the target tomato fruit, and simultaneously performs sorting according to the depth of the fruit and determination of the accessible space of the picking robot arm); Further obtain the spatial coordinate information in a relatively closed environment, including segmenting the tomato fruit point cloud information through mask information, specifically: First, the corresponding tomato mask is obtained based on the tomato fruit frame detected by the palm camera; Then, the point cloud and image are registered:

[0068] Where: Represents a 2D pixel point in homogeneous coordinate form [ x,y,1 ] T ; X Indicates the corresponding 3D world coordinate point; K represents the intrinsic parameter matrix of the camera:

[0069] Where: f x , f y Indicates the focal length of the camera (i.e., pixel unit); c x , c y Represents the coordinates of the center of the image; [ R | t ] represents the external parameter matrix of the camera:

[0070] Where: R represents the rotation matrix, t Represents translation vectors, which describe the transformation from the world coordinate system to the camera coordinate system; r xy When the camera coordinate system is transformed to the world coordinate system, the rotation matrix is Axis and The rotational component about the axis; t x , t y , t z Represents the world coordinate system x, y, z Displacement of the axis; Finally, determine whether the point is in the mask foreground area based on the projection coordinates:

[0071] Where: Represents the coordinates of the 3D world coordinate point X projected onto the 2D image; like If it falls in the foreground area of ​​the 2D mask, the corresponding 3D world coordinate point is retained; otherwise, the corresponding 3D world coordinate point is deleted; After completing the segmentation of the tomato point cloud information, denoising, clustering and other operations are performed to obtain the point cloud information of the target fruit.

[0072] Step S6: Approximate sphere fitting is performed on the tomato point cloud in the picking area to obtain the optimal picking object; specifically: First, for each point in the tomato discrete point cloud ( x i ,y i ,z i ) to fit the approximate spherical equation:

[0073] Where: x 0 ,y 0 ,z 0) represents the coordinates of the center of an approximate sphere; r represents the radius of the approximate sphere; After that, define the objective function S(x) :

[0074] right At the current iteration point x (k) Expand and keep only the first-order terms:

[0075] Where: express exist x (k) The i-th row of the Jacobian matrix at ; Then the objective function S(x) Approximately:

[0076] make J k Is J i,k Composition m x n The Vjacob matrix is:

[0077] Then the objective function is approximated Taking the derivative and equalizing it to zero, we get the iterative formula:

[0078] The approximate sphere equation is iteratively optimized through the above iterative formula, and finally the coordinates of the sphere center and the sphere radius are obtained.

[0079] Step S7, determining the number of tomatoes and estimating the picking posture (the number of tomatoes is determined by obtaining the center of the approximate sphere in the statistical image); "Picking pose estimation" (e.g. Figure 5 Specifically: For the pose of a single tomato fruit: the spatial coordinates of the centroid of the tomato fruit and the stalk are obtained by point cloud segmentation and approximate sphere fitting using a palm camera. C 0. C 1 (where the centroid coordinates of the fruit are the coordinates of the approximate sphere center, and the centroid coordinates of the stalk are the center of the identification frame), C 0. C1 and the origin of the initial gripper coordinate system O G Forming the growth reference surface, vector Indicates the direction of the fruit; According to the direction of the fruit, the Y-axis direction of the gripper coordinate system during picking is obtained:

[0080] After that, the X-axis direction basis of the gripper coordinate system is obtained:

[0081] Get the Z-axis direction basis based on the X-axis direction basis through the Y-axis direction:

[0082] At this time, the position of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} is:

[0083] Where: P 0 represents the three-dimensional spatial coordinates of the picking point; For multi-fruit tomato posture: Different from the pose estimation of a single-fruit tomato, when using the gripper to pick multiple-fruit tomatoes, if the picking pose is estimated by the fruit stalk and the tomato centroid, it is easy to collide with the fruit, causing damage or picking failure due to the direction of the Y-axis of the gripper coordinate system. Therefore, when picking multiple-fruit tomatoes, the single-fruit centroid is used to estimate the picking pose. C 0. Multi-point cloud sphere centroid C 2 and the origin of the initial gripper coordinate system O G Form the picking reference surface, the centroid of the multi-fruit point cloud sphere ( X,Y,Z ) Specifically:

[0084] Where: x i ,y i ,z i ) represents the centroid coordinates of each single fruit in a multi-fruit tomato. n represents the total number of fruits in multi-fruited tomatoes; At this time, the vector Indicates the Z-axis direction of the parallel gripper n Z2 , i.e. the gripping direction of the gripper, and obtain the direction bases of the gripper on the X-axis and Y-axis respectively n X2 , n Y2 :

[0085] Finally, the position and posture of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} are obtained: .

[0086] Embodiment 2: As a preferred embodiment of the present invention, based on the solution of Example 1, in order to avoid problems such as approaching singularity or iterative instability during the iterative optimization of the approximate sphere equation, the iterative formula in step S6 of Example 1 is optimized as follows:

[0087] The approximate sphere equation is iteratively optimized through the above iterative formula, and finally the coordinates of the sphere center and the sphere radius are obtained; where: I represents the identity matrix; λ Represents the damping factor. The damping factor is determined according to the actual situation of the iteration. If this iteration causes the objective function value to decrease, the damping factor can be appropriately reduced; if the objective function value increases, the damping factor is increased.

Claims

1. A spatially heterogeneous visual positioning system for a tomato picking robot, characterized by: include: Step S1, designing a tomato picking robot, including a mobile chassis, a lifting column, a picking robot arm, a fixed camera, a palm camera, a gripper and a control system; at the same time, calibrating the positioning of the tomato picking robot, including hand-eye calibration of the picking robot arm and conversion between coordinate systems; Step S2, obtaining a tomato RGB image and a depth image in a non-enclosed environment by using a fixed camera; Step S3, using the improved yolov10 to detect the ripe tomato fruit, and obtaining the spatial coordinate information of the fruit under the field of view of the fixed camera; Step S4, optimizing the detected fruit depth distance, determining the accessible space of the picking robot arm, and moving the picking robot arm to the vicinity of the optimal target area; Step S5: Use a palm camera to perform secondary spatial information perception on the target, and further obtain spatial coordinate information in a relatively closed environment; Step S6, performing approximate sphere fitting on the tomato point cloud in the picking area to obtain the optimal picking object; Step S7: determine the number of tomatoes and estimate the picking posture.

2. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 1, characterized in that: In the step S1, when the positioning of the tomato picking robot is calibrated, the positioning system includes the coordinate system {L} of the lifting column base, the coordinate system {R} of the picking robot arm, the coordinate system {Fc} of the fixed camera, the coordinate system {Pc} of the palm camera, the flange coordinate system {F}, the gripper coordinate system {G} and the given calibration plate coordinate system {C}, and the hand-eye calibration of the picking robot arm and the conversion between the coordinate systems include the hand-eye calibration of the fixed camera and the hand-eye calibration of the palm camera; Among them, the hand-eye calibration of the fixed camera is: Fix the calibration plate at the end of the picking robot arm and move the picking robot arm at multiple positions. In this process, keep the position relationship between the calibration plate and the end of the picking robot arm unchanged, and ensure that the relative position between the base of the picking robot arm and the fixed camera remains unchanged. The transformation relationship is: Where: , Respectively represent i Group, j In group data a Coordinate system to b Homogeneous transformation matrix of coordinate system; By constructing a nonlinear equation system, the transformation matrix from the picking robot coordinate system {R} to the fixed camera coordinate system {Fc} is obtained. : The hand-eye calibration of the palm camera is: Since the relative posture between the end of the picking robot arm and the calibration plate is always kept unchanged during the movement of the picking robot arm, the transformation relationship during the movement is: Similarly, by constructing a nonlinear equation system, the transformation matrix from the palm coordinate system {Pc} to the picking robot arm coordinate system {R} is obtained; In addition, for any picking robot arm posture, the transformation relationship between the fixed camera coordinate system {Fc}, the palm camera coordinate system {Pc} and the world coordinate system {L} is: Where: is the constant transformation matrix of the coordinate system {L} relative to the coordinate system {R} determined by the assembly relationship between the picking robot arm and the lifting column; is the transformation matrix of coordinate system {R} relative to coordinate system {F} obtained by inverse kinematics.

3. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 1 or 2, characterized in that: The improved yolov10 in step S3 includes using Dysample upsampling technology to improve the Neck part in the yolov10 network and introducing RMTBlock to improve the Bottleneck structure in the C2F module; Among them, the Dysample upsampling technology is used to improve the Neck part of the yolov10 network. First, the input feature map F in Apply a small-scale convolution and K d , get the local feature response map R d : Then, according to the local feature response map R d , get the dynamic upsampling scale factor : Where: β represents the regulating factor; mean(R d ) Represents the local feature response map R d Perform global average pooling to obtain the overall feature density; Finally, apply the dynamic upsampling scale factor , through bilinear interpolation method to input feature map F in Upsample to obtain the final upsampled output F up : The Bottleneck structure in the C2F module is improved by introducing RMTBlock as follows: For the input feature map F in Use convolutions of different sizes and K d1 、K d3 、K d5 , obtain the characteristic map of the response scale F d1 、 F d3 、F d5 ; Then, the feature maps of different scales are weighted fused to obtain the fused feature map F merge : Where: Represents the feature maps F d1 、F d3 、F d5 The weights of are obtained through learning; Finally, use 1x1 convolution and fusion feature map F merge Compress and output refined features.

4. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 1 or 3, characterized in that: The "determining the accessible space of the picking robot arm" in step S4 specifically includes: detecting the coordinates of the center point of each tomato fruit and determining whether it is located in the working space of the picking robot arm, including: First, combine the fruit bounding box and obtain the center point of the corresponding bounding box ( x c ,y c ): Where: x min 、x max Respectively represent the minimum and maximum horizontal coordinates in the bounding box; y min 、y max Respectively represent the minimum and maximum vertical coordinates in the bounding box; Then, a mathematical model of the picking robot arm workspace is established: Assume that the position that the picking robot's gripper can reach is represented by the vector PX Indicates that the joint angle of the picking robot arm is The position of the gripper is obtained by the forward kinematics formula: Where: f(·) It is a function designed according to the kinematics of the picking robot arm, usually containing trigonometric functions and transformation matrices, which are used to describe the mapping from joint angles to the end effector positions; Define the picking robot arm workspace W: Where: represents the possible range of all joint angles, R 3 represents three-dimensional real number space; After that, the center point of the bounding box ( x c ,y c ) Transform the coordinate system of the picking robot arm and obtain the corresponding point in the coordinate system of the picking robot arm : Where: M represents the 4x4 homogeneous transformation matrix determined by the camera positioning parameters, installation angle and relative position; Finally, check the transformed coordinate points Whether it is located in the workspace W, that is, Whether it belongs to W; if the tomato fruit is not in the working space of the picking robot arm, replace the next target tomato.

5. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 4, characterized in that: The step S5 of "further acquiring spatial coordinate information in a relatively closed environment" includes segmenting the tomato fruit point cloud information through mask information, specifically: First, the corresponding tomato mask is obtained based on the tomato fruit frame detected by the palm camera; Then, the point cloud and image are registered: Where: Represents a 2D pixel point in homogeneous coordinate form [ x,y,1 ] T ; X Indicates the corresponding 3D world coordinate point; K represents the intrinsic parameter matrix of the camera: Where: f x , f y Indicates the focal length of the camera; c x , c y Represents the coordinates of the center of the image; [ R | t ] represents the external parameter matrix of the camera: Where: R represents the rotation matrix, t Represents translation vectors, which describe the transformation from the world coordinate system to the camera coordinate system; r xy When the camera coordinate system is transformed to the world coordinate system, the rotation matrix is Axis and The rotational component about the axis; t x , t y , t z Represents the world coordinate system x, y, z Displacement of the axis; Finally, determine whether the point is in the mask foreground area based on the projection coordinates: Where: Represents the coordinates of the 3D world coordinate point X projected onto the 2D image; like If it falls in the foreground area of ​​the 2D mask, the corresponding 3D world coordinate point is retained; Otherwise, delete the corresponding 3D world coordinate point; After completing the segmentation of the tomato point cloud information, denoising and clustering operations are performed to obtain the point cloud information of the target fruit.

6. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 5, characterized in that: The step S6 of "approximate sphere fitting of the tomato point cloud in the area to be picked" is specifically as follows: First, for each point in the tomato discrete point cloud ( x i ,y i ,z i ) to fit the approximate spherical equation: Where: x 0 ,y 0 ,z 0) represents the coordinates of the center of an approximate sphere; r represents the radius of the approximate sphere; After that, define the objective function S(x) : right At the current iteration point x (k) Expand and keep only the first-order terms: Where: express exist x (k) The i-th row of the Jacobian matrix at ; Then the objective function S(x) Approximately: make J k Is J i,k Composition m x n The Vjacob matrix is: Then the objective function is approximated Taking the derivative and equalizing it to zero, we get the iterative formula: The approximate sphere equation is iteratively optimized through the above iterative formula, and finally the coordinates of the sphere center and the sphere radius are obtained.

7. The spatially heterogeneous visual positioning system for a tomato picking robot according to claim 6, characterized in that: The "picking posture estimation" in step S7 is specifically: For the pose of a single tomato fruit: the spatial coordinates of the centroid of the tomato fruit and the stalk are obtained by point cloud segmentation and approximate sphere fitting using a palm camera. C 0. C 1, C 0. C 1 and the origin of the initial gripper coordinate system O G Forming the growth reference surface, vector Indicates the direction of the fruit; According to the direction of the fruit, the Y-axis direction of the gripper coordinate system during picking is obtained: After that, the X-axis direction basis of the gripper coordinate system is obtained: Get the Z-axis direction basis based on the X-axis direction basis through the Y-axis direction: At this time, the position of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} is: Where: P 0 represents the three-dimensional spatial coordinates of the picking point; For multi-fruit tomato posture: When picking multi-fruit tomatoes, the centroid of the single fruit is used to C 0. Multi-point cloud sphere centroid C 2 and the origin of the initial gripper coordinate system O G Form the picking reference surface, the centroid of the multi-fruit point cloud sphere ( X,Y,Z ) Specifically: Where: x i ,y i ,z i ) represents the centroid coordinates of each single fruit in a multi-fruit tomato. n represents the total number of fruits in multi-fruited tomatoes; At this time, the vector Indicates the Z-axis direction of the parallel gripper n Z2 , i.e. the gripping direction of the gripper, and obtain the direction bases of the gripper on the X-axis and Y-axis respectively n X2 , n Y2 : Finally, the position and posture of the gripper coordinate system {G} relative to the picking robot arm coordinate system {R} are obtained: 。

Images