Double-arm picking robot system and control method

Abstract

The invention provides a double-arm picking robot system and a control method, a robot body is mounted on a moving module through a rotary lifting waist and hip joint, and a storage module is arranged on the moving module; the moving module can drive the robot body and the storage module to move; a right multi-degree-of-freedom mechanical arm and a left multi-degree-of-freedom mechanical arm are arranged on the two sides of the robot body, a right arm tail end picking gripper is arranged at the right end of the right multi-degree-of-freedom mechanical arm, and a left arm tail end picking gripper is arranged at the tail end of the left multi-degree-of-freedom mechanical arm; an image acquisition module is arranged at the top of the robot body, a laser radar module is arranged on the moving module, and the image acquisition module and the laser radar module perform data transmission with the robot control module. Two arms are adopted for cooperative work, the structure of the humanoid mechanical arm is more suitable for the picking scene, and cooperation can be more effectively carried out.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and specifically, to a two-arm picking robot system and a control method thereof. Background Art

[0002] Existing single-arm collaborative robotic arms are installed on an AGV mobile chassis and mainly perform inspection of targets on tracks during picking. However, some deficiencies have also been found in practice. In terms of the hardware platform, these mainly manifest as follows: a. A single arm cannot achieve the best picking efficiency, and the single working space area is limited; b. In practical engineering applications, there are not only working states on tracks, but there must be a large number of non-track environments, lacking a complete autonomous navigation function.

[0003] The patent document with the publication number CN111034465A discloses an apple high-efficiency picking robot, which includes a robot housing, and also includes a rotating assembly, a lifting assembly, a movable assembly, a grasping assembly, and a cutting assembly. The robot housing includes a base and a top cover, and the four sides between the base and the top cover are respectively fixed by a bolt. The rotating assembly is arranged on one side of the top of the top cover, the lifting assembly is arranged on the top of the rotating assembly, the movable assembly is arranged on the lifting assembly, the grasping assembly is arranged at the end of the movable assembly, the cutting assembly is arranged above the grasping assembly, and a blanking box is arranged on the top of the top cover and beside the rotating assembly, and a buffer chute for preventing apples from being damaged is also arranged on the blanking box. The rotating assembly includes a turntable and a gear drive mechanism. However, this patent document still has a single arm and has the defect of a limited working space area.

[0004] The present invention is a two-arm picking robot, which adopts a humanoid upper body. Specifically, the structure above the waist is mounted on a mobile chassis. The two arms of the humanoid robot are different from the structures of traditional collaborative robots. A binocular depth camera is added to the eyes of the head for intelligent environmental perception. At the height of the waist of the chassis, a forward obstacle avoidance binocular depth camera is added for short-distance movement obstacle avoidance and dynamic path planning; the depth camera for target detection is installed at the end effector of the left arm.

[0005] The present invention uses two arms, which can effectively increase the working area, and the number of pickings per stop can be greatly improved. Since the number of stops is reduced, the overall efficiency is improved even more. The present invention adopts two arms to work together, and the configuration of the humanoid robotic arm is more suitable for the picking scenario and can cooperate more effectively. The present invention has a complete autonomous navigation function. In this part, in the application field of humanoid robots, it is the content of environmental intelligence, which is very different from traditional compound robots. With the ability of environmental intelligent recognition, there is a basis for increasing the ability to autonomously receive and execute tasks, including the ability of more intelligent and real-time obstacle avoidance and dynamic local path planning. Summary of the Invention

[0006] Aiming at the defects in the prior art, the purpose of the present invention is to provide a two-arm picking robot system and a control method.

[0007] A two-arm picking robot system provided by the present invention includes: a robot body, a storage module, a moving module, and a robot control module;

[0008] The robot body is installed on the moving module through a rotating and lifting waist-hip joint, and the storage module is arranged on the moving module; the moving module can move the robot body and the storage module.

[0009] On both sides of the robot body, there are a multi-degree-of-freedom mechanical right arm and a multi-degree-of-freedom mechanical left arm. At the right end of the multi-degree-of-freedom mechanical right arm, there is a right-arm end picking gripper, and at the end of the multi-degree-of-freedom mechanical left arm, there is a left-arm end picking gripper;

[0010] On the top of the robot body, there is an image acquisition module, and on the moving module, there is a lidar module. The image acquisition module and the lidar module collect data and transmit it to the robot control module.

[0011] The robot control module controls the operation of the rotating and lifting waist-hip joint, the multi-degree-of-freedom mechanical right arm, the multi-degree-of-freedom mechanical left arm, the right-arm end picking gripper, the left-arm end picking gripper, and the moving module.

[0012] Preferably, the image acquisition module is a first depth camera;

[0013] The first depth camera is connected to the robot body through a neck rotating joint and is used for environmental intelligent cognition;

[0014] A second depth camera is arranged on the robot body and is used for short-distance movement obstacle avoidance and dynamic path planning;

[0015] A third depth camera is arranged on the left-arm end picking gripper and is used for detecting picking targets.

[0016] Preferably, the moving module includes: a moving chassis; on the moving chassis, there are rear chassis track wheels, rear chassis universal wheels, chassis drive wheels, front chassis track wheels, and front chassis universal wheels;

[0017] The front chassis track wheels and the front chassis universal wheels are located at the front end of the moving chassis, and the rear chassis track wheels and the rear chassis universal wheels are located at the rear end of the moving chassis;

[0018] The chassis drive wheels are located between the front chassis track wheels and the rear chassis track wheels.

[0019] Preferably, the lidar module includes: a rear lidar and a front lidar;

[0020] The rear lidar and the front lidar are arranged on the mobile chassis. The rear lidar is located at the front end of the mobile chassis, and the front lidar is located at the rear end of the mobile chassis;

[0021] A rear anti-collision strip is provided at the rear end of the mobile chassis, and a front anti-collision strip is provided at the front end of the mobile chassis.

[0022] Preferably, the multi-degree-of-freedom robotic right arm is a six-degree-of-freedom robotic right arm;

[0023] The six-degree-of-freedom robotic right arm includes: a first right-arm joint, a second right-arm joint, a third right-arm joint, a fourth right-arm joint, a fifth right-arm joint, and a sixth right-arm joint connected in sequence;

[0024] The first right-arm joint is connected to the robot body, and the sixth right-arm joint is connected to the end-of-right-arm picking gripper;

[0025] The multi-degree-of-freedom robotic left arm is a six-degree-of-freedom robotic left arm;

[0026] The six-degree-of-freedom robotic left arm includes: a first left-arm joint, a second left-arm joint, a third left-arm joint, a fourth left-arm joint, a fifth left-arm joint, and a sixth left-arm joint;

[0027] The first left-arm joint is connected to the robot body, and the sixth left-arm joint is connected to the end-of-left-arm picking gripper;

[0028] The rotary lifting waist and hip joint includes: a waist rotation joint, a first waist lifting joint, and a second waist lifting joint connected in sequence;

[0029] The waist rotation joint is connected to the robot body, and the second waist lifting joint is connected to the mobile module.

[0030] Preferably, the robot control module includes: a vision service module, a motion planning module, a robotic arm control module, a vehicle control module, a software communication module, a python-ab-api module, and a natural language interaction application module;

[0031] The vision service module uses the image acquisition module to capture scene image information, segments the picking target from the scene image information through the vision weight model file, calculates the D coordinates of the picking target, and detects and locates the picking target;

[0032] The motion planning module plans the picking motion paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm according to the information of the picking target and the workspace configuration;

[0033] The robotic arm control module receives the motion paths output by the motion planning module, and controls the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm to execute the motion paths respectively;

[0034] The vehicle control module interacts with the lidar module. The lidar module can perform SLAM mapping and navigation, and the vehicle control module controls the operation of the mobile module;

[0035] The software communication module is used for communication between modules;

