A laser inertial simultaneous localization and mapping method and device

Abstract

The application belongs to the technical field of mobile robot positioning and mapping, and discloses a laser inertial synchronous positioning and mapping method and equipment, wherein continuous pose prediction is obtained by integrating high-frequency data of an inertial measurement unit, and then motion error in a laser point cloud collection period is compensated; a tightly coupled filtering framework is constructed to perform multiple iterative optimization on the robot pose, SCD features are extracted from selected key frames to provide closed loop constraints and eliminate poor trace error; the global pose is optimized and the global map is adjusted to improve real-time performance. The environment point cloud information extracted by the three-dimensional laser radar is used for pose estimation of the robot carrier, and in the mapping and positioning process, the point cloud does not need to be subjected to geometric feature extraction and matching, and is good in adaptability to the mobile robot platform limited in computing resources, meanwhile, the complete three-dimensional point cloud information is reserved, the robustness of state estimation is higher, and then the stability and precision are improved.

Description

Technical Field

[0001] This invention belongs to the technical field of mobile robot localization and mapping, and more specifically, relates to a laser inertial synchronous localization and mapping method and device. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) is a key technology for robots to acquire data through sensors in unknown environments, estimate their own position and orientation, and perform environmental perception and map building. It is an important prerequisite for robots to achieve autonomous decision-making and navigation. SLAM systems are mainly divided into laser SLAM and visual SLAM based on the type of sensor. Most existing popular SLAM frameworks rely on stable and repeatable geometric features such as corners and planes. In environments where features are missing, repetitive, or the scene changes drastically, methods based on feature extraction and matching are prone to failure and loss of localization.

[0003] Taking laser SLAM as an example, the front-end odometry based on the direct method can construct richer environmental constraints by directly utilizing all the structural information of the original point cloud. However, due to the limited computing resources of mobile robot platforms, it is impossible to add a loop closure detection module to perform matching calculations on a large number of keyframes. In the back-end optimization process, it is also necessary to manage a large number of constraints and variables, making it difficult to guarantee the real-time performance of the system and the accuracy of the front-end odometry. Summary of the Invention

[0004] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a laser inertial synchronous positioning mapping method and device, which aims to solve the problem of low stability and accuracy of existing mapping methods in complex environments.

[0005] To achieve the above objectives, according to one aspect of the present invention, a laser inertial synchronous positioning and mapping method is provided, comprising the following steps: Step 1: Use a multi-line lidar to acquire a 3D point cloud of the environment, and use an inertial measurement unit to acquire the angular velocity and linear acceleration information of the robot carrier. Step 2: Integrate the obtained angular velocity and linear acceleration information to obtain the rotation matrix and displacement vector of the robot carrier. Based on the obtained rotation matrix and displacement vector, calculate the relative pose transformation of each laser point in the scanning cycle. Based on the relative pose transformation, realize the motion distortion correction of the single-frame three-dimensional point cloud. Step 3: Perform a nearest neighbor search on the laser point in the global coordinate system to select several nearest neighbor points, construct a fitting plane based on the searched nearest neighbor points, and calculate the point-to-surface residual between the current laser point and the fitting plane as the observation value. Step four: Continuously integrate the angular velocity and linear acceleration information collected by the inertial measurement unit to obtain the rotation matrix and displacement vector of the robot carrier. Use the obtained rotation matrix and displacement vector as prior information. Define the robot carrier state variables and error state variables based on the prior information. Calculate the covariance matrix based on the robot carrier state variables and error state variables. Update the error state variables based on the covariance matrix and the observed values. Incorporate the updated error state variables into the robot carrier state variables. Step 5: Select key frames based on the 3D point cloud and extract key frame SCD features. Match and filter the key frame SCD features with historical key frame SCD features. Establish loop constraints for the two frames corresponding to the filtered key frame SCD features and historical key frame SCD features. Then, calculate the relative pose transformation of the two frames based on the robot carrier state variable values ​​corresponding to the two frames. This is called loop frame pose transformation. Step six: Using the rotation matrix and displacement vector in the robot carrier state variables obtained in step four as inter-frame constraints of adjacent keyframes, and the pose transformation of the loop frame as loop constraints, a pose graph optimization problem is constructed. The pose graph optimization problem is solved using a nonlinear optimization library to obtain a globally consistent map.

