Laser radar mapping method based on loopback detection and vision-inertial odometer

Abstract

The invention discloses a laser radar mapping method based on loopback detection and a visual-inertial odometer. The visual-inertial odometer (VIO) and laser radar data are combined with loopback detection. Through comprehensive utilization of rich perception of VIO to the environment, high-precision distance measurement provided by the laser radar and a correction function of loopback detection, the method can significantly improve the accuracy and robustness of the autonomous navigation system. In a dynamic and uncertain environment, the method can effectively solve the technical problems of sensor error accumulation, environment change, positioning errors caused by dynamic obstacles and the like, and provides a more reliable and effective solution for navigation of an automatic driving vehicle and an advanced robot in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the field of three-dimensional reconstruction, and involves multi-sensor fusion, computer vision, key frame matching, global optimization, etc., and in particular integrates a laser radar (LIDAR) and a visual-inertial odometry (VIO) system, and specifically relates to a laser radar mapping method based on loop detection and visual-inertial odometry. Background Art

[0002] Accurate localization and mapping in dynamic and uncertain environments are critical for autonomous vehicles and advanced robots. Vision-based methods are particularly suitable for place recognition, but their sensitivity to initialization, lighting, and range make them unreliable when used alone to support autonomous navigation systems; methods based on laser radar (LIDAR) and inertial measurement units (IMU) are popular, but their reliance on initial pose estimates, rapid growth of accumulated errors, and lack of external environmental reference points make long-term autonomous navigation based solely on LIDAR and IMU unreliable. Visual-inertial odometry (VIO) uses cameras to estimate the motion of the device, but is limited by the camera's field of view and lighting conditions, while LiDAR provides high-precision distance measurements but lacks sufficient information for estimating the device's pose. To overcome these limitations, some systems have begun to combine VIO with LiDAR data to obtain more accurate and robust navigation results. Loop closure methods improve the accuracy of long-term navigation by effectively correcting accumulated localization errors. When the system recognizes that the current location matches a previously visited location, loop closure detection eliminates errors caused by sensor noise, environmental changes or dynamic obstacles by adjusting the previous trajectory estimate. It is particularly suitable for complex or large-scale environments, and can significantly improve the consistency of the map and the robustness of the overall navigation system. It enhances the system's adaptability to environmental changes, enabling it to maintain a high level of positioning accuracy when facing long-term operation and constantly changing scenarios.

[0003] In the existing technology, there are problems such as sensor error accumulation, environmental changes, and positioning errors caused by dynamic obstacles. These problems will affect the accuracy and robustness of the autonomous navigation system, and loop detection can help the system make effective corrections when facing these challenges, thereby improving the performance and reliability of the navigation system. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention provides a lidar mapping method based on loop closure detection and visual-inertial odometry, combining visual-inertial odometry (VIO) and lidar data with loop closure detection. By comprehensively utilizing the rich perception of the environment by VIO, the high-precision distance measurement provided by lidar, and the correction function of loop closure detection, our method can significantly improve the accuracy and robustness of the autonomous navigation system. In dynamic and uncertain environments, this method can effectively solve technical problems such as sensor error accumulation, environmental changes, and positioning errors caused by dynamic obstacles, providing a more reliable and effective solution for the navigation of autonomous vehicles and advanced robots in complex environments.

[0005] The present invention uses existing consumer-grade realsense cameras and livox radars, and takes point cloud images, depth images and inertial data as input to design a lidar mapping method based on loop detection and visual-inertial odometer. In a long-running three-dimensional mapping system, due to the noise of the sensor and the drift of the inertial measurement unit, the system has cumulative errors, which will lead to a decrease in the accuracy of the system positioning and attitude. In order to solve this problem, we use loop detection technology in the lidar mapping process, select lidar key frames in the mapping process, and trigger loop detection when the system detects that there is a potential match between the current scan frame and the historical lidar key frame. The trajectory between these two time points is adjusted to eliminate errors caused by sensor noise, environmental changes or dynamic obstacles. The lidar inertial odometer mainly provides distance measurement, but its perception ability for detailed information such as color and texture of the environment is relatively limited. At the same time, due to the dependence on initial attitude estimation, the rapid growth of cumulative errors and the lack of external environmental reference points, it will seriously affect the integrity of the map and the visualization friendliness of the map. In order to solve this problem, we use the attitude optimization method of visual-inertial odometer data. Using the data provided by the visual-inertial odometer, an attitude estimation algorithm is used to estimate the current position of the device, and an attitude optimization algorithm is used to optimize the estimated attitude.

