Agricultural vehicle positioning and mapping method and system based on multi-sensor fusion

Abstract

A kind of agricultural vehicle positioning and mapping method and system based on multi-sensor fusion. By installing laser radar, double GNSS antenna, IMU and odometer on the agricultural machine, the internal parameters, external parameters and time synchronization of each sensor are calibrated first to construct a unified space-time reference. The system adopts a tightly coupled fusion of laser radar, IMU and odometer to realize high-frequency local positioning, combines GNSS / IMU for global correction and error suppression, and corrects the cumulative drift through factor graph optimization and loop detection, finally generates a high-precision three-dimensional dense map. Further, from the 3D crop row form, methods such as RANSAC are used to separate ground and crop point clouds, extract crop row centerline, ridge and obstacle information, and identify the passable space. The invention effectively overcomes the excessive dependence of traditional navigation on GNSS signals and can realize accurate positioning and inter-row passable area perception of agricultural machinery in signal-limited environments, providing reliable environmental map support for unmanned agricultural operations.

Description

Technical Field

[0001] This invention belongs to the field of agricultural machinery automation and intelligence, specifically relating to a method for vehicle positioning and farmland environment mapping (SLAM) in an automatic navigation system for agricultural machinery. Background Technology

[0002] GNSS high-precision positioning-based automatic navigation systems for agricultural machinery have been widely applied to tractors, rice transplanters, and harvesters. Once a driver activates the navigation system, the onboard computer takes over the steering wheel. The driver only needs to monitor the movement of the agricultural machinery and the surrounding environment. Precise sowing and transplanting trajectory control ensures perfectly consistent row spacing for crops, resulting in better crop growth and increased yields. Precise harvesting path planning and navigation control improve the harvester's operational efficiency, all of which have yielded significant economic benefits. However, existing automatic navigation systems for agricultural machinery still have some technical functions that need improvement. For example, due to the lack of an environmental perception system, automatic navigation cannot significantly reduce the labor costs of agricultural production. Furthermore, interference factors such as rainy days and large trees obstructing the view may make it difficult for the navigation system to quickly achieve centimeter-level fixed solution accuracy upon startup, or during operation, certain factors may cause a sudden degradation from a fixed solution to a floating-point solution, leading to a sudden and significant path deviation. Research and development have been conducted on perceiving the surrounding environment of agricultural machinery and environmental mapping. For example, the invention patent "Agricultural Machinery Navigation Method for Farmland Environment Perception" (CN 108107887 A) uses radar to detect obstacle targets and their location, and then uses machine vision to identify the category of the target; the invention patent "An Agricultural Machinery Unmanned Driving Navigation System Based on Farmland Environment Perception" (CN 113359126 A) proposes a method for detecting and controlling agricultural implements to avoid obstacles whose height does not exceed the vehicle's ground clearance; the invention patent application "A Vehicle Obstacle Avoidance Method, Device, Equipment and Medium" (CN 119672667 A) uses machine vision to obtain the size and pixel center point coordinates of the target obstacle, uses a depth detection sensor to obtain its depth, and determines the relative distance between the target obstacle and the vehicle; based on the distance threshold, it delineates emergency obstacle avoidance zones and warning zones; the invention patent application "Environmental Perception Method, Device, System and Storage Medium for Agricultural Machinery Autonomous Driving" (CN120410766 A) obtains a farmland environment perception model by training agricultural environment data through simulation, and inputs multi-sensor fusion data into the above model to obtain the farmland environment perception result. The invention patent application "A Path Navigation Method for Unmanned Agricultural Machinery Based on Farmland Environmental Perception and Judgment" (CN120821184A) focuses on pre-sensing terrain undulations, predicting risks and deviations, and proactively generating pre-corrected paths with terrain compensation, thereby avoiding the technical problem of causal misalignment where risks precede control from the source. From the aforementioned published literature, it can be seen that the environmental perception of agricultural machinery automatic navigation systems has evolved from early obstacle detection and location to the perception of crop types and ground terrain undulations.The automation technology of agricultural machinery is evolving from assisted driving to unmanned driving and agricultural robots. This places higher and more detailed demands on the environmental perception systems of agricultural machinery. Row-to-row operations are a common scenario, such as tractors pulling cultivators for mechanical weeding, high-clearance plant protection machines operating between rows with their spray booms extended, and cabbage harvesters needing to travel between rows for harvesting. The space between crop rows becomes the area accessible to the tires and tracks of agricultural machinery, and the machinery can only move within this narrow space. Automatic navigation systems cannot ignore the presence of crop rows. Current publicly available literature lacks in-depth research on methods for environmental mapping and vehicle positioning within crop row spaces. Summary of the Invention

[0003] To overcome the shortcomings of existing agricultural machinery automatic navigation systems that rely too heavily on GNSS high-precision positioning and have weak ability to perceive the space between crop rows, this invention provides a technical solution based on multi-sensor data fusion. This solution fuses data from GNSS / IMU global satellite positioning systems with lidar / IMU / odometer positioning systems to estimate the vehicle's position, attitude, and motion state, create a field map, and extract road information from 3D crop row morphology. This ensures that agricultural machinery automatic navigation systems are no longer unable to carry out agricultural machinery operations due to insufficient satellite search data or external interference signals.

