Field crop weeding robot autonomous navigation system and control method

Abstract

The invention discloses a field crop weeding robot autonomous navigation system which is composed of a core module and a sensing module, and the core module is responsible for information receiving, data processing, algorithm operation, control instruction issuing and motion control functions of the field crop weeding robot autonomous navigation system; the core module comprises an autonomous navigation system master controller, a chassis master controller and a remote control terminal; the sensing module is responsible for sensing environment information of the autonomous navigation system of the field crop weeding robot, so that the robot can construct a map of a working environment and realize positioning and repositioning functions; the invention further discloses a control method of the autonomous navigation system of the field crop weeding robot, full-autonomous navigation can be achieved in the field weeding operation process, good navigation stability is achieved, the labor cost is reduced, and meanwhile the operation efficiency of the field crop weeding robot is effectively improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of agricultural robot control, and specifically relates to an autonomous navigation system for a field crop weeding robot. The present invention also relates to a control method for the autonomous navigation system of the field crop weeding robot. Background Art

[0002] Weeding operations for field crops have always relied on manual labor. The working environment is harsh, the work intensity is high, and the labor cost is high, which hinders the development of field agriculture. Thanks to the development of robot perception and control technologies, revolutionary changes have taken place in all walks of life. Developing a field crop weeding robot is an inevitable trend to achieve automation and intelligence in field agriculture.

[0003] The autonomous navigation ability of a field crop weeding robot is the key for the robot to perform weeding operations in a farmland environment, mainly including remote monitoring of the robot, perception of the operation environment by the robot and map construction, autonomous navigation decision-making, and path planning, etc., covering multiple control technology fields, with relatively high technical difficulty, which restricts the research and development of field operation weeding robots. Among them, whether the autonomous navigation decision-making and specific path planning are intelligent and reasonable enough greatly affects the working quality and efficiency of the robot. Summary of the Invention

[0004] The purpose of the present invention is to provide an autonomous navigation system for a field crop weeding robot, which can achieve full autonomous navigation during the field weeding operation, and has good navigation stability, effectively reducing labor costs while improving the operation efficiency of the field crop weeding robot.

[0005] Another purpose of the present invention is to provide a control method for the autonomous navigation system of the field crop weeding robot.

[0006] The first technical solution adopted by the present invention is that the autonomous navigation system for a field crop weeding robot consists of a core module and a perception module. The core module is responsible for information reception, data processing, algorithm operation, control instruction issuance, and motion control functions of the autonomous navigation system for the field crop weeding robot;

[0007] The core module includes a main control of the autonomous navigation system, a main control of the chassis, and a remote control terminal;

[0008] The perception module is responsible for perceiving the environmental information of the autonomous navigation system for the field crop weeding robot, and then realizing the map construction of the operation environment by the robot and the realization of the positioning and repositioning functions;

[0009] The perception module includes a wheel odometer, an inertial measurement unit IMU, a lidar, a global navigation satellite system GNSS module, and a vision sensor.

[0010] The characteristics of the first technical solution of the present invention also lie in that

[0011] The autonomous navigation system master controller serves as the upper computer in the entire navigation system. The chassis master controller is the motion control core of the field crop weeding robot, responsible for controlling the robot to perform simple movements and process some data. It serves as the lower computer in the entire navigation system. The remote control terminal is a terminal used by users to remotely monitor the system when there is a network connection. This terminal is wirelessly connected to the robot.

[0012] The wheel odometer is used to obtain the distance the robot chassis has moved, which is used for the basic positioning of the robot. The inertial measurement unit (IMU) is used to obtain the yaw, pitch and roll angle data of the chassis, which is used to obtain the robot's posture data. The lidar uses infrared beams to detect the robot's surrounding environment and generate point cloud information, which is used for the robot to draw maps, locate, relocate and implement obstacle avoidance functions; the visual sensor is used to detect weeds, send the obtained weed information to the remote control terminal and assist the main control to start, stop and change the position of the robot. The global satellite navigation system (GNSS) module is used to obtain the absolute position information of the robot, which is used to implement the robot's repositioning function.

[0013] The second technical solution adopted by the present invention is a control method for the autonomous navigation system of a field crop weeding robot, which simultaneously uses two technical means: multi-sensor fusion positioning and autonomous navigation decision-making. The autonomous navigation decision-making technology specifically includes work area partitioning, navigation sequence establishment and further optimization of the navigation sequence. Work area partitioning means that after the robot constructs a map of the crop weeding area, the confirmed overall work area is divided into several rectangular areas suitable for executing full coverage path planning CCPP. Navigation sequence establishment refers to the process in which the robot autonomously calculates and chooses to go to the next rectangular area based on the current position. The calculation result is only a local optimal solution. Further optimization of the navigation sequence means that by increasing the calculation depth, the local optimal solution obtained is closer to the global optimal solution.

[0014] The second technical solution of the present invention is also characterized in that:

[0015] Please follow the steps below to implement:

[0016] Step 1. Start the robot: The robot power supply starts to supply power to the robot chassis, autonomous navigation system main controller, and chassis main controller. The autonomous navigation system main controller and chassis main controller start and complete initialization. All sensors are powered and initialized.

[0017] Step 2: The main controller of the autonomous navigation system transmits data and interacts with commands with the main chassis controller through the CAN bus. On the premise of ensuring network connection, the remote control terminal remotely connects to the main controller of the autonomous navigation system through the SSH protocol, starts the ROS core. Through this process, the remote control terminal can achieve distributed control of the autonomous navigation system, including multiple functions such as task scheduling, status monitoring, sensor data acquisition and processing, intervention path planning, and motion control;

[0018] Step 3: The remote control terminal controls the autonomous navigation system to start the multi-sensor fusion solution through input commands to achieve precise mapping and positioning of the field weeding robot;

[0019] Step 4: Control the robot to move to the area to be weeded, construct a 3D lidar map based on multi-sensor fusion for the operation area, and save the map. At this stage, the starting point information when constructing the map must be recorded, and this starting point is the origin of the 3D coordinate system in the constructed map;

[0020] Step 5: Perform segmentation processing on the constructed map;

[0021] Step 6: At the beginning of the autonomous navigation stage of the weeding operation, the robot will start to load the previously saved map file, and read the coordinates of the rectangular areas segmented on the map extracted in Step 5. These coordinates include the specific positions of the upper left, upper right, lower left, and lower right corners of each rectangular area. Finally, save these coordinates in an array;

[0022] Step 7: The robot performs repositioning through the multi-sensor fusion solution in Step 3. Specifically, the robot matches the processed sensor data with the read map file by real-time processing of sensor data and cooperating with the submap mechanism and loop closure detection mechanism of the Cartographer algorithm, so as to achieve the positioning and confirmation of the current coordinates. After completing the repositioning, the robot determines its coordinate information on the read map, records the current position information as the starting point of the navigation operation, and uses this as a benchmark for path planning and decision-making in the subsequent navigation process;

[0023] Step 8: Based on the current coordinates, calculate the cost values of all remaining coordinates saved in the array, and finally select the coordinate with the lowest cost value for priority navigation, and temporarily block the remaining corner coordinates in the corresponding rectangular area;

[0024] Step 9: After determining the navigation order of the rectangular area selected in Step 8, go to the rectangular area and perform weeding operations. The navigation in the rectangular area for weeding operations is realized by means of the online CCPP algorithm based on sensor data. When navigating, only need to connect sensor data such as 3D lidar to the algorithm interface;

