Long-range farmland map construction method and system based on multi-sensor fusion

Abstract

A long-range farmland map construction method and system based on multi-sensor fusion, comprising: a robot continuously acquires positioning data through a GPS module, and in the time period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain a state variable containing the real-time position and heading of the robot; the robot scans the external environment information in real time through its laser radar and converts it into current point cloud data, and at the same time, the robot scans the RGB information of the external environment in real time through a visual sensor, compares the RGB information with the historical RGB information collected in history, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to a mapping module of the laser radar to obtain enhanced point cloud data under the current state variable; after the robot completes the preset route in the farmland map construction task, the point cloud data under each state variable are fused to obtain a point cloud map as the execution result of the farmland map construction task.

Description

Technical Field

[0001] The present invention relates to the fields of multi-sensor fusion, real-time positioning, mapping and computer vision technology, and in particular to a long-range farmland map construction method and system based on multi-sensor fusion. Background Art

[0002] In the entire agricultural production chain, the management of agricultural production in the middle and late stages occupies an important position. In the process of operation management, the traditional management model requires a lot of labor and is time-consuming, which greatly restricts the development of agriculture. On the other hand, due to the large amount of labor invested in the management of the late stage of agriculture, it is time-consuming and has low management efficiency, which also increases the cost of agricultural production. Therefore, the use of unmanned agricultural machinery (abbreviated as: intelligent agricultural machinery) for agricultural operation management has become a hot research topic. However, in the operation of intelligent agricultural machinery, environmental perception technology is of paramount importance.

[0003] The environment in which intelligent agricultural machinery travels is perceived through the Simultaneous Localization and Mapping (SLAM) technology. SLAM is a real-time positioning and mapping technology. The main function of this technology is to perceive the external environment through sensors and collect environmental information to generate a full-scene map, which is a technology for intelligent agricultural machinery to achieve self-positioning in an unknown environment. SLAM technology was first proposed by Smith, Self and Cheeseman. Because this technology has important theoretical and practical value, it has been applied to autonomous navigation technology by many scholars. At present, SLAM technology has become a key technology for the control and development of intelligent agricultural machinery. The laser radar sensor with SLAM technology as the core is one of the important sensors for autonomous navigation of intelligent agricultural machinery. Laser radar is less affected by climate factors such as light and wind in outdoor field environments, and laser radar also has the advantages of 360-degree all-round perception. Lidar sensors have a better ability to perceive the surrounding environment in actual farmland environments, which is of great research significance for improving the walking accuracy of autonomous navigation.

[0004] The most basic technical requirement for unmanned driving of intelligent agricultural machinery in agricultural scenarios is: the construction of full-scene maps. The accuracy of full-scene map construction will directly affect the accuracy of the path driving of intelligent agricultural machinery in the later stage. In addition, the positioning accuracy of unmanned driving in farmland scenarios is required to be centimeter-level accuracy. However, during the long-distance mapping process, the external environmental information perceived by the lidar is real-time and changeable. During the mapping process, the SLAM technology of a single lidar sensor is based on distance for loop detection, lacking visual information perceived by the external environment, and the loop detection efficiency is low, resulting in a significant reduction in mapping accuracy. Therefore, it is difficult to meet actual application needs by relying solely on a single lidar sensor for mapping. Second, the mapping process requires that the positioning signal is continuous and the positioning frequency reaches more than 10Hz. Traditional GPS positioning is difficult to meet actual needs. Summary of the invention

[0005] The purpose of the present invention is to solve (overcome) the problem of low accuracy in long-range mapping in the above-mentioned prior art, and propose a method for constructing a long-range accurate full-scene map based on multi-sensor fusion. It includes the following key points 1, visual sensor and lidar sensor fusion technology: Since the loop detection model based on the lidar sensor is based on distance judgment, the lack of visual information perceived by the external environment will lead to low loop detection efficiency. Therefore, on the basis of SLAM mapping with a single lidar sensor, a visual sensor is introduced to scan the RGB information of the external environment in real time, and through the visual word bag loop detection technology, the detected information is fed back to the SLAM module of the lidar in real time, so as to further compensate and optimize the perceived environment map in real time; Technical effect: In the long-range mapping process, the compensation technology for loop detection by the visual sensor proposed in the present invention significantly improves the mapping accuracy, especially in the turning and edge parts of the map.

[0006] Key point 2, IMU and GPS fusion technology: Before obtaining new GPS data, the previous frame of GPS data and the current IMU data will be input into the Kalman filter to instantly predict the driving position of the mapping robot. Until new GPS data is received, the position prediction will continue based on the current GPS data; Technical effect: When the GPS signal is temporarily lost or the GPS positioning frequency suddenly decreases, the algorithm proposed by the present invention will compensate the GPS data in real time to meet the mapping accuracy requirements.

[0007] In view of the shortcomings of the prior art, the present invention proposes a long-range farmland map construction method based on multi-sensor fusion, which includes:

[0008] Step 1: After starting the farmland map construction task, the robot continuously obtains positioning data through its GPS module, and in the period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain the state variables containing the real-time position and heading of the robot;

[0009] Step 2: The robot uses its laser radar to scan the external environment information in real time and converts it into current point cloud data. At the same time, the robot uses the visual sensor to scan the RGB information of the external environment in real time, compares the RGB information with the historical RGB information collected in the past, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to the laser radar mapping module to obtain the enhanced point cloud data under the current state variable;