[0004] The technical solution adopted by this invention to solve its technical problem is as follows: A direction-finding auxiliary GNSS antenna and a main GNSS antenna are installed side-by-side on the top of the agricultural machinery, one on the left and one on the right, symmetrically with respect to the longitudinal centerline of the vehicle; or the auxiliary GNSS antenna and the main GNSS antenna are installed one in front of the other. Two feed lines connect the GNSS antennas to the GNSS receiver, respectively. The GNSS receiver then transmits data to the intelligent navigation display terminal via an RS232 serial communication line. A laser radar facing directly forward is installed at the head of the longitudinal centerline on the top of the chassis, and a network cable connects the laser radar to the intelligent navigation display terminal. An IMU is installed on the surface at the center of the rear axle, and an RS232 serial communication line connects the IMU to the intelligent navigation display terminal. The system comprises: a speed sensor installed at the transmission gear of each of the left and right rear wheels to form an odometer; a signal line connecting the odometer to the intelligent navigation display terminal; and a data processing and fusion module for receiving and processing point cloud data from the lidar, acceleration and angular velocity data from the IMU, pulse signals from the odometer, and positioning, direction finding, and velocity data from the GNSS receiver, fusing the data through Kalman filtering and SLAM algorithms to achieve vehicle positioning and farmland environment mapping; a navigation control module for generating control commands to adjust the vehicle's heading based on the positioning and mapping results; and a display and human-machine interaction module for displaying the environmental map, vehicle position and status in real time, and receiving operation commands.

