Grapefruit picking optimization method and system based on hand-foot cooperation

Abstract

The invention discloses a grapefruit picking optimization method based on hand and foot cooperation. The method comprises the steps that an initial track is generated according to the position of a target grapefruit and the current position of an end effector of a mechanical arm; moving along an initial track, and detecting a path obstacle in real time; dynamically selecting a rigid obstacle avoidance strategy cooperatively completed by the mechanical arm and the chassis or a flexible penetration strategy cooperatively completed by the mechanical arm and the chassis based on physical characteristics of obstacles; and circularly executing detection and strategy selection until the target position threshold range is reached. According to the invention, utilization of computing resources can be optimized, dynamic environment adaptability is enhanced, cooperative control performance is enhanced, operation efficiency is improved, and service life of equipment is prolonged.

Description

Technical Field

[0001] The present invention relates to the field of intelligent agricultural machinery, and in particular to a grapefruit picking optimization method and system based on hand-foot collaboration. Background Art

[0002] Conventional grapefruit-picking robots generally employ fixed path planning and a single obstacle avoidance strategy, which presents the following technical limitations: Existing methods rely on a pre-set obstacle database, making them difficult to handle unstructured obstacles such as swaying branches and insects in orchards. Mechanical detour strategies are still used when encountering flexible obstacles, resulting in lengthy paths and low operational efficiency. Traditional robotic arms and mobile platforms employ separate control architectures, which are prone to motion interference during dynamic obstacle avoidance and lack vibration suppression mechanisms when penetrating flexible obstacles, leading to instability in the end effector's posture. Real-time three-dimensional path planning in complex environments consumes significant computing resources, and frequent path replanning can easily cause system delays, increasing the rate of fruit picking failures, requiring urgent improvement. Summary of the Invention

[0003] To solve the technical problems in the background technology, the present invention proposes a grapefruit picking optimization method based on hand-foot coordination, comprising:

[0004] S1, generate the initial trajectory according to the target grapefruit position and the current position of the robot end effector;

[0005] S2, moving along the initial trajectory and detecting obstacles on the path in real time;

[0006] S3. Dynamically select a rigid obstacle avoidance strategy coordinated by the manipulator and chassis or a flexible penetration strategy coordinated by the manipulator and chassis based on the physical characteristics of the obstacle;

[0007] S4. Loop detection and strategy selection until the target position threshold range is reached.

[0008] Furthermore, the rigid obstacle avoidance strategy specifically includes: when a rigid obstacle is detected, exiting the initial trajectory; regenerating the obstacle avoidance trajectory; and adjusting the manipulator joint space parameters and chassis displacement vector to adapt to the obstacle avoidance trajectory.

[0009] Furthermore, adjusting the manipulator's joint spatial parameters involves inversely calculating the target configuration of each joint based on the geometric characteristics of the obstacle avoidance trajectory, and achieving spatial alignment between the end-point pose and the obstacle avoidance trajectory through closed-loop joint angle control. Furthermore, the manipulator's redundant degrees of freedom are synchronously decoupled, and joint motion priority constraints are established to prevent loss of control caused by singular configurations.

[0010] Furthermore, adjusting the chassis displacement vector specifically includes: calculating the chassis lateral compensation displacement based on the spatial offset of the obstacle avoidance trajectory, establishing a geometric projection relationship between the chassis motion vector and the trajectory of the robot end, and achieving lateral slip compensation through chassis wheel speed differential control to maintain the relative position of the robot arm workspace and the target grapefruit unchanged.

[0011] Furthermore, when adjusting the spatial parameters of the robot arm joints and the chassis displacement vector, it is also necessary to verify the dynamic matching of the robot arm joint parameters and the chassis displacement vector within the kinematic framework. By feedback on the deviation between the actual trajectory of the end effector and the planned trajectory, the joint acceleration curve and the chassis velocity profile are iteratively optimized to ensure that there is no motion interference during the detour.