[0010] Step 3: Repeat steps 1 and 2 until the robot completes the preset route in the farmland map construction task, fuses the current point cloud data or enhanced point cloud data under each state variable to obtain a point cloud map, and converts it into a two-dimensional map as the execution result of the farmland map construction task.

[0011] The method for constructing a long-range farmland map based on multi-sensor fusion, wherein step 1 comprises:

[0012] Step 11: The Kalman filter in the robot selects the state variable as the vehicle posture X i =(x i y i ψ i ); let t k The estimated state x at time i Affected by system noise G i-1 Drive, expressed in equation form:

[0013] X i =A i，i-1 X i-1 +W i-1 G i-1 (1)

[0014] In the formula, A i，i-1 t i-1 to i The one-step transfer matrix at time, W i-1 The matrix is ​​driven by system noise;

[0015] In the GPS positioning system, X i The measurement satisfies the linear relationship, and the measurement equation is:

[0016] Z i =H i X i +V i (2)

[0017] In the formula, H i is the measurement matrix, V i is the measurement noise sequence, Z i is the positioning data at the i-th moment;

[0018] Step 12: Set Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, both of which are non-negative definite matrices; when the Kalman filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix:

[0019]

[0020] Where ed is the distance estimation noise, ey ​​is the heading estimation noise; when the inertial sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for:

[0021]

[0022] Where er is the wheel speed noise, ei is the angular velocity noise;

[0023] Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 :

[0024]

[0025] The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when the difference between the previous and next positions is small, that is, w*dt≤0.0001:

[0026]

[0027] When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001:

[0028]

[0029] Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period:

[0030] td=v*dt (8)

[0031] tr=w*dt (9)

[0032] Then the one-step prediction mean square error matrix is ​​calculated by

[0033]

[0034] When the GPS positioning data is updated, the GPS positioning result is merged with the current dead reckoning result;

[0035] First, the estimated noise parameters are obtained based on the current GPS working state, and the system measurement noise variance matrix R is calculated by formula (3): i ; At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment;

[0036] Calculate the filter gain K:

[0037]

[0038]

[0039] The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning.

[0040]

[0041]

[0042] Finally, update the mean square error matrix P, where I is the 3*3 identity matrix:

[0043]

[0044] Combining equations (1) to (16), the robot's state variable X is finally output i .

[0045] The method for constructing a long-range farmland map based on multi-sensor fusion, wherein step 2 comprises:

[0046] Step 21: Project the received point cloud data to the distance image. Each point in the distance image represents a pixel of the image. The pixel value d i Target point D i Euclidean distance to the robot lidar; apply image-based segmentation methods to the range image to group points into multiple clusters, record the class label of the point in the range image, the coordinates (x, y) in the range image, and the value d of the point i ;

[0047] Step 22: Divide the distance image into multiple sub-images, sort the points in each row of the sub-image based on the smoothness c of the sub-image, and set the threshold C th Determine whether each point is a plane point or a corner point;

[0048] Step 23, the lidar odometer module finds the transition between the two scans by performing plane point and corner point scan matching;

[0049] Step 24: Align the feature points with the surrounding point cloud to further optimize the pose transformation, but at a lower frequency; LeGO-LOAM chooses to store each feature point set instead of a single point cloud.

[0050] The method for constructing a long-range farmland map based on multi-sensor fusion, wherein step 2 comprises:

[0051] The current RGB information is converted into a bag-of-words vector BRIEF descriptor B(p) by the following formula, and then the descriptor is mapped to the corresponding point cloud data through the timestamp as the bag-of-words vector of the current point cloud data;

[0052]

[0053] In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The 2D offset of the i-th test point relative to the patch center;

[0054] Convert the descriptor B(p) to the bag-of-words space to form a bag-of-words vector V that describes the current visual image t , and measure the similarity score s(v1, v2) between two bag-of-words vectors in the bag-of-words space, where

[0055]

[0056] Normalize these scores into a vector V t The best score expected in this sequence is the normalized similarity score η:

[0057]

[0058] In the formula, v t-Δt The bag-of-words vector representing the previous image;

[0059] If η is greater than the threshold, the historical point cloud data corresponding to the historical RGB information with a similarity greater than the threshold and the current point cloud data are fed back to the lidar mapping module.

[0060] The present invention also proposes a long-range farmland map construction system based on multi-sensor fusion, which includes:

[0061] The initial module is used to start the farmland map construction task. The robot continuously obtains positioning data through its GPS module, and in the period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain the state variables containing the real-time position and heading of the robot;

[0062] The scanning module is used for the robot to scan the external environment information in real time through its laser radar and convert it into current point cloud data. At the same time, the robot scans the RGB information of the external environment in real time through the visual sensor, compares the RGB information with the historical RGB information collected in the past, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to the laser radar mapping module to obtain the enhanced point cloud data under the current state variable;

[0063] The mapping module is used to fuse the current point cloud data or enhanced point cloud data under each state variable to obtain a point cloud map after the robot completes the preset route in the farmland map construction task, and convert it into a two-dimensional map as the execution result of the farmland map construction task.