[0005] First, each sensor needs to be calibrated in an in-vehicle environment to correct the intrinsic parameters, obtain the extrinsic parameters, and calibrate the synchronization time difference. The specific steps are as follows: S101: Intrinsic parameter correction calibration of a single sensor. The IMU is calibrated using the six-sided method to obtain deterministic error—zero bias (…). b a , b g ), scale factor error ( Scale a , Scale g ), non-orthogonal error ( T a , T g ), and random errors—angle random walk, velocity random walk, and zero-bias instability—are used for noise covariance modeling of the state estimation filter. The calibration models for the accelerometer and gyroscope are as follows: a corrected = T a * (diag( Scale a ) * a raw + b a (1) g corrected = T g * (diag( Scale g ) * g raw + b g (2) In the formula, a raw , g raw These are the original acceleration and angular velocity readings. a corrected , g corrected These are the corrected acceleration and angular velocity. The lidar uses motor speed deviation, scanning angle deviation, and harness angle provided by the manufacturer; the odometer is obtained through on-site measurement to determine the odometer scale factor (left wheel). k l Right wheel k r The calibration model includes: wheel radius, wheel spacing, and tire slip coefficient. (3) (4) (5) (6) (7) (8) In the formula, d l , d r This is the actual distance the left and right wheels moved. ∆pls It is the number of pulses. v l , v r It is the translational speed of the left and right wheel centers. v It is the translational speed of the rear axle center. ω It is the angular velocity of the car body. L This refers to the wheelbase between the left and right wheels. Satellite positioning systems obtain baseline vectors through calibration. B vec =[ ΔE,ΔN,ΔU ] T Baseline length B Original direction finding yaw angle α .

[0006] S102: Construct a unified coordinate system, including: Local coordinate system {W}, with its origin at the projection of the vehicle's rear axle center onto the ground (initial moment), X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the forward direction of the vehicle (directly forward), and Z-axis defined as perpendicular to the ground upward. Vehicle body coordinate system {B}, aligned with the local coordinate system {W}, but with its origin at the rear axle center. IMU coordinate system {I}, with its origin at the center of the IMU measurement chip, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as directly forward, Z-axis defined as perpendicular to the ground upward, pitch as the rotation angle around the Y-axis, roll as the rotation angle around the X-axis, and yaw as the rotation angle around the Z-axis. LiDAR coordinate system {L}, with its origin at the center of the LiDAR scanning sphere, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the direction towards the ground, and Z-axis defined as directly forward. GNSS antenna coordinate system {G}, with its origin at the phase center point of the main GNSS antenna, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the forward direction of the vehicle (directly forward), and Z-axis defined as perpendicular to the ground upward.

[0007] S103: Extrinsic parameter calibration between sensors. Loose coupling calibration of the satellite positioning system and IMU, using trajectory matching method to calibrate extrinsic parameters of the GNSS lever arm. The attitude matrix of the IMU relative to the ground coordinate system Let the calibration model of the main GNSS antenna be: (9) (10) In the formula, It refers to the three-dimensional coordinates of the phase center of the main GNSS antenna in the ground coordinate system. It is the lever arm of the vehicle coordinate system relative to the ground coordinate system. It is the rotation matrix of the vehicle coordinate system relative to the ground coordinate system. It is a rotation matrix composed of roll, pitch, and yaw angles measured in real time by the IMU.

[0008] S104: External parameter calibration of lidar and IMU. The agricultural machinery performs linear acceleration / deceleration, turning, and uphill / downhill movements at the calibration site. The pose changes estimated by the lidar and IMU are used to solve for the lidar's lever vector. Rotation matrix The model for converting laser point clouds into ground coordinates is as follows: (11) In the formula, These are the three-dimensional coordinates of the point cloud in the ground coordinate system. It is a three-dimensional coordinate system in the laser coordinate system of point cloud.

[0009] S105: Calibration of the LiDAR / IMU / odometer positioning system. First, calibrate the IMU and odometer to obtain the extrinsic parameter rotation matrix. Translation matrix Then, combining the calibration results of the lidar and IMU, a joint verification and accuracy evaluation of the lidar / IMU / odometer positioning system was conducted. Among these, the vehicle displacement increment was: (12) In the formula, It refers to the vehicle body displacement in the ground coordinate system. It is the vehicle displacement output by the odometer.

[0010] S106: Calibrate the time synchronization difference. Because agricultural machinery generally operates at low speeds and is typically equipped with only low-cost sensors, it is difficult for sensors to achieve true time synchronization. Therefore, a software synchronization method is used, which uses the data output frequency (10Hz) of the satellite positioning system as a fixed frequency. Each time a PPS pulse signal is received, the synchronization time is determined by combining it with the GGA data frame time. The time difference between the closest synchronization time of the lidar / IMU / odometer is then calibrated.

[0011] S107: Joint calibration of GNSS / IMU global satellite positioning system and lidar / IMU / odometer positioning system, driving agricultural machinery to slowly rotate in place for 15-20 minutes to optimize external parameters: Further linear acceleration / deceleration, large-arc turns, and figure-eight driving excitation motions were performed to iteratively optimize the extrinsic parameters until the translational error (ATE) RMS value stabilized within 2–5 cm.

[0012] S108: All calibration parameters (internal parameters, external parameters, time difference) are stored in the system configuration file. The calibration accuracy is verified by trajectory comparison and error statistics (ATE, RPE). A unified spatiotemporal reference that can be used for positioning and mapping algorithms is output.

[0013] The methods for locating agricultural machinery and vehicles and mapping farmland environment are as follows: S201: Acquire synchronization data. At the arrival of the PPS pulse or GGA data frame of each satellite positioning system, as the synchronization time point, acquire data from the IMU, lidar, and odometer, and correct the data according to the calibrated internal parameters.

[0014] S202: Local Positioning. A tightly coupled lidar / IMU / odometer positioning system outputs high-frequency local pose.

[0015] S203: Global Correction. The GNSS / IMU global positioning system provides global position and heading, which is fused with the lidar / IMU / odometry positioning system through the ESKF algorithm.

[0016] S204: Backend optimization. Factor graph fusion of multi-source observations optimizes the global pose sequence.

[0017] S205: Loop Closure Correction. Loop closures are detected based on point cloud descriptors, and accumulated errors are corrected.

[0018] S206: Map generation. Point cloud alignment and fusion to output a dense 3D map.

[0019] S207: Generate passable space. Extract crop row parameters and obstacle locations from the 3D crop row morphology, and generate passable space. After completing this step, return to step S201 and wait for synchronization data to arrive, continuously processing in a loop.

[0020] Further, in step S202, the vehicle's state vector at time k of the local positioning system of the lidar / IMU / odometer is defined as: ; The position in the formula is The speed is Quaternion posture is accelerometer zero bias is The gyroscope has zero bias. .

[0021] The short-term attitude change information obtained by the joint calibration of the lidar point cloud, mileage, angular velocity and acceleration output by the IMU, and their integration is input into the tightly coupled front-end fusion module of lidar and inertial navigation. The unified state constraints are constructed with the inertial navigation pre-integration model through the laser point cloud feature extraction and scanning registration algorithm (including but not limited to the matching method based on feature points, the optimization method based on point-to-line / point-to-surface constraints, or the registration method based on the probability model) to realize high-frequency attitude estimation of agricultural machinery at continuous time.

[0022] The IMU pre-integration model is as follows: (13) (14) (15) In the formula , These are the observables of angular velocity and acceleration, respectively. , To achieve zero bias for the gyroscope and accelerometer, , , Let K represent the attitude, velocity, and position at time k.

[0023] The lidar point cloud uses a feature matching method: extracting geometric features such as corner points and planar points, constructing residuals using point-to-line and point-to-plane constraints, and performing tight coupling optimization with the IMU pre-integration results. The odometer provides the left and right wheel speeds v1 and vr, and the vehicle speed v and angular velocity ω are derived using formulas (7) and (8). This constraint is added as a factor to the optimization graph.

[0024] Furthermore, in step S203, the GNSS / IMU global satellite positioning system provides global position and heading information. The position of the vehicle coordinate origin is obtained from the position of the main GNSS antenna using formula (9).

[0025] Coordinate system integration and data fusion processing. The LiDAR point cloud is transformed from the laser coordinate system {L} to the ground coordinate system {B} using formula (11), which initially realizes the vehicle body origin position correction and point cloud coordinate transformation.

[0026] The tightly coupled fusion framework updates the state variables. Error-state Kalman filtering (ESKF) is used for high-frequency state prediction and updating; the error state is defined as:

[0027] Linearized state propagation: (16) In the formula , Let Jacobian matrix be the IMU error propagation matrix.

[0028] Measurement updates are performed using lidar, odometer, and GNSS observations as observation constraints. (17) In the formula, K is the Kalman gain. For geometric measurement models; Using Lie algebras as the "error state" of attitude for minor corrections: (18) Further, in step S204, the pose estimated by the front end and the multi-frame point cloud are used as the observation input to the back-end optimization module of the factor graph. Global consistency optimization is performed using a graph-based nonlinear optimization framework (GTSAM) to obtain a temporally continuous and drift-controlled global pose sequence. Subsequently, combined with a loop closure detection algorithm based on global geometric descriptors, similarity judgment and spatial constraint supplementation are performed on historical point cloud frames to further correct accumulated errors and construct a globally optimized pose. (19) In the formula, It is the IMU pre-integration residual. LiDAR point cloud matching residuals It is the GNSS position and heading residual. Odometer motion constraint residuals Loop closure detection residual.

[0029] Furthermore, in step S205, the revisiting region is identified based on the global descriptor of the point cloud, and spatial constraints are added to correct the cumulative drift.

[0030] Furthermore, in step S206, using the optimized global pose X*, all lidar point clouds are aligned to the {W} coordinate system, and voxel filtering and surface reconstruction are performed to generate a high-precision, highly consistent 3D dense map of farmland crop rows with complete geometric structure, including information on crop rows, field ridges, obstacles, and other terrain features.

[0031] Further, in step S207, the parameters of the crop rows are extracted using the spatial map of the crop rows, as follows: S301: Use radius filtering to remove noisy point cloud clusters; use the RANSAC rule to fit the ground plane and separate the ground and crop point clouds: normalized normal vector formed by three non-collinear points. If the following inequalities are satisfied, then p i The point is an interior point: (20) In the formula p i It refers to a point in a point cloud. d It is a plane constant. T This is a threshold. Process all points in the point cloud. If the number of inliers in the current model is greater than the number of inliers in the optimal model, update the optimal model. The optimal model can be obtained through iterative calculation. All inliers are refitted to the ground plane. Set a distance threshold, traverse the remaining points, and group points whose distance to each other is less than the threshold into the same cluster. Clusters where the number of points with a height higher than the set crop height exceeds 20% are classified as obstacle point clouds. The remaining 3D point clouds are projected onto a new 2D plane, creating 5mm*5mm grid cells. Accumulate along the vertical Z-axis to create a height density map, generating a 2D grid map. Each cell stores features such as point cloud density and average height. Along the X-axis, generate a height density curve, smoothed using a Gaussian filter. Under row spacing constraints, use a peak detection algorithm to obtain a series of peaks. These form a group of centerline points on a 2D plane.

[0032] S302: Cluster and group the centerline point cluster data. Apply RANSAC iterative clustering in the far region to detect main lines, i.e., potential field ridges; when randomly sampling two points to generate a straight line hypothesis, if the line connecting the two points is too steep (the angle is close to perpendicular), abandon the sampling and resample to increase the probability of sampling an approximately horizontal line. If a model with a sufficient number of interior points (greater than a threshold) is successfully found... M 1 Then check M 1 The angle. If the angle is close to horizontal, it is initially marked as a "candidate field ridge line", and its interior point set is... B candidate Remove these interior points from the original graph to obtain the remaining set of points.

[0033] S303: In the remaining point set, from near to far, run the RANSAC rule (the model chooses a straight line or an arc based on prior knowledge) to obtain a new row model. M row and its interior point set inliers row .if M row If the angle is close to vertical (i.e., consistent with the expected direction of the crop row) and there are enough inliers, then this is considered a crop row. After completing one round of searching, remove these inliers from the remaining point set.

[0034] Continue clustering the remaining point set using the RANSAC rule to obtain a new row model. M row and its interior point set inliers row Remove interior points. Repeat the process to find new models and interior points until the remaining points are less than a threshold.

[0035] S304: Fit an initial row model to each set of interior points one by one. L i (Straight line / circular arc); Find the distance model within the entire point group P. L i Points within a certain threshold are added. S i .

[0036] S305: Use the enlarged version S i Refit a more accurate model L inew Then search for nearby points again, repeating this process until no new points are added.

[0037] S306: If B candidate and L inew A continuous line spanning the width of multiple rows of crops, with clear intersections or endpoints at the far ends of each row of crops, is ultimately identified as the field ridge point set B. If... B candidate If the ridges are weak or nonexistent, it may be due to the absence of clear field ridges or the natural end of crop rows. It is advisable to make a judgment when agricultural machinery vehicles are closer.

[0038] Point clouds classified as obstacles have been marked and will be processed separately. At this point, the reconstruction of the 3D field environment map, the extraction of crop row parameters from the 3D crop row morphology, and the vehicle's deviation distance and heading deviation angle have been completed.

[0039] The beneficial effect of this invention is that it uses multi-sensor data fusion technology to construct a 3D environmental map around agricultural machinery. In particular, it can extract drivable roads from the crop row morphology. Even if the GNSS signal is weak or even absent, the wheels of the agricultural machinery can drive autonomously, which helps to promote the technological development of agricultural robots. Attached Figure Description

[0040] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0041] Figure 1 An example of installing a vehicle positioning and farmland environment mapping system on a tractor.

[0042] Figure 2 This is an example of sensor calibration and time synchronization calibration.

[0043] Figure 3 This is an example of an algorithm for agricultural machinery vehicle positioning and farmland environment mapping that integrates GNSS / IMU global satellite positioning system with lidar / IMU / odometer positioning system.

[0044] Figure 4 This is step S207—an example of extracting crop row parameters from 3D crop row morphology.

[0045] In the diagram, 1 is a tractor, 2 is a lidar, 3 is an auxiliary GNSS antenna, 4 is a main GNSS antenna, 5 is an intelligent navigation display terminal, 6 is a GNSS receiver, and 7 is an IMU. Detailed Implementation

[0046] Figure 1 This is an embodiment of a vehicle positioning and farmland environment mapping system installed on a tractor. A direction-finding auxiliary GNSS antenna 3 and a main GNSS antenna 4 are installed at the front and rear of the tractor 1, respectively. Two feed lines connect the GNSS antennas 3 and 4 to the GNSS receiver 6, which then transmits data to the intelligent navigation display terminal 5 via an RS232 serial communication line. A lidar 2 facing forward is installed at the head of the longitudinal centerline on the top of the chassis, and a network cable connects the lidar 2 to the intelligent navigation display terminal 5. An IMU 7 is installed on the surface at the center of the rear axle, and an RS232 serial communication line connects the IMU 7 to the intelligent navigation display terminal 5. A speed sensor is installed at the transmission gear of each of the left and right rear wheels. An odometer is formed, and a signal line connects the odometer to the intelligent navigation display terminal 5. The intelligent navigation display terminal 5 integrates: a data processing and fusion module, used to receive and process point cloud data from the lidar 2, acceleration and angular velocity data from the IMU 7, pulse signals from the odometer, and positioning, direction finding, and velocity data from the GNSS receiver 6, and to perform data fusion through Kalman filtering and SLAM algorithms to achieve vehicle positioning and farmland environment mapping; a navigation control module, used to generate control commands to adjust the vehicle's heading based on the positioning and mapping results; and a display and human-machine interaction module, used to display the environmental map, vehicle position and status in real time, and to receive operation commands.

[0047] Figure 2 This is an example of sensor calibration and time synchronization calibration, and the specific steps are as follows: S101: Intrinsic parameter correction calibration of a single sensor. The IMU7 uses a six-sided calibration method to obtain deterministic error—zero bias (…). b a ,b g ), scale factor error ( Scale a , Scale g ), non-orthogonal error ( T a , T g ), and random errors—angle random walk, velocity random walk, and zero-bias instability—are used for noise covariance modeling of the state estimation filter. For the accelerometer and gyroscope, the calibration models are shown in formulas (1) and (2), respectively. The lidar 2 uses the motor speed deviation, scanning angle deviation, and beam angle provided by the manufacturer; the odometer is measured on-site to obtain the odometer scale factor (left wheel). k l Right wheel k r The calibration model includes wheel radius, wheel spacing, and tire slip coefficient, as shown in formulas (3)-(8). The satellite positioning system obtains the baseline vector through calibration. B vec =[ ΔE,ΔN,ΔU ] T Baseline length B Original direction finding yaw angle α .

