Pineapple field harvesting robot automatic navigation method based on multi-modal data fusion

Abstract

The invention discloses an automatic navigation method for a pineapple field harvesting robot based on multi-modal data fusion. Firstly, a plurality of sensors in the pineapple field harvesting robot are transformed into the same coordinate system; a pineapple field image is collected and preprocessed, and navigation line information is extracted from the preprocessed pineapple field image; the information of the satellite navigation sensor and the inertial sensor and the information of the photoelectric encoder and the inertial sensor are fused by using an extended Kalman filtering algorithm; and inputting the navigation line information, the pose information and the mileage information into a model prediction controller optimized by adopting a grey wolf optimization algorithm to obtain optimal control information, and controlling the pineapple field harvesting robot to autonomously realize path tracking according to the optimal control information. According to the automatic navigation method for the pineapple field harvesting robot, the adaptability, the stability and the navigation precision of the pineapple field harvesting robot in operation in a complex environment can be improved.

Description

Technical Field

[0001] The invention belongs to the technical field of agricultural intelligent equipment, and particularly relates to an automatic navigation method for a pineapple field harvesting robot based on multimodal data fusion. Background Art

[0002] Pineapple is an important economic crop in tropical and subtropical regions, and its traditional picking method mainly relies on manual operation. However, manual picking has problems such as high labor intensity, low efficiency, and high cost, which cannot meet the needs of efficient production in modern agriculture. With the development of agricultural mechanization and intelligent technology, agricultural robots have gradually become an important direction to solve this problem.

[0003] Existing agricultural robot navigation technologies mainly include navigation methods based on mechanical tracks, satellite navigation, laser radar and machine vision. Early navigation systems mostly relied on mechanical tracks or ultrasonic sensors. Although these systems have certain stability, they are limited by the layout of physical facilities, have poor flexibility and are difficult to adapt to complex field environments. The application of satellite navigation technology has improved the accuracy of robot positioning and navigation to a certain extent, but because GNSS signals are easily blocked, especially in mountainous areas or areas with dense vegetation, navigation performance will be significantly reduced. Laser radar is also widely used in agricultural robot navigation due to its advantages in obstacle detection and path planning, but its equipment cost is relatively high and it is easily disturbed by strong light and dusty environments, which limits its large-scale promotion and application.

[0004] In order to improve the adaptability of agricultural robots in complex environments, navigation technology based on machine vision has gradually emerged in recent years. Machine vision uses cameras to capture environmental information and extracts navigation paths through algorithms. It has the characteristics of low cost and wide applicability. However, a single visual navigation system is prone to reduced recognition accuracy in complex environments such as lighting changes, 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 such as vision, inertial sensors, and satellite navigation to achieve accurate perception of the robot's position and path, thereby improving the robustness of the navigation system. However, the current multi-sensor fusion methods generally use traditional data processing algorithms, which have weak adaptability to nonlinearity and multivariables and show 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 used. Although the PD fuzzy controller is easy to implement, it has poor adaptability to nonlinearity and uncertainty in complex environments; MPC performs better than traditional control methods in path tracking, but due to the complexity of parameter adjustment and limited adaptability to dynamic environments, its performance still has room for improvement.

[0006] In summary, the existing navigation technology based on machine vision has the following shortcomings:

[0007] First, the single-sensor navigation system has poor adaptability in complex field environments and is easily affected by external interference; second, the multi-sensor data fusion algorithm has limited adaptability to nonlinear and multi-variable environments, and its accuracy needs to be further improved; in addition, the path tracking control algorithm shows insufficient robustness in dynamic environments and limited control accuracy; in addition, some navigation methods are costly or complex to deploy, making it difficult to meet the actual application needs of agriculture. Summary of the invention

[0008] In order to overcome the shortcomings of the prior art, the present invention provides an automatic navigation method for a pineapple field harvesting robot based on multimodal data fusion, wherein the automatic navigation method for the pineapple field harvesting robot can improve the adaptability, stability and navigation accuracy of the pineapple field harvesting robot operating in a complex agricultural environment.

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

