Intelligent tea picking robot picking path planning method based on bimodal fusion attention and hierarchical adaptive optimization

Abstract

The invention relates to an intelligent tea picking robot picking path planning method based on bimodal fusion attention and hierarchical adaptive optimization, and belongs to the technical field of intelligent path planning. The method comprises the steps that RGB images and depth information are collected in real time, bimodal feature fusion is carried out after features are extracted, then the environment, branches and leaves are recognized, and a structured local environment model is generated; constructing a comprehensive picking effect maximization function, and searching an optimal global path meeting the function and constraint conditions by applying a hierarchical heterogeneous adaptive optimization algorithm; in a transition stage when the robot moves between the two picking points, monitoring the environment state around the advancing path in real time to evaluate a potential collision risk level; and if necessary, dynamically generating a local correction path through a local path planning mechanism, and fusing or reconnecting the local correction path to the original global path. According to the method, the problems of low path planning precision, poor environmental adaptability and low picking efficiency in a complex and unstructured tea garden environment are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent path planning, and relates to path planning of an intelligent tea-picking robot, and specifically to a picking path planning method for an intelligent tea-picking robot based on dual-modal fusion attention and hierarchical adaptive optimization. Background Art

[0002] As global consumers continue to increase their interest in healthy lifestyles and natural products, tea, a popular natural beverage, is experiencing steady market growth. In recent years, my country's tea industry has been actively transforming and upgrading towards ecological cultivation, product standardization, branded operations, internationalized markets, and deep industrial integration. Despite the overall positive development of the industry, its production methods and technological capabilities, particularly in the tea harvesting process, remain significantly behind the overall requirements of agricultural modernization. The harvesting of premium teas relies heavily on traditional manual labor, leading to a particularly low level of mechanization and automation. Statistics show that tea harvesting accounts for over 60% of the total labor employed in tea garden management during the production cycle, making it the most labor-intensive, labor-intensive, and operationally demanding step in the entire tea production process. Therefore, innovating existing harvesting methods and vigorously promoting mechanized, automated, and even intelligent harvesting processes has become an inevitable trend and urgent need to ensure the sustainable development of the premium tea industry and alleviate labor shortages.

[0003] However, the complexity of the actual tea garden environment poses a severe challenge to existing intelligent picking technologies: the color and texture of tea buds are highly similar to those of the surrounding leaves and branches, and there is widespread severe mutual occlusion and overlap; variable lighting conditions (such as shadows and strong light reflections) further increase the difficulty of perception. These factors have led to the existing picking methods based on a single information source or a simple fusion strategy being insufficient in terms of precise positioning of picking points, detailed perception of obstacles, and robustness in complex environments. This is especially true in the critical path planning link of planning how the robot's end effector (gripper) can safely and efficiently reach the target and perform precise picking actions. Traditional path planning algorithms often find it difficult to fully adapt to the dense, unstructured three-dimensional environment of tea trees, which can easily lead to unreasonable planned paths, high collision risks, or low operating efficiency, thereby affecting the overall picking success rate and the integrity of the picked tea leaves. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a picking path planning method for an intelligent tea picking robot based on dual-modal fusion attention and hierarchical adaptive optimization of RGB features and depth features, aiming to solve the problems of low path planning accuracy, poor environmental adaptability, and low picking efficiency of existing intelligent picking technologies in complex and unstructured tea garden environments, so as to enable the robot's end gripper to reach the target picking point safely, efficiently and accurately under multiple constraints and complete the picking operation.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A picking path planning method for an intelligent tea-picking robot based on bimodal fusion attention and hierarchical adaptive optimization, the method comprising:

[0007] The robot's RGB-D camera collects RGB images and depth information in real time, pre-processes them, and extracts features. A dual-modal fusion attention mechanism is then used to fuse these features. Based on the fused features, the robot performs semantic segmentation of the environment and identifies branches and leaves, generating a structured local environment model.

[0008] Based on a structured local environment model, a function is constructed to maximize the overall picking effect, while also generating hard constraints for the picking effect. A hierarchical heterogeneous adaptive optimization algorithm is applied to find the optimal global path that satisfies this function and the hard constraints for the picking effect. This optimal global path is converted into a sequence of instructions for the robot to understand and execute, and the instruction sequence is output, allowing the robot to perform the picking task according to the optimal global path.

[0009] During the transition phase when the robot moves between two picking points, the local path planning mechanism based on dynamic environment perception is activated, and the sensors configured on the robot are used to monitor the environmental status around the travel path in real time. The potential collision risk level is evaluated in real time based on the detected dynamic changes in the environment and the current state of the robot. When the assessed risk level exceeds the preset safety threshold, a local correction path is dynamically generated through the local path planning mechanism, and the local correction path is integrated into or reconnected to the original global path, and then switched for execution.

[0010] Furthermore, the collected RGB image I r and depth information I d Preprocessing is performed. First, the image noise is removed by filtering, then invalid values ​​are eliminated or interpolated for the depth information. Finally, data alignment is performed based on the camera's intrinsic and extrinsic parameters to ensure that the RGB pixels accurately match the corresponding 3D space points. The aligned color image I1, depth image I2, and point cloud data P are obtained:

[0011]

[0012] Where S represents the preprocessing function, Nr Represents the noise of the original color image, N d represents the noise of the original depth image; DC represents the function of generating the aligned point cloud from the depth map, K represents the camera intrinsic parameter, and T represents the camera extrinsic parameter.

