Path planning method and system for decoupling composite mechanical arm of harvesting robot

Abstract

The present invention provides a path planning method and system for decoupling a composite manipulator of a harvesting robot, including: constructing and storing a reachability map of the manipulator space; calculating the cost of the manipulator posture operation and lifting control according to the reachability map; and planning the path of the composite manipulator according to the cost decoupling. The present invention solves the problems of high computational complexity and low completeness of path planning for redundant manipulators, and realizes the coordinated movement of the lifting mechanism joint and the manipulator through decoupling, ensuring high-probability path planning of the manipulator at different fruit hanging heights, and generating the optimal path to ensure the successful execution of the picking task, reducing the calculation time of redundant path planning, ensuring the efficient execution of the tomato picking task, improving the path planning success rate of the picking robot and reducing the planning time.

Description

Technical Field

[0001] The present invention belongs to the field of path planning, and in particular, relates to a path planning method and system for decoupling a composite mechanical arm of a picking robot. Background Art

[0002] With the development of agricultural automation, the application of automated harvesting technology in agricultural production has become increasingly important. Tomatoes are widely grown economic crops around the world. Their growth process is complex, especially in greenhouse environments. The heights and growth postures of different fruits vary greatly, making the robot's autonomous harvesting task extremely challenging. The existing tomato picking task relies on a large amount of manpower and is labor-intensive. In addition, in a high temperature and high humidity environment, workers face health risks from long-term work, which further increases labor costs. In order to accelerate the process of "machine replacement" in the tomato production process, research on phenotypic analysis of tomato plants and related aspects of automated harvesting has been gradually carried out.

[0003] The harvesting robot faces many challenges in the unstructured harvesting environment. When the tomato fruits are hung at different heights and in different postures, the working range of the harvesting robot's manipulator arm is difficult to cover all the areas where the fruit can be picked. At the same time, the complex grasping posture further reduces the probability completeness of the manipulator's multi-waypoint path planning.

[0004] The patent document "A fruit picking method, picking robot and electronic device based on a picking robot" (CN118493395A) discloses the parking position of the vehicle to optimize the placement of the robot arm, and to avoid collision and energy consumption as the loss function determination criteria, emphasizing the best placement position in the horizontal plane. However, for each new scene, a grid map needs to be constructed, and the inverse kinematics of reaching the target needs to be calculated, and then the placement position of the robot arm in the plane needs to be searched based on multi-objective optimization, rather than using prior knowledge to directly optimize the position. Planning the placement position of the robot arm is time-consuming.

[0005] Therefore, how to build a composite robot by combining articulated robot arms and Cartesian robot arm components in a narrow greenhouse environment, and effectively plan and control the coordinated movement of the robot arm and the lifting mechanism joints through decoupling has become a key technical problem that needs to be solved urgently. Summary of the invention

[0006] In view of the defects in the prior art, the object of the present invention is to provide a path planning method and system for decoupling the composite mechanical arm of a picking robot.

[0007] A path planning method for decoupling a composite mechanical arm of a picking robot provided by the present invention includes:

[0008] Step S1: construct and store the reachability map of the robot space;

[0009] Step S2: Calculate the cost of the robot arm posture operation and lifting control according to the reachability map;

[0010] Step S3: Plan the path of the composite robot arm according to the cost decoupling.

[0011] Preferably, the step S1 comprises:

[0012] Step S1.1: Input the URDF model of the picking robot;

[0013] Step S1.2: Generate a set of sampling points and a corresponding set of sampling postures;

[0014] Step S1.3: Screen the sampling points that meet the conditions and store them in the accessibility map.

[0015] The URDF model provides the physical parameters of the robotic arm, including joint positions, link lengths, and collision models.

[0016] In step S1.2, the P x , P y , P z The sampling point set S point and R-based x , R y , R z The sampling pose set S pose ;

[0017] P x ={-0.61+0.05a|a=0, 1,..., 24}, P y ={-0.20+0.05b|b=0, 1,..., 16}, P z ={0.00+0.05k|k=0, 1,...,17}, R x ={-180+15c|c=0, 1, ..., 12}, R y =0, R z ={-45+15n|n=0, 1,..., 6};

[0018] Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis;

[0019] P y Represents the value of the discrete point sampled in the workspace on the Y-axis;

[0020] P z Represents the value of the discrete point sampled in the workspace on the Z axis;

[0021] R x Indicates the rotation angle around the X axis;

[0022] Ry represents the rotation angle around the Y axis;

[0023] R z Indicates the rotation angle around the Z axis;

[0024] a represents the value corresponding to the calculation of P x The a-th index of ;

[0025] b represents the calculation of P y The b-th index of ;

[0026] k represents the value corresponding to the calculation of P z The kth index of ;

[0027] c represents the value corresponding to the calculation of R x The c-th index of ;

[0028] n represents the value corresponding to the calculation R z The nth index of .

[0029] The screening in step S1.3 is to determine p i 、r i Does it meet the solvable constraint condition? If so, store it in the reachability map M arm If not, continue to traverse the next p i 、r i .

[0030] The solvable constraint condition is that there exists an inverse kinematics solution with self-collision detection;

[0031] Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ;

[0032] r i Indicates that in the sampling posture set S pose Any i-th posture in .

[0033] Preferably, in step S2, based on the reachability map, the manipulator posture operation cost and the lifting control cost of the lifting mechanism joints are constructed, including a similarity calculation step, a gravity cost calculation step, and a movement cost calculation step.

[0034] The similarity calculation step comprises:

[0035] Step S2.1: In the reachability map M arm Select the closest point in the YZ plane (y c , z c ) and its corresponding point set Set point ;

[0036] Step S2.2: The two rotation angles of the target pose are compared with the reachable pose of a point on the reachability map, and the angle difference is calculated.