[0048] S102: Construct a unified coordinate system, including: Local coordinate system {W}, with its origin at the projection of the vehicle's rear axle center onto the ground (initial moment), X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the forward direction of the vehicle (directly forward), and Z-axis defined as perpendicular to the ground upward. Vehicle body coordinate system {B}, aligned with the local coordinate system {W}, but with its origin at the rear axle center. IMU7 coordinate system {I}, with its origin at the center of the IMU7 measurement chip, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as directly forward, Z-axis defined as perpendicular to the ground upward, pitch as the rotation angle around the Y-axis, roll as the rotation angle around the X-axis, and yaw as the rotation angle around the Z-axis. LiDAR coordinate system {L}, with its origin at the center of the LiDAR 2 scanning sphere, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the direction towards the ground, and Z-axis defined as directly forward. GNSS antenna coordinate system {G}, with its origin at the phase center point of the main GNSS antenna 4, X-axis defined as the right-hand direction of the vehicle, Y-axis defined as the forward direction of the vehicle (directly forward), and Z-axis defined as perpendicular to the ground upward.

[0049] S103: Extrinsic parameter calibration between sensors. Loosely coupled calibration of the satellite positioning system and IMU7, using the trajectory matching method to calibrate extrinsic parameters of the GNSS lever arm. The attitude matrix of IMU7 relative to the ground coordinate system The calibration model of the main GNSS antenna 4 is given by formulas (9) and (10).