[0025] Step 10: After completing the weeding operation in each rectangular area, the robot will calculate the cost value again based on the current position to optimize the navigation sequence in real time. That is, the robot will loop through Step 8 and Step 9 until the weeding operation in all areas is completed;

[0026] Step 11: The robot performs a reset operation and returns to the navigation start position, and the autonomous navigation of the weeding operation ends.

[0027] The specific multi-sensor fusion scheme in Step 3 is as follows:

[0028] First, use the Kalman filter and complementary filter to fuse and filter the robot attitude data obtained by the nine-axis IMU, suppress the data disturbance and drift of the IMU, and synchronize the time of the IMU data with the time of ROS to facilitate the next multi-sensor data fusion;

[0029] Then use the extended Kalman filter algorithm to fuse the wheel odometer and the IMU to obtain the fused odometer data;

[0030] Next, fuse the fused odometer data with the lidar through the Cartographer algorithm, and the adjustment of the fusion weight parameters can be achieved by modifying the corresponding covariance matrix;

[0031] Finally, align the local geographic coordinate system obtained by the GNSS module with the map coordinate system of the established map. When the navigation system runs for too long and the errors of sensors such as the wheel odometer, IMU, and lidar accumulate to the threshold and cannot be accurately repositioned through the loop closure detection link of the Cartographer algorithm alone, the positioning information of the weeding robot is calibrated with the positioning information obtained by the GNSS module.

[0032] In Step 3, the extended Kalman filter algorithm is used to fuse the wheel odometer and the IMU to obtain the fused odometer data, which is specifically implemented according to the following steps:

[0033] Let the state vector be:

[0034]

[0035] where: x, y represent the positions in the global coordinate system; θ represents the heading angle, v represents the linear velocity, ω bias , a bias represent the zero biases of the angular velocity and acceleration of the IMU, and use the angular velocity ω m and acceleration a m of the IMU for prediction:

[0036] v k|k-1 = v k-1 +(a m -αbias，k-1 )·Δt

[0037] θ k|k-1 = θ k-1 +(ω m - ω bias，k-1 )·Δt

[0038]

[0039] ω bias，k|k-1 = ω bias，k-1

[0040] a bias，k|k-1 = a bias，k-1

[0041] The Jacobian matrix F, i.e., the state transition matrix, is expressed as follows:

[0042]

[0043] where a eff = a m - a bias，k-1

[0044] The process noise covariance Q is expressed as follows:

[0045]

[0046] Update step;

[0047] Assume that the wheel odometer provides the linear velocity v odom and the heading angle θ odom , then the observation vector and the observation equation are:

[0048]

[0049] The observation matrix H is:

[0050]

[0051] The observation noise covariance R is:

[0052]

[0053] The Kalman gain K is:

[0054] K = P k|k-1 H T (HP k|k-1 H T + R) -1

[0055] State update:

[0056] x k = xk|k-1 +K(z - h(x k|k-1 ))

[0057] Covariance update:

[0058] P k =(I - KH)P k|k-1

[0059] The propagation formula of covariance is:

[0060] P k|k-1 = FP k-1 F T + Q

[0061] Assume the robot moves in a straight line, but there is an angular velocity bias error in the IMU. The fusion process is divided into prediction and update. The prediction step uses F to predict the state: position, velocity, and heading angle, and the covariance matrix P k|k-1 reflects the uncertainty of the heading angle;

[0062] The update step includes using the wheel odometer to provide the heading angle observation value θ odom and the linear velocity observation value v odom , calculating the Kalman gain K, which will determine how to fuse the predicted value and the observed value according to P k|k-1 , and finally the fused x k will be closer to the true value; at each moment, the prediction and update steps are executed sequentially to output the fused state x k .

[0063] In step 3, the fused odometer data is fused with the lidar through the Cartographer algorithm. The adjustment of the fusion weight parameter can be achieved by modifying the corresponding covariance matrix, and the specific implementation is as follows:

[0064] Define the optimization variable as the set of robot poses:

[0065]

[0066] where, x i represents the pose of the robot in the global coordinate system at the i-th moment, including the position x i , y i and the heading angle θ i ;

[0067] The optimization objective function is:

[0068]

[0069] where, is the set of all constraints, and e ij (x) is the pose x iThe residual between and x j is Ω ij is the information matrix,

[0070] Assume that from time i to j, the relative pose measured by the odometer is Then the residual is

[0071]

[0072] The corresponding information matrix Ω odom is calculated from the covariance matrix ∑ of the odometer odom as follows:

[0073]

[0074] The lidar constraint is expressed as:

[0075] The pose x is obtained through scan matching i with respect to x j the estimate of The residual is:

[0076]

[0077] The lidar information matrix Ω lidar is dynamically adjusted according to the scan matching score:

[0078]

[0079] where s ij ∈ [0, 1] is the scan matching score, reflecting the confidence of the match, and σ x , σ y are the basic noise parameters of the lidar in the translation direction, and σ θ is the basic noise parameter in the rotation direction;

[0080] The information matrix is the inverse matrix of the covariance matrix: Ω = ∑ -1

[0081] The smaller the covariance matrix Σ, the larger the value of the information matrix Ω, and the higher the weight of this constraint in the optimization; in Cartographer, the covariance parameters in the configuration file are directly modified to control the weights of different sensors in the optimization. The weight values correspond to the diagonal elements of the information matrix. The larger the value, the smaller the covariance. Finally, the optimization objective function is solved to build the map.

[0082] Step 5 is specifically implemented according to the following steps:

[0083] First, the saved map file needs to be exported and converted into a picture format. Then, the range of the operation area is manually marked. Next, image processing is performed using the OpenCV library in Python to further segment the marked operation area into several rectangular areas. It is stipulated that the minimum width of the rectangular area shall not be less than the width of the robot used. If the width of some segmented areas is less than the width of the robot, the area will be merged with the adjacent rectangular area.

[0084] Combined with the origin information recorded in step 4, during the segmentation process, with the help of image processing technology, the coordinates of the four corners of each rectangular area are extracted as the basis for subsequent calculations.

[0085] The calculation formula for the cost value in step 8 is expressed as follows:

[0086]

[0087] where n is the number of sampling points of non-set rectangular areas, α is the weight corresponding to the sampling point when calculating the cost value, x1 is the abscissa value of the current coordinate, x2 is the abscissa value of the considered coordinate, y1 is the ordinate value of the current coordinate, y2 is the ordinate value of the considered coordinate, and the sampling points are several points evenly distributed on the line segment between the current coordinate and the considered coordinate.

[0088] The calculation depth is introduced. The larger the set calculation depth, the more iterative calculations in the navigation decision-making process, and the closer the obtained local optimal solution is to the global optimal solution.

[0089] Suppose the calculation depth is set to 2. The global cost value will be calculated continuously twice. After the first global cost value calculation is completed, the optimal point is selected, the four coordinates of the rectangular area to which the point belongs are removed from the array, and the departure point of the rectangular area is calculated. Finally, based on this departure point, the next global cost value calculation is performed, and the optimal navigation sequence is selected by integrating the sum of the two global cost values. The calculation of the departure point is based on the selected optimal point as the starting point, calculated using the weeding width of the robot and the width of the rectangular area, to obtain how many times the robot needs to reciprocate to complete the weeding operation in the rectangular area and obtain the coordinates of the point where the robot leaves after completing the weeding operation in the rectangular area. The calculation formula for the global cost value after introducing the calculation depth can be expressed as follows:

[0090] L G =L L1 +L L2 +L L3 +…+L Lm