[0037] In step S2.2, for the set Set of midpoints in the horizontal plane YZ point Each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1. If yes, calculate the similarity cost and store the similarity cost of each point in the set Set pose ;

[0038] Among them, Set point Represents the target position (y c , z c ) corresponds to a set of points with different X-axis positions;

[0039] Set pose Represents the set of poses for computing similarity costs.

[0040] The similarity cost is calculated for:

[0041]

[0042] Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture;

[0043] They represent the x-axis and z-axis rotation angles of the reachable posture of the j-th point on the reachability map respectively;

[0044] d a Indicates the minimum attitude angle resolution on the reachability map;

[0045] Set pose Represents the set of prior reachability poses at a discrete point on the prior map;

[0046] Indicates the similarity between the jth reachable posture of the i-th point and the target posture;

[0047] It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

[0048] Preferably, step S2 includes a gravity cost calculation step: determining the center position of the area by calculating the upper and lower limits, and calculating the gravity cost of the center gravity of the manipulator

[0049]

[0050] Among them, a k Represents the width of any continuous reachable area;

[0051] Indicates the starting position of any continuously reachable area;

[0052] Indicates the end position of any continuously reachable area;

[0053] Represents the location of a discrete point in the ascending and descending direction of the accessibility map.

[0054] The movement cost calculation step includes: calculating the position h of the lifting mechanism joint: i ,

[0055] Among them, h c It is represented as the current position of the lifting mechanism joint;

[0056] Indicates the value of the target pose relative to the current base of the robot;

[0057] The movement cost of the lifting mechanism joint is

[0058] Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

[0059] Preferably, the step S3 comprises:

[0060] Step S3.1: Obtain the joint height set of the lifting mechanism;

[0061] Step S3.2: compile the cost into a cost matrix of (n, 3) dimensions;

[0062] Step S3.3: Sort the cost matrix to obtain an ordered joint height set Set height ;

[0063] Step S3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm .

[0064] The cost includes similarity cost, gravity cost and movement cost.

[0065] In step S3.1, multi-objective optimization sorting is performed based on the cost to generate an ordered lifting mechanism joint height set {h1, ...h n}, where n represents the total number of n lifting mechanism joint heights h.

[0066] The sorting described in step S3.3 is a fast non-dominated sorting method, which sorts the accessibility probabilities of discrete points along the ascending and descending moving directions.

[0067] In step S3.4, path planning is performed on the joint lifting height and TCP target posture, using a time-optimal path parameterization algorithm, and the sampling method is RRT* sampling.

[0068] The judgment includes:

[0069] Step S3.4.1: Determine h i Is it in the range [h min ,h max ], if not, skip the lifting height, if so, perform inverse kinematics to obtain the target joint angle of the robot arm, execute step S3.4.2 and perform obstacle avoidance planning in C space;

[0070] Step S3.4.2: Determine whether there is a result for the inverse kinematics solution. If so, output the lifting height h i and the robot arm path; if it does not exist, execute step S3.4.1 to determine the next joint lifting height;

[0071] Among them, h i ∈{h1,……h n}, h i Represents the lifting height of the i-th joint.

[0072] A path planning system for decoupling a composite mechanical arm of a picking robot provided by the present invention comprises:

[0073] Module M1: Build and store the reachability map of the robot space;

[0074] Module M2: Calculate the cost of manipulator posture manipulation and lifting control based on the reachability map;

[0075] Module M3: Plan the path of the composite robot based on cost decoupling.

[0076] Preferably, the module M1 comprises:

[0077] Module M1.1: Input the URDF model of the picking robot;

[0078] Module M1.2: Generate a set of sampling points and a corresponding set of sampling postures;

[0079] Module M1.3: Screen the sampling points that meet the requirements and store them in the accessibility map.

[0080] The URDF model provides the physical parameters of the robotic arm, including joint positions, link lengths, and collision models.

[0081] The module M1.2 generates a P x , P y , P z The sampling point set S point and R-based x , R y , R z The sampling pose set S pose ;

[0082] P x ={-0.61+0.05a|a=0, 1,..., 24}, P y ={-0.20+0.05b|b=0, 1,..., 16}, P z ={0.00+0.05k|k=0, 1,...,17}, R x ={-180+15c|c=0, 1, ..., 12}, R y =0, R z ={-45+15n|n=0, 1,..., 6};

[0083] Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis;

[0084] P y Represents the value of the discrete point sampled in the workspace on the Y-axis;

[0085] P z Represents the value of the discrete point sampled in the workspace on the Z axis;

[0086] R x Indicates the rotation angle around the X axis;

[0087] R y Indicates the rotation angle around the Y axis;

[0088] R z Indicates the rotation angle around the Z axis;

[0089] a represents the value corresponding to the calculation of P x The a-th index of ;

[0090] b represents the calculation of P y The b-th index of ;

[0091] k represents the value corresponding to the calculation of P zThe kth index of ;

[0092] c represents the value corresponding to the calculation of R x The c-th index of ;

[0093] n represents the value corresponding to the calculation R z The nth index of .

[0094] The screening described in the module M1.3 is to determine p i 、r i Does it meet the solvable constraint condition? If so, store it in the reachability map M arm If not, continue to traverse the next p i 、r i .

[0095] The solvability constraint is that there exists an inverse kinematics solution with self-collision detection.

[0096] Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ;

[0097] r i Indicates that in the sampling posture set S pose Any i-th posture in .

[0098] Preferably, the module M2 constructs the manipulator posture operation cost and the lifting control cost of the lifting mechanism joints based on the reachability map, including a similarity calculation module, a gravity cost calculation module, and a movement cost calculation module.

[0099] The similarity calculation module comprises:

[0100] Module M2.1: On the accessibility map M arm Select the closest point in the YZ plane (y c , z c ) and its corresponding point set Set point ;

[0101] Module M2.2: Compare the two rotation angles of the target pose with the reachable pose of a point on the reachability map and calculate the angle difference.