[0050] S104: External parameter calibration of LiDAR 2 and IMU7. Tractor 1 performs linear acceleration / deceleration, turning, and uphill / downhill movements on the calibration site. The lever vector of LiDAR 2 is calculated based on the pose changes estimated by LiDAR 2 and IMU7 respectively. Rotation matrix The model for converting laser point clouds into ground coordinates is shown in formula (11).

[0051] S105: Calibration of the LiDAR / IMU / odometer positioning system. First, calibrate the IMU7 and odometer to obtain the extrinsic rotation matrix. Translation matrix Then, combining the calibration results of LiDAR 2 and IMU 7, a joint verification and accuracy evaluation of the LiDAR / IMU / odometer positioning system is performed. The vehicle displacement increment is given by formula (12).

[0052] S106: Calibrate the time synchronization difference. Because the tractor 1 generally operates at a low speed and is usually equipped with only low-cost sensors, it is difficult for the sensors to achieve complete time synchronization. Therefore, a software synchronization method is used, that is, using the data output frequency (10Hz) of the satellite positioning system as a fixed frequency, and using the time of each PPS pulse signal received, combined with the time of the GGA data frame, as the synchronization time, to calibrate the time difference between the closest synchronization time of the lidar / IMU / odometer.

[0053] S107: Joint calibration of GNSS / IMU global satellite positioning system and lidar / IMU / odometer positioning system, driving tractor 1 to perform slow rotation in place for 15-20 minutes to optimize external parameters: Further linear acceleration / deceleration, large-arc turns, and figure-eight driving excitation motions were performed to iteratively optimize the extrinsic parameters until the translational error (ATE) RMS value stabilized within 2–5 cm.