[0036] The python-ab-api module is used to call each module and / or receive information from each module;

[0037] The natural language interaction application module is used to realize human-machine interaction with the robot.

[0038] The present invention also provides a control method for a two-arm picking robot, which is used to control the above two-arm picking robot system, and specifically includes the following steps:

[0039] Vehicle chassis motion control step: Perform SLAM mapping through the lidar module, plan the motion trajectory of the mobile module, and enable the mobile module to perform navigation and inspection according to the motion trajectory; Collect images of the picking target in real time through the image acquisition module, track and detect the picking target based on the collected image data, obtain the position information of the picking target, and adjust and control the speed of the mobile module in real time according to the position information of the picking target;

[0040] Robotic arm motion control step: Define the motion spaces of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm so that the picking object can be located in the workspace; Determine the joint angles of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm through inverse kinematics according to the position information of the picking target; Obtain the collision-free motion paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm according to the determined joint angles and collision avoidance configuration parameters, so that the picking claws at the end of the right arm and the picking claws at the end of the left arm approach the picking target in a collision-free manner;

[0041] Two-arm collaborative motion control step: Optimize and adjust the motion paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm so that the motion spaces of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm do not overlap;

[0042] Target tracking and feedback steps: Real-time tracking of the picking target, and real-time feedback of the pose of the picking target. According to the feedback of the pose of the picking target, dynamically adjust and plan the movement paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm;

[0043] Shearing and grasping steps: After the picking claws at the ends of the right arm and the left arm reach the position of the picking target, complete the shearing and grasping operation, and place the picking target in the storage module.

[0044] Preferably, determining the joint angles of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm through inverse kinematics specifically includes the following steps:

[0045] Steps of establishing the robot model and coordinate system:

[0046] Using the Denavit-Hartenberg parameter method to establish a coordinate system for each joint of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm, assigning a number i to each joint, and defining the D-H parameters of each joint; i = 1, 2,..., 6;

[0047] a_{i - 1}: Link length, the distance along the x_{i - 1} axis from the z_{i - 1} axis to the z_i axis;

[0048] a_{i - 1}: Link twist angle, the angle between the z_{i - 1} axis and the z_i axis around the x_{i - 1} axis;

[0049] d_i: Joint offset, the distance along the z_{i - 1} axis from the x_{i - 1} axis to the x_i axis;

[0050] theta_i: Joint angle, the angle between the x_{i - 1} axis and the x_i axis around the z_{i - 1} axis;

[0051] Steps of inverse kinematics solution:

[0052] According to the position information and pose of the picking claws at the ends of the right arm and the left arm, solve the six joint angles theta_1, theta_2, theta_3, theta_4, theta_5, theta_6;

[0053] Solving for theta_1:

[0054] The angle between the projections of the picking claws at the ends of the right arm and the left arm on the x - y plane of the base coordinate system and the x axis is related to theta_1. By analyzing the coordinates of the positions (x, y, z) of the picking claws at the ends of the right arm and the left arm, use the arctangent function to determine the value of theta_1:

[0055] theta_1 = arctan2(y, x) + {constant offset};

[0056] Solve for theta_3:

[0057] Simplify the multi - degree - of - freedom robotic right arm and the multi - degree - of - freedom robotic left arm into a planar 2 - link mechanism, and solve through geometric relationships and trigonometric functions; According to the distance from the shoulder joint to the center of the wrist joint and the information related to the target position, calculate theta_3 through the cosine theorem:

[0058] Cos(theta_3) = (l_1^2 + l_2^2 - d^2) / (2l_1 * l_2);

[0059] Where l_1, l_2 are the lengths of the relevant links, and d is a specific geometric distance;

[0060] Solve for theta_2:

[0061] After obtaining theta_3, calculate theta_2 using geometric relationships and trigonometric functions; Considering the structures of the multi - degree - of - freedom robotic right arm and the multi - degree - of - freedom robotic left arm and the determined parameters, further solve in combination with the positions of the end - effector grippers of the right arm and the end - effector grippers of the left arm;

[0062] Solve for theta_4, theta_5 and theta_6:

[0063] Determine by the decomposition of the rotation matrix and the pose relationship, relate the pose matrices of the end - effector grippers of the right arm and the end - effector grippers of the left arm to the transformation matrix of the known joint angles, and solve using trigonometric relationships and the properties of the rotation matrix.

[0064] Preferably, obtaining the collision - free motion paths of the multi - degree - of - freedom robotic right arm and the multi - degree - of - freedom robotic left arm specifically includes the following steps:

[0065] Model the working space where the robot is located, use a three - dimensional space representation, describe the obstacles with polygons, and record their position and size information;

[0066] Collision - free configuration parameters include: the shape and size of the robot, the motion constraints of the robot, and the safety distance; The shape and size of the robot include: the radius, length, and width of the robot; The motion constraints of the robot include: the maximum speed, acceleration, and turning radius; The safety distance is the minimum distance between the robot and the obstacle;

[0067] Use the random tree path - search algorithm, starting from the starting point in the three - dimensional space, continuously generate new points randomly, grow the tree towards this point until the tree contains the picking target point, and find a collision - free path;

[0068] Adopt the method of spline curve fitting to fit the path points of the collision-free path into a smooth curve;

[0069] According to the motion constraints and task requirements of the robot, allocate appropriate speeds to each point on the collision-free path;

[0070] During the process of generating the collision-free path, continuously check the distance between the robot and surrounding obstacles in real time to ensure that the safety distance requirement is met at any time;

[0071] After the collision-free path is generated, perform global collision detection on the entire collision-free path to ensure that the robot will not collide with obstacles during the entire movement process. If a collision is detected during the process, readjust the collision-free path.

[0072] Preferably, in the steps of dual-arm collaborative motion control:

[0073] According to the position information of the picking target in the image data, allocate the picking targets in different ranges to the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm;

[0074] Based on the multi-degree-of-freedom robotic left arm, determine the picking target entering the working area. After determining that the picking target enters the working area, according to the configured optimal picking position parameters, control the speed control and stop of the mobile module, so that the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm start the picking task in their respective working spaces, and schedule the use of the motion path planning in the robotic arm motion control steps to perform the picking work until there are no pickable picking targets in the working spaces of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm, and then switch to the patrol state for patrol.

[0075] Compared with the prior art, the present invention has the following beneficial effects:

[0076] 1. The present invention improves the picking efficiency. The dual-arm picking robot can perform multiple picking tasks simultaneously, significantly improving the picking efficiency.

[0077] 2. The present invention has higher flexibility and adaptability. The dual-arm design enables the robot to work collaboratively with both arms to pick tomatoes blocked by fruit leaves, adapting to complex tomato planting environments.

[0078] 3. The present invention has higher intelligence and autonomy. The dual-arm picking robot is equipped with more advanced vision recognition systems and intelligent control systems, which can perceive the surrounding environment more precisely and accurately in real time, and make autonomous decisions and execute picking tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments read in conjunction with the accompanying drawings:

[0080] Figure 1 Schematic diagram of the three-dimensional structure of the two-arm picking robot system Figure 1 ;

[0081] Figure 2 Schematic diagram of the structure of the depth camera highlighting the robot's head;

[0082] Figure 3 Schematic diagram of the structure of the mobile module Figure 1 ;

[0083] Figure 4 Schematic diagram of the structure of the mobile module Figure 2 ;

[0084] Figure 5 Schematic diagram of the structure highlighting the waist rotation joint;

[0085] Figure 6 Schematic diagram of the structure highlighting the waist lifting joint;

[0086] Figure 7 Schematic diagram of the structure highlighting the multi-degree-of-freedom robotic right arm and multi-degree-of-freedom robotic left arm;

[0087] Figure 8 Schematic diagram of the architecture of the robot control module;