[0091] where m is the set calculation depth, L LmRepresents one of the local cost values in the depth calculation of the m-th layer. At the same time, the coordinates involved in the calculation are copied to a two-dimensional array, and the coordinates corresponding to each cost value are stored in the cells of this two-dimensional array according to a certain calculation method, which is convenient for retrieving the coordinate navigation order corresponding to the optimal global cost value in reverse after finally obtaining it;

[0092] The calculation method of the specific cell where the coordinates corresponding to the cost value are stored is as follows: for the a-th global cost value L obtained after introducing the calculation depth Ga in, the horizontal and vertical coordinate values corresponding to the b-th local cost value L Lb are respectively stored in Array[a - 1][2b - 2] and Array[a - 1][2b - 1] of the two-dimensional array. After finally comparing to obtain the optimal global cost value, then the row starting point of the two-dimensional array corresponding to this global cost value is located by the array pointer, and the coordinates are read and navigated in sequence.

[0093] The beneficial effect of the present invention is that the autonomous navigation system mainly consists of two modules, including a core module and a sensing module, and at the same time uses two main technical means, including multi-sensor fusion positioning and autonomous navigation decision-making. The core module is mainly responsible for functions such as information reception, data processing, algorithm operation, control instruction issuance, and motion control of the autonomous navigation system of the field crop weeding robot. The sensing module is mainly responsible for the environmental information perception of the autonomous navigation system of the field crop weeding robot, and can realize functions such as high-precision mapping, positioning, and repositioning in cooperation with multi-sensor fusion positioning. Finally, the constructed map is segmented through autonomous navigation decision-making to obtain the required coordinate information, and the optimal navigation order is determined based on the coordinates of the robot itself, greatly improving the navigation efficiency, and finally completing the navigation of the field crop weeding operation, which can be widely applied to the weeding navigation operation of field crops. BRIEF DESCRIPTION OF THE DRAWINGS

[0094] Figure 1 is the overall flow schematic diagram of the present invention;

[0095] Figure 2 is the schematic diagram of the regional segmentation of the constructed map provided by the embodiment of the present invention;

[0096] Figure 3 is the autonomous navigation flow schematic diagram of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0097] The present invention will be described in detail below in conjunction with the drawings and specific embodiments.

[0098] The autonomous navigation system of the field crop weeding robot of the present invention consists of a core module and a sensing module. The core module is responsible for functions such as information reception, data processing, algorithm operation, control instruction issuance, and motion control of the autonomous navigation system of the field crop weeding robot;

[0099] The core module includes the main control of the autonomous navigation system, the main control of the chassis, and the remote control terminal;

[0100] The perception module is responsible for perceiving the environmental information of the autonomous navigation system of the field crop weeding robot, and then realizes the map construction of the robot for the operation environment and the realization of the positioning and repositioning functions. It is an indispensable part of autonomous navigation.

[0101] The perception module includes a wheel odometer, an inertial measurement unit IMU (Inertial Measurement Unit), a lidar, and a global navigation satellite system GNSS (Global Navigation Satellite System) module and a vision sensor.

[0102] The main control of the autonomous navigation system is the core main control of the field crop weeding robot. All core control algorithms and most data processing run in it. It serves as the upper computer in the entire navigation system. The main control of the chassis is the motion control core of the field crop weeding robot, responsible for controlling the robot to move simply and processing some data. It serves as the lower computer in the entire navigation system. The remote control terminal is the terminal used by the user to remotely monitor the system when there is a network connection. This terminal is wirelessly connected to the robot.

[0103] The wheel odometer is used to obtain the moved distance of the robot chassis for the realization of the basic positioning of the robot. The inertial measurement unit IMU is used to obtain the yaw angle, pitch angle, and roll angle data of the chassis for the acquisition of the pose data of the robot. The lidar detects the surrounding environment of the robot through infrared beams and generates point cloud information for the realization of functions such as map drawing, positioning, repositioning, and obstacle avoidance of the robot; the vision sensor is used to detect weeds, send the obtained weed information to the remote control terminal, and assist the main control in starting and stopping the robot and changing positions. The global navigation satellite system GNSS module is used to obtain the absolute position information of the robot for the realization of the repositioning function of the robot.

[0104] The control method of the autonomous navigation system of the field crop weeding robot of the present invention combines Figure 3, and simultaneously uses two technical means: multi-sensor fusion positioning and autonomous navigation decision-making. The autonomous navigation decision-making technology specifically includes work area partitioning, navigation sequence establishment and further optimization of navigation sequence. Work area partitioning means that after the robot constructs a map of the crop weeding area, the confirmed overall work area is divided into several rectangular areas suitable for executing complete coverage path planning CCPP (Complete Coverage Path Planning). Navigation sequence establishment refers to the process of the robot autonomously calculating and choosing to go to the next rectangular area based on the current position. The calculation result is only a local optimal solution. Further optimization of the navigation sequence means making the local optimal solution closer to the global optimal solution by increasing the calculation depth.

[0105] Multi-sensor fusion positioning is fundamental to autonomous robot navigation. The significance of this technology lies in the fact that autonomous navigation in farmland environments requires relatively accurate environmental perception. However, each sensor output contains noise and measurement errors. Using a single sensor alone can lead to the accumulation of errors, ultimately reducing the robot's positioning accuracy. Therefore, during the autonomous navigation of a field crop weeding robot, at least two sensors must be involved in the calculations during any movement.

[0106] The multi-sensor fusion solution specifically integrates the data from sensors such as wheel odometers, IMUs, lidars, and GNSS modules through multiple fusion algorithms such as Kalman filters, complementary filters, extended Kalman filters, and cartographer algorithms, calibrates them against each other, and ultimately outputs relatively accurate robot positioning information.

[0107] Autonomous navigation decision-making technology is key to enabling autonomous robot navigation. This technology is crucial for autonomous robot navigation in farmland environments. It requires not only appropriate path planning algorithms to plan navigation paths while enabling obstacle avoidance, but also intelligent autonomous navigation decision-making to improve navigation efficiency and free up manual labor.

[0108] The main control of the autonomous navigation system uses the IBOX-105-6L2C industrial control computer.

[0109] The chassis main control uses STM32F407ZGT6 single-chip microcomputer, and uses CAN (Controller Area Network) bus to connect with the autonomous navigation system main control and realize data transmission.

[0110] The remote control terminal is a personal computer installed with Ubuntu system and ROS (Robot Operating System) basic environment, and is remotely connected to the autonomous navigation system main control using SSH (Secure Shell Security Shell Protocol).

[0111] The raw data of the wheel odometer is calculated by the chassis main control according to the movement of the robot, and is finally transmitted to the autonomous navigation system main control for processing and release.

[0112] The IMU uses the WHEELTEC N100 nine-axis IMU module, which is connected to the autonomous navigation system main control via USB (Universal Serial Bus).

[0113] The laser radar used is the Sagitar RS-LiDAR-16 laser radar, which is connected to the autonomous navigation system main control through the Ethernet interface.

[0114] The GNSS module uses a GPS Beidou dual-mode positioning module and is connected to the autonomous navigation system main control through a USB serial port.

[0115] The robot chassis uses a crawler chassis with its own power supply.

[0116] The implementation flow diagram of the autonomous navigation system of the field crop weeding robot is as follows: Figure 1 As shown. Figure 1 ,

[0117] Please follow the steps below to implement:

[0118] Step 1. Start the robot: The robot power supply starts to supply power to the robot chassis, autonomous navigation system main controller, and chassis main controller. The autonomous navigation system main controller and chassis main controller start and complete initialization. All sensors are powered and initialized.

[0119] Step 2: The autonomous navigation system master controller exchanges data and commands with the chassis master controller via the CAN bus. Under the premise of ensuring network connectivity, the remote control terminal remotely connects to the autonomous navigation system master controller via the SSH protocol and starts the ROS core. Through this process, the remote control terminal can achieve distributed control of the autonomous navigation system, including task scheduling, status monitoring, sensor data acquisition and processing, intervention path planning, and motion control.

[0120] Step 3: The remote control terminal inputs commands to control the autonomous navigation system to start the multi-sensor fusion solution to achieve accurate mapping and positioning of the field weeding robot;

[0121] The details of the multi-sensor fusion solution in step 3 are as follows:

[0122] First, use the Kalman filter and complementary filter to fuse and filter the robot attitude data obtained by the nine-axis IMU, suppress the data disturbance and drift of the IMU, and synchronize the time of the IMU data with the time of ROS, which is convenient for the next multi-sensor data fusion;

[0123] Then, use the extended Kalman filter algorithm to fuse the wheel odometer and the IMU to obtain the fused odometer data;

[0124] Next, fuse the fused odometer data with the lidar through the Cartographer algorithm, and the adjustment of the fusion weight parameters can be achieved by modifying the corresponding covariance matrix;

[0125] Finally, align the local geographical coordinate system obtained by the GNSS module with the map coordinate system of the established map. When the navigation system runs for too long and the errors of sensors such as the wheel odometer, IMU, and lidar accumulate to a threshold value (the threshold value is an internal variable of the main controller and needs to be set by oneself), and accurate relocalization cannot be achieved solely through the loop closure detection link of the Cartographer algorithm, the positioning information of the weeding robot is calibrated by means of the positioning information obtained by the GNSS module.

[0126] In step 3, the extended Kalman filter algorithm is used to fuse the wheel odometer and the IMU to obtain the fused odometer data, which is specifically implemented according to the following steps:

[0127] Let the state vector be:

[0128] where: x and y represent the positions in the global coordinate system; θ represents the heading angle (yaw angle), v represents the linear velocity (along the x-axis of the body coordinate system), ω bias , a bias represent the zero biases of the angular velocity and acceleration of the IMU, and use the angular velocity ω m and acceleration a m for prediction:

[0129] v k|k-1 = v k-1 + (a m - α bias，k-1 ) · Δt

[0130] θ k|k-1 = θ k-1 + (ω m - ω bias，k-1 ) · Δt

[0131]

[0132] ω bias，k|k-1 = ωbias，k-1

[0133] a bias,k|k-1 = a bias,k-1

[0134] The Jacobian matrix F, i.e., the state transition matrix, is expressed as follows:

[0135]

[0136] where a eff = a m -a bias，k-1

[0137] The process noise covariance Q is expressed as follows:

[0138]

[0139] Update step (based on wheel odometer data);

[0140] Assume that the wheel odometer provides the linear velocity v odom and the heading angle θ odom , then the observation vector and the observation equation are:

[0141]

[0142] The observation matrix H is:

[0143]

[0144] The observation noise covariance R is:

[0145]

[0146] The Kalman gain K is:

[0147] K = P k|k-1 H T (HP k|k-1 H T + R) -1

[0148] State update:

[0149] x k = x k|k-1 + K(z - h(x k|k-1 ))

[0150] Covariance update:

[0151] P k = (I - KH)P k|k-1

[0152] The propagation formula for the covariance is:

[0153] P k|k-1 = FP k-1 F T + Q

[0154] Assume the robot moves in a straight line, but there is an angular velocity bias error in the IMU. The fusion process is divided into prediction and update. In the prediction step, F is used to predict the state: position, velocity, and heading angle. Due to the uncalibrated bias, the predicted value of the heading angle θ k|k-1 will deviate from the true value, and the in Q will quantify the bias noise, and the covariance matrix P k|k-1 reflects the uncertainty of the heading angle;

[0155] In the update step, it includes using the wheel odometer to provide the observed value of the heading angle θ odom and the observed value of the linear velocity v odom , calculate the Kalman gain K, and K will determine how to fuse the predicted value and the observed value according to P k|k-1 (including the influence of Q), and finally the fused x k will be closer to the true value; at each moment, the prediction and update steps are executed sequentially to output the fused state x k .

[0156] In step 3, the fused odometer data is fused with the lidar through the Cartographer algorithm. The adjustment of the fusion weight parameter can be achieved by modifying the corresponding covariance matrix, and the specific implementation is as follows:

[0157] The core of Cartographer is SLAM based on graph optimization, and its goal is to minimize the weighted residuals of all sensor constraints.

[0158] Define the optimization variables as the set of robot poses:

[0159]

[0160] where, x i represents the pose of the robot in the global coordinate system at the i-th moment, including the position x i , y i and the heading angle θ i ;

[0161] The optimization objective function is:

[0162]

[0163] where, is the set of all constraints, and e ij (x) is the residual between the pose x i and x j , and Ω ijis the information matrix,

[0164] Assume that from time i to j, the relative pose measured by the odometer is Then the residual is

[0165]

[0166] The corresponding information matrix Ω odom is calculated from the covariance matrix ∑ of the odometer odom as follows:

[0167]

[0168] The lidar constraint is expressed as:

[0169] The pose x is obtained through scan matching i relative to x j is estimated The residual is:

[0170]

[0171] The lidar information matrix Ω lidar is dynamically adjusted according to the scan matching score:

[0172]

[0173] where s ij ∈ [0, 1] is the scan matching score, reflecting the confidence of the match (s ij = 1 means completely credible), σ x , σ y is the base noise parameter in the translation direction of the lidar (unit: m), and σ θ is the base noise parameter in the rotation direction (unit: rad);

[0174] The information matrix is the inverse matrix of the covariance matrix: Ω = ∑ -1

[0175] The smaller the covariance matrix ∑ (higher sensor accuracy), the larger the value of the information matrix Ω, and the higher the weight of this constraint in the optimization; in Cartographer, the covariance parameters in the configuration file are directly modified to control the weights of different sensors in the optimization. The weight values correspond to the diagonal elements of the information matrix, and the larger the value, the smaller the covariance. Finally, the optimization objective function is solved to build the map.

[0176] Step 4: Control the robot to move to the area to be weeded, construct a 3D lidar map based on multi-sensor fusion for the operation area, and save the map. At this stage, the starting point information when constructing the map must be recorded, and this starting point is the origin of the 3D coordinate system in the constructed map;

[0177] Step 5, as Figure 2 shown, perform segmentation processing on the constructed map;

[0178] Step 5 is specifically implemented according to the following steps:

[0179] First, the saved map file needs to be exported and converted to an image format (such as PNG, JPEG, etc.) for subsequent image analysis processing. Then, manually mark the scope of the operation area to ensure the accuracy and integrity of the marking. Next, use the OpenCV library in Python for image processing, and further segment the marked operation area into several rectangular areas; it should be noted that certain constraint conditions must be met during the area segmentation. Specifically, it is stipulated that the minimum width of the rectangular area shall not be less than the width of the robot used, which is to ensure that each segmented area has enough space for the robot to pass through smoothly during operation, avoiding the robot being unable to pass through or the work efficiency being reduced due to a too narrow area. In actual operation, if the width of some segmented areas is less than the width of the robot, then this area will be merged with the adjacent rectangular area to ensure the actual usability of all segmented areas.

