Multi-sensor fusion-based bergamot pear picking equipment control method and picking equipment

Abstract

The invention provides a bergamot pear picking equipment control method based on multi-sensor fusion and picking equipment, and relates to the technical field of mechanical arm control. The mapping relation between mechanical arm space parameters and the position of a picking executor in bergamot pear picking equipment is established, and the initial position of the picking executor is obtained to serve as a current path point; determining a picking position based on the position of the bergamot pear to be picked, and determining a shortest picking path and a single movement step length by combining the initial position and the picking position; determining a plurality of pre-selected track points based on the current path point, selecting an optimal picking actuator position from the pre-selected track points based on the position relation and control energy consumption, adjusting the current track point to a mechanical arm space parameter corresponding to the picking actuator position based on a PID algorithm, updating the current path point in real time, and repeating iteration to determine a next path point. In the bergamot pear picking process, efficient, stable and low-control-energy-consumption control of the mechanical arm is achieved.

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 caused by multi-degree-of-freedom motion of the mechanical arm while achieving high-precision positioning. Such devices often lack optimization of multi-joint motion coordination of the mechanical arm, resulting in shaking and response lag 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: A pear picking device control method based on multi-sensor fusion, the specific steps comprising: Step 1: Establish a 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; Step 2: Obtain the initial position of the picking actuator as the current path point, and determine the picking position based on the position of the pear to be picked. Combine the initial position and the picking position to determine the shortest picking path and single movement step; Step 3: Determine a plurality of pre-selected trajectory points with the current path point as the center and the moving step length as the radius, and select candidate trajectory points from them that are closer to the picking location 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; 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 in combination with the single moving step length and the control energy consumption, select the candidate trajectory point corresponding to the minimum fitness value as the next path point, adjust the mechanical arm space parameters based on the PID algorithm until the picking executor position moves to the next path point; Step 5: Real-time update the current path point, repeat iteration to determine the next path point, until the picking executor reaches the picking location.

[0007] 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; The mapping relationship is: ; Wherein, is the pear picking executor position coordinate, 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.

[0008] Further, the logic for generating pre-selected trajectory points is: taking the current path point as the center and the moving step length as the radius to make a circle, selecting N points on the circle at equal intervals as pre-selected trajectory points, and rotating the circle by 2 degrees each time to generate N new pre-selected trajectory points, until it is rotated 89 times, a total of 90 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 the pre-selected trajectory point is selected as the candidate trajectory point.

[0009] Further, for each candidate trajectory point, the mechanical arm space parameters are solved based on the mapping relationship within the feasible range of the mechanical arm space parameters to obtain at least one solution of the mechanical arm space parameter combination, forming a solution set of the mechanical arm space parameters, if the solution set is empty, the corresponding candidate trajectory point is cancelled, each solution in the solution set of the mechanical arm space parameters includes the moving distance of the bottom translational joint, the moving distance of the telescopic joint, the twisting angle of the twisting joint and the tilting angle of the tilting joint, and the change amount of the mechanical arm space parameters in each solution in the solution set of the mechanical arm space parameters relative to the mechanical arm space parameters of the current path point is calculated, and the change amount of each solution is formed.

[0010] Further, the initial mechanical arm space parameters, the change amount of the mechanical arm space parameters and the corresponding control energy consumption data in the historical operation of the same mechanical arm are obtained, the initial mechanical arm space parameters and the change amount of the mechanical arm space parameters are taken as inputs, and the corresponding control energy consumption is taken as a label, a control energy consumption prediction model is constructed and trained, for a candidate trajectory point, the change amount of each solution in the solution set corresponding to the candidate trajectory point position and the mechanical arm space parameters of the current trajectory point are input into the control energy consumption prediction model to obtain the corresponding control energy consumption of each solution, and the solution with the lowest control energy consumption is selected from the solution set as the mechanical arm space parameters of the trajectory point.