[0064] The long-range farmland map construction system based on multi-sensor fusion, wherein the initial module includes:

[0065] Module 11 is used to call the Kalman filter in the robot to select the state variable as the vehicle posture X i =(x i y i ω i ); let t k The estimated state x at time i Affected by system noise G i-1 Drive, expressed in equation form:

[0066] X i =A i，i-1 X i-1 +W i-1 G i-1 (1)

[0067] In the formula, A i，i-1 t i-1 to i The one-step transfer matrix at time, W i-1 The matrix is ​​driven by system noise;

[0068] In the GPS positioning system, X i The measurement satisfies the linear relationship, and the measurement equation is:

[0069] Z i =H i X i +V i (2)

[0070] In the formula, H i is the measurement matrix, V i is the measurement noise sequence, Z i is the positioning data at the i-th moment;

[0071] Module 12, for setting Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, both of which are non-negative definite matrices; when the Kalman filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix:

[0072]

[0073] Where ed is the distance estimation noise, ey ​​is the heading estimation noise; when the inertial sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for:

[0074]

[0075] Where er is the wheel speed noise, ei is the angular velocity noise;

[0076] Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 :

[0077]

[0078] The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when the difference between the previous and next positions is small, that is, w*dt≤0.0001:

[0079]

[0080] When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001:

[0081]

[0082] Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period:

[0083] td=v*dt (8)

[0084] tr=w*dt (9)

[0085] Then the one-step prediction mean square error matrix is ​​calculated by

[0086]

[0087] When the GPS positioning data is updated, the GPS positioning result is merged with the current dead reckoning result;

[0088] First, the estimated noise parameters are obtained based on the current GPS working state, and the system measurement noise variance matrix R is calculated by formula (3): i ; At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment;

[0089] Calculate the filter gain K:

[0090]

[0091]

[0092] The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning.

[0093]

[0094]

[0095] Finally, update the mean square error matrix P, where I is the 3*3 identity matrix:

[0096]

[0097] Combining equations (1) to (16), the robot's state variable X is finally output i .

[0098] The long-range farmland map construction system based on multi-sensor fusion, wherein the scanning module includes:

[0099] Module 21 is used to project a frame of received point cloud data into a distance image. Each point in the distance image represents a pixel of the image. The value of the pixel d i Target point D iEuclidean distance to the robot lidar; apply image-based segmentation methods to the range image to group points into multiple clusters, record the class label of the point in the range image, the coordinates (x, y) in the range image, and the value d of the point i ;

[0100] Module 22 is used to divide the distance image into multiple sub-images, sort the points in each row of the sub-images based on the smoothness c of the sub-images, and sort the points by setting a threshold C th Determine whether each point is a plane point or a corner point;

[0101] Module 23, configured to enable the laser radar odometer module to find the transition between two scans by performing plane point and corner point scan matching;

[0102] Module 24 is used to align feature points with the surrounding point cloud to further optimize the pose transformation, but runs at a lower frequency; LeGO-LOAM chooses to store each feature point set instead of a single point cloud.

[0103] The long-range farmland map construction method based on multi-sensor fusion, wherein the scanning module is used to convert the current RGB information into a bag-of-words vector BRIEF descriptor B(p) through the following formula, and then the descriptor is mapped to the corresponding point cloud data through the timestamp as the bag-of-words vector of the current point cloud data;

[0104]

[0105] In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The 2D offset of the i-th test point relative to the patch center;

[0106] Convert the descriptor B(p) to the bag-of-words space to form a bag-of-words vector V that describes the current visual image t , and measure the similarity score s(v1, v2) between two bag-of-words vectors in the bag-of-words space, where

[0107]

[0108] Normalize these scores into a vector V t The best score expected in this sequence is the normalized similarity score η:

[0109]

[0110] In the formula, v t-Δt The bag-of-words vector representing the previous image;

[0111] If η is greater than the threshold, the historical point cloud data corresponding to the historical RGB information with a similarity greater than the threshold and the current point cloud data are fed back to the lidar mapping module.

[0112] The present invention also proposes a storage medium for storing a program for executing any of the above-mentioned long-range farmland map construction methods based on multi-sensor fusion.

[0113] The present invention also proposes a client, which is used for any of the above-mentioned long-range farmland map construction systems based on multi-sensor fusion.

[0114] It can be seen from the above scheme that the advantages of the present invention are:

[0115] This invention realizes instant positioning and high-precision mapping through multi-sensor fusion technology in the long-range mapping module. On the basis of meeting the positioning frequency and accuracy, by introducing a visual sensor to scan the RGB information of the external environment in real time, and through visual word bag loop detection, the detected information is fed back to the SLAM mapping technology of the lidar in real time, so as to further compensate and optimize the perceived environmental map, so as to achieve the purpose of high precision. The long-range mapping accuracy proposed by the present invention has been comprehensively improved compared with the mapping accuracy of single sensor mapping, especially in the turning and edge parts of the map. BRIEF DESCRIPTION OF THE DRAWINGS

[0116] Figure 1 Actual photos of the carrier for mapping hardware integration;

[0117] Figure 2 Figure 1. The main improved model diagram optimized for loop closure detection.

[0118] Figure 3 This is the main improved model diagram based on IMU and Kalman filter optimization;

[0119] Figure 4 It is the overall module flow chart;