[0006] Furthermore, for the laser point after distortion correction, a nearest neighbor search is performed on the global map to obtain a predetermined number of nearest neighbor points in the global map that are the first few nearest neighbor points of the current laser point, and the Euclidean distance between these nearest neighbor points and the laser point is returned. If the distance exceeds a certain threshold, it is considered that the neighboring points cannot form an effective planar constraint.

[0007] Furthermore, for neighborhood points that can form effective planar constraints, the normal vector of the fitted plane is solved. and intercept For any laser point that has undergone distortion correction The following formula is used to determine whether the distance from the laser point to the fitted plane is less than a threshold. If the threshold is not met, the plane constraint is discarded:

[0008] Normalize the plane normal vectors that satisfy the plane constraints, and calculate the normalized intercepts. The point-to-surface residual from the laser point coordinates to the fitted plane is calculated and used as the observation value for subsequent filtering.

[0009] Furthermore,

[0010] in Indicates laser point Point-to-surface residuals This represents the normal vector of the fitted plane corresponding to that point. and These represent the rotation matrix and position vector of the current state, respectively.

[0011] Furthermore, the filtering sub-steps in step four are as follows: S41: Define observation data ,in Represent the observation equation, To observe the noise, Given the covariance matrix of the observation noise, the Jacobian of the relative error state of the observation equation is calculated using the following formula. ,in The predicted values ​​of the robot carrier state variables are obtained from pre-integration:

[0012] for The abbreviation of .

[0013] S42: Calculate the Kalman gain according to the following formula. :

[0014] S43: Update the error state variables and their covariance matrix according to the following formula:

[0015] S44: Incorporate the updated error state variables into the robot's carrier state variables: This completes a single iteration of the filter optimization process.

[0016] Furthermore, the 3D point cloud is projected onto a 2D plane, and with the lidar coordinate system as the origin, the 3D point cloud is divided along the azimuth direction into... Each sector is then divided into several smaller sectors along the direction of increasing radius. A ring, the 3D point cloud is divided into a circle. The set of sub-blocks; for each sub-block, the maximum height of the laser points in the sub-block is taken as the set of laser points in the sub-block, and SCD is represented as a... OK Column matrix .

[0017] Furthermore, the similarity between two point clouds is calculated by the mean of the cosine distances of multiple column vectors, for the SCD representation of the current frame. SCD representation of historical frames The similarity is calculated using the following formula:

[0018] in For matrix The j List, For matrix The j List; The representation method of SCD depends on the position and orientation of the LiDAR, and the matrix needs to be traversed during the similarity calculation process. The column offset is determined by the minimum distance. Determine the optimal offset :

[0019] Historical keyframes whose similarity scores meet a set threshold are selected as loopback frames, and the optimal offset is used. Calculate the initial yaw angle of the current frame relative to the loopback frame. To coarsely align the current frame with the loopback frame: ; The coarsely aligned historical frames and loopback frames are further registered using ICP, and the pose transformations between loopback frames are recorded and fed into the pose graph optimization.

[0020] Furthermore, the robot carrier's state variables... Defined as: ,in , represents the rotation matrix, Indicates displacement. Indicates speed, This represents the gyroscope zero bias vector of the inertial measurement unit. This represents the accelerometer zero bias vector of the inertial measurement unit. Represents gravitational acceleration; define the error state variable as... , with the state variables of the robot carrier Each item corresponds one-to-one, and the prediction is made according to the following formula:

[0021] in Represents error state variables The covariance matrix, The covariance matrix includes noise from accelerometer and gyroscope measurements, as well as the noise term with zero bias. and These are the state transition matrix and the noise driving matrix, respectively; For robot carrier state variables The predicted value; The predicted value of the covariance matrix; This represents the predicted value of the error state variable.

[0022] The present invention also provides a laser inertial synchronous positioning and mapping system, the system including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the laser inertial synchronous positioning and mapping method as described above.

[0023] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the laser inertial synchronous positioning and mapping method as described above.

[0024] In summary, compared with the prior art, the laser inertial synchronous positioning and mapping method and equipment provided by the present invention have the following advantages: 1. This invention utilizes environmental point cloud information extracted by 3D LiDAR for pose estimation of a robot carrier. During mapping and localization, there is no need to extract and match geometric features of the point cloud. It is well adapted to mobile robot platforms with limited computing resources, while preserving complete 3D point cloud information, resulting in stronger robustness of state estimation and thus improving stability and accuracy.