[0012] Furthermore, the flexible penetration strategy specifically includes: when a flexible obstacle is detected, the robotic arm maintains its initial trajectory to penetrate the flexible obstacle; during the penetration process, the robotic arm dynamically adjusts its posture according to the reaction force it receives to offset the component force that may cause the normal deviation of the robotic arm; at the same time, the vibration accumulation effect during the penetration process is reduced or eliminated through the coordinated speed of the chassis and the robotic arm.

[0013] Furthermore, the offset of the component force of the normal offset of the robotic arm specifically includes: constructing a real-time mapping model from the contact force space to the joint torque space, decomposing the contact force detected by the six-dimensional force sensor into normal and tangential components, generating a reverse compensation torque for the normal offset component, and dynamically adjusting the end posture angle through joint torque closed-loop control to maintain the axial consistency of the penetration direction and the initial trajectory.

[0014] Furthermore, the vibration accumulation effect during the penetration process is reduced or eliminated through the speed coordination of the chassis and the robotic arm. Specifically, the following steps are taken: establishing an equivalent mass-spring model of the robotic arm-chassis system, identifying the energy accumulation mode during the penetration process through the vibration frequency characteristics, and adopting a speed feedforward compensation algorithm to inject a regulating component with an opposite phase to the robotic arm vibration into the chassis translation speed, thereby forming an energy cancellation effect within the mechanical system.

[0015] Furthermore, during the penetration phase, the vector synthesis relationship between the linear velocity of the robot arm end and the chassis movement speed is maintained, and the robot arm joint velocity distribution is adjusted in real time through the inverse kinematic solution. When the vibration amplitude is detected to exceed the safety threshold, the chassis micro-reciprocating motion mode is triggered, and the system inertia force is used to offset the rebound impact of the flexible obstacle.

[0016] The present invention proposes a grapefruit picking optimization method based on hand-foot collaboration, which avoids the computational burden of continuous global path planning through a straight initial trajectory and an on-demand strategy switching mechanism; the flexible penetration strategy reduces the frequency of path replanning and significantly reduces the system computing power requirements; multimodal perception fusion accurately distinguishes the characteristics of rigid / flexible obstacles to achieve intelligent strategy selection; the dynamic trajectory prediction model effectively copes with time-varying obstacles and improves the foresight of decision-making; the robotic arm-chassis collaborative motion model actively offsets vibration energy and significantly suppresses the end vibration of the penetration process; the normal offset compensation mechanism ensures the stability of the penetration direction and improves the picking accuracy; the flexible penetration strategy greatly shortens the path length and significantly reduces the time spent on picking a single fruit; the collaborative parameter adjustment mechanism ensures the real-time switching of strategies and maintains the continuity of operations; the buffering mechanism of the rigid obstacle avoidance strategy effectively reduces the impact force of collisions, and the vibration elimination algorithm reduces the wear of mechanical components and enhances the durability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0018] Figure 2 It is a partial flow diagram of the present invention;

[0019] Figure 3 It is a partial flow diagram of the present invention;

[0020] Figure 4 It is a partial flow diagram of the present invention;

[0021] Figure 5 It is a partial flow diagram of the present invention. DETAILED DESCRIPTION

[0022] The present invention proposes a grapefruit picking optimization method based on hand-foot coordination, comprising:

[0023] S1. Generate the initial trajectory based on the target grapefruit position and the current position of the robot end effector.

[0024] S11. Obtain the three-dimensional coordinates of the target grapefruit through multi-sensor fusion, such as combining a visual camera with a laser rangefinder. The visual camera is used to identify the center point of the grapefruit outline and use the outline center point as the target point. The laser rangefinder measures the straight-line distance to the outline center point. After data fusion, the target point coordinates are output.

[0025] S12. Real-time acquisition of the state of the picking front end, including reading the angle value of each joint encoder, calculating the current position of the end through forward kinematics, and detecting the real-time posture of the end effector.