[0180] Combined with the origin information (usually the starting point of the map) recorded in Step 4, during the segmentation process, use image processing technology to extract the coordinates of the four corners of each rectangular area as the basis for subsequent calculations.

[0181] Step 6, at the beginning of the autonomous navigation stage of the weeding operation, the robot will start to load the previously saved map file and read the coordinates of the rectangular areas segmented on the map extracted in Step 5. These coordinates include the specific positions of the upper left corner, upper right corner, lower left corner, and lower right corner of each rectangular area, and finally save these coordinates in an array;

[0182] Step 7, the robot performs repositioning through the multi-sensor fusion scheme in Step 3. Specifically, the robot matches the processed sensor data with the read map file by performing real-time processing of the sensor data and cooperating with the submap mechanism and loop closure detection mechanism of the Cartographer algorithm, so as to achieve the positioning and confirmation of the current coordinates. After completing the repositioning, the robot determines its coordinate information on the read map, records the current position information as the starting point of the navigation operation, and uses this as a benchmark for path planning and decision-making during the subsequent navigation process; this marked information not only provides an initial reference point for the execution of the navigation task, but also provides basic data for subsequent autonomous navigation decision-making.

[0183] Step 8: Based on the current coordinates, calculate the cost values for all remaining coordinates stored in the array. Finally, select the coordinate with the lowest cost value for priority navigation and temporarily block the remaining corner coordinates in the corresponding rectangular area;

[0184] The calculation formula for the cost value in Step 8 is expressed as follows:

[0185]

[0186] Where n is the number of sampling points in the non-set rectangular area, α is the weight corresponding to the sampling point during cost value calculation, x1 is the abscissa value of the current coordinate, x2 is the abscissa value of the coordinate under consideration, y1 is the ordinate value of the current coordinate, y2 is the ordinate value of the coordinate under consideration, and the sampling points are several points evenly distributed on the line segment between the current coordinate and the coordinate under consideration;

[0187] To further optimize the navigation logic, the calculation depth is introduced. The larger the set calculation depth, the more iterative calculations are performed during the navigation decision-making process, and the closer the obtained local optimal solution is to the global optimal solution;

[0188] Assume that when the calculation depth is set to 2, the global cost value will be calculated twice continuously. After the first global cost value calculation is completed, select the optimal point, remove the four coordinates of the rectangular area to which this point belongs from the array, and calculate the departure point for this rectangular area. Finally, based on this departure point, perform the next global cost value calculation, and select the optimal navigation order by synthesizing the sum of the two global cost values; the calculation of the departure point is based on the selected optimal point as the starting point, and is calculated using the weeding width of the robot and the width of the rectangular area to obtain how many times the robot needs to reciprocate in this rectangular area to complete the weeding operation, and obtain the coordinates of the point where the robot leaves when completing the weeding operation in this rectangular area;

[0189] The calculation formula for the global cost value after introducing the calculation depth can be expressed as follows:

[0190] L G =L L1 +L L2 +L L3 +…+L Lm

[0191] Where m is the set calculation depth, and L Lm represents one of the local cost values in the m-th layer depth calculation, and at the same time, the coordinates involved in the calculation are copied to a two-dimensional array. The coordinates corresponding to each cost value are stored in the cells of this two-dimensional array according to a certain calculation method, which is convenient for finally retrieving the coordinate navigation order corresponding to the optimal global cost value in reverse;

[0192] The calculation method for the specific unit storing the coordinate corresponding to the cost value is as follows: the a-th global cost value L obtained after introducing the calculation depth Ga In, the b-th local cost value L Lb The corresponding horizontal and vertical coordinate values are respectively stored in the two-dimensional arrays Array[a - 1][2b - 2] and Array[a - 1][2b - 1]. After finally comparing to obtain the optimal global cost value, then position the array pointer to the starting row of the two-dimensional array corresponding to this global cost value, and read the coordinates in sequence for navigation; for example, the second local cost value L G3 In L2 The corresponding horizontal and vertical coordinate values are respectively stored in the two-dimensional arrays Array[2][2] and Array[2][3]. When reading several coordinates corresponding to this global cost value, the array pointer is positioned to Array[2][0], and the coordinate information is read in sequence.

[0193] Regarding the specific setting of the calculation depth, if the depth is set too small, it may lead to an unreasonable navigation sequence and ultimately affect the navigation efficiency; if the depth is set too large, it will result in a relatively large calculation amount, and at the same time, the amount of data to be saved is relatively large, with high performance requirements for the main control of the autonomous navigation system.

[0194] Step 9: After determining the navigation sequence of the selected rectangular area in Step 8, go to the rectangular area and perform weeding operations. The navigation in the rectangular area for weeding operations is realized by means of the online CCPP algorithm based on sensor data. CCPP is a conventional technology in this industry. When navigating, only need to connect the sensor data such as 3D lidar to the algorithm interface, which will not be elaborated here;

[0195] Step 10: After completing the weeding operation in each rectangular area, the robot will calculate the cost value again based on the current position to optimize the navigation sequence in real time, that is, the robot will loop through Step 8 and Step 9 until the weeding operations in all areas are completed;

[0196] Step 11: The robot performs a reset operation and returns to the navigation start position, and the autonomous navigation for weeding operations ends.

[0197] Embodiment 1

[0198] The autonomous navigation system of the field crop weeding robot of the present invention is composed of a core module and a sensing module. The core module is responsible for information reception, data processing, algorithm operation, control instruction issuance, and motion control functions of the autonomous navigation system of the field crop weeding robot;

[0199] The core module includes the main control of the autonomous navigation system, the main control of the chassis, and a remote control terminal;

[0200] The perception module is responsible for the environmental information perception of the autonomous navigation system of the field crop weeding robot, and then realizes the map construction of the robot for the operation environment and the realization of the positioning and repositioning functions, which is an indispensable part of autonomous navigation.

[0201] The perception module includes a wheel odometer, an inertial measurement unit IMU (Inertial Measurement Unit), a lidar, a global navigation satellite system GNSS (Global Navigation Satellite System) module, and a vision sensor.

[0202] Embodiment 2

[0203] The autonomous navigation system of the field crop weeding robot of the present invention is composed of a core module and a perception module. The core module is responsible for information reception, data processing, algorithm operation, control instruction issuance, and motion control functions of the autonomous navigation system of the field crop weeding robot.

[0204] The core module includes a main controller of the autonomous navigation system, a main controller of the chassis, and a remote control terminal.

[0205] The perception module is responsible for the environmental information perception of the autonomous navigation system of the field crop weeding robot, and then realizes the map construction of the robot for the operation environment and the realization of the positioning and repositioning functions, which is an indispensable part of autonomous navigation.

[0206] The perception module includes a wheel odometer, an inertial measurement unit IMU (Inertial Measurement Unit), a lidar, a global navigation satellite system GNSS (Global Navigation Satellite System) module, and a vision sensor.

[0207] The main controller of the autonomous navigation system is the core main controller of the field crop weeding robot. All core control algorithms and most data processing run therein. It serves as the upper computer in the entire navigation system. The main controller of the chassis is the motion control core of the field crop weeding robot, responsible for controlling the robot to perform simple movements and processing some data. It serves as the lower computer in the entire navigation system. The remote control terminal is a terminal used by users to remotely monitor the system when there is a network connection. This terminal is wirelessly connected to the robot.

[0208] Embodiment 3