[0025] 2. This invention efficiently utilizes high-frequency inertial measurement data to provide motion priors for 3D point clouds, removing motion distortion to ensure geometric consistency in point cloud registration. Simultaneously, through tight-coupled filtering in step four, inertial data and point cloud data are fused, and lidar observation data is used to effectively correct the zero bias of the inertial data, achieving stable pose estimation.

[0026] 3. The tightly coupled filtering realizes the prediction and updating of the error state. Since the update amount is small, it can significantly reduce the linearization error generated in the calculation process. At the same time, it decouples from the robot carrier state variables obtained by integrating the inertial measurement data, and can obtain better and more convergent pose estimation results.

[0027] 4. The laser inertial synchronous positioning and mapping method adds a loop closure detection step, which constructs a stable and reliable loop closure constraint through two steps: coarse alignment and precise registration, thereby further improving the accuracy of positioning and mapping. Attached Figure Description

[0028] Figure 1 This is a flowchart of a laser inertial synchronous positioning and mapping method provided in an embodiment of the present invention; Figure 2 This is a flowchart of laser point cloud distortion correction provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the laser dot surface residual provided in an embodiment of the present invention; Figure 4This is a flowchart of the tightly coupled filtering optimization provided in an embodiment of the present invention; Figure 5 This is a flowchart of the loop closure correction method according to an embodiment of the present invention. Detailed Implementation

[0029] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0030] This invention provides a laser inertial synchronous positioning and mapping method. The method obtains continuous pose prediction by integrating high-frequency data from the inertial measurement unit, thereby compensating for motion errors within the laser point cloud acquisition cycle. A tightly coupled filtering framework is constructed to iteratively optimize the robot pose multiple times. SCD features are extracted from selected keyframes to provide closed-loop constraints and eliminate bad track errors. The global pose is optimized and the global map is adjusted. At the same time, an incremental kd-tree is used to improve map management efficiency and enhance real-time performance.

[0031] Please see Figure 1 The location mapping method mainly includes the following steps: Step 1: Use a multi-line lidar to acquire a 3D point cloud of the environment, and use an inertial measurement unit to acquire the angular velocity and linear acceleration information of the robot carrier.

[0032] In this embodiment, an RS-16 multi-line lidar is used to acquire 3D point cloud data of the environment at a frequency of 10Hz, and an HFI-A9 inertial measurement unit is used to collect the linear acceleration and angular velocity information of the robot carrier at a frequency of 150Hz. The collected information is published by the ROS (Robot Operation System) to display the robot carrier's state variables. Defined as: ,in , represents the rotation matrix, Indicates displacement. Indicates speed, This represents the gyroscope zero bias vector of the inertial measurement unit. This represents the accelerometer zero bias vector of the inertial measurement unit. It represents the acceleration due to gravity.

[0033] Step 2: Integrate the obtained angular velocity and linear acceleration information to obtain the rotation matrix and displacement vector of the robot carrier. Based on the obtained rotation matrix and displacement vector, calculate the relative pose transformation of each laser point in the scanning cycle. Based on the relative pose transformation, realize the motion distortion correction of the single-frame three-dimensional point cloud.

[0034] Please see Figure 2 The steps for distortion correction of a single frame of 3D point cloud are as follows: S21: For a single frame of original 3D point cloud (originally acquired), calculate the scanning period of the single frame of 3D point cloud based on the inherent frequency of the lidar, and acquire high-frequency inertial measurement data within this scanning period, including angular velocity data and acceleration data. After the lidar and inertial measurement unit clocks are synchronized, accurately timestamp the start and end times of the original 3D point cloud scan, and also record the accurate timestamp of each lidar point in the frame of 3D point cloud relative to the start time.

[0035] S22, using the lidar pose corresponding to the start time of the 3D point cloud in this frame as a reference frame, integrate all inertial measurement values ​​within the scanning period, derive a series of discrete state estimates from the initial state, and add corresponding timestamps to construct a continuous inertial trajectory from the start time to the end time of the 3D point cloud in this frame.