[0013] Furthermore, the preprocessed RGB image I1 is subjected to feature extraction to obtain the visual feature representation F r , extract the features of the preprocessed depth information I2 to obtain the local and global geometric structure representation F g ; Use dual-modal fusion attention mechanism to fuse F r and F g :

[0014]

[0015] Where A r With A g Represents the features after attention weighting or interaction; C represents the dual-modal fusion attention module function, which is used to calculate and generate feature representations adjusted by the attention mechanism; Fu represents the feature fusion strategy function, which is used to combine the two feature streams; F f represents the final deep fusion feature representation;

[0016] The deep fusion feature representation F f As input, a segmentation network is used to predict the branches and leaves of each point or area in the environment. Using the point cloud data structure with branch and leaf labels, the aligned point cloud data P is integrated with the semantic label L obtained by the segmentation network to generate a structured local environment model M, which identifies the location, type and geometry of free space and obstacles.

[0017] Furthermore, a function Y is constructed based on the structured local environment model to maximize the comprehensive picking effect:

[0018] Y=w1·f1+w2·f2+w3·f3+w4·f4+w5·f5

[0019] Where f1 represents the picking point access sequence optimization function, f2 represents the variable lift-off height function based on the picking depth, f3 represents the Z-axis height planning function for crossing obstacles, f4 represents the descending and ascending speed planning function, and f5 represents the gripper positioning accuracy function; w1, w2, w3, w4, and w5 are weight coefficients; the decision variable set X = (T, H1, H0, v d ,v u ,d p ), where T represents the access sequence of the picking point, H1 represents the actual lifting height of the picking point, H0 represents the maximum height of the obstacle on the picking point path, and v d 、vu Denote the rising speed and the falling speed respectively, d p Indicates the deviation distance between the center of the gripper and the position directly above the picking point;

[0020] Among them, the picking point access order optimization function is as follows:

[0021]

[0022] Where, represents the sum of the masses of all selected picking points, and T represents the access sequence of picking points (p0, p1, ..., p n ), n is the total number of picking points, p k represents the picking points selected for picking and meeting the preset quality standards, k = 0, 1, 2, ..., n; Represents the total cost of the path composed of all picking points; for the picking point quality score Q(p k ), if the number of surrounding obstacles does not exceed 10 and the light intensity is between 50Lux and 1000Lux, the quality is 1, otherwise the quality is 0; D(p i ,p i+1 ) represents the Euclidean distance between two adjacent picking points; ρ(p i ) represents the regional density factor of the i-th picking point; δ(p i ) represents the picking difficulty coefficient of the i-th picking point; represents the lighting condition coefficient of the i-th picking point;

[0023] The variable lift-off height function based on picking depth is as follows:

[0024]

[0025] Where, H1(p i ) is the actual lifting height of the i-th selected picking point; H2(p i ) is the picking depth of the i-th selected picking point; f(H2) represents the ideal lifting height based on the picking depth of the picking point; β is the lifting height adjustment coefficient;

[0026] The Z-axis height planning function for crossing obstacles is as follows:

[0027]

[0028] Where, H(p i ,p i+1 ) represents the maximum height between two adjacent picking points moving along the Z axis; H o (p i ,p i+1) represents the maximum height of the obstacle on the path between the two picking points; y is the safety margin parameter;

[0029] The descent and ascent speed planning functions are as follows:

[0030]

[0031] Where w d and w u is the weight coefficient of the descending and ascending stages; σ d and σ u are the deviation tolerance parameters for the ascending and descending stages respectively; v is the optimal speed; v d (p i ) represents the descending speed of the i-th picking point, v u (p i ) represents the rising speed of the i-th picking point;

[0032] The gripper positioning accuracy function is as follows:

[0033]

[0034] Where, d p (p i ) is the deviation distance between the center of the gripper and the position just above the picking point; σ p is the positioning deviation tolerance parameter;

[0035] The generated hard constraints for the picking effect include: 1) Picking point selection constraint: only picking points that meet the quality requirements are selected; 2) Sequence integrity constraint: each picking point appears at most once in the sequence; 3) Obstacle avoidance constraint: the robot height must exceed the height of the obstacles on the path plus the minimum safety margin; 4) Lifting height constraint: the lifting height must not be lower than the minimum safe lifting height calculated based on the picking depth; 5) Speed ​​constraint: the descending and ascending speeds must be within a safe range; 6) Positioning accuracy constraint: the deviation distance between the center of the gripper and the top of the picking point does not exceed the maximum allowable value.

[0036] Furthermore, a hierarchical heterogeneous adaptive optimization algorithm is applied to find the optimal global path that satisfies the function and the hard constraints of the picking effect. First, iterations are performed based on the initial feasible path parameters. In each iteration, a candidate geometric path is generated based on the current path parameters, and the comprehensive picking effect function value Y(X) of the candidate geometric path is calculated. At the same time, the candidate geometric path and its dynamic characteristics are checked for hard constraints. Then, based on the calculated Y(X) and the constraint satisfaction, the next set of path parameters is generated through the built-in optimization logic, driving the path solution to evolve towards a higher Y(X) value while strictly satisfying all constraints. Finally, when the improvement in the Y(X) value under all constraints does not meet the requirements or when the preset termination condition is reached, the iteration is stopped, and the optimal path parameters and the corresponding optimal geometric path that can maximize the objective function Y(X) are output.