[0120] Figure 5a Traditional existing technology renderings;

[0121] Figure 5b This is a technical effect diagram of the improved present invention. DETAILED DESCRIPTION

[0122] Based on the above problems, the present invention optimizes the full-scene mapping process through two improvement measures to meet the actual application requirements:

[0123] In the process of long-distance mapping, due to the real-time and changing external environment information, the SLAM loop detection technology of a single sensor is inefficient, resulting in low mapping accuracy. The present invention proposes a long-distance loop detection technology, which introduces a visual sensor to scan the RGB information of the external environment in real time, and feeds back the detected information to the SLAM mapping module of the laser radar in real time through the visual word bag loop detection (Bags of Binary Words) technology, thereby further compensating and optimizing the perceived environment map in real time, greatly reducing the mapping error.

[0124] Since the Inertial Measurement Unit (IMU) can provide linear velocity and angular velocity information of an object moving in a short period of time and the transmission frequency of the IMU can reach 100 times the transmission frequency of GPS, the IMU module is integrated on the basis of GPS positioning. When the GPS signal is temporarily lost or the GPS positioning frequency is reduced, the IMU feedback data is used to predict the agricultural machinery posture and map position through Kalman filtering, thus ensuring the positioning requirements for mapping.

[0125] After studying the full-scene SLAM mapping technology, the inventors found that the above defects can be solved by (1) introducing a visual sensor to scan the RGB information of the external environment in real time, and using the visual word bag loop detection technology to feed back the detected information to the SLAM mapping module of the laser radar in real time, thereby further compensating and optimizing the perceived environment map in real time. (2) By using an IMU sensor and a Kalman filter to perform positioning prediction between two frames of data sent by the GPS, the influence of the temporary loss of the GPS signal or the sudden decrease in the GPS positioning frequency on the mapping accuracy is completely avoided. The method of the present invention can optimize and update the map in real time through multi-sensor fusion during the mapping process, so that the mapping accuracy is greatly improved. In order to make the above-mentioned features and effects of the present invention more clear and easy to understand, the following is a special embodiment, and a detailed description is given in conjunction with the drawings in the specification as follows.

[0126] The hardware integration carrier of the method proposed in the present invention is as follows Figure 1 shown.

[0127] The specific operation flow chart of the method proposed in the present invention is as follows Figure 2 , 3 shown.

[0128] The overall algorithm of the present invention is as follows Figure 4 The specific implementation is as follows:

[0129] 1) Systematically integrate the hardware and initialize the similarity function value (selected according to the actual application mapping accuracy requirements);

[0130] 2) The GPS and IMU sensors are integrated through the Kalman filter to form a multi-sensor fusion positioning module, which specifically includes:

[0131] 2.1) GPS measurement equation: The Kalman filter in the system selects the state variable as the vehicle posture X i =(x i y i ψ i ). Let t k The estimated state x at time i Affected by system noise G i-1 Drive, expressed in equation form:

[0132] X i =A i，i-1 X i-1 +W i-1 G i-1 (1)

[0133] In the formula, A i，i-1 t i-1 to i The one-step transfer matrix at time (derived from Formula 5), ​​W i-1 is the system noise driving matrix (derived in Equation 6).

[0134] In the GPS positioning system, X i The measurement satisfies the linear relationship. The measurement equation of the above formula 1 satisfies the linear form as follows:

[0135] Z i =H i X i +V i (2)

[0136] In the formula, H i is the measurement matrix, V i is the measurement noise sequence.

[0137] 2.2) Kalman filter prediction process: Let Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, which is a non-negative definite matrix in the present invention. When the filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix

[0138]

[0139] Where ed is the distance estimation noise and ey is the heading estimation noise. When the IMU sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for:

[0140]

[0141] Where er is the wheel speed noise and ei is the angular velocity noise.

[0142] Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 :

[0143]

[0144] The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when the difference between the previous and next positions is small, that is, w*dt≤0.0001:

[0145]

[0146] When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001:

[0147]

[0148] Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period:

[0149] td=v*dt (8)

[0150] tr=w*dt (9)

[0151] Then the one-step prediction mean square error matrix is ​​calculated by formula (10):

[0152]

[0153] When the GPS positioning data is updated, the GPS positioning result is fused with the current dead reckoning result.

[0154] First, the estimated noise parameters are obtained based on the current GPS operating state, and the system measurement noise variance matrix R is calculated by formula (3): i At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment.

[0155] Calculate the filter gain K:

[0156]

[0157]

[0158] The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning.

[0159]

[0160]

[0161] Finally, update the mean square error matrix P, where I is the 3*3 identity matrix:

[0162]

[0163] Combining equations (1) to (16), the carrier state variable X is finally output by equation 15: i , that is, the position and heading of the vehicle.

[0164] 3) Based on the multi-sensor fusion positioning module, the laser radar scans the external environment information in real time and converts it into point cloud data;

[0165] 4) Dynamically stitch the point cloud data through the Lego-Loam module, and perform real-time loop detection on the point cloud data based on distance, including

[0166] 4.1) Point cloud segmentation module

[0167] First, the received point cloud is projected onto a 1800*16 image (horizontal accuracy 0.2°, 16-line lidar). Each received point represents a pixel of the image, and the pixel value d i The corresponding point D i The Euclidean distance to the sensor.

