An Automatic Navigation Method for Pineapple Field Harvesting Robots Based on Multimodal Data Fusion

Abstract

This invention discloses an automatic navigation method for a pineapple harvesting robot based on multimodal data fusion. First, multiple sensors in the pineapple harvesting robot are transformed to the same coordinate system. Then, images of the pineapple field are acquired and preprocessed, and navigation line information is extracted from the preprocessed images. An extended Kalman filter algorithm is used to fuse information from satellite navigation sensors and inertial sensors, as well as information from photoelectric encoders and inertial sensors, to obtain the pose and odometer information of the pineapple harvesting robot. The navigation line information, pose information, and odometer information are input into a model predictive controller optimized using the Grey Wolf optimization algorithm to obtain optimal control information, which is then used to control the pineapple harvesting robot to autonomously track its path. This automatic navigation method for pineapple harvesting robots can improve the adaptability, stability, and navigation accuracy of pineapple harvesting robots operating in complex environments.

Description

Technical Field

[0001] This invention belongs to the field of agricultural intelligent equipment technology, specifically relating to an automatic navigation method for pineapple field harvesting robots based on multimodal data fusion. Background Technology

[0002] Pineapples are an important economic crop in tropical and subtropical regions, and their traditional harvesting methods mainly rely on manual labor. However, manual harvesting suffers from high labor intensity, low efficiency, and high costs, making it difficult to meet the demands of efficient modern agricultural production. With the development of agricultural mechanization and intelligent technologies, agricultural robots are gradually becoming an important direction for solving this problem.

[0003] Existing agricultural robot navigation technologies mainly include navigation methods based on mechanical tracks, satellite navigation, lidar, and machine vision. Early navigation systems relied heavily on mechanical tracks or ultrasonic sensors. While these systems offered a degree of stability, their flexibility was limited by the layout of physical facilities, making them difficult to adapt to complex field environments. Satellite navigation technology improved the accuracy of robot positioning and navigation to some extent, but GNSS signals are easily blocked, especially in mountainous or densely vegetated areas, where navigation performance significantly degrades. LiDAR, due to its advantages in obstacle detection and path planning, is also widely used in agricultural robot navigation, but its high equipment cost and susceptibility to interference from strong light and dusty environments limit its large-scale application.

[0004] To improve the adaptability of agricultural robots in complex environments, machine vision-based navigation technologies have emerged in recent years. Machine vision uses cameras to capture environmental information and algorithms to extract navigation paths, offering advantages such as low cost and wide applicability. However, single-vision navigation systems are prone to decreased recognition accuracy in complex environments with varying lighting, background interference, and vegetation occlusion. Therefore, multi-sensor fusion technology has become an important direction for improving robot navigation performance. Multi-sensor fusion technology integrates data from multiple sensors, including vision, inertial sensors, and satellite navigation, to achieve accurate perception of the robot's position and path, improving the robustness of the navigation system. However, current multi-sensor fusion methods generally employ traditional data processing algorithms, which have weak adaptability to nonlinear and multivariable conditions, exhibiting certain limitations in dynamic field environments.

[0005] In addition, in terms of navigation path tracking control, PD fuzzy controllers or model predictive controllers (MPC) are currently mostly used. Among them, PD fuzzy controllers are easy to implement, but they are less adaptable to nonlinearity and uncertainty in complex environments. MPC performs better than traditional control methods in path tracking, but due to the complexity of parameter tuning and limited adaptability to dynamic environments, there is still room for improvement in its performance.

[0006] In summary, existing machine vision-based navigation technologies have the following shortcomings:

[0007] First, single-sensor navigation systems are poorly adaptable to complex field environments and are easily affected by external interference. Second, multi-sensor data fusion algorithms have limited adaptability to nonlinear and multivariable environments, and their accuracy needs further improvement. In addition, path tracking control algorithms exhibit insufficient robustness in dynamic environments, limiting their control accuracy. Furthermore, some navigation methods are costly or complex to deploy, making it difficult to meet the needs of practical agricultural applications. Summary of the Invention

[0008] To overcome the shortcomings of existing technologies, this invention provides an automatic navigation method for pineapple field harvesting robots based on multimodal data fusion. This automatic navigation method can improve the adaptability, stability, and navigation accuracy of pineapple field harvesting robots in complex agricultural environments.

[0009] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0010] An automated navigation method for pineapple field harvesting robots based on multimodal data fusion includes the following steps:

[0011] Step S1: Transform the coordinates of the vision sensor, inertial sensor, and satellite navigation sensor in the pineapple field harvesting robot to the same coordinate system;

[0012] Step S2: Acquire images of the pineapple field, preprocess the images, and extract navigation line information from the preprocessed images.

[0013] Step S3: Use the extended Kalman filter algorithm to fuse information from satellite navigation sensors and inertial sensors to obtain the pose information of the pineapple harvesting robot.

[0014] Step S4: Use the extended Kalman filter algorithm to fuse information from the photoelectric encoder and inertial sensor to obtain the mileage information of the pineapple field harvesting robot:

[0015] Step S5: Input the navigation line information, pose information and odometer information into the model predictive controller optimized by the gray wolf optimization algorithm. The optimized model predictive controller outputs the optimal control information to the pineapple field harvesting robot, thereby controlling the pineapple field harvesting robot to autonomously track the navigation path.