[0088] Figure 9 Schematic diagram highlighting that the tomato planting scenario is an unstructured environment;

[0089] Figure 10 Schematic diagram of the principle of the safety cooperation movement rules of the multi-degree-of-freedom robotic right arm and multi-degree-of-freedom robotic left arm;

[0090] Figure 11 Schematic diagram of the robot's picking Figure 1 ;

[0091] Figure 12 Schematic diagram of the robot's picking Figure 2 ;

[0092] Figure 13 Schematic diagram of the structure of the picking gripper at the end of the left arm Figure 1 ;

[0093] Figure 14 Schematic diagram of the structure of the picking gripper at the end of the left arm Figure 2 ;

[0094] Figure 15 Top view of the two-arm picking robot system;

[0095] Figure 16 Rear view of the dual-arm picking robot system;

[0096] Figure 17 Bottom view of the dual-arm picking robot system;

[0097] Figure 18 Schematic diagram of the three-dimensional structure of the dual-arm picking robot system Figure 2 ;

[0098] Figure 19 Schematic diagram of the three-dimensional structure of the dual-arm picking robot system Figure 3 ;

[0099] Figure 20 Picking schematic of the robot Figure 3 .

[0100] As shown in the figure:

[0101] Image acquisition module 101 Fruit stalk guiding part 2147

[0102] Neck rotation joint 102 Fruit stalk clamping part 2148

[0103] Right-arm end picking gripper 201 Pitching torsion spring follower 2149

[0104] Sixth right-arm joint 202 Guide rail lower cover 21410

[0105] Fifth right-arm joint 203 Bearing 21411

[0106] Fourth right-arm joint 204 Torsion spring 21412

[0107] Third right-arm joint 205 Torsion spring center fixing part 21413

[0108] Second right-arm joint 206 Torsion spring short arm fixing part 21414

[0109] First right-arm joint 207 Waist rotation joint 301

[0110] First left-arm joint 208 First waist lifting joint 302

[0111] Second left-arm joint 209 Second waist lifting joint 303

[0112] Third left-arm joint 210 Rear laser radar 401

[0113] Fourth left-arm joint 211 Rear anti-collision strip 402

[0114] Fifth left-arm joint 212 Rear chassis track wheel 403

[0115] Sixth left arm joint 213 Rear universal wheel of the chassis 404

[0116] Grasping claw at the end of the left arm 214 Driving wheel of the chassis 405

[0117] Finger part 2141 Front track wheel of the chassis 406

[0118] Upper cover of the guide rail 2142 Front universal wheel of the chassis 407

[0119] Cylinder 2143 Front anti-collision strip 408

[0120] Side-mounted flange frame 2144 Front lidar 409

[0121] Vision camera 2145 Storage module 410

[0122] Blade 2146 Mobile chassis 411 Specific implementation mode

[0123] The present invention will be described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several changes and improvements can still be made. These all belong to the protection scope of the present invention.

[0124] Example 1:

[0125] As Figures 1 - 20 shown, this embodiment provides a double-arm picking robot system, including: a robot body, a storage module 410, a moving module, and a robot control module; the robot body is installed on the moving module through a rotating and lifting waist and hip joint, and the storage module 410 is arranged on the moving module; the moving module can move the robot body and the storage module 410; both sides of the robot body are provided with a multi-degree-of-freedom mechanical right arm and a multi-degree-of-freedom mechanical left arm, the right end of the multi-degree-of-freedom mechanical right arm is provided with a grasping claw 201 at the end of the right arm, and the end of the multi-degree-of-freedom mechanical left arm is provided with a grasping claw 214 at the end of the left arm; an image acquisition module 101 is arranged on the top of the robot body, a lidar module is arranged on the moving module, and the image acquisition module 101 and the lidar module collect data and transmit it to the robot control module; the robot control module controls the operation of the rotating and lifting waist and hip joint, the multi-degree-of-freedom mechanical right arm, the multi-degree-of-freedom mechanical left arm, the grasping claw 201 at the end of the right arm, the grasping claw 214 at the end of the left arm, and the moving module. The storage module 410 is a fruit basket.

[0126] The mobile module includes: a mobile chassis 411; a rear chassis track wheel 403, a rear chassis caster 404, a chassis drive wheel 405, a front chassis track wheel 406, and a front chassis caster 407 are arranged on the mobile chassis 411; the front chassis track wheel 406 and the front chassis caster 407 are located at the front end of the mobile chassis 411, and the rear chassis track wheel 403 and the rear chassis caster 404 are located at the rear end of the mobile chassis 411; the chassis drive wheel 405 is located between the front chassis track wheel 406 and the rear chassis track wheel 403.

[0127] The lidar module includes: a rear lidar 401 and a front lidar 409; the rear lidar 401 and the front lidar 409 are arranged on the mobile chassis 411, the rear lidar 401 is located at the front end of the mobile chassis 411, and the front lidar 409 is located at the rear end of the mobile chassis 411; a rear anti-collision strip 402 is arranged at the rear end of the mobile chassis 411, and a front anti-collision strip 408 is arranged at the front end of the mobile chassis 411.

[0128] The image acquisition module 101 is a first depth camera; the first depth camera is connected to the robot body through a neck rotating joint 102 and is used for intelligent environmental perception; a second depth camera is arranged on the robot body and is used for short-distance movement obstacle avoidance and dynamic path planning; a third depth camera is arranged on the picking gripper 214 at the end of the left arm and is used for detecting picking targets. The first depth camera is a D435 RGB-D depth camera.

[0129] The underlying api of the first depth camera acquires an image data stream, extracts image frames from the image data stream, aligns the 2D image with the image frames containing depth information, then uses the yolo detection module for inference to obtain result data, and exchanges the result data using the mqtt method.

[0130] The multi-degree-of-freedom robotic right arm is a six-degree-of-freedom robotic right arm; the six-degree-of-freedom robotic left arm includes: a first right-arm joint 207, a second right-arm joint 206, a third right-arm joint 205, a fourth right-arm joint 204, a fifth right-arm joint 203, and a sixth right-arm joint 202 connected in sequence; the first right-arm joint 207 is connected to the robot body, and the sixth right-arm joint 202 is connected to the end-effector gripper 201 of the right arm; the multi-degree-of-freedom robotic left arm is a six-degree-of-freedom robotic left arm. The six-degree-of-freedom robotic left arm includes: a first left-arm joint 208, a second left-arm joint 209, a third left-arm joint 210, a fourth left-arm joint 211, a fifth left-arm joint 212, and a sixth left-arm joint 213; the first left-arm joint 208 is connected to the robot body, and the sixth left-arm joint 213 is connected to the end-effector gripper 214 of the left arm. The rotating and lifting waist and hip joint includes: a waist rotation joint 301, a first waist lifting joint 302, and a second waist lifting joint 303 connected in sequence; the waist rotation joint 301 is connected to the robot body, and the second waist lifting joint 303 is connected to the mobile module.

[0131] The robot control module includes: a vision service module, a motion planning module, a robotic arm control module, a vehicle control module, a software communication module, a python-ab-api module, and a natural language interaction application module; the vision service module uses the image acquisition module 101 to capture scene image information, segments the picking target from the scene image information through the vision weight model file, calculates the 3D coordinates of the picking target, and detects and locates the picking target; the motion planning module plans the picking motion paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm according to the information of the picking target and the workspace configuration; the robotic arm control module receives the motion paths output by the motion planning module and controls the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm to execute the motion paths respectively; the vehicle control module interacts with the lidar module, the lidar module can perform SLAM mapping and navigation, and the vehicle control module controls the operation of the mobile module; the software communication module is used for communication between modules; the python-ab-api module is used to call each module and / or receive information from each module; the natural language interaction application module is used to realize human-machine interaction with the robot.

[0132] This embodiment also provides a control method for a dual-arm picking robot, which is used to control the above-mentioned dual-arm picking robot system, and specifically includes the following steps:

[0133] Vehicle chassis motion control steps: Perform SLAM mapping through the lidar module, plan the motion trajectory of the mobile module, and enable the mobile module to perform navigation inspections according to the motion trajectory; Collect images of the picking target in real time through the image acquisition module 101, track and detect the picking target based on the collected image data, obtain the position information of the picking target, and adjust the speed of the control mobile module in real time according to the position information of the picking target;

[0134] Manipulator motion control steps: Define the motion space of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm so that the picking object can be located in the working space; According to the position information of the picking target, determine the joint angles of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm through inverse kinematics; According to the determined joint angles and the collision avoidance configuration parameters, obtain the collision-free motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm, so that the picking gripper 201 at the end of the right arm and the picking gripper 214 at the end of the left arm approach the picking target in a collision-free manner;

[0135] Dual-arm collaborative motion control steps: Optimize and adjust the motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm so that the motion spaces of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm do not overlap;

[0136] Target tracking and feedback steps: Track the picking target in real time and feedback the pose of the picking target in real time. According to the feedback pose of the picking target, dynamically adjust and plan the motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm;

[0137] Shearing and grasping steps: After the picking gripper 201 at the end of the right arm and the picking gripper 214 at the end of the left arm reach the position of the picking target, complete the shearing and grasping operation and place the picking target in the storage module 410.

[0138] Determine the joint angles of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm through inverse kinematics, specifically including the following steps:

[0139] Steps for establishing the robot model and coordinate system:

[0140] Use the Denavit-Hartenberg parameter method to establish a coordinate system for each joint of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm, assign a number i to each joint, and define the D-H parameters of each joint; i = 1, 2,..., 6;

[0141] a_{i - 1}: Link length, the distance along the x_{i - 1} axis from the z_{i - 1} axis to the z_i axis;

[0142] a_{i - 1}: Link twist angle, the angle between the z_{i - 1} axis and the z_i axis around the x_{i - 1} axis;

[0143] $d_i$: Joint offset, the distance along the $z_{i - 1}$ axis from the $x_{i - 1}$ axis to the $x_i$ axis;

[0144] $\theta_i$: Joint angle, the angle between the $x_{i - 1}$ axis and the $x_i$ axis around the $z_{i - 1}$ axis;

[0145] The $z_i$ axis represents the $z$ axis of the $i$-th joint coordinate system;

[0146] The $z_{i - 1}$ axis represents the $z$ axis of the $(i - 1)$-th joint coordinate system;

[0147] The $x_i$ axis represents the $x$ axis of the $i$-th joint coordinate system;

[0148] The $x_{i - 1}$ axis represents the $x$ axis of the $(i - 1)$-th joint coordinate system;

[0149] Steps for inverse kinematics solution:

[0150] According to the position information and postures of the picking gripper 201 at the right arm end and the picking gripper 214 at the left arm end, solve for the six joint angles $\theta_1$, $\theta_2$, $\theta_3$, $\theta_4$, $\theta_5$, $\theta_6$;

[0151] Solve for $\theta_1$:

[0152] The angle between the projections of the picking gripper 201 at the right arm end and the picking gripper 214 at the left arm end on the $x - y$ plane of the base coordinate system and the $x$ axis is related to $\theta_1$. By analyzing the coordinates of the positions $(x, y, z)$ of the picking gripper 201 at the right arm end and the picking gripper 214 at the left arm end, use the arctangent function to determine the value of $\theta_1$:

[0153] $\theta_1=\arctan2(y, x)+\{constant offset\}$;

[0154] Solve for $\theta_3$:

[0155] Simplify the multi - degree - of - freedom robotic right arm and the multi - degree - of - freedom robotic left arm into a planar 2 - link mechanism and solve through geometric relationships and trigonometric functions; According to the distance from the shoulder joint to the wrist joint center and the relevant information of the target position, calculate $\theta_3$ using the cosine theorem:

[0156] $\cos(\theta_3)=(l_1^2 + l_2^2 - d^2) / (2l_1*l_2)$;

[0157] where $l_1$, $l_2$ are the relevant link lengths and $d$ is a specific geometric distance;

[0158] Solve for $\theta_2$:

[0159] After obtaining theta_3, calculate theta_2 using geometric relationships and trigonometric functions; considering the structures of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm and the determined parameters, further solve in combination with the positions of the picking gripper 201 at the end of the right arm and the picking gripper 214 at the end of the left arm;

[0160] Solve for theta_4, theta_5, and theta_6:

[0161] Determine by the decomposition of the rotation matrix and the pose relationship, relate the pose matrices of the picking gripper 201 at the end of the right arm and the picking gripper 214 at the end of the left arm to the transformation matrix of the known joint angles, and solve using trigonometric relationships and the properties of the rotation matrix.

[0162] Obtain the collision-free motion paths of the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm, which specifically include the following steps:

[0163] Model the working space where the robot is located, use a three-dimensional space representation, describe the obstacles with polygons, and record their position and size information;

[0164] The collision-free configuration parameters include: the shape and size of the robot, the motion constraints of the robot, and the safety distance; the shape and size of the robot include: the radius, length, and width of the robot; the motion constraints of the robot include: the maximum speed, acceleration, and turning radius; the safety distance is the minimum distance between the robot and the obstacle;

[0165] Use the random tree path search algorithm, starting from the starting point in the three-dimensional space, continuously randomly generate new points, grow the tree towards this point until the tree contains the picking target point, and find a collision-free path;

[0166] Adopt the method of spline curve fitting to fit the path points of the collision-free path into a smooth curve;

[0167] According to the motion constraints of the robot and the task requirements, allocate appropriate speeds to each point on the collision-free path;

[0168] During the process of generating the collision-free path, continuously check the distance between the robot and the surrounding obstacles in real time to ensure that the safety distance requirement is met at any time;

[0169] After the collision-free path is generated, perform global collision detection on the entire collision-free path to ensure that the robot will not collide with obstacles during the entire motion process. If a collision is detected during the process, readjust the collision-free path.

[0170] In the steps of dual-arm collaborative motion control:

[0171] According to the position information of the picking targets in the image data, the picking targets in different ranges are allocated to the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm;

[0172] Determine the picking target entering the working range based on the multi-degree-of-freedom mechanical left arm. After determining that the picking target enters the working range, control the speed control and stop of the mobile module according to the configured optimal picking position parameters, so that the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm are respectively in their respective working spaces. Start the picking task, and use the motion path planning in the robotic arm motion control step to schedule the picking work until there are no picking targets to be picked in the working spaces of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm, and then switch to the inspection state for inspection.

[0173] like Figure 10 As shown in the figure, the safe cooperative motion rules of the multi-DOF robot right arm and the multi-DOF robot left arm are as follows:

[0174] Step a: Detect whether there is a picking target through the visual service module. If yes, collect the picking target. If no, complete the parking and picking.

[0175] Step b: after detecting that there is a picking target, determine whether the picking target is located in the working space of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm. If so, proceed to step c; if not, abandon the current picking and determine whether to proceed to the next picking;

[0176] Step c: Determine whether it is suitable for single arm, if yes, proceed to step d; if no, proceed to step e;

[0177] Step d: dispatching the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm respectively, and performing the multi-degree-of-freedom mechanical right arm operation and the multi-degree-of-freedom mechanical left arm operation respectively based on the coordination of the two arms of the system, and determining whether to perform the next picking;

[0178] Step e: determine whether there is leaf occlusion. If so, use an occlusion processing algorithm to process, specifically: generate an operation task of pushing away the leaves, and proceed to step f. If not, give up the current picking and determine whether to proceed to the next picking;

[0179] Step f: Determine whether the picking target is located in the working space. If so, perform a double-arm collaborative operation to complete the leaf-pushing action and the target-picking action in succession. After completion, determine whether to proceed to the next picking. If not, give up this picking and determine whether to proceed to the next picking.