[0011] Further, the formula for calculating the fitness is: ; Wherein, is the fitness of the mth candidate trajectory point, is the control energy consumption from the current trajectory point to the mth candidate trajectory point, is a single moving step, is the projection length of the line segment between the mth candidate trajectory point and the current path point on the shortest picking path.

[0012] The application further provides a multi-sensor fusion-based fragrant pear picking device, which is characterized in that the picking device is used to realize the multi-sensor fusion-based fragrant pear picking device control method, and specifically comprises: A pose-position mapping module is configured to establish a mapping relationship between the mechanical arm space parameters and the picking executor position in the fragrant pear picking device, and the mechanical arm space parameters collected by the sensor include the moving distance of the bottom translational joint, the moving distance of the telescopic joint, the twisting angle of the twisting joint and the tilting angle of the tilting joint. A position analysis module is configured to obtain the initial position of the picking executor as the current path point, and determine the picking position based on the position of the fragrant pear to be picked, and determine the shortest picking path and the single moving step in combination with the initial position and the picking position. ​​​A trajectory screening module is configured to determine a plurality of preselected trajectory points with the current path point as the center and a moving step as the radius, screen candidate trajectory points close to the picking position from the plurality of preselected trajectory points, determine a mechanical arm space parameter change corresponding to each candidate trajectory point based on the mapping relationship, and determine control energy consumption for reaching each candidate trajectory point based on the mechanical arm space parameter change. 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 a single moving step 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 space parameter based on a PID algorithm until the picking executor position moves to the next path point. A control iteration module is configured to update the current path point in real time, repeat iteration to determine the next path point, and determine the picking executor to reach the picking position.

[0013] Compared with the prior art, the present application has the following beneficial effects: The present application establishes a mapping relationship between the multi-joint motion parameters of the mechanical arm and the position of the end executor, combines trajectory optimization and control energy consumption prediction algorithm, and realizes efficient, stable and low control energy consumption control of the mechanical arm in the process of picking the fragrant pear. The method can quickly select the optimal motion path with comprehensive performance from multiple candidate trajectories, effectively reduces the invalid joint motion and system energy loss, and enhances the adaptability and control accuracy of the mechanical arm in the complex space. In addition, the introduction of closed-loop PID control and 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

[0014] Figure 1 The figure is a schematic diagram of the overall method of the present application. Figure 2 The figure is an energy consumption comparison chart of the present application and the traditional method. Figure 3 The figure is a schematic diagram of the overall system structure of the present application. DETAILED DESCRIPTION

[0015] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application is further described in detail below with specific examples.

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

[0017] Embodiment: Please refer to Figures 1-2 The present application provides a technical solution: A multi-sensor fusion-based pear picking device control method, the specific steps comprising: 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 twist joint and the angle tilted by the tilt joint. Further, the mechanical arm space parameters include: the distance moved by the bottom translational joint, the distance moved by the telescopic joint, the angle twisted by the twist joint, the angle tilted by the tilt joint; A space coordinate system is established with the center of the base of the 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.

[0018] The distance moved by the bottom translational joint and the distance moved by the telescopic joint 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 measuring end is connected to the moving platform of the translational joint. When the platform moves, the sensor directly measures the linear distance moved relative to the base. The main body of the linear displacement sensor is fixed on the fixed sleeve of the telescopic arm, and the measuring end is connected to the end of the telescopic rod. When the telescopic rod is extended or retracted, the sensor directly measures the length change.

[0019] 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. The absolute encoder is crucial because it can provide the absolute angular position of the joint immediately upon power-up, without the need for a reset. Specifically, the absolute rotary encoder is installed directly behind the motor of the twist joint and the pitch joint, and is coaxially installed with the servo motor that drives the joint, directly measuring the angle of the twist joint and the angle of the pitch joint.

[0020] A backstepping control algorithm based on Lyapunov stability theory is used to optimize the motion control of the robot 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 robot arm can reach in space, to determine the mapping relationship; The mapping relationship is: ; Wherein, is the position coordinates of the picking implement, is the distance from the pitch joint axis to the bottom prismatic joint axis, is the distance from the extension joint axis to the bottom prismatic joint axis, is the distance of the bottom prismatic joint movement, is the distance of the extension joint movement, is the angle of the twist joint, is the angle of the pitch joint. 、 is a fixed parameter, not a variable.