[0006] System overview as attached Figure 1 Shown

[0007] The present invention makes comprehensive use of sensor data such as LiDAR, IMU and visual-inertial odometry to estimate the state and trajectory of the robot. LiDAR is responsible for building the geometry of the global map, registering the input LiDAR scans, and estimating the robot's state by minimizing the point-plane residual. The visual-inertial odometry (VIO) is responsible for building the texture of the map. It uses the input image to present the RGB color of each point and updates the system state by minimizing the frame-to-frame PnP reprojection error and the frame-to-map photometric error. At the same time, the present invention also integrates a loop detection function to further improve the estimation accuracy of the state and trajectory.

[0008] The specific implementation steps of the present invention are as follows:

[0009] Step 1: Select keyframe;

[0010] When the LiDAR scan arrives, feature extraction is performed first. Edge and plane features are extracted by evaluating the roughness of points in the local area. The edge and plane features extracted from the LiDAR scan at time i are expressed as and All features extracted at time i constitute a lidar frame F i When the change of the robot posture exceeds the user-defined threshold, a new robot state x is obtained. When the change of the robot posture at time i+1 is different from the previous state x i When the comparison exceeds any user-defined threshold, the lidar frame F is selected. i+1 As a key frame. The newly saved key frame F i+1 With the new robot state x i+1 The two key frames F i and F i+1 The lidar frames between the two are discarded. The user-defined thresholds include the robot position threshold and the robot rotation change threshold.

[0011] Furthermore, the robot position threshold and the robot rotation change threshold are selected as 1m and 10° respectively.

[0012] Step 2: Scan frame matching;

[0013] When a new robot state x i+1 When it is acquired, first search the historical keyframes to find the keyframe close to x in Euclidean space. i+1 The prior state provides an initial estimate for a more accurate positioning of the current robot state.

[0014] Step 3: Trigger loop detection;

[0015] If a potential match is detected between the current scan frame and a historical key frame, that is, the change between the robot state of the current scan frame and the robot state of the historical key frame is less than the set matching threshold, loop detection is triggered.

[0016] Step 4: Scan matching and trajectory adjustment;

[0017] When loop detection is triggered, the trajectory between the current scan frame and the matched historical keyframe is scanned and matched, and the point cloud is registered using the lidar data to estimate the relative motion between the corresponding time points of the two frames. This relative motion is called the loop closure constraint.

[0018] Step 5: Error elimination;

[0019] After acquiring the relative motion, this information is used to adjust the trajectory between the current scan frame and the matched historical keyframe to reduce the error caused by sensor noise, environmental changes or dynamic obstacles. Specifically, the relative motion information is used to update the trajectory so that the updated trajectory is more consistent with the acquired relative motion.

[0020] Step 6: Pose optimization of visual-inertial odometry;

[0021] After loop closure detection and point cloud registration, the texture of the global map is rendered via visual-inertial odometry (VIO), and the robot state is optimized by minimizing the photometric error. Specifically, the keyframes are projected from the global map into the image at the corresponding moment, and then the robot state is iteratively optimized by minimizing the photometric error of the map points within the extended Kalman filter (ESIKF) framework. The photometric error is calculated using the color of a single map point. To ensure robust and fast convergence, map points are first tracked using inter-frame optical flow, and the system state is optimized by minimizing the perspective projection error of the tracked map points. Then, the system state estimate is further optimized by minimizing the frame-to-map photometric error between the tracked map points.

[0022] The beneficial effects of the present invention are as follows:

[0023] Compared with the prior art, the method of the present invention combines visual inertial odometry (VIO), lidar data and loop detection to make up for the limitations of each when used alone. By introducing the loop detection function in the navigation system, we can effectively correct the positioning errors caused by sensor error accumulation, environmental changes and dynamic obstacles, and improve the accuracy and robustness of long-term navigation. This comprehensive method can obtain more accurate position information under different environmental conditions, improve the positioning accuracy of the navigation system, and make it more robust, able to adapt to various complex environments, avoid the problem of navigation system performance degradation during long-term operation, and provide a more reliable and effective solution for the navigation of autonomous vehicles and advanced robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 A posture optimization framework for using visual-inertial odometer data in an embodiment of the present invention;