[0010] An automatic navigation method for a pineapple field harvesting robot based on multimodal data fusion comprises the following steps:

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

[0012] Step S2: collecting pineapple field images, preprocessing the pineapple field images, and extracting navigation line information from the preprocessed pineapple field images;

[0013] Step S3: Use the extended Kalman filter algorithm to fuse the information of the satellite navigation sensor and the inertial sensor to obtain the position information of the pineapple field harvesting robot:

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

[0015] Step S5: input the navigation line information, posture information and mileage information into the model prediction controller optimized by the Grey Wolf Optimization Algorithm, and output the optimal control information to the pineapple field harvesting robot through the optimized model prediction controller, so as to control the pineapple field harvesting robot to autonomously realize navigation path tracking.

[0016] Preferably, in step S1, the coordinate systems of the visual sensors and inertial sensors in the pineapple field harvesting robot are transformed into 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 points of the identified pineapples are calculated, the center points of the identified pineapples are clustered using an agglomerative hierarchical clustering algorithm, a straight line is fit to each cluster point using the least squares method of Manhattan distance or Euclidean distance, 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 steps of constructing the pineapple recognition model are:

[0019] Step S201: collecting pineapple field images, wherein the collected pineapple field images include pineapple field images under different occlusion conditions, pineapple field images under different illumination conditions, and pineapple field images under different background conditions, so as to form an original data set;

[0020] Step S202: performing data enhancement on the constructed original data set, wherein the data enhancement includes random cropping, rotation and color conversion;

[0021] Step S203: Divide the data set into a training set, a test set, and a validation set in a ratio of 7:2:1;

[0022] Step S204: marking the pineapple field images in the training set and the validation set to generate marked images;

[0023] Step S205: setting parameters of the YOLOv10 neural network model, training the YOLOv10 neural network model through the training set, and then verifying the YOLOv10 neural network model through the verification set, so as to obtain a trained pineapple recognition model.

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

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

[0026]

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

[0028] The covariance matrix used is:

[0029]

[0030] Where: Represent the variance of latitude, longitude, driving 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] Where: lat k-1 is the latitude of the previous moment; lng k-1 is the longitude of the previous moment; v k-1 is the driving speed at the previous moment; yaw k|k-1 is the heading angle at the last moment; Δt is the time interval;

[0037] The measurement prediction equation used is:

[0038]

[0039] Preferably, in step S4, in the process of fusing the information of 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 respectively:

[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] Where: p x 、p yis the displacement along the X-axis and Y-axis in the world coordinate system; v x and v y is the speed along the X-axis and Y-axis in the world coordinate system; q is the rotation angle in the world coordinate system; a x and a y is the acceleration along the X-axis and Y-axis in the inertial sensor coordinate system; ω is the angular velocity around the Z-axis in the inertial sensor coordinate system; s, θ, and v are the mileage, rotation angle, and driving speed of the pineapple field harvesting robot in the world coordinate system, respectively;

[0044] The observation model equation used is:

[0045]

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

[0047]

[0048] The state update equation used is:

[0049]

[0050] Where: q k-1 is the heading angle at the last moment. k is the heading angle at the current moment, θ k is the rotation angle at the current moment in the world coordinate system; v x 、v y They are the speeds along the X-axis and Y-axis at the current moment in the world coordinate system respectively; They are the displacements along the X-axis and Y-axis at the last moment in the world coordinate system; and are the displacements along the X-axis and Y-axis at the current moment in the world coordinate system; s k is the mileage at the current moment in the world coordinate system; v k is the current driving speed; are the corrected angular velocity and acceleration respectively.

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

[0052]

[0053] Where: v is the driving speed of the pineapple field harvesting robot; is the posture; s is the mileage; v left 、v rightThey are respectively the left wheel speed and the right wheel speed of the mobile chassis in the pineapple field harvesting robot;

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

[0055]

[0056] Where: L is the wheelbase of the mobile chassis; dt is the time step; v k+1 is the driving speed at the next moment; is the position at the next moment; s k+1 Mileage for the next moment.