[0026] S2. Move along the initial trajectory and detect obstacles on the path in real time.

[0027] Path obstacle detection uses multimodal environmental perception, deploys a stereo vision module to collect real-time three-dimensional point clouds ahead of the trajectory, uses millimeter-wave radar to scan the spatial depth information of the path, and activates flexible tactile sensors to monitor changes in end contact force. Based on point cloud clustering analysis, it can distinguish between fixed obstacles such as branches and dynamic obstacles such as swaying branches and flying insects. It can also establish an obstacle motion trajectory prediction model to estimate the path occupancy in the next few seconds.

[0028] S3. Dynamically select a rigid obstacle avoidance strategy performed by the robot arm and chassis in collaboration or a flexible penetration strategy performed by the robot arm and chassis in collaboration based on the physical characteristics of the obstacle.

[0029] When the obstacle is rigid, a rigid obstacle avoidance strategy is adopted; when the obstacle is flexible, a flexible obstacle avoidance strategy is adopted. Rigid obstacles have non-deformable properties and high structural strength, and maintain geometric stability under external forces, such as the trunk of a pomelo tree; flexible obstacles have elastic deformation capabilities and low energy dissipation characteristics, and the contact process is accompanied by recoverable deformation, such as the leaves and small branches of a pomelo tree.

[0030] The rigid obstacle avoidance strategy specifically includes:

[0031] S311: When a rigid obstacle is detected, exit the initial trajectory.

[0032] S312: Regenerate the obstacle avoidance trajectory.

[0033] S313. Adjust the joint space parameters of the robotic arm and the chassis displacement vector to adapt to the obstacle avoidance trajectory.

[0034] Adjusting the spatial parameters of the manipulator's joints involves inversely calculating the target configuration of each joint based on the geometric characteristics of the obstacle avoidance trajectory, and achieving spatial alignment between the end-point pose and the obstacle avoidance trajectory through closed-loop control of the joint angles. Furthermore, the manipulator's redundant degrees of freedom are synchronously decoupled, and joint motion priority constraints are established to prevent loss of control caused by singular configurations.

[0035] Adjusting the chassis displacement vector involves calculating the chassis' lateral compensation displacement based on the spatial offset of the obstacle avoidance trajectory and establishing a geometric projection relationship between the chassis motion vector and the robot's end-of-arm trajectory. Lateral slip compensation is achieved through chassis wheel speed differential control, maintaining the relative position of the robot's workspace and the target grapefruit unchanged.

[0036] When adjusting the spatial parameters of the robot arm joints and the chassis displacement vector, it is also necessary to verify the dynamic matching of the robot arm joint parameters and the chassis displacement vector within the kinematic framework. By feedback on the deviation between the actual trajectory of the end effector and the planned trajectory, the joint acceleration curve and the chassis velocity profile are iteratively optimized to ensure that there is no motion interference during the detour.

[0037] S314: The end of the robotic arm is caused to move around using an obstacle avoidance trajectory.

[0038] Flexible penetration strategies specifically include:

[0039] S321: When a flexible obstacle is detected, the robotic arm maintains the initial trajectory to penetrate the flexible obstacle.

[0040] S322. During the penetration process, the robot arm dynamically adjusts its posture according to the reaction force it receives to offset the force component that may cause the normal deviation of the robot arm. At the same time, the vibration accumulation effect during the penetration process is reduced or eliminated through the coordinated speed of the chassis and the robot arm.

[0041] The offset of the component force of the normal offset of the robotic arm specifically includes: constructing a real-time mapping model from the contact force space to the joint torque space, decomposing the contact force detected by the six-dimensional force sensor into normal and tangential components, generating a reverse compensation torque for the normal offset component, and dynamically adjusting the end posture angle through joint torque closed-loop control to maintain the axial consistency between the penetration direction and the initial trajectory.