[0025] Figure 2 is the projection error Perspective-n-Point. DETAILED DESCRIPTION

[0026] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0027] The laser radar mapping method based on loop detection and visual-inertial odometer of the present invention has the following specific implementation steps:

[0028] Step 1: Select keyframe;

[0029] When a LiDAR scan arrives, feature extraction is first performed. Edge and plane features are extracted by evaluating the roughness of points on a local area. Points with a large roughness value are classified as edge features. Similarly, a plane feature is classified by a small roughness value. The edge and plane features extracted from the LiDAR scan at time i are represented as and All features extracted at time i constitute a lidar frame F i .

[0030] The roughness calculation steps are as follows:

[0031] (1) Neighborhood point selection: For each point p on the point cloud generated by LiDAR scanning i , select points within a certain range (pre-set) around it as the neighborhood point set

[0032] (2) Roughness calculation: Calculate the roughness value r of the point i , using the following formula: in is the number of neighboring points in the neighborhood point set, ||p i -p j || represents point p i and the neighborhood point p j The Euclidean distance between .

[0033] Feature classification:

[0034] (1) Edge feature: Points with larger roughness values ​​are classified as edge features. Specifically, an edge feature threshold T is set edge , if r i >T edge , then pi Classified as a marginal feature.

[0035] (2) Plane feature: Points with larger roughness values ​​are classified as edge features. Specifically, a plane feature threshold T is set plane , if r i <T plane , then p i Classified as a planar feature.

[0036] When the change in the robot posture exceeds a user-defined threshold, a new robot state x is obtained. We adopt the concept of keyframe selection widely used in the field of visual SLAM. Using a simple but effective heuristic method, when the change in the robot posture at time i+1 is compared with the previous state x i When the comparison exceeds any user-defined threshold, the lidar frame F is selected. i+1 As a key frame. The newly saved key frame F i+1 With the new robot state x i+1 The two key frames F i and F i+1 The lidar frames between the two are discarded. The user-defined thresholds include the robot position threshold and the robot rotation change threshold.

[0037] Furthermore, the robot position threshold and the robot rotation change threshold are selected as 1m and 10° respectively.

[0038] Step 2: Scan frame matching;

[0039] When a new robot state x i+1 When it is acquired, first search the historical keyframes to find the keyframe close to x in Euclidean space. i+1 The prior state provides an initial estimate for more accurate positioning of the current robot state. By searching historical keyframes and finding a prior state close to the new state, the system can converge to the actual position of the robot more quickly, thereby improving the efficiency and accuracy of loop detection.

[0040] Step 3: Trigger loop detection;

[0041] If a potential match is detected between the current scan frame and a historical key frame, that is, the change between the robot state of the current scan frame and the robot state of the historical key frame is less than the set matching threshold, loop detection is triggered.

[0042] Step 4: Scan matching and trajectory adjustment;

[0043] When loop detection is triggered, the trajectory between the current scan frame and the matched historical key frame is scanned and matched, and the LiDAR data is used for point cloud registration to estimate the relative motion between the corresponding time points of the two frames. This relative motion is called the loop closure constraint. Therefore, scan matching is an important step in loop detection, which is used to determine the relative position relationship between the current scan frame and the historical key frame. The specific implementation steps are as follows:

[0044] (1) Point cloud registration;

[0045] The purpose of point cloud registration is to find an optimal rigid body transformation (translation and rotation) so that the current scan frame point cloud P and the historical key frame point cloud Q are aligned in the same coordinate system. Generally, the ICP (Iterative Closest Point) algorithm is used for point cloud registration.

[0046] (2) Rigid body transformation;

[0047] Assume that the current scan frame point cloud is P and the historical key frame point cloud is Q. The rigid body transformation includes the rotation matrix R and the translation vector t. For point p i ∈P, the transformed point is p i '=Rp i +t. The goal is to minimize the error between the transformed point cloud P' and the point cloud Q.

