Shallot picking device control method based on multi-sensor fusion and picking device

Abstract

The present application provides a kind of based on multi-sensor fusion's control method and picking equipment of fragrant pear picking equipment, it is related to mechanical arm control technical field, the present application establishes the mapping relationship of mechanical arm space parameter and picking executor position in fragrant pear picking equipment, obtains the initial position of picking executor to as current path point, and determines picking position based on the position of fragrant pear to be picked, determine the shortest picking path and single movement step in combination with initial position and picking position;Determine a plurality of preselected trajectory points based on current path point, and select the optimal picking executor position from preselected trajectory points based on position relationship and control energy consumption, adjust current trajectory point to the mechanical arm space parameter corresponding to picking executor position based on PID algorithm, real-time update current path point, repeat iteration to determine next path point, until picking executor reaches picking position, efficient, smooth and low control energy consumption control of mechanical arm in fragrant pear picking process.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm control, in particular to a pear picking device control method based on multi-sensor fusion and a picking device. BACKGROUND

[0002] Pear is one of the characteristic fruits in China. Its picking operation has long relied on manual work, which is low in efficiency, high in labor intensity and high in cost. With the advancement of agricultural modernization, automated picking devices have become an important development direction for intelligent management of orchards. Pear picking automation is an important research direction in the field of agricultural robots, aiming to achieve accurate and efficient harvesting of fruits through mechanical arms and control algorithms. Traditional picking devices rely on pre-programmed trajectories or simple position control based on vision, which can achieve automated operation to some extent, but generally lack comprehensive optimization of multi-joint motion coordination and system control energy consumption. With the development of precision agriculture, the adaptability, stability and energy efficiency of picking devices in orchard environments have higher requirements.

[0003] However, existing picking control methods often ignore the energy loss and motion instability problems caused by multi-degree-of-freedom motion of mechanical arms while achieving high-precision positioning. Such devices often lack optimization of multi-joint motion coordination of mechanical arms, resulting in shaking and response lag phenomena during motion, which not only affects picking accuracy, but also causes high system energy consumption.

[0004] The above information disclosed in the background section is only used to enhance the understanding of the background of the present disclosure, and therefore it can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0005] The purpose of the present application is to provide a pear picking device control method based on multi-sensor fusion and a picking device to solve the problems raised in the background.

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

[0007] A pear picking device control method based on multi-sensor fusion, the specific steps include:

[0008] Step 1: Establish the mapping relationship between the mechanical arm space parameters and the picking actuator position in the pear picking device. The mechanical arm space parameters collected by the sensor include the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle twisted by the twisting joint and the angle tilted by the tilting joint;

[0009] Step 2: Obtain the initial position of the picking executor as the current path point, and determine the picking position based on the position of the pear to be picked, and determine the shortest picking path and single movement step length based on the initial position and picking position;

[0010] Step 3: Determine a plurality of pre-selected trajectory points with the current path point as the center and the movement step length as the radius, and select candidate trajectory points from them that are closer to the picking position than the current path point, determine the mechanical arm space parameter change corresponding to each candidate trajectory point based on the mapping relationship, and determine the control energy consumption to reach each candidate trajectory point based on the mechanical arm space parameter change;

[0011] Step 4: Determine the projection length of the line segment between each candidate trajectory point and the current path point on the shortest picking path, and determine the fitness value of each candidate trajectory point based on the single movement step length and the control energy consumption, select the candidate trajectory point corresponding to the minimum fitness value as the next path point, and adjust the mechanical arm space parameters based on the PID algorithm until the picking executor moves to the next path point;

[0012] Step 5: Real-time update the current path point, repeat iteration to determine the next path point, until the picking executor reaches the picking position.

[0013] Further, the mechanical arm space parameters include: the distance of the bottom translational joint movement, the distance of the telescopic joint movement, the angle of the torsion joint twist, and the angle of the pitch joint pitch. A space coordinate system is established with the center of the pear picking device base as the origin;

[0014] The mapping relationship is:

[0015] ;

[0016] Wherein, is the position coordinate of the pear picking executor, is the distance from the pitch joint axis to the bottom translational joint axis, is the distance from the telescopic joint axis to the bottom translational joint axis, is the distance of the bottom translational joint movement, is the distance of the telescopic joint movement, is the angle of the torsion joint twist, is the angle of the pitch joint pitch.