[0021] Step 2: Obtain the initial position of the picking implement 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 the picking position; 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 implement in the initial position, and the intersection of the line and the boundary of the pear area is called the picking position.

[0022] The shortest picking path is the line connecting the current path point and the picking position; the single movement step is determined according to the shortest picking path, specifically: ; Wherein, is the single movement step, is the length of the shortest picking path, is the preset movement step number, in this embodiment, .

[0023] Step 3: Determine a plurality of pre-selected trajectory points with the current path point as the center and a movement step length as the radius, and select candidate trajectory points from the pre-selected trajectory points that are closer to the picking location 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 of reaching each candidate trajectory point based on the mechanical arm space parameter change; Further, the logic for generating pre-selected trajectory points is: taking the current path point as the center and a movement step length as the radius to make a circle, selecting N points on the circle at equal intervals 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 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 the pre-selected trajectory point is selected as a 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 approaches the target, completely avoiding invalid exploration or circular paths that may occur in complex spaces. Among the 90 N pre-selected points, as many as half of the points may deviate from the target or cause backtracking. This step of screening immediately eliminates all these invalid options, reducing the number of "candidate points" that require subsequent complex energy consumption calculations to a smaller set. This significantly reduces the computational load.

[0024] Further, for each candidate trajectory point, the feasible range of the mechanical arm space parameters can be obtained through the mechanical arm manual, and the mechanical arm space parameters are solved based on the mapping relationship to obtain a solution of at least one mechanical arm space parameter combination, which constitutes a mechanical arm space parameter solution set. If the solution set is empty, the corresponding candidate trajectory point is cancelled. Each solution in the mechanical arm space parameter solution set includes the movement distance of the bottom prismatic joint, the movement distance of the extension joint, the angle of the torsion joint, and the angle of the pitch joint, and the change amount of the mechanical arm space parameters in each solution in the mechanical arm space parameter solution set relative to the mechanical arm space parameters of the current path point is calculated to constitute the change amount of each solution.

[0025] The solving of the mechanical arm space parameters based on the mapping relationship can use existing technologies, including but not limited to inverse kinematics solving. The following is the solving process of inverse kinematics solving: The feasible range of the robot arm space parameters is obtained through the robot arm specification, and a 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 to-be-solved parameters, including the movement distance of the bottom prismatic 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 through algebraic transformation to obtain a solution set. The solutions in the solution set that satisfy the constraint condition constitute the robot arm space parameter solution set. If there is no solution that satisfies the constraint condition, the solution set is empty, and the corresponding candidate trajectory point is cancelled.

[0026] The change amount of the robot arm space parameters includes the change amount of the movement distance of the bottom prismatic 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.

[0027] 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 the label. A control energy consumption prediction model is constructed and trained. 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 reach the candidate trajectory point. 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.

[0028] 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. Constructing and training a control energy consumption prediction model can utilize existing technologies. This model employs a feedforward neural network, taking the initial robotic arm spatial parameters and their changes as inputs, with the corresponding control energy consumption as the label. Training the model specifically includes: an input layer, hidden layers, an output layer, and an activation function. The input layer receives the initial robotic arm spatial parameters and their changes; the hidden layer processes the data on these parameters; and the model consists of multiple layers, each containing multiple time points. Each hidden layer's time point is connected to the previous layer via weights, used to process the input initial robotic arm spatial parameters. The model performs feature abstraction and nonlinear transformation on changes in the spatial parameters of the robotic arm; by using the ReLU activation function, a nonlinear relationship is introduced, enabling 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 from the hidden layer for the corresponding control energy consumption of the output; the root mean square error loss function is adopted; the input data is processed through the network once to obtain the output result, and the loss function is calculated based on the predicted and true values. The gradient of the loss function with respect to each weight and bias is calculated using the chain rule, and the weights and biases of the network are updated using the gradient descent algorithm to minimize the loss function. In this embodiment, the training parameters are set as follows: Loss function: Root mean square error loss function; Optimizer: Adam, initial learning rate 0.001; Batch size: 32, number of training rounds: 1000; Use an early stopping strategy to prevent overfitting.