[0016] Preferably, in step S1, the coordinate systems of the vision sensor and inertial sensor in the pineapple field harvesting robot are transformed to the world coordinate system, and the corresponding transformation matrix is ​​obtained.

[0017] Preferably, in step S2, a pineapple recognition model is used to identify pineapples in the pineapple field image, the pixel coordinates of the center point of the identified pineapple are calculated, agglomerative hierarchical clustering algorithm is used to cluster the center points of the identified pineapples, and the Manhattan distance or Euclidean distance least squares method is used to fit a straight line to each cluster point, and the average value of the abscissa of each straight line is calculated as the abscissa of the navigation line.

[0018] Preferably, in step S2, the construction steps of the pineapple recognition model are as follows:

[0019] Step S201: Collect pineapple field images. The collected pineapple field images include pineapple field images under different shading conditions, different lighting conditions, and different background conditions, which constitute the original dataset.

[0020] Step S202: Perform data augmentation on the constructed original dataset, wherein the data augmentation includes random cropping, rotation, and color transformation;

[0021] Step S203: Divide the dataset into training, testing, and validation sets in a 7:2:1 ratio;

[0022] Step S204: Label the pineapple field images in the training and validation sets to generate labeled images;

[0023] Step S205: Set the parameters of the YOLOv10 neural network model, train the YOLOv10 neural network model using the training set, and then validate the YOLOv10 neural network model using the validation set to obtain the trained pineapple recognition model.

[0024] Preferably, in step S205, the parameters of the YOLOv10 neural network model are set as follows: the initial learning rate is 0.01; the minimum learning rate is 0.001; and the remaining parameters are automatically adjusted using the cosine annealing algorithm.

[0025] Preferably, in step S3, during the process of fusing information from satellite navigation sensors and inertial sensors using the extended Kalman filter algorithm, the state vector used is:

[0026]

[0027] In the formula: lat is latitude; lng is longitude; v is speed; yaw is heading angle;

[0028] The covariance matrix used is:

[0029]

[0030] In the formula: These represent the variances of latitude, longitude, speed, and heading angle, respectively.

[0031] The state prediction equation used is:

[0032] lat k|k-1 =lat k-1 +Δt×v k-1 ×cos(yaw k-1 );

[0033] lng k|k-1 =lng k-1 +Δt×v k-1 ×cos(yaw k-1 );

[0034] v k|k-1 =v k-1 ;

[0035] yaw k|k-1 =yaw k-1 ;

[0036] In the formula: lat k-1 The latitude of the previous moment; lng k-1 The longitude at the previous moment; v k-1 The speed at the previous moment; yaw k|k-1 The heading angle at the previous moment; Δt is the time interval;

[0037] The measurement prediction equation used is:

[0038]

[0039] Preferably, in step S4, during the process of fusing the information from the photoelectric encoder and the inertial sensor using the extended Kalman filter algorithm, the state vector x2, control vector u2, and observation vector z used are as follows:

[0040] x2=(q,v x ,v y ,p x ,p y ) T ;

[0041] u2=(a x ,a y ,ω) T ;

[0042] z = (s, θ, v) T ;

[0043] In the formula: p x p yv represents the displacement along the X and Y axes in the world coordinate system. x and v y q represents the velocity along the X and Y axes in the world coordinate system; q represents the rotation angle in the world coordinate system; a x and a y ω represents the acceleration along the X and Y axes in the inertial sensor coordinate system; ω represents the angular velocity around the Z axis in the inertial sensor coordinate system; s, θ, and v represent the mileage, rotation angle, and travel speed of the pineapple field harvesting robot in the world coordinate system, respectively.

[0044] The observation model equations used are as follows:

[0045]

[0046] θ k =q k -q k-1 ;

[0047]

[0048] The state update equation used is:

[0049]

[0050] In the formula: q k-1 The heading angle at the previous moment. q k Let θ be the heading angle at the current moment. k v is the rotation angle at the current moment in the world coordinate system. x v y These are the velocities along the X and Y axes in the world coordinate system at the current moment; These represent the displacements along the X and Y axes in the world coordinate system at the previous moment, respectively. and s represents the displacement along the X and Y axes in the world coordinate system at the current moment; k v represents the distance at the current moment in the world coordinate system. k The speed at the current moment; These are the corrected angular velocity and acceleration, respectively.

[0051] Preferably, in step S5, the state variable x3 and control variable u3 of the model prediction controller are respectively:

[0052]

[0053] In the formula: v is the speed of the pineapple harvesting robot; s represents pose; s represents mileage; v represents position. left v rightThese are the left and right wheel speeds of the mobile chassis in the pineapple field harvesting robot, respectively.

[0054] The state transition equation of the model predictive controller is as follows:

[0055]

[0056] In the formula: L is the wheelbase of the mobile chassis; dt is the time step; v k+1 The speed at the next moment; The pose at the next moment; s k+1 This is the mileage for the next moment.

[0057] Preferably, in step S5, the Grey Wolf optimization algorithm is used to optimize the Q matrix and R matrix in the model predictive controller at each time step. The specific process is as follows:

[0058] Step S501: Determine the population size N of gray wolves and the dimension D of the search space; randomly initialize the positions of gray wolves, with each position representing a set of parameter values ​​for a Q matrix and an R matrix;

[0059] Step S502: Construct the objective function with the premise of minimizing error and control cost;

[0060] Step S503: For each gray wolf, i.e., for each set of Q and R parameter values, substitute them into the objective function to calculate the fitness value;