[0048] (3) ICP algorithm;

[0049] For each point p in the point cloud P i , find the closest point q in the point cloud Q i , The error function is defined as The error function E(R, t) is minimized by iteratively optimizing the rotation matrix R and the translation vector t.

[0050] (4) Calculation of loop closure constraints;

[0051] The optimal rigid body transformation parameter rotation matrix R and translation vector t are obtained through the ICP algorithm, which represents the relative motion between the current scan frame and the historical key frame. This relative motion is called the loop closure constraint C. loop , C loop =(R,t), which can be used to correct the trajectory of the robot or vehicle and reduce the cumulative error.

[0052] Step 5: Error elimination;

[0053] After obtaining the relative motion, use this information to adjust the trajectory between the current scan frame and the matched historical keyframe to reduce the error caused by sensor noise, environmental changes, or dynamic obstacles. Specifically, the relative motion information is used to update the trajectory so that the updated trajectory is more consistent with the obtained relative motion. The goal is to make the error of the loop closure constraint as small as possible to ensure the consistency and accuracy of the trajectory.

[0054] Specific steps to adjust the trajectory:

[0055] (1) Global trajectory map optimization;

[0056] Incorporate loop closure constraints into the global trajectory graph optimization. The trajectory graph is usually composed of nodes and edges. The nodes represent the pose at each moment, and the edges represent the relative motion between adjacent frames or loop closure constraints. The goal of global trajectory graph optimization is to minimize the error on all edges, including the relative motion error between sequential frames (temporally continuous frames) and the error of loop closure constraints.

[0057] (2) Error function definition;

[0058] For each loop closure constraint C loop , define the error function E loop =||C loop -(R est ,t est )||, where (R est ,t est ) is the estimated relative motion.

[0059] (3) Overall optimization goal;

[0060] Combined with the relative motion error E between sequential frames seq and loop closure constraint error E loop , define the overall optimization goal in, εseq is the set of edges between sequential frames, εloop is the set of loop closure edges.

[0061] (4) Optimization algorithm Use a graph optimization algorithm, such as g2o (General Graph Optimization) or CeresSolver, to minimize the overall error E total , optimize the pose of each node.

[0062] Step 6: Pose optimization of visual-inertial odometry;

[0063] After loop detection and point cloud registration, the visual-inertial odometry (VIO) system can use this information to further optimize the pose. Loop detection provides additional constraints that help correct long-term accumulated drift. The texture of the global map is rendered by visual-inertial odometry (VIO), and the robot state is optimized by minimizing the photometric error. Specifically, the keyframes are projected from the global map to the image at the corresponding moment, and then the robot state is iteratively optimized by minimizing the photometric error of the map points within the extended Kalman filter (ESIKF) framework. The photometric error is calculated using the color of a single map point. The color of the map point is rendered simultaneously with the VIO, which is an inherent property of the map point and is invariant to the translation and rotation of the camera. In order to ensure robust and fast convergence, the present invention designs a two-step process. First, the map points are tracked using inter-frame optical flow, and the state of the system is optimized by minimizing the perspective projection error of the tracked map points. Then, the state estimate of the system is further optimized by minimizing the frame-to-map photometric error between the tracked map points. The specific implementation is as follows:

[0064] The inter-frame optical flow is used to track the map points, and the robot's state is optimized by minimizing the PnP projection error of the tracked map points.

[0065] Assume that in the previous image frame I k-1 Track m map points And in I k-1 Project up to get the map projection point Use the Lucas-Kanade optical flow method to find the map projection point in the current image frame I k The position in is represented by Then, the robot’s state is optimized by calculating and optimizing their projection errors through EKF.

[0066] (1) Projection error Perspective-n-Point, as shown in the attached Figure 2 shown.

[0067] For the sth tracked map point Projection Error The calculation formula is as follows:

[0068]

[0069] in, It is a map point in the world coordinate system, including the location G p and color c; C p s =[ C p x , C p y , C pz ] T is the coordinate of the point in the camera coordinate system; is the rotation matrix from the inertial coordinate system to the world coordinate system; is the rotation matrix from the camera coordinate system to the inertial coordinate system; is the translation vector from the camera coordinate system to the inertial coordinate system; G p s is a map point in the world coordinate system; The projection function is the current state estimate of the state vector x in each ESIKF iteration (the process of applying the extended Kalman filter algorithm for state estimation over multiple time steps). The calculation is as follows:

[0070]

[0071] Among them, Δt k-1,k The previous image frame I k-1 and the current image frame I k The first term is the pinhole projection function, and the second term is the time correction factor. is the pixel focal length, and is the offset of the principal point from the upper left corner of the image plane, They represent the x and y coordinates of the point in the camera coordinate system projected onto the image plane through the pixel focal length, i.e., the principal point coordinates. There is measurement noise in the calculation of the projection error in formula (2), which consists of two sources: one is the map projection point The pixel tracking error is the map point in the world coordinate system. G p s Position error:

[0072]

[0073] in, is a map point in the world coordinate system G p s The true value of is the error of the map point position, which follows a normal distribution is the map projection point The true value of is the pixel tracking error, which follows a normal distribution

[0074] Next, obtain the true zero residual The first-order Taylor expansion of is:

[0075]

[0076] Among them, xk is the current state estimate, α s is Gaussian noise, which follows normal distribution N(0,Σα s ), and the Jacobian matrix of the residual function with respect to the state estimation error and the noise covariance matrix The calculation is as follows:

[0077]

[0078] in, is the partial derivative of the residual with respect to the state estimate increment, is the covariance matrix of the pixel tracking error, is the covariance matrix of the map point position error, is the Jacobian matrix of the residuals versus the map point positions, is the partial derivative of the residual with respect to the map point location.

[0079] (2) Frame-to-frame VIO ESIKF update;

[0080] Formula (6) constitutes an observation distribution, which is combined with the prior distribution from the IMU propagation to obtain the maximum a posteriori estimated state estimate increment. (The IMU sensor provides inertial measurement information, including acceleration and angular velocity. In frame-to-frame VIO, the information of the IMU sensor is usually used to predict the motion of the camera and generate a priori estimates. This prior distribution is derived based on the measurement of the IMU sensor and the dynamic model of the camera.) By minimizing the state estimate increment The optimal estimate of the state estimation increment is obtained by taking the sum of the squares of the errors and the weighted sum of the observation residuals, and obtaining the state estimation increment of the maximum a posteriori estimate. The formula is as follows:

[0081]

[0082] in, is the square of the Mahalanobis distance with covariance Σ; is the state estimate obtained by IMU propagation; The state error is changed from The tangent space is projected onto The Jacobian matrix of the tangent space is used for the optimization process of the state estimation increment; is the increment of state estimation in this step The Jacobian matrix of ;

[0083] H in formula (10) is the Jacobian matrix of the observation residual, which describes the sensitivity of the observation residual to the state estimate, that is, the impact of the change of the state estimate on the observation residual.

[0084] In formula (11) It is a vector of observation residuals, which contains the observation residuals projected from each feature point (extracted from the camera image in each frame by the feature detection algorithm, and these feature points have corresponding map points in the map).

[0085] P in formula (12) is a matrix used to calculate the Kalman gain.

[0086] The Kalman gain calculation formula is as follows:

[0087] K=(H T R -1 H+P -1 ) -1 H T R -1 (13)

[0088] By applying the observation residual correction to the state estimate, the updated state estimate is obtained. The observation residual correction refers to adjusting the current state estimate using the observation residual (i.e., the difference between the predicted feature point position and the actual observed feature point position), and the formula is as follows:

[0089]

[0090] (3) Photometric update from frame to map;

[0091] Get the initial optimized state estimate The drift of the system is then reduced by minimizing the photometric error of the tracked points.

[0092] For the sth tracked map point Frame to map photometric error As shown in formula (15):

[0093]

[0094] Among them, γ s and c s are the colors of the map points observed in the current image and the colors of the map points stored in the global map, respectively. Considering their measurement noise, they are shown in formulas (16) and (17):

[0095]

[0096] in, Yes s The true value of Yes s The measurement noise is normally distributed. It is c s The true value of is the estimated error of the last rendering, which follows a normal distribution is the impulse random walk process noise, which follows a normal distribution

[0097] True zero residual The first-order Taylor expansion of is as follows:

[0098]