[0102] In the module M2.2, for the set Set of midpoints in the horizontal plane YZ point Each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1. If yes, calculate the similarity cost and store the similarity cost of each point in the set Set pose ;

[0103] Among them, Set point Represents the target position (y c , z c ) corresponds to a set of points with different X-axis positions;

[0104] Set pose represents the pose set for calculating similarity cost;

[0105] The similarity cost is calculated for:

[0106]

[0107] Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture;

[0108] They represent the x-axis and z-axis rotation angles of the reachable posture of the j-th point on the reachability map respectively;

[0109] d a Indicates the minimum attitude angle resolution on the reachability map;

[0110] Set pose Represents the set of prior reachability poses at a discrete point on the prior map;

[0111] Indicates the similarity between the jth reachable posture of the i-th point and the target posture;

[0112] It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

[0113] Preferably, the module M2 includes a gravity cost calculation module: the center position of the area is determined by calculating the upper and lower limits, and the gravity cost of the central gravity of the manipulator is calculated.

[0114]

[0115] Among them, a k Represents the width of any continuous reachable area;

[0116] Indicates the starting position of any continuously reachable area;

[0117] Indicates the end position of any continuously reachable area;

[0118] Represents the location of a discrete point in the ascending and descending direction of the accessibility map.

[0119] The movement cost calculation module includes: calculating the position h of the lifting mechanism joint i ,

[0120] Among them, h c It is represented as the current position of the lifting mechanism joint;

[0121] Indicates the value of the target pose relative to the current base of the robot;

[0122] The movement cost of the lifting mechanism joint is

[0123] Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

[0124] Preferably, the module M3 comprises:

[0125] Module M3.1: Get the joint height set of the lifting mechanism;

[0126] Module M3.2: compile the cost into a (n, 3)-dimensional cost matrix;

[0127] Module M3.3: Sort the cost matrix to obtain an ordered joint height set Set height ;

[0128] Module M3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm .

[0129] The cost includes similarity cost, gravity cost and movement cost.

[0130] In the module M3.1, multi-objective optimization sorting is performed based on cost to generate an ordered lifting mechanism joint height set {h1,...h n}, where n represents the total number of n lifting mechanism joint heights h.

[0131] The sorting described in the module M3.3 is a fast non-dominated sorting method, which sorts the accessibility probabilities of discrete points along the ascending and descending moving directions.

[0132] The module M3.4 performs path planning for the joint lifting height and TCP target posture, adopts the time optimal path parameterization algorithm, and the sampling method is RRT* sampling.

[0133] The judgment includes:

[0134] Module M3.4.1: Determine h i Is it in the range [h min ,h max ], if not, skip the lifting height, if so, perform inverse kinematics to obtain the target joint angle of the robot arm, trigger module M3.4.2 and execute obstacle avoidance planning in C space;

[0135] Module M3.4.2: Determine whether there is a result for the inverse kinematics solution. If so, output the lift height h i and the robot arm path; if it does not exist, the module M3.4.1 is triggered to determine the next joint lifting height;

[0136] Among them, h i ∈{h1,……h n}, h i Represents the lifting height of the i-th joint.

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

[0138] 1. The present invention solves the problems of high computational complexity and low probability completeness of path planning for redundant robotic arms, thereby improving the path planning success rate of the picking robot and reducing the planning time.

[0139] 2. The present invention realizes the coordinated movement of the lifting mechanism joints and the robotic arm by decoupling, ensures high-probability path planning of the robotic arm at different fruit hanging heights, and generates the optimal path to ensure the successful execution of the picking task, thereby ensuring high-completeness and high-efficiency path planning.

[0140] 3. The present invention can not only effectively cope with complex scenarios with different fruit hanging heights, but also eliminate the need for inverse kinematics solutions and other accessibility judgments during the optimization process, reduce the calculation time of redundant path planning, and ensure the efficient execution of tomato picking tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0141] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0142] Figure 1 Schematic diagram of the path planning method for decoupling the composite manipulator arm of the picking robot.

[0143] Figure 2 Schematic diagram of constructing a reachability map of discrete points in the workspace of a composite manipulator of a picking robot.

[0144] Figure 3 Schematic diagram of the calculation of the center of gravity for the composite manipulator operation of the picking robot. DETAILED DESCRIPTION

[0145] The present invention is 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 are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0146] In order to overcome the problems of high computational complexity and low completeness of path planning probability of redundant mechanical arms in the prior art, increase the success rate of path planning of redundant mechanical arms of picking robots, and reduce the path planning time, a path planning method for decoupling a composite mechanical arm of a picking robot is provided according to the present invention, comprising:

[0147] Step S1: construct and store the reachability map of the robot space;

[0148] Specifically, the discrete point positions in the workspace are obtained by discretely sampling the tool center position (TCP) of the robot. Figure 2 Taking the picking robot as an example, based on the URDF (Unified Robot Description Format) model, a discrete point reachability map of the robotic arm's workspace is constructed, and the inverse kinematics solution and self-collision detection are combined to form a reachability map of the workspace.

[0149] The robot reachability map construction methods include:

[0150] Step S1.1: Input the URDF model of the picking robot into the physical simulation engine. The model provides physical parameters for collision detection and kinematics solution, including the physical parameters of the robotic arm, such as joint position, link length, collision model, etc. These parameters provide necessary support for collision detection and inverse kinematics solution.

[0151] Step S1.2: The system discretely samples the TCP position within a defined spatial boundary, or spatial range, and each sampling point represents a specific spatial position in the robot arm workspace.

[0152] P x ={-0.61+0.05a|a=0, 1,..., 24};

[0153] P y ={-0.20+0.05b|b=0, 1,..., 16};

[0154] P z ={0.00+0.05k|k=0, 1,...,17};

[0155] R x ={-180+15c|c=0, 1,..., 12};