[0037] The optimal geometric path is converted into a complete and smooth motion trajectory with precise time information, and a constraint compliance check is performed to verify the optimal global path. The optimal global path is converted into an instruction sequence for the robot to understand and execute and output, so that the robot can perform the picking task according to the optimal global path.

[0038] Furthermore, when the robot travels between adjacent picking points along the optimal global path, it uses sensors configured on the robot to monitor the local environmental information around the path in real time. This local environmental information is compared with the expected environmental state around the path segment based on the global environmental model to detect significant environmental changes or unexpected obstacles:

[0039]

[0040] Where ΔE(t) represents the environmental change or obstacle information detected at time t; is an environmental difference measurement function, which is used to compare the local environmental information at a certain moment with the expected environment and output the environmental change or the detected new obstacle information; M1(t) represents the local environmental information obtained by the sensor at time t, M e (t,P g ) represents the environmental state at a certain moment predicted by the global model, P g Indicates the global path segment currently being executed;

[0041] According to the identified environmental changes or new obstacle information, the complete features of the dynamic obstacle at time t are extracted: K is the total number of obstacles, and each obstacle is represented by o i (t) = {q i (t),v i (t),s i (t),τ i},qi (t) represents the coordinate vector of the center position of the ith obstacle at time t, v i (t) represents the estimated velocity vector of the i-th obstacle at time t, s i (t) represents the geometric size parameters of the i-th obstacle at time t, τ i Indicates the type of the i-th obstacle; based on the obstacle information and the current state of the robot, calculate the risk level of the current path segment:

[0042]

[0043] Where R(t) is the quantitative risk level; α i Represents the risk weight coefficient for different obstacle types; R c (P g ,o i (t)) represents the collision risk assessment function, which is used to calculate the robot along the global path P at time t g When walking, the i-th obstacle o i (t) is the risk of collision; ψ is the dynamic risk weight coefficient; R d (X r (t)) represents the risk function based on the dynamic state of the robot at time t. If the speed or acceleration is too large, the function value will be larger; X r (t) is the state of the robot at time t; d min (P g ,o i (t)) represents the minimum distance between the global path and the obstacle, κ is the distance scaling parameter; γ(v i (t),τ i ) represents the risk adjustment function based on obstacle speed and type.

[0044] Furthermore, when the assessed risk level exceeds the preset safety threshold, a local correction path is dynamically generated through the local path planning mechanism, and the elastic band algorithm is called to locally deform the current global path segment to generate a dynamic local correction path that can safely bypass the risk obstacle. The elastic band algorithm achieves path deformation by simulating the internal elasticity and external repulsive force of the path points to describe the position update process of the i-th picking point from discrete time step t to t+1, as shown in the following formula:

[0045]

[0046] Where, and They represent the positions of the i-th picking point on the path at iteration steps t+1 and t respectively; Δt represents the iteration step length, which controls the deformation speed; represents the internal elastic force acting on the i-th picking point at time t; represents the external environmental force at the i-th picking point at time t; represents the damping force acting on the i-th picking point, Indicates the speed of the picking point.

[0047] The beneficial effects of the present invention are:

[0048] 1) Improved accuracy and robustness of environmental perception in complex environments: The present invention intelligently integrates RGB images and depth information collected in real time by RGB-D cameras by adopting a deep fusion processing method based on a dual-modal attention mechanism. Compared with traditional methods that simply splice or are based on single-modal features, this method can use the attention mechanism to focus on the most informative feature combination for environmental understanding (especially the identification of obstacles such as branches and leaves), effectively overcoming the shortcomings of a single sensor's insufficient perception ability under unfavorable conditions such as complex lighting, similar textures, and mutual occlusion. This significantly improves the recognition accuracy and segmentation accuracy of key environmental elements such as branches and leaves in the picking operation area, providing a higher quality and more reliable environmental model for subsequent path planning;

[0049] 2) The comprehensive efficiency optimization and high-quality picking guarantee of picking path planning are realized: the present invention constructs and solves a constrained optimization problem aimed at maximizing the comprehensive picking efficiency function Y. This objective function innovatively quantifies and weightedly comprehensively evaluates multiple key factors affecting the final picking effect, such as the quality and cost of picking point access, the suitability of the gripper lifting height, obstacle crossing safety and efficiency, optimization of movement speed, and final positioning accuracy. By applying the "hierarchical heterogeneous adaptive optimization algorithm" to solve this goal under the premise of meeting all safety and functional hard constraints, this method can generate a truly "good" picking path - not only feasible or efficient, but also close to the optimal picking performance in multiple dimensions, thereby ensuring low damage, high success rate and high precision in the picking process, directly serving to improve the quality of the final picked tea and the overall efficiency of the operation;

[0050] 3) Enhanced the robot's ability to adapt autonomously and operate safely in dynamic and unknown environments: The present invention introduces a real-time local path planning mechanism based on dynamic environment perception. By continuously using sensors to monitor the actual environment around the robot's path and comparing it with the expected model, dynamic obstacles or environmental changes caused by shaking branches or other unforeseen factors can be detected in a timely manner. Once the assessed collision risk exceeds the threshold, the system can quickly trigger and execute local path corrections (for example, using an elastic band algorithm) to guide the robot to safely bypass. This online, small-scale path adjustment capability greatly improves the robustness and safety of the robot's operations in complex, unstructured and dynamically changing tea garden environments, avoids planning failures or collisions due to small real-time disturbances in the environment, and ensures the continuity and reliability of the picking task.