[0099] in, is the observed residual, and is the increment of state estimation in this step The Jacobian matrix of is the observed residual β s The covariance matrix of is the Jacobian matrix of the residual to the map point position, and the formula is as follows:

[0100]

[0101] (4) Frame to map VIO ESIKF update;

[0102] Formula (18) constructs another state estimation increment The observed distribution of is combined with the prior distribution from the IMU propagation to obtain Maximum a posteriori (MAP) estimate of:

[0103]

[0104] in, is the state estimate increment The Jacobian matrix of .

[0105] The latest state estimate is obtained by minimizing the sum of squared errors of the state estimate increment and the weighted sum of the observation residuals using formula (22). The frame-to-map VIO ESIKF update is iterated until convergence. The converged state estimate is then used to: (1) render the map texture; (2) update the current map point set For use in the next frame; (3) as the starting point for IMU propagation in the next frame for attitude optimization of the visual-inertial odometry.

[0106]

[0107] The latest state estimation is used to correct and update the map, completing the entire lidar mapping process based on loop closure detection and visual-inertial odometry.

[0108] The above contents are further detailed descriptions of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, they can also make several substitutions or modifications to these described embodiments without departing from the concept of the present invention, and these substitutions or modifications should be regarded as belonging to the protection scope of the present invention.

[0109] Parts of the present invention that are not described in detail belong to the well-known technologies of those skilled in the art.

Claims

1. A laser radar mapping method based on loop closure detection and visual-inertial odometer, characterized in that: The specific implementation steps are as follows: Step 1: Select keyframe; When the LiDAR scan arrives, feature extraction is first performed; edge and plane features are extracted by evaluating the roughness of points on the local area; The edge and plane features extracted from the LiDAR scan at time i are expressed as and All features extracted at time i constitute a lidar frame F i ; When the change of the robot posture exceeds the user-defined threshold, a new robot state x is obtained; when the change of the robot posture at time i+1 is different from the previous state x i When the comparison exceeds any user-defined threshold, the lidar frame F is selected. i+1 As keyframe; newly saved keyframe F i+1 With the new robot state x i+1 Associated; two key frames F i and F i+1 The laser radar frames between are discarded; the user-defined thresholds include a robot position threshold and a robot rotation change threshold; Step 2: Scan frame matching; When a new robot state x i+1 When it is acquired, first search the historical keyframes to find the keyframe close to x in Euclidean space. i+1 The prior state of This prior state provides an initial estimate for a more accurate localization of the current robot state; Step 3: Trigger loop detection; If a potential match is detected between the current scan frame and a historical key frame, that is, the change between the robot state of the current scan frame and the robot state of the historical key frame is less than the set matching threshold, loop detection is triggered; Step 4: Scan matching and trajectory adjustment; When loop detection is triggered, the trajectory between the current scan frame and the matched historical keyframe is scanned and matched, and the point cloud is registered using the lidar data to estimate the relative motion between the corresponding time points of the two frames; this relative motion is called the loop closure constraint; Step 5: Error elimination; After acquiring the relative motion, use this information to adjust the trajectory between the current scan frame and the matched historical keyframe to reduce the error caused by sensor noise, environmental changes or dynamic obstacles; specifically, use the relative motion information to update the trajectory so that the updated trajectory is more consistent with the acquired relative motion; Step 6: Pose optimization of visual-inertial odometry; After loop detection and point cloud registration, the texture of the global map is rendered by the visual-inertial odometry (VIO), and the robot state is optimized by minimizing the photometric error. Specifically, the keyframes are projected from the global map to the image at the corresponding moment, and then the robot state is iteratively optimized by minimizing the photometric error of the map points in the extended Kalman filter (ESIKF) framework. The photometric error is calculated using the color of a single map point. To ensure robust and fast convergence, the map points are first tracked using inter-frame optical flow, and the state of the system is optimized by minimizing the perspective projection error of the tracked map points. The system's state estimate is then further optimized by minimizing the frame-to-map photometric error between tracked map points.