[0156] R y =0;

[0157] R z ={-45+15n|n=0, 1,..., 6};

[0158] Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis;

[0159] P y Represents the value of the discrete point sampled in the workspace on the Y-axis;

[0160] P z Represents the value of the discrete point sampled in the workspace on the Z axis;

[0161] R x The sampling posture is expressed in the form of Euler angles, which represents the rotation angle around the X axis;

[0162] R y Indicates the rotation angle around the Y axis;

[0163] R z Indicates the rotation angle around the Z axis;

[0164] a represents the value corresponding to the calculation of P x The ath index is used to determine the x coordinate of the point, starting from -0.61 with a step size of 0.05;

[0165] b represents the calculation of P y The b-th index is used to determine the y coordinate of the point, starting from -0.20 with a step size of 0.05;

[0166] k represents the value corresponding to the calculation of P z The kth index of is used to determine the z coordinate of the point, starting from 0.00 with a step size of 0.05;

[0167] c represents the value corresponding to the calculation of R x The cth index is used to determine the x rotation angle, starting from -180 degrees with a step size of 15 degrees;

[0168] n represents the value corresponding to the calculation R z The nth index is used to determine the z rotation angle, starting from -45 degrees and the step size is 15 degrees;

[0169] Generate based on P x , P y , Pz The sampling point set and R x , R y , R z The sampled pose set.

[0170] Step S1.3: Solve the inverse kinematics problem, determine the self-collision, and select the spatial sampling points that meet the conditions and store them in the reachability map.

[0171] Determine p i 、r i Does it meet the solvable constraint condition? If so, store it in the reachability map M arm If not, continue to traverse the next p i 、r i .

[0172] The solvable constraints are solved by inverse kinematics solution with self-collision detection.

[0173] At each spatial sampling point, the system further solves the inverse kinematics problem of different TCP postures and determines whether the robot arm has self-collision through collision detection. If a TCP posture is valid at a spatial point, it means that the robot arm can operate at this spatial position. Then, the posture and its corresponding spatial point are stored in the reachability map to form the minimum storage unit of the reachability map. Each valid spatial point and its corresponding feasible posture are finally stored in the reachability map M. arm middle.

[0174] Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ;

[0175] r i Indicates that in the sampling posture set S pose Any i-th posture in ;

[0176] S point Represents the point set generated based on position coordinate sampling, including different P x , P y and P z coordinate;

[0177] S pose Represents the posture combination obtained based on posture angle sampling, including different R x , R y , R z attitude angle;

[0178] After completing the height position of the robotic arm, the inverse kinematics of the robotic arm that has arrived is calculated once. In this way, a custom robotic arm reachability spatial prior model is constructed, and the system gradually forms a reachability map of the robotic arm in the entire workspace. Each storage unit in the reachability map contains valid spatial points and their corresponding posture data. Subsequently, according to the given arrival posture, the prior map is directly searched, and no additional calculation of the arrival position is required, which provides the necessary offline prior data support for subsequent path planning and task execution, greatly saving the time of path planning.

[0179] Step S2: Calculate the cost of the robot arm posture operation and the cost of lifting control;

[0180] Before performing multi-objective optimization, it is necessary to construct the multi-objective optimization operation cost of the joint control of the robot arm and the lifting mechanism. Specifically, based on the reachability map, the robot arm posture operation cost and the lifting control cost are calculated, and the height position of the robot arm base is optimized using multi-objective optimization.

[0181] Three optimization objectives are proposed, and specific equations for similarity cost and gravity cost are also proposed. The optimization objectives similarity cost and gravity cost emphasize the accessibility of the target posture by giving the first priority to maximizing the reachability of the end of the robot arm in space in the prior map. The last optimization objective, the joint motion cost of the lifting mechanism, is to minimize the motion path.

[0182] Before performing multi-objective optimization on the placement height of the robot, it is necessary to determine the operation cost of the robot and the lifting mechanism joints. The lifting mechanism joints can move along the X-axis direction of the track (i.e., the X-axis of the robot base coordinate system) to adjust the position of the robot base to match the optimal prior position and target pose on the reachability map, so that the robot can reach the target pose with the highest probability. This probability is defined as the similarity cost, and the similarity cost calculation method includes:

[0183] Step S2.1: In the reachability map M arm Select the closest point in the YZ plane (y c , z c ) and its corresponding point set Set point .

[0184] Step S2.2: Two rotation angles of the target pose (r x and r z ) and the reachable posture of a point on the reachability map ( and ) to compare and calculate the angle difference;

[0185] For the set of points Set with different X values ​​found in the horizontal plane YZ point , for each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1; if yes, calculate the similarity cost. Store the similarity cost of each point in the set Set pose .

[0186] Among them, Set point Represents the target pose (y c , z c ) corresponding to the set of points with different x values;

[0187] Set pose The pose set representing the similarity cost is used to store the pose similarities of different poses;

[0188] There are multiple reachable poses at a discrete point on the prior map, Indicates the similarity between the jth reachable posture of the point and the target posture, It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

[0189] The pose that is most similar to the given pose of the discrete point is called the similarity cost

[0190]

[0191] Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture;

[0192] They represent the x-axis and z-axis rotation angles of the j-th reachable position on the reachability map respectively;

[0193] d a Indicates the minimum attitude angle resolution on the reachability map;

[0194] represents the set of prior reachability poses at a discrete point i on the prior map.

[0195] Each time the robot reaches the target position, it returns to the center of the reachable workspace. This center position is usually at the midpoint of the range of motion of each joint. This position allows the robot to respond and adjust its motion more flexibly, avoiding complex path problems caused by large-angle joint adjustments.

[0196] Gravity cost calculation steps: Figure 3 For example, for a continuously reachable area, the center position of the area is determined by calculating the upper and lower limits, and the gravity cost of the center gravity of the manipulator is calculated using the following formula