[0180] Determine whether to proceed to the next picking. If so, step b starts again. If not, the current parking picking is completed.

[0181] like Figure 13 andFigure 14 As shown, the picking gripper at the end of the left arm 214 includes: a finger member 2141, a guide rail upper cover 2142, a cylinder 2143, a side-mounted flange frame 2144, a vision camera 2145, a blade 2146, a fruit stalk guide member 2147, a fruit stalk clamping member 2148, a pitch torsion spring follower 2149, a guide rail lower cover 21410, a bearing 21411, a torsion spring 21412, a torsion spring center fixing member 21413, and a torsion spring short arm fixing member 21414.

[0182] Among them, the bearing 21411 is in transition fit with the guide rail lower cover 21410, and then the bearing 21411 and the guide rail lower cover 21410 are inserted into the feature shaft of the side-mounted flange frame 2144, and a shaft circlip is used to axially restrain the guide rail lower cover 21410 and the bearing 21411; the vision camera 2145 is connected to the side-mounted flange frame by a thread; the cylinder block of the cylinder 2143 is connected to the guide rail upper cover 2142 by a thread; the finger member 2141 is connected to the piston rod of the cylinder 2143 by a thread; the guide rail upper cover 2142 is connected to the guide rail lower cover 21410 by a thread; the finger member forms a moving pair between the guide rail upper cover and the guide rail lower cover; the fruit stalk guide member 2147 is connected to the guide rail upper cover 2142 by a thread; the blade 2146 and the fruit stalk clamping member 2148 are clamped between the guide rail upper cover 2142 and the fruit stalk guide member 2147; the pitch torsion spring follower 2149 is connected to the guide rail lower cover 21410 by a thread; the torsion spring center fixing member 21413 is connected to the guide rail lower cover 21410 by a thread; the torsion spring short arm fixing member 21414 is connected to the guide rail lower cover 21410 by a thread; the long arm of the torsion spring 12 is inserted into the torsion spring hole of the pitch torsion spring follower 2149; the large diameter of the torsion spring 12 is sleeved on the torsion spring center fixing member 21413, and the short arm is clamped into the gap formed by the guide rail lower cover 21410 and the torsion spring short arm fixing member 21414.

[0183] In this embodiment, when the front end of the finger member 2141 contacts the fruit stalk and is forced to tilt downward, the cutting surface of the finger member 2141 is perpendicular to the fruit stalk for cutting. Through the cooperation of the torsion spring 21412 and the angled torsion spring hole in the pitch torsion spring follower 2149, the whole can remain horizontal when not under force.

[0184] The working process of the picking gripper 214 at the end of the left arm in this embodiment is as follows: After the visual camera 2145 recognizes the cluster tomatoes, the robotic arm extends the end effector to the bottom of the cluster tomatoes. After the square hollow at the front end of the finger member 2141 aligns with the bottom of the cluster tomatoes, the end effector is lifted upward until the edge at the front end of the finger member 2141 touches the fruit stalk. The front end tilts downward under the tangential force until the front plane is perpendicular to the fruit stalk. The air rod in the cylinder 2143 is pushed out, causing the finger member and the blade 2146 to bite and cut the fruit stalk. The upper part of the fruit stalk is discharged through the front inclined plane of the guide rail upper cover 2142, and the lower part of the fruit stalk is clamped by the fruit stalk clamping member 2148 and the finger member 2141, thus completing the integrated cutting and clamping of the cluster tomatoes.

[0185] Example 2:

[0186] Those skilled in the art can understand this embodiment as a more specific illustration of Embodiment 1.

[0187] This embodiment provides a dual-arm picking robot system, including: an image acquisition module 101, a neck rotating joint 102, a right-arm end picking gripper 201, a sixth right-arm joint 202, a fifth right-arm joint 203, a fourth right-arm joint 204, a third right-arm joint 205, a second right-arm joint 206, a first right-arm joint 207, a first left-arm joint 208, a second left-arm joint 209, a third left-arm joint 210, a fourth left-arm joint 211, a fifth left-arm joint 212, a sixth left-arm joint 213, a left-arm end picking gripper 214, a waist rotating joint 301, a first waist lifting joint 302, a second waist lifting joint 303, a rear lidar 401, a rear anti-collision strip 402, a rear chassis track wheel 403, a rear chassis universal wheel 404, a chassis drive wheel 405, a front chassis track wheel 406, a front chassis universal wheel 407, a front anti-collision strip 408, a front lidar 409, a storage module 410, and a mobile chassis 411.

[0188] The dual-arm picking robot system in this embodiment mainly involves the following parts:

[0189] 1. Robot body

[0190] The robot body consists of a mobile chassis, a rotating and lifting waist and hip joint, a dual-arm 6*2-degree-of-freedom robotic arm, and a 3-joint picking robotic wrist, forming an 18-degree-of-freedom picking robot body. As Figure 1 shown, it is a humanoid multi-joint tomato picking robot system with an added rotating and lifting waist and hip joint.

[0191] (1) Head eyes

[0192] The depth camera D435(101) is installed inside the eyes of the humanoid robot's head, which is used to observe the environment in a large range, intelligently recognize various object objects in the environment, and can be used for global path planning. At the same time, with the environmental intelligence ability, it can also better accept and execute tasks. Such as Figure 2 As shown, it is the camera on the robot's head.

[0193] (2) Mobile chassis

[0194] According to the characteristics of tomato planting in the facility environment, there is a guide rail that can move in a straight line. Therefore, the mobile chassis is used to replace the two feet of the humanoid robot, making it walk fast, with a stable gait and high straight-line motion accuracy. Such as Figure 3 As shown, the chassis is set on the guide rail. The robot chassis includes: rear lidar 401, rear anti-collision strip 402, rear track wheels 403 of the chassis, rear universal wheels 404 of the chassis, drive wheels 405 of the chassis, front track wheels 406 of the chassis, front universal wheels 407 of the chassis, front anti-collision strip 408, and front lidar 409. Through lidar SLAM mapping and navigation, the differential double-wheel chassis is controlled to control the automatic up-and-down rail and automatic rail change of the robot chassis. Such as Figure 3 and Figure 4 As shown is the robot's mobile chassis.

[0195] (3) Waist and hip rotation

[0196] According to the characteristics of tomato planting, with multi-row close planting and double-sided row planting on one track, when the picking robot walks on one track, it needs to achieve double-row picking. The robot's body has a waist joint (301) that controls the rotation of the robot's body by ±180°. To achieve single-track multi-row picking. Such as Figure 5 As shown, it is the waist rotation joint of the robot.

[0197] (4) Upper limb lifting joint

[0198] According to the characteristics of tomato planting, the growth height is within the range of 0.3 - 4.0m, and the picking area according to the human height is within the range of 0.4 - 1.4m. In order to control the height of the picking robot's arm span and improve stability, there is an upper limb folding and lifting joint at the hip. The lifting mechanism is driven by a motor to increase the picking height range. Such as Figure 6 As shown, it is the upper limb folding and lifting joint.

[0199] (5) Robot arm

[0200] It includes two independent 6-axis collaborative robotic arms. The sixth right-arm joint 202, the fifth right-arm joint 203, the fourth right-arm joint 204, the third right-arm joint 205, the second right-arm joint 206, and the first right-arm joint 207 form a 6-axis collaborative robotic arm. The first left-arm joint 208, the second left-arm joint 209, the third left-arm joint 210, the fourth left-arm joint 211, the fifth left-arm joint 212, and the sixth left-arm joint 213 form a 6-axis collaborative robotic arm. Each robotic arm consists of 6 independent joints, and a single robotic arm can achieve 6 degrees of freedom of movement. The end-effector grippers 201 and 214 of the robot can grasp the target fruits and vegetables. As Figure 7 shown, it is dual-robotic-arm picking.