[0209] The field crop weeding robot autonomous navigation system of the present invention is composed of a core module and a perception module. The core module is responsible for information reception, data processing, algorithm operation, control instruction issuance and motion control functions of the field crop weeding robot autonomous navigation system;

[0210] The core modules include the autonomous navigation system master control, chassis master control and remote control terminal;

[0211] The perception module is responsible for the environmental information perception of the field crop weeding robot's autonomous navigation system, thereby enabling the robot to build a map of the working environment and realize positioning and repositioning functions. It is an indispensable part of autonomous navigation.

[0212] The wheel odometer is used to obtain the distance the robot chassis has moved, which is used for the basic positioning of the robot. The inertial measurement unit (IMU) is used to obtain the yaw, pitch and roll angle data of the chassis, which is used to obtain the robot's posture data. The lidar uses infrared beams to detect the robot's surrounding environment and generate point cloud information, which is used for the robot to draw maps, locate, relocate and implement obstacle avoidance functions; the visual sensor is used to detect weeds, send the obtained weed information to the remote control terminal and assist the main control to start, stop and change the position of the robot. The global satellite navigation system (GNSS) module is used to obtain the absolute position information of the robot, which is used to implement the robot's repositioning function.

[0213] Example 4

[0214] The present invention provides a control method for an autonomous navigation system of a field crop weeding robot, which simultaneously utilizes two technical means: multi-sensor fusion positioning and autonomous navigation decision-making. The autonomous navigation decision-making technology specifically includes work area partitioning, navigation sequence establishment, and further optimization of the navigation sequence. Work area partitioning refers to the division of the overall work area confirmed after the robot constructs a map of the crop weeding area into several rectangular areas suitable for executing complete coverage path planning CCPP (Complete Coverage Path Planning). Navigation sequence establishment refers to the process in which the robot autonomously calculates and chooses to go to the next rectangular area based on the current position. The calculation result is only a local optimal solution. Further optimization of the navigation sequence refers to a method of increasing the calculation depth so that the obtained local optimal solution is closer to the global optimal solution.

[0215] Example 5

[0216] The present invention is specifically implemented according to the following steps:

[0217] Step 1. Start the robot: The robot power supply starts to supply power to the robot chassis, autonomous navigation system main controller, and chassis main controller. The autonomous navigation system main controller and chassis main controller start and complete initialization. All sensors are powered and initialized.

[0218] Step 2: The main controller of the autonomous navigation system transmits data and interacts with instructions with the main chassis controller through the CAN bus. On the premise of ensuring network connection, the remote control terminal remotely connects to the main controller of the autonomous navigation system through the SSH protocol, starts the ROS core. Through this process, the remote control terminal can achieve distributed control of the autonomous navigation system, including multiple functions such as task scheduling, status monitoring, sensor data acquisition and processing, intervention path planning, and motion control;

[0219] Step 3: The remote control terminal controls the autonomous navigation system to start the multi-sensor fusion scheme through input commands to achieve precise mapping and positioning of the field weeding robot;

[0220] Step 4: Control the robot to move to the area to be weeded, construct a 3D lidar map based on multi-sensor fusion for the operation area, and save the map. At this stage, the starting point information when constructing the map must be recorded, and this starting point is the origin of the 3D coordinate system in the constructed map;

[0221] Step 5: As Figure 2 shown, perform segmentation processing on the constructed map;

[0222] Step 6: At the beginning of the autonomous navigation stage of the weeding operation, the robot will start to load the previously saved map file, and read the coordinates of the rectangular areas segmented on the map extracted in Step 5. These coordinates include the specific positions of the upper left corner, upper right corner, lower left corner, and lower right corner of each rectangular area. Finally, save these coordinates in an array;

[0223] Step 7: The robot performs repositioning through the multi-sensor fusion scheme in Step 3. Specifically, the robot matches the processed sensor data with the read map file by real-time processing of sensor data and cooperating with the submap mechanism and loop detection mechanism of the Cartographer algorithm, so as to achieve the positioning and confirmation of the current coordinates. After completing the repositioning, the robot determines its coordinate information on the read map, records the current position information as the starting point of the navigation operation, and uses this as a benchmark for path planning and decision-making in the subsequent navigation process; this annotation information not only provides an initial reference point for the execution of the navigation task, but also provides basic data for subsequent autonomous navigation decisions.

[0224] Step 8: Based on the current coordinates, calculate the cost values of all the remaining coordinates saved in the array, and finally select the coordinate with the lowest cost value for priority navigation, and temporarily mask the remaining corner coordinates in the corresponding rectangular area;

[0225] Step 9: After determining the navigation sequence for the selected rectangular area in Step 8, move to the rectangular area and perform weeding operations. The navigation for weeding operations in the rectangular area is achieved by means of an online CCPP algorithm based on sensor data. CCPP is a conventional technology in this industry. When navigating, only need to connect sensor data such as 3D lidar to the algorithm interface, which will not be elaborated here;

[0226] Step 10: After completing the weeding operation for each rectangular area, the robot will calculate the cost value again based on the current position to optimize the navigation sequence in real time. That is, the robot will loop through Step 8 and Step 9 until the weeding operations for all areas are completed;

[0227] Step 11: The robot performs a reset operation and returns to the navigation start position, and the autonomous navigation for weeding operations ends.

[0228] Embodiment 6

[0229] The control method of the autonomous navigation system of the field crop weeding robot of the present invention is specifically implemented according to the following steps:

[0230] Step 1: Start the robot: The robot power supply starts to supply power to the robot chassis, the main control of the autonomous navigation system, and the main control of the chassis. The main control of the autonomous navigation system and the main control of the chassis start and complete initialization, and each sensor gets powered and completes initialization;

[0231] Step 2: The main control of the autonomous navigation system conducts data transmission and command interaction with the main control of the chassis through the CAN bus. On the premise of ensuring network connection, the remote control terminal remotely connects to the main control of the autonomous navigation system through the SSH protocol and starts the ROS core. Through this process, the remote control terminal can achieve distributed control of the autonomous navigation system, including multiple functions such as task scheduling, status monitoring, sensor data acquisition and processing, intervention path planning, and motion control;

[0232] Step 3: The remote control terminal controls the autonomous navigation system to start a multi-sensor fusion scheme to achieve precise mapping and positioning of the field weeding robot by inputting commands;

[0233] Step 4: Control the robot to move to the area to be weeded, construct a 3D lidar map of the operation area based on multi-sensor fusion, and save the map. At this stage, the starting point information when constructing the map must be recorded, and this starting point is the origin of the three-dimensional coordinate system in the constructed map;

[0234] Step 5: As Figure 2 shown, perform segmentation processing on the constructed map;

[0235] Step 5 is specifically implemented according to the following steps:

[0236] First, the saved map file needs to be exported and converted into an image format (such as PNG, JPEG, etc.) for subsequent image analysis and processing. Then, the range of the operation area is manually marked to ensure the accuracy and integrity of the marking. Next, the OpenCV library in Python is used for image processing to further segment the marked operation area into several rectangular areas; it is stipulated that the minimum width of the rectangular area shall not be less than the width of the robot used. If the width of some segmented areas is less than the width of the robot, the area will be merged with the adjacent rectangular area to ensure the actual usability of all segmented areas.

[0237] Combined with the origin information (usually the starting point of the map) recorded in step 4, during the segmentation process, with the help of image processing technology, the coordinates of the four corners of each rectangular area are extracted as the basis for subsequent calculations.