[0061] Step S504: Find the three gray wolves with the best fitness values ​​in the current population and label them as α wolf, β wolf and δ wolf, where α wolf, β wolf and δ wolf represent the optimal solution, the second best solution and the third best solution, respectively;

[0062] Step S505: Search for the optimal control input by continuously updating the positions of α wolf, β wolf, and δ wolf; stop iterating when the maximum number of iterations is reached or the convergence condition is met.

[0063] Step S506: After the iteration is completed, the parameter values ​​of the Q matrix and R matrix corresponding to the position of wolf α are used as the optimized result.

[0064] Preferably, in step S5, the population size in the gray wolf optimization algorithm is set to 10, the maximum number of iterations is 10, and the MPC optimization problem is solved using the CVXPY high-precision solver.

[0065] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0066] 1. The automatic navigation method for pineapple picking robots based on multimodal data fusion of the present invention combines visual sensors, inertial sensors and satellite navigation. It achieves multi-sensor data fusion through extended Kalman filter (EKF), uses YOLOv10 neural network to detect the position of pineapple in real time, extracts navigation lines by aggregated hierarchical clustering and least squares method, and achieves high-precision path tracking by combining the improved model predictive controller (GWO-MPC) with the gray wolf optimization algorithm.

[0067] 2. The automatic navigation method for pineapple harvesting robots based on multimodal data fusion of this invention utilizes the Extended Kalman Filter (EKF) algorithm for data fusion, improving the system's perception accuracy in complex environments. Theoretical analysis shows that the EKF algorithm can effectively reduce sensor noise and accumulated errors. Combined with multi-sensor collaboration, it avoids the failure problem of a single sensor under conditions such as changes in lighting and signal obstruction. Simultaneously, the model predictive control method improved by the Grey Wolf Optimization algorithm overcomes the shortcomings of traditional control algorithms, such as oscillation and overshoot on complex paths. The Grey Wolf Optimization algorithm can dynamically adjust control parameters, significantly improving the accuracy and robustness of path tracking. Furthermore, experimental results in field operations show that the heading angle error range is -2.35 to 4.31°, the maximum deviation is 13.25 cm, and the relative mileage error is 13.45%. The system can operate stably in complex field terrain and environments, fully verifying the applicability and robustness of the automatic navigation method for pineapple harvesting robots based on multimodal data fusion in actual agricultural operations.

[0068] 3. This invention utilizes the YOLOv10 algorithm to perform real-time detection of pineapple fruits. By extracting the center point of the pineapple and combining clustering and fitting techniques, a navigation path is generated. This method overcomes the limitations of traditional path-line-based navigation techniques in occluded field scenarios. Experimental results show that the average processing time for the navigation path extracted using this method is 0.238 seconds per photo, and the error of the extracted navigation path is around 1°.

[0069] 4. The automatic navigation method for pineapple picking robots based on multimodal data fusion of the present invention can effectively solve the problems of adaptability and accuracy of existing visual navigation technology in complex environments, and provide an economical and practical navigation solution for the application of agricultural robots in field operations. Attached Figure Description

[0070] Figure 1 This is a structural block diagram of the automatic navigation system for pineapple field harvesting robots based on multimodal data fusion, as described in this invention.

[0071] Figure 2 This is a flowchart illustrating the automatic navigation method for pineapple field harvesting robots based on multimodal data fusion according to the present invention.

[0072] Figure 3 This is a flowchart illustrating the process of extracting navigation lines.

[0073] Figure 4 Images of pineapple fields against a complex background were collected.

[0074] Figure 5 These are the original images and their corresponding labeled images under different environmental conditions.

[0075] Figure 6 The image shows the detection performance of the pineapple recognition model under different environmental conditions.

[0076] Figure 7 A flowchart (shown as a result) for extracting navigation lines.

[0077] Figure 8 A flowchart illustrating the extended Kalman filter algorithm.

[0078] Figure 9 This is a schematic diagram of a two-wheel differential drive model.

[0079] Figure 10 This is a flowchart illustrating the trajectory tracking algorithm based on GWO-MPC in this invention. Detailed Implementation

[0080] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0081] See Figures 1-10 The automatic navigation system for a pineapple field harvesting robot based on multimodal data fusion of the present invention includes a vision sensor, an inertial measurement unit (IMU), a satellite navigation sensor (GNSS), an optical encoder, and a control unit mounted on the mobile chassis of the pineapple field harvesting robot.

[0082] The mobile chassis adopts a tracked chassis, which has a larger ground contact area compared to traditional wheeled chassis, resulting in stronger obstacle-crossing ability and terrain adaptability. It can maintain stable operation, especially in complex environments such as wet and soft ground, muddy terrain, and undulating terrain. The main body of the tracked chassis adopts a gantry structure, which allows the pineapple harvesting robot to adapt to the planting characteristics of pineapple fields and achieve row-crossing movement. This not only effectively reduces damage to crops but also significantly improves the flexibility and passability of pineapple field operations, providing a reliable guarantee for efficient operation in complex terrain. The parameter settings of the mobile chassis are shown in Table 1.

[0083] Table 1: Specific parameters of the mobile chassis

[0084]

[0085] The vision sensor uses an Intel D455 depth camera;