[0042] The vibration accumulation effect during the penetration process is reduced or eliminated through the speed coordination of the chassis and the robotic arm. Specifically, the equivalent mass-spring model of the robotic arm-chassis system is established, the energy accumulation mode during the penetration process is identified through the vibration frequency characteristics, and a speed feedforward compensation algorithm is used to inject a regulation component with an opposite phase to the robotic arm vibration into the chassis translation speed, thereby forming an energy cancellation effect within the mechanical system.

[0043] During the penetration phase, the vector synthesis relationship between the linear velocity of the robot arm end and the chassis movement speed is maintained, and the robot arm joint velocity distribution is adjusted in real time through the inverse kinematic solution. When the vibration amplitude is detected to exceed the safety threshold, the chassis is triggered to move in a slight reciprocating motion mode, and the system inertia force is used to offset the rebound impact of the flexible obstacle.

[0044] The dynamic selection of rigid obstacle avoidance strategies based on the physical characteristics of obstacles specifically includes:

[0045] S331. Detect a dynamic combination of the first physical characteristics of an obstacle.

[0046] The first physical property dynamic combination specifically includes: ΔD1, D2, F (t+Δt) 、F (t) ;

[0047] Obstacle deformation rate δ = (ΔD1 / D2) × 100%, contact force gradient

[0048] Among them, ΔD1 is specifically the displacement average of the feature points after continuous multi-frame point cloud matching of the binocular vision module integrated on the picking front end; D2 is the initial obstacle surface distance measured by the laser profile sensor integrated on the picking front end; F (t) is the contact force modulus of the six-dimensional force sensor integrated on the picking front end at time t; F (t+Δt) is the contact force modulus of the six-dimensional force sensor integrated on the picking front end at the moment (t+Δt).

[0049] S332: Dynamically analyze the obstacle deformation rate and the contact force gradient of the obstacle based on the first physical characteristic.

[0050] S333: When the deformation rate of the obstacle is lower than a preset value and the contact force gradient of the obstacle is higher than a preset value, it is determined that the obstacle constitutes a rigid block to the picking front end, and a rigid obstacle avoidance strategy is selected to be executed.

[0051] Dynamic selection of flexible penetration strategies based on the physical characteristics of obstacles specifically includes:

[0052] S341. Detect the dynamic combination of the second physical characteristics of the obstacle.

[0053] The second physical characteristic dynamic combination specifically includes: ΔL, L0, P r 、P t , d; deformation rate ε=(ΔL / L0)×100%; contact force fluctuation frequency α=20log10(P r / P t ) / d; where: ΔL is the amount of obstacle compression; L0 is the diameter of the initial contact area of ​​the pressure-sensitive array, P r is the millimeter-wave radar receiving power, Pt is the millimeter-wave radar transmitting power, and d is the penetration depth of the radar wave into the obstacle.

[0054] S342. Analyze the obstacle deformation rate and contact force fluctuation frequency through dynamic combination of the second physical characteristics.

[0055] S343. When the deformation rate is higher than the preset value and the contact force fluctuation frequency is lower than the preset value, the flexible penetration strategy is selected.

[0056] S4. Loop detection and strategy selection until the target position threshold range is reached.

[0057] After identifying the target pomelo, the present invention directly uses the trajectory of the straight line to the target pomelo as the original trajectory. There is no need for complex path planning or advance planning of avoidance strategies, which greatly saves computing power. At the same time, when encountering obstacles, the method of avoiding and circumventing them is not always adopted. For example, when encountering flexible obstructions caused by leaves, there is no need to re-plan the path, but a flexible penetration strategy, that is, a squeezing strategy, is adopted. For the scenario of pomelo trees, the thin branches and leaves occupy most of the obstructions. Using this method not only greatly improves the picking efficiency but also saves computing power.