[0017] Further, the logic for generating pre-selected trajectory points is: taking the current path point as the center and the movement step length as the radius to make a circle, selecting N points on the circle as pre-selected trajectory points, and rotating the circle by 2 degrees each time to generate N new pre-selected trajectory points, until the circle is rotated 89 times, a total of 90 Given N pre-selected trajectory points, calculate the distance between each pre-selected trajectory point and the picking location, and compare it with the distance between the current path point and the picking location. If the distance between the pre-selected trajectory point and the picking location is less than the distance between the current path point and the picking location, then select the pre-selected trajectory point as a candidate trajectory point.

[0018] Furthermore, for each candidate trajectory point, within the feasible range of the robot arm's spatial parameters, the robot arm's spatial parameters are solved based on the mapping relationship to obtain at least one combination of robot arm spatial parameters, forming a solution set of robot arm spatial parameters. If the solution set is empty, the corresponding candidate trajectory point is canceled. Each solution in the solution set of robot arm spatial parameters includes the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle of twisting of the torsional joint, and the angle of pitch of the pitch joint. The change in the robot arm's spatial parameters of each solution in the solution set relative to the robot arm's spatial parameters of the current path point is calculated, and the change in each solution is constituted.

[0019] Furthermore, the initial robotic arm spatial parameters, changes in the robotic arm spatial parameters, and corresponding control energy consumption data from the historical operation of the same robotic arm are obtained. Using the initial robotic arm spatial parameters and changes in the robotic arm spatial parameters as inputs, and the corresponding control energy consumption as labels, a control energy consumption prediction model is constructed and trained. For a candidate trajectory point, the changes in each solution in the solution set corresponding to the candidate trajectory point position and the robotic arm spatial parameters of the current trajectory point are input into the control energy consumption prediction model to obtain the control energy consumption corresponding to each solution. The solution with the lowest control energy consumption is selected from the solution set as the robotic arm spatial parameters of that trajectory point.

[0020] Furthermore, the formula for calculating fitness is:

[0021] ;

[0022] in, For the first The fitness of each candidate trajectory point For the current trajectory point to reach the th Control energy consumption of each candidate trajectory point The step size for a single movement. For the first The projection length of the line connecting each candidate trajectory point and the current path point on the shortest picking path.

[0023] This invention also provides a pear harvesting device based on multi-sensor fusion, characterized in that: the harvesting device is used to implement the aforementioned pear harvesting device control method based on multi-sensor fusion, specifically including:

[0024] A pose mapping module is configured to establish a mapping relationship between a mechanical arm spatial parameter and a picking actuator position in a pear picking device, and the mechanical arm spatial parameter collected by a sensor includes a distance of a bottom prismatic joint movement, a distance of a telescopic joint movement, an angle of a twist joint twist, and an angle of a pitch joint pitch.

[0025] A position analysis module is configured to obtain an initial position of the picking actuator as a current path point, and determine a picking position based on a position of a pear to be picked, and determine a shortest picking path and a single movement step length based on the initial position and the picking position.

[0026] A trajectory screening module is configured to determine a plurality of preselected trajectory points with the current path point as a center and the movement step length as a radius, and screen out candidate trajectory points close to the picking position from the plurality of preselected trajectory points, determine a mechanical arm spatial parameter variation corresponding to each candidate trajectory point based on the mapping relationship, and determine a control energy consumption for reaching each candidate trajectory point based on the mechanical arm spatial parameter variation.

[0027] A trajectory optimization module is configured to determine a projection length of a line segment between each candidate trajectory point and the current path point on the shortest picking path, and determine an adaptability value of each candidate trajectory point based on the single movement step length and the control energy consumption, select a candidate trajectory point corresponding to a minimum adaptability value as a next path point, and adjust the mechanical arm spatial parameter based on a PID algorithm until the picking actuator position moves to the next path point.

[0028] A control iteration module is configured to update the current path point in real time, and repeat iteration to determine the next path point until the picking actuator reaches the picking position.

[0029] Compared with the prior art, the present application has the following advantages:

[0030] The present application establishes a mapping relationship between a multi-joint movement parameter of a mechanical arm and a position of an end effector, and combines a trajectory optimization algorithm and a control energy consumption prediction algorithm to achieve efficient, stable and low control energy consumption control of the mechanical arm in a pear picking process. The method can quickly select a motion path with the best comprehensive performance from a plurality of candidate trajectories, effectively reduces invalid joint movement and system energy loss, and enhances the adaptability and control accuracy of the mechanical arm in a complex space. In addition, the introduction of a closed-loop PID control and an online parameter updating mechanism further improves the stability and reliability of the picking process, and provides key technical support for long-term deployment and application of automatic picking equipment. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 It is a whole method flowchart of the present application;

[0032] Figure 2 It is an energy consumption comparison chart of the present application and a traditional method;

[0033] Figure 3 The whole system structure schematic diagram of the present application. DETAILED DESCRIPTION

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

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

[0036] Embodiment:

[0037] Please refer to Figures 1-2 The present application provides a technical solution:

[0038] A control method for a multi-sensor fusion-based fragrant pear picking device, comprising the following specific steps:

[0039] Step 1: Establish the mapping relationship between the spatial parameters of the mechanical arm in the fragrant pear picking device and the position of the picking actuator. The spatial parameters of the mechanical arm collected by the sensor include the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle twisted by the twisting joint and the angle tilted by the tilting joint.

[0040] Further, the spatial parameters of the mechanical arm include the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle twisted by the twisting joint, and the angle tilted by the tilting joint. A spatial coordinate system is established with the center of the base of the fragrant pear picking device as the origin, the positive east direction of the base plane as the X-axis, the positive south direction as the Y-axis, and the vertical upward direction as the Z-axis. For single control, the position of the base is not changed, only the mechanical arm is moved to achieve the purpose of single picking.

[0041] The distance of the bottom translational joint movement and the distance of the telescopic joint movement are collected using a linear displacement sensor. Specifically, the fixed end of the linear displacement sensor is installed on the base of the mechanical arm, and the measurement end is connected to the moving platform of the translational joint. When the platform moves, the sensor directly measures the linear distance of its movement relative to the base. The main body of the linear displacement sensor is fixed on the fixed sleeve of the telescopic arm, and the measurement end is connected to the end of the telescurable rod. When the telescurable rod extends or retracts, the sensor directly measures the length change.

[0042] An absolute rotary encoder is used to measure the angle of the twist joint and the angle of the pitch joint. An optical absolute encoder or a magneto-absolute encoder is used. The absolute encoder is crucial because it can provide the absolute angular position of the joint immediately upon power-up without the need for reset. Specifically, the absolute rotary encoder is directly installed at the rear end of the joint twist and pitch joint motor. The absolute rotary encoder is coaxially installed with the servo motor that drives the joint, directly measuring the angle of the twist joint twist and the angle of the pitch joint pitch.

[0043] A backstepping control algorithm based on Lyapunov stability theory is used to optimize the motion control of the mechanical arm and improve its trajectory tracking accuracy. By establishing similar mechanical arm forward kinematics calculation formula, and establishing a model on Matlab to test the range that the mechanical arm can reach in space, to determine the mapping relationship;

[0044] The mapping relationship is:

[0045] ;

[0046] wherein, is the position coordinate of the picking executor, is the distance from the pitch joint axis to the bottom translational joint axis, is the distance from the telescopic joint axis to the bottom translational joint axis, is the distance of the bottom translational joint movement, is the distance of the telescopic joint movement, is the angle of the twist joint twist, is the angle of the pitch joint pitch. 、 is a fixed parameter, not a variable.

[0047] Step 2: Obtain the initial position of the picking executor as the current path point, and determine the picking position based on the position of the pear to be picked, and determine the shortest picking path and single movement step based on the initial position and picking position;

[0048] The determination logic of the picking position is: a line is established between the center point of the pear to be picked and the center point of the picking executor in the initial position, and the intersection of this line and the boundary of the pear region is called the picking position.

[0049] The shortest picking path is a line connecting the current path point and the picking position; the single movement step is determined according to the shortest picking path, specifically:

[0050]

[0051] wherein, is the single movement step, is the length of the shortest picking path, is the preset movement step number, in the embodiment, .

[0052] Step 3: determining a plurality of preselected trajectory points with the current path point as the center and the movement step as the radius, and screening candidate trajectory points close to the picking position from the preselected trajectory points, determining the mechanical arm space parameter change amount corresponding to each candidate trajectory point based on the mapping relationship, and determining the control energy consumption to reach each candidate trajectory point based on the mechanical arm space parameter change amount;