[0054] S108: All calibration parameters (internal parameters, external parameters, time difference) are stored in the system configuration file. The calibration accuracy is verified by trajectory comparison and error statistics (ATE, RPE). A unified spatiotemporal reference that can be used for positioning and mapping algorithms is output.

[0055] Figure 3 This is an example of an algorithm for tractor positioning and farmland environment mapping that integrates GNSS / IMU global satellite positioning system and lidar / IMU / odometer positioning system. The method is as follows: S201: Acquire synchronization data. At the arrival of the PPS pulse or GGA data frame from each satellite positioning system, as the synchronization time point, acquire data from IMU7, LiDAR 2, and the odometer, and correct the data according to the calibrated internal parameters.

[0056] S202: Local Positioning. A tightly coupled lidar / IMU / odometer positioning system outputs high-frequency local pose.

[0057] S203: Global Correction. The GNSS / IMU global positioning system provides global position and heading, which is fused with the lidar / IMU / odometry positioning system through the ESKF algorithm.

[0058] S204: Backend optimization. Factor graph fusion of multi-source observations optimizes the global pose sequence.

[0059] S205: Loop Closure Correction. Loop closures are detected based on point cloud descriptors, and accumulated errors are corrected.

[0060] S206: Map generation. Point cloud alignment and fusion to output a dense 3D map.

[0061] S207: Generate passable space. Extract crop row parameters and obstacle locations from the 3D crop row morphology, and generate passable space. After completing this step, return to step S201 and wait for synchronization data to arrive, continuously processing in a loop.

[0062] Further, in step S202, the vehicle's state vector at time k of the local positioning system of the lidar / IMU / odometer is defined as:

[0063] The position in the formula is The speed is Quaternion posture is accelerometer zero bias is The gyroscope has zero bias. .

[0064] The short-term attitude change information obtained by the joint calibration of the lidar point cloud, mileage, angular velocity and acceleration output by the IMU and their integration is input into the tightly coupled front-end fusion module of lidar and inertial navigation. The algorithm of scanning registration based on lidar point cloud feature extraction and optimization method based on point-to-line / point-to-surface constraints is used to construct a unified state constraint with the inertial navigation pre-integration model, thereby realizing high-frequency attitude estimation of the tractor at continuous time.

[0065] The IMU pre-integration model is given by formulas (11)-(13).

[0066] The lidar point cloud employs a feature matching method: extracting geometric features such as corner points and planar points, constructing residuals using point-to-line and point-to-plane constraints, and performing tight coupling optimization with IMU pre-integration results. The odometry provides left and right wheel speeds. v 1 , v r The vehicle speed is derived using formulas (7) and (8). v With angular velocity ω This constraint is added as a factor to the optimization graph.

[0067] Furthermore, in step S203, the GNSS / IMU global satellite positioning system provides global position and heading information. The position of the vehicle coordinate origin is obtained from the position of the main GNSS antenna 4 using formula (9).

[0068] Coordinate system integration and data fusion processing. The LiDAR point cloud is transformed from the laser coordinate system {L} to the ground coordinate system {B} using formula (11), which initially realizes the vehicle body origin position correction and point cloud coordinate transformation.

[0069] The tightly coupled fusion framework updates the state variables. Error-state Kalman filtering (ESKF) is used for high-frequency state prediction and updating; the error state is defined as:

[0070] Linearized state propagation uses formula (15).

[0071] For measurement updates, lidar 2, odometer, and GNSS observations are all used as observation constraints, and formula (17) is applied. The “Li algebra” is used as the “error state” of the attitude for small corrections, using formula (18).

[0072] Further, in step S204, the pose estimated by the front end and the multi-frame point cloud are used as the observation input of the factor graph to the back-end optimization module. Global consistency optimization is performed through the graph-based nonlinear optimization framework (GTSAM) to obtain a temporally continuous and drift-controlled global pose sequence. Subsequently, combined with the loop closure detection algorithm based on global geometric descriptors, similarity judgment and spatial constraint supplementation are performed on the historical point cloud frames to further correct the accumulated error and construct the globally optimized pose application formula (19).

[0073] Furthermore, in step S205, the revisiting region is identified based on the global descriptor of the point cloud, and spatial constraints are added to correct the cumulative drift.

[0074] Furthermore, in step S206, the optimized global pose is utilized. X*All lidar point clouds are aligned to the {W} coordinate system, and voxel filtering and surface reconstruction are performed to generate a high-precision, highly consistent 3D dense map of farmland crop rows with complete geometric structure, including information on crop rows, field ridges, obstacles and other terrain features.