[0058] The above are only preferred specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with this technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solutions and inventive concepts of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. A grapefruit picking optimization method based on hand-foot coordination, characterized in that: include: Generate an initial trajectory based on the target grapefruit position and the current position of the robot end effector; Move along the initial trajectory and detect obstacles on the path in real time; Dynamically select a rigid obstacle avoidance strategy or a flexible penetration strategy based on the physical characteristics of the obstacle. The detection and strategy selection are performed in a loop until the target position threshold range is reached.

2. The grapefruit picking optimization method according to claim 1, wherein The rigid obstacle avoidance strategy specifically includes: exiting the initial trajectory when a rigid obstacle is detected; regenerating the obstacle avoidance trajectory; and adjusting the joint space parameters of the robotic arm and the chassis displacement vector to adapt to the obstacle avoidance trajectory.

3. The grapefruit picking optimization method according to claim 2, wherein Adjusting the manipulator's joint spatial parameters involves inversely calculating the target configuration of each joint based on the geometric characteristics of the obstacle avoidance trajectory, and achieving spatial alignment between the end-point pose and the obstacle avoidance trajectory through closed-loop joint angle control. Furthermore, the manipulator's redundant degrees of freedom are synchronously decoupled, and joint motion priority constraints are established to prevent loss of control caused by singular configurations.

4. The grapefruit picking optimization method according to claim 3, wherein: Adjusting the chassis displacement vector specifically includes: calculating the chassis lateral compensation displacement based on the spatial offset of the obstacle avoidance trajectory, establishing a geometric projection relationship between the chassis motion vector and the trajectory of the robot end, and achieving lateral slip compensation through chassis wheel speed differential control to maintain the relative position of the robot arm workspace and the target grapefruit unchanged.

5. The grapefruit picking optimization method according to claim 4, wherein: When adjusting the spatial parameters of the robot arm joints and the chassis displacement vector, it is also necessary to verify the dynamic matching of the robot arm joint parameters and the chassis displacement vector within the kinematic framework. By feedback on the deviation between the actual trajectory of the end effector and the planned trajectory, the joint acceleration curve and the chassis velocity profile are iteratively optimized to ensure that there is no motion interference during the detour.

6. The grapefruit picking optimization method according to claim 1, wherein: The flexible penetration strategy specifically includes: when a flexible obstacle is detected, the robotic arm maintains its initial trajectory to penetrate the flexible obstacle; during the penetration process, the robotic arm's posture is dynamically adjusted according to the reaction force it receives to offset the component force that may cause the normal deviation of the robotic arm. At the same time, the vibration accumulation effect during the penetration process is reduced or eliminated through the coordinated speed of the chassis and the robotic arm.

7. The grapefruit picking optimization method according to claim 6, wherein: The offset of the component force of the normal offset of the robotic arm specifically includes: constructing a real-time mapping model from the contact force space to the joint torque space, decomposing the contact force detected by the six-dimensional force sensor into normal and tangential components, generating a reverse compensation torque for the normal offset component, and dynamically adjusting the end posture angle through joint torque closed-loop control to maintain the axial consistency between the penetration direction and the initial trajectory.

8. The grapefruit picking optimization method according to claim 6, wherein: The vibration accumulation effect during the penetration process is reduced or eliminated through the speed coordination of the chassis and the robotic arm. Specifically, the equivalent mass-spring model of the robotic arm-chassis system is established, the energy accumulation mode during the penetration process is identified through the vibration frequency characteristics, and a speed feedforward compensation algorithm is used to inject a regulation component with an opposite phase to the robotic arm vibration into the chassis translation speed, thereby forming an energy cancellation effect within the mechanical system.

9. The grapefruit picking optimization method according to claim 6, wherein: During the penetration phase, the vector synthesis relationship between the linear velocity of the robot arm end and the chassis movement speed is maintained, and the robot arm joint velocity distribution is adjusted in real time through the inverse kinematic solution. When the vibration amplitude is detected to exceed the safety threshold, the chassis is triggered to perform a slight reciprocating motion mode, and the system inertia force is used to offset the rebound impact of the flexible obstacle.

Images