[0197]

[0198] Among them, a k Represents the width of any continuous reachable area;

[0199] Indicates the starting position of any continuously reachable area;

[0200] Indicates the end position of any continuously reachable area;

[0201] Represents the location of a discrete point in the ascending and descending direction of the accessibility map.

[0202] Movement cost calculation steps: The movement distance of the lifting mechanism joint is used as a cost target in motion planning. The optimization goal is to reduce the operation time by minimizing the lifting height. In order to place the robot's TCP at a discrete point with a high probability of being reachable in the reachability map, the robot base needs to move, and the position h of the lifting mechanism joint is i It can be calculated by the relevant formula:

[0203]

[0204] Among them, h c It is represented as the current position of the lifting mechanism joint;

[0205] Indicates the value of the target pose relative to the current base of the robot.

[0206] The movement cost of the lifting mechanism joint is

[0207]

[0208] Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

[0209] The reachability map of the robot arm is calculated offline. For discrete points in the reachability map, not only the number of reachable poses is stored, but also the reachable poses of each discrete point are directly stored. During the reasoning process, the closest discrete point is directly indexed to determine the similarity between the reachable pose and the reachable pose. In this way, the reachability judgment such as inverse kinematics solution is eliminated during the optimization process, which reduces the algorithm solution time.

[0210] Step S3: planning the path of the composite robot arm according to the cost decoupling;

[0211] The path planning system controls the coordinated motion of the robot arm and the lifting mechanism joints respectively by decoupling, thus realizing high-completeness path planning for the fruit picking posture. The path planning system is decoupled into two planning subsystems, the lifting mechanism joint position planning subsystem and the robot arm joint position path planning subsystem.

[0212] The lifting mechanism joint position planning subsystem is responsible for planning the placement height of the robot arm base. The lifting mechanism joint can move along the track (the X-axis of the robot arm base coordinate system) and adjust the height of the base to ensure that the robot arm is as close to the target posture as possible under different fruit hanging heights and end approach postures, and perform path planning for the picking task with the maximum completeness probability.

[0213] Step S3.1: Obtain the lifting mechanism joint height set according to the similarity cost.

[0214] Based on the information in the reachability map and the given target arrival posture, a multi-objective optimization sorting is performed based on the similarity cost, gravity cost, and lifting mechanism joint movement cost. By performing multi-objective optimization on these costs, an ordered list of candidate lifting mechanism joint heights is generated to ensure that the robot arm can perform the picking task with the highest completeness probability at different heights. The lifting mechanism joint height set {h1,...h n}, n means there are n lifting mechanism joints with height h in total.

[0215] Step S3.2: Compile the cost into a (n, 3)-dimensional cost matrix.

[0216] Step S3.3: Sort the cost matrix to obtain an ordered joint height set Set height .

[0217] After determining the candidate heights of the lifting mechanism joints, the robot arm joint path planning subsystem performs a robot arm path planning traversal on each ordered list of candidate joint heights.

[0218] After determining the operation cost, that is, providing the operation cost for different positions of the discrete points along the X-axis, the system sorts the accessibility probabilities of the discrete points along the X-axis direction (the direction of lifting and lowering), and sets a threshold to filter out the spatial positions with lower accessibility probabilities.

[0219] The positions of different lifting heights are optimized and sorted based on the fast non-dominated sorting method to obtain the Pareto frontier, saving time loss in the multi-objective optimization process. Building an offline prior knowledge map is time-consuming. When building the prior knowledge, the reachability of different reaching positions in the manipulator reachability space has been obtained, and there is no need to perform inverse kinematics solution judgment in the multi-objective optimization process.

[0220] Step S3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm . Perform path planning for the new robot arm placement height and TCP target position, and use the time-optimal path parameterization algorithm to generate the motion trajectory of each joint of the robot arm. i Is it in the range [h min ,h max ] If yes, then perform inverse kinematics to obtain the target joint angle of the robot arm and perform obstacle avoidance planning in C space (Configuration Space); if no, skip the lifting height;

[0221] Determine whether the inverse kinematics solution has a result. If so, output the lifting height h i and the robot arm path; if it does not exist, continue to judge the next joint lifting height.

[0222] Among them, h i ∈{h1,……h n}, h i Represents the lifting height of the i-th joint.

[0223] In more preferred examples, an inverse kinematics solver is used to calculate the joint angle configuration of the robot arm in the target posture. In order to generate a path from the initial joint angle configuration to the target joint configuration, the system can use sampling methods such as RRT* to sample and plan the path in the configuration space. When performing path planning, the system first selects a candidate height for the joint of the lifting mechanism and executes the robot path planning at this height. The path generated for each candidate height will record indicators such as the length of the path, the probability of accessibility, and the complexity of joint motion. Finally, the system selects an optimal path for execution based on these indicators.

[0224] In the composite robot controller, the position control of the lifting mechanism joints is performed first, and then the position sequence action is performed on the robot joint sequence, which emphasizes the control of the lifting mechanism joints to optimize the placement height of the robot arm, and the optimization is the placement position in the vertical direction. Through this decoupled path planning system, the present invention can achieve the coordinated movement of the robot arm and the lifting mechanism joints, and greatly improve the efficiency and success rate of path planning through multi-objective optimization. The present invention can not only effectively cope with complex scenarios with different fruit hanging heights, but also reduce the calculation time of redundant path planning, ensuring the efficient execution of tomato picking tasks.

[0225] The present invention also provides a path planning system for the decoupling of a picking robot's composite robotic arm. The path planning system for the decoupling of a picking robot's composite robotic arm can be realized by executing the process steps of the path planning method for the decoupling of a picking robot's composite robotic arm. That is, those skilled in the art can understand the path planning method for the decoupling of a picking robot's composite robotic arm as a preferred implementation of the path planning system for the decoupling of a picking robot's composite robotic arm.