[0168] After the image is completed, a matrix is ​​obtained, and each column of the matrix is ​​estimated (that is, the ground is estimated), and the ground points are extracted before segmentation. Points that may represent the ground are marked as ground points and are not used for segmentation.

[0169] Then, an image-based segmentation method is applied to the range image to group the points into multiple clusters. Points of the same class have the same label (the ground is a special class); at the same time, in order to complete feature extraction quickly and reliably, classes with less than 30 points are ignored.

[0170] After completing this step, only the points that may represent large objects are retained in the image, and then three values ​​of the points are recorded: (1) the class label; (2) the coordinates of the point in the matrix (x, y); (3) the value of the point, i.e., d i Used in the following modules.

[0171] 4.2) Feature Extraction Module

[0172] The first step is to extract feature points. In order to extract features evenly from all directions, the image is horizontally divided into several equal sub-images. Then we sort the points in each row of the sub-image based on their smoothness c. Then we use the set threshold C th To determine plane points or corner points.

[0173] 4.3) LiDAR odometer module

[0174] The LiDAR odometry module estimates the sensor motion between two consecutive scans by performing point-edge and point-plane scan matching to find the transition between the two scans.

[0175] The lidar odometry module estimates the sensor motion between two consecutive scans and uses the pose output in formula (15) as the initialization state of the odometry. It then calculates the transformation between the two scans by performing point-edge and point-plane scan matching based on the initialization state. This transformation information is used for the preliminary registration of the radar point cloud in the point cloud registration module.

[0176] 4.4) Point cloud registration module

[0177] In the point cloud registration module, the transformation between the two scans output from the radar odometry is first used to preliminarily align the feature points with the surrounding point cloud map, and then the posture transformation is further optimized, but it runs at a lower frequency; LeGO-LOAM chooses to store each feature point set (including plane point set or corner point set) instead of a single point cloud map.

[0178] Assume M t-1 is a collection of all previous feature point sets. When scanning, M t-1 Each feature set in M ​​is also associated with the sensor’s pose. 1-1 To get the surrounding point cloud map The first method is to obtain a set of feature points in the sensor field of view. For simplicity, we select feature points whose sensor poses are within 100 meters of the sensor's current position. We then transform and fuse the selected feature points into a single surrounding map, resulting in

[0179] The sensor pose of each feature point set can be modeled as a node in the pose graph. The feature point set can be regarded as the sensor measurement value of this node. Since the pose estimation drift of the point cloud registration module is very low, it is assumed that there is no drift in a short time. In this way, the nearest set of feature points can be selected to form Then, the transformation obtained after LM optimization can be used to add new nodes with The spatial constraints between the selected nodes in . Drift of this module can be further eliminated by performing loop closure detection. In this case, new constraints are added if a match is found between the current set of feature points and the previous set of feature points using ICP. The estimated pose of the sensor is then updated by sending the pose graph to the optimization system.

[0180] 5) The loop detection mechanism in the Lego-Loam algorithm is compensated by a loop detection model based on a visual sensor, as follows:

[0181] 5.1) Each frame of visual data is converted into a bag-of-words vector BRIEF descriptor B(p) by formula (17), and then the descriptor is mapped to the corresponding point cloud data through the timestamp as the bag-of-words vector of the point cloud data. For each feature point p in the visual data, its descriptor is calculated according to the following formula:

[0182]

[0183] In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The two-dimensional offset of the ith test point relative to the center of the patch (the center of the pixel range considered), L b The range of pixels considered when computing the descriptor.

[0184] 5.2) The descriptor B(p) is converted to the bag-of-words space to form a bag-of-words vector describing a frame of visual image. In order to measure the similarity between two bag-of-words vectors v1 and v2, the present invention calculates a score s(v1, v2), which is between [0, 1].

[0185]

[0186] 5.3) The present invention uses an image database to store and retrieve similar images.T , convert it into a bag-of-words vector V t , and then stored in the image database. When a new image is acquired and converted into a bag-of-words vector, it can be determined whether it is similar to the previous image by accessing and comparing the image database. From formula (18), the range of these scores depends largely on the query image and the distribution of words it contains. Then, the present invention normalizes these scores into vector V t The best score expected in this sequence is obtained by normalizing the similarity score η

[0187]

[0188] In the formula, v t-Δt Bag-of-words vector representing the previous image.

[0189] By solving η, if it is greater than the set value, the number of previous point cloud data frames similar to the current frame will be returned to the lidar odometry module, so as to further correct and optimize the map and improve the accuracy of map construction. The principle is that after similar scenes are detected through visual images, the current scene and similar scenes can be optimized again in the corresponding module to improve the accuracy of point cloud map construction. The reason for improving accuracy in this way is that the laser point cloud lacks color information, and it is impossible to judge whether a similar scene has been reached through the point cloud, so it is impossible to make timely optimization when reaching a similar scene.

[0190] That is, the present invention will input the current point cloud and the previous similar point cloud into the laser radar odometer module at the same time, and the previous similar point cloud part can be multiple point clouds. By inputting at least two point cloud images into the odometer module, the map construction accuracy can be improved. How to coordinate the two parts with each other to optimize the final result can adopt any existing technology. It is not the core invention point of the present invention. The core invention point of the present invention lies in how to select historical point clouds similar to the current point cloud.