[0238] Step 6: At the beginning of the autonomous navigation stage of the weeding operation, the robot will start to load the previously saved map file and read the coordinates of the rectangular areas segmented on the map extracted in step 5. These coordinates include the specific positions of the upper left corner, upper right corner, lower left corner, and lower right corner of each rectangular area. Finally, these coordinates are saved in an array.

[0239] Step 7: The robot performs relocalization through the multi-sensor fusion scheme in step 3. Specifically, the robot matches the processed sensor data with the read map file by real-time processing of sensor data and cooperating with the submap mechanism and loop closure detection mechanism of the Cartographer algorithm, so as to achieve the positioning and confirmation of the current coordinates. After completing the relocalization, the robot determines its coordinate information on the read map, records the current position information as the starting point of the navigation operation, and uses this as a reference for path planning and decision-making during the subsequent navigation process.

[0240] Step 8: Based on the current coordinates, calculate the cost values for all the remaining coordinates saved in the array. Finally, select the coordinate with the lowest cost value for priority navigation and temporarily mask the remaining corner coordinates in the corresponding rectangular area.

[0241] Step 9: After the navigation order of the rectangular area selected in step 8, go to the rectangular area and perform the weeding operation. The navigation for the weeding operation in the rectangular area is realized by means of the online CCPP algorithm based on sensor data. During navigation, only the sensor data such as 3D lidar needs to be connected to the algorithm interface, which will not be elaborated here.

[0242] Step 10: After completing the weeding operation in each rectangular area, the robot will calculate the cost value again based on the current position to optimize the navigation sequence in real time. That is, the robot will loop through Step 8 and Step 9 until the weeding operation in all areas is completed;

[0243] Step 11: The robot performs a reset operation and returns to the navigation start position, and the autonomous navigation of the weeding operation ends.

Claims

1. An autonomous navigation system for a weeding robot for field crops, characterized in that, It consists of a core module and a perception module. The core module is responsible for information reception, data processing, algorithm operation, control command issuance, and motion control functions of the field crop weeding robot autonomous navigation system. The core modules include autonomous navigation system master control, chassis master control and remote control terminal; The perception module is responsible for the environmental information perception of the field crop weeding robot's autonomous navigation system, thereby enabling the robot to build a map of the working environment and realize positioning and repositioning functions; The perception module includes a wheel odometer, an inertial measurement unit (IMU), a lidar, a global satellite navigation system (GNSS) module, and a visual sensor.

2. The autonomous navigation system of the field crop weeding robot according to claim 1, characterized in that The autonomous navigation system master controller serves as the upper computer in the entire navigation system. The chassis master controller is the motion control core of the field crop weeding robot, responsible for controlling the robot to perform simple movements and process some data. It serves as the lower computer in the entire navigation system. The remote control terminal is a terminal used by users to remotely monitor the system when there is a network connection. This terminal is wirelessly connected to the robot.

3. The autonomous navigation system of the field crop weeding robot according to claim 1, wherein The wheel odometer is used to obtain the distance moved by the robot chassis, which is used for the basic positioning of the robot. The inertial measurement unit (IMU) is used to obtain the yaw angle, pitch angle and roll angle data of the chassis, which is used to obtain the robot's posture data. The lidar uses infrared beams to detect the robot's surrounding environment and generate point cloud information, which is used for the robot to draw maps, locate, relocate and implement obstacle avoidance functions; the visual sensor is used to detect weeds, send the obtained weed information to the remote control terminal, and assist the main control to start, stop and change the position of the robot. The global satellite navigation system (GNSS) module is used to obtain the absolute position information of the robot, which is used to implement the robot's repositioning function.

4. Control method for autonomous navigation system of weeding robot for field crops, characterized in that, At the same time, two technical means are used, namely multi-sensor fusion positioning and autonomous navigation decision-making. The autonomous navigation decision-making technology specifically includes work area partitioning, navigation sequence establishment and further optimization of navigation sequence. Work area partitioning means that after the robot constructs a map of the crop weeding area, the confirmed overall work area is divided into several rectangular areas suitable for executing full coverage path planning CCPP. Navigation sequence establishment refers to the process of the robot autonomously calculating and choosing to go to the next rectangular area based on the current position. The calculation result is only a local optimal solution. Further optimization of the navigation sequence means making the obtained local optimal solution closer to the global optimal solution by increasing the calculation depth.

5. The control method of the autonomous navigation system of the field crop weeding robot according to claim 4, characterized in that, Please follow the steps below to implement: Step 1. Start the robot: The robot power supply starts to supply power to the robot chassis, autonomous navigation system main controller, and chassis main controller. The autonomous navigation system main controller and chassis main controller start and complete initialization. All sensors are powered and initialized. Step 2: The main controller of the autonomous navigation system transmits data and interacts with commands with the main controller of the chassis through the CAN bus. On the premise of ensuring network connection, the remote control terminal remotely connects to the main controller of the autonomous navigation system through the SSH protocol and starts the ROS core. Through this process, the remote control terminal can achieve distributed control of the autonomous navigation system, including multiple functions such as task scheduling, status monitoring, sensor data acquisition and processing, intervention path planning, and motion control; Step 3: The remote control terminal controls the autonomous navigation system to start the multi-sensor fusion scheme by inputting commands to achieve precise mapping and positioning of the field weeding robot; Step 4: Control the robot to move to the area to be weeded, construct a 3D lidar map of the operation area based on multi-sensor fusion, and save the map. At this stage, the starting point information when constructing the map must be recorded, and this starting point is the origin of the 3D coordinate system in the constructed map; Step 5: Perform segmentation processing on the constructed map; Step 6: At the beginning of the autonomous navigation stage of the weeding operation, the robot will start to load the previously saved map file and read the coordinates of the rectangular areas segmented on the map extracted in Step 5. These coordinates include the specific positions of the upper left corner, upper right corner, lower left corner, and lower right corner of each rectangular area. Finally, save these coordinates in an array; Step 7: The robot performs repositioning through the multi-sensor fusion scheme in Step 3. Specifically, the robot matches the processed sensor data with the read map file by real-time processing of sensor data and cooperating with the submap mechanism and loop closure detection mechanism of the Cartographer algorithm to achieve the positioning and confirmation of the current coordinates. After completing the repositioning, the robot determines its coordinate information on the read map, records the current position information as the starting point of the navigation operation, and uses this as a reference for path planning and decision-making in the subsequent navigation process; Step 8: Based on the current coordinates, calculate the cost values of all the remaining coordinates saved in the array. Finally, select the coordinate with the lowest cost value for priority navigation and temporarily mask the remaining corner coordinates in the corresponding rectangular area; Step 9: After determining the navigation order of the rectangular area selected in Step 8, go to the rectangular area and perform weeding operations. The navigation of the weeding operation in the rectangular area is achieved by means of the online CCPP algorithm based on sensor data. When navigating, only need to connect the sensor data such as 3D lidar to the algorithm interface; Step 10: After completing the weeding operation of each rectangular area, the robot will calculate the cost value again based on the current position to optimize the navigation order in real time, that is, the robot will loop through Step 8 and Step 9 until the weeding operation of all areas is completed; Step 11: The robot performs a reset operation and returns to the navigation start position, and the autonomous navigation of the weeding operation ends.