[0053] Further, the logic for generating the preselected trajectory points is: taking the current path point as the center and the movement step as the radius to make a circle, selecting N points on the circle at equal intervals as preselected trajectory points, and rotating the circle by 2 degrees each time in the clockwise direction to generate N new preselected trajectory points, until the circle is rotated 89 times, a total of 90 N preselected trajectory points, calculating the distance between each preselected trajectory point and the picking position, and comparing it with the distance between the current path point and the picking position, if the distance between the preselected trajectory point and the picking position is less than the distance between the current path point and the picking position, the preselected trajectory point is selected as the candidate trajectory point. Through the screening condition (the distance between the new point and the target point must be less than the distance between the current point and the target point), the algorithm forces the movement trajectory of the mechanical arm to always be inside a “converging sphere” with the target point as the center and the current distance as the radius. This ensures that each step of the path is effective and approaching the target, completely avoiding invalid exploration or circular paths that may occur in complex spaces. In the 90 N preselected points generated, there may be as many as half of the points deviating from the target or causing backtracking. This step of screening immediately eliminates all these invalid options, reducing the number of “candidate points” that need to be calculated in the subsequent complex energy consumption calculation to a smaller set. This significantly reduces the computational load.

[0054] ​Further, for each candidate trajectory point, the feasible range of the robot arm space parameters is obtained from the robot arm specification, and the robot arm space parameters are solved based on the mapping relationship to obtain at least one solution of the robot arm space parameter combination, forming a solution set of the robot arm space parameters, if the solution set is empty, the corresponding candidate trajectory point is cancelled, each solution in the solution set of the robot arm space parameters includes the movement distance of the bottom translational joint, the movement distance of the extension joint, the twist angle of the twist joint and the tilt angle of the tilt joint, and the change amount of the robot arm space parameters in each solution in the solution set of the robot arm space parameters is calculated relative to the robot arm space parameters of the current path point, and the change amount of each solution is formed.

[0055] The solving of the robot arm space parameters based on the mapping relationship can adopt existing technologies, including but not limited to inverse kinematics solving, and the following is the solving process of inverse kinematics solving:

[0056] The feasible range of the robot arm space parameters is obtained from the robot arm specification, and the constraint condition is set based on the feasible range, the candidate trajectory point coordinates are input, and the target coordinates are substituted into the mapping relationship to obtain a nonlinear equation set composed of three equations. The equation set contains four parameters to be solved, including the movement distance of the bottom translational joint, the movement distance of the extension joint, the twist angle of the twist joint and the tilt angle of the tilt joint; the nonlinear equation set is solved by algebraic transformation to obtain a solution set, and the solutions in the solution set that meet the constraint condition are formed into a solution set of the robot arm space parameters, if there is no solution that meets the constraint condition, the solution set is empty, and the corresponding candidate trajectory point is cancelled.

[0057] The change amount of the robot arm space parameters includes the change amount of the movement distance of the bottom translational joint, the change amount of the movement distance of the extension joint, the change amount of the twist angle of the twist joint and the change amount of the tilt angle of the tilt joint.

[0058] Further, the initial robot arm space parameters, the change amount data of the robot arm space parameters and the corresponding control energy consumption data in the historical operation of the same robot arm are obtained, the initial robot arm space parameters and the change amount of the robot arm space parameters are input, and the corresponding control energy consumption is labeled to construct and train a control energy consumption prediction model, the optimal robot arm space parameters of each candidate trajectory point, the change amount of the robot arm space parameters of the current trajectory point and the robot arm space parameters of the current trajectory point are input into the trained control energy consumption prediction model to obtain the control energy consumption of the current trajectory point to the candidate trajectory point, and the solution with the lowest control energy consumption is selected from the solution set as the optimal robot arm space parameters of the trajectory point.

[0059] 80% of the historical initial robot arm space parameters, the change amount data of the robot arm space parameters and the corresponding control energy consumption data are used as the training set, and 20% are used as the test set;