[0029] 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. Furthermore, 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.

[0030] is normalized to 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 projection 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 consumption, for each control, if the single control energy consumption is the lowest but the distance to the target position is farther, the number of controls needed will increase. For the entire control, the control energy consumption does not decrease. The embodiment maps each candidate trajectory point and the line segment between the current path 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 consumption is actually increased.

[0031] 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 quantity in real time according to the error between the current joint state fed back 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 quantity drives the motor or actuator of each joint to move, gradually adjusts the posture of the robot arm, 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 prior art and will not be described here.

[0032] 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.

[0033] Please refer to Figure 2 In the embodiment, the scheme (candidate trajectory optimization control) in the 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 consumption is as shown in Figure 2 The energy consumption of the scheme in the embodiment is reduced by 9.78% compared with the traditional scheme.

[0034] Please refer to Figure 3 The application further provides a fragrant pear picking device based on multi-sensor fusion, characterized in that: the picking device is used to realize the control method of the fragrant pear picking device based on multi-sensor fusion, and specifically comprises: A pose mapping module is configured to establish a mapping relationship between a mechanical arm space parameter and a picking actuator position in the pear picking device, and the mechanical arm space parameter collected by the sensor includes a distance of a bottom translational joint movement, a distance of a telescopic joint movement, an angle of a twist joint twist, and an angle of a tilt joint tilt. 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. 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 space parameter change 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 change amount. 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 space parameter based on a PID algorithm until the picking actuator position moves to the next path point. 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.

[0035] The above formulas are all dimensionless numerical calculations, and the formulas are obtained by software simulation of a large amount of data to obtain a formula closest to the actual situation. The preset parameters in the formula are set by a person skilled in the art according to the actual situation.

[0036] The above embodiments can be realized wholly or partially by software, hardware, firmware or any combination thereof. When realized by software, the above embodiments can be realized in the form of a computer program product. A person skilled in the art can realize that the units and algorithm steps of the 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 the specific application and design constraints of the technical solutions.

[0037] The units described as separate components can or can not be physically separated, and the components shown as units can or can not be physical units, which can 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 embodiments according to actual needs.

[0038] The above merely provides the specific implementation of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art should not easily think of the changes or replacements within the technical range disclosed by the present application, which should be covered in the protection scope of the present application.

Claims

1. A method for controlling a multi-sensor fusion-based pear picking device, characterized by, The specific steps include: Step 1: Establish the mapping relationship between the spatial parameters of the mechanical arm and the position of the picking executor in the pear picking device, wherein the spatial parameters of the mechanical arm collected by the sensor include the movement distance of the bottom translational joint, the movement distance of the telescopic joint, the angle of the twist joint, and the angle of the pitch joint; 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 the picking position; Step 3: Determine a plurality of preselected trajectory points with the current path point as the center and the movement step as the radius, and select candidate trajectory points from the plurality of preselected trajectory points that are closer to the picking position than the current path point, determine the change amount of the spatial parameters of the mechanical arm corresponding to each candidate trajectory point based on the mapping relationship, and determine the control energy consumption for reaching each candidate trajectory point based on the change amount of the spatial parameters of the mechanical arm; 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 and the 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 mechanical arm based on the PID algorithm until the position of the picking executor moves to the next path point; Step 5: Real-time update the current path point, and repeat the iteration to determine the next path point until the picking executor reaches the picking position.