[0051] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0053] Figure 1 A schematic flow chart of a picking path planning method for an intelligent tea-picking robot based on dual-modal fusion attention and hierarchical adaptive optimization provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0054] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0055] This paper proposes a picking path planning method for an intelligent tea-picking robot based on bimodal fusion attention and hierarchical adaptive optimization. This method combines advanced multimodal environmental perception technology with an optimized path planning strategy with efficiency constraints. It uses the rich visual and depth information obtained by RGB-D cameras to perform detailed environmental modeling, and uses a hierarchical heterogeneous adaptive optimization algorithm to solve a complex constrained optimization problem with the goal of maximizing comprehensive picking efficiency. This method is supplemented by real-time dynamic environmental perception and a local path correction mechanism, thus providing a complete, highly robust, and efficient path planning solution for intelligent tea picking.

[0056] like Figure 1 As shown, a picking path planning method for an intelligent tea-picking robot based on dual-modal fusion attention and hierarchical adaptive optimization is provided in one embodiment of the present invention. The method is as follows:

[0057] S1: Based on the RGB images and depth information collected in real time by the RGB-D camera, a deep fusion processing method of color texture and geometric features based on a bimodal attention mechanism is adopted to accurately identify and extract the branch and leaf information for path planning, and finally construct a structured local environment model with geometric semantics.

[0058] Specifically:

[0059] S11: Synchronously collect and pre-process multimodal sensor data. Through the synchronous trigger mechanism, the RGB-D camera configured on the intelligent tea picking robot simultaneously collects color images I r and depth information I d After acquisition, the raw data is preprocessed, including: (1) applying filters to remove image noise N r 、N d (2) Remove or interpolate invalid values ​​(such as zero depth and infinity) of the depth data; (3) Align the data based on the camera's intrinsic and extrinsic parameters to ensure that the RGB pixels accurately match their corresponding three-dimensional space points, and obtain the aligned color image I1 and point cloud data P:

[0060]

[0061] Among them, I1 represents the preprocessed color image, I2 represents the preprocessed depth image, S represents the preprocessing function, I r Represents the original color image input, N r Represents the noise of the original color image, I d Represents the original depth image of the input, N d Denotes the noise of the original depth image. DC denotes the function for generating the aligned point cloud from the depth map, K denotes the camera intrinsic parameter, T denotes the camera extrinsic parameter, and P denotes the aligned point cloud data in a unified coordinate system.

[0062] S12: Extracting single-modal initial features. Perform feature extraction on the pre-processed RGB image I1 and depth information I2 to obtain the basic representation of each modality. For RGB images, use the pre-trained convolutional neural network backbone network CNN to extract multi-level visual feature maps. These features contain information such as color, texture, and edge, and extract visual feature representation F r For depth information, calculate geometric features, such as the normal vector N, curvature B, height information Z of each point, and apply a specific point cloud feature extraction network to obtain local and global geometric structure representation F g .

[0063] S13: Applying a bimodal attention mechanism for deep feature fusion. The attention mechanism is used to intelligently combine features from RGB and depth / geometry modalities to generate more informative and robust fused features.

[0064] Specifically, the interdependence or complementarity between the two modal features can be calculated through the bimodal fusion attention module. This module generates an attention weight map by calculating the interaction of the query, key, and value matrices. These weights indicate which regions should be given more attention during fusion. These attention weights are then applied to the original unimodal features, and a specific fusion strategy is used to combine the weighted features.

[0065]

[0066] Among them, A r With A g Represents the features after attention weighting or interaction; C represents the bimodal attention module function, which is used to calculate and generate the feature representation adjusted or enhanced by the attention mechanism; Fu represents the feature fusion strategy function, which is used to effectively combine the two feature streams; F f represents the final deep fusion feature representation.

[0067] S14: Perform semantic segmentation of environmental elements and recognition of branches and leaves based on deep fusion feature representation. f As input, a segmentation network is used to predict branches and leaves for each point or region in the environment. Using the point cloud data structure with branch and leaf labels, the aligned point cloud data P is integrated with the semantic labels L obtained by the segmentation network to generate a structured local environment model M that explicitly identifies the location, type, and geometry of free space and obstacles.

[0068] S2: Based on the structured local environment model constructed in S1, a hierarchical heterogeneous adaptive optimization algorithm is applied to construct and solve a function aimed at maximizing the comprehensive picking effect. This function quantifies the scoring items of factors such as the quality and cost of picking point access, the rationality of the gripper rising height, the safety efficiency of obstacle crossing, the speed optimization degree and the positioning accuracy through weighted combination to plan and generate a global picking path instruction sequence from the current state of the robot to the preparatory position of the next target picking point, which meets all picking efficiency constraints and has the best utility.

[0069] Specifically:

[0070] S21: Based on the structured local environment model M, the core mathematical optimization problem to be solved in this picking global path planning task is accurately constructed. The goal is to find a path from the robot's current state to the next target point's preparatory position. This path can be able to construct a multi-objective function Y that quantifies the quality and efficiency of the tea picking operation under the premise of satisfying a series of hard constraints on the picking effect:

[0071] Y=w1·f1+w2·f2+w3·f3+w4·f4+w5·f5(3)