[0086] The inertial sensor used is the MPU6050 sensor, which has built-in DMP (Digital Motion Processing) firmware. The DMP firmware has an automatic deviation calibration function and uses quaternions to represent attitude angles, which can effectively avoid the gimbal lock problem that occurs when using Euler angles during large-angle rotation.

[0087] After initialization, the satellite navigation sensor can search for the four satellites required for positioning and obtain the positioning result within 42 seconds. In addition, in order to reduce the burden of data processing, this embodiment mainly reads statements in "RMC" format, which contain information such as timestamps, latitude and longitude, ground speed and heading to the ground required by subsequent algorithms.

[0088] The control unit includes a PC, a microcontroller, and an SBMCB5 controller. In addition, the mobile chassis can also be remotely controlled by a remote controller.

[0089] The above describes the basic structure of the automatic navigation system for pineapple field harvesting robots based on multimodal data fusion of the present invention. The following describes the automatic navigation method for pineapple field harvesting robots based on multimodal data fusion of the present invention based on these basic structures.

[0090] See Figures 1-10 The present invention provides an automatic navigation method for pineapple field harvesting robots based on multimodal data fusion, comprising the following steps:

[0091] Step S1: Transform the coordinates of the vision sensor, inertial sensor, and satellite navigation sensor in the pineapple field harvesting robot to the same coordinate system;

[0092] In this embodiment, to unify accurate navigation and positioning from different sensors, the coordinate system of each sensor must be transformed into world coordinates. Since the data obtained by the satellite navigation sensor is in the world coordinate system, no transformation is required. Therefore, only the coordinate systems of the visual sensor and the inertial sensor need to be transformed into the world coordinate system. Specifically:

[0093] In inertial sensors, the geographic coordinate system must be rotated three times in the order of the z, x, and y axes to align with the world coordinate system. Therefore, the transformation matrix for converting the geographic coordinate system to the world coordinate system is... as follows:

[0094]

[0095] In the formula: γ is the yaw angle, γ is the roll angle, and θ is the pitch angle.

[0096] The transformation equation for converting pixel coordinates in the image coordinate system of a vision sensor to the world coordinate system is as follows:

[0097]

[0098] In the formula: (u,v) are pixel coordinates; Z c For depth; (X) w ,Y w Z w () represents world coordinates; This is the camera intrinsic parameter matrix; This is the camera extrinsic parameter matrix.

[0099] Step S2: Acquire images of the pineapple field, preprocess the images, and extract navigation line information from the preprocessed images.

[0100] In this embodiment, a pineapple recognition model is used to identify pineapple fruits in pineapple field images. The construction steps of the pineapple recognition model are as follows:

[0101] Step S201: Collect pineapple field images. The collected pineapple field images include pineapple field images under different shading conditions, different lighting conditions, and different background conditions, which constitute the original dataset.

