Multi-thread path planning method and device of flexible cable type picking mechanical arm

Abstract

The application provides a kind of flexible cable type picking mechanical arm multi-thread path planning method and device, it is related to picking mechanical arm technical field, the core step of the present application is based on the initial geometric path generated by fast random search tree, at least three optimization threads with different optimization objectives are triggered in parallel, respectively generate candidate paths focusing on dexterity, tension smoothness and obstacle avoidance robustness, and finally the optimal path to be executed is screened out through multi-objective evaluation in sliding window. The parallel multi-thread optimization step can simultaneously explore and integrate the path optimization potential of different performance dimensions, significantly improving the overall quality of the planned path. Compared with the existing single thread or serial optimization technology, the local optimal or performance short board problem caused by a single optimization objective is effectively avoided. The present application systematically promotes the overall balance of the final execution path in length, safety, stability and reliability, and realizes efficient and stable picking in complex orchard environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of picking mechanical arm, in particular to a multi-thread path planning method and device of a flexible cable picking mechanical arm. BACKGROUND

[0002] In the field of orchard intelligent picking, especially for the picking of high-value woody oil crops such as camellia oleifera fruits and olives, using picking mechanical arms to replace manual labor to complete the picking of economic crops such as camellia oleifera fruits has become an important development direction. Camellia oleifera fruits usually grow in the interior of the tree canopy or in dense areas of branches and leaves, surrounded by a large number of small branches, leaves and irregular spatial structures. The picking environment has typical characteristics of unstructured, strong occlusion and limited space. In such scenarios, the mechanical arm needs to complete precise obstacle avoidance and end positioning operations in a small space, which puts high requirements on the environmental perception accuracy, motion continuity and real-time adjustment capability of path planning. In particular, the flexible cable picking mechanical arm relies on the coordinated changes of multiple driving ropes for end motion, which has the advantages of strong flexibility and large workspace, but is also more sensitive to the stability and rationality of path planning.

[0003] In traditional path planning technology, commonly used algorithms include A algorithm 、 D algorithm, rapid random tree, etc. These algorithms search for a path from the starting position to the target position by constructing a graph model or a tree structure. However, existing path planning methods still face some technical challenges in practical applications. First, due to the complexity of the scene, the irregular distribution of obstacles and the limited motion of the mechanical arm, a single path planning algorithm often cannot efficiently handle various unexpected situations. Second, many existing technologies focus too much on avoiding obstacles or simply generating paths, and fail to effectively consider multi-objective optimization problems such as path smoothness, picking time, and flexibility of mechanical arm motion.

[0004] In addition, existing technologies have low computational efficiency when facing complex scenarios. Path optimization usually relies on a single objective function, and lacks a comprehensive optimization strategy specific to the needs of picking tasks, which results in poor path planning results and fails to meet the high requirements of flexible cable picking mechanical arms for path precision and efficiency. The above information disclosed in the background section is only used to enhance the understanding of the background of the present disclosure, and therefore it can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0005] The present application aims to provide a multi-thread path planning method and device for a flexible cable picking mechanical arm to solve the problems raised in the background.

[0006] To achieve the above-mentioned purpose, the present application provides the following technical solutions:

[0007] A multi-thread path planning method of a flexible cable picking mechanical arm, the specific steps comprising:

[0008] Step 1: Obtain the scene point cloud information of the target planning area, perform semantic segmentation on the scene point cloud to classify the camellia fruit area, obstacle area and background area, determine the initial pose and target picking pose of the mechanical arm end based on the classification result, and perform voxelization processing on the scene point cloud, establish a three-dimensional occupancy grid map and mark the grid area in a free state;

[0009] Step 2: In the set of grid areas marked as free state, based on the initial pose and target picking pose of the mechanical arm end, execute the rapid random search tree algorithm, when sampling new nodes in the random search tree expansion, introduce a node screening mechanism based on dexterity estimation, and generate an initial geometric path;

[0010] Step 3: Based on the initial geometric path, trigger at least three optimization threads with different optimization objectives in parallel, and each thread optimizes the initial geometric path according to its optimization objective to obtain at least three candidate paths;

[0011] Step 4: Based on the generated candidate paths, evaluate and calculate the comprehensive score of each path in a sliding window, the length of the sliding window is fixed, the candidate paths are comprehensively evaluated in the window using a multi-objective cost function, and the to-be-executed path is selected according to the comprehensive score of all sliding windows in each candidate path, and the mechanical arm moves along the to-be-executed path.

[0012] Further, a pre-trained three-dimensional segmentation model is used to process the scene point cloud information, specifically including:

[0013] The three-dimensional segmentation model takes the scene point cloud information as input, and the semantic labels of each point cloud point as labels, thereby segmenting the scene point cloud information into three types of point cloud clusters: target camellia fruit, obstacle and background;

[0014] For each target camellia fruit point cloud cluster, calculate the center point coordinates of its three-dimensional bounding box, and offset the center point in the opposite direction of gravity by a preset grabbing distance to obtain the position coordinates of the target picking pose corresponding to the camellia fruit.

[0015] Further, the logic of establishing a three-dimensional occupancy grid map is:

[0016] Based on all the target camellia fruit point cloud clusters and the obstacle point cloud clusters, an axis-aligned three-dimensional bounding box is calculated which surrounds all of them; the axis-aligned three-dimensional bounding box is uniformly expanded outward by a preset safety margin in the three-dimensional space, and the space region of the expanded axis-aligned three-dimensional bounding box is the working space of the mechanical arm; the working space is discretized into a uniform three-dimensional voxel network, and a three-dimensional occupancy grid map of the obstacle is generated based on the obstacle point cloud clusters by using an expansion and erosion algorithm; in the three-dimensional occupancy grid map, the voxels where the obstacle point cloud clusters are located and the adjacent voxels are marked as obstacle regions and are recorded as an occupancy state; for the voxels where the target camellia fruit point cloud is located and the adjacent voxels, they are marked as a temporary free space which can be penetrated, and the rest of the voxels are marked as a free space, and the temporary free space and the free space are marked as a free state; the safety margin is set to be not less than 1.5 times the maximum circumscribed sphere radius of the end effector of the mechanical arm.

[0017] Further, the fast random search tree algorithm performs path search by iteratively generating random sampling points and adding new nodes to the random tree, and the node screening mechanism based on dexterity estimation is as follows:

[0018] For each new node randomly sampled in the expansion process of the fast random search tree algorithm, a force transmission matrix composed of unit directional vectors of all driven ropes when the end of the mechanical arm is located at the pose corresponding to the new node is calculated; the condition number of the force transmission matrix is calculated, and the reciprocal of the condition number is defined as the dexterity estimation value of the new node; the condition number is the ratio of the maximum singular value to the minimum singular value of the force transmission matrix;

[0019] The dexterity estimation value of the new node is compared with a preset safety threshold, and the rejection probability of the new node is calculated, the rejection probability = 1 - (dexterity estimation value / safety threshold), when the calculation result is greater than 1, it is taken as 1, and when it is less than 0, it is taken as 0;

[0020] When executing the fast random search tree algorithm, a uniform random number between 0 and 1 is generated every time a new node is generated and its rejection probability is calculated; if the uniform random number is less than the rejection probability, it is determined that the new node is located in a singular region, and it is discarded and resampled; if the uniform random number is not less than the rejection probability, the node is accepted;

[0021] For the accepted new node, it is added to the random tree according to the standard process of the fast random search tree, and is connected to the existing tree nodes; the above process is repeated until the random tree successfully connects the initial pose and the target picking pose, at this time the generated node sequence is backtracked, which is the initial geometric path.

[0022] Further, the trigger includes a first optimization thread, a second optimization thread and a third optimization thread.

[0023] The first optimization thread traverses all nodes on the initial geometric path, calculates the minimum singular value of the rope traction direction matrix corresponding to each node, and takes the minimum singular value as a local dexterity index of the node;

[0024] For a node with a local dexterity index lower than a first preset threshold, the first optimization thread calculates a gradient direction of the local dexterity index with respect to the position and pose parameters of the robot arm end; by moving the node along the gradient direction by a preset fixed step, a new node sequence is generated, thereby obtaining a first candidate path under the first optimization thread.