[0057] Preferably, in step S5, the gray wolf optimization algorithm is used to optimize the Q matrix and the R matrix in the model predictive controller in each time step, and the specific process is:

[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, each position of gray wolves represents a set of parameter values ​​of Q matrix and R matrix;

[0059] Step S502: constructing an objective function based on minimizing error and controlling cost;

[0060] Step S503: For each gray wolf, that is, 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 mark them as α wolf, β wolf and δ wolf, respectively, where α wolf, β wolf and δ wolf represent the optimal solution, the second optimal solution and the third optimal solution, respectively;

[0062] Step S505: Searching for the optimal control input by continuously updating the positions of α wolf, β wolf and δ wolf; stopping the iteration 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 the R matrix corresponding to the position of the α wolf are taken as the optimized results.

[0064] Preferably, 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.

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

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

[0067] 2. The pineapple picking robot automatic navigation method based on multimodal data fusion of the present invention uses the extended Kalman filter (EKF) algorithm for data fusion, which improves the perception accuracy of the system in complex environments. Theoretical analysis shows that the extended Kalman filter (EKF) algorithm can effectively reduce sensor noise and cumulative error, and combined with multi-sensor collaboration, it avoids the failure of a single sensor under conditions such as illumination changes and signal shielding. At the same time, the model predictive control method improved by the gray wolf optimization algorithm overcomes the shortcomings of the traditional control algorithm being easy to oscillate and overshoot on complex paths; wherein, the gray wolf optimization algorithm adopted can dynamically adjust the control parameters, significantly improving the accuracy and robustness of path tracking; and the experimental results in the field operation experiment show that the error range of the heading angle is -2.35 to 4.31°, the maximum deviation is 13.25cm, and the mileage relative error is 13.45%. The system can operate stably under complex field terrain and environment, which fully verifies the applicability and robustness of the pineapple picking robot automatic navigation method based on multimodal data fusion of the present invention in actual agricultural operations.

[0068] 3. The present invention uses the YOLOv10 algorithm to perform real-time detection of pineapple fruits, and generates a navigation path by extracting the center point of the pineapple and combining clustering and fitting techniques. This method overcomes the limitations of traditional navigation technology based on path line extraction in field occlusion scenes. Experimental results show that the navigation path extracted using this method has an average processing time of 0.238 seconds per photo, and the error of the extracted navigation path is about 1°.

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

[0070] Figure 1 The present invention is a structural block diagram of the automatic navigation system of the pineapple field harvesting robot based on multimodal data fusion.

[0071] Figure 2 The present invention is a flow chart of the automatic navigation method of a pineapple field harvesting robot based on multimodal data fusion.

[0072] Figure 3 Schematic diagram of the process of extracting navigation lines.

[0073] Figure 4 The images of pineapple fields collected against a complex background.

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

[0075] Figure 6 This is a diagram showing the detection effect of the pineapple recognition model under different environmental conditions.

[0076] Figure 7 Flowchart (effect diagram) of navigation line extraction.

[0077] Figure 8 This is a flow chart of the extended Kalman filter algorithm.

[0078] Fig. 9 Schematic diagram of the two-wheel differential drive model.

[0079] Fig.10 It is a flow chart of the trajectory tracking algorithm based on GWO-MPC in the present invention. DETAILED DESCRIPTION

[0080] The present invention is further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0081] See also Figure 1-Figure 10 The automatic navigation system of the pineapple field harvesting robot based on multimodal data fusion of the present invention comprises a visual sensor, an inertial sensor (IMU) and a satellite navigation sensor (GNSS), a photoelectric encoder and a control unit arranged on a mobile chassis in the pineapple field harvesting robot, wherein:

[0082] The mobile chassis adopts a crawler chassis. Compared with the traditional wheeled chassis, the crawler chassis has a larger ground contact area, stronger obstacle crossing ability and terrain adaptability, and can maintain stable operation in complex environments such as wet and soft ground, muddy terrain and undulating terrain; wherein the main body of the crawler chassis adopts a gantry structure, so that the pineapple field harvesting robot can adapt to the planting characteristics of the pineapple field and realize cross-row walking, which can not only effectively reduce the pressure loss of crops, but also significantly improve the flexibility and passability of pineapple field operations, and provide reliable guarantee for efficient operations in complex terrain; wherein the parameter settings of the mobile chassis are shown in Table 1;