[0191] 6) Convert the 3D point cloud map into a 2D map through the Octomap module;

[0192] 7) The 2D map is saved through the Mapsever module to form the long-range grid map required for practical applications.

[0193] FIG5 shows the measured results of the present invention. FIG5(a) shows the experimental results when a traditional single lidar sensor is used for mapping, and FIG5(b) shows the mapping results of the multi-sensor fusion long-range accurate full-scene map construction method proposed by the present invention. It can be seen from FIG5 that the long-range mapping accuracy proposed by the present invention has been comprehensively improved compared with the mapping accuracy of a single sensor, especially in the turning and edge parts of the map.

[0194] The following is a system embodiment corresponding to the above method embodiment. This embodiment can be implemented in conjunction with the above embodiment. The relevant technical details mentioned in the above embodiment are still valid in this embodiment. In order to reduce repetition, they are not repeated here. Accordingly, the relevant technical details mentioned in this embodiment can also be applied in the above embodiment.

[0195] The present invention also proposes a long-range farmland map construction system based on multi-sensor fusion, which includes:

[0196] The initial module is used to start the farmland map construction task. The robot continuously obtains positioning data through its GPS module, and in the period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain the state variables containing the real-time position and heading of the robot;

[0197] The scanning module is used for the robot to scan the external environment information in real time through its laser radar and convert it into current point cloud data. At the same time, the robot scans the RGB information of the external environment in real time through the visual sensor, compares the RGB information with the historical RGB information collected in the past, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to the laser radar mapping module to obtain the enhanced point cloud data under the current state variable;

[0198] The mapping module is used to fuse the current point cloud data or enhanced point cloud data under each state variable to obtain a point cloud map after the robot completes the preset route in the farmland map construction task, and convert it into a two-dimensional map as the execution result of the farmland map construction task.

[0199] The long-range farmland map construction system based on multi-sensor fusion, wherein the initial module includes:

[0200] Module 11 is used to call the Kalman filter in the robot to select the state variable as the vehicle posture X i =(x i y i ψ i ); let t k The estimated state x at time i Affected by system noise G i-1 Drive, expressed in equation form:

[0201] X i =A i，i-1 X i-1 +W i-1 G i-1 (1)

[0202] In the formula, A i，i-1 ti-1 to i The one-step transfer matrix at time, W i-1 The matrix is ​​driven by system noise;

[0203] In the GPS positioning system, X i The measurement satisfies the linear relationship, and the measurement equation is:

[0204] Z i =H i X i +V i (2)

[0205] In the formula, H i is the measurement matrix, V i is the measurement noise sequence, Z i is the positioning data at the i-th moment;

[0206] Module 12, for setting Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, both of which are non-negative definite matrices; when the Kalman filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix:

[0207]

[0208] Where ed is the distance estimation noise, ey ​​is the heading estimation noise; when the inertial sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for:

[0209]

[0210] Where er is the wheel speed noise, ei is the angular velocity noise;

[0211] Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 :

[0212]

[0213] The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when the difference between the previous and next positions is small, that is, w*dt≤0.0001:

[0214]

[0215] When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001:

[0216]

[0217] Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period:

[0218] td=v*dt (8)

[0219] tr=w*dt (9)

[0220] Then the one-step prediction mean square error matrix is ​​calculated by

[0221]

[0222] When the GPS positioning data is updated, the GPS positioning result is merged with the current dead reckoning result;

[0223] First, the estimated noise parameters are obtained based on the current GPS operating state, and the system measurement noise variance matrix R is calculated by formula (3): i ; At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment;

[0224] Calculate the filter gain K:

[0225]

[0226]

[0227] The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning.

[0228]

[0229]

[0230] Finally, update the mean square error matrix P, where I is the 3*3 identity matrix:

[0231]

[0232] Combining equations (1) to (16), the robot's state variable X is finally output i .

[0233] The long-range farmland map construction system based on multi-sensor fusion, wherein the scanning module includes:

[0234] Module 21 is used to project a frame of received point cloud data into a distance image. Each point in the distance image represents a pixel of the image. The value of the pixel d i Target point D i Euclidean distance to the robot lidar; apply image-based segmentation methods to the range image to group points into multiple clusters, record the class label of the point in the range image, the coordinates (x, y) in the range image, and the value d of the point i ;

[0235] Module 22 is used to divide the distance image into multiple sub-images, sort the points in each row of the sub-images based on the smoothness c of the sub-images, and sort the points by setting a threshold C th Determine whether each point is a plane point or a corner point;

[0236] Module 23, configured to enable the laser radar odometer module to find the transition between two scans by performing plane point and corner point scan matching;

[0237] Module 24 is used to align feature points with the surrounding point cloud to further optimize the pose transformation, but runs at a lower frequency; LeGO-LOAM chooses to store each feature point set instead of a single point cloud.

[0238] The long-range farmland map construction method based on multi-sensor fusion, wherein the scanning module is used to convert the current RGB information into a bag-of-words vector BRIEF descriptor B(p) through the following formula, and then the descriptor is mapped to the corresponding point cloud data through the timestamp as the bag-of-words vector of the current point cloud data;

[0239]