[0025] Further, the second optimization thread discretizes the initial geometric path into a series of ordered path points according to time intervals; for each intermediate path point that is neither a starting point nor a terminal point, taking adjustment of its three-dimensional space coordinates as an optimization variable, taking the minimum amplitude of rope tension changes at the path point and its two adjacent path points as an optimization objective, and taking satisfaction of the robot arm end static force balance equation at the three points and all rope tension values within a preset positive tension range as constraint conditions, a local optimization problem is constructed; by iteratively solving the local optimization problems of all intermediate path points, the new space coordinates of the optimized intermediate path points are obtained in sequence; the starting point, the new space coordinates of all optimized intermediate path points, and the terminal point are jointly connected to form a new path as a second candidate path.

[0026] Further, the third optimization thread sets a dynamic evaluation window with a fixed length, and intercepts K consecutive path points from the initial geometric path corresponding to the currently planned starting position as a to-be-evaluated path segment; for each path point in the to-be-evaluated path segment, the tension feasibility is calculated, specifically: under the premise of satisfying the robot arm end static force balance, a maximum positive scalar factor is found, such that when the basic tension values of the ropes are multiplied by the positive scalar factor, they can still be within the preset positive tension range; the maximum positive scalar factor is the tension feasibility of the predicted path point; if the tension feasibility of any path point in the to-be-evaluated path segment is detected to be lower than a second preset threshold, the path point is marked as a potential singular point; if there is at least one potential singular point, the third optimization thread takes an unmarked path point before the first potential singular point as a starting point of local re-planning, takes the end point of the to-be-evaluated path segment as a terminal point, and re-plans a local obstacle avoidance path from the starting point to the terminal point; the local obstacle avoidance path replaces the original path segment from the starting point to the terminal point in the initial geometric path, thereby forming a third candidate path; K is the number of path points on the to-be-evaluated path segment.

[0027] Further, the calculation logic of the multi-objective cost function is specifically:

[0028] extract three candidate path segments corresponding to the preset sliding window in space and equal in length from the first candidate path, the second candidate path and the third candidate path respectively, and mark them as the first candidate path segment, the second candidate path segment and the third candidate path segment respectively; wherein each candidate path segment is composed of a series of ordered discrete spatial positions, which are collectively referred to as path points in this step;

[0029] calculate the multi-objective cost function of each candidate path segment respectively, including path length cost, dexterity safety cost, tension smoothness cost and collision risk cost;

[0030] weight and sum the path length cost, the dexterity safety cost, the tension smoothness cost and the collision risk cost according to the preset weight coefficient to obtain the comprehensive score of each candidate path segment;

[0031] The preset sliding window starts from the initial pose, and each time the comprehensive score of the current sliding window is calculated, the endpoint of the current sliding window is taken as a new starting point, and the sliding window is moved towards the endpoint of each candidate path to form a new candidate path segment and calculate the comprehensive score of the new candidate path segment; repeat the process until the endpoint of the sliding window coincides with the endpoint of each candidate path;

[0032] calculate the average value of the comprehensive scores of all candidate path segments in each candidate path, and take the average value as the comprehensive score of the corresponding candidate path;

[0033] select the candidate path with the lowest comprehensive score as the to-be-executed path segment.

[0034] Further, the path length cost is to calculate the total length of the spatial trajectory of the candidate path segment;

[0035] The dexterity safety cost is to calculate the minimum value of the local dexterity index corresponding to all path points on the candidate path, and take the reciprocal of the minimum value as the dexterity safety cost;

[0036] The tension smoothness cost is to calculate the sum of the variances of the tension change curve of each driving rope for tracking the path along the candidate path;

[0037] The collision risk cost is to perform collision detection between the end effector envelope sphere corresponding to each path point on the candidate path segment and the voxel marked as occupied state in the three-dimensional occupancy grid map; calculate the distance from the surface of the envelope sphere at each path point to the nearest voxel marked as occupied state, and take the minimum value of all distances as the safety distance; the collision risk cost is represented by a function in the form of negative exponential function.

[0038] The application further provides a multi-thread path planning device of a flexible cable picking mechanical arm, which is used for realizing the multi-thread path planning method of the flexible cable picking mechanical arm and comprises the following modules:

[0039] An environment perception and initialization module is configured to acquire scene point cloud information of a target planning area, perform semantic segmentation on the scene point cloud to classify an oil tea fruit area, an obstacle area and a background area, determine an initial pose and a target picking pose of an end of the mechanical arm based on the classification result, perform voxelization processing on the scene point cloud, establish a three-dimensional occupancy grid map and mark a grid area in a free state;

[0040] An initial path planning module is configured to perform a rapid random search tree algorithm in the set of grid areas marked in the free state based on the initial pose and the target picking pose of the end of the mechanical arm, and introduce a node screening mechanism based on dexterity estimation when a new node is sampled in the random search tree expansion to generate an initial geometric path.

[0041] A multi-thread optimization module is configured to trigger at least three optimization threads with different optimization objectives in parallel based on the initial geometric path, and each thread optimizes the initial geometric path according to its optimization objective to obtain at least three candidate paths.

[0042] A sliding window evaluation and decision module is configured to evaluate and calculate a comprehensive score of each path in a sliding window based on the generated candidate paths, the length of the sliding window is fixed, the candidate paths are comprehensively evaluated by using a multi-objective cost function in the window, and a to-be-executed path is screened out according to the comprehensive scores of all sliding windows in each candidate path, and the mechanical arm moves along the to-be-executed path.

[0043] In the above technical solution, the application has the following technical effects and advantages:

[0044] By introducing a multi-thread optimization mechanism and a multi-objective cost function, the application effectively solves the problems of low path planning efficiency and poor path quality in the prior art, accurately classifies the scene through semantic segmentation, enables the mechanical arm to avoid obstacles in a complex environment, ensures the safety and efficiency of path planning, and simultaneously optimizes different objectives in parallel based on the initial geometric path to generate multiple candidate paths and select the optimal path through a comprehensive scoring mechanism, thereby realizing multi-objective path optimization, improving the adaptability and picking efficiency of the mechanical arm in a complex environment, and finally realizing efficient flexible cable picking and solving the limitations of the existing path planning method in a complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 The figure is a schematic diagram of the overall method of the application.

[0046] Figure 2 Fig. 2 is a schematic diagram of the time variation of the rope tension values before optimization of the present application;

[0047] Figure 3 Fig. 3 is a schematic diagram of the time variation of the rope tension values after optimization of the present application;

[0048] Figure 4 Fig. 4 is a schematic diagram of the structure of the device of the present application. DETAILED DESCRIPTION

[0049] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with specific examples.

[0050] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present application should be understood as the common meanings understood by those with ordinary skills in the art to which the present application belongs. The terms "first", "second" and similar terms used in the present application do not represent any order, number or importance, but are only used to distinguish different components. The terms "including", "containing" and similar terms mean that the elements or objects before the terms cover the elements or objects listed after the terms and their equivalents, without excluding other elements or objects. The terms "connected" or "connected" and similar terms are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. The terms "up", "down", "left", "right" and the like only represent relative positional relationships, which can change accordingly when the absolute positions of the described objects change.

[0051] Embodiment:

[0052] Please refer to Figures 1 to 3 The present application provides a technical solution:

[0053] A multi-thread path planning method for a flexible cable type picking mechanical arm, the specific steps comprising:

[0054] Step 1: Obtain the scene point cloud information of the target planning area, perform semantic segmentation on the scene point cloud to classify the camellia fruit area, obstacle area and background area, determine the initial pose and target picking pose of the end of the mechanical arm based on the classification result, and perform voxelization processing on the scene point cloud, establish a three-dimensional occupancy grid map and mark the grid area in a free state.

[0055] In this embodiment, the three-dimensional occupancy grid map is constructed specifically as follows:

[0056] Scene point cloud information is acquired through 3D vision sensors mounted on the base or end effector of the robotic arm used for flexible harvesting. These 3D vision sensors can be depth cameras (such as Kinect or Intel RealSense series) or LiDAR (such as Velodyne or Livox series). Depth cameras are less expensive and suitable for close-range environments with minimal lighting changes; LiDAR offers high ranging accuracy and strong resistance to lighting interference, making it suitable for complex outdoor lighting conditions. Before the robotic arm begins its harvesting task, the control system actively controls the sensors mounted on the base or end effector to swing or rotate, scanning the canopy of the fruit trees from one or more preset angles. This active multi-view scanning strategy effectively reduces visual occlusion from a single viewpoint, obtaining a more complete and detailed original point cloud, providing reliable data for subsequent accurate semantic segmentation and obstacle modeling.