[0201] Eye-in-Hand: On one arm, specifically configured here to install a depth vision camera D405 at the end of the left arm, which is used for the detection and positioning of the picking target.

[0202] 2. Software Platform and Architecture

[0203] The humanoid picking robot is a complex system. From the perspective of software control, there are two major types of motion execution mechanisms. One is a mobile chassis that needs to be able to move autonomously (walk), and the other is the dual arms that execute tasks. At the same time, the execution of the dual-arm picking task is based on the visual guidance of a depth camera, which is a core visual service system. These three parts constitute the main controlled entities and application functions of the picking system, which are the main implementation objectives of the control software.

[0204] Fundamentally, the software system of the humanoid picking robot is a robot control system. ROS is a currently widely used robot control system. It can manage and transmit multi-sensor data: cameras, robotic arms, lidar, chassis, etc., which can all be defined as its module nodes, and these nodes communicate by subscribing to the data of relevant topics. Thus, ROS itself is a very good modular system architecture. Based on the ROS ecosystem, there are very good mature modules for reference in the autonomous navigation of the mobile chassis and the motion control of the robotic arms. Its plug-in mechanism also provides a huge space for optimization and expansion. Therefore, the software structure of the humanoid robot system integrates the ROS system for the motion control of the mobile chassis for robot autonomous navigation and the humanoid robotic arms. At the same time, considering the real-time performance and efficiency of visual processing, for the use of the depth camera for identifying and positioning the picking target around the depth visual service function, we do not use the node module communication method of ROS. Instead, we directly collect the camera image and video stream using the underlying api of the depth camera, extract the image frames from the image data stream, align the 2D images with the depth information frames, and then use the yolo detection module for inference to obtain the result data, and use the mqtt method to exchange the result data with the application.

[0205] In addition, compared with traditional robots, more intelligent humanoid robots must have the ability to recognize environmental intelligence and have a more intelligent interaction method with humans. Therefore, at the application level, we introduced the ollama large language model as the carrier of the robot's artificial intelligence to provide a natural language interaction method for the human-machine intelligent interaction of the picking robot. The specific implementation architecture is as Figure 8 shown.

[0206] (1) Visual service module

[0207] Use the depth camera installed on the end effector to obtain images, detect and locate cluster tomatoes. Use the depth camera to capture the on-site scene, and segment the object to be cut through the visual weight model file. Then, it calculates the 3D coordinates of the object and publishes this data through the public topic.

[0208] (2) Motion planning module

[0209] It is the moveit control node that plans the motion path of the robotic arm for picking according to the picking target information and the workspace configuration.

[0210] (3) Robotic arm control module

[0211] 2 ROS node instances accept the motion path of the motion planning module and control the left and right robotic arms to execute the path respectively.

[0212] (4) Vehicle control module

[0213] It is used to interact with the chassis ROS navigation module node.

[0214] (5) Software communication module

[0215] Some of these modules belong to ROS nodes. For the consideration of visual processing efficiency, the visual service module does not use ros node drive, but directly reads and processes images and makes inferences. The communication between these modules includes both the communication between nodes in the ros domain and the communication through the mqtt communication service, and provides a unified SDK api for calling and using in the application.

[0216] (6) python-ab-api

[0217] The picking application uniformly uses this api for function calls or receiving module information. For example, on the one hand, the application orders the visual service topic through the api interface to obtain the target tomato positioning information, and also sends a motion planning request call to the motion planning module through the api, and finally drives the robotic arm to the target point to execute the picking action through the robotic arm control module.

[0218] (7) Natural language interaction application module

[0219] Use the open-source large language model ollama as a platform to establish a local knowledge base for the picking robot system, and uniformly access relevant information to the large language model for more convenient retrieval, query, and use. More importantly, the API of the picking robot can be docked with olloama, enabling the system to be called and used through a common natural language interaction UI.

[0220] 3. Robot control system

[0221] In the control system of a mobile composite robot, the motion control module is one of the most important core modules. Based on the object position information obtained through visual service processing, it plans the actions of (double) arm grasping or shearing. And because it is located on a moving chassis, the overall motion control becomes more complex. Generally speaking, the following four parts cooperate to jointly complete the picking task.

[0222] (1) Vehicle chassis motion control

[0223] Based on vision-based target tracking and detection, adjust the control speed in real time, and precisely control the movement of the vehicle chassis and picking in real time.

[0224] The implementation of this control strategy depends on the real-time detection of the target. When the main target is detected to approach the working space, deceleration control needs to be carried out according to its arrival at the best position in the working space (i.e., close to the center position and biased towards the rear).

[0225] (2) Manipulator motion control

[0226] First, define the motion space of the manipulator to ensure that the picking object is located in the working space. At this time, actually two steps are taken to obtain a safe picking path. First, determine the joint angles according to the target position through inverse kinematics, and then calculate the required motion trajectory according to the collision avoidance configuration parameters to ensure that the end effector can effectively approach and position the target object in an optimized and collision-free manner.

[0227] Inverse kinematics solution of joint angles:

[0228] a. Establish a robot model and coordinate system

[0229] Establish a coordinate system for each joint, usually using the Denavit-Hartenberg (D-H) parameter method. Assign a number i (i = 1, 2,..., 6) to each joint, and define the D-H parameters of each joint:

[0230] 1) a_{i - 1}: Link length, that is, the distance from the z_{i - 1} axis to the z_i axis along the x_{i - 1} axis.

[0231] 2) a_{i - 1}: The torsional angle of the connecting rod, i.e., the angle between the z_{i - 1} axis and the z_i axis around the x_{i - 1} axis.

[0232] 3) d_i: The joint offset, i.e., the distance from the x_{i - 1} axis to the x_i axis along the z_{i - 1} axis.

[0233] 4) theta_i: The joint angle, i.e., the angle between the x_{i - 1} axis and the x_i axis around the z_{i - 1} axis.

[0234] b. Inverse kinematics solution

[0235] Given the target position and orientation of the end - effector, solve for the six joint angles theta_1, theta_2, theta_3, theta_4, theta_5, theta_6.

[0236] Solve for theta_1

[0237] The angle between the projection of the end - effector position on the x - y plane of the base coordinate system and the x - axis is related to theta_1. By analyzing the coordinates of the end - effector position (x, y, z) and using the arctangent function, the value of theta_1 can be initially determined:

[0238] theta_1 = arctan2(y, x)+{constant offset}

[0239] Solve for theta_3

[0240] Simplify the robotic arm into a planar 2 - link mechanism and solve through geometric relations and trigonometric functions. Given the distance from the shoulder joint to the center of the wrist joint and relevant information about the target position, theta_3 can be calculated using the cosine theorem:

[0241] Cos(theta_3)=(l_1^2 + l_2^2 - d^2) / (2l_1*l_2)

[0242] Where l_1, l_2 are the relevant link lengths and d is a specific geometric distance.

[0243] Solve for theta_2

[0244] After obtaining theta_3, calculate theta_2 using geometric relations and trigonometric functions. The structure of the robotic arm and the determined parameters need to be considered, and combined with the end - effector position for further solution.

[0245] Solve for theta_4, theta_5 and theta_6

[0246] Determined by the decomposition of the rotation matrix and the pose relationship. The pose matrix of the end effector is related to the transformation matrix of the known joint angles, and solved using trigonometric function relationships and the properties of the rotation matrix.

[0247] Collision-free motion trajectory planning

[0248] Calculating and obtaining the required motion trajectory based on collision-free configuration parameters is a key task, aiming to ensure that the robot does not collide with obstacles in the surrounding environment during the movement from the starting point to the target point. The following is a detailed description of this process:

[0249] a. Environment modeling and parameter definition