[0036] S23, for each laser point and its corresponding timestamp in the 3D point cloud, find the two closest state estimates before and after the timestamp of the laser point in the discrete state sequence obtained by pre-integration of the inertial measurement unit, and record their timestamps and poses respectively. Based on the recorded timestamps and poses, interpolation is used to estimate the lidar pose of the laser point at the corresponding time. In this embodiment, linear interpolation is used for the translation part, and spherical linear interpolation is used for the rotation part.

[0037] S24. Using the laser point's pose at the corresponding moment obtained by interpolation calculation, the laser point is uniformly transformed to the reference coordinate system at the start of the point cloud. The above transformation is performed on all laser points contained in the frame of the 3D point cloud. The resulting laser points are all located in the same reference coordinate system, and a single frame of 3D point cloud after removing motion distortion is obtained.

[0038] Step 3: Perform a nearest neighbor search on the laser point in the global coordinate system to select several nearest neighbor points. Construct a fitting plane based on the searched nearest neighbor points, and calculate the point-to-surface residual between the current laser point and the fitting plane as the observation value.

[0039] Please see Figure 3For the distorted laser point, a nearest neighbor search is performed on the global map to obtain the nearest neighbor points to the current laser point. The Euclidean distance between these nearest neighbor points and the laser point is returned. If the distance exceeds a certain threshold, the neighboring points are considered unable to form a valid planar constraint. In this embodiment, the number of nearest neighbor points should be no less than 5.

[0040] Solve for the normal vector of the fitted plane from the neighborhood points that can form an effective planar constraint. and intercept For any laser point that has undergone distortion correction The following formula is used to determine whether the distance from the laser point to the fitted plane is less than a threshold. If the threshold is not met, the plane constraint is discarded:

[0041] Normalize the plane normal vectors that satisfy the plane constraints, and calculate the normalized intercepts. The point-to-surface residual from the laser point coordinates to the fitted plane is calculated and used as the observation value for subsequent filtering.

[0042] in Indicates laser point Point-to-surface residuals This represents the normal vector of the fitted plane corresponding to that point. and These represent the rotation matrix and position vector of the current state, respectively.

[0043] Step four: Continuously integrate the angular velocity and linear acceleration information collected by the inertial measurement unit to obtain the rotation matrix and displacement vector of the robot carrier. Use the obtained rotation matrix and displacement vector as prior information. Define the robot carrier state variables and error state variables based on the prior information. Calculate the covariance matrix based on the robot carrier state variables and error state variables. Update the error state variables based on the covariance matrix and the observed values. Incorporate the updated error state variables into the robot carrier state variables.

[0044] Please see Figure 4 In this process, the observed values ​​enter the filter, and the prediction steps of the corresponding tightly coupled filter framework include: defining the error state variable as... , with the state variables of the robot carrier Each item corresponds one-to-one, and the prediction is made according to the following formula:

[0045] in Represents error state variables The covariance matrix, The covariance matrix includes noise from accelerometer and gyroscope measurements, as well as the noise term with zero bias. and These are the state transition matrix and the noise driving matrix, respectively; For robot carrier state variables The predicted value; The predicted value of the covariance matrix; This represents the predicted value of the error state variable.

[0046] In this embodiment, the matrix and The specific form is as follows:

[0047] Specifically, the filtering sub-steps are as follows: S41: Define observation data ,in Represent the observation equation, To observe the noise, Given the covariance matrix of the observation noise, the Jacobian of the relative error state of the observation equation is calculated using the following formula. ,in The predicted values ​​of the robot carrier state variables are obtained from pre-integration:

[0048] for The abbreviation of .

[0049] S42: Calculate the Kalman gain according to the following formula. :

[0050] S43: Update the error state variables and their covariance matrix according to the following formula:

[0051] S44: Incorporate the updated error state variables into the robot's carrier state variables: This completes a single iteration of the filter optimization process.

[0052] The final robot carrier state variable is obtained only if the number of iterations or the error state correction value reaches a certain threshold. This is the final pose output value. In this embodiment, the threshold for the number of iterations is 20.

[0053] Step 5: Select keyframes based on the 3D point cloud and extract keyframe SCD features. Match and filter the keyframe SCD features with historical keyframe SCD features. Establish loop constraints for the two frames corresponding to the filtered keyframe SCD features and historical keyframe SCD features. Then, calculate the relative pose transformation of the two frames based on the robot carrier state variable values ​​corresponding to the two frames. This is called loop frame pose transformation.