[0226] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.

[0227] A path planning system for decoupling a composite mechanical arm of a picking robot provided by the present invention comprises:

[0228] Module M1: Build and store the reachability map of the robot space;

[0229] Module M2: Calculate the cost of manipulator posture manipulation and lifting control based on the reachability map;

[0230] Module M3: Plan the path of the composite robot based on cost decoupling.

[0231] In more preferred embodiments, the module M1 includes:

[0232] Module M1.1: Input the URDF model of the picking robot;

[0233] Module M1.2: Generate a set of sampling points and a corresponding set of sampling postures;

[0234] Module M1.3: Screen the sampling points that meet the requirements and store them in the accessibility map.

[0235] The URDF model provides the physical parameters of the robotic arm, including joint positions, link lengths, and collision models.

[0236] The module M1.2 generates a P x , P y , P z The sampling point set S point and R-based x , R y , R z The sampling pose set S pose ;

[0237] P x ={-0.61+0.05a|a=0, 1,..., 24}, P y =[-0.20+0.05b|b=0, 1, ..., 16}, P z ={0.00+0.05k|k=0, 1,...,17}, R x ={-180+15c|c=0, 1, ..., 12}, R y =0, R z ={-45+15n|n=0, 1,..., 6};

[0238] Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis;

[0239] P y Represents the value of the discrete point sampled in the workspace on the Y-axis;

[0240] P z Represents the value of the discrete point sampled in the workspace on the Z axis;

[0241] R x Indicates the rotation angle around the X axis;

[0242] Ry represents the rotation angle around the Y axis;

[0243] R z Indicates the rotation angle around the Z axis;

[0244] a represents the value corresponding to the calculation of P x The a-th index of ;

[0245] b represents the calculation of P y The b-th index of ;

[0246] k represents the value corresponding to the calculation of P z The kth index of ;

[0247] c represents the value corresponding to the calculation of R x The c-th index of ;

[0248] n represents the value corresponding to the calculation R z The nth index of .

[0249] The screening in the module M1.3 is to determine whether pi and ri meet the solvable constraint conditions. If so, store them in the reachability map M arm If not, continue to traverse the next p i 、r i .

[0250] The solvability constraint is that there exists an inverse kinematics solution with self-collision detection.

[0251] Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ;

[0252] r i Indicates that in the sampling posture set S pose Any i-th posture in .

[0253] In more preferred examples, the module M2 constructs a multi-objective optimization operation cost for the joint control of the robot arm and the lifting mechanism based on the reachability map, including a similarity calculation module, a gravity cost calculation module, and a movement cost calculation module.

[0254] The similarity calculation module comprises:

[0255] Module M2.1: On the accessibility map M arm Select the closest point in the YZ plane (y c , z c ) and its corresponding point set Set point ;

[0256] Module M2.2: Compare the two rotation angles of the target pose with the reachable pose of a point on the reachability map and calculate the angle difference.

[0257] In the module M2.2, for the set Set of midpoints in the horizontal plane YZ point Each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1. If yes, calculate the similarity cost and store the similarity cost of each point in the set Setpose ;

[0258] Among them, Set point Represents the target pose (y c , z c ) corresponding to the set of points with different x values;

[0259] Set pose represents the pose set for calculating similarity cost;

[0260] The similarity cost is calculated for:

[0261]

[0262] Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture;

[0263] They represent the x-axis and z-axis rotation angles of the reachable posture of the j-th point on the reachability map respectively;

[0264] d a Indicates the minimum attitude angle resolution on the reachability map;

[0265] Set pose Represents the set of prior reachability poses at a discrete point on the prior map;

[0266] Indicates the similarity between the jth reachable posture of the i-th point and the target posture;

[0267] It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

[0268] In more preferred embodiments, the module M2 includes a gravity cost calculation module: the center position of the area is determined by calculating the upper and lower limits, and the gravity cost of the center gravity of the manipulator is calculated.

[0269]

[0270] Among them, a k Represents the width of any continuous reachable area;

[0271] Indicates the starting position of any continuously reachable area;

[0272] Indicates the end position of any continuously reachable area;

[0273] Represents the location of a discrete point in the ascending and descending direction of the accessibility map.

[0274] The movement cost calculation module includes: calculating the position h of the lifting mechanism joint i ,

[0275] Among them, h c It is represented as the current position of the lifting mechanism joint;

[0276] Indicates the value of the target pose relative to the current base of the robot;

[0277] The movement cost of the lifting mechanism joint is

[0278] Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

[0279] In more preferred embodiments, the module M3 includes:

[0280] Module M3.1: Get the joint height set of the lifting mechanism;

[0281] Module M3.2: compile the cost into a (n, 3)-dimensional cost matrix;

[0282] Module M3.3: Sort the cost matrix to obtain an ordered joint height set Set height ;

[0283] Module M3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm .

[0284] The cost includes similarity cost, gravity cost and movement cost.

[0285] In the module M3.1, multi-objective optimization sorting is performed based on cost to generate an ordered lifting mechanism joint height set {h1,...h n}, where n represents the total number of n lifting mechanism joint heights h.

[0286] The sorting described in the module M3.3 is a fast non-dominated sorting method, which sorts the accessibility probabilities of discrete points along the ascending and descending moving directions.

[0287] The module M3.4 performs path planning for the joint lifting height and TCP target posture, adopts the time optimal path parameterization algorithm, and the sampling method is RRT* sampling.

[0288] The judgment includes:

[0289] Module M3.4.1: Determine h i Is it in the range [h min ,h max ], if not, skip the lifting height, if so, perform inverse kinematics to obtain the target joint angle of the robot arm, trigger module M3.4.2 and execute obstacle avoidance planning in C space;

[0290] Module M3.4.2: Determine whether there is a result for the inverse kinematics solution. If so, output the lift height h i and the robot arm path; if it does not exist, the module M3.4.1 is triggered to determine the next joint lifting height;