[0102] In this embodiment, the image acquisition took place in a pineapple plantation in Shenwan Town, Zhongshan City, Guangdong Province, China. During the image acquisition process, the mobile chassis was manually controlled to move at a constant speed of approximately 0.2 meters per second to simulate normal operation. The industrial camera captured images at a resolution of 1280x720 pixels and a frame rate of 30 frames per second. Figure 4 The image data collected from complex pineapple fields is presented, including 2,624 images of pineapples and 79 images of field roads, including those without field roads. Figure 4 (a) in the middle, and packaged pineapple ( Figure 4 (b) in the middle), wide road ( Figure 4 (c) and narrow road ( Figure 4 The case of (d) in the example.

[0103] Step S202: Perform data augmentation on the constructed original dataset, wherein the data augmentation includes random cropping, rotation, and color transformation;

[0104] In this embodiment, in order to address the complexity of the field environment, the collected original dataset images include complex backgrounds, varying lighting conditions, and occlusions. Therefore, data augmentation techniques (such as random cropping, rotation, and color conversion) are employed to further enhance the model's generalization ability. In addition, 24 images were excluded from the original dataset because they were excessively blurry.

[0105] Step S203: Divide the dataset into training, testing, and validation sets in a 7:2:1 ratio;

[0106] In this embodiment, the remaining images are automatically and randomly divided into a training set, a validation set, and a test set in a ratio of 7:2:1 using a script; wherein the training set contains 1,828 images, the validation set contains 517 images, and the test set contains 263 images.

[0107] Step S204: Label the pineapple field images in the training and validation sets to generate labeled images;

[0108] In this embodiment, Labellme is used to annotate images in the training and validation sets to generate labeled images; Figure 5 This presents the original images and their corresponding labeled datasets under different environmental conditions, including normal images ( Figure 5 (a) in the middle), complex background image ( Figure 5 (b) in the image and the highlight image ( Figure 5 (c) in the middle.

[0109] Step S205: Set the parameters of the YOLOv10 neural network model, train the YOLOv10 neural network model using the training set, and then validate the YOLOv10 neural network model using the validation set to obtain the trained pineapple recognition model.

[0110] In this embodiment, the parameters of the YOLOv10 neural network model are set as follows: initial learning rate of 0.01; minimum learning rate of 0.001; and other parameters are automatically adjusted using cosine annealing (CAA) algorithm to accelerate convergence, reduce training time, and improve the accuracy of the pineapple detection model. This also ensures that the training model meets the requirements of speed and accuracy in the complex environment of pineapple fields.

[0111] The formula for the Cosine Annealing (CAA) algorithm is as follows:

[0112]

[0113] In the formula: η t Let η be the learning rate for the t-th iteration. min For the minimum learning rate, η max For the maximum learning rate, Tcur T is the current iteration number. max This represents the total number of iterations.

[0114] The recognition accuracy of the pineapple recognition model in this invention will be described below.

[0115] In this embodiment, the following four metrics are used: precision (P), recall (R), mean precision (mAP), and F1 score. The specific calculation formulas are as follows:

[0116]

[0117] In the formula: TP represents true positives; FP represents false positives; FN represents false negatives; N represents the total number of target classes; AP i Let be the average precision of the i-th category;

[0118] Table 2: Training results of six different YOLVO10 variant models under different modes

[0119]

[0120] In normal training mode (i.e., without using the annealing cosine algorithm), the training parameters are set as follows:

[0121] The initial learning rate is 0.01, the learning decay coefficient is 0.0005, the minimum learning rate is 0.001, the decay rate is 0.5, and the batch size is 16.

[0122] In the training mode combined with the annealing cosine algorithm, the training parameters are set to an initial learning rate of 0.01 and a minimum learning rate of 0.001, while other parameters are set automatically by the annealing cosine algorithm.

[0123] As shown in Table 2, the YOLOv10-S and YOLOv10-M variant models exhibit superior performance after integrating CAA, with significant improvements in recall, precision, and mAP50. The YOLOv10-S variant model trained with CAA shows the most significant performance improvement, achieving a precision of 0.868 and a mean precision (mAP50-95) of 0.592. Furthermore, the lower validation loss of the YOLOv10-S variant model indicates its strong generalization ability and stability, highlighting the effectiveness of CAA in optimizing the training process. Other YOLOv10 variant models also show varying degrees of performance improvement after integrating CAA.

[0124] Using the constructed pineapple recognition model, the detection performance under different conditions is as follows: Figure 6 As shown, where, Figure 6 (a) in the image represents normal lighting and a complex background; Figure 6 (b) in the image represents a bright environment with a complex background; Figure 6 (c) represents a low-light environment with a complex background; Figure 6 (d) in the text represents normal lighting; Figure 6 (e) in the image represents a high-light environment; Figure 6 (f) represents a low-light environment; the experimental data from multiple experiments are summarized in Table 3:

[0125] Table 3: Index parameters under different conditions

[0126]

[0127] As shown in Table 3, the pineapple recognition model of the present invention performs best under normal lighting conditions. With the increase of the number of images, the recall and precision are significantly improved, resulting in a higher F1 score. Under complex background, strong light and weak light conditions, the performance of the model decreases, but it is still within an acceptable range and meets the experimental requirements.

[0128] Overall, different lighting and background environments have a significant impact on model performance; however, since the dataset contains images with various lighting and background conditions, external interference can be effectively reduced, enhancing the robustness and stability of the model.

[0129] In addition, before determining the location of the pineapple, internal parameters need to be used to correct image distortion to ensure that the extracted pineapple center coordinates are accurate and reliable.

[0130] Once the pineapple recognition model identifies the pineapple, it calculates the pixel coordinates of the center point of the identified pineapple fruit, specifically:

[0131] Use bounding boxes to mark the detected pineapples and output the pixel coordinates of the four corners of the bounding boxes. Then use the following formula to calculate the pixel coordinates of the center point of the pineapple.

[0132]

[0133] In the formula: p 20 and p 10 These represent the x-coordinates of the bottom right and top left corners of the bounding box, respectively; p 21 and p 11 These represent the ordinates of the bottom right and top left corners of the bounding box, respectively.

[0134] Next, the agglomerative hierarchical clustering algorithm (AHC) is used to cluster the center points of the identified pineapple fruits. For each cluster point, the least squares method of Manhattan distance or Euclidean distance is used to fit a straight line, and the average value of the x-coordinate of each straight line is calculated as the x-coordinate of the navigation line.

[0135]

[0136] Figure 7 The process and results of navigation line extraction are shown, including the following two cases.

[0137] The first category represents pineapples that have grown normally: Figure 7 (a) in the image is the original image; Figure 7 (b) in the diagram is a schematic diagram after the pineapple's location has been identified; Figure 7 (c) in the diagram is a schematic diagram after processing using the agglomerative hierarchical clustering algorithm; Figure 7 (d) in the diagram is a schematic diagram after fitting the straight line and the navigation line;

[0138] The second category includes pineapples affected by bagging and lodging: Figure 7 (e) in the image is the original image; Figure 7 (f) in the diagram is a schematic diagram after the pineapple's location is identified; Figure 7 (g) in the diagram is a schematic diagram after processing using the agglomerative hierarchical clustering algorithm; Figure 7 (h) in the diagram is a schematic diagram after fitting the straight line and the navigation line;

[0139] The average processing time for extracting navigation lines using the method of this invention is 0.238 seconds per image, demonstrating stronger adaptability and real-time performance.

[0140] Step S3: Use the extended Kalman filter algorithm to fuse information from satellite navigation sensors and inertial sensors to obtain the pose information of the pineapple harvesting robot.

[0141] In this embodiment, the state vector x1 used is:

[0142]

[0143] In the formula: lat is latitude; lng is longitude; v is speed; yaw is heading angle;

[0144] The covariance matrix used is:

[0145]

[0146] In the formula: These represent the variances of latitude, longitude, speed, and heading angle, respectively.

[0147] The state prediction equation used is:

[0148] lat k|k-1 =lat k-1 +Δt×v k-1 ×cos(yaw k-1 );;

[0149] lng k|k-1 =lng k-1 +Δt×vk-1 ×cos(yaw k-1 );

[0150] v k|k-1 =v k-1 ;

[0151] yaw k|k-1 =yaw k-1 ;

[0152] In the formula: lat k-1 The latitude of the previous moment; lng k-1 The precision of the previous time step; v k-1 The velocity at the previous moment; yaw k|k-1 The heading angle at the previous moment; Δt is the time interval;

[0153] The measurement prediction equation used is:

[0154]

[0155] By iteratively performing the prediction and update steps, a more accurate and stable state vector can be provided; the specific process of the extended Kalman filter algorithm can be found in [reference needed]. Figure 8 The fused information is then used as the pose information for the mobile chassis.

[0156] The accuracy and stability of the algorithm are evaluated by comparing the latitude, longitude, speed and yaw angle estimated by the fusion algorithm with the actual measured values. The specific evaluation indicators include the errors of latitude, longitude, speed and yaw angle. All errors are expressed as root mean square error (RMSE) to quantify the performance of the algorithm. The error analysis is shown in Table 4.

[0157] Table 4: Condition Error of Mobile Chassis

[0158]

[0159]

[0160] Over time, the predictions of the Extended Kalman Filter (EKF) become more accurate, although latitude and longitude errors increase slightly but remain at a low level overall. Speed ​​and yaw angle errors gradually decrease, and the yaw angle maintains high accuracy after six time steps, making it suitable for navigation applications.

[0161] Step S4: Use the extended Kalman filter algorithm to fuse information from the photoelectric encoder and inertial sensor to obtain the mileage information of the pineapple field harvesting robot:

[0162] In this embodiment, the state vector x2, control vector u2, and observation vector z used are as follows:

[0163] x2=(q,v x ,v y ,p x ,p y ) T ;

[0164] u2=(a x ,a y ,ω) T ;

[0165] z = (s, θ, v) T ;

[0166] In the formula: p x p y v represents the displacement along the X and Y axes in the world coordinate system. x and v y q represents the velocity along the X and Y axes in the world coordinate system; q represents the rotation angle in the world coordinate system; a x and a y ω represents the acceleration along the X and Y axes in the inertial sensor coordinate system; ω represents the angular velocity around the Z axis in the inertial sensor coordinate system; s, θ, and v represent the displacement, rotation angle, and speed of the pineapple field harvesting robot in the world coordinate system, respectively.

[0167] To ensure the accuracy of the information provided by the inertial sensors, they need to be calibrated before data fusion to obtain the accelerometer bias. and gyroscope bias

[0168]

[0169] The observation model equations used are as follows:

[0170]

[0171] θ k =q k -q k-1 ;

[0172]

[0173] The state update equation used is:

[0174]

[0175]

[0176] In the formula: q k-1 The heading angle at the previous moment. q k Let θ be the heading angle at the current moment.k v is the rotation angle at the current moment in the world coordinate system. x v y These are the velocities along the X and Y axes in the world coordinate system at the current moment; These represent the displacements along the X and Y axes in the world coordinate system at the previous moment, respectively. and s represents the displacement along the X and Y axes in the world coordinate system at the current moment; k v represents the current distance (or displacement) in the world coordinate system; k The speed at the current moment; These are the corrected angular velocity and acceleration, respectively.

[0177] By analyzing the system prediction equation with respect to the state vector x2 k-1 We derive the Jacobian matrix of the system model equations by taking the derivative; similarly, we derive the observation model equations with respect to the state vector x2. k The derivative is used to obtain its Jacobian matrix;

[0178] During the movement of the mobile chassis, mileage information is obtained by iteratively calculating each time step according to the Extended Kalman Filter (EKF) program. The EKF algorithm's process is similar to... Figure 8 The pattern shown is similar.

[0179] The performance of the information fusion algorithm under different road conditions was evaluated. The results show that the fusion algorithm exhibits high accuracy and robustness on both flat and bumpy roads. On flat roads, the experimental results show that the average error between the mileage calculated by the information fusion algorithm and the actual measurement value is 6.64%, indicating that it has good tracking ability. Although the error increases on bumpy roads, the average error only rises to 11.11%, which shows that the information fusion algorithm of the present invention can effectively adapt to complex environments while ensuring reliable mileage measurement.

[0180] Step S5: Input the navigation line information, pose information and odometer information into the model predictive controller optimized by the gray wolf optimization algorithm. The optimized model predictive controller outputs the optimal control information to the pineapple field harvesting robot, thereby controlling the pineapple field harvesting robot to autonomously track the navigation path.

[0181] In this embodiment, in order to apply the (MPC) method, the nonlinear kinematic model of the mobile chassis must be approximated as linear. Therefore, the mobile chassis is simplified to a two-wheel differential drive model, i.e., as follows: Figure 9 As shown; where the state variable x3 and control variable u3 of the model predictive controller (MPC) are defined as follows:

[0182]

[0183] In the formula: v is the average speed of the pineapple harvesting robot. s represents the pose (e.g., yaw / heading angle), and s represents the mileage; v left v right These are the left and right wheel speeds of the mobile chassis in the pineapple field harvesting robot, respectively.

[0184] The state transition equation of the model predictive controller is as follows:

[0185]

[0186] In the formula: L is the wheelbase of the mobile chassis; dt is the time step; v k+1 The speed at the next moment; The pose (e.g., yaw / heading angle) at the next moment; s k+1 This is the odometer reading (i.e., mileage) for the next moment;

[0187] The MPC optimization problem was solved using the CVXPY high-precision solver to obtain the optimal control input for adjusting the speed and heading angle of the mobile chassis.

[0188] Furthermore, since the Q matrix measures the error between the system state and the reference trajectory, while the R matrix measures the magnitude of the control input, the Grey Wolf optimization algorithm is used to optimize the Q and R matrices in the Model Predictive Controller (MPC) at each time step. The specific process is as follows:

[0189] Step S501: Determine the population size N of the gray wolves and the dimension D of the search space; randomly initialize the positions of the gray wolves, with each position representing a set of parameter values ​​for the Q matrix and R matrix; in this embodiment, the population size in the gray wolf optimization algorithm is set to 10, and the maximum number of iterations is 10.

[0190] Step S502: Construct the objective function with the premise of minimizing error and control cost;

[0191] Step S503: For each gray wolf, i.e., for each set of Q and R parameter values, substitute them into the objective function to calculate the fitness value;

[0192] Step S504: Find the three gray wolves with the best fitness values ​​in the current population and label them as α wolf, β wolf and δ wolf, where α wolf, β wolf and δ wolf represent the optimal solution, the second best solution and the third best solution, respectively;

[0193] Step S505: Search for the optimal control input by continuously updating the positions of α wolf, β wolf, and δ wolf; stop iterating when the maximum number of iterations is reached or the convergence condition is met.

[0194] Step S506: After the iteration is completed, the parameter values ​​of the Q matrix and R matrix corresponding to the position of the α wolf are used as the optimized results and output to the Model Predictive Controller (MPC). The Model Predictive Controller (MPC) outputs the optimal control information to the pineapple field harvesting robot based on the navigation line information, pose information and odometer information, thereby controlling the pineapple field harvesting robot to autonomously achieve navigation path tracking.

[0195] The autonomous navigation method of this invention is shown in Table 5:

[0196] Table 5: Experimental data from the pineapple field

[0197]

[0198]

[0199] As shown in Table 5, the navigation system using GWO-MPC achieved decimeter-level accuracy without damaging the pineapple plants; the yaw angle error was between -2.35° and 4.31°, the maximum lateral offset error was 13.25 cm, and the relative ranging error was 4.65%.

[0200] Finally, this invention proposes an automatic navigation system and method for a pineapple harvesting robot based on multimodal data fusion. It utilizes visual sensors to detect the pineapple's position and then extracts a navigation line based on this information. An extended Kalman filter (EKF) is used to fuse sensor data to obtain the pose and odometer information of the moving chassis. The Grey Wolf Optimization Algorithm-Model Predictive Control (GWO-MPC) is applied to navigation path tracking to achieve pose and odometer data fusion for the moving chassis. Experiments conducted in a pineapple field demonstrate that:

[0201] (1) The navigation line extraction method, mobile chassis pose acquisition method and odometer information acquisition method proposed in this invention meet the navigation requirements of the pineapple field harvesting robot, and the gray wolf optimization algorithm can effectively balance the real-time performance and accuracy of model predictive control.

[0202] (2) Although strong light and uneven terrain will affect navigation performance, the error is still within an acceptable range.

[0203] (3) Compared with the navigation experiment under ideal conditions, the error in the pineapple field navigation increased, but did not damage the crops; the pineapple field navigation experiment showed that the error range of the heading angle was -2.35° to 4.31°, the error of the relative odometer was 13.45%, and the error of the maximum lateral offset was 13.25 cm; the final results showed that the navigation method proposed in this invention has high navigation accuracy and strong robustness.

[0204] The above are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above content. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A method for automatic navigation of a pineapple field harvesting robot based on multi-modal data fusion, characterized in that, The method comprises the following steps: Step S1: the coordinates of the vision sensor, inertial sensor and satellite navigation sensor in the pineapple field harvesting robot are transformed into the same coordinate system; Step S2: a pineapple field image is collected, and navigation line information is extracted from the pineapple field image after preprocessing; Step S3: the information of the satellite navigation sensor and the inertial sensor is fused by using an extended Kalman filtering algorithm to obtain the pose information of the pineapple field harvesting robot; Step S4: the information of the optical encoder and the inertial sensor is fused by using an extended Kalman filtering algorithm to obtain the mileage information of the pineapple field harvesting robot; Step S5: the navigation line information, the pose information and the mileage information are input into a model predictive controller optimized by using a grey wolf optimization algorithm, and optimal control information is output by the model predictive controller to the pineapple field harvesting robot, so that the pineapple field harvesting robot autonomously realizes navigation path tracking.

2. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 1, characterized in that, In step S1, the coordinate systems of the vision sensor and the inertial sensor in the pineapple field harvesting robot are transformed into the world coordinate system, and the corresponding transformation matrix is obtained.

3. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 2, characterized in that, In step S2, the pineapple recognition model is used to identify the pineapples in the pineapple field image, the pixel coordinates of the center points of the identified pineapples are calculated, the condensation hierarchical clustering algorithm is used to cluster the center points of the identified pineapples, the least square method of Manhattan distance or Euclidean distance is used to fit a straight line for each cluster point, and the average value of the abscissa of each straight line is calculated as the abscissa of the navigation line.

4. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 3, characterized in that, In step S2, the construction steps of the pineapple recognition model are as follows: Step S201: collect pineapple field images, and the collected pineapple field images include pineapple field images under different occlusion conditions, pineapple field images under different light conditions and pineapple field images under different background conditions to form an original data set; Step S202: data augmentation is performed on the constructed original data set, wherein the data augmentation includes random cropping, rotation and color conversion; Step S203: the data set is divided into a training set, a test set and a validation set in a ratio of 7:2:1; Step S204: the pineapple field images in the training set and the validation set are labeled to generate labeled images; Step S205: the parameters of the YOLOv10 neural network model are set, the YOLOv10 neural network model is trained by using the training set, and then the YOLOv10 neural network model is verified by using the validation set, so as to obtain the trained pineapple recognition model.

5. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 4, characterized in that, In step S205, the parameters of the YOLOv10 neural network model are set as follows: the initial learning rate is 0.01; the minimum learning rate is 0.001; and the remaining parameters are automatically adjusted by using a cosine annealing algorithm.

6. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 4, characterized in that, In step S3, in the process of fusing the information of the satellite navigation sensor and the inertial sensor by using the extended Kalman filtering algorithm, the state vector used is: Wherein: lat is the latitude; lng is the longitude; v is the driving speed; and yaw is the heading angle; The covariance matrix used is: In the formulae: respectively denote the variance of latitude, longitude, travel speed and heading angle; The state prediction equation used is: lat k|k-1 = lat k-1 + Δt x v k-1 x cos(yaw k- 1); lng k|k-1 = lng k-1 + Δt × v k-1 × cos(yaw k-1 ) v k|k-1 = v k-1 ; yaw k|k-1 yaw k-1 yaw where: lat k-1 is the latitude at the previous time; lng k-1 is the longitude at the previous time; v k-1 is the travel speed at the previous time; yaw k|k-1 is the heading angle at the previous time; Δt is the time interval; The measurement prediction equation used is:

7. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 4, characterized in that, In step S4, in the process of fusing the information of the optical encoder and the inertial sensor by using the extended Kalman filtering algorithm, the state vector x2, the control vector u2 and the observation vector z used are respectively: x2 = (q, v x , p y , p x , v y ) T ; u2 = (a x ,a y ,ω) T ; z = (s, 0, v) T ; wherein: p x , p y are displacements along the X and Y axis directions in the world coordinate system; v x and v y are velocities along the X and Y axis directions in the world coordinate system; q is a rotation angle in the world coordinate system; a x and a y are accelerations along the X and Y axis directions in the inertial sensor coordinate system; ω is an angular velocity around the Z axis in the inertial sensor coordinate system; s, θ, and v are the distance traveled, the rotation angle, and the travel speed of the pineapple field harvesting robot in the world coordinate system, respectively; The observation model equation used is: θ k = q k - q k-1 ; The state update equation used is: In the formula: q k-1 is the heading angle at the last time point. k is the heading angle at the current time point. k is the rotation angle at the current time point in the world coordinate system. x , v y are the velocities along the X-axis and Y-axis directions at the current time point in the world coordinate system, respectively. are the displacements along the X-axis and Y-axis directions at the last time point in the world coordinate system, respectively. and are the displacements along the X-axis and Y-axis directions at the current time point in the world coordinate system, respectively. k is the distance at the current time point in the world coordinate system. k is the driving speed at the current time point. are the corrected angular velocity and acceleration, respectively.

8. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 7, characterized in that, In step S5, the state variable x3 and the control variable u3 of the model predictive controller are respectively: In the formula, v is the driving speed of the pineapple field harvesting robot; is the pose; s is the mileage; v left , v right are respectively the left wheel speed and the right wheel speed of the mobile chassis in the pineapple field harvesting robot; The state transition equation of the model predictive controller is as follows: where: L is the wheelbase of the mobile chassis; dt is the time step; v k+1 is the driving speed at the next time instant; is the pose at the next time instant; s k+1 is the distance traveled at the next time instant.

9. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 8, characterized in that, In step S5, the Q matrix and the R matrix in the model predictive controller are optimized by using the grey wolf optimization algorithm at each time step, and the specific process is as follows: Step S501: determine the population size N of the grey wolf and the dimension D of the search space; randomly initialize the position of the grey wolf, and the position of each grey wolf represents a set of parameter values of the Q matrix and the R matrix; Step S502: construct a target function on the premise of minimizing the error and the control cost; Step S503: for each grey wolf, that is, for each set of parameter values of Q and R, substitute it into the target function to calculate the fitness value; Step S504: find the three grey wolves with the best fitness values in the current population, and mark them as alpha wolf, beta wolf and delta wolf respectively, wherein the alpha wolf, the beta wolf and the delta wolf respectively represent the optimal solution, the suboptimal solution and the third optimal solution; Step S505: search for the optimal control input by constantly updating the positions of the alpha wolf, the beta wolf and the delta wolf; when the maximum number of iterations is reached or the convergence condition is met, stop iteration; Step S506: after the iteration is completed, the parameter values of the Q matrix and the R matrix corresponding to the position of the alpha wolf are taken as the optimized results.

10. The automatic navigation method of pineapple field harvesting robot based on multi-modal data fusion according to claim 9, characterized in that, In step S5, the population size in the grey wolf optimization algorithm is set to 10, the maximum number of iterations is 10, and the CVXPY high-precision solver is used to solve the MPC optimization problem.

Images