[0083] Table 1: Specific parameters of mobile chassis

[0084]

[0085] The visual sensor adopts Intel D455 depth camera;

[0086] The inertial sensor uses an MPU6050 sensor, which has a built-in DMP (digital motion processing) firmware. The DMP firmware has an automatic deviation calibration function and uses quaternions to represent the attitude angle, which can effectively avoid the gimbal lock problem that occurs when using Euler angles when rotating at large angles;

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

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

[0089] The above is the basic structure of the pineapple field harvesting robot automatic navigation system based on multimodal data fusion of the present invention. Based on these basic structures, the pineapple field harvesting robot automatic navigation method based on multimodal data fusion of the present invention is introduced below.

[0090] See also Figure 1-Figure 10 The automatic navigation method of a pineapple field harvesting robot based on multimodal data fusion of the present invention comprises the following steps:

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

[0092] In this embodiment, in order to unify the precise navigation and positioning from different sensors, the coordinate system of each sensor must be converted into the world coordinate system; since the data obtained by the satellite navigation sensor is in the world coordinate system, no conversion is required; therefore, only the coordinate system of the visual sensor and the inertial sensor needs to be transformed into the world coordinate system; specifically:

[0093] In the inertial sensor, the geographic coordinate system must be rotated three times in the order of z, x, and y axes to align with the world coordinate system, so the transformation matrix that converts the geographic coordinate system to the world coordinate system is as follows:

[0094]

[0095] Where: is the yaw angle, γ is the roll angle, θ is the pitch angle;

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

[0097]

[0098] Where: (u, v) is the pixel coordinate; Z c is the depth; (X w ,Y w ,Z w ) is the world coordinate; is the camera intrinsic parameter matrix; is the camera extrinsic matrix.

[0099] Step S2: collecting pineapple field images, preprocessing the pineapple field images, and extracting navigation line information from the preprocessed pineapple field images;

[0100] In this embodiment, a pineapple recognition model is used to identify pineapple fruits in a pineapple field image, wherein the steps of constructing the pineapple recognition model are as follows:

[0101] Step S201: collecting pineapple field images, wherein the collected pineapple field images include pineapple field images under different occlusion conditions, pineapple field images under different illumination conditions, and pineapple field images under different background conditions, so as to form an original data set;