[0291] Among them, h i ∈{h1,……h n}, h i Represents the lifting height of the i-th joint.

[0292] The above describes the specific embodiments of the present invention. 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. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A path planning method for decoupling a composite mechanical arm of a picking robot, characterized in that: include: Step S1: construct and store the reachability map of the robot space; Step S2: Calculate the manipulator posture operation cost and the lifting control cost of the lifting mechanism joint according to the reachability map; Step S3: planning the path of the composite robot arm according to the cost decoupling; In the step S2, based on the reachability map, the manipulator posture operation cost and the lifting control cost of the lifting mechanism joint are constructed, including a similarity calculation step, a gravity cost calculation step, and a movement cost calculation step; The similarity calculation step comprises: Step S2.1: In the reachability map M arm Select the closest point in the YZ plane (y c , z c ) and the point set Set point ; Step S2.2: compare the two rotation angles of the target pose with the reachable pose of a point on the reachability map, and calculate the angle difference; In step S2.2, for the set Set of points in the horizontal plane YZ point Each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1. If yes, calculate the similarity cost and store the similarity cost of each point in the set Set pose ; Among them, Set point Represents the target position (y c , z c ) corresponds to a set of points with different X-axis positions; Set pose represents the pose set for calculating similarity cost; The similarity cost is calculated for: Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture; They represent the x-axis and z-axis rotation angles of the reachable posture of the j-th point on the reachability map respectively; d a Indicates the minimum attitude angle resolution on the reachability map; Set pose Represents the set of prior reachability poses at a discrete point on the prior map; Indicates the similarity between the jth reachable posture of the i-th point and the target posture; It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

2. The path planning method for decoupling the composite mechanical arm of a picking robot according to claim 1, characterized in that: The step S1 comprises: Step S1.1: Input the URDF model of the picking robot; Step S1.2: Generate a set of sampling points and a corresponding set of sampling postures; Step S1.3: Screen the sampling points that meet the conditions and store them in the accessibility map; The URDF model provides the physical parameters of the robot, including joint positions, link lengths, and collision models; In step S1.2, the P x , P y , P z The sampling point set S point and R-based x , R y , R z The sampling pose set S pose ; P x ={-0.61+0.05a|a=0,1,..,24}、P y ={-0.20+0.05b|b=0,1,...,16}、P z ={0.00+0.05k|k=0,1,...,17}、R x ={-180+15cc=0,1,...,12}、R y =0、Rz={-45+15n|n=0,1,...,6}; Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis; P y Represents the value of the discrete point sampled in the workspace on the Y-axis; P z Represents the value of the discrete point sampled in the workspace on the Z axis; R x Indicates the rotation angle around the X axis; R y Indicates the rotation angle around the Y axis; R z Indicates the rotation angle around the Z axis; a represents the value corresponding to the calculation of P x The a-th index of ; b represents the calculation of P y The b-th index of ; k represents the value corresponding to the calculation of P z The kth index of ; c represents the value corresponding to the calculation of R x The c-th index of ; n represents the value corresponding to the calculation of R z The nth index of ; The screening in step S1.3 is to determine p i 、r i Does it meet the solvable constraint condition? If so, store it in the reachability map M arm If not, continue to traverse the next p i 、r i ; The solvable constraint condition is that there exists an inverse kinematics solution with self-collision detection; Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ; r i Indicates that in the sampling posture set S pose Any i-th posture in .

3. The path planning method for decoupling the composite mechanical arm of a picking robot according to claim 1, characterized in that: The step S2 includes a gravity cost calculation step: determining the center position of the area by calculating the upper and lower limits, and calculating the gravity cost of the center gravity of the manipulator Among them, a k Represents the width of any continuous reachable area; Indicates the starting position of any continuously reachable area; Indicates the end position of any continuously reachable area; Represents the location of a discrete point in the ascending and descending direction in the accessibility map; The movement cost calculation step includes: calculating the position h of the lifting mechanism joint: i , Among them, h c It is represented as the current position of the lifting mechanism joint; Indicates the value of the target pose relative to the current base of the robot; The movement cost of the lifting mechanism joint is Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

4. The path planning method for decoupling the composite mechanical arm of a picking robot according to claim 1, characterized in that: The step S3 comprises: Step S3.1: Obtain the joint height set of the lifting mechanism; Step S3.2: compile the cost into a cost matrix of (n, 3) dimensions; Step S3.3: Sort the cost matrix to obtain an ordered joint height set Set height ; Step S3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm ; The cost includes similarity cost, gravity cost and movement cost; In step S3.1, multi-objective optimization sorting is performed based on the cost to generate an ordered lifting mechanism joint height set {h1, ...h n }, where n represents the total number of n lifting mechanism joint heights h; The sorting described in step S3.3 is a fast non-dominated sorting method, which sorts the accessibility probabilities of discrete points along the ascending and descending movement directions; In step S3.4, path planning is performed on the joint lifting height and TCP target posture, using a time-optimal path parameterization algorithm, and the sampling method is RRT* sampling to sample the robot arm path; The judgment includes: Step S3.4.1: Determine h i Is it in the range [h min ,h max ], if not, skip the lifting height, if so, perform inverse kinematics to obtain the target joint angle of the robot arm, execute step S3.4.2 and perform obstacle avoidance planning in C space; Step S3.4.2: Determine whether there is a result for the inverse kinematics solution. If so, output the lifting height h i and the robot arm path; if it does not exist, execute step S3.4.1 to determine the next joint lifting height; Among them, h i ∈{h1,……h n }, h i Represents the lifting height of the i-th joint.