[0057] Due to sensor noise, ambient light interference, and multi-view stitching errors, raw point clouds typically contain noisy points, outliers, and redundant data, necessitating preprocessing to improve data quality. Specifically, this includes:

[0058] Sampling Filtering: To reduce the computational load of subsequent processing and preserve the basic shape characteristics of the point cloud, the original point cloud is first downsampled. For example, a voxel grid filter can be used. This filter divides the point cloud space into multiple tiny three-dimensional voxels (cubic lattices), and then approximates the points within each voxel using the centroid (or center point) of all points within that voxel. Specifically, a voxel size parameter is set (e.g., 0.005 meters, or 5 millimeters). During processing, all points in the original point cloud that fall within the same voxel are replaced with a single point (such as the average of their coordinates). This process significantly reduces the number of points while preserving the overall geometry.

[0059] Statistical Outlier Removal: Even after sampling and filtering, some isolated noise points (outliers) that are significantly deviated from the main point cloud cluster may still exist in the point cloud. Therefore, a statistical outlier removal algorithm is used. This algorithm iterates through each point in the point cloud, calculates its average distance to all Q nearest neighbors (assuming this distance follows a Gaussian distribution), and calculates the mean and standard deviation of the distances across the entire point cloud. A distance threshold is set, for example, mean ± n * standard deviation (n is a preset coefficient, usually 1, 2, or 3). If the average distance between a point and its neighbors exceeds this threshold, the point is considered an outlier and is removed. This step filters out discrete noise points floating in the air or attached to object surfaces, resulting in a denoised and more structurally clear scene point cloud.

[0060] A pre-trained 3D segmentation model is used to process scene point cloud information. Specifically, this involves: first, collecting a large amount of point cloud data of fruit trees containing camellia fruit, supporting branches, and background elements (such as leaves, sky, and ground), and manually or semi-automatically labeling each point cloud point with semantic tags (e.g., camellia fruit, supporting branch, background). Then, using these labeled datasets, a deep learning model is trained. The input to this model is a point cloud set consisting of N points, where each point contains its 3D spatial coordinate information.

[0061] The overall network structure typically follows an encoder-decoder architecture. The encoder is responsible for extracting multi-level, representative features from the input point cloud data; the decoder is responsible for upsampling the extracted abstract features and predicting their semantic categories point by point.

[0062] At the beginning of the encoder, the network first downsamples the input point cloud and aggregates local features through an ensemble abstraction layer. This layer uses a farthest-point sampling algorithm to select a smaller set of center points from the original point cloud. Then, for each center point, all neighboring points are found in the network's 3D space with a defined radius. Next, a small feedforward neural network independently boosts the features of each neighboring point, and then uses a symmetric pooling function, such as taking the maximum value, to aggregate the features of all points in that neighborhood into a single feature vector, which serves as the new feature of that center point. This process reduces the number of points and the computational cost, while also ensuring that each point's features include contextual information about its surrounding local geometry. By stacking multiple such ensemble abstraction layers, the network can construct hierarchical structures of point clouds at different scales and obtain increasingly abstract and global feature representations.

[0063] Corresponding to the encoder's downsampling process, the decoder needs to perform feature upsampling to restore the density of the original point cloud and output point-by-point predictions. For this, the decoder uses a feature propagation layer. When the decoder needs to propagate lower-number, higher-layer features back to higher-number, lower-layer features, it employs a distance-based inverse distance weighted interpolation method. Specifically, for each point in the previous decoding layer, the nearest points are found in the corresponding encoder layer. Then, based on the feature values ​​of these nearest neighbors and their distance weights to that point, a weighted sum is obtained to obtain the interpolated feature for that point. Next, this interpolated feature is concatenated with the features passed from skip connections in the corresponding encoder layer to form a richer feature vector. Finally, this concatenated feature is further processed by several shared multilayer perceptrons to fuse information from different levels. By repeating this process, the features are ultimately propagated back to the order of magnitude of the original input point cloud.

[0064] At the end of the decoder, the final layer of the network is a shared multilayer perceptron, which acts as a point-by-point classifier. At this point, each point possesses a feature descriptor that integrates local and global information. This final multilayer perceptron outputs a vector of length equal to the number of classes for each point, where the value of each dimension typically represents the confidence score of the point belonging to a particular class. In our specific application, there are three classes, corresponding to the target camellia fruit, supporting branches, and background. To obtain the final semantic label for each point, an activation function is typically applied to the class dimension of this output vector, such as taking the maximum index, thus assigning a specific class to each point. Ultimately, the network outputs a sequence of semantic labels corresponding one-to-one with the input point cloud, thus completing the task of segmenting the scene point cloud into different semantic point cloud clusters.

[0065] For each segmented target camellia fruit point cloud cluster, calculate the center point coordinates of its 3D bounding box (e.g., axial bounding box AABB). The center point coordinates of this bounding box... This can characterize the spatial location of the camellia fruit. However, during harvesting, the end effector (such as a gripper or suction cup) should typically not directly contact the fruit surface, but rather act on the fruit stem. Therefore, this center point is offset by a preset gripping distance in the opposite direction of gravity (i.e., the vertically upward direction, set as the positive Z-axis). The position coordinates of the target picking pose corresponding to the camellia fruit are obtained, i.e. The grabbing distance The specific value should take into account both the size of the end effector and the average length of the camellia fruit stalk. For example, if the distance from the center of the end effector clamp to the front end is 20 mm, and the stalk length is approximately 10-15 mm, then... It can be set to 30-35 mm to ensure that the end can reliably grip or contact the fruit stem without damaging the fruit.

[0066] To ensure the stability of the harvesting action and reduce damage to the fruit, the posture of the end effector when approaching the target needs to be reasonably set. First, based on the segmentation results, the connection relationship between the target camellia fruit point cloud cluster and the supporting branch point cloud cluster is analyzed. Through principal component analysis or local geometric simulation, the approximate axial direction vector of the target camellia fruit stalk is determined. A preferred posture setting logic is: set the gripping axis direction of the robotic arm end effector (i.e., the opening and closing direction of the gripper or the central axis direction of the suction cup) to be perpendicular to the axial direction of the target camellia fruit stalk. This perpendicular relationship makes the direction of force more conducive to the separation of the stalk when gripping or cutting the stalk, while avoiding twisting or squeezing the fruit.

[0067] In this embodiment, the initial pose is specifically defined as: at the start of this harvesting task, when all the drive ropes of the robotic arm are in the initial retracted or pre-tensioned state, the pose of the robotic arm end effector (i.e., the connection point of the end effector) in the base coordinate system. This pose is usually a known, fixed zeroing or preparation position, serving as the starting point for each path planning.

[0068] Based on all the segmented target camellia fruit point cloud clusters and supporting branch point cloud clusters, an axis-aligned 3D bounding box that can completely enclose all point cloud clusters is calculated. Considering the robotic arm end effector and possible swing margin, this axis-aligned 3D bounding box is uniformly extended outward in 3D space by a preset safety margin. The spatial region of the extended axis-aligned 3D bounding box is the robotic arm's workspace. The setting of this safety margin is crucial; its value should be no less than 1.5 times the maximum circumscribed sphere radius of the robotic arm end effector. For example, if the end effector is a spherical structure with a diameter of 40 mm, the maximum circumscribed sphere radius is 20 mm, and the safety margin should be at least 30 mm. This method of setting values ​​provides sufficient buffer space for path planning algorithms and possible control errors, ensuring that even with minor deviations in path tracking, the robotic arm end effector and body will not collide with the workspace boundary or obstacles near the boundary.

[0069] The workspace defined above is discretized into a uniform three-dimensional voxel network along the X, Y, and Z coordinate axes. Each voxel is a small cube, and its size (resolution) is a key parameter. Higher resolution (smaller voxels) results in higher map accuracy but also greater computational complexity; lower resolution may fail to accurately represent fine branches. A reasonable value is typically 1 / 5 to 1 / 10 of the feature size (such as diameter) of the robotic arm's end effector. For example, if the end effector diameter is 40 mm, the size can be set to 5 mm to 8 mm. This allows for the identification of major obstacle branches while maintaining computational efficiency. In this embodiment, the obstacle region mainly refers to supporting branches.

