A control method of pepper picking robot based on multi-sensor fusion
Through the multi-sensor fusion of vision sensors, lidar and inertial measurement units, fruit position changes are predicted and path compensation parameters are generated, and the robotic arm movement trajectory is dynamically adjusted, which solves the problem of fruit position fluctuations in the existing technology and improves the picking accuracy and efficiency.
Patent Information
- Application Number
- CN202411328711.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-24
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2044-09-24
AI Technical Summary
Existing pepper picking robots based on multi-sensor fusion are difficult to track the dynamic position of the fruit in real time under the action of wind, resulting in lag or misalignment of the grab action, increasing the complexity of picking operations and damage risks, and reducing efficiency and success rates.
Through vision sensors, lidar and inertial measurement units, fruit position information is collected in real time, path compensation parameters are generated, motion trajectory and grasping actions of the robotic arm, predict fruit position changes, and control the robotic arm point by point to accurately follow the dynamic position of the fruit. .
It improves the identification and positioning accuracy of the picking system, reduces the impact of fruit position fluctuations caused by wind, improves the picking success rate and stability, reduces the risk of fruits being repeatedly impacted and damaged, and improves the picking efficiency.
Smart Images

Figure CN118848992B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of pepper picking robots, and in particular to a pepper picking robot control method based on multi-sensor fusion. Background Art
[0002] The pepper picking robot is an intelligent automation device used in the agricultural field, which is designed to replace manual precision picking of mature peppers. The robot is usually composed of a mobile platform, a robotic arm, an end effector and a variety of sensors. Through multi-sensor fusion technology and an intelligent control system, it can realize the recognition, positioning, grasping and sorting of peppers. During the working process, the robot first uses a visual sensor to collect images of crops, and uses an image processing algorithm to identify the position, shape and maturity of peppers. Then, it combines the data of the distance sensor and the laser sensor to calculate the relative position between the pepper and the robotic arm to ensure that the robotic arm can accurately reach the target position. Then, the end effector performs the grasping action under the guidance of the control system, and monitors the grasping force in real time through the force control sensor to ensure that the picking is completed smoothly without damaging the fruit. The role of the control system is to coordinate the data input of multiple sensors, comprehensively analyze the current environment and target information, and generate the best motion trajectory and operation strategy to cope with complex and dynamic planting environments. Through precise control, the robot can improve the picking efficiency and automation while ensuring the integrity of the peppers, making agricultural production more sustainable and economical.
[0003] The existing control technology of pepper picking robot based on multi-sensor fusion mainly realizes precise picking operation by comprehensively processing the data of multiple sensors such as visual sensors, distance sensors, laser radar, force control sensors, etc. Specifically, the robot first uses visual sensors to obtain crop images and identifies the specific position and morphological characteristics of peppers through image processing algorithms. Based on the recognition, laser radar and distance sensors are used to further determine the spatial distance and angle relationship between peppers and the robotic arm, providing high-precision three-dimensional position information. At the same time, the force control sensor monitors the force applied by the end of the robotic arm during the picking process, and adjusts the grasping action through real-time feedback to ensure that the fruit is not damaged during the picking process. These multi-source sensor data are integrated through data fusion algorithms (such as Kalman filtering, weighted averaging, etc.), and the system will generate control instructions based on these processed data, including the motion trajectory of the robotic arm, the grasping force and speed of the end effector, etc. During the execution process, the control system will continue to receive real-time sensor data and dynamically adjust the picking action to cope with environmental changes, fruit swing or position offset. The core of this process is to achieve comprehensive perception of the environment through sensor fusion, thereby ensuring that the robot can stably and accurately complete the picking task in a complex planting environment, while improving the overall picking efficiency and fruit integrity rate through flexible control strategies.
[0004] The prior art has the following deficiencies:
[0005] In open fields or ventilated greenhouses, fruits may sway slightly under the action of wind, especially when the fruits are hanging on long branches. This swaying will cause small but continuous changes in the fruit position data collected by the sensor. Since the real-time position of the fruit is unstable, the system may grab it based on the position before the swing when planning the path, but the actual position of the fruit has shifted at this time, resulting in inaccurate grabbing action. The existing technology has difficulty tracking the dynamic position of the fruit in real time when dealing with such slight swings, causing the grabbing action of the robot arm to lag or misalign, and then requiring multiple path adjustments, which not only increases the complexity of the picking operation, but may also cause the fruit to be repeatedly collided, increasing the risk of damage, and ultimately reducing the picking efficiency and success rate.
[0006] The above information disclosed in this Background section is only for enhancement of understanding of the background of the present disclosure and therefore it may contain information that does not constitute the prior art that is already known to one of ordinary skill in the art. Summary of the invention
[0007] The purpose of the present invention is to provide a pepper picking robot control method based on multi-sensor fusion to solve the problems in the above-mentioned background technology.
[0008] In order to achieve the above object, the present invention provides the following technical solution: a pepper picking robot control method based on multi-sensor fusion, specifically comprising the following steps:
[0009] The position information of pepper fruits is collected in real time through visual sensors, laser radar and inertial measurement units, and the collected position information of pepper fruits is transmitted to the central control system for storage and processing;
[0010] Analyze the position information of pepper fruits transmitted to the central control system and predict the position changes of fruits during the picking process;
[0011] In the case of a large predicted change in the position of the fruit, the fruit position information transmitted to the central control system is analyzed to generate path compensation parameters;
[0012] Based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time;
[0013] According to the adjusted path planning, the motion trajectory and grasping action of the robotic arm are controlled in real time to perform fruit picking.
[0014] Preferably, analyzing the position information of pepper fruits transmitted to the central control system to predict the position changes of the fruits during the picking process specifically includes the following steps:
[0015] Extracting the position information of the pepper fruit and transmitting it to the central control system, including the three-dimensional position coordinates, movement speed and movement direction data of the fruit;
[0016] Preprocessing the extracted three-dimensional position coordinates, movement speed and movement direction data of the fruit;
[0017] The three-dimensional position coordinates, movement speed and movement direction data of the pre-processed fruit are analyzed to generate the position drift coefficient and acceleration change index of the fruit respectively;
[0018] The generated position drift coefficient and acceleration change index are used to construct a position change prediction model to generate a predicted position change coefficient. The generated predicted position change coefficient is compared with a pre-set predicted position change coefficient threshold to predict the position change of fruits during the picking process.
[0019] Preferably, the logic for obtaining the position drift coefficient and the acceleration change index is as follows:
[0020] Extract the three-dimensional position coordinates of the pre-processed fruit, including the three-dimensional position coordinates of the fruit at different times within a period of time, and calibrate the three-dimensional position coordinates of the fruit at different times within a period of time as , and , The number representing the three-dimensional position coordinates of the fruit at different times over a period of time. , is a positive integer;
[0021] Calculate the displacement of the fruit in each time period. The specific calculation formula is as follows:
[0022] In the formula, For the fruit at the time point and The displacement between
[0023] Calculate the position drift coefficient. The specific calculation formula is as follows:
[0024] In the formula, is the position drift coefficient;
[0025] Extract the movement speed of the pre-processed fruit, including the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times, and calibrate the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times as and , Indicates the actual speed of the fruit at different times within a period of time and the time interval between two adjacent moments. , is a positive integer;
[0026] Calculate the acceleration change of the fruit at two adjacent moments. The specific calculation formula is as follows:
[0027] In the formula, For the fruit Moment and The change in acceleration between moments, and The fruits are Moment and The actual speed at the moment, for Moment and the time interval between moments;
[0028] Calculate the acceleration change index. The specific calculation formula is as follows:
[0029] In the formula, is the acceleration change index.
[0030] Preferably, the position drift coefficient generated is and acceleration change index Construct a position change prediction model and generate the predicted position change coefficient through weighted summation , and the generated predicted position variation coefficient The coefficient of variation of the predicted position is set to a pre-set threshold value. Compare and predict the position changes of fruits during picking. The specific comparison and analysis are as follows:
[0031] like , the position of the fruit during picking changes little;
[0032] like , the position of the fruit during the picking process varies greatly.
[0033] Preferably, when it is predicted that the position of the fruit changes greatly, the fruit position information transmitted to the central control system is analyzed to generate the path compensation parameters, which specifically includes the following steps:
[0034] Extracting the three-dimensional position coordinates and time interval data of the fruit for transmission to the central control system, including the three-dimensional position coordinates of the fruit at different time points and the time intervals between adjacent time points;
[0035] Preprocessing the extracted three-dimensional position coordinates and time interval data of the fruit;
[0036] The three-dimensional position coordinates and time interval data of the pre-processed fruits are analyzed to generate the swing amplitude coefficient and the swing frequency index of the fruits respectively;
[0037] The generated swing amplitude coefficient and swing frequency index are used to construct a path compensation model, and the path compensation parameters are generated through weighted summation.
[0038] Preferably, the acquisition logic of the swing amplitude coefficient and the swing frequency index is as follows:
[0039] Extract the three-dimensional position coordinates and time interval data of the pre-processed fruit, including the three-dimensional position coordinates, time intervals and corresponding time points of the fruit at different times within a period of time, and use the function to convert the three-dimensional position coordinates of the fruit at different times within a period of time into a time series. , and To express it, the time interval is also expressed as a time series using the function To express, is a time point, and the time period is defined as ;
[0040] Calculate the fruit in the time period Inside The specific calculation formula for the swing amplitude at the moment is as follows:
[0041] In the formula, For the fruit in the time period Inside The swing amplitude of the moment;
[0042] Calculate the swing amplitude coefficient. The specific calculation formula is as follows:
[0043] In the formula, is the swing amplitude coefficient;
[0044] Calculate the fruit in the time period Inside The specific calculation formula for the swing frequency at the moment is as follows:
[0045] In the formula, For the fruit in the time period Inside The frequency of the moment's swing;
[0046] Calculate the swing frequency index. The specific calculation formula is as follows:
[0047] In the formula, is the swing frequency index.
[0048] Preferably, based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time, which specifically includes the following steps:
[0049] After obtaining the generated path compensation parameters, calculate the correction amount of the robot path point, add the correction amount to the initial path point coordinates to obtain a new path point;
[0050] According to the new path points, the motion trajectory of the robot arm is updated in real time, and the movement direction and speed of the robot arm are adjusted according to the changes in the path compensation parameters at each path point;
[0051] When performing the grasping action, the position and posture of the robot arm are adjusted point by point, each path point is used as a control node, and the angle and grasping position of the robot arm are adjusted at each node.
[0052] In the above technical solution, the technical effects and advantages provided by the present invention are:
[0053] 1. The present invention realizes high-precision collection of real-time position information of fruits through multi-sensor fusion of visual sensors, lidar and inertial measurement units, and uses a dynamic position prediction model to predict the position changes of fruits during the picking process. Compared with the traditional single sensor solution, the advantage of this technology is that it can combine the data of multiple sensors for comprehensive analysis, generate position drift coefficients and acceleration change indexes, and accurately predict the dynamic position changes of fruits. This not only improves the recognition and positioning accuracy of the system in complex environments, but also effectively reduces the impact of fruit position fluctuations caused by wind on picking operations, thereby improving the success rate and stability of picking.
[0054] 2. When the present invention detects that the position of the fruit has changed significantly, the system can automatically generate path compensation parameters and dynamically adjust the motion trajectory of the robot arm. By calculating the swing amplitude coefficient and frequency index, the system can take into account the swing amplitude and frequency of the fruit when updating the robot arm path in real time, so that each path point can accurately locate the actual position of the fruit. This adjustment mechanism based on path compensation enables the robot arm to flexibly respond to the displacement deviation caused by the swing of the fruit when performing the grasping action, greatly reducing the multiple path adjustment operations caused by the change of the fruit position, improving the picking efficiency, and reducing the risk of multiple collisions and damage to the fruit.
[0055] 3. The present invention adopts a point-by-point control method to gradually adjust the position and posture of the robot arm according to the changes in the path compensation parameters. By finely adjusting the movement direction, speed and grasping position of the robot arm at each path point, it is ensured that the robot arm can always accurately follow the dynamic position of the fruit. This high-precision point-by-point control can not only cope with the subtle swing of the fruit, but also avoid the misalignment of the grasping action caused by the lag in path point updates in traditional path planning, thereby achieving continuous and stable grasping operations, effectively improving the operating accuracy and overall operating efficiency of the picking system. BRIEF DESCRIPTION OF THE DRAWINGS
[0056] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present invention. For ordinary technicians in this field, other drawings can also be obtained based on these drawings.
[0057] Figure 1 The present invention is a flowchart of a method for controlling a pepper picking robot based on multi-sensor fusion. DETAILED DESCRIPTION
[0058] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in a variety of forms and should not be construed as limited to the examples set forth herein; rather, these example embodiments are provided so that the description of the present disclosure will be more comprehensive and complete, and the concept of the example embodiments will be fully conveyed to those skilled in the art.
[0059] The present invention provides Figure 1 A pepper picking robot control method based on multi-sensor fusion is shown, which specifically includes the following steps:
[0060] The position information of pepper fruits is collected in real time through visual sensors, laser radar and inertial measurement units, and the collected position information of pepper fruits is transmitted to the central control system for storage and processing;
[0061] The position information of pepper fruits is collected in real time through visual sensors, laser radars and inertial measurement units (IMUs). Specifically, the visual sensor is responsible for obtaining the two-dimensional image data of the fruit, including the color, shape and edge contour information of the fruit; the laser radar measures the distance between the fruit and the sensor by emitting lasers and receiving reflected signals to generate the depth data of the fruit; the IMU is used to sense the posture and motion changes of the system to correct the offset and jitter that may occur on the mobile platform of the sensor, ensuring that the collected position information is accurate and stable. Through the data fusion algorithm, the image data of the visual sensor, the depth information of the laser radar and the posture data of the IMU are integrated to generate the three-dimensional spatial coordinates of the fruit (such as X, Y, Z position), and the position changes of the fruit are updated in real time. The collected information mainly includes the three-dimensional position, posture angle, spatial motion trend, etc. of the fruit, which provide accurate input for subsequent path planning.
[0062] The collected pepper fruit location information can be transmitted to the central control system in a variety of ways, among which the common transmission methods include wired communication (such as Ethernet), wireless communication (such as Wi-Fi, ZigBee), and interface communication between edge computing devices and the central system. During the transmission process, the sensor data is first encapsulated into a unified data packet format, including fields such as timestamp, sensor ID, data type, and location information to ensure the integrity and traceability of the data during transmission. Then, the data packet is sent to the central control system through a transmission protocol (such as TCP / IP, UDP). After receiving the data, the central control system parses the raw data of each sensor through the data parsing module and stores the data in a database or real-time buffer. After the storage is completed, the system will call the data processing module to further process and analyze the sensor fusion data, such as position calibration, noise filtering, and dynamic tracking analysis, to ensure that the generated fruit location information is accurate and reliable, and can be used for subsequent path planning and grasping decisions.
[0063] Analyze the position information of pepper fruits transmitted to the central control system and predict the position changes of fruits during the picking process;
[0064] In this embodiment, the position information of pepper fruits transmitted to the central control system is analyzed to predict the position changes of the fruits during the picking process, which specifically includes the following steps:
[0065] Extracting the position information of the pepper fruit and transmitting it to the central control system, including the three-dimensional position coordinates, movement speed and movement direction data of the fruit;
[0066] The three-dimensional position coordinates, movement speed and movement direction data of the pepper fruit can be extracted from the position information of the pepper fruit transmitted to the central control system in a variety of ways. Specifically, first, the data tags of different sensors, such as the data fields of the visual sensor, lidar and inertial measurement unit (IMU), are parsed from the sensor data stream received by the central control system. For the three-dimensional position coordinate data of the fruit, the two-dimensional coordinate information of the fruit is extracted by parsing the image data provided by the visual sensor and the depth data of the lidar, and the three-dimensional position coordinates of the fruit are calculated by using image processing algorithms (such as edge detection and feature matching), and the three-dimensional position coordinates of the fruit are calculated in combination with the distance measurement value of the lidar. The movement speed and movement direction data are calculated by analyzing the three-dimensional position changes at multiple time points, where the movement speed is obtained by the time difference method (such as calculating the displacement of adjacent time points divided by the time interval), and the movement direction is determined based on the change vector of the three-dimensional position coordinates. All extracted data are uniformly formatted and structured through the data parsing module for structured storage, so that the subsequent algorithm can perform fusion analysis and prediction on these parameters.
[0067] Preprocessing the extracted three-dimensional position coordinates, movement speed and movement direction data of the fruit;
[0068] Preprocessing is performed to improve the accuracy and stability of the data and avoid the subsequent analysis results being affected by factors such as noise, measurement errors, and sensor jitter. Specifically, the preprocessing steps include denoising, data smoothing, and coordinate correction. First, the three-dimensional position coordinates, movement speed, and movement direction data of the fruit are denoised, and the random noise caused by sensor errors and environmental interference is eliminated through filtering algorithms (such as Kalman filtering or mean filtering); secondly, data smoothing is performed to eliminate sudden jumps caused by inconsistent data sampling frequencies, and the data curve is made smoother and more continuous through interpolation algorithms or weighted averaging methods; finally, coordinate correction is performed, and the three-dimensional coordinates are corrected in combination with the posture information provided by the inertial measurement unit (IMU) to ensure the consistency of data between different sensors. The preprocessed data has higher reliability and consistency, providing a stable input basis for subsequent position change prediction and path planning.
[0069] The three-dimensional position coordinates, movement speed and movement direction data of the pre-processed fruit are analyzed to generate the position drift coefficient and acceleration change index of the fruit respectively;
[0070] The generated position drift coefficient and acceleration change index are used to construct a position change prediction model to generate a predicted position change coefficient. The generated predicted position change coefficient is compared with a pre-set predicted position change coefficient threshold to predict the position change of fruits during the picking process.
[0071] The pre-set threshold of the predicted position change coefficient can be determined by historical data analysis and multiple experimental statistics. Specifically, first, through a large number of picking experiments, the movement data of the fruit under different environmental conditions are collected, including the three-dimensional position coordinates, movement speed and direction data of the fruit under wind force, mechanical arm movement, different growth stages, etc. Statistical analysis is performed on these historical data to calculate the trend of fruit position change under different position drift coefficients and acceleration change indexes, and identify the critical value range that causes the grasping action to fail or have large errors. Then, using these critical values as initial references, the threshold of the position change coefficient is gradually adjusted in the simulation test, and the threshold range is optimized until the prediction accuracy of the system in various environments reaches the expected standard. Finally, based on the optimized threshold range, the threshold parameter is set to the default value of the system through the configuration module in the software. The whole process is automatically optimized through machine learning algorithms or regression analysis methods to ensure that the threshold has good adaptability and robustness in different environments, thereby providing a reliable judgment basis for position change prediction.
[0072] In this embodiment, the logic for obtaining the position drift coefficient and the acceleration change index is as follows:
[0073] Extract the three-dimensional position coordinates of the pre-processed fruit, including the three-dimensional position coordinates of the fruit at different times within a period of time, and calibrate the three-dimensional position coordinates of the fruit at different times within a period of time as , and , The number representing the three-dimensional position coordinates of the fruit at different times over a period of time. , is a positive integer;
[0074] There are many ways to extract the three-dimensional position coordinates of the fruit at different time points from the data transmitted to the central control system. First, the system obtains the coordinate data of the fruit at different time points by parsing the timestamp and location information fields in the sensor data packet. Specifically, the visual sensor and the lidar jointly obtain the image data and depth information of the fruit, where the visual sensor provides the two-dimensional image coordinates (such as and coordinates), the lidar generates depth information by measuring the distance between the fruit and the sensor (i.e. During the data fusion process, the system synchronizes these data according to the timestamps of each sensor, combines the attitude information provided by the inertial measurement unit (IMU), corrects the errors caused by changes in sensor position or angle, and finally extracts the three-dimensional position coordinate data of the fruit. , and ,in Indicates the time point number. The extracted three-dimensional coordinate data is formatted and stored through the data analysis and processing module for subsequent calculation and analysis.
[0075] Calculate the displacement of the fruit in each time period. The specific calculation formula is as follows:
[0076] In the formula, For the fruit at the time point and The displacement between
[0077] Calculate the position drift coefficient. The specific calculation formula is as follows:
[0078] In the formula, is the position drift coefficient;
[0079] The size of the position drift coefficient directly reflects the overall displacement of the fruit over a period of time. The larger the coefficient, the more significant the position change of the fruit. Specifically, the position drift coefficient quantifies the overall movement trend of the fruit by calculating the cumulative value of the displacement of the fruit between adjacent time points. If the position drift coefficient is large, it means that the fruit has undergone more frequent or larger displacements during the picking process. This situation usually means that there is a large instability in the spatial position of the fruit, and the system needs to frequently adjust the path and strategy during grasping to adapt to the position changes of the fruit. Therefore, the larger the position drift coefficient, the greater the predicted change in the fruit position, and the more path compensation and dynamic adjustment the system needs to perform when performing the grasping operation to ensure that the target fruit is accurately grasped.
[0080] Extract the movement speed of the pre-processed fruit, including the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times, and calibrate the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times as and , Indicates the actual speed of the fruit at different times within a period of time and the time interval between two adjacent moments. , is a positive integer;
[0081] To extract the actual speed of the fruit at different time points and the time interval between two adjacent time points, this can be achieved by parsing the timestamp and position data of the sensor data. Specifically, the central control system first extracts the three-dimensional position coordinates related to the movement of the fruit and its corresponding timestamp information from the sensor data stream. Then, the system calculates the change in the three-dimensional position coordinates between adjacent time points and combines the corresponding timestamp difference to obtain the actual speed of the fruit in each time period. . Time interval It is directly calculated by the difference of the timestamps of two adjacent time points. After the extraction is completed, these speed and time interval data will be stored in a structured manner and used as the basic data for subsequent acceleration change calculation and motion prediction.
[0082] Calculate the acceleration change of the fruit at two adjacent moments. The specific calculation formula is as follows:
[0083] In the formula, For the fruit Moment and The change in acceleration between moments, and The fruits are Moment and The actual speed at the moment, for Moment and the time interval between moments;
[0084] Calculate the acceleration change index. The specific calculation formula is as follows:
[0085] In the formula, is the acceleration change index.
[0086] The magnitude of the acceleration change index directly reflects the severity of the speed change of the fruit during movement. The larger the index, the more frequent and larger the amplitude of the acceleration fluctuation of the fruit, which usually means that the fruit will have a large position change in a short period of time. When the acceleration change index is high, the movement trajectory of the fruit is unstable, and rapid displacement or direction changes may occur, making the position of the fruit difficult to accurately predict during the picking process. At this time, the system needs to adjust the grasping strategy and path compensation more frequently during path planning to adapt to the position changes of the fruit. Therefore, the larger the acceleration change index, the more complex and unpredictable the position change of the fruit, and the system needs more flexible real-time adjustments to ensure successful picking.
[0087] In this embodiment, the position drift coefficient generated and acceleration change index Construct a position change prediction model and generate the predicted position change coefficient through weighted summation , and the generated predicted position variation coefficient The coefficient of variation of the predicted position is set to a pre-set threshold value. Compare and predict the position changes of fruits during picking. The specific comparison and analysis are as follows:
[0088] like , the position change of the fruit during the picking process is small, which means that the position change of the fruit during the picking process is small, and the movement trajectory of the fruit is relatively stable and highly predictable. At this time, the system does not need to make major adjustments in path planning and grasping actions, and the robotic arm can perform the picking operation according to the initially set path. In this case, the picking success rate is high, and the movement path of the robotic arm is stable and efficient, which reduces the system's excessive dependence on real-time data during execution and reduces the complexity of path compensation and dynamic adjustment. Therefore, the operation efficiency is improved, the picking time is shortened, and the energy consumption is relatively low;
[0089] like The position of the fruit changes greatly during the picking process, indicating that the position of the fruit changes greatly during the picking process. There are obvious fluctuations or irregular changes in the movement trajectory of the fruit, which causes the system to frequently adjust the path and grasping action to adapt to the dynamic changes of the fruit. At this time, the system must monitor the position changes of the fruit in real time, and use path compensation and dynamic adjustment strategies to ensure accurate grasping of the target fruit. Due to the need for continuous position correction and path replanning, the complexity of the picking process increases, the operating accuracy and response speed of the robot arm become the key, the system energy consumption increases and the operation time may be extended. In addition, frequent path adjustments may lead to a decrease in the grasping success rate and increase the risk of fruit damage.
[0090] To generate the position drift coefficient and acceleration change index Construct a position change prediction model and generate the predicted position change coefficient through weighted summation , first of all, it is necessary to determine the appropriate weight coefficient to reflect the influence of these two parameters on the change of fruit position in different scenarios. In the specific implementation, and They represent two key aspects of the overall drift of the fruit position and the change of the motion trajectory. Therefore, the weight coefficients of these two parameters can be set through empirical data or experimental analysis. and The weight coefficient is usually set based on the actual application environment of the system. For example, when the fruit movement is mainly affected by wind, the position drift coefficient Weight should be larger, and when the fruit is disturbed by the movement of the robot arm, the acceleration change index Weight The calculation formula of the position change prediction model is: ;in, and The value satisfies , which can be obtained through experimental data tuning. and Combined with the generated When the set threshold is exceeded, the system determines that the fruit position changes dramatically and needs to dynamically adjust the path; conversely, if The system can perform picking along the initial path if the weight coefficient is small. In this way, by flexibly adjusting the weight coefficient, the model can accurately predict the position change of the fruit in different environments and provide a reliable basis for path planning.
[0091] In the case of a large predicted change in the position of the fruit, the fruit position information transmitted to the central control system is analyzed to generate path compensation parameters;
[0092] In this embodiment, when it is predicted that the position of the fruit changes greatly, the fruit position information transmitted to the central control system is analyzed to generate the path compensation parameters, which specifically includes the following steps:
[0093] Extracting the three-dimensional position coordinates and time interval data of the fruit for transmission to the central control system, including the three-dimensional position coordinates of the fruit at different time points and the time intervals between adjacent time points;
[0094] Extracting the three-dimensional position coordinates and time interval data of the fruit transmitted to the central control system can be achieved by parsing the timestamp and position fields in the sensor data packet. Specifically, the system extracts the three-dimensional position coordinate information of the fruit at different time points from the data stream collected by the visual sensor, lidar and inertial measurement unit (IMU). The visual sensor provides two-dimensional image coordinates, and the lidar provides depth information. After data fusion, these data generate the three-dimensional position coordinates of the fruit. The time interval data is calculated by extracting the timestamps of adjacent time points, that is, the timestamp difference represents the time interval between adjacent time points. Through the data parsing module, these three-dimensional position coordinates and time interval data will be structured and stored and formatted into a unified data format for subsequent processing. This process is automatically completed through the data processing module in the software, realizing real-time data extraction and calculation, and providing accurate input data for subsequent analysis and path compensation.
[0095] Preprocessing the extracted three-dimensional position coordinates and time interval data of the fruit;
[0096] The main purpose of preprocessing is to improve the accuracy and stability of the data, to ensure that interference factors are eliminated when analyzing the swing amplitude and frequency of the fruit, and to obtain reliable parameters. Specifically, preprocessing includes the following steps: First, the three-dimensional position coordinates and time interval data are denoised, and a filtering algorithm (such as Kalman filtering or median filtering) is used to eliminate random noise in the sensor data to avoid interference from the external environment or measurement errors. Secondly, data smoothing is performed to eliminate sudden jumps caused by inconsistent data sampling frequency or sensor jitter through interpolation algorithms or weighted average algorithms to ensure data continuity and consistency. Finally, coordinate correction is performed, and the coordinate offset caused by sensor angle changes or mobile platform is corrected in combination with the attitude information provided by the inertial measurement unit (IMU). Through these preprocessing steps, the extracted data can be guaranteed to have higher accuracy and stability, laying the foundation for the subsequent generation of parameters such as swing amplitude and frequency.
[0097] The three-dimensional position coordinates and time interval data of the pre-processed fruits are analyzed to generate the swing amplitude coefficient and the swing frequency index of the fruits respectively;
[0098] The generated swing amplitude coefficient and swing frequency index are used to construct a path compensation model, and the path compensation parameters are generated through weighted summation.
[0099] In this embodiment, the logic for obtaining the swing amplitude coefficient and the swing frequency index is as follows:
[0100] Extract the three-dimensional position coordinates and time interval data of the pre-processed fruit, including the three-dimensional position coordinates, time intervals and corresponding time points of the fruit at different times within a period of time, and use the function to convert the three-dimensional position coordinates of the fruit at different times within a period of time into a time series. , and To express it, the time interval is also expressed as a time series using the function To express, is a time point, and the time period is defined as ;
[0101] Extracting the preprocessed three-dimensional position coordinates and time interval data of the fruit can be achieved by parsing the timestamps and position information in the sensor data stream. Specifically, the central control system receives data from multiple sensors, including image data provided by visual sensors, depth data provided by lidar, and attitude information provided by inertial measurement units (IMUs). The system first pairs the data according to the timestamps synchronized by the sensors, fuses the image coordinates, depth information, and attitude data at each moment, and generates the corresponding three-dimensional position coordinates. The time interval data is calculated by the difference between the timestamps of adjacent time points to ensure that the sampling frequency between different time points is consistent. Through the data parsing and fusion module, these three-dimensional position coordinates, time intervals, and corresponding time point information are extracted and stored in a structured format in the central control system for subsequent swing amplitude and frequency analysis. This process is implemented through the software's automated processing module to ensure the real-time and accuracy of data extraction.
[0102] Calculate the fruit in the time period Inside The specific calculation formula for the swing amplitude at the moment is as follows:
[0103] In the formula, For the fruit in the time period Inside The swing amplitude of the moment;
[0104] Calculate the swing amplitude coefficient. The specific calculation formula is as follows:
[0105] In the formula, is the swing amplitude coefficient;
[0106] The swing amplitude coefficient reflects the amplitude of the change in the position of the fruit within a certain period of time. The larger the coefficient, the greater the swing of the fruit in space. The generation of path compensation parameters directly depends on the swing amplitude coefficient. When the coefficient is large, the system will judge that the position of the fruit is unstable and generate a larger path compensation parameter to ensure that the robot arm can follow the swing of the fruit to adjust the grasping action. In other words, the larger the swing amplitude coefficient, the larger the correction amount of path compensation will be, so as to avoid grasping failure due to position offset during the picking process.
[0107] Calculate the fruit in the time period Inside The specific calculation formula for the swing frequency at the moment is as follows:
[0108] In the formula, For the fruit in the time period Inside The frequency of the moment's swing;
[0109] Calculate the swing frequency index. The specific calculation formula is as follows:
[0110] In the formula, is the swing frequency index.
[0111] The swing frequency index indicates the swing frequency of the fruit during the picking process. The larger the index, the more frequently the fruit swings and the faster the position changes. When generating path compensation parameters, the size of the swing frequency index directly affects the response speed of the compensation. When the swing frequency index is high, the system needs to adjust the path planning more quickly to keep up with the rapid movement of the fruit. Therefore, the larger the frequency index, the faster the dynamic adjustment speed involved in the path compensation parameters will be, ensuring that the robot arm can respond to frequent swing changes in a timely manner and maintain real-time tracking of the fruit position.
[0112] Based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time;
[0113] In this embodiment, based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time, which specifically includes the following steps:
[0114] After obtaining the generated path compensation parameters, calculate the correction amount of the robot path point, add the correction amount to the initial path point coordinates to obtain a new path point;
[0115] In this step, the path compensation parameter represents the path deviation caused by the swing of the fruit. Through the path planning algorithm, the system first calculates the correction amount of each path point based on the compensation parameter. The correction amount can be obtained by differential calculation, that is, the correction value is dynamically generated according to the difference between the current path compensation parameter and the compensation parameter at the previous moment. After the correction amount is calculated, the system adds it to the coordinates of the initial path point to generate a new path point. The purpose of this approach is to update the path in time when the position of the fruit changes, to ensure that the robot arm can move along the trajectory adjusted in real time, so as to track the position of the fruit more accurately.
[0116] According to the new path points, the motion trajectory of the robot arm is updated in real time, and the movement direction and speed of the robot arm are adjusted according to the changes in the path compensation parameters at each path point;
[0117] In this step, the system adjusts the motion trajectory of the robot arm by continuously updating the path points. When updating the path in real time, the system dynamically calculates the motion direction and speed at each path point based on the changes in the path compensation parameters. The specific approach includes using an interpolation algorithm or a path optimization algorithm at each path point to adjust the movement direction of the robot arm so that it can effectively avoid the swinging trajectory of the fruit. At the same time, according to the size of the compensation parameters, the movement speed of the robot arm is dynamically adjusted to ensure that the speed is appropriately reduced when the fruit swings more, so as to better follow the target. The core of this step lies in the real-time and flexibility of path planning, which enables the robot arm to accurately respond to the dynamic changes of fruit swinging.
[0118] When performing the grasping action, the position and posture of the robot arm are adjusted point by point, each path point is used as a control node, and the angle and grasping position of the robot arm are adjusted at each node.
[0119] When performing the grasping action, the system regards each path point as a control node and adjusts the position and posture of the robot arm point by point. In the specific implementation, the control module will calculate the joint angle and posture of the robot arm at each node through the inverse kinematics algorithm based on the information of the path point (including position, posture, angle, etc.), and perform smooth transitions between nodes. At the same time, at each node, the system will make fine adjustments based on the real-time position data of the fruit to ensure that the grasping position is always consistent with the position of the fruit. This point-by-point control method can effectively reduce the grasping error caused by the swing of the fruit and improve the response speed and action accuracy of the system during the grasping process.
[0120] According to the adjusted path planning, the motion trajectory and grasping action of the robotic arm are controlled in real time to perform fruit picking.
[0121] In this step, the goal of the system is to ensure that the robot arm accurately completes the fruit grabbing action according to the real-time updated path. First, the control system generates the motion trajectory of the robot arm through the updated path point information calculated by the path planning module, and smoothly transitions the movement between the path points through the interpolation algorithm, so that the robot arm can maintain stable operation on the path. Subsequently, the system dynamically adjusts the key parameters of the grabbing action, such as the grabbing speed, grabbing torque, and the closing angle of the gripper, based on the real-time sensor data. In specific implementation, the system will monitor the subtle changes in the position of the fruit in real time through a closed-loop feedback control mechanism, and correct the posture and grabbing action of the robot arm according to these changes, so as to avoid grabbing failures caused by fruit swinging. During the grabbing process, the system will perform inverse kinematics calculations for each joint of the robot arm, and gradually adjust the angles of each joint of the robot arm so that it can move accurately in multi-dimensional space and keep pace with the fruit. The reason for adopting this approach is that real-time path control and grabbing action adjustment can minimize the deviation caused by fruit swing, ensuring that the robot arm can quickly perform grabbing actions when the fruit is in the best position, thereby improving the success rate and efficiency of picking.
[0122] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters in the formula are set by technicians in this field according to actual conditions.
[0123] The above embodiments can be implemented in whole or in part by software, hardware, firmware or any other combination. When implemented by software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center by wired or wireless (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains one or more available media sets. The available medium can be a magnetic medium (for example, a floppy disk, a hard disk, a tape), an optical medium (for example, a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state hard disk.
[0124] It should be understood that in the various embodiments of the present application, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.
[0125] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.
[0126] In the several embodiments provided in the present application, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.
[0127] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
[0128] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.
[0129] 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 who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.
Claims
1. A pepper picking robot control method based on multi-sensor fusion, characterized in that: The specific steps include: The position information of pepper fruits is collected in real time through visual sensors, laser radar and inertial measurement units, and the collected position information of pepper fruits is transmitted to the central control system for storage and processing; Analyze the position information of pepper fruits transmitted to the central control system and predict the position changes of fruits during the picking process; In the case of a large predicted change in the position of the fruit, the fruit position information transmitted to the central control system is analyzed to generate path compensation parameters; Based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time; According to the adjusted path planning, the robot arm's motion trajectory and grasping action are controlled in real time to perform fruit picking; When it is predicted that the fruit position changes greatly, the fruit position information transmitted to the central control system is analyzed to generate path compensation parameters, which specifically includes the following steps: Extracting the three-dimensional position coordinates and time interval data of the fruit for transmission to the central control system, including the three-dimensional position coordinates of the fruit at different time points and the time intervals between adjacent time points; Preprocessing the extracted three-dimensional position coordinates and time interval data of the fruit; The three-dimensional position coordinates and time interval data of the pre-processed fruits are analyzed to generate the swing amplitude coefficient and the swing frequency index of the fruits respectively; The generated swing amplitude coefficient and swing frequency index are used to construct a path compensation model, and the path compensation parameters are generated by weighted summation; The acquisition logic of the swing amplitude coefficient and the swing frequency index is as follows: Extract the three-dimensional position coordinates and time interval data of the pre-processed fruit, including the three-dimensional position coordinates, time intervals and corresponding time points of the fruit at different times within a period of time, and use the function to convert the three-dimensional position coordinates of the fruit at different times within a period of time into a time series. , and To express it, the time interval is also expressed as a time series using the function To express, is a time point, and the time period is defined as ; Calculate the fruit in the time period Inside The specific calculation formula for the swing amplitude at the moment is as follows: In the formula, For the fruit in the time period Inside The swing amplitude of the moment; Calculate the swing amplitude coefficient. The specific calculation formula is as follows: In the formula, is the swing amplitude coefficient; Calculate the fruit in the time period Inside The specific calculation formula for the swing frequency at the moment is as follows: In the formula, For the fruit in the time period Inside The frequency of the moment's swing; Calculate the swing frequency index. The specific calculation formula is as follows: In the formula, is the swing frequency index.
2. The method for controlling a pepper picking robot based on multi-sensor fusion according to claim 1, characterized in that: The position information of pepper fruits transmitted to the central control system is analyzed to predict the position changes of the fruits during the picking process, which specifically includes the following steps: Extracting the position information of the pepper fruit and transmitting it to the central control system, including the three-dimensional position coordinates, movement speed and movement direction data of the fruit; Preprocessing the extracted three-dimensional position coordinates, movement speed and movement direction data of the fruit; The three-dimensional position coordinates, movement speed and movement direction data of the pre-processed fruit are analyzed to generate the position drift coefficient and acceleration change index of the fruit respectively; The generated position drift coefficient and acceleration change index are used to construct a position change prediction model to generate a predicted position change coefficient. The generated predicted position change coefficient is compared with a pre-set predicted position change coefficient threshold to predict the position change of fruits during the picking process.
3. The method for controlling a pepper picking robot based on multi-sensor fusion according to claim 2 is characterized in that: The logic for obtaining the position drift coefficient and acceleration change index is as follows: Extract the three-dimensional position coordinates of the pre-processed fruit, including the three-dimensional position coordinates of the fruit at different times within a period of time, and calibrate the three-dimensional position coordinates of the fruit at different times within a period of time as , and , The number representing the three-dimensional position coordinates of the fruit at different times over a period of time. , is a positive integer; Calculate the displacement of the fruit in each time period. The specific calculation formula is as follows: In the formula, For the fruit at the time point and The displacement between Calculate the position drift coefficient. The specific calculation formula is as follows: In the formula, is the position drift coefficient; Extract the movement speed of the pre-processed fruit, including the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times, and calibrate the actual speed of the fruit at different times within a period of time and the time interval between two adjacent times as and , Indicates the actual speed of the fruit at different times within a period of time and the time interval between two adjacent moments. , is a positive integer; Calculate the acceleration change of the fruit at two adjacent moments. The specific calculation formula is as follows: In the formula, For the fruit Moment and The change in acceleration between moments, and The fruits are Moment and The actual speed at the moment, for Moment and the time interval between moments; Calculate the acceleration change index. The specific calculation formula is as follows: In the formula, is the acceleration change index.
4. The method for controlling a pepper picking robot based on multi-sensor fusion according to claim 3 is characterized in that: The position drift coefficient generated and acceleration change index Construct a position change prediction model and generate the predicted position change coefficient through weighted summation , and the generated predicted position variation coefficient The coefficient of variation of the predicted position is set to a pre-set threshold value. Compare and predict the position changes of fruits during picking. The specific comparison and analysis are as follows: like , the position of the fruit during picking changes little; like , the position of the fruit during the picking process varies greatly.
5. The method for controlling a pepper picking robot based on multi-sensor fusion according to claim 4, characterized in that: Based on the generated path compensation parameters, the path planning and grasping action of the robot arm are dynamically adjusted to correct the motion trajectory of the robot arm in real time. Specifically, the following steps are included: After obtaining the generated path compensation parameters, calculate the correction amount of the robot path point, add the correction amount to the initial path point coordinates to obtain a new path point; According to the new path points, the motion trajectory of the robot arm is updated in real time, and the movement direction and speed of the robot arm are adjusted according to the changes in the path compensation parameters at each path point; When performing the grasping action, the position and posture of the robot arm are adjusted point by point, each path point is used as a control node, and the angle and grasping position of the robot arm are adjusted at each node.