[0060] The control energy consumption prediction model can be constructed and trained using existing technologies. The control energy consumption prediction model uses a feedforward neural network with initial robot spatial parameters and changes in robot spatial parameters as inputs, and corresponding control energy consumption as labels. Training the control energy consumption prediction model specifically includes: an input layer, a hidden layer, an output layer, and an activation function. The input layer is responsible for receiving the initial robot spatial parameters and changes in robot spatial parameters. The hidden layer is used for data processing of the initial robot spatial parameters and changes in robot spatial parameters. It is composed of multiple layers, each containing multiple time nodes. Each hidden layer time node is connected to the previous layer through weights, used for feature abstraction and nonlinear transformation of the input initial robot spatial parameters and changes in robot spatial parameters. By using the ReLu activation function, a nonlinear relationship is introduced to enable the model to fit complex feature relationships. An independent neuron is set in the output layer to convert the local and high-level feature representations extracted by the hidden layer for output corresponding control energy consumption. The root mean square error loss function is used. The input data is calculated through the network once to get the output result. The loss function is calculated according to the predicted value and the true value. The gradient of the loss function to each weight and bias is calculated through the chain rule. The network weights and biases are updated using the gradient descent algorithm to minimize the loss function. In this embodiment, the training parameters are set as follows:

[0061] Loss function: root mean square error loss function

[0062] Optimizer: Adam, initial learning rate 0.001

[0063] Batch size: 32, number of training rounds: 1000 rounds

[0064] Use early stopping strategy to prevent overfitting.

[0065] Step 4: Determine the projection length of the line between each candidate trajectory point and the current path point on the shortest picking path, and determine the fitness value of each candidate trajectory point based on the single movement step and control energy consumption. Select the candidate trajectory point corresponding to the minimum fitness value as the next path point. Adjust the robot spatial parameters based on the PID algorithm until the picking executor position moves to the next path point.

[0066] Further, the fitness formula is:

[0067] ;

[0068] Where, is the fitness of the th candidate trajectory point, is the control energy consumption from the current trajectory point to the th candidate trajectory point, is a single moving step, is the projection length of the line segment between the candidate trajectory point and the current path point on the shortest picking path. is the projection length of the line segment between the candidate trajectory point and the current path point on the shortest picking path.

[0069] is to normalize the projection length of the line segment between the candidate trajectory point and the current path point on the shortest picking path, represents the control energy consumption per unit projection length, The greater the value, the greater the control energy consumption per unit length. The candidate trajectory point corresponding to the minimum fitness value is selected as the next path point. In order to select the control strategy with the minimum control energy, for each control, if the single control energy is the lowest, but the distance to the target position is farther, the number of controls will increase. For the whole control, the control energy does not decrease. This embodiment maps each candidate trajectory point to the shortest path and calculates the unit energy of each control based on the unit energy for control, in order to avoid local optimization, but the global control energy is actually increased.

[0070] When adjusting the spatial parameters of the robot arm based on the PID algorithm, the adjustment starts from the bottom up. When the lower joint is adjusted to the appropriate position, the upper joint adjustment is performed. The spatial parameters of the candidate trajectory point corresponding to the minimum fitness value are used as the target value. The PID controller calculates the control amount in real time according to the error between the current joint state feedback by the sensor and the target value: the proportional gain quickly responds to the error, the integral gain eliminates the static error, and the differential gain suppresses the overshoot. The control amount drives the joint motor or actuator to move, gradually adjusts the robot posture, and finally the picking actuator accurately reaches the target trajectory point. This process is realized layer by layer for each joint, and the bottom joint is adjusted first, then the upper joint is adjusted, to ensure smooth movement. This is the prior art and will not be described here.

[0071] Step 5: Real-time update of the current path point, repeated iteration to determine the next path point, until the picking actuator reaches the picking position.

[0072] Please refer to Figure 2 In this embodiment, the scheme (candidate trajectory optimization control) in this embodiment and the traditional scheme (pre-programmed trajectory control) are used, and the same picking control is performed using the above two schemes. The picking control energy is shown in Figure 2 The energy consumption of the scheme in this embodiment is reduced by 9.78% compared with the traditional scheme.

[0073] Please refer to Figure 3The application further provides a multi-sensor fusion-based fragrant pear picking device, characterized in that: the picking device is used to realize the multi-sensor fusion-based fragrant pear picking device control method, and specifically comprises:

[0074] A pose mapping module is configured to establish a mapping relationship between a mechanical arm space parameter and a picking actuator position in the fragrant pear picking device, and the mechanical arm space parameter collected by the sensor includes a distance of a bottom prismatic joint movement, a distance of a telescopic joint movement, an angle of a twist joint twist, and an angle of a pitch joint pitch.