2. The multi-sensor fusion-based control method of a pear picking device according to claim 1, characterized in that: The spatial parameters of the mechanical arm include: the movement distance of the bottom translational joint, the movement distance of the telescopic joint, the angle of the twist joint, and the angle of the pitch joint; and a space coordinate system is established with the center of the base of the pear picking device as the origin; The mapping relationship is: ； wherein, is the position coordinate of the pear picking effector, is the distance of the pitch joint axis to the bottom prismatic joint axis, is the distance of the extension joint axis to the bottom prismatic joint axis, is the distance of the bottom prismatic joint movement, is the distance of the extension joint movement, is the angle of the twist joint twist, is the angle of the pitch joint pitch.

3. The multi-sensor fusion-based control method of a pear picking device according to claim 1, characterized in that, The logic of generating pre-selected trajectory points is: taking the current path point as the center and the moving step as the radius to make a circle, selecting N points on the circle at equal intervals as pre-selected trajectory points, and rotating the circle by 2 degrees clockwise each time to generate N new pre-selected trajectory points, until the circle is rotated 89 times, a total of 90 N pre-selected trajectory points are obtained. The distance between each pre-selected trajectory point and the picking position is calculated and compared with the distance between the current path point and the picking position. If the distance between the pre-selected trajectory point and the picking position is less than the distance between the current path point and the picking position, the pre-selected trajectory point is selected as a candidate trajectory point.

4. The multi-sensor fusion-based control method of a pear picking device according to claim 3, characterized in that: For each candidate trajectory point, the spatial parameters of the mechanical arm are solved based on the mapping relationship within the feasible range of the spatial parameters of the mechanical arm to obtain at least one solution of the spatial parameters of the mechanical arm, which constitutes a solution set of the spatial parameters of the mechanical arm, and if the solution set is empty, the corresponding candidate trajectory point is cancelled; each solution in the solution set of the spatial parameters of the mechanical arm includes the movement distance of the bottom translational joint, the movement distance of the telescopic joint, the angle of the twist joint, and the angle of the pitch joint, and the change amount of the spatial parameters of the mechanical arm in each solution in the solution set of the spatial parameters of the mechanical arm is calculated relative to the spatial parameters of the mechanical arm of the current path point, and the change amount of each solution is constituted.

5. The multi-sensor fusion-based control method of a pear picking device according to claim 1, wherein: The initial spatial parameters of the mechanical arm, the change amount of the spatial parameters of the mechanical arm, and the corresponding control energy consumption data in the historical operation of the same mechanical arm are obtained, the initial spatial parameters of the mechanical arm and the change amount of the spatial parameters of the mechanical arm are taken as inputs, and the corresponding control energy consumption is taken as a label, a control energy consumption prediction model is constructed and trained, for a candidate trajectory point, the change amount of each solution in the solution set corresponding to the position of the candidate trajectory point and the spatial parameters of the mechanical arm of the current trajectory point are input into the control energy consumption prediction model to obtain the control energy consumption corresponding to each solution, and the solution with the lowest control energy consumption is selected from the solution set as the optimal spatial parameters of the mechanical arm for the trajectory point. ​ 6. The pear picking device control method based on multi-sensor fusion according to claim 1, wherein: The formula for calculating the fitness is: ； wherein, is the fitness of the th candidate trajectory point, is the control energy of the current trajectory point to reach the th candidate trajectory point, is the single movement step length, is the projection length of the line segment between the th candidate trajectory point and the current path point on the shortest picking path.

7. A multi-sensor fusion based pear picking device, characterized by: The picking device is used for realizing the multi-sensor fusion-based control method of the pear picking device according to any one of claims 1-6, and specifically comprises: A pose mapping module is configured to establish a mapping relationship between a mechanical arm space parameter and a picking actuator position in the pear picking device, and the mechanical arm space parameter collected by the sensor includes a distance of a bottom translational joint movement, a distance of a telescopic joint movement, an angle of a torsion joint torsion, and an angle of a pitch joint pitch; 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 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; 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 change 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 change amount; 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 until the picking actuator position moves to the next path point; A control iteration module is configured to update the current path point in real time, repeat iteration to determine the next path point, and determine the picking actuator to reach the picking position.

Images