[0072] Among them, f1 represents the function for optimizing the order of picking point visits, f2 represents the function for variable lift-off height based on picking depth, f3 represents the function for planning the Z-axis height to cross obstacles, f4 represents the function for planning the descending and ascending speeds, and f5 represents the function for gripper positioning accuracy. w1 to w5 are the weight coefficients corresponding to f1 to f5, and the five functions f1 to f5 evaluate five considerations respectively. The decision variable set X for path planning is (T, H1, H0, v d ,v u ,d p ), where T represents the access sequence of the picking point, H1 represents the actual lifting height of the picking point, H0 represents the maximum height of the obstacle on the picking point path, and v d 、v u Denote the rising speed and the falling speed respectively, d p Indicates the deviation distance between the center of the gripper and the position directly above the picking point.

[0073] Specifically, these scoring functions are defined as follows:

[0074] (1) Picking point access order optimization function f1(T) and cost function C v (p i ,p i+1 ):

[0075]

[0076] in, represents the sum of the masses of all selected picking points, and T represents the access sequence of picking points (p0, p1, ..., p n ), is the path component of the decision variable set X, and n is the total number of picking points. k represents the picking points selected for picking and meeting the preset quality standards, k = 0, 1, 2, …, n. Represents the total cost of the path composed of all picking points, and the quality score Q(p k ), if the number of surrounding obstacles does not exceed 10 and the light intensity is between 50Lux and 1000Lux, the quality is 1, otherwise the quality is 0 (that is, the point is not selected). i ,p i+1 ) represents the Euclidean distance between two adjacent picking points. ρ(p i ) represents the regional density factor of the i-th picking point, which is evaluated by the distribution of picking points in the region. i ) represents the picking difficulty coefficient of the i-th picking point, which is used to measure the operation difficulty of the picking point, usually considering factors such as obstacle density, terrain complexity, and lighting conditions. Represents the lighting condition coefficient of the i-th picking point.

[0077] (2) The variable lifting height function f2(H1) based on the picking depth and the ideal lifting height function f(H2) are:

[0078]

[0079] Among them, H1(p i ) is the actual lifting height of the i-th selected picking point, which is the path component of the decision variable set X. i ) is the picking depth of the i-th selected picking point. f(H2) represents the ideal lift-off height based on the picking depth of the picking point. β ranges from 0 to 0.5, indicating that a greater picking depth requires a greater lift-off height. Function f2(H1) models the nonlinear relationship between picking depth and ideal lift-off height. Its purpose is to adjust the lift-off height based on changes in picking depth, ensuring that the machine avoids lift-offs that are too low or too high during path planning, thereby improving picking efficiency and safety.

[0080] (3) Z-axis height planning function f3(H) to cross obstacles o ):

[0081]

[0082] Among them, H(p i ,p i+1 ) represents the maximum height between two adjacent picking points moving along the Z axis.o (p i ,p i+1 ) represents the maximum height of obstacles on the path between the two picking points and is the path component of the decision variable set X. y is the safety margin parameter, usually 5 to 10 cm.

[0083] (4) Descent and ascent speed planning function f4(v d ,v u ):

[0084]

[0085] Among them, n is the total number of picking points, w d and w u is the weight coefficient of the descending and ascending stages, and the sum of the two weights is 1. d and σ u are the deviation tolerance parameters for the ascending and descending stages, which are the tolerance for speed deviation. v is the optimal speed, v d (p i ) represents the descending speed of the i-th picking point, v u (p i ) represents the rising speed of the i-th picking point, and both are path components of the decision variable set X. exp(·) is an exponential function.

[0086] (5) Gripper positioning accuracy function f5(d):

[0087]

[0088] Among them, n is the total number of picking points, d p (p i ) is the deviation distance between the center of the gripper and the position just above the picking point, and is the path component in the decision variable set X. p is the positioning deviation tolerance parameter.

[0089] The hard constraints on picking effects include:

[0090] 1) Picking point selection constraints: only picking points that meet quality requirements are selected;

[0091] 2) Sequence integrity constraint: Each picking point appears at most once in the sequence;

[0092] 3) Obstacle avoidance constraint: The robot's height must exceed the height of the obstacles on the path plus a minimum safety margin;

[0093] 4) Lifting height constraint: The lifting height must not be lower than the minimum safe lifting height calculated based on the picking depth;

[0094] 5) Speed ​​constraint: descent and ascent speeds must be within a safe range;

[0095] 6) Positioning accuracy constraint: The deviation distance between the center of the gripper and the picking point directly above it does not exceed the maximum allowable value.

[0096] S22: Execute the "hierarchical heterogeneous adaptive optimization algorithm" to find the best global path that meets the goals and rules defined in step S21. This process is an iterative optimization cycle:

[0097] The algorithm starts with an initial feasible path parameter. In each iteration, it generates a candidate geometric path based on the current path parameters and performs a comprehensive evaluation of its overall harvesting effectiveness. This evaluation strictly follows a predetermined formula to calculate the path's score in five areas: access point quality and cost, lift height, obstacle safety efficiency, speed suitability, and positioning accuracy. A weighted summation is then performed to determine the total effectiveness value Y(X) for the current path. Simultaneously, the candidate path and its dynamic characteristics are rigorously checked against hard constraints to ensure compliance with safety, kinematics, and harvesting process requirements. Based on the calculated effectiveness value Y(X) and constraint satisfaction, the algorithm applies its built-in optimization logic to intelligently generate the next set of "more promising" path parameters, driving the path solution toward higher Y(X) values ​​while strictly satisfying all constraints. This iterative cycle of "evaluation-constraint checking-improvement generation" continues until the algorithm converges to the optimal solution (i.e., Y(X) no longer significantly improves and all constraints are satisfied) or a pre-set termination condition is met. The algorithm ultimately outputs the optimal path parameters and corresponding geometric path that maximize the objective function Y(X).