[0075] A position analysis module is configured to acquire an initial position of the picking actuator as a current path point, determine a picking position based on a position of a fragrant pear to be picked, and determine a shortest picking path and a single movement step length in combination with the initial position and the picking position.

[0076] A trajectory screening module is configured to determine a plurality of preselected trajectory points with the current path point as a center and the movement step length as a radius, screen out candidate trajectory points close to the picking position from the plurality of preselected trajectory points, determine a mechanical arm space parameter variation amount corresponding to each candidate trajectory point based on the mapping relationship, and determine a control energy consumption for reaching each candidate trajectory point based on the mechanical arm space parameter variation amount.

[0077] A trajectory optimization module is configured to determine a projection length of a line segment between each candidate trajectory point and the current path point on the shortest picking path, determine an adaptability value of each candidate trajectory point in combination with the single movement step length and the control energy consumption, select a candidate trajectory point corresponding to a minimum adaptability value as a next path point, adjust the mechanical arm space parameter based on a PID algorithm, and move the picking actuator position to the next path point.

[0078] A control iteration module is configured to update the current path point in real time, repeat iteration to determine the next path point, and move the picking actuator to the picking position.

[0079] The above formulas are all dimensionless values, and the formulas are obtained by simulating a nearest real situation through a large amount of data, and preset parameters in the formulas are set by a person skilled in the art according to actual conditions.

[0080] The above embodiments can be realized by software, hardware, firmware or any combination thereof, in whole or in part. When realized by software, the above embodiments can be realized in the form of a computer program product in whole or in part. A person skilled in the art can realize that units and algorithm steps of examples described in combination with the embodiments disclosed herein can be realized by electronic hardware or a combination of computer software and electronic hardware. Whether the functions are realized by hardware or software methods depends on specific application and design constraints of the technical solutions.

[0081] The units described as separate components may or may not be physically separate, and the components displayed as units may or may not be physical units, and may be located in one place, or distributed on multiple network units. Part or all of the units can be selected to achieve the purpose of the embodiment of the present application according to actual needs.

[0082] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto, any person skilled in the art cannot easily think of changes or replacements within the technical range disclosed by the present application, and should be covered within the protection scope of the present application.

Claims

1. A control method for a pear harvesting device based on multi-sensor fusion, characterized in that, The specific steps include: Step 1: Establish the mapping relationship between the spatial parameters of the robotic arm and the position of the picking actuator in the pear picking equipment. The spatial parameters of the robotic arm collected by the sensor include the distance of movement of the bottom translational joint, the distance of movement of the telescopic joint, the angle of twisting of the torsional joint, and the angle of pitch of the pitch joint. Step 2: Obtain the initial position of the picking actuator as the current path point, and determine the picking position based on the location of the pear to be picked. Combine the initial position and the picking position to determine the shortest picking path and the single movement step size. Step 3: Using the current path point as the center and the moving step size as the radius, determine multiple pre-selected trajectory points, and select candidate trajectory points that are closer to the picking position than the current path point. Based on the mapping relationship, determine the change in the robotic arm spatial parameters corresponding to each candidate trajectory point, and determine the control energy consumption to reach each candidate trajectory point based on the change in the robotic arm spatial parameters. Step 4: Determine the projection length of the line connecting each candidate trajectory point and the current path point on the shortest picking path, and determine the fitness value of each candidate trajectory point by combining the single movement step size and control energy consumption. Select the candidate trajectory point corresponding to the minimum fitness value as the next path point, and adjust the spatial parameters of the robotic arm based on the PID algorithm until the picking actuator moves to the next path point. Step 5: Update the current path point in real time, repeat the iteration to determine the next path point, until the picking executor reaches the picking position; The formula for calculating fitness is: in, For the first The fitness of each candidate trajectory point For the current trajectory point to reach the th Control energy consumption of each candidate trajectory point The step size for a single movement. For the first The projection length of the line connecting each candidate trajectory point and the current path point on the shortest picking path.