[0240] In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The 2D offset of the i-th test point relative to the patch center;

[0241] Convert the descriptor B(p) to the bag-of-words space to form a bag-of-words vector V that describes the current visual image t , and measure the similarity score s(v1, v2) between two bag-of-words vectors in the bag-of-words space, where

[0242]

[0243] Normalize these scores into a vector V tThe best score expected in this sequence is the normalized similarity score η:

[0244]

[0245] In the formula, v t-Δt The bag-of-words vector representing the previous image;

[0246] If η is greater than the threshold, the historical point cloud data corresponding to the historical RGB information with a similarity greater than the threshold and the current point cloud data are fed back to the lidar mapping module.

[0247] The present invention also proposes a storage medium for storing a program for executing any of the above-mentioned long-range farmland map construction methods based on multi-sensor fusion.

[0248] The present invention also proposes a client, which is used for any of the above-mentioned long-range farmland map construction systems based on multi-sensor fusion.

Claims

1. A long-range farmland map construction method based on multi-sensor fusion, characterized in that: include: Step 1: After starting the farmland map construction task, the robot continuously obtains positioning data through its GPS module, and in the period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain the state variables containing the real-time position and heading of the robot; Step 2: The robot uses its laser radar to scan the external environment information in real time and converts it into current point cloud data. At the same time, the robot uses the visual sensor to scan the RGB information of the external environment in real time, compares the RGB information with the historical RGB information collected in the past, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to the laser radar mapping module to obtain the enhanced point cloud data under the current state variable; Step 3, repeating steps 1 and 2 until the robot completes the preset route in the farmland map construction task, fusing the current point cloud data or enhanced point cloud data under each state variable to obtain a point cloud map, and converting it into a two-dimensional map as the execution result of the farmland map construction task; The step 2 includes: Step 21: Project the received point cloud data to the distance image. Each point in the distance image represents a pixel of the image. The pixel value d i Represents the corresponding point D i Euclidean distance to the robot lidar; apply image-based segmentation methods to the range image to group points into multiple clusters, record the class label of the point in the range image, the coordinates in the range image, and the value d of the point i ; Step 22: Divide the distance image into multiple sub-images, sort the points in each row of the sub-image based on the smoothness c of the sub-image, and set the threshold C th Determine whether each point is a plane point or a corner point; Step 23, the lidar odometer module finds the transition between the two scans by performing plane point and corner point scan matching; Step 24: Align the feature points with the surrounding point cloud to further optimize the pose transformation, but run at a lower frequency; LeGO-LOAM chooses to store each feature point set instead of a single point cloud; This step 2 includes: The current RGB information is converted into a bag-of-words vector BRIEF descriptor B(p) by the following formula, and then the descriptor is mapped to the corresponding point cloud data through the timestamp as the bag-of-words vector of the current point cloud data; In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The 2D offset of the i-th test point relative to the patch center; Convert the descriptor B(p) to the bag-of-words space to form a bag-of-words vector V that describes the current visual image t , and measure the similarity score s(v1, v2) between two bag-of-words vectors in the bag-of-words space, where Normalize these scores to get the normalized similarity score η: In the formula, v t-Δt The bag-of-words vector representing the previous image; If η is greater than the threshold, the historical point cloud data corresponding to the historical RGB information with a similarity greater than the threshold and the current point cloud data are fed back to the lidar mapping module.

2. The long-range farmland map construction method based on multi-sensor fusion according to claim 1, characterized in that: This step 1 includes: Step 11: The Kalman filter in the robot selects the state variable as the vehicle posture X i =(x i y i ψ i ), x i Represents t i The x-coordinate and y-coordinate of the vehicle in the world coordinate system at this moment i represents the y coordinate, ψ i Represents t i The vehicle's heading at the moment; let t k The estimated state X at time i Affected by system noise G i-1 Drive, expressed in equation form: X i =A i，i-1 X i-1 +W i-1 G i-1 (1) In the formula, A i，i-1 t i-1 to i The one-step transfer matrix at time, W i-1 is the system noise driving matrix; In the GPS positioning system, X i The measurement satisfies the linear relationship, and the measurement equation is: Z i =H i X i +V i (2) In the formula, H i is the measurement matrix, V i is the measurement noise sequence, Z i is the positioning data at the i-th moment; Step 12: Set Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, both of which are non-negative definite matrices; when the Kalman filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix: Where ed is the distance estimation noise, ey ​​is the heading estimation noise; when the inertial sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for: Where er is the wheel speed noise, ei is the angular velocity noise, and dt represents the unit time; Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 : The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when w*dt≤0.0001: When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001: Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period: td=v*dt (8) tr=w*dt (9) Then the one-step prediction mean square error matrix is ​​calculated by When the GPS positioning data is updated, the GPS positioning result is merged with the current dead reckoning result; First, the estimated noise parameters are obtained based on the current GPS operating state, and the system measurement noise variance matrix R is calculated by formula (3): i ; At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment; Calculate the filter gain K: The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning; Finally, update the mean square error matrix P, where I is the 3*3 identity matrix: Combining equations (1) to (16), the robot's state variable X is finally output i .