[0250] Environment modeling: Model the working space where the robot is located, using a three-dimensional spatial representation. Obstacles are described by polygons, recording their positions, dimensions, and other information.

[0251] Collision-free configuration parameters: According to the shape and size of the robot, such as radius, length, and width; the motion constraints of the robot, maximum speed, acceleration, turning radius; safety distance, the minimum distance between the robot and obstacles.

[0252] b. Use the Rapidly-exploring Random Tree (RRT) path search algorithm

[0253] Starting from the starting point, continuously generate new points randomly, and grow the tree towards this point until the tree contains the target point or a collision-free path is found.

[0254] c. Trajectory optimization

[0255] Smoothing process: The initial path obtained through path search is discontinuous or too tortuous, and needs to be smoothed. The method of spline curve fitting is used to fit the path points into a smooth curve to make the movement of the robot more fluent.

[0256] Velocity planning: According to the motion constraints of the robot and the task requirements, allocate appropriate velocities to each point on the trajectory to ensure safety and stability.

[0257] d. Use a combination of local and global collision detection

[0258] Local collision detection: During the process of generating the trajectory, continuously check the distance between the robot and the surrounding obstacles to ensure that the safety distance requirement is met at any time.

[0259] Global collision detection: After the trajectory generation is completed, perform global collision detection on the entire trajectory to ensure that the robot does not collide with obstacles during the entire movement process. If a collision is detected during the process, readjust the trajectory.

[0260] Dual-arm collaborative motion mechanism

[0261] The tomato planting scenario belongs to an unstructured environment, and the growth process has natural randomness. The planting density is high, the branches and leaves are intertwined, and the heights are inconsistent in all directions. When two-arm picking robots work simultaneously, there is a risk of interference, collision, and pulling of branches and leaves. As Figure 9 shown.

[0262] Two-arm setting: In principle, the two arms do not overlap, collide, or interfere with each other during picking.

[0263] Hybrid working strategy: According to the planting density, a hybrid working strategy of parallel and sequential picking is adopted.

[0264] Specifically, when the planting density is high, after parking, multiple clusters of tomatoes grow closely in a single-frame visual image. A single robotic arm is used for picking to avoid collisions (the robotic arm itself is equipped with an anti-collision parking sensor); when the planting density is low, after parking, a small number of clusters of tomatoes grow closely in a single-frame visual image, and two robotic arms pick simultaneously to avoid collisions.

[0265] In this embodiment, the picking vision camera is installed at the end of the left arm. Due to the complexity of the above-mentioned on-site environment, when using two arms for picking, the following control method is implemented:

[0266] First, the motion working spaces of the two arms cannot overlap, and this space configuration is used to plan the motion paths of the robotic arms.

[0267] Second, the central global motion controller needs to allocate picking tasks according to the target positions within the visual range. For example, according to the x-axis numerical range of the camera, the pending picking targets are assigned to the left and right arms;

[0268] The picking task is a motion control process of the robotic arm. According to the detected pose information of the target, the motion control module is called to specifically implement the above-mentioned robotic arm motion control.

[0269] Third, the control center synchronizes the tasks of the two arms. First, based on the left arm, the target entering the working area is determined, the vehicle speed is controlled, and the vehicle stops and enters the picking state. Finally, it is necessary to ensure that both arms have completed the picking work before continuing the inspection.

[0270] After determining the target entering the working area, according to the configured optimal picking position parameters, the vehicle is controlled to stop. After stopping, the two arms are in their respective working spaces, and the picking task is started. The motion control module of the robotic arm is scheduled to plan the motion path for picking work. Until the work tasks of each arm are completed and there are no pickable fruits (clusters) in the area, the inspection state is switched to conduct the inspection. As Figure 10 shown.

[0271] (3) Target tracking and feedback: Position servo

[0272] Real-time tracking of the target and feedback of the target pose are extremely important for maintaining the accuracy or success rate of picking. In practice, it is not uncommon for abnormal camera depth information to occur due to factors such as light reflection. At this time, it is necessary to dynamically adjust the observation position and re-dynamically adjust and plan the path of the robotic arm.

[0273] (4) Shearing and grasping mechanism

[0274] After the end of the robotic arm can reach the predetermined target position, it is also necessary to design a good end effector that combines shearing and clamping to complete the final shearing and grasping operation. The mechanism uses a motor to control its final shearing posture and action implementation.

[0275] The present invention adopts the collaborative work of two arms, and the configuration of the humanoid robotic arm is more suitable for the picking scenario and can cooperate more effectively.

[0276] In the description of the present application, it should be understood that the orientation or positional relationship indicated by the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation to the present application.

[0277] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be arbitrarily combined with each other.

Claims

1. A dual-arm picking robot system, characterized in that: include: A robot body, a storage module (410), a moving module and a robot control module; The robot body is mounted on the mobile module via a rotating lifting waist and hip joint, and the storage module (410) is arranged on the mobile module; the mobile module can move with the robot body and the storage module (410); A multi-degree-of-freedom mechanical right arm and a multi-degree-of-freedom mechanical left arm are arranged on both sides of the robot body, a right-arm end picking gripper (201) is arranged at the right end of the multi-degree-of-freedom mechanical right arm, and a left-arm end picking gripper (214) is arranged at the end of the multi-degree-of-freedom mechanical left arm; An image acquisition module (101) is arranged on the top of the robot body, and a laser radar module is arranged on the mobile module, and the image acquisition module (101) and the laser radar module collect data and transmit it to the robot control module; The robot control module controls the operation of the rotating and lifting waist and hip joint, the multi-degree-of-freedom mechanical right arm, the multi-degree-of-freedom mechanical left arm, the picking gripper (201) at the end of the right arm, the picking gripper (214) at the end of the left arm, and the moving module.

2. The dual-arm picking robot system according to claim 1, characterized in that: The image acquisition module (101) is a first depth camera; The first depth camera is connected to the robot body via a neck rotation joint (102) for intelligent recognition of the environment; The robot body is provided with a second depth camera for close-range movement, obstacle avoidance and dynamic path planning; A third depth camera is arranged on the picking gripper (214) at the end of the left arm, for detecting picking targets.

3. The dual-arm picking robot system according to claim 1, characterized in that: The mobile module comprises: a mobile chassis (411); the mobile chassis (411) is provided with chassis rear track wheels (403), chassis rear universal wheels (404), chassis driving wheels (405), chassis front track wheels (406) and chassis front universal wheels (407); The chassis front track wheel (406) and the chassis front universal wheel (407) are located at the front end of the mobile chassis (411), and the chassis rear track wheel (403) and the chassis rear universal wheel (404) are located at the rear end of the mobile chassis (411); The chassis driving wheel (405) is located between the chassis front track wheel (406) and the chassis rear track wheel (403).

4. The dual-arm picking robot system according to claim 3, characterized in that: The laser radar module comprises: a rear laser radar (401) and a front laser radar (409); The rear laser radar (401) and the front laser radar (409) are arranged on the mobile chassis (411), the rear laser radar (401) is located at the front end of the mobile chassis (411), and the front laser radar (409) is located at the rear end of the mobile chassis (411); The rear end of the mobile chassis (411) is provided with a rear anti-collision strip (402), and the front end of the mobile chassis (411) is provided with a front anti-collision strip (408).

