A hand-foot cooperation-based grapefruit picking optimization method and system

Abstract

The application discloses a hand-foot coordination-based grapefruit picking optimization method, which comprises the following steps: generating an initial trajectory according to the position of a target grapefruit and the current position of an end effector of a mechanical arm; moving along the initial trajectory and detecting path obstacles in real time; dynamically selecting a rigid obstacle avoidance strategy completed by the mechanical arm and a chassis in cooperation or a flexible penetration strategy completed by the mechanical arm and the chassis in cooperation based on the physical characteristics of the obstacles; and cyclically performing detection and strategy selection until the target position threshold range is reached. The application can optimize the utilization of computing resources, enhance the dynamic environment adaptability, strengthen the cooperative control performance, improve the work efficiency and prolong the service life of the equipment.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent agricultural machinery, and particularly relates to a grapefruit picking optimization method and system based on hand-foot cooperation. BACKGROUND

[0002] The prior art traditional grapefruit picking robot generally adopts fixed path planning and single obstacle avoidance strategy, and has the following technical limitations: the existing method relies on a preset obstacle database and is difficult to cope with unstructured obstacles such as dynamic changes in branches and leaves in the orchard, fly interference, etc. When facing flexible obstacles, the mechanical detour strategy is still used, resulting in a long path and low work efficiency; the traditional mechanical arm and mobile platform adopt a separate control architecture, which is prone to motion interference during dynamic obstacle avoidance, lacks a vibration suppression mechanism when penetrating flexible obstacles, and causes the end effector pose to be unstable; real-time three-dimensional path planning in complex environments requires a large amount of computing resources, and frequent path re-planning can easily cause system delays, resulting in an increased picking failure rate, which needs to be improved. SUMMARY

[0003] To solve the technical problems in the background art, the present application provides a grapefruit picking optimization method based on hand-foot cooperation, comprising:

[0004] S1. Generating an initial trajectory according to the target grapefruit position and the current position of the end effector of the mechanical arm;

[0005] S2. Moving along the initial trajectory and detecting the path obstacles in real time;

[0006] S3. Dynamically selecting a rigid obstacle avoidance strategy completed by the mechanical arm and the chassis in cooperation or a flexible penetration strategy completed by the mechanical arm and the chassis in cooperation based on the physical properties of the obstacles;

[0007] S4. Detecting and selecting the strategy cyclically until the target position threshold range is reached.

[0008] Further, the rigid obstacle avoidance strategy specifically includes: when a rigid obstacle is detected, exiting the initial trajectory; generating an obstacle avoidance trajectory again; adjusting the joint space parameters of the mechanical arm and the displacement vector of the chassis to adapt to the obstacle avoidance trajectory.

[0009] Further, adjusting the joint space parameters of the mechanical arm specifically includes: based on the geometric features of the obstacle avoidance trajectory, inversely solving the target configuration of each joint of the mechanical arm, and achieving spatial matching of the end pose and the obstacle avoidance trajectory through joint angle closed-loop control. Decouple the redundant degrees of freedom of the mechanical arm synchronously, and establish joint motion priority constraints to avoid motion out of control caused by singular configurations.

[0010] Further, the adjusting the chassis displacement vector specifically comprises: calculating a chassis lateral compensation displacement according to a spatial offset of the obstacle avoidance trajectory, establishing a geometric projection relationship between the chassis motion vector and the mechanical arm end trajectory, and realizing lateral slip compensation through chassis wheel speed differential control to keep the relative pose between the mechanical arm workspace and the target pomelo unchanged.

[0011] Further, when adjusting the mechanical arm joint space parameters and the chassis displacement vector, the dynamic matching of the mechanical arm joint parameters and the chassis displacement vector needs to be verified in the kinematics framework, the joint acceleration curve and the chassis speed profile are iteratively optimized through the deviation feedback of the actual trajectory of the end effector and the planned trajectory to ensure that there is no motion interference during the detour process.

[0012] Further, the flexible penetration strategy specifically comprises: when a flexible obstacle is detected, the mechanical arm maintains the initial trajectory to penetrate the flexible obstacle; during the penetration process, the posture of the mechanical arm is dynamically adjusted according to the reaction force acting on the mechanical arm to offset the component force that may cause the normal deviation of the mechanical arm, and at the same time, the vibration accumulation effect in the penetration process is reduced or eliminated through the speed coordination of the chassis and the mechanical arm.