[0075] Figure 4 This is an example of step S207—extracting crop row parameters from 3D crop row morphology—which is performed according to the following steps: S301: Use radius filtering to remove noisy point cloud clusters; use the RANSAC rule to fit the ground plane and separate the ground and crop point clouds: normalized normal vector formed by three non-collinear points. If inequality (20) is satisfied, then point pi is an interior point. Process all points in the point cloud. If the number of interior points in the current model is greater than the number of interior points in the best model, update the best model. The best model can be obtained through iterative calculation. All interior points are refitted to the ground plane. Set a distance threshold, traverse the remaining points, and group points whose distance to each other is less than the threshold into the same cluster. Among them, the number of point cloud heights higher than the crop set height in the cluster is more than 20% and is classified as obstacle point cloud. The remaining 3D point cloud is projected onto a new 2D plane, and 5mm*5mm grid cells are created. Accumulate along the vertical direction (Z-axis) to create a height density map and generate a 2D grid map. Each cell stores features such as point cloud density and average height. Along the X-axis, a height density curve is generated and smoothed using Gaussian filtering. Under the constraint of row spacing, a peak detection algorithm is used to obtain a series of peaks. These form a group of centerline points on a 2D plane.

[0076] S302: Cluster and group the centerline point cluster data. Apply RANSAC iterative clustering in the far region to detect main lines, i.e., potential field ridges; when randomly sampling two points to generate a straight line hypothesis, if the line connecting the two points is too steep (the angle is close to perpendicular), abandon the sampling and resample to increase the probability of sampling an approximately horizontal line. If a model with a sufficient number of interior points (greater than a threshold) is successfully found... M 1 Then check M 1 The angle. If the angle is close to horizontal, it is initially marked as a "candidate field ridge line", and its interior point set is... B candidate Remove these interior points from the original graph to obtain the remaining set of points.

[0077] S303: In the remaining point set, from near to far, run the RANSAC rule (the model chooses a straight line or an arc based on prior knowledge) to obtain a new row model. M row and its interior point set inliersrow .if M row If the angle is close to vertical (i.e., consistent with the expected direction of the crop row) and there are enough inliers, then this is considered a crop row. After completing one round of searching, remove these inliers from the remaining point set.

[0078] Continue clustering the remaining point set using the RANSAC rule to obtain a new row model. M row and its interior point set inliers row Remove interior points. Repeat the process to find new models and interior points until the remaining points are less than a threshold.

[0079] S304: Fit an initial row model to each set of interior points one by one. L i (Straight line / circular arc); Find the distance model within the entire point group P. L i Points within a certain threshold are added. S i .

[0080] S305: Use the enlarged version S i Refit a more accurate model L inew Then search for nearby points again, repeating this process until no new points are added.

[0081] S306: If B candidate and L inew A continuous line spanning the width of multiple rows of crops, with clear intersections or endpoints at the far ends of each row of crops, is ultimately identified as the field ridge point set B. If... B candidate If the ridges are weak or nonexistent, it may be due to the absence of clear field ridges or the natural end of crop rows. It is advisable to make a judgment when agricultural machinery vehicles are closer.

[0082] Point clouds classified as obstacles are marked and will be processed separately. At this point, the reconstruction of the 3D field environment map, the extraction of crop row parameters from the 3D crop row morphology, and the vehicle's deviation distance and heading deviation angle have been completed. Regardless of the presence or absence of satellite positioning signals, agricultural vehicles can now detect traversable roads or areas.

Claims

1. A method and system for agricultural machinery vehicle localization and mapping based on multi-sensor fusion, characterized in that: The methods for locating agricultural machinery and vehicles and mapping farmland environment are as follows: S201: Acquire synchronization data. At the arrival of each PPS pulse or GGA data frame from each satellite positioning system, as the synchronization time point, acquire data from the IMU, lidar, and odometer, and correct the data according to the calibrated internal parameters. S202: Local positioning, tightly coupled lidar / IMU / odometer positioning system, outputting high-frequency local pose; S203: Global correction, the GNSS / IMU global satellite positioning system provides global position and heading, and is fused with the lidar / IMU / odometer positioning system through the ESKF algorithm; S204: Backend optimization, factor graph fusion of multi-source observations, and optimization of global pose sequence; S205: Loop closure correction, based on point cloud descriptors to detect loop closures and correct accumulated errors; S206: Map generation, point cloud alignment and fusion, outputting a dense 3D map; S207: Passable space generation. Extract crop row parameters and obstacle locations from the 3D crop row shape to generate passable space. After completing this step, return to step S201 and wait for the arrival of synchronization data. The process is continuously looped.

2. The method and system for agricultural machinery vehicle localization and mapping based on multi-sensor fusion as described in claim 1, characterized in that: In step S202, the local positioning system of the lidar / IMU / odometer is at a certain time. k The state vector of the vehicle body is defined as: ； The short-term attitude change information obtained by the joint calibration of the lidar point cloud, mileage, angular velocity and acceleration output by the IMU and their integration is input into the tightly coupled front-end fusion module of lidar and inertial navigation. The algorithm of scanning registration based on the laser point cloud feature extraction and the optimization method based on point-to-line / point-to-surface constraints is used to construct a unified state constraint with the inertial navigation pre-integration model, thereby realizing high-frequency attitude estimation of agricultural machinery at continuous time. The IMU pre-integration model is as follows: (13) (14) (15) The lidar point cloud employs a feature matching method: extracting geometric features such as corner points and planar points, constructing residuals using point-to-line and point-to-plane constraints, and performing tight coupling optimization with IMU pre-integration results; left and right wheel speeds are provided by the odometry. v 1 , v r Obtain vehicle speed v With angular velocity ω This constraint is added as a factor to the optimization graph.