2. The laser radar mapping method based on loop closure detection and visual-inertial odometer according to claim 1 is characterized in that: The roughness calculation steps in step 1 are as follows: (1) Neighborhood point selection: For each point p on the point cloud generated by LiDAR scanning i , select points within a certain range around it as the neighborhood point set (2) Roughness calculation: Calculate the roughness value r of the point i , using the following formula: in is the number of neighboring points in the neighborhood point set, ||p i -p j || represents point p i and the neighborhood point p j The Euclidean distance between Feature classification: (1) Edge feature: Set an edge feature threshold T edge , if r i >T edge , then p i Classified as marginal features; (2) Plane feature: Set a plane feature threshold T plane , if r i <T plane , then p i Classified as a planar feature.

3. The laser radar mapping method based on loop closure detection and visual-inertial odometer according to claim 1, characterized in that: The robot position threshold and robot rotation change threshold are selected as 1m and 10° respectively.

4. The laser radar mapping method based on loop closure detection and visual-inertial odometer according to any one of claim 1, characterized in that: Step 4: The specific implementation steps are as follows: (1) Point cloud registration; The purpose of point cloud registration is to find an optimal rigid body transformation so that the current scan frame point cloud P and the historical key frame point cloud Q are aligned in the same coordinate system; the ICP algorithm is used for point cloud registration; (2) Rigid body transformation; Assume that the current scan frame point cloud is P, and the historical key frame point cloud is Q; the rigid body transformation includes the rotation matrix R and the translation vector t; for point p i ∈P, the transformed point is p i '=Rp i +t; the goal is to minimize the error between the transformed point cloud P' and the point cloud Q; (3) ICP algorithm; For each point p in the point cloud P i , find the closest point q in the point cloud Q i , The error function is defined as Minimize the error function E(R,t) by iteratively optimizing the rotation matrix R and the translation vector t; (4) Calculation of loop closure constraints; The optimal rigid body transformation parameter rotation matrix R and translation vector t are obtained through the ICP algorithm, which represents the relative motion between the current scan frame and the historical key frame; this relative motion is called the loop closure constraint C loop , C loop =(R,t).

5. The laser radar mapping method based on loop detection and visual-inertial odometer according to claim 4 is characterized in that: Specific steps for adjusting the trajectory in step 5: (1) Global trajectory map optimization; The loop closure constraint is incorporated into the global trajectory graph optimization; the trajectory graph consists of nodes and edges, where nodes represent the pose at each moment and edges represent the relative motion between adjacent frames or the loop closure constraint; the goal of global trajectory graph optimization is to minimize the error on all edges, including the relative motion error between sequential frames (temporally continuous frames) and the error of the loop closure constraint; (2) Error function definition; For each loop closure constraint C loop , define the error function E loop =||C loop -(R est ,t est )||, where (R est ,t est ) is the estimated relative motion; (3) Overall optimization goal; Combined with the relative motion error E between sequential frames seq and loop closure constraint error E loop , define the overall optimization goal in, εseq is the set of edges between sequential frames, εloop is the set of loop closure edges; (4) The optimization algorithm uses a graph optimization algorithm to minimize the overall error E total , optimize the pose of each node.