3. A long-range farmland map construction system based on multi-sensor fusion, characterized in that: include: The initial module is used to start the farmland map construction task. The robot continuously obtains positioning data through its GPS module, and in the period between receiving two frames of positioning data, the robot uses its inertial sensor to update the positioning data to obtain the state variables containing the real-time position and heading of the robot; The scanning module is used for the robot to scan the external environment information in real time through its laser radar and convert it into current point cloud data. At the same time, the robot scans the RGB information of the external environment in real time through the visual sensor, compares the RGB information with the historical RGB information collected in the past, and feeds back the historical point cloud data corresponding to the historical RGB information with a similarity greater than a threshold and the current point cloud data to the laser radar mapping module to obtain the enhanced point cloud data under the current state variable; A mapping module is used to fuse the current point cloud data or enhanced point cloud data under various state variables to obtain a point cloud map after the robot completes the preset route in the farmland map construction task, and convert it into a two-dimensional map as the execution result of the farmland map construction task; The scanning module includes: Module 21 is used to project a frame of received point cloud data into a distance image. Each point in the distance image represents a pixel of the image. The value of the pixel d i Represents the corresponding point D i Euclidean distance to the robot lidar; apply image-based segmentation methods to the range image to group points into multiple clusters, record the class label of the point in the range image, the coordinates in the range image, and the value d of the point i ; Module 22 is used to divide the distance image into multiple sub-images, sort the points in each row of the sub-images based on the smoothness c of the sub-images, and sort the points by setting a threshold C th Determine whether each point is a plane point or a corner point; Module 23, configured to enable the laser radar odometer module to find the transition between two scans by performing plane point and corner point scan matching; Module 24 is used to register feature points with surrounding point clouds to further optimize pose transformation, but runs at a lower frequency; LeGO-LOAM chooses to store each feature point set instead of a single point cloud; The scanning module is used to convert the current RGB information into a bag-of-words vector BRIEF descriptor B(p) through the following formula, and then correspond the descriptor to the corresponding point cloud data through the timestamp as the bag-of-words vector of the current point cloud data; In the formula, B i (p) is the i-th bit of the descriptor; I(.) is the intensity of the pixel in the smoothed image; a i and b i The 2D offset of the i-th test point relative to the patch center; Convert the descriptor B(p) to the bag-of-words space to form a bag-of-words vector V that describes the current visual image t , and measure the similarity score s(v1, v2) between two bag-of-words vectors in the bag-of-words space, where Normalize these scores to get the normalized similarity score η: In the formula, v t-Δt The bag-of-words vector representing the previous image; If η is greater than the threshold, the historical point cloud data corresponding to the historical RGB information with a similarity greater than the threshold and the current point cloud data are fed back to the lidar mapping module.

4. The long-range farmland map construction system based on multi-sensor fusion as claimed in claim 3, characterized in that: This initial module includes: Module 11 is used to call the Kalman filter in the robot to select the state variable as the vehicle posture X i =(x i y i ψ i ); let t k The estimated state X at time i Affected by system noise G i-1 Drive, expressed in equation form: X i =A i，i-1 X i-1 +W i-1 G i-1 (1) In the formula, A i，i-1 t i-1 to i The one-step transfer matrix at time, W i-1 is the system noise driving matrix; In the GPS positioning system, X i The measurement satisfies the linear relationship, and the measurement equation is: Z i =H i X i +V i (2) In the formula, H i is the measurement matrix, V i is the measurement noise sequence, Z i is the positioning data at the i-th moment; Module 12, for setting Q i is the variance matrix of the system noise sequence, R i is the variance matrix of the measured noise sequence, both of which are non-negative definite matrices; when the Kalman filter is initialized, the first frame of GPS position coordinates and heading are taken as the initial value of the filter state X o =(x o y o ψ o ), the initial estimated mean square error matrix: Where ed is the distance estimation noise, ey ​​is the heading estimation noise; when the inertial sensor velocity information and gyroscope angular velocity information are updated, the prediction solution is obtained by dead reckoning, and the system noise variance matrix Q i-1 for: Where er is the wheel speed noise, ei is the angular velocity noise; Calculate the one-step transfer matrix A from time i-1 to time i during the time update process i，i-1 : The system noise driving matrix W during the time update process is calculated by formulas (6) and (7): i-1 , when w*dt≤0.0001: When the rotation angle between the current and the next moment is large, that is, w*dt>0.0001: Where td is the distance the vehicle travels during the time period, and tr is the angle the vehicle turns during the time period: td=v*dt (8) tr=w*dt (9) Then the one-step prediction mean square error matrix is ​​calculated by When the GPS positioning data is updated, the GPS positioning result is merged with the current dead reckoning result; First, the estimated noise parameters are obtained based on the current GPS operating state, and the system measurement noise variance matrix R is calculated by formula (3): i ; At this time, the measured value X i =(x i y i ψ i ), which is the positioning coordinates and heading obtained by GPS at this moment; Calculate the filter gain K: The state estimate is calculated by equations (14) and (15), where is the prediction result obtained by dead reckoning; Finally, update the mean square error matrix P, where I is the 3*3 identity matrix: Combining equations (1) to (16), the robot's state variable X is finally output i .

5. A storage medium for storing a program for executing the long-range farmland map construction method based on multi-sensor fusion as described in claim 1 or 2.

6. A client, used for the long-range farmland map construction system based on multi-sensor fusion as described in claim 3 or 4.

Images