5. The dual-arm picking robot system according to claim 1, characterized in that: The multi-degree-of-freedom mechanical right arm is a six-degree-of-freedom mechanical right arm; The six-degree-of-freedom mechanical left arm comprises: a first right arm joint (207), a second right arm joint (206), a third right arm joint (205), a fourth right arm joint (204), a fifth right arm joint (203) and a sixth right arm joint (202) which are connected in sequence; The first right arm joint (207) is connected to the robot body, and the sixth right arm joint (202) is connected to the picking gripper (201) at the end of the right arm; The multi-degree-of-freedom mechanical left arm is a six-degree-of-freedom mechanical left arm; The six-degree-of-freedom mechanical left arm comprises: a first left arm joint (208), a second left arm joint (209), a third left arm joint (210), a fourth left arm joint (211), a fifth left arm joint (212) and a sixth left arm joint (213); The first left arm joint (208) is connected to the robot body, and the sixth left arm joint (213) is connected to the picking gripper (214) at the end of the left arm; The rotating and lifting waist and hip joint comprises: a waist rotating joint (301), a first waist lifting joint (302) and a second waist lifting joint (303) which are connected in sequence; The waist rotation joint (301) is connected to the robot body, and the second waist lifting joint (303) is connected to the moving module.

6. The dual-arm picking robot system according to claim 1, characterized in that: The robot control module includes: a visual service module, a motion planning module, a robotic arm control module, a vehicle control module, a software communication module, a python-ab-api module and a natural language interaction application module; The visual service module uses the image acquisition module (101) to capture scene image information, segments a picking target from the scene image information through a visual weight model file, calculates the 3D coordinates of the picking target, and detects and locates the picking target; The motion planning module plans the motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm for picking according to the information of the picking target and the workspace configuration; The robotic arm control module receives the motion path output by the motion planning module, and controls the multi-degree-of-freedom robotic right arm and the multi-degree-of-freedom robotic left arm to execute the motion path respectively; The vehicle control module interacts with the laser radar module, the laser radar module can perform SLAM mapping and navigation, and the vehicle control module controls the operation of the mobile module; The software communication module is used for communication between modules; The python-ab-api module is used to call each module and / or receive information from each module; The natural language interaction application module is used to realize human-computer interaction with the robot.

7. A control method for a dual-arm picking robot, characterized in that: Used to control the dual-arm picking robot system according to any one of claims 1 to 6, specifically comprising the following steps: The vehicle chassis motion control step includes: performing SLAM mapping through the laser radar module, planning the motion trajectory of the mobile module, and making the mobile module perform navigation inspection according to the motion trajectory; performing real-time image acquisition of the picking target through the image acquisition module (101), tracking and detecting the picking target based on the acquired image data, obtaining the position information of the picking target, and adjusting and controlling the speed of the mobile module in real time according to the position information of the picking target; Robot arm motion control step: defining the motion space of the multi-degree-of-freedom robot right arm and the multi-degree-of-freedom robot left arm so that the picking object can be located in the workspace; determining the joint angles of the multi-degree-of-freedom robot right arm and the multi-degree-of-freedom robot left arm through inverse kinematics according to the position information of the picking target; According to the determined joint angles and collision-free configuration parameters, the collision-free motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm are obtained, so that the picking claw (201) at the end of the right arm and the picking claw (214) at the end of the left arm approach the picking target in a collision-free manner; Dual-arm collaborative motion control step: optimizing and adjusting the motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm so that the motion spaces of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm do not overlap; Target tracking and feedback step: real-time tracking of the picking target and real-time feedback of the position and posture of the picking target, and dynamically adjusting and planning the motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm according to the feedback of the position and posture of the picking target; Shearing and grabbing step: After the picking claw (201) at the end of the right arm and the picking claw (214) at the end of the left arm reach the position of the picking target, the shearing and grabbing operation is completed, and the picking target is placed in the storage module (410).

8. The dual-arm picking robot control method according to claim 7, characterized in that: The determining of the joint angles of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm by inverse kinematics specifically comprises the following steps: Steps to build robot model and coordinate system: A Denavit-Hartenberg parameter method is used to establish a coordinate system for each joint of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm, a number i is assigned to each joint, and a DH parameter of each joint is defined; i = 1, 2, ..., 6; a_{i-1}: connecting rod length, the distance from the z_{i-1} axis to the z_i axis along the x_{i-1} axis; a_{i-1}: connecting rod torsion angle, the angle between z_{i-1} axis and z_i axis around x_{i-1} axis; d_i: joint offset, the distance from the x_{i-1} axis to the x_i axis along the z_{i-1} axis; theta_i: joint angle, the angle between the x_{i-1} axis and the x_i axis around the z_{i-1} axis; Inverse kinematics solution steps: According to the position information and posture of the picking claw (201) at the end of the right arm and the picking claw (214) at the end of the left arm, six joint angles theta_1, theta_2, theta_3, theta_4, theta_5, theta_6 are solved; Solving for theta_1: The projections of the picking claw (201) at the end of the right arm and the picking claw (214) at the end of the left arm on the xy plane of the base coordinate system are related to the x-axis angle and theta_1. By analyzing the coordinates of the positions (x, y, z) of the picking claw (201) at the end of the right arm and the picking claw (214) at the end of the left arm, the value of theta_1 is determined using the inverse tangent function: theta_1 = arctan2(y,x) + {constant offset}; Solving for theta_3: The multi-DOF mechanical right arm and the multi-DOF mechanical left arm are simplified into a planar 2-link mechanism, which is solved by geometric relationships and trigonometric functions; theta_3 is calculated by the cosine theorem based on the distance from the shoulder joint to the center of the wrist joint and relevant information on the target position: Cos(theta_3)=(l_1^2+l_2^2-d^2) / (2l_1*l_2); Among them, l_1, l_2 are the lengths of the relevant connecting rods, and d is the specific geometric distance; Solving for theta_2: After theta_3 is obtained, theta_2 is calculated using geometric relationships and trigonometric functions; the structure and determined parameters of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm are considered, and the position of the picking gripper (201) at the end of the right arm and the picking gripper (214) at the end of the left arm are further solved; Solving for theta_4, theta_5, and theta_6: The posture matrices of the picking gripper (201) at the end of the right arm and the picking gripper (214) at the end of the left arm are determined by decomposing the rotation matrix and the posture relationship, and the transformation matrix of the known joint angle is connected, and the solution is obtained by using the trigonometric function relationship and the properties of the rotation matrix.

9. The dual-arm picking robot control method according to claim 7, characterized in that: The obtaining of the collision-free motion paths of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm specifically comprises the following steps: The robot's workspace is modeled using three-dimensional space representation, obstacles are described using polygons, and their position and size information are recorded; The collision avoidance configuration parameters include: the shape and size of the robot, the robot's motion constraints and the safety distance; the shape and size of the robot include: the robot's radius, length and width; the robot's motion constraints include: maximum speed, acceleration and turning radius; the safety distance is the minimum distance between the robot and the obstacle; Using the random tree path search algorithm, starting from the starting point in the three-dimensional space, a new point is continuously randomly generated, and the tree is grown toward the point until the tree contains the picking target point and a collision-free path is found; The path points of the collision-free path are fitted into a smooth curve using the spline curve fitting method; Assign appropriate velocities to each point on the collision-free path based on the robot's motion constraints and task requirements; In the process of generating a collision-free path, the distance between the robot and surrounding obstacles is checked in real time to ensure that the safety distance requirements are met at any time; After the collision-free path is generated, a global collision detection is performed on the entire collision-free path to ensure that the robot does not collide with obstacles during the entire movement process. If a collision is detected during the process, the collision-free path is readjusted.

10. The dual-arm picking robot control method according to claim 7, characterized in that: In the dual-arm collaborative motion control step: According to the position information of the picking targets in the image data, the picking targets in different ranges are allocated to the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm; Determine the picking target entering the working interval based on the multi-degree-of-freedom mechanical left arm. After determining that the picking target enters the working interval, control the speed control and stop of the mobile module according to the configured optimal picking position parameters, so that the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm start the picking task in their respective working spaces, and schedule the picking work using the motion path planning in the mechanical arm motion control step, until there are no picking targets to be picked in the working spaces of the multi-degree-of-freedom mechanical right arm and the multi-degree-of-freedom mechanical left arm, and then switch to the inspection state for inspection.

Images