[0098] S23: The optimal geometric path output from step S22 is converted into a complete, smooth motion trajectory with precise timing information. A final, comprehensive constraint compliance check is performed to ensure that it is both optimized and safe in actual execution. The final verified complete spatiotemporal motion trajectory is converted into a specific instruction sequence that can be directly understood and executed by the robot's underlying controller and output.

[0099] S3: When executing the global path generated in step S2, especially during the transition phase when the robot moves between two picking points, a local path planning mechanism based on dynamic environment perception is activated. This mechanism continuously uses sensors such as RGB-D cameras configured on the robot to monitor the environmental conditions around the travel path in real time. Based on these detected dynamic changes and the current state of the robot, the potential collision risk level is evaluated in real time. Once the assessed risk exceeds the preset safety threshold, the mechanism is triggered to dynamically generate a small-scale local correction path, integrate or reconnect the correction path to the original global path, and switch execution.

[0100] Specifically:

[0101] S31: As the robot moves along a global path segment, it continuously uses sensors (e.g., RGB-D cameras) to collect real-time local environmental information. This information is compared with the expected environmental state around that path segment based on the global environmental model to detect significant environmental changes or unexpected obstacles:

[0102]

[0103] Wherein, ΔE(t) represents the environmental change or obstacle information detected at time t. It is an environmental difference measurement function used to compare the local environmental information at a certain moment with the expected environment and output the environmental change or the new obstacle information detected. M1(t) represents the local environmental information obtained by the sensor at time t, M e (t,P g ) represents the environmental state at a certain moment predicted by the global model, P g Indicates the global path segment currently being executed;

[0104] S32: Based on the new obstacle information or environmental changes identified in S31, extract the complete features of the dynamic obstacle at time t: K is the total number of obstacles. Each obstacle is represented by o i (t) = {q i (t),v i (t),s i (t),τ i},q i (t) represents the coordinate vector of the center position of the ith obstacle at time t, v i (t) represents the estimated velocity vector of the i-th obstacle at time t, s i (t) represents the geometric size parameters of the i-th obstacle at time t, τ i Indicates the type of the i-th obstacle, that is, a hard obstacle or a soft obstacle. Based on the detected obstacle information and the current state of the robot, the risk level of the current path segment is calculated:

[0105]

[0106] Where R(t) is the quantitative risk level, such as 1, 2, etc. i Represents the risk weight coefficient for different obstacle types. Hard obstacles and flexible obstacles will be given different weights. c (P g ,o i (t)) represents the collision risk assessment function, which is used to calculate the robot's collision risk along the global path P at time t. g When walking, the i-th obstacle o i(t) Risk of collision. ψ represents the dynamic risk weight coefficient, R d (X r (t)) represents the risk function based on the dynamic state of the robot at time t. If the speed or acceleration is too large, the function value will be larger, and X r (t) is the state of the robot at time t. min (P g ,o i (t)) represents the minimum distance between the global path and the obstacle, and κ is the distance scaling parameter that controls the risk attenuation rate. i (t),τ i ) represents the risk adjustment function based on obstacle speed and type.

[0107] The assessed risk level is compared with a preset risk trigger threshold to determine whether local route replanning is necessary. If the calculated risk level R(t) is greater than the risk trigger threshold R, local route replanning is initiated. Otherwise, local route replanning is not required and the original route continues.

[0108] S33: If local replanning is required, the elastic band algorithm is called to locally deform the current global path segment to generate a dynamic local correction path that can safely bypass the risky obstacles. This algorithm implements path deformation by simulating the internal elasticity and external repulsion of the path points, describing how the position of the i-th picking point is updated from discrete time step t to t+1, as shown in the following formula:

[0109]

[0110] in, and They represent the positions of the i-th picking point on the path at iteration steps t+1 and t respectively; Δt represents the iteration step length, which controls the deformation speed; represents the internal elastic force acting on the i-th picking point at time t, maintaining the continuity and smoothness of the path; Represents the external environmental force of the i-th picking point at time t, mainly coming from obstacles of exclusion; represents the damping force acting on the i-th picking point, Indicates the speed of the picking point, which is used to ensure the convergence of the algorithm.

[0111] For internal elastic force

[0112]

[0113] Among them, k eIt is an elastic force, similar to the spring tension, which keeps the adjacent picking points at a reasonable distance; k c It is the curvature force that prevents the path from being excessively curved and maintains overall smoothness. These are three consecutive picking points on the path at time t. Through the internal elastic force, the path can maintain its structure and continuity during the deformation process, avoiding sharp turns or unreasonable local deformation.

[0114] For external environmental forces

[0115]

[0116] U r Represents the repulsive potential field function:

[0117]

[0118] in, is the gradient operator, which calculates the directional derivative of the potential field; η is the potential field strength parameter, which controls the magnitude of the repulsive force; d0 is the influence range of the obstacle, and no repulsive force will be generated beyond the influence range of the obstacle; For the i-th picking point x to the obstacle o i (t) the shortest distance.

[0119] For the damping force

[0120]