3. The method and system for agricultural machinery vehicle localization and mapping based on multi-sensor fusion according to claim 1, characterized in that: In step S203, the GNSS / IMU global satellite positioning system provides global position and heading information; the position of the vehicle's coordinate origin is obtained from the position of the main GNSS antenna; the lidar point cloud is transformed from the lidar coordinate system {L} to the ground coordinate system {B}, initially realizing the vehicle's origin position correction and point cloud coordinate transformation; the tightly coupled fusion framework updates the state variables, and uses Error State Kalman Filter (ESKF) for high-frequency state prediction and updating, where the error state is defined as: ； Linearized state propagation: （16） Measurement updates are performed using lidar, odometer, and GNSS observations as observation constraints. （17） Using "Li algebra" as the "error state" of attitude for minor corrections: .

4. The method and system for agricultural machinery vehicle localization and mapping based on multi-sensor fusion as described in claim 1, characterized in that: In step S204, the pose estimated by the front end and the multi-frame point cloud are used as the observation input of the factor graph to the back-end optimization module. Global consistency optimization is performed through a graph-based nonlinear optimization framework to obtain a temporally continuous and drift-controlled global pose sequence. Subsequently, by combining a loop closure detection algorithm based on global geometric descriptors, similarity determination and spatial constraint supplementation are performed on historical point cloud frames to further correct accumulated errors and construct a globally optimized pose: （19） Furthermore, in step S205, the revisiting region is identified based on the global descriptor of the point cloud, and spatial constraints are added to correct the accumulated drift; Furthermore, in step S206, the optimized global pose is utilized. X * All lidar point clouds are aligned to the {W} coordinate system, and voxel filtering and surface reconstruction are performed to generate a high-precision, highly consistent 3D dense map of farmland crop rows with complete geometric structure, including information on crop rows, field ridges, obstacles and other terrain features.

5. The method and system for agricultural machinery vehicle localization and mapping based on multi-sensor fusion according to claim 1, characterized in that: In step S207, the parameters of the crop rows are extracted from the 3D crop row morphology, and the following steps are performed: S301: Use radius filtering to remove noisy point cloud clusters; use the RANSAC rule to fit the ground plane and separate the ground and crop point clouds: normalized normal vector formed by three non-collinear points. If the following inequalities are satisfied, then p i The point is an interior point: （20） Process all points in the point cloud. If the number of inliers in the current model is greater than the number of inliers in the optimal model, update the optimal model. The optimal model can be obtained through iterative calculation. Refit the ground plane with all inliers. Set a distance threshold, traverse the remaining points, and group points whose distance to each other is less than the threshold into the same cluster. In the cluster, point clouds with a height exceeding 20% ​​of the crop's set height are classified as obstacle point clouds. The remaining 3D point clouds are projected onto a new 2D plane, creating 5mm*5mm grid cells. These cells are accumulated along the vertical Z-axis to create a height density map, generating a 2D grid map. Each cell stores features such as point cloud density and average height. A height density curve is generated along the X-axis and smoothed using a Gaussian filter. Under row spacing constraints, a peak detection algorithm is used to obtain a series of peaks. These constitute a group of centerline points on a 2D plane; S302: Cluster the centerline point group data and apply RANSAC iterative clustering in the remote area to detect the main lines, i.e. potential field ridges; When generating a straight line hypothesis by randomly sampling two points, if the line connecting the two points is too steep, the sampling is abandoned and resampled to increase the probability of sampling an approximate horizontal line; if a model M1 with enough interior points is successfully found, the angle of M1 is checked. If the angle is close to horizontal, it is initially marked as a "candidate field ridge line", and its interior point set is: B candidate ; Remove these interior points from the original graph to obtain the remaining point set; S303: In the remaining point set, from near to far, apply the RANSAC rule. The model selects either a straight line or an arc based on prior knowledge, resulting in a new row model. M row and its interior point set inliers row ;if M row If the angle is close to vertical, that is, consistent with the expected direction of the crop row, and there are enough inner points, then this is considered a crop row; after completing one round of search, these inner points are removed from the remaining point set; Continue clustering the remaining point set using the RANSAC rule to obtain a new row model. M row and its interior point set inliers row Remove interior points; repeat the process to find new models and interior points until the remaining points are less than a threshold. S304: Fit an initial row model to each set of interior points one by one. L i A straight line or an arc; throughout the entire point group P In the search for distance models L i Points within a certain threshold are added. S i ; S305: Refit a more accurate model using the enlarged Si. L inew Then search for nearby points again, repeating this process until no new points are added; S306: If B candidate and L inew A continuous line that spans the width of multiple rows of crops, and has clear intersections or endpoints with each row of crops at its far end, is ultimately identified as a set of field ridge points. B ; if B candidate If the ridges are weak or nonexistent, it may be due to the absence of clear field ridges or the natural end of crop rows. It is advisable to make a judgment when agricultural machinery vehicles are closer.

Images