[0054] Please see Figure 5 The SCD features of keyframes are obtained as follows: the 3D point cloud is projected onto a 2D plane, and the 3D point cloud is divided along the azimuth direction with the LiDAR coordinate system as the origin. Each sector is then divided into several smaller sectors along the direction of increasing radius. A ring, the 3D point cloud is divided into a circle. The set of sub-blocks. For each sub-block, the maximum height of the laser points in the sub-block represents the set of laser points in the sub-block, and SCD is represented as a... OK Column matrix .

[0055] In this embodiment, , , For the first i The nth ring, the first j Set of laser points within a sector Using encoding functions For matrix Encode:

[0056] The similarity between two point clouds is calculated by the mean of the cosine distances of multiple column vectors, for the SCD representation of the current frame. SCD representation of historical frames The similarity is calculated using the following formula:

[0057] in For matrix The j List, For matrix The j List.

[0058] The representation method of SCD depends on the position and orientation of the LiDAR, and the matrix needs to be traversed during the similarity calculation process. Possible column offset scenarios, and by minimum distance Determine the optimal offset :

[0059] Historical keyframes whose similarity scores meet a set threshold are selected as loopback frames, and the optimal offset is used. Calculate the initial yaw angle of the current frame relative to the loopback frame. To coarsely align the current frame with the loopback frame:

[0060] The coarsely aligned historical frames and loopback frames are further registered using ICP (Iterative Closest Point) to record the pose transformations between loopback frames, and then fed into the pose graph optimization.

[0061] Step six: Using the rotation matrix and displacement vector in the robot carrier state variables obtained in step four as inter-frame constraints of adjacent keyframes, and the pose transformation of the loop frame as loop constraints, a pose graph optimization problem is constructed. The pose graph optimization problem is solved using a nonlinear optimization library to obtain a globally consistent map.

[0062] Incremental kd-trees are used for global map management.

[0063] In this embodiment, the adjacent frame constraints provided by the odometer have the same weight as the loop closure frame constraints provided by the loop closure module. The nonlinear optimization library used is Ceres, and the maximum number of iterations for solving is 200.

[0064] The present invention also provides a laser inertial synchronous positioning and mapping system, the system including a memory and a processor, the memory storing a computer program, and the processor executing the computer program to perform the laser inertial synchronous positioning and mapping method as described above.

[0065] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the laser inertial synchronous positioning and mapping method as described above.

[0066] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A laser inertial synchronous positioning and mapping method, characterized in that, The steps are as follows: Step 1: Use a multi-line lidar to acquire a 3D point cloud of the environment, and use an inertial measurement unit to acquire the angular velocity and linear acceleration information of the robot carrier. Step 2: Integrate the obtained angular velocity and linear acceleration information to obtain the rotation matrix and displacement vector of the robot carrier. Based on the obtained rotation matrix and displacement vector, calculate the relative pose transformation of each laser point in the scanning cycle. Based on the relative pose transformation, realize the motion distortion correction of the single-frame three-dimensional point cloud. Step 3: Perform a nearest neighbor search on the laser point in the global coordinate system to select several nearest neighbor points, construct a fitting plane based on the searched nearest neighbor points, and calculate the point-to-surface residual between the current laser point and the fitting plane as the observation value. Step four: Continuously integrate the angular velocity and linear acceleration information collected by the inertial measurement unit to obtain the rotation matrix and displacement vector of the robot carrier. Use the obtained rotation matrix and displacement vector as prior information. Define the robot carrier state variables and error state variables based on the prior information. Calculate the covariance matrix based on the robot carrier state variables and error state variables. Update the error state variables based on the covariance matrix and the observed values. Incorporate the updated error state variables into the robot carrier state variables. Step 5: Select key frames based on the 3D point cloud and extract key frame SCD features. Match and filter the key frame SCD features with historical key frame SCD features. Establish loop constraints for the two frames corresponding to the filtered key frame SCD features and historical key frame SCD features. Then, calculate the relative pose transformation of the two frames based on the robot carrier state variable values ​​corresponding to the two frames. This is called loop frame pose transformation. Step six: Using the rotation matrix and displacement vector in the robot carrier state variables obtained in step four as inter-frame constraints of adjacent keyframes, and the pose transformation of the loop frame as loop constraints, a pose graph optimization problem is constructed. The pose graph optimization problem is solved using a nonlinear optimization library to obtain a globally consistent map.