5. A path planning system for decoupling a composite mechanical arm of a picking robot, characterized in that: include: Module M1: Build and store the reachability map of the robot space; Module M2: Calculate the manipulator posture operation cost and the lifting control cost of the lifting mechanism joints according to the reachability map; Module M3: Plan the path of the composite robot based on cost decoupling; The module M2 constructs the manipulator posture operation cost and the lifting control cost of the lifting mechanism joint based on the reachability map, including a similarity calculation module, a gravity cost calculation module, and a movement cost calculation module; The similarity calculation module comprises: Module M2.1: On the accessibility map M arm Select the closest point in the YZ plane (y c , z c ) and the point set Set point ; Module M2.2: Compare the two rotation angles of the target pose with the reachable pose of a point on the reachability map and calculate the angle difference; In the module M2.2, for the set Set of midpoints in the horizontal plane YZ point Each point p at different heights i , to determine whether the point on the prior reachability map has a prior pose. If not, the similarity cost Set to 1. If yes, calculate the similarity cost and store the similarity cost of each point in the set Set pose ; Among them, Set point Represents the target position (y c , z c ) corresponds to a set of points with different X-axis positions; Set pose represents the pose set for calculating similarity cost; The similarity cost is calculated for: Among them, r x 、r z Respectively represent the x-axis and z-axis rotation angles of the target posture; They represent the x-axis and z-axis rotation angles of the reachable posture of the j-th point on the reachability map respectively; d a Indicates the minimum attitude angle resolution on the reachability map; Set pose Represents the set of prior reachability poses at a discrete point on the prior map; Indicates the similarity between the jth reachable posture of the i-th point and the target posture; It represents the maximum similarity between the reachable posture of the i-th point and the target posture.

6. The path planning system for decoupling the composite mechanical arm of a harvesting robot according to claim 5, characterized in that: The module M1 comprises: Module M1.1: Input the URDF model of the picking robot; Module M1.2: Generate a set of sampling points and a corresponding set of sampling postures; Module M1.3: Screen the sampling points that meet the conditions and store them in the accessibility map; The URDF model provides the physical parameters of the robot, including joint positions, link lengths, and collision models; The module M1.2 generates a P x , P y , P z The sampling point set S point and R-based x , R y , R z The sampling pose set S pose ; P x ={-0.61+0.05a|a=0,1,...,24}、P y ={-0.20+0.05b|b=0,1,...,16}、P z ={0.00+0.05k|k=0,1,...,17}、R x ={-180+15c|c=0,1,...,12}、R y =0、R z ={-45+15n|n=0,1,...,6}; Among them, P x Represents the value of the discrete point sampled in the workspace on the X-axis; P y Represents the value of the discrete point sampled in the workspace on the Y-axis; P z Represents the value of the discrete point sampled in the workspace on the Z axis; R x Indicates the rotation angle around the X axis; R y Indicates the rotation angle around the Y axis; R z Indicates the rotation angle around the Z axis; a represents the value corresponding to the calculation of P x The a-th index of ; b represents the calculation of P y The b-th index of ; k represents the value corresponding to the calculation of P z The kth index of ; c represents the value corresponding to the calculation of R x The c-th index of ; n represents the value corresponding to the calculation of R z The nth index of ; The screening described in the module M1.3 is to determine p i 、r i Does it meet the solvable constraint condition? If so, store it in the reachability map M arm If not, continue to traverse the next p i 、r i ; The solvable constraint condition is that there exists an inverse kinematics solution with self-collision detection; Among them, p i Indicates that in the sampling point set S point Any i-th spatial point in ; r i Indicates that in the sampling posture set S pose Any i-th posture in .

7. The path planning system for decoupling the composite mechanical arm of a harvesting robot according to claim 5, characterized in that: The module M2 includes a gravity cost calculation module: the center position of the area is determined by calculating the upper and lower limits, and the gravity cost of the center gravity of the manipulator is calculated. Among them, a k Represents the width of any continuous reachable area; Indicates the starting position of any continuously reachable area; Indicates the end position of any continuously reachable area; Represents the location of a discrete point in the ascending and descending direction in the accessibility map; The movement cost calculation module includes: calculating the position h of the lifting mechanism joint i , Among them, h c It is represented as the current position of the lifting mechanism joint; Indicates the value of the target pose relative to the current base of the robot; The movement cost of the lifting mechanism joint is Among them, h max 、h min They represent the upper and lower limit positions of the lifting mechanism joints respectively.

8. The path planning system for decoupling the composite mechanical arm of a harvesting robot according to claim 5, characterized in that: The module M3 comprises: Module M3.1: Get the joint height set of the lifting mechanism; Module M3.2: compile the cost into a (n, 3)-dimensional cost matrix; Module M3.3: Sort the cost matrix to obtain an ordered joint height set Set height ; Module M3.4: Determine the lifting height h of each joint i , get the target joint height h lift and the robot path τ arm ; The cost includes similarity cost, gravity cost and movement cost; In the module M3.1, multi-objective optimization sorting is performed based on cost to generate an ordered lifting mechanism joint height set {h1,...h n }, where n represents the total number of n lifting mechanism joint heights h; The sorting described in the module M3.3 is a fast non-dominated sorting method, which sorts the accessibility probabilities of discrete points along the ascending and descending movement directions; In the module M3.4, path planning is performed for the joint lifting height and TCP target posture, and a time-optimal path parameterization algorithm is used. The sampling method is RRT* sampling to sample the robot arm path; The judgment includes: Module M3.4.1: Determine h i Is it in the range [h min ,h max ], if not, skip the lifting height, if so, perform inverse kinematics to obtain the target joint angle of the robot arm, trigger module M3.4.2 and execute obstacle avoidance planning in C space; Module M3.4.2: Determine whether there is a result for the inverse kinematics solution. If so, output the lift height h i and the robot arm path; if it does not exist, the module M3.4.1 is triggered to determine the next joint lifting height; Among them, h i ∈{h1,……h n }, h i Represents the lifting height of the i-th joint.

Images