[0013] Further, the offsetting of the component force of the normal deviation of the mechanical arm specifically comprises: 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 an inverse compensation torque for the normal deviation 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] Further, the reducing or eliminating the vibration accumulation effect in the penetration process through the speed coordination of the chassis and the mechanical arm specifically comprises: establishing an equivalent mass-spring model of the mechanical arm-chassis system, identifying the energy accumulation mode in the penetration process through vibration frequency characteristics, and using a speed feedforward compensation algorithm to inject an adjustment component in the chassis translation speed that is opposite in phase to the mechanical arm vibration, forming an energy offset effect inside the mechanical system.

[0015] Further, during the penetration phase, the vector composition relationship between the mechanical arm end linear velocity and the chassis moving velocity is maintained, the mechanical arm joint speed distribution is adjusted in real time through kinematics inverse solution, and when the vibration amplitude is detected to exceed a safety threshold, a chassis micro-amplitude reciprocating motion mode is triggered to utilize the system inertia force to offset the flexible obstacle rebound impact.

[0016] The present application proposes a hand-foot cooperative grape picking optimization method, which avoids the calculation burden of continuous global path planning through a straight initial trajectory and an on-demand strategy switching mechanism; a flexible penetration strategy reduces the frequency of path re-planning, significantly reducing the system computing power demand; multi-modal perception fusion accurately distinguishes the characteristics of rigid / flexible obstacles, enabling intelligent strategy selection; a dynamic trajectory prediction model effectively deals with time-varying obstacles, improving decision foresight; a robot-chassis cooperative motion model actively offsets vibration energy, significantly suppressing end vibration during penetration; a normal offset compensation mechanism ensures the stability of the penetration direction, improving picking accuracy; the flexible penetration strategy significantly shortens the path length, significantly reducing the time-consuming of single fruit picking; the cooperative parameter adjustment mechanism ensures the real-time of strategy switching, maintaining the continuity of the operation; the buffer mechanism of the rigid obstacle avoidance strategy effectively reduces the impact force, the vibration elimination algorithm reduces the wear of mechanical parts, and enhances the durability of the system. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 The present application is a whole process schematic diagram;

[0018] Figure 2 The present application is a part of the process schematic diagram;

[0019] Figure 3 The present application is a part of the process schematic diagram;

[0020] Figure 4 The present application is a part of the process schematic diagram;

[0021] Figure 5 The present application is a part of the process schematic diagram. DETAILED DESCRIPTION

[0022] The present application proposes a hand-foot cooperative grape picking optimization method, which includes:

[0023] S1, generating an initial trajectory according to the target grape position and the current position of the robot end effector.

[0024] S11, obtaining the three-dimensional coordinates of the target grape through multi-sensor fusion, such as a vision camera combined with a laser range finder, the vision camera is used to identify the outline center point of the grape, and the outline center point is taken as the target point, the laser range finder measures the straight-line distance of the outline center point, and the target point coordinates are output after data fusion.

[0025] S12, real-time acquisition of the picking front-end state, 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, moving along the initial trajectory and detecting the path obstacles in real time.

[0027] The detection of the path obstacle specifically adopts multi-modal environment perception. A stereo vision module is deployed to collect three-dimensional point clouds in front of the trajectory in real time. A millimeter wave radar scans the depth information of the path space. A flexible tactile sensor is activated to monitor the change of the end contact force. The point cloud clustering analysis can distinguish between fixed obstacles such as branches and dynamic obstacles such as swaying branches and flying insects. A motion trajectory prediction model of the obstacle can be established to calculate the path occupation within the next few seconds.

[0028] S3, dynamically selecting a rigid obstacle avoidance strategy completed by the mechanical arm and the chassis in cooperation or a flexible penetration strategy completed by the mechanical arm and the chassis in cooperation based on the physical characteristics of the obstacle.

[0029] When the obstacle is a rigid obstacle, a rigid obstacle avoidance strategy is adopted. When the obstacle is a flexible obstacle, a flexible obstacle avoidance strategy is adopted. The rigid obstacle has the properties of non-deformability and high structural strength, and remains stable in geometric shape under external force, such as a pomelo tree trunk. The flexible obstacle has the properties of elastic deformation and low energy dissipation, and the contact process is accompanied by recoverable deformation, such as pomelo tree leaves and small branches.

[0030] The rigid obstacle avoidance strategy specifically includes:

[0031] S311, when a rigid obstacle is detected, the initial trajectory is exited.

[0032] S312, a new obstacle avoidance trajectory is generated.

[0033] S313, the joint space parameters of the mechanical arm and the displacement vector of the chassis are adjusted to adapt to the obstacle avoidance trajectory.

[0034] The adjustment of the joint space parameters of the mechanical arm specifically includes: based on the geometric characteristics of the obstacle avoidance trajectory, the target position of each joint of the mechanical arm is solved in reverse, and the spatial matching of the end position and the obstacle avoidance trajectory is realized through closed-loop control of the joint angle. The redundant degrees of freedom of the mechanical arm are decoupled synchronously, and the joint motion priority constraint is established to avoid motion out of control caused by singular position.

[0035] The adjustment of the displacement vector of the chassis specifically includes: the lateral compensation displacement of the chassis is calculated according to the spatial offset of the obstacle avoidance trajectory, and the geometric projection relationship between the motion vector of the chassis and the trajectory of the end of the mechanical arm is established. The lateral slip compensation is realized through differential control of the wheel speed of the chassis, and the relative position and pose of the working space of the mechanical arm and the target pomelo are kept unchanged.

[0036] When adjusting the joint space parameters of the mechanical arm and the displacement vector of the chassis, it is also necessary to verify the dynamic matching of the joint parameters of the mechanical arm and the displacement vector of the chassis in the kinematics framework. Through the deviation feedback of 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 process.

[0037] S314, make the mechanical arm end adopt an obstacle avoidance trajectory to make a detour.

[0038] The flexible penetration strategy specifically includes:

[0039] S321, when detecting a flexible obstacle, make the mechanical arm maintain the initial trajectory to penetrate the flexible obstacle.

[0040] S322, during the penetration process, dynamically adjust the posture of the mechanical arm according to the reaction force acting on the mechanical arm to offset the component force that may cause the normal deviation of the mechanical arm, and simultaneously reduce or eliminate the vibration accumulation effect in the penetration process through the speed coordination of the chassis and the mechanical arm.

[0041] The offsetting of the component force of the normal deviation of the mechanical 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 deviation component, 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.

[0042] The reduction or elimination of the vibration accumulation effect in the penetration process through the speed coordination of the chassis and the mechanical arm specifically includes: establishing an equivalent mass-spring model of the mechanical arm-chassis system, identifying the energy accumulation mode in the penetration process through vibration frequency characteristics, using a speed feedforward compensation algorithm to inject an adjustment component with a phase opposite to that of the mechanical arm vibration in the chassis translation speed, and forming an energy offset effect inside the mechanical system.

[0043] Maintain the vector composition relationship between the linear velocity of the mechanical arm end and the movement speed of the chassis during the penetration phase, adjust the joint speed distribution of the mechanical arm in real time through inverse kinematics, and when the vibration amplitude is detected to exceed the safety threshold, trigger the chassis micro-amplitude reciprocating motion mode, and use the system inertia force to offset the rebound impact of the flexible obstacle.

[0044] Wherein, dynamically selecting a rigid obstacle avoidance strategy based on the physical characteristics of the obstacle specifically includes:

[0045] S331, detecting the dynamic combination of the first physical characteristics of the obstacle.

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

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

[0048] Wherein, AD1 is the average displacement of 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 spacing measured by the laser profile sensor integrated on the picking front end; F (t) is the contact force module value of the six-dimensional force sensor integrated on the picking front end at time t. (t+Δt) is the contact force module value of the six-dimensional force sensor integrated on the picking front end at (t+Δt) time.

[0049] S332, dynamically combining the obstacle deformation rate and the contact force gradient of the obstacle according to the first physical property.

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

[0051] The flexible penetration strategy based on the dynamic selection of the physical properties of the obstacle specifically includes:

[0052] S341, detecting the second physical property dynamic combination of the obstacle.

[0053] The second physical property dynamic combination specifically includes: AL, L0, P r , P t , d; deformation rate ε=(AL / L0)×100%; contact force fluctuation frequency α=20log10(P r / P t ) / d; wherein: AL is the compression amount of the obstacle; L0 is the initial contact area diameter 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 radar wave penetration depth of the obstacle.

[0054] S342, analyzing the obstacle deformation rate and the contact force fluctuation frequency through the second physical property dynamic combination.

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

[0056] S4, the detection and strategy selection are executed in a loop until the target position threshold range is reached.