2. The laser inertial synchronous positioning and mapping method as described in claim 1, characterized in that: For the laser point after distortion correction, perform a nearest neighbor search on the global map to obtain a predetermined number of nearest neighbor points that are the first few in the global map and return the Euclidean distance between these nearest neighbor points and the laser point. If the distance exceeds a certain threshold, it is considered that the neighboring points cannot form an effective planar constraint.

3. The laser inertial synchronous positioning and mapping method as described in claim 2, characterized in that: Solve for the normal vector of the fitted plane from the neighborhood points that can form an effective planar constraint. and intercept For any laser point that has undergone distortion correction The following formula is used to determine whether the distance from the laser point to the fitted plane is less than a threshold. If the threshold is not met, the plane constraint is discarded: Normalize the plane normal vectors that satisfy the plane constraints, and calculate the normalized intercepts. The point-to-surface residual from the laser point coordinates to the fitted plane is calculated and used as the observation value for subsequent filtering.

4. The laser inertial synchronous positioning and mapping method as described in claim 3, characterized in that: in Indicates laser point Point-to-surface residuals This represents the normal vector of the fitted plane corresponding to that point. and These represent the rotation matrix and position vector of the current state, respectively.

5. The laser inertial synchronous positioning and mapping method as described in claim 1, characterized in that: The filtering sub-steps in step four are as follows: S41: Define observation data ,in Represent the observation equation, To observe the noise, Given the covariance matrix of the observation noise, the Jacobian of the relative error state of the observation equation is calculated using the following formula. ,in The predicted values ​​of the robot carrier state variables are obtained from pre-integration: for abbreviation; S42: Calculate the Kalman gain according to the following formula. : S43: Update the error state variables and their covariance matrix according to the following formula: S44: Incorporate the updated error state variables into the robot's carrier state variables: This completes a single iteration of the filter optimization process.

6. The laser inertial synchronous positioning and mapping method as described in claim 1, characterized in that: The 3D point cloud is projected onto a 2D plane, and with the lidar coordinate system as the origin, the 3D point cloud is divided along the azimuth direction into... Each sector is then divided into several smaller sectors along the direction of increasing radius. A ring, the 3D point cloud is divided into a circle. The set of sub-blocks; for each sub-block, the maximum height of the laser points in the sub-block is taken as the set of laser points in the sub-block, and SCD is represented as a... OK Column matrix .

7. The laser inertial synchronous positioning and mapping method as described in claim 6, characterized in that: The similarity between two point clouds is calculated by the mean of the cosine distances of multiple column vectors, for the SCD representation of the current frame. SCD representation of historical frames The similarity is calculated using the following formula: in For matrix The j List, For matrix The j List; The representation method of SCD depends on the position and orientation of the LiDAR, and the matrix needs to be traversed during the similarity calculation process. The column offset is determined by the minimum distance. Determine the optimal offset : Historical keyframes whose similarity scores meet a set threshold are selected as loopback frames, and the optimal offset is used. Calculate the initial yaw angle of the current frame relative to the loopback frame. To coarsely align the current frame with the loopback frame: ； The coarsely aligned historical frames and loopback frames are further registered using ICP, and the pose transformations between loopback frames are recorded and fed into the pose graph optimization.

8. The laser inertial synchronous positioning and mapping method as described in claim 1, characterized in that: The robot carrier's state variables Defined as: ,in , represents the rotation matrix, Indicates displacement. Indicates speed, This represents the gyroscope zero bias vector of the inertial measurement unit. This represents the accelerometer zero bias vector of the inertial measurement unit. Represents gravitational acceleration; define the error state variable as... , with the state variables of the robot carrier Each item corresponds one-to-one, and the prediction is made according to the following formula: in Represents error state variables The covariance matrix, The covariance matrix includes noise from accelerometer and gyroscope measurements, as well as the noise term with zero bias. and These are the state transition matrix and the noise driving matrix, respectively; For robot carrier state variables The predicted value; The predicted value of the covariance matrix; This represents the predicted value of the error state variable.

9. A laser inertial synchronous positioning and mapping system, characterized in that: The system includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it performs the laser inertial synchronous positioning and mapping method according to any one of claims 1-8.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the laser inertial synchronous positioning and mapping method according to any one of claims 1-8.

Images