[0102] In this embodiment, image acquisition takes place in a pineapple orchard in Shenwan Town, Zhongshan City, Guangdong Province, China; during the image acquisition process, the mobile chassis is manually controlled to move at a constant speed of about 0.2 m / s to simulate normal operation; the industrial camera shoots at a resolution of 1280x720 pixels and a frame rate of 30 frames / s; Figure 4 The paper presents image data collected from complex pineapple fields, including 2,624 pineapple images and 79 field road images, including images without field roads ( Figure 4 (a) in the packaged pineapple ( Figure 4 (b) in), wide road ( Figure 4 (c)) and narrow roads ( Figure 4 The situation in (d)).

[0103] Step S202: performing data enhancement on the constructed original data set, wherein the data enhancement includes random cropping, rotation and color conversion;

[0104] In this embodiment, in order to address the complexity of the wild environment, the collected original data set images include factors such as complex backgrounds, changing lighting conditions and occlusions, so data augmentation techniques (such as random cropping, rotation, and color conversion) are used to further enhance the generalization ability of the model; in addition, since 24 images are overly blurred, they are excluded from the original data set.

[0105] Step S203: Divide the data set into a training set, a test set, and a validation set in a ratio of 7:2:1;

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

[0107] Step S204: marking the pineapple field images in the training set and the validation set to generate marked images;

[0108] In this embodiment, Labelme is used to annotate the images in the training set and the validation set to generate labeled images; Figure 5 The original images and their corresponding labeled datasets under different environmental conditions are shown, including normal images ( Figure 5 (a) in the figure), complex background image ( Figure 5 (b) in the figure) and the highlight image ( Figure 5 (c) in the figure.

[0109] Step S205: setting parameters of the YOLOv10 neural network model, training the YOLOv10 neural network model through the training set, and then verifying the YOLOv10 neural network model through the verification set to obtain a trained pineapple recognition model.

[0110] In this embodiment, 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 (CAA) to accelerate the convergence speed, reduce the training time, and improve the accuracy of the pineapple detection model, which also ensures that the training model meets the speed and accuracy requirements in the complex environment of the pineapple field.

[0111] The formula of the cosine annealing algorithm (CAA) is as follows:

[0112]

[0113] Where: η t is the learning rate of the tth iteration, η min is the minimum learning rate, η max is the maximum learning rate, Tcur is the current iteration number, T max is the total number of iterations.

[0114] The recognition accuracy of the pineapple recognition model in the present invention is described below.

[0115] The following four indicators are used in this embodiment: precision (P), recall (R), mean average precision (mAP) and F1 score. The specific calculation formula is as follows:

[0116]

[0117] Where: TP is a true positive; FP is a false positive; FN is a false negative; N is the total number of target categories; AP i is the average accuracy of the i-th category;

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

[0119]

[0120] In normal training mode (i.e., without using the annealed 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 decline 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, and other parameter settings are automatically completed by the annealing cosine algorithm.

[0123] As shown in Table 2, the YOLOv10-S and YOLOv10-M variant models show excellent performance after integrating CAA, with significant improvements in recall, precision and mAP50; the performance of the YOLOv10-S variant model trained with CAA has the most significant improvement, with an accuracy of 0.868 and an average precision (mAP50-95) of 0.592. In addition, the lower validation loss of the YOLOv10-S variant model shows that it has strong generalization ability and stability, which highlights the effectiveness of CAA in optimizing the training process; after integrating CAA, other YOLOv10 variant models also show varying degrees of performance improvement.

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

[0125] Table 3: Indicator parameters under different conditions

[0126]

[0127] As can be seen from Table 3, the pineapple recognition model of the present invention performs best under normal lighting conditions. As the number of images increases, the recall rate and precision rate 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 is still within an acceptable range and meets the experimental requirements.

[0128] Overall, different lighting and background environments have a significant impact on the performance of the model; 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 position of the pineapple, the image distortion needs to be corrected using internal parameters to ensure that the extracted coordinates of the pineapple center are accurate and reliable;

[0130] When the pineapple recognition model identifies a pineapple, the pixel coordinates of the center point of the identified pineapple fruit are calculated as follows:

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

[0132]

[0133] Where: p 20 and p 10 Respectively represent the horizontal coordinates of the lower right corner and upper left corner of the bounding box; p 21 and p 11 Represents the ordinates of the lower right and upper 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, and the least squares method of Manhattan distance or Euclidean distance 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, specifically;

[0135]

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

[0137] The first category is normal growth of pineapple: Figure 7 (a) in the figure is the original image; Figure 7 (b) is a schematic diagram after the pineapple position is identified; Figure 7 (c) is a schematic diagram after processing using the agglomerative hierarchical clustering algorithm; Figure 7 (d) is a schematic diagram after fitting the straight line and the navigation line;

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

[0139] By adopting the method of the present invention, the average processing time for extracting the navigation line is 0.238 seconds per image, and the adaptability and real-time performance are stronger.

[0140] Step S3: Use the extended Kalman filter algorithm to fuse the information of the satellite navigation sensor and the inertial sensor to obtain the position information of the pineapple field 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 driving speed; yaw is heading angle;

[0144] The covariance matrix used is:

[0145]

[0146] Where: Represent the variance of latitude, longitude, driving 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] Where: lat k-1 is the latitude of the previous moment; lng k-1 is the accuracy of the previous moment; v k-1 is the speed at the previous moment; yaw k|k-1 is the heading angle at the last moment; Δt is the time interval;

[0153] The measurement prediction equation used is:

[0154]

[0155] By iteratively executing 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 referred to Figure 8 ,The fused information is then used as the pose information of the mobile chassis.

[0156] The accuracy and stability of the algorithm are evaluated by comparing the latitude, longitude, driving speed and yaw angle estimated by the fusion algorithm with the actual measured values; the specific evaluation indicators include the errors of latitude, longitude, driving 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: Status error of mobile chassis

[0158]

[0159]

[0160] As time goes by, the predictions of the Extended Kalman Filter (EKF) become more accurate, and although the latitude and longitude errors increase slightly, they remain low overall. The velocity and yaw errors gradually decrease, and after 6 time steps, the yaw angle maintains a high accuracy, which makes the yaw angle suitable for navigation applications.

[0161] Step S4: Use the extended Kalman filter algorithm to fuse the information of the photoelectric encoder and the 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:

[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] Where: p x 、p y is the displacement along the X-axis and Y-axis in the world coordinate system; v x and v y is the speed along the X-axis and Y-axis in the world coordinate system; q is the rotation angle in the world coordinate system; a x and a y is the acceleration along the X-axis and Y-axis in the inertial sensor coordinate system; ω is the angular velocity around the Z-axis in the inertial sensor coordinate system; s, θ and v are the displacement, rotation angle and moving speed of the pineapple field harvesting robot in the world coordinate system respectively;

[0167] In order to ensure the accuracy of the information provided by the inertial sensor, it needs to be calibrated before data fusion to obtain the accelerometer bias and gyro bias

[0168]

[0169] The observation model equation used is:

[0170]

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

[0172]

[0173] The state update equation used is:

[0174]

[0175]

[0176] Where: q k-1 is the heading angle at the last moment. k is the heading angle at the current moment, θk is the rotation angle at the current moment in the world coordinate system; v x 、v y They are the speeds along the X-axis and Y-axis at the current moment in the world coordinate system respectively; They are the displacements along the X-axis and Y-axis at the last moment in the world coordinate system; and are the displacements along the X-axis and Y-axis at the current moment in the world coordinate system; s k is the mileage (or displacement) at the current moment in the world coordinate system; v k is the current driving speed; are the corrected angular velocity and acceleration respectively.

[0177] By predicting the system equation about the state vector x2 k-1 Derived to derive the Jacobian matrix of the system model equation, and similarly for the observation model equation with respect to the state vector x2 k Derivatively obtain its Jacobian matrix;

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

[0179] The performance of the information fusion algorithm under different road conditions was evaluated; the results showed that the fusion algorithm exhibited high accuracy and robustness both on flat and unobstructed roads and on rough and bumpy roads; on flat roads, the experimental results showed that the average error between the mileage calculated by the information fusion algorithm and the actual measurement value was 6.64%, which demonstrated its good tracking capability; although the error increased on bumpy roads, the average error only rose to 11.11%, which demonstrated 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, posture information and mileage information into the model prediction controller optimized by the Grey Wolf Optimization Algorithm, and output the optimal control information to the pineapple field harvesting robot through the optimized model prediction controller, so as to control the pineapple field harvesting robot to autonomously realize navigation path tracking.

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

[0182]

[0183] Where: v is the average speed of the pineapple field harvesting robot, is the position (such as yaw angle / heading angle), s is the mileage; v left 、v right They are respectively the left wheel speed and the right wheel speed of the mobile chassis in the pineapple field harvesting robot;

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

[0185]

[0186] Where: L is the wheelbase of the mobile chassis; dt is the time step; v k+1 is the speed of the next moment; is the posture at the next moment (such as yaw angle / heading angle); s k+1 The odometer reading at the next moment (i.e. mileage);

[0187] The MPC optimization problem is 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] In addition, since the Q matrix measures the error between the system state and the reference trajectory, and the R matrix measures the size of the control input, the gray wolf optimization algorithm is used to optimize the Q matrix and R matrix 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 gray wolves and the dimension D of the search space; randomly initialize the positions of gray wolves, each position of gray wolves represents a set of parameter values ​​of 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: constructing an objective function based on minimizing error and controlling cost;

[0191] Step S503: For each gray wolf, that is, 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 mark them as α wolf, β wolf and δ wolf, respectively, where α wolf, β wolf and δ wolf represent the optimal solution, the second optimal solution and the third optimal solution, respectively;

[0193] Step S505: Searching for the optimal control input by continuously updating the positions of α wolf, β wolf and δ wolf; stopping the iteration 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 the R matrix corresponding to the position of the α wolf are taken 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 according to the navigation line information, posture information and mileage information, so as to control the pineapple field harvesting robot to autonomously realize navigation path tracking.

[0195] The autonomous navigation method of the present invention is adopted, as shown in Table 5:

[0196] Table 5: Experimental data of pineapple field

[0197]

[0198]

[0199] As can be seen from 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, the present invention proposes an automatic navigation system and method for a pineapple field harvesting robot based on multimodal data fusion. The system detects the position of the pineapple using a visual sensor and then extracts a navigation line based on this information. The extended Kalman filter (EKF) is used to fuse sensor data to obtain the position and mileage information of the mobile chassis. The gray wolf optimization algorithm-model predictive control (GWO-MPC) is applied to navigation path tracking to achieve the fusion of the position and mileage data of the mobile chassis. Experiments are carried out in pineapple fields, and the results show that:

[0201] (1) The navigation line extraction method, mobile chassis posture acquisition method and odometer information acquisition method proposed in the present invention meet the navigation requirements of the pineapple field harvesting robot, and the gray wolf optimization algorithm can effectively balance the real-time and accuracy of the 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 pineapple field navigation increased, but no damage was caused to the crops; the pineapple field navigation experiment showed that the error range of the heading angle was -2.35° to 4.31°, the error relative to the odometer was 13.45%, and the maximum lateral offset error was 13.25 cm; the final results showed that the navigation method proposed in the present invention has higher navigation accuracy and stronger robustness.

[0204] The above is a preferred embodiment of the present invention, but the embodiment of the present invention is not limited to the above content. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. An automatic navigation method for a pineapple field harvesting robot based on multimodal data fusion, characterized in that: The following steps are involved: Step S1: transforming the coordinates of the visual sensor, inertial sensor and satellite navigation sensor in the pineapple field harvesting robot into the same coordinate system; Step S2: collecting pineapple field images, preprocessing the pineapple field images, and extracting navigation line information from the preprocessed pineapple field images; Step S3: Use the extended Kalman filter algorithm to fuse the information of the satellite navigation sensor and the inertial sensor to obtain the position information of the pineapple field harvesting robot: Step S4: Use the extended Kalman filter algorithm to fuse the information of the photoelectric encoder and the inertial sensor to obtain the mileage information of the pineapple field harvesting robot: Step S5: input the navigation line information, posture information and mileage information into the model prediction controller optimized by the Grey Wolf Optimization Algorithm, and output the optimal control information to the pineapple field harvesting robot through the optimized model prediction controller, so as to control the pineapple field harvesting robot to autonomously realize navigation path tracking.

2. The pineapple field harvesting robot automatic navigation method based on multimodal data fusion according to claim 1, characterized in that: In step S1, the coordinate systems of the visual 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 pineapple field harvesting robot automatic navigation method based on multimodal 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 center points of the identified pineapples are clustered using the agglomerative hierarchical clustering algorithm, and a straight line is fit to each cluster point using the least squares method of Manhattan distance or Euclidean distance, and the average value of the horizontal coordinate of each straight line is calculated as the horizontal coordinate of the navigation line.

4. The pineapple field harvesting robot automatic navigation method based on multimodal data fusion according to claim 3, characterized in that: In step S2, the steps of constructing the pineapple recognition model are: Step S201: collecting pineapple field images, wherein the collected pineapple field images include pineapple field images under different occlusion conditions, pineapple field images under different illumination conditions, and pineapple field images under different background conditions, so as to form an original data set; Step S202: performing data enhancement on the constructed original data set, wherein the data enhancement includes random cropping, rotation and color conversion; Step S203: Divide the data set into a training set, a test set, and a validation set in a ratio of 7:2:1; Step S204: marking the pineapple field images in the training set and the validation set to generate marked images; Step S205: setting parameters of the YOLOv10 neural network model, training the YOLOv10 neural network model through the training set, and then verifying the YOLOv10 neural network model through the verification set, so as to obtain a trained pineapple recognition model.

5. The pineapple field harvesting robot automatic navigation method based on multimodal 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 using the cosine annealing algorithm.

6. The automatic navigation method of the pineapple field harvesting robot based on multimodal 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 using the extended Kalman filter algorithm, the state vector used is: In the formula: lat is latitude; lng is longitude; v is driving speed; yaw is heading angle; The covariance matrix used is: Where: Represent the variance of latitude, longitude, driving speed and heading angle respectively; The state prediction equation used is: years k|k-1 =years k-1 +Δt×v k-1 ×something(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 ; Where: lat k-1 is the latitude of the previous moment; lng k-1 is the longitude of the previous moment; v k-1 is the driving speed at the previous moment; yaw k|k-1 is the heading angle at the last moment; Δt is the time interval; The measurement prediction equation used is:

7. The automatic navigation method of a pineapple field harvesting robot based on multimodal data fusion according to claim 4, characterized in that: In step S4, in the process of fusing the information of 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: x2=(q,v x ,v y ,p x ,p y ) T ; u2=(a x ,a y ,ω) T ; z=(s,θ,v) T ; Where: p x 、p y is the displacement along the X-axis and Y-axis in the world coordinate system; v x and v y is the speed along the X-axis and Y-axis in the world coordinate system; q is the rotation angle in the world coordinate system; a x and a y is the acceleration along the X-axis and Y-axis in the inertial sensor coordinate system; ω is the angular velocity around the Z-axis in the inertial sensor coordinate system; s, θ, and v are the mileage, rotation angle, and driving 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: Where: q k-1 is the heading angle at the last moment. k is the heading angle at the current moment, θ k is the rotation angle at the current moment in the world coordinate system; v x 、v y They are the speeds along the X-axis and Y-axis at the current moment in the world coordinate system respectively; They are the displacements along the X-axis and Y-axis at the last moment in the world coordinate system; and are the displacements along the X-axis and Y-axis at the current moment in the world coordinate system; s k is the mileage at the current moment in the world coordinate system; v k is the current driving speed; are the corrected angular velocity and acceleration respectively.

8. The automatic navigation method of a pineapple field harvesting robot based on multimodal data fusion according to claim 7, characterized in that: In step S5, the state variable x3 and control variable u3 of the model predictive controller are respectively: Where: v is the driving speed of the pineapple field harvesting robot; is the posture; s is the mileage; v left 、v right They 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 moment; is the position at the next moment; s k+1 Mileage for the next moment.

9. The automatic navigation method of a pineapple field harvesting robot based on multimodal data fusion according to claim 8, characterized in that: In step S5, the gray wolf optimization algorithm is used to optimize the Q matrix and R matrix in the model predictive controller in each time step. The specific process is as follows: 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, each position of gray wolves represents a set of parameter values ​​of Q matrix and R matrix; Step S502: constructing an objective function based on minimizing error and controlling cost; Step S503: For each gray wolf, that is, each set of Q and R parameter values, substitute them into the objective function to calculate the fitness value; Step S504: Find the three gray wolves with the best fitness values ​​in the current population, and mark them as α wolf, β wolf and δ wolf, respectively, where α wolf, β wolf and δ wolf represent the optimal solution, the second optimal solution and the third optimal solution, respectively; Step S505: Searching for the optimal control input by continuously updating the positions of α wolf, β wolf and δ wolf; stopping the iteration when the maximum number of iterations is reached or the convergence condition is met; Step S506: After the iteration is completed, the parameter values ​​of the Q matrix and the R matrix corresponding to the position of the α wolf are taken as the optimized results.

10. The automatic navigation method of a pineapple field harvesting robot based on multimodal 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