6. The control method of the autonomous navigation system of the field crop weeding robot according to claim 5, characterized in that, The specific details of the multi-sensor fusion scheme in Step 3 are as follows: First, use the Kalman filter and complementary filter to fuse and filter the robot attitude data obtained by the nine-axis IMU, suppress the data disturbance and drift of the IMU, and at the same time synchronize the time of the IMU data with the time of ROS to facilitate the next multi-sensor data fusion; Then, use the extended Kalman filter algorithm to fuse the wheel odometer and IMU to obtain the fused odometer data; Next, fuse the fused odometer data with the lidar through the Cartographer algorithm, and the adjustment of the fusion weight parameters can be achieved by modifying the corresponding covariance matrix; Finally, align the local geographic coordinate system obtained by the GNSS module with the map coordinate system of the established map. When the navigation system runs for too long and the errors of sensors such as the wheel odometer, IMU, and lidar accumulate to a threshold and cannot be accurately repositioned through the loop closure detection link of the Cartographer algorithm alone, use the positioning information obtained by the GNSS module to calibrate the positioning information of the weeding robot.

7. The control method of the autonomous navigation system of the field crop weeding robot according to claim 6, characterized in that, In step 3, the extended Kalman filter algorithm is used to fuse the wheel odometer and IMU to obtain the fused odometer data, which is specifically implemented according to the following steps: Let the state vector be: where: x, y represent positions in the global coordinate system; θ represents the heading angle, v represents the linear velocity, ω bias , a bias represent the zero biases of the angular velocity and acceleration of the IMU. Using the angular velocity ω m and acceleration a m for prediction: v k|k-1 = v k-1 + (a m - a bias，k-1 )·Δt θ k|k-1 = θ k-1 + (ω m - ω bias，k-1 )·Δt ω bias，k|k-1 = ω bias，k-1 a bias，k|k-1 = a bias，k-1 The Jacobian matrix F, i.e., the state transition matrix, is expressed as follows: where a eff = a m -a bias，k-1 The process noise covariance Q is expressed as follows: Update step; Assume that the wheel odometer provides the linear velocity v odom and the heading angle θ odom , then the observation vector and the observation equation are as follows: The observation matrix H is: The observation noise covariance R is: The Kalman gain K is: K = P k|k-1 H T (HP k|k-1 H T + R) -1 State update: x k = x k|k-1 + K(z - h(x k|k-1 )) Covariance update: P k =(I - KH)P k|k-1 The propagation formula of the covariance is: P k|k-1 = FP k-1 F T + Q Assume that the robot moves in a straight line, but there is a zero bias error in the angular velocity of the IMU. The fusion process is divided into prediction and update. In the prediction step, the F is used to predict the states: position, velocity, and heading angle, and the covariance matrix P k|k-1 reflects the uncertainty of the heading angle; The update step includes using a wheel odometer to provide the heading angle observation value θ odom and the linear velocity observation value v odom , calculating the Kalman gain K, which determines how to fuse the predicted value and the observation value according to P k|k-1 . Finally, the fused x k will be closer to the true value. At each moment, the prediction and update steps are executed sequentially to output the fused state x k .

8. The control method of the autonomous navigation system of the field crop weeding robot according to claim 7, characterized in that, In step 3, the fused odometer data is fused with the lidar through the Cartographer algorithm, and the adjustment of the fusion weight parameters can be achieved by modifying the corresponding covariance matrix, which is specifically implemented according to the following steps: Define the optimization variable as the set of robot poses: where x i represents the pose of the robot in the global coordinate system at the $i$-th moment, including the position $x i , y i and the heading angle $\theta i ; The optimization objective function is: wherein, is the set of all constraints, and e ij (x) is the residual between the pose x i and x j , and Ω ij is the information matrix. Suppose that from time i to j, the relative pose measured by the odometer is Then the residual is The corresponding information matrix Ω odom is calculated from the covariance matrix Σ of the odometer odom as follows: The lidar constraint is expressed as: Obtain the pose x through scan matching i Relative to x j Estimation of The residual is: Lidar information matrix Ω lidar Dynamically adjusted according to the scan matching score: Among them, s ij ∈ [0, 1] is the scan matching score, reflecting the confidence of the match, and σ x , σ y is the basic noise parameter of the lidar in the translation direction, and σ θ is the basic noise parameter in the rotation direction; The information matrix is the inverse matrix of the covariance matrix: Ω = ∑ -1 The smaller the covariance matrix Σ, the larger the value of the information matrix Ω, and the higher the weight of this constraint in the optimization; in Cartographer, directly modify the covariance parameters in the configuration file to control the weights of different sensors in the optimization. The weight values correspond to the diagonal elements of the information matrix. The larger the value, the smaller the covariance. Finally, solve the optimization objective function to build a map.

9. The control method of the autonomous navigation system of the field crop weeding robot according to claim 8, characterized in that, Step 5 is specifically implemented according to the following steps: First, export the saved map file and convert it to a picture format. Then, manually mark the range of the operation area. Next, use the OpenCV library in Python for image processing to further segment the marked operation area into several rectangular areas; it is stipulated that the minimum width of the rectangular area shall not be less than the width of the used robot. If the width of some segmented areas is less than the width of the robot, this area will be merged with the adjacent rectangular area; Combined with the origin information recorded in step 4, during the segmentation process, use image processing technology to extract the coordinates of the four corners of each rectangular area as the basis for subsequent calculations.

10. The control method of the autonomous navigation system of the field crop weeding robot according to claim 9, characterized in that, The calculation formula of the cost value in step 8 is expressed as follows: Wherein, n is the number of sampling points of the non-set rectangular area, α is the weight corresponding to the sampling point during the calculation of the cost value, x1 is the abscissa value of the current coordinate, x2 is the abscissa value of the considered coordinate, y1 is the ordinate value of the current coordinate, y2 is the ordinate value of the considered coordinate, and the sampling points are several points evenly distributed on the line segment between the current coordinate and the considered coordinate; The calculation depth is introduced. The larger the set calculation depth is, the more iterative calculations are performed during the navigation decision-making process, and the closer the obtained local optimal solution is to the global optimal solution; Assume that when the calculation depth is set to 2, the global cost value will be calculated twice continuously. After the first global cost value calculation is completed, the optimal point is selected, the four coordinates of the rectangular area to which the point belongs are removed from the array, and the departure point of the rectangular area is calculated. Finally, the next global cost value calculation is performed based on the departure point, and the optimal navigation order is selected by combining the sum of the two global cost values; The calculation of the departure point is based on the selected optimal point as the starting point, and is calculated by using the weeding width of the robot and the width of the rectangular area, so as to obtain how many times the robot needs to reciprocate in the rectangular area to complete the weeding operation, and obtain the coordinates of the point where the robot leaves when completing the weeding operation of the rectangular area; The formula for the global cost value after introducing the calculation depth can be expressed as follows: L G = L L1 + L L2 + L L3 + … + L Lm where m is the set calculation depth, L Lm represents one of the partial cost values in the calculation at the m-th layer depth. At the same time, the coordinates involved in the calculation are copied to a two-dimensional array, and the coordinates corresponding to each cost value are stored in the cells of this two-dimensional array according to a certain calculation method, which is convenient for retrieving the coordinate navigation order corresponding to the cost value in reverse after obtaining the optimal global cost value finally; The calculation method of the specific unit for storing the coordinates corresponding to the cost value is as follows: the a-th global cost value L obtained after introducing the calculation depth Ga In, the b-th local cost value L Lb The corresponding horizontal and vertical coordinate values are respectively stored in the two-dimensional arrays Array[a - 1][2b - 2] and Array[a - 1][2b - 1]. After finally comparing to obtain the optimal global cost value, then position the array pointer to the row start point of the two-dimensional array corresponding to this global cost value, and read the coordinates in sequence and navigate.

Images