2. The control method for a pear harvesting device based on multi-sensor fusion according to claim 1, characterized in that: The spatial parameters of the robotic arm include: the distance the bottom translational joint moves, the distance the telescopic joint moves, the angle of the torsion joint twists, and the angle of the pitch joint pitches; a spatial coordinate system is established with the center of the base of the pear picking equipment as the origin; The mapping relationship is as follows: in, The coordinates of the position of the pear picking actuator. This is the distance from the pitch joint axis to the bottom translation joint axis. This is the distance from the extension joint axis to the bottom translational joint axis. This represents the distance the bottom translational joint moves. This refers to the distance the joint can move. To the angle of joint torsion, The pitch angle of the pitch joint.

3. The control method for a pear harvesting device based on multi-sensor fusion according to claim 1, characterized in that, The logic for generating pre-selected trajectory points is as follows: Using the current path point as the center and the movement step size as the radius, draw a circle. Select N points at equal intervals on the circle as pre-selected trajectory points. Rotate the circle clockwise by 2 degrees each time to generate N new pre-selected trajectory points, continuing this process 89 times until a total of 90 points are obtained. Given N pre-selected trajectory points, calculate the distance between each pre-selected trajectory point and the picking location, and compare it with the distance between the current path point and the picking location. If the distance between the pre-selected trajectory point and the picking location is less than the distance between the current path point and the picking location, then select the pre-selected trajectory point as a candidate trajectory point.

4. The control method for a pear harvesting device based on multi-sensor fusion according to claim 3, characterized in that: For each candidate trajectory point, within the feasible range of the robot arm's spatial parameters, the robot arm's spatial parameters are solved based on the mapping relationship to obtain at least one combination of robot arm spatial parameters, forming a solution set of robot arm spatial parameters. If the solution set is empty, the corresponding candidate trajectory point is canceled. Each solution in the solution set of robot arm spatial parameters includes the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle of twisting of the torsional joint, and the angle of pitch of the pitch joint. The change in the robot arm's spatial parameters of each solution in the solution set relative to the robot arm's spatial parameters of the current path point is calculated, and the change in each solution is constituted.

5. The control method for a pear harvesting device based on multi-sensor fusion according to claim 1, characterized in that: The initial robotic arm spatial parameters, changes in the robotic arm spatial parameters, and corresponding control energy consumption data are obtained from the historical operation of the same robotic arm. Using the initial robotic arm spatial parameters and changes in the robotic arm spatial parameters as inputs and the corresponding control energy consumption as labels, a control energy consumption prediction model is constructed and trained. For a candidate trajectory point, the changes in each solution in the solution set corresponding to the candidate trajectory point position and the robotic arm spatial parameters of the current trajectory point are input into the control energy consumption prediction model to obtain the control energy consumption corresponding to each solution. The solution with the lowest control energy consumption is selected from the solution set as the optimal robotic arm spatial parameters for that trajectory point.

6. A pear-picking device based on multi-sensor fusion, characterized in that: The harvesting equipment is used to implement the pear harvesting equipment control method based on multi-sensor fusion as described in any one of claims 1-5, specifically including: The posture mapping module is used to establish the mapping relationship between the spatial parameters of the robotic arm and the position of the picking actuator in the pear picking equipment. The spatial parameters of the robotic arm collected by the sensor include the distance of movement of the bottom translational joint, the distance of movement of the telescopic joint, the angle of twisting of the torsional joint, and the angle of pitch of the pitch joint. The position analysis module is used to obtain the initial position of the picking actuator as the current path point, and determine the picking position based on the location of the pear to be picked. It combines the initial position and the picking position to determine the shortest picking path and the single movement step size. The trajectory filtering module is used to determine multiple pre-selected trajectory points with the current path point as the center and the moving step size as the radius, and to filter out candidate trajectory points that are closer to the picking position than the current path point. Based on the mapping relationship, the module determines the change in the robotic arm spatial parameters corresponding to each candidate trajectory point, and determines the control energy consumption to reach each candidate trajectory point based on the change in the robotic arm spatial parameters. The trajectory optimization module is used to determine the projection length of the line connecting each candidate trajectory point and the current path point on the shortest picking path, and to determine the fitness value of each candidate trajectory point by combining the single movement step size and control energy consumption. The candidate trajectory point corresponding to the minimum fitness value is selected as the next path point. The spatial parameters of the robotic arm are adjusted based on the PID algorithm until the picking actuator moves to the next path point. The control iteration module is used to update the current path point in real time, repeat the iteration to determine the next path point, until the picking executor reaches the picking position.

Images