[0070] First, the point cloud clusters of supporting branches are projected onto a voxel mesh. All voxels containing at least one supporting branch point are initially marked as obstacles. However, the point cloud itself is sparse, and direct projection may not fully reflect the actual physical size of the branches. Therefore, a dilatational erosion algorithm (equivalent to voxel dilatation in 3D space) is used to dilate the obstacle region. Specifically, for all voxels already marked as obstacles, all their adjacent voxels (e.g., using 6-neighborhood, 18-neighborhood, or 26-neighborhood) are also marked as obstacles. The number of dilatation layers (or radius) should be related to the estimated radius of the branch and the voxel size. For example, if the branch radius is approximately 5 mm and the voxel size is 5 mm, at least one voxel layer of dilatation is needed to ensure safety. The dilated regions together constitute the obstacle region, and their voxel states are marked as occupied.

[0071] For the target camellia fruit dot cloud cluster, the voxel containing it and its directly adjacent voxels are marked as temporary free space. This area is considered passable during pathfinding, but because it contains the target object, the subsequent harvesting action will briefly pause in this area and perform a grabbing operation. All voxels within the workspace that do not belong to either the obstacle zone or the temporary free space are marked as free space. All voxel states within temporary free space and free space are marked as free state.

[0072] Step 2: Within the set of grid regions marked as free states, execute the fast random search tree algorithm based on the initial pose of the robotic arm end effector and the target picking pose. When sampling new nodes during the expansion of the random search tree, introduce a node selection mechanism based on dexterity estimation to generate the initial geometric path.

[0073] In this embodiment, the initial geometric path is generated based on an improved Fast Random Search Tree (FRS) algorithm. This algorithm progressively expands the search tree by randomly sampling points in free space and attempting to connect them to already generated search tree nodes until a collision-free path is generated connecting the initial pose node to the target picking pose node. However, for underconstrained or redundant drive systems such as cable-driven robotic arms, not all geometrically collision-free paths are mechanically feasible. If the end effector is in or near a singular configuration with extremely poor dexterity on some poses along the path, the drive cable may not be able to effectively transmit the required force or torque, leading to difficulties in tension distribution or even loss of control. Therefore, to ensure the planned initial path has basic mechanical feasibility, this method introduces a node selection mechanism based on dexterity estimation in the node expansion stage of the standard FRS algorithm. This mechanism actively removes newly sampled nodes located in or near mechanically singular regions, thereby guiding the search tree to grow in a spatial region with better mechanical performance.

[0074] The Fast Random Search Tree algorithm performs path search by iteratively generating random sampling points and adding new nodes to a random tree species. Its node selection mechanism based on agility estimation is as follows:

[0075] For each new node (corresponding to a candidate pose of the robotic arm's end effector) randomly sampled during the expansion process by the Fast Random Search Tree algorithm, the force transfer matrix is ​​calculated, consisting of the unit direction vectors of all driving cables when the end effector is in the pose corresponding to the new node. This matrix describes the linear mapping between the tension vector of the driving cables and the net force and net torque vectors acting on the end effector. Specifically, when the robotic arm has h driving cables, the unit direction vector of each driving cable from the end effector connection point to the corresponding pulley exit point (or guide point) is calculated when the end effector is in the pose of the new node. In addition, the torque vector (i.e., the cross product of the position vector and the direction vector) of each cable direction vector relative to the end effector's centroid also needs to be calculated. These h direction vectors and h torque vectors are arranged column-wise to form a 6-row h-column force transfer matrix (the dimension may be reduced in simplified models such as planar motion). Subsequently, the condition number of this force transfer matrix is ​​calculated. The condition number is the ratio of the maximum singular value to the minimum singular value of the matrix, and it is an indicator of the stability of matrix inversion or the sensitivity of the solution of a system of linear equations to input errors. For a force transfer matrix, a larger condition number means a smaller minimum singular value, indicating an ill-conditioned matrix. In this case, even a small change in end force or torque may cause drastic changes in rope tension, or even prevent static equilibrium from being achieved within a reasonable tension range. This pose is considered to have poor dexterity or be close to singular. Therefore, the reciprocal of the condition number of the force transfer matrix is ​​defined as the dexterity estimate of the new node. The dexterity estimate is a dimensionless number between 0 and 1. The closer the value is to 1, the better the force transfer performance of the node pose and the further away from singularity; the closer the value is to 0, the worse the force transfer performance and the closer it is to a singular configuration. The condition number is the ratio of the maximum singular value to the minimum singular value of the force transfer matrix.

[0076] For example, suppose a cable-driven robotic arm model with two-dimensional planar motion has four cables driving the end effector. At a new sampling node, the unit direction vectors (two-dimensional vectors) of the four cables are calculated as p1, p2, p3, and p4. The force transfer matrix R can be constructed as a four-column matrix, with each column consisting of the direction vector of the corresponding cable (the number of rows increases if torque balance is considered). Singular value decomposition is performed on matrix R to obtain its singular values. ;in This represents the largest singular value in the force transmission matrix. Let r represent the smallest non-zero singular value of the force transmission matrix, and r be the rank of matrix R. Represents intermediate singular values. Condition number. The dexterity estimate of this node is... .

[0077] To quantify the dexterity estimate into a decision to accept or reject a new sampled node, a preset safety threshold is introduced. This safety threshold is an empirical or theoretical value between 0 and 1, used to define the acceptable lower limit of dexterity. The specific method for determining the safety threshold is as follows: it can typically be obtained through dexterity analysis of the robotic arm's workspace. For example, in offline mode, a large number of end-effector poses are uniformly sampled within the workspace, the dexterity estimate for each pose is calculated, and its distribution is statistically analyzed. The safety threshold can be set as the lower quantile of this distribution (e.g., the 10th percentile), or it can be set based on a critical empirical value that can stably maintain tension distribution in actual control. For example, if the analysis finds that when the dexterity estimate is below 0.15, the rope tension is very likely to exceed the reasonable output range of the motor or result in negative tension (slack), then the safety threshold can be set to 0.15. The setting of this threshold needs to strike a balance between the mechanical feasibility of the path and the search efficiency of the algorithm: a threshold that is too high may lead to the rejection of a large number of nodes, making the search difficult or even causing failure; a threshold that is too low may allow nodes with poor mechanical performance to enter the path, burdening subsequent optimization.

[0078] For each new node, after calculating its dexterity estimate, the rejection probability of the new node is calculated using the following formula: Rejection Probability = 1 - (Dexterity Estimate / Safety Threshold). A result greater than 1 is set to 1 (indicating certain rejection), and a result less than 0 is set to 0 (indicating certain acceptance). This means that when a node's dexterity estimate is greater than or equal to the safety threshold, its rejection probability is 0 or negative (set to 0), and the node will be directly accepted. When a node's dexterity estimate is less than the safety threshold, the rejection probability will be between 0 and 1, and the lower the dexterity estimate, the higher the rejection probability.

[0079] When executing the Fast Random Search Tree algorithm, after generating a new node and calculating its rejection probability, a uniformly random number between 0 and 1 is generated. This random number is compared with the rejection probability calculated in the previous step: if the uniformly random number is less than its rejection probability, the new node is determined to be in a singular region, discarded, and resampling is performed; if the uniformly random number is not less than its rejection probability, the node is accepted.

[0080] For each accepted new node, following the standard procedure of a fast random search tree, its nearest neighbor is found in the current search tree, and the line segment connecting the nearest neighbor and the new node is checked to ensure it lies entirely within the free-state grid area (i.e., collision-free). If the collision detection passes, the new node is formally added to the random tree, and an edge is created pointing from the nearest neighbor to the new node. This sampling, evaluation, decision-making, and connection process, including dexterity filtering, is repeated, allowing the random tree to grow and expand primarily in the free-space region with high mechanical dexterity. This mechanism essentially adds a heuristic of mechanical feasibility to path search.

[0081] The process continues iteratively until the random tree successfully grows to the target picking pose node, or the tree growing backward from the target point successfully connects with the tree growing from the initial point. At this point, starting from the target point, the sequence of nodes traversed back along the parent-child node relationships to the initial pose node constitutes an initial geometric path from the initial pose to the target picking pose. This path is not only collision-free in geometric space, but also, due to the dexterity-based node selection, its force transmission characteristics at the traversed poses are probabilistically superior to paths obtained through pure random search, laying a solid foundation for subsequent refined optimization.

[0082] Step 3: Based on the initial geometric path, trigger at least three optimization threads with different optimization objectives in parallel. Each thread optimizes the initial geometric path according to its optimization objective, resulting in at least three candidate paths.

[0083] In this embodiment, the optimization threads are triggered by a first optimization thread, a second optimization thread, and a third optimization thread;

[0084] The first optimization thread traverses all nodes on the initial geometric path, calculates the minimum singular value of the rope traction direction matrix corresponding to each node, and uses the minimum singular value as the local dexterity index of that node.

[0085] The specific method for setting the rope traction direction matrix is ​​as follows: A specific geometric path contains several nodes, each connected to multiple ropes. The ropes apply tension or thrust, affecting the motion state of the nodes. For each node… Construct a rope traction direction matrix The rope traction direction matrix Specifically, it is expressed as follows:

[0086]

[0087] In the formula, For nodes The corresponding rope traction direction matrix, where For the first The rope at the node The traction direction vector at point i, where i is the index of the node and j is the index of the rope. The total number of ropes;

[0088] The minimum singular value of the rope traction direction matrix corresponding to each node is obtained through singular value decomposition. Singular value decomposition is an important tool in linear algebra that can be used to analyze the properties of matrices. By performing singular value decomposition on any rope traction direction matrix, a diagonal matrix containing all the singular values ​​of the rope traction direction matrix is ​​obtained. The minimum singular value is the minimum element of the diagonal matrix.

[0089] The specific logic of using the minimum singular value as the local dexterity index of a node is as follows: a smaller minimum singular value means that the movement of the node is restricted by a larger traction force in a certain direction, thus reducing the flexibility of the node. Conversely, a larger minimum singular value indicates that the node is more flexible in multiple directions and can move more freely. If a node can remain relatively stable under multiple tension directions, its minimum singular value will be larger. Conversely, a smaller minimum singular value may mean that it is prone to deformation or instability in certain situations.

[0090] Therefore, for nodes whose local dexterity index is lower than the first preset threshold, the first optimization thread calculates the gradient direction of the local dexterity index relative to the position and pose parameters of the robotic arm end effector; by moving the node along the gradient direction by a preset fixed step, a new node sequence is generated, thereby obtaining the first candidate path under the first optimization thread.

[0091] It should be noted that in path optimization problems, the local dexterity index is a key indicator for evaluating whether a robotic arm can perform tasks flexibly and stably in a certain state. When the local dexterity index is lower than the first preset threshold, it indicates that the robotic arm's flexibility and stability at that node are insufficient, which may lead to operation failure, inaccuracy, or danger. Therefore, calculating the gradient direction and adjusting the node position is to improve path quality and ensure the success and reliability of the operation. The gradient is the rate of change of the local dexterity index relative to the position and pose parameters of the robotic arm's end effector. By calculating the gradient direction, we can determine how to adjust the position and pose of the node in the parameter space to maximize the improvement of the dexterity index. Therefore, a new node sequence is generated by moving the node along the gradient direction by a preset fixed step size.

[0092] The first preset threshold is obtained through expert experience, specifically including: by analyzing past path data, the distribution of dexterity index can be obtained, and a reasonable threshold can be determined based on historical data as the first preset threshold, so that most effective operations are above this standard.

[0093] The preset fixed step size can be selected by conducting multiple experiments with different step sizes, from which the step size that can obtain the best dexterity improvement and path quality within a fixed time can be selected. At the beginning, a smaller step size can be selected to ensure the stability and controllability of each adjustment, and to avoid excessive changes that may lead to system instability or path infeasibility.

[0094] The second optimization thread discretizes the initial geometric path into a series of ordered path points at time intervals. For each intermediate path point that is neither the starting point nor the ending point, the optimization variable is adjusted to change its three-dimensional spatial coordinates. The optimization objective is to minimize the change in rope tension between the path point and its two adjacent path points. The constraints are that the static equilibrium equation of the robotic arm end effector is satisfied at all three points and all rope tension values ​​are within a preset positive tension range. A local optimization problem is constructed. By iteratively solving the local optimization problem of all intermediate path points, the optimized new spatial coordinates of each intermediate path point are obtained sequentially. The starting point, the optimized new spatial coordinates of all intermediate path points, and the ending point are connected to form a new path, which serves as the second candidate path.

[0095] The goal of the second optimization thread is to minimize the variation in rope tension by adjusting the three-dimensional spatial coordinates of the midpoint of the path, while simultaneously satisfying static equilibrium and tension constraints.

[0096] Set a time interval, and based on the set time interval, discretize the initial geometric path according to the time interval to generate a series of ordered path points, including the starting point, intermediate path points and the ending point, and record the three-dimensional coordinates of each path point.

[0097] The optimization function is set with the minimum change in rope tension between the path point and its two adjacent path points as the optimization objective. The optimization function is specifically expressed as follows:

[0098]

[0099] In the formula, intermediate path point The range of rope tension variation at that point intermediate path point The tension of the rope at that point, This represents the q-th intermediate path point. intermediate path point That is, the rope tension at the (q-1)th intermediate path point. intermediate path point , which is the rope tension at the (q+1)th intermediate path point, where q is the index of the intermediate path point;

[0100] The optimization objective is to minimize the change in rope tension at the path point and its two adjacent path points, i.e., using the middle path point as the target. Rope tension variation range The optimization objective is to minimize the value of . At the same time, the constraints are that all three points satisfy the static equilibrium equation of the end effector of the robotic arm and that all rope tension values ​​are within the preset positive tension range. For each intermediate path point, the coordinates of the intermediate path point are used as the optimization variable, and iterative optimization algorithms, such as gradient descent, Newton's method, and genetic algorithm, are used to solve the local optimization problem. The gradient descent, Newton's method, and genetic algorithm are all conventional optimization algorithms and will not be described in detail here.

[0101] After iterative optimization of all intermediate path points, the optimized new spatial coordinates are obtained. The starting point, the optimized new spatial coordinates of all intermediate path points, and the ending point are connected together to form a new path, which serves as the second candidate path.

[0102] To illustrate the effectiveness of the second optimization thread (tension feasibility and smoothness optimization), the following specific embodiment is provided. In this embodiment, the flexible cable-type picking robotic arm uses four independently driven synthetic fiber ropes, which converge at their ends to an end effector with a universal joint and a passive rotary joint. To verify the impact of path optimization on the internal mechanical state of the drive system, a picking approach path lasting 3 seconds, including a combination of steering and lifting motions, was planned.

[0103] First, an initial geometric path from starting point A to transition point B is generated based on an improved fast random search tree algorithm. Then, according to the inverse kinematics and statics model of the robotic arm, for each planned pose point (31 points in total) discrete at 0.1-second intervals along this path, we solve its static equilibrium equations. Where T represents the tension value, This is the transpose of the 4×6 LiJacobi matrix calculated based on the end pose (six degrees of freedom parameters) at that point. For the end load (including the actuator's own weight, the simulated fruit weight, and the estimated inertial force, forming a 6-dimensional force or torque vector), a set of four-dimensional rope tension vectors is obtained by solving. This process simulates the instantaneous rope tension theoretically required to track the path, and the data is recorded as the dataset before optimization, as shown in the table below:

[0104] Table 1: Schematic diagram of tension values ​​of each rope before optimization

[0105]

[0106] Subsequently, the second optimization thread is initiated. This thread takes the discrete point sequence of the initial path as input, aims to minimize the tension variation between adjacent points, and is constrained by the tension at each point satisfying 10N≤T≤50N (where T corresponds to the previous thread). The spatial coordinates of intermediate path points are iteratively adjusted. After optimization, the static solution process is repeated on the generated new path (second candidate path) to obtain a new set of four-dimensional rope tension vector sequences, which are recorded as the optimized dataset, as shown in the table below:

[0107] Table 2: Schematic diagram of optimized rope tension values

[0108]

[0109] Plotting the tension data sequences before and after optimization as a curve over time yields the desired result. Figure 2 and Figure 3 The comparison chart shown clearly illustrates that before optimization, the rope tension (especially T1 and T4) exhibited sharp peaks in the middle of the movement, with T1 exceeding the safe upper limit of 50N at t=1.5 seconds, and T2 approaching the lower limit of 10N. This indicates that the initial path carries the risk of traversing unusual configurations or inefficient force transmission regions, potentially leading to rope breakage or uncontrolled slack during actual execution. In contrast, the optimized tension curves show that the tension of all ropes is smoothly constrained within a robust range of 20N to 35N, with gradual and synchronized changes, completely eliminating the risk of exceeding limits. In this embodiment, T1 corresponds to rope number 1, T2 to rope number 2, T3 to rope number 3, and T4 to rope number 4.

[0110] In the tension curves before optimization, the tensions of ropes 1 and 4 showed a significant trend of first increasing and then decreasing, while rope 2 showed a mirror change of first decreasing and then increasing. This directly reflects that the initial geometric path traversed a region extremely unfavorable to the geometry of the rope system. When the end effector of the robotic arm moved to the middle of this path, its pose caused the angle between ropes 1 and 4 and the end load direction to decrease, shortening the lever arm. To balance the same load torque, these two ropes had to provide greater tension, causing the tension to rise sharply to a dangerous level. At the same time, to maintain static equilibrium, as a necessary result of mechanical coupling, the tensions of ropes 2 and 3 were correspondingly squeezed and decreased. In particular, the tension of rope 2 once approached the lower limit, posing a risk of slack. This dramatic fluctuation of one increasing while the other decreases reveals that the path point was in a near-singular configuration, with low force transmission efficiency, a severe imbalance in load distribution among the ropes, and the system on the verge of instability.

[0111] After adjustments by the second optimization thread, the optimized path fundamentally improved the geometry of force transmission. The tension curves of ropes 1, 2, and 3 became smooth and nearly parallel monotonically increasing, reflecting an ideal optimization result: the end effector of the robotic arm moves along a force-friendly path. Due to the path adjustment, the component force changes of the end effector load in each rope direction became more uniform and predictable. The tension of the three ropes increased steadily with similar amplitudes, indicating that the load was evenly and coordinately distributed across the three ropes, which worked together to bear the increased load during movement (e.g., counteracting changes in the gravitational component). The competitive relationship of forces within the system was transformed into a cooperative relationship, thus achieving smoothness and predictability of tension.

[0112] The increased tension reduction in rope 4 after optimization reflects the algorithm's proactive load redistribution. In the initial path, to maintain the unfavorable configuration, rope 4 had to share an excessive load with rope 1. The optimization algorithm, while maintaining overall balance, reallocated the tasks of each rope. It cleverly adjusted the end path, allowing ropes 1, 2, and 3 to more effectively bear the main load, thus allowing rope 4 to unload more. This intentional tension reduction is not a performance degradation, but rather a release of rope 4 from a near-overloaded state, safely returning its tension to the central area, while ensuring the tension of other ropes remains within a comfortable range. Overall, this achieves a fundamental shift from a dangerously polarized tension state to a centralized, balanced, and robust state, significantly improving the reliability and safety of the entire execution process.

[0113] This embodiment, through specific numerical calculations and graphical comparisons, intuitively demonstrates that the tension feasibility and smoothness optimization thread can effectively transform theoretically risky paths into feasible paths with stable and reliable internal force states in the driving system, thereby ensuring the motion stability and safety of the flexible cable robotic arm in actual complex environments.

[0114] The third optimization thread sets up a dynamic evaluation window of fixed length. Starting from the initial geometric path and corresponding to the starting position of the current plan, it extracts K consecutive path points forward as the path segment to be evaluated. For each path point within the path segment to be evaluated, its tension feasibility is calculated. Specifically, under the premise of satisfying the static balance of the robotic arm end effector, a maximum positive scalar factor is found such that when the basic tension value of each rope is multiplied by this positive scalar factor, it is still within the preset positive tension range. This maximum positive scalar factor is the tension feasibility of the predicted path point. If a path point is detected to be evaluated... If the tension feasibility of any path point within the evaluated path segment is lower than the second preset threshold, then the path point is marked as a potential singularity. If at least one potential singularity exists, the third optimization thread uses an unmarked path point before the first potential singularity as the starting point for local replanning, and uses the end point of the path segment to be evaluated as the endpoint to replan a local obstacle avoidance path from the starting point to the endpoint. The local obstacle avoidance path replaces the original path segment from the starting point to the endpoint in the initial geometric path, thereby forming a third candidate path. K is the number of path points on the extracted path segment to be evaluated.

[0115] The third optimization thread evaluates the feasibility of path segments by setting a fixed-length dynamic evaluation window, ensuring the safety and effectiveness of the path.

[0116] The length of the fixed-length dynamic evaluation window can be set according to the movement speed and control cycle of the robotic arm to ensure that the evaluation window covers the range of movement of the robotic arm within a fixed time.

[0117] The specific method for obtaining the basic tension value of each rope is as follows: establish a static model of the robotic arm, including each joint of the robotic arm, connecting ropes, applied external forces, etc., use the static equilibrium equation to solve the basic tension of each rope, or adopt an experimental method, by installing tension sensors at the connection points of the ropes to measure the tension applied to the ropes in real time, and record the sensor values ​​under different working conditions, including different loads and different motion states, to obtain the actual data of the basic tension.

[0118] The specific method for setting the second preset threshold is as follows: collect tension measurement data under similar operating conditions in the past, combine expert experience, and statistically analyze historical data to determine a reasonable feasibility threshold. Specifically, calculate the average value of tension feasibility in historical data and use it as the second preset threshold.

[0119] Step 4: Based on the generated candidate paths, evaluate and calculate the comprehensive score of each path within a sliding window. The length of the sliding window is fixed. A multi-objective cost function is used to comprehensively evaluate the candidate paths within the window. The path to be executed is selected based on the comprehensive score of all sliding windows within each candidate path, and the robotic arm moves along the path to be executed.

[0120] In this embodiment, the calculation logic of the multi-objective cost function is specifically as follows:

[0121] From the first candidate path, the second candidate path, and the third candidate path, extract three candidate path segments that correspond spatially to the preset sliding window and have the same length. These segments are denoted as the first candidate path segment, the second candidate path segment, and the third candidate path segment, respectively. Each candidate path segment consists of a series of ordered discrete spatial locations, which are collectively referred to as path points in this step.

[0122] Define a fixed-length sliding window, which physically represents the range of paths the robotic arm will traverse in a short future stroke. The window length L is set in units of the number of path points, for example, L is 5 to 10 path points. The choice of this length needs to balance the real-time performance and forward-looking nature of the evaluation: if it is too short, the evaluation field of view is limited and it cannot effectively warn of risks ahead; if it is too long, the computational load increases, and the reference value of path information that is too far away is reduced for the current decision. During the evaluation, the sliding window starts from the beginning of each candidate path (i.e., the path point corresponding to the current position of the robotic arm's end point) and sequentially intercepts L consecutive path points to form a candidate path segment to be evaluated. After the evaluation of the current window is completed, the window slides backward by one step (usually set to 1 path point) to evaluate the next overlapping path segment until the end of the window reaches the end of the candidate path. This sliding mechanism ensures that every local segment on the entire path can be included in the evaluation system.

[0123] Calculate the multi-objective cost function for each candidate path segment, including path length cost, dexterity safety cost, tension smoothing cost, and collision risk cost;

[0124] The path length cost is the total length of the spatial trajectory of the candidate path segments;

[0125] This cost directly reflects the spatial efficiency of the path, encouraging the selection of shorter routes to save time and energy. It calculates the sum of Euclidean distances between all adjacent path points on a candidate path segment, assuming the path segment contains M ordered path points. ,in Given its three-dimensional coordinates, the formula for calculating the path length cost is: ,in, It is the Euclidean norm;

[0126] The dexterity safety cost is calculated by taking the minimum value of the local dexterity index corresponding to all path points on the candidate path and taking its reciprocal as the dexterity safety cost.

[0127] This cost is inherited from the concerns of the first optimization thread in steps 2 and 3. It is used to quantify the dynamic performance of the robotic arm on the path segment, avoid passing through singular regions with low dexterity, and calculate the local dexterity index corresponding to all path points on the path segment. The minimum value of (i.e., the reciprocal of the condition number of the force transfer matrix, or the minimum singular value calculated in the first thread of step 3) is taken, and its reciprocal is amplified so that the point with lower dexterity contributes more to the cost. The calculation formula is: When there are points with extremely low dexterity in the path segment (i.e., When the value approaches 0, this cost increases dramatically, thus effectively eliminating paths containing potentially exotic configurations.

[0128] The cost of tension smoothing is the sum of the variances of the tension change curves of each driving rope along the candidate path when moving along the path.

[0129] This cost corresponds to the objective of the second optimization thread in step 3, which aims to evaluate the smoothness of the change in drive rope tension while tracking this path segment. Excessive tension fluctuations can affect control accuracy and increase mechanical wear. First, based on the static model of the robotic arm, a set of basic rope tensions required to maintain the static balance of the end-effector pose at each path point on the path segment is calculated. Then calculate the sum of the variances of the tension change sequences of all driving ropes. Assuming there are N driving ropes in total, the calculation formula is: Where u is the index of the path point and v is the index of the rope. This represents the tension value of the v-th rope at the u-th path point. This represents the variance of the sequence. The smaller this cost, the more gently the rope tension changes along that path segment.

[0130] The collision risk cost is calculated by performing collision detection between the end effector envelope sphere corresponding to each path point on the candidate path segment and the voxel marked as occupied in the 3D occupied grid map; the distance from the surface of the envelope sphere at each path point to the nearest voxel marked as occupied is calculated, and the minimum value among all distances is taken as the safe distance; the collision risk cost is represented by a function that decays in the form of a negative exponential function.

[0131] This cost is a direct reflection of safety; it quantifies the proximity of a path segment to obstacles in the environment, for each waypoint on the path segment. The maximum physical envelope of the end effector (simplified to a sphere with radius ) For the detection model, calculate the distance from its surface to the nearest voxel marked as occupied in the 3D occupied raster map. The minimum distance among all path points is taken as the safe distance for that path segment. The collision risk cost is mapped to this safety distance using a negative exponential function: ,in, As a normal attenuation coefficient, this functional form means that when the safe distance... When the value is large (the path is far from the obstacle), the cost approaches 0; when... As the path decreases (closer to the obstacle), the cost begins to increase slowly; while when... When the value is very small (on the verge of collision), the cost will rise sharply to a very large value, and the coefficient will... This is used to adjust the sensitivity of the cost to changes in distance, for example, it can be set to... This causes the cost to increase rapidly when the safety distance is less than the radius of the end effector.

[0132] Based on the preset weighting coefficients, the path length cost, dexterity safety cost, tension smoothing cost, and collision risk cost are weighted and summed to obtain the comprehensive score of each candidate path segment.

[0133] For each candidate path segment captured by the sliding window, its multi-objective comprehensive score By weighted summing of the above four cost sub-functions, we obtain: ;in, These are preset weighting coefficients, all of which are positive numbers. These coefficients determine the relative importance of each performance indicator in the final decision and need to be tuned according to specific task requirements. For example, in areas with dense obstacles, the collision risk weight can be appropriately increased. In stages requiring high-precision operation, the tension smoothing weight can be increased. .

[0134] The preset sliding window starts from the initial pose. After calculating the comprehensive score of the current sliding window, the end point of the current sliding window is used as the new starting point. The window slides towards the end points of each candidate path to form a new candidate path segment, and the comprehensive score of the new candidate path segment is calculated. This process is repeated until the end point of the sliding window coincides with the end points of each candidate path.

[0135] Calculate the average of the comprehensive scores of all candidate path segments in each candidate path, and use the average as the comprehensive score of the corresponding candidate path;

[0136] Using the sliding window mechanism, all windows on the candidate path are traversed to calculate a series of local comprehensive scores. (Total D windows), global comprehensive score of the candidate path Defined as the arithmetic mean of all these local scores: Where b is the index of the window; the method of calculating the average value ensures that the overall performance of the entire path is considered in a balanced way, avoiding the excessive influence of a certain extremely good or extremely poor path segment.

[0137] Select the candidate path with the lowest overall score as the path segment to be executed;

[0138] Repeat the sliding window evaluation and global score calculation process for the first, second, and third candidate paths respectively to obtain three corresponding global comprehensive scores. Based on the principle of cost minimization, the candidate path with the lowest global comprehensive score is selected as the final path to be executed. This path is considered to have achieved the best balance between path length, operational dexterity, control smoothness, and obstacle avoidance safety.

[0139] Finally, the planning system outputs this path to be executed to the motion controller of the flexible cable harvesting robotic arm. The controller converts the sequence of path points into length or tension commands for each drive rope, driving the end effector of the robotic arm to move along the optimized, safe, smooth, and efficient trajectory until the harvesting of the target camellia fruit is completed.

[0140] Please see Figure 4 The present invention also provides a multi-threaded path planning device for a flexible cable harvesting robotic arm. This device is used to implement the aforementioned multi-threaded path planning method for a flexible cable harvesting robotic arm, comprising:

[0141] The environment perception and initialization module is used to acquire scene point cloud information of the target planning area, perform semantic segmentation on the scene point cloud to classify the camellia fruit area, obstacle area and background area, determine the initial pose of the robotic arm end and the target picking pose based on the classification results, and perform voxelization processing on the scene point cloud to establish a three-dimensional occupied grid map and mark the grid area in the free state.

[0142] The initial path planning module is used to execute a fast random search tree algorithm based on the initial pose of the robotic arm end effector and the target picking pose within a set of grid regions marked as free states. When sampling new nodes during the expansion of the random search tree, a node selection mechanism based on dexterity estimation is introduced to generate the initial geometric path.

[0143] The multi-threaded optimization module is used to trigger at least three optimization threads with different optimization objectives in parallel based on the initial geometric path. Each thread optimizes the initial geometric path according to its optimization objective to obtain at least three candidate paths.

[0144] The sliding window evaluation and decision module is used to evaluate and calculate the comprehensive score of each path within the generated candidate path in the sliding window. The length of the sliding window is fixed, and a multi-objective cost function is used to comprehensively evaluate the candidate paths within the window. The path to be executed is selected based on the comprehensive score of all sliding windows within each candidate path, and the robotic arm moves along the path to be executed.

[0145] The above formulas are all dimensionless calculations. The formulas are derived from software simulations based on a large amount of collected data to obtain the most recent real-world results. The preset parameters in the formulas are set by those skilled in the art according to the actual situation.

[0146] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented in software, the above embodiments can be implemented, in whole or in part, as a computer program product. Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution.

[0147] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0148] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A multi-threaded path planning method for a flexible cable-type harvesting robotic arm, characterized in that, include: Step 1: Obtain scene point cloud information of the target planning area, perform semantic segmentation on the scene point cloud to classify the camellia fruit area, obstacle area and background area, determine the initial pose of the robotic arm end and the target picking pose based on the classification results, and perform voxelization on the scene point cloud to establish a 3D occupied grid map and mark the grid area in the free state. Step 2: Within the set of grid regions marked as free states, execute the fast random search tree algorithm based on the initial pose of the robotic arm end effector and the target picking pose. When sampling new nodes during the expansion of the random search tree, introduce a node selection mechanism based on dexterity estimation to generate the initial geometric path. Step 3: Based on the initial geometric path, trigger at least three optimization threads with different optimization objectives in parallel. Each thread optimizes the initial geometric path according to its optimization objective, resulting in at least three candidate paths. Step 4: Based on the generated candidate paths, evaluate and calculate the comprehensive score of each path within a sliding window. The length of the sliding window is fixed. A multi-objective cost function is used to comprehensively evaluate the candidate paths within the window. The path to be executed is selected based on the comprehensive score of all sliding windows within each candidate path. The robotic arm moves along the path to be executed. The optimization threads that are triggered include a first optimization thread, a second optimization thread, and a third optimization thread; The first optimization thread traverses all nodes on the initial geometric path, calculates the minimum singular value of the rope traction direction matrix corresponding to each node, and uses the minimum singular value as the local dexterity index of that node. For nodes whose local dexterity index is lower than the first preset threshold, the first optimization thread calculates the gradient direction of the local dexterity index relative to the position and pose parameters of the robotic arm end effector; by moving the node along the gradient direction by a preset fixed step, a new node sequence is generated, thereby obtaining the first candidate path under the first optimization thread. The second optimization thread discretizes the initial geometric path into a series of ordered path points at time intervals. For each intermediate path point that is neither the starting point nor the ending point, the optimization variable is adjusted to its three-dimensional spatial coordinates. The optimization objective is to minimize the change in rope tension at the path point and its two adjacent points. The constraints are that the static equilibrium equation of the robotic arm end effector is satisfied at all three points and all rope tension values ​​are within a preset positive tension range. A local optimization problem is constructed. By iteratively solving the local optimization problem of all intermediate path points, the optimized new spatial coordinates of each intermediate path point are obtained. The starting point, the optimized new spatial coordinates of all intermediate path points, and the ending point are connected to form a new path, which serves as the second candidate path. The third optimization thread sets up a dynamic evaluation window of fixed length. Starting from the initial geometric path and corresponding to the starting position of the current plan, it intercepts K consecutive path points in the future as the path segment to be evaluated. For each path point in the path segment to be evaluated, its tension feasibility is calculated. Specifically, under the premise of satisfying the static balance of the robotic arm end effector, a maximum positive scalar factor is found such that when the basic tension value of each rope is multiplied by the positive scalar factor, it is still within the preset positive tension range. The maximum positive scalar factor is the tension feasibility of the predicted path point. If it is detected that the tension feasibility of any path point in the path segment to be evaluated is lower than the second preset threshold, the path point is marked as a potential singularity. If there is at least one potential singularity, the third optimization thread takes an unmarked path point before the first potential singularity as the starting point of local replanning, and takes the end point of the path segment to be evaluated as the end point to replan a local obstacle avoidance path from the starting point to the end point. The original road segment from the starting point to the ending point in the initial geometric path is replaced with a local obstacle avoidance path to form a third candidate path; K is the number of path points on the intercepted path segment to be evaluated; The calculation logic of the multi-objective cost function is as follows: From the first candidate path, the second candidate path, and the third candidate path, extract three candidate path segments that correspond spatially to the preset sliding window and have the same length. These segments are denoted as the first candidate path segment, the second candidate path segment, and the third candidate path segment, respectively. Each candidate path segment consists of a series of ordered discrete spatial locations, which are collectively referred to as path points in this step. Calculate the multi-objective cost function for each candidate path segment, including path length cost, dexterity safety cost, tension smoothing cost, and collision risk cost; Based on the preset weighting coefficients, the path length cost, dexterity safety cost, tension smoothing cost, and collision risk cost are weighted and summed to obtain the comprehensive score of each candidate path segment. The preset sliding window starts from the initial pose. After calculating the comprehensive score of the current sliding window, the end point of the current sliding window is used as the new starting point. The window slides towards the end points of each candidate path to form a new candidate path segment, and the comprehensive score of the new candidate path segment is calculated. This process is repeated until the end point of the sliding window coincides with the end points of each candidate path. Calculate the average of the comprehensive scores of all candidate path segments in each candidate path, and use the average as the comprehensive score of the corresponding candidate path; The candidate path with the lowest overall score is selected as the path segment to be executed.

2. The multi-threaded path planning method for a flexible cable-type harvesting robotic arm according to claim 1, characterized in that, A pre-trained 3D segmentation model is used to process the scene point cloud information, specifically including: This 3D segmentation model takes scene point cloud information as input and uses the pre-labeled semantic labels of each point cloud point as labels, thereby segmenting the scene point cloud information into three types of point cloud clusters: target camellia fruit, obstacles, and background. For each target camellia fruit point cloud cluster, calculate the coordinates of the center point of its 3D bounding box, and offset the center point by a preset grasping distance in the opposite direction of gravity to obtain the position coordinates of the target picking pose corresponding to the camellia fruit.

3. The multi-threaded path planning method for a flexible cable-type harvesting robotic arm according to claim 2, characterized in that, The logic for building a 3D occupied grid map: Based on all target camellia fruit point cloud clusters and obstacle point cloud clusters, an axis-aligned 3D bounding box is calculated to completely enclose them. This axis-aligned 3D bounding box is then uniformly expanded outward in 3D space by a preset safety margin. The spatial region of the expanded axis-aligned 3D bounding box is the workspace of the robotic arm. The workspace is discretized into a uniform 3D voxel network. Based on the obstacle point cloud clusters, a dilatational erosion algorithm is used to generate a 3D occupancy grid map of the obstacles. In this 3D occupancy grid map, the voxels containing the obstacle point cloud clusters and their adjacent voxels are marked as obstacle regions and recorded as occupied states. For the voxels containing the target camellia fruit point cloud and their adjacent voxels, they are marked as permeable temporary free space, and the remaining voxels are marked as free space. Temporary free space and free space are marked as free states. The safety margin is set to be no less than 1.5 times the maximum circumscribed sphere radius of the robotic arm's end effector.

4. The multi-threaded path planning method for a flexible cable-type harvesting robotic arm according to claim 1, characterized in that, The Fast Random Search Tree algorithm performs path search by iteratively generating random sampling points and adding new nodes to a random tree species. Its node selection mechanism based on agility estimation is as follows: For each new node randomly sampled during the expansion process by the Fast Random Search Tree algorithm, calculate the force transmission matrix composed of the unit direction vectors of all driving ropes when the end of the robotic arm is in the pose corresponding to the new node; Calculate the condition number of the force transfer matrix and define its reciprocal as the dexterity estimate of the new node; the condition number is the ratio of the maximum singular value to the minimum singular value of the force transfer matrix. The dexterity estimate of the new node is compared with a preset security threshold, and the rejection probability of the new node is calculated. The rejection probability is 1 - (dexterity estimate / security threshold). When the calculation result is greater than 1, it is taken as 1, and when it is less than 0, it is taken as 0. When executing the Fast Random Search Tree algorithm, after generating a new node and calculating its rejection probability, a uniformly random number between 0 and 1 is generated. If the uniformly random number is less than its rejection probability, the new node is determined to be in a singular region, discarded, and resampled. If the uniformly random number is not less than its rejection probability, the node is accepted. For each new node that is accepted, it is added to the random tree according to the standard procedure of the fast random search tree and connected to the existing tree nodes. The above process is repeated until the random tree successfully connects the initial pose and the target picking pose. At this point, the node sequence generated by backtracking is the initial geometric path.

5. The multi-threaded path planning method for a flexible cable-type harvesting robotic arm according to claim 1, characterized in that, The path length cost is the total length of the spatial trajectory of the candidate path segments; The dexterity safety cost is calculated by taking the minimum value of the local dexterity index corresponding to all path points on the candidate path and taking its reciprocal as the dexterity safety cost. The cost of tension smoothing is the sum of the variances of the tension change curves of each driving rope along the candidate path when moving along the path. The collision risk cost is to calculate the collision detection between the end effector envelope sphere corresponding to each path point on the candidate path segment and the voxels marked as occupied in the 3D occupied grid map; Calculate the distance from the surface of the envelope sphere at each path point to the nearest voxel marked as occupied, and take the minimum value among all distances as the safe distance; The cost of collision risk is represented as a function that decays exponentially.

6. A multi-threaded path planning device for a flexible cable-type harvesting robotic arm, characterized in that, The multi-threaded path planning device for a flexible cable-type harvesting robotic arm is used to implement the multi-threaded path planning method for a flexible cable-type harvesting robotic arm as described in any one of claims 1-5, including: The environment perception and initialization module is used to acquire scene point cloud information of the target planning area, perform semantic segmentation on the scene point cloud to classify the camellia fruit area, obstacle area and background area, determine the initial pose of the robotic arm end and the target picking pose based on the classification results, and perform voxelization processing on the scene point cloud to establish a three-dimensional occupied grid map and mark the grid area in the free state. The initial path planning module is used to execute a fast random search tree algorithm based on the initial pose of the robotic arm end effector and the target picking pose within a set of grid regions marked as free states. When sampling new nodes during the expansion of the random search tree, a node selection mechanism based on dexterity estimation is introduced to generate the initial geometric path. The multi-threaded optimization module is used to trigger at least three optimization threads with different optimization objectives in parallel based on the initial geometric path. Each thread optimizes the initial geometric path according to its optimization objective to obtain at least three candidate paths. The sliding window evaluation and decision module is used to evaluate and calculate the comprehensive score of each path within the generated candidate path in the sliding window. The length of the sliding window is fixed, and a multi-objective cost function is used to comprehensively evaluate the candidate paths within the window. The path to be executed is selected based on the comprehensive score of all sliding windows within each candidate path, and the robotic arm moves along the path to be executed.

Images