6. The laser radar mapping method based on loop closure detection and visual-inertial odometer according to any one of claims 1 to 5, characterized in that: Step 6 is implemented as follows: Use inter-frame optical flow to track map points and optimize the robot's state by minimizing the PnP projection error of tracked map points; Assume that in the previous image frame I k-1 Track m map points And in I k-1 Project up to get the map projection point Use the Lucas-Kanade optical flow method to find the map projection point in the current image frame I k The position in is represented by Then, the robot’s state is optimized by calculating and optimizing their projection errors through EKF; (1) Projection error Perspective-n-Point; For the sth tracked map point Projection Error The calculation formula is as follows: in, It is a map point in the world coordinate system, including the location G p and color c; C p s =[ C p x , C p y , C p z ] T is the coordinate of the point in the camera coordinate system; is the rotation matrix from the inertial coordinate system to the world coordinate system; is the rotation matrix from the camera coordinate system to the inertial coordinate system; is the translation vector from the camera coordinate system to the inertial coordinate system; G p s is a map point in the world coordinate system; The projection function is the current state estimate of the state vector x in each ESIKF iteration (the process of applying the extended Kalman filter algorithm for state estimation over multiple time steps). The calculation is as follows: Among them, Δt k-1,k The previous image frame I k-1 and the current image frame I k The first term is the pinhole projection function, and the second term is the time correction factor; is the pixel focal length, and is the offset of the principal point from the upper left corner of the image plane, They represent the x and y coordinates of the point in the camera coordinate system projected onto the image plane through the pixel focal length, i.e., the principal point coordinates; there is measurement noise in the calculation of the projection error in formula (2), which consists of two sources: one is the map projection point The pixel tracking error is the map point in the world coordinate system. G p s Position error: in, is a map point in the world coordinate system G p s The true value of is the error of the map point position, which follows a normal distribution is the map projection point The true value of is the pixel tracking error, which follows a normal distribution Next, obtain the true zero residual The first-order Taylor expansion of is: Among them, x k is the current state estimate, α s is Gaussian noise, which follows normal distribution N(0,Σα s ), and the Jacobian matrix of the residual function with respect to the state estimation error and the noise covariance matrix The calculation is as follows: in, is the partial derivative of the residual with respect to the state estimate increment, is the covariance matrix of the pixel tracking error, is the covariance matrix of the map point position error, is the Jacobian matrix of the residuals versus the map point positions, is the partial derivative of the residual with respect to the map point position; (2) Frame-to-frame VIO ESIKF update; Formula (6) constitutes an observation distribution, which is combined with the prior distribution from the IMU propagation to obtain the maximum a posteriori estimate of the state estimate increment; by minimizing the state estimate increment The optimal estimate of the state estimation increment is obtained by taking the sum of the squares of the errors and the weighted sum of the observation residuals, and obtaining the state estimation increment of the maximum a posteriori estimate; the formula is as follows: in, is the square of the Mahalanobis distance with covariance Σ; is the state estimate obtained by IMU propagation; The state error is changed from The tangent space is projected onto The Jacobian matrix of the tangent space is used for the optimization process of the state estimation increment; is the increment of state estimation in this step The Jacobian matrix of ; H in formula (10) is the Jacobian matrix of the observation residual, which describes the sensitivity of the observation residual to the state estimate, that is, the impact of the change of the state estimate on the observation residual; In formula (11) is a vector of observation residuals, which contains the observation residuals projected from each feature point (extracted from the camera image in each frame by the feature detection algorithm, and these feature points have corresponding map points in the map); P in formula (12) is a matrix used to calculate the Kalman gain; The Kalman gain calculation formula is as follows: K=(H T R -1 H+P -1 ) -1 H T R -1 (13) By applying the observation residual correction to the state estimate, the updated state estimate is obtained; the observation residual correction refers to using the observation residual to adjust the current state estimate, and the formula is as follows: (3) Photometric update from frame to map; Get the initial optimized state estimate Then the drift of the system is reduced by minimizing the photometric error of the tracked point; For the sth tracked map point Frame to map photometric error As shown in formula (15): Among them, γ s and c s are the colors of the map points observed in the current image and the colors of the map points stored in the global map, respectively. Considering their measurement noise, they are shown in formulas (16) and (17): in, Yes s The true value of Yes s The measurement noise is normally distributed. It is c s The true value of is the estimated error of the last rendering, which follows a normal distribution is the impulse random walk process noise, which follows a normal distribution True zero residual The first-order Taylor expansion of is as follows: in, is the observed residual, and is the increment of state estimation in this step The Jacobian matrix of is the observed residual β s The covariance matrix of is the Jacobian matrix of the residual to the map point position, and the formula is as follows: (4) Frame to map VIO ESIKF update; Formula (18) constructs another state estimation increment The observed distribution of is combined with the prior distribution from the IMU propagation to obtain Maximum a posteriori (MAP) estimate of: in, is the state estimate increment The Jacobian matrix of ; The latest state estimate is obtained by minimizing the sum of squared errors of the state estimate increment and the weighted sum of the observation residuals through formula (22); the frame-to-map VIO ESIKF update is iterated until convergence; the converged state estimate is then used to: (1) render the map texture; (2) update the current map point set For use in the next frame; (3) as the starting point of IMU propagation in the next frame for attitude optimization of the visual-inertial odometry; The latest state estimation is used to correct and update the map, completing the entire lidar mapping process based on loop closure detection and visual-inertial odometry.

Images