[0057] The application directly takes the trajectory of a straight line reaching the target pomegranate as the original trajectory after identifying the target pomegranate, does not need to perform complex path planning, and does not need to plan an avoidance strategy in advance, thereby greatly saving computing power, and when encountering an obstacle, does not always adopt an avoidance bypassing manner, for example, when encountering flexible blocking caused by leaves, path planning does not need to be performed again, but a flexible penetration strategy, i.e., a squeezing strategy, is adopted, for the pomegranate tree scene, thin branches and leaves occupy most of the blocking, and the adoption of the strategy greatly improves picking efficiency and saves computing power.

[0058] The above is only a preferred specific embodiment of the application, but the protection scope of the application is not limited thereto, any person skilled in the art can make equivalent replacements or changes according to the technical solution and the inventive concept of the application within the technical range disclosed by the application, which should be covered within the protection scope of the application.

Claims

1. A hand-foot coordination-based grapefruit picking optimization method, characterized in that, The method comprises the following steps: generating an initial trajectory according to the target position of the grapefruit and the current position of the end effector of the robot arm; moving along the initial trajectory and detecting path obstacles in real time; dynamically selecting a rigid obstacle avoidance strategy completed by the robot arm and the chassis or a flexible penetration strategy completed by the robot arm and the chassis based on the physical characteristics of the obstacles; repeatedly performing the detection and strategy selection until the target position threshold range is reached; the rigid obstacle avoidance strategy specifically comprises: when a rigid obstacle is detected, exiting the initial trajectory; regenerating an obstacle avoidance trajectory; adjusting the joint space parameters of the robot arm and the displacement vector of the chassis to adapt to the obstacle avoidance trajectory; the flexible penetration strategy specifically comprises: when a flexible obstacle is detected, causing the robot arm to maintain the initial trajectory to penetrate the flexible obstacle; during the penetration process, dynamically adjusting the posture of the robot arm according to the reaction force acting on the robot arm to offset the component force that may cause the normal deviation of the robot arm, and simultaneously reducing or eliminating the vibration accumulation effect in the penetration process through the speed coordination of the chassis and the robot arm; the offsetting of the component force of the normal deviation of the robot arm specifically comprises: 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 deviation component, 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; during the penetration phase, maintaining the vector composition relationship between the linear velocity of the end effector of the robot arm and the moving speed of the chassis, adjusting the joint velocity distribution of the robot arm in real time through inverse kinematics, and triggering the chassis micro-amplitude reciprocating motion mode when the vibration amplitude exceeds the safety threshold to offset the rebound impact of the flexible obstacle by using the inertial force of the system.

2. The method of claim 1, wherein, adjusting the joint space parameters of the robot arm specifically comprises: based on the geometric characteristics of the obstacle avoidance trajectory, inversely solving the target configuration of each joint of the robot arm, and realizing the spatial matching of the end posture and the obstacle avoidance trajectory through joint angle closed-loop control; simultaneously decoupling the redundant degrees of freedom of the robot arm, and establishing joint motion priority constraints to avoid motion out of control caused by singular configurations.

3. The method of claim 2, wherein, adjusting the displacement vector of the chassis specifically comprises: calculating the lateral compensation displacement of the chassis according to the spatial offset of the obstacle avoidance trajectory, establishing the geometric projection relationship between the motion vector of the chassis and the trajectory of the end effector of the robot arm, and realizing lateral slip compensation through differential control of the wheel speed of the chassis to maintain the relative posture between the working space of the robot arm and the target grapefruit.

4. The method of claim 3, wherein, When adjusting the joint space parameters of the robot arm and the displacement vector of the chassis, it is also necessary to verify the dynamic matching of the joint parameters of the robot arm and the displacement vector of the chassis within the kinematic framework, iteratively optimize the joint acceleration curve and the chassis speed profile through the deviation feedback of the actual trajectory of the end effector and the planned trajectory, and ensure that there is no motion interference during the detour process.

5. The method of claim 1, wherein, reducing or eliminating the vibration accumulation effect in the penetration process through the speed coordination of the chassis and the robot arm specifically comprises: establishing an equivalent mass-spring model of the robot arm-chassis system, identifying the energy accumulation mode in the penetration process through vibration frequency characteristics, and using a speed feedforward compensation algorithm to inject an adjustment component in the opposite phase of the robot arm vibration into the translation speed of the chassis, thereby forming an energy offset effect within the mechanical system.

Images