[0121] μ is the damping coefficient, which is usually between 0 and 0.4 and controls the deceleration effect; is the instantaneous velocity of the i-th picking point at time t, which can be approximately calculated by the difference between two consecutive iterative positions:

[0122] S34: Continuously execute the iterative optimization process, calculating the update amount of the path control points in each iteration based on the local environmental potential field and the path intrinsic energy function, and adjusting their spatial positions accordingly. This iterative process continuously drives the path shape to evolve towards a stable state that better satisfies specific constraints until the position change of all control points on the path converges to within a preset small threshold, and the objective function reaches a local optimum and no longer improves significantly. At this point, a fully optimized local corrected path that can effectively avoid transient obstacles is obtained. The path control points are used to adjust the discrete amount of the corrected path segment shape.

[0123] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A picking path planning method for an intelligent tea-picking robot based on dual-modal fusion attention and hierarchical adaptive optimization, characterized in that: The method includes: The robot's RGB-D camera collects RGB images and depth information in real time, pre-processes them, and extracts features. A dual-modal fusion attention mechanism is then used to fuse these features. Based on the fused features, the robot performs semantic segmentation of the environment and identifies branches and leaves, generating a structured local environment model. Based on a structured local environment model, a function is constructed to maximize the overall picking effect, while also generating hard constraints for the picking effect. A hierarchical heterogeneous adaptive optimization algorithm is applied to find the optimal global path that satisfies this function and the hard constraints for the picking effect. This optimal global path is converted into a sequence of instructions for the robot to understand and execute, and the instruction sequence is output, allowing the robot to perform the picking task according to the optimal global path. During the transition phase when the robot moves between two picking points, the local path planning mechanism based on dynamic environment perception is activated, and the sensors configured on the robot are used to monitor the environmental status around the travel path in real time. The potential collision risk level is evaluated in real time based on the detected dynamic changes in the environment and the current state of the robot. When the assessed risk level exceeds the preset safety threshold, a local correction path is dynamically generated through the local path planning mechanism, and the local correction path is integrated into or reconnected to the original global path, and then switched for execution.

2. The method according to claim 1, characterized in that For the collected RGB image I r and depth information I d Preprocessing is performed. First, the image noise is removed by filtering, then invalid values ​​are eliminated or interpolated for the depth information. Finally, data alignment is performed based on the camera's intrinsic and extrinsic parameters to ensure that the RGB pixels accurately match the corresponding 3D space points. The aligned color image I1, depth image I2, and point cloud data P are obtained: Where S represents the preprocessing function, N r Represents the noise of the original color image, N d represents the noise of the original depth image; DC represents the function of generating the aligned point cloud from the depth map, K represents the camera intrinsic parameter, and T represents the camera extrinsic parameter.

3. The method according to claim 2, characterized in that Feature extraction is performed on the preprocessed RGB image I1 to obtain the visual feature representation F r , extract the features of the preprocessed depth information I2 to obtain the local and global geometric structure representation F g ; Use dual-modal fusion attention mechanism to fuse F r and F g : Where A r With A g Represents the features after attention weighting or interaction; C represents the dual-modal fusion attention module function, which is used to calculate and generate feature representations adjusted by the attention mechanism; Fu represents the feature fusion strategy function, which is used to combine the two feature streams; F f represents the final deep fusion feature representation; The deep fusion feature representation F f As input, a segmentation network is used to predict the branches and leaves of each point or area in the environment. Using the point cloud data structure with branch and leaf labels, the aligned point cloud data P is integrated with the semantic label L obtained by the segmentation network to generate a structured local environment model M, which identifies the location, type and geometry of free space and obstacles.

4. The method according to claim 3, characterized in that Based on the structured local environment model, a function Y is constructed to maximize the comprehensive picking effect: Y=w1·f1+w2·f2+w3·f3+w4·f4+w5·f5 Where f1 represents the picking point access sequence optimization function, f2 represents the variable lift-off height function based on the picking depth, f3 represents the Z-axis height planning function for crossing obstacles, f4 represents the descending and ascending speed planning function, and f5 represents the gripper positioning accuracy function; w1, w2, w3, w4, and w5 are weight coefficients; the decision variable set X = (T, H1, H0, v d ,v u ,d p ), where T represents the access sequence of the picking point, H1 represents the actual lifting height of the picking point, H0 represents the maximum height of the obstacle on the picking point path, and v d 、v u Denote the rising speed and the falling speed respectively, d p Indicates the deviation distance between the center of the gripper and the position directly above the picking point; Among them, the picking point access order optimization function is as follows: Where, represents the sum of the masses of all selected picking points, and T represents the access sequence of picking points (p0, p1, ..., p n ), n is the total number of picking points, p k represents the picking points selected for picking and meeting the preset quality standards, k = 0, 1, 2, ..., n; Represents the total cost of the path composed of all picking points; for the picking point quality score Q(p k ), if the number of surrounding obstacles does not exceed 10 and the light intensity is between 50Lux and 1000Lux, the quality is 1, otherwise the quality is 0; D(p i ,p i+1 ) represents the Euclidean distance between two adjacent picking points; ρ(p i ) represents the regional density factor of the i-th picking point; δ(p i ) represents the picking difficulty coefficient of the i-th picking point; represents the lighting condition coefficient of the i-th picking point; The variable lift-off height function based on picking depth is as follows: Where, H1(p i ) is the actual lifting height of the i-th selected picking point; H2(p i ) is the picking depth of the i-th selected picking point; f(H2) represents the ideal lifting height based on the picking depth of the picking point; β is the lifting height adjustment coefficient; The Z-axis height planning function for crossing obstacles is as follows: Where, H(p i ,p i+1 ) represents the maximum height between two adjacent picking points moving along the Z axis; H o (p i ,p i+1 ) represents the maximum height of the obstacle on the path between the two picking points; y is the safety margin parameter; The descent and ascent speed planning functions are as follows: Where w d and w u is the weight coefficient of the descending and ascending stages; σ d and σ u are the deviation tolerance parameters for the ascending and descending stages respectively; v is the optimal speed; v d (p i ) represents the descending speed of the i-th picking point, v u (p i ) represents the rising speed of the i-th picking point; The gripper positioning accuracy function is as follows: Where, d p (p i ) is the deviation distance between the center of the gripper and the position just above the picking point; σ p is the positioning deviation tolerance parameter; The generated hard constraints for the picking effect include: 1) Picking point selection constraint: only picking points that meet the quality requirements are selected; 2) Sequence integrity constraint: each picking point appears at most once in the sequence; 3) Obstacle avoidance constraint: the robot height must exceed the height of the obstacles on the path plus the minimum safety margin; 4) Lifting height constraint: the lifting height must not be lower than the minimum safe lifting height calculated based on the picking depth; 5) Speed ​​constraint: the descending and ascending speeds must be within a safe range; 6) Positioning accuracy constraint: the deviation distance between the center of the gripper and the top of the picking point does not exceed the maximum allowable value.

5. The method according to claim 4, characterized in that A hierarchical heterogeneous adaptive optimization algorithm is applied to find the optimal global path that satisfies the function and the hard constraints of the picking effect. First, iterations are performed based on the initial feasible path parameters. In each iteration, a candidate geometric path is generated based on the current path parameters, and the comprehensive picking effect function value Y(X) of the candidate geometric path is calculated. At the same time, the candidate geometric path and its dynamic characteristics are checked for hard constraints. Then, based on the calculated Y(X) and the constraint satisfaction, the next set of path parameters is generated through built-in optimization logic, driving the path solution to evolve towards a higher Y(X) value while strictly satisfying all constraints. Finally, the iteration is terminated when the improvement in the Y(X) value under all constraints does not meet the requirements or when the preset termination condition is reached. The optimal path parameters and the corresponding optimal geometric path that maximize the objective function Y(X) are output. The optimal geometric path is converted into a complete and smooth motion trajectory with precise time information, and a constraint compliance check is performed to verify the optimal global path. The optimal global path is converted into an instruction sequence for the robot to understand and execute and output, so that the robot can perform the picking task according to the optimal global path.

6. The method according to claim 5, characterized in that When the robot travels between adjacent picking points along the optimal global path, it uses its sensors to monitor the local environment around the path in real time. This local environment information is compared with the expected environmental state around the path segment based on the global environmental model to detect significant environmental changes or unexpected obstacles. Where ΔE(t) represents the environmental change or obstacle information detected at time t; is an environmental difference measurement function, which is used to compare the local environmental information at a certain moment with the expected environment and output the environmental change or the detected new obstacle information; M1(t) represents the local environmental information obtained by the sensor at time t, M e (t,P g ) represents the environmental state at a certain moment predicted by the global model, P g Indicates the global path segment currently being executed; According to the identified environmental changes or new obstacle information, the complete features of the dynamic obstacle at time t are extracted: K is the total number of obstacles, and each obstacle is represented by o i (t) = {q i (t),v i (t),s i (t),τ i },q i (t) represents the coordinate vector of the center position of the ith obstacle at time t, v i (t) represents the estimated velocity vector of the i-th obstacle at time t, s i (t) represents the geometric size parameters of the i-th obstacle at time t, τ i Indicates the type of the i-th obstacle; based on the obstacle information and the current state of the robot, calculate the risk level of the current path segment: Where R(t) is the quantitative risk level; α i Indicates the risk weight coefficient for different obstacle types; R c (P g ,o i (t)) represents the collision risk assessment function, which is used to calculate the robot along the global path P at time t g When walking, the i-th obstacle o i (t) is the risk of collision; ψ is the dynamic risk weight coefficient; R d (X r (t)) represents the risk function based on the dynamic state of the robot at time t. If the speed or acceleration is too large, the function value will be larger; X r (t) is the state of the robot at time t; d min (P g ,o i (t)) represents the minimum distance between the global path and the obstacle, κ is the distance scaling parameter; γ(v i (t),τ i ) represents the risk adjustment function based on obstacle speed and type.

7. The method according to claim 6, characterized in that When the assessed risk level exceeds the preset safety threshold, a local correction path is dynamically generated through the local path planning mechanism. The elastic band algorithm is called to locally deform the current global path segment to generate a dynamic local correction path that can safely bypass the risk obstacle. The elastic band algorithm implements path deformation by simulating the internal elasticity and external repulsive force of the path points to describe the position update process of the i-th picking point from discrete time step t to t+1, as shown in the following formula: Where, and They represent the positions of the i-th picking point on the path at iteration steps t+1 and t respectively; Δt represents the iteration step length, which controls the deformation speed; represents the internal elastic force acting on the i-th picking point at time t; represents the external environmental force at the i-th picking point at time t; represents the damping force acting on the i-th picking point, Indicates the speed of the picking point.

Images