A laser radar inertial odometer fusion method based on MSCKF

Abstract

The present invention provides a laser radar inertial odometer fusion method based on MSCKF. It relates to the field of positioning technology. The present application obtains the posture, velocity, gyroscope bias and accelerometer bias solved by the inertial measurement unit, and the posture state information solved by the laser radar; creates a motion model of the inertial measurement unit to use the accelerometer and gyroscope data of the inertial measurement unit for state prediction, and updates the state vector to the next moment; pre-processes the laser radar point cloud data, and extracts key frames from the laser radar frames; calculates the posture of the laser radar in the global coordinate system G when the key frames are collected based on the posture calculated by the inertial measurement unit and the matching results of the static feature points of each key frame; calculates the posture of the laser radar in the global coordinate system G based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain a measurement model by modeling the measurement residuals; the measurement model is optimized by minimizing the reprojection error, and the system state is corrected in the update step of the Kalman filter.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a MSCKF-based laser radar inertial odometer fusion method. Background Art

[0002] The Multi-State Constraint Kalman Filter (MSCKF) algorithm was first proposed to realize real-time vision-assisted inertial navigation, and was later widely used in visual-inertial SLAM and visual-inertial odometry (VIO). MSCKF performs state estimation based on the extended Kalman filter (EKF) by fusing the visual information of the camera and the inertial information of the inertial measurement unit (IMU). It implements state constraints by adding multi-frame feature point observation information, thereby optimizing the pose estimation and improving the estimation accuracy. Since MSCKF has significant advantages in scenarios that require high-precision real-time positioning, especially in embedded systems with multi-sensor fusion and limited computing resources, its efficient computing performance, ability to fuse visual and inertial data, good real-time and robustness make it an ideal choice for applications such as drones, robot navigation, autonomous driving, AR / VR devices, etc. Although the MSCKF algorithm has been widely used in the field of vision and inertial navigation fusion, in actual use, the vision-based navigation method is easily affected by factors such as lighting changes, insufficient environmental features, occlusion and perspective changes, including the difficulty in distinguishing dynamic objects from static backgrounds in dynamic scenes, resulting in reduced final positioning and attitude determination accuracy. Summary of the invention

[0003] In order to solve the above technical problem or at least partially solve the above technical problem, the present invention provides a laser radar inertial odometer fusion method based on MSCKF.

[0004] In a first aspect, the present invention provides a laser radar inertial odometer fusion method based on MSCKF, comprising:

[0005] The state vector is obtained through the inertial measurement unit and the lidar. The state vector includes: the posture, velocity, gyroscope bias and accelerometer bias calculated by the inertial measurement unit, and the posture state information calculated by the most recent set number of lidar frames.

[0006] The motion model of the IMU uses the accelerometer and gyroscope data of the IMU to make state predictions and update the state vector to the next moment;

[0007] Collect and preprocess the LiDAR point cloud data, extract key frames from the LiDAR frames; calculate the LiDAR position in the global coordinate system G when collecting key frames based on the position calculated by the inertial measurement unit and the matching results of the static feature points of each key frame;

[0008] Based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain a measurement model;

[0009] The reprojection error in the lidar observation is used to correct the system state in the update step of the Kalman filter. The measurement model is optimized by minimizing the reprojection error, thereby updating the position and posture of the inertial measurement unit and lidar of the entire system.

[0010] Furthermore, when the motion model of the inertial measurement unit performs motion state propagation prediction, a discrete integration method is used to update the state vector to the next moment.

[0011] Furthermore, the curvature and the preset curvature threshold are used to extract feature points from the pre-processed point cloud data to obtain static feature points that meet the requirements;

[0012] Perform feature matching and tracking on static feature points, obtain the static feature point retention rate of the current lidar frame relative to the previous key frame through feature matching and tracking, and determine whether the current lidar frame is a key frame based on the static feature point retention rate of the current lidar frame relative to the previous key frame.

[0013] Furthermore, the process of acquiring static feature points includes:

[0014] Calculate the curvature of a point in point cloud data:

[0015] ;

[0016] in Indicate point The point set in the domain of Indicate point points within the domain of

[0017] Points are classified by curvature value. When the curvature value of the point is higher than the given curvature threshold , then the point is a corner point, when the curvature value of the point is lower than the curvature threshold , then the point is a plane point;

[0018] The points belonging to the corner points are sequentially registered, the ICP algorithm is used to register two consecutive frames of point clouds, and the current LiDAR frame point cloud is transformed into the previous frame coordinate system;

[0019] Determine whether points in the area where the corner point is located have spatial displacement exceeding a preset spatial displacement threshold in multiple frames;

[0020] If not, it means that the corner point belongs to a static feature; the corner points that meet the static features are retained as static feature points, and the set formed is defined as ,in Indicates the current laser radar frame point cloud extracted Static feature points.

[0021] Furthermore, count the last key frame The number of static feature points in the current LiDAR frame that are observed and tracked, and the retention rate of static feature points in the current LiDAR frame relative to the previous key frame is calculated:

[0022] ;

[0023] in Indicates the retention rate of static feature points in the current lidar frame relative to the previous key frame. Indicates the previous keyframe The total number of static feature points in , Represents the total number of static feature points in the current lidar frame, Indicates the last key frame The number of static feature points in the current LiDAR frame that are observed and tracked. Less than the pre-set retention rate threshold , it means that the laser radar frame state changes greatly, and the current laser radar frame is used as the new key frame. will be entered into the keyframe array, and the previous keyframe and the current keyframe The lidar frames in between are discarded.

[0024] Furthermore, the pose of the lidar in the global coordinate system G when the key frame is collected is calculated based on the pose calculated by the inertial measurement unit and the static feature point matching results of each key frame:

[0025] According to the matching results of the static feature points of each key frame, the least squares method is used to calculate the position and posture of the lidar device of each key frame:

[0026] ;

[0027] in, and are the rotation matrix and translation vector respectively, and are the matching static feature points in the source keyframe and the target keyframe, respectively. Represents the total number of static feature points. When collecting the current lidar frame, the lidar is relative to the global coordinate system G The pose estimate is:

[0028] ;

[0029] ;

[0030] in, Represents the laser radar relative to the global coordinate system G The pose estimation, Represents the laser radar relative to the global coordinate system G The position estimate, Represents the coordinate system from the inertial measurement unit I To the global reference frame G The posture information, Represents the position information of the inertial measurement unit coordinate system I in the global reference system G.

[0031] Furthermore, the measurement residuals of all detected static feature points are modeled to obtain the measurement model:

[0032] ;

[0033] in, represents the projection of the measurement information in the left null space of the measurement matrix, represents the projection of the measurement noise into the left null space of the measurement matrix, Measurement matrix, represents the state error vector, where is the block vector Composition, expressed as follows:

[0034] ;

[0035] in Indicates the total number of static feature points currently detected, block vector For the The projection of static feature points in the left null space, It is expressed as:

[0036] ,in, represents the left null space transformation matrix of the measurement matrix, Indicates Static feature points The measurement vector of It is expressed as:

[0037] ;

[0038] in, Indicates The number of laser radar frames where static feature points are detected, Indicates Static feature points In the LiDAR frames The measurement residual of is expressed as:

[0039] ;

[0040] in, Indicates Static feature points In the The observation value of the lidar frame, Indicates Static feature points In the LiDAR frames The predicted value of is expressed as:

[0041] ;

[0042] in , and Indicates Static feature points In the LiDAR frames The predicted three-dimensional coordinates of and Respectively expressed in LiDAR frames The pose estimation result of Indicates Static feature points In the global coordinate system G The location information in.

[0043] Furthermore, the Earth-centered Earth-fixed coordinate system is selected as the global reference system G .

[0044] In a second aspect, the present invention provides a MSCKF-based laser radar inertial odometer fusion device, comprising: at least one processing unit, the processing unit, the storage unit, the inertial measurement unit and the laser radar unit are interconnected through a bus unit, the storage unit stores a computer program and data collected by the inertial measurement unit and the laser radar unit, and when the computer program is executed by the processing unit, the laser radar inertial odometer fusion method based on MSCKF is implemented.

[0045] In a third aspect, the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the MSCKF-based laser radar inertial odometer fusion method is implemented.

[0046] The above technical solution provided by the embodiment of the present invention has the following advantages compared with the prior art:

[0047] The traditional MSCKF algorithm is an algorithm based on vision-assisted inertial navigation and positioning. It combines visual information and inertial measurement information, and realizes real-time state estimation based on the extended Kalman filter algorithm to complete high-precision real-time motion tracking. MSCKF performs well in maintaining a balance between system computational efficiency and accuracy, and is one of the classic visual-inertial fusion methods. The advantage of traditional MSCKF is that it reduces computational complexity by marginalizing the old camera pose and avoiding direct estimation of 3D feature points, thus meeting the requirements of real-time applications. However, since the vision-based solution is sensitive to lighting conditions and environmental textures, it is easy to cause problems that lead to algorithm performance degradation in some special scenarios. In addition, MSCKF is sensitive to the initial state. If the initialization is inaccurate, the estimation result may deviate greatly, resulting in the failure of the algorithm to converge.

[0048] The solution based on LiDAR is insensitive to lighting conditions and environmental textures, and has strong adaptability, especially in scenes such as complex terrain, narrow spaces, and textureless areas. It can effectively overcome the problems that visual solutions cannot avoid. By using LiDAR information as an information source for MSCKF to replace visual information, problems that cannot be solved in visual solutions can be solved. At the same time, since LiDAR data information is relatively rich, it can reduce the errors introduced by nonlinear problems in the algorithm, and avoid the problem of decreased accuracy of the MSCKF algorithm under nonlinear characteristics. By fusing LiDAR with inertial navigation, not only the engineering practicality of the system is improved, but also a theoretical basis and effective verification are provided for the subsequent fusion of multi-source navigation data based on vision, LiDAR, and inertial measurement.

[0049] The present invention proposes a laser radar inertial odometer fusion algorithm and solution based on MSCKF. In the traditional MSCKF algorithm solution, vision is used as the main information source for algorithm fusion and solution, but the vision-based solution is easily affected by environmental factors in certain special environments, resulting in a significant performance degradation. Therefore, this technical solution solves the problem that the vision solution is more sensitive to light conditions and environmental textures by fusing the laser radar with the inertial navigation, while improving the engineering practicality of the system, and also provides a theoretical basis and effective verification for the subsequent multi-source navigation data fusion based on vision, laser radar and inertial measurement. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0051] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0052] Figure 1 A flowchart of a laser radar inertial odometer fusion method based on MSCKF provided in an embodiment of the present invention;

[0053] Figure 2 A flowchart of extracting key frames from laser radar frames provided by an embodiment of the present invention;

[0054] Figure 3 A flowchart for obtaining static feature points provided by an embodiment of the present invention;

[0055] Figure 4 A schematic diagram of the MSCKF laser radar inertial odometer fusion device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0056] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0057] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0058] This application involves a variety of coordinate systems. First, the coordinate systems involved in this application are explained. This application selects the Earth-centered Earth-fixed coordinate system as the global reference system. G The inertial measurement unit coordinate system is represented byI The laser radar coordinate system is represented by L express.

[0059] Example 1

[0060] like Figure 1 As shown, the technology of the present invention realizes a laser radar inertial odometer fusion method based on MSCKF, and the present invention includes the following steps:

[0061] Step 1: Obtain the state vector through the inertial measurement unit and the lidar. The state vector includes: the pose, velocity, gyroscope bias and accelerometer bias calculated by the inertial measurement unit, and the pose state information calculated by the latest set number of lidar frames. Among them, the pose calculated by the inertial measurement unit includes position and attitude.

[0062] The state vector is defined as follows:

[0063] ;

[0064] in , ;

[0065] in Represents the state variables solved by the inertial measurement unit, including attitude, position, velocity, gyroscope bias, and accelerometer bias. Represents the coordinate system from the inertial measurement unit I To the global reference frame G The posture information, and Represent the inertial measurement unit coordinate system I Relative to the global reference frame G speed and position information, and Denote the bias of the gyroscope and accelerometer, respectively. Represents the state vector obtained by the laser radar, which is the position state information of the laser radar of the most recent set number of frames, and is used to construct the constraints of the laser radar observation. and are the most recent set number of lidar frames. Frame LiDAR frame contains the laser radar coordinate system L To the global reference frame G posture and position.

[0066] Step 2: Establish the motion model of the inertial measurement unit and predict the motion state based on the real-time data collected by the inertial measurement unit. The motion model of the inertial measurement unit uses the accelerometer and gyroscope data of the inertial measurement unit to propagate the motion state in the state prediction, and uses the discrete integration method to update the state vector to the next moment.

[0067] Step 3: Collect lidar point cloud data in real time, pre-process the raw point cloud data, filter and correct the distortion of the raw point cloud data to improve the quality of the point cloud data and the efficiency and accuracy of subsequent algorithms, and extract key frames from the lidar frames.

[0068] Assume that a laser radar scan contains points, the lidar frame point cloud set is defined as Among them The data format of each point is .in Indicates The three-dimensional coordinates of the points, Indicates The reflection intensity of each point. After preprocessing, the point cloud is represented as .

[0069] like Figure 2 As shown in Figure 1, the process of extracting key frames from lidar frames includes:

[0070] The curvature and the preset curvature threshold are used to extract feature points from the preprocessed point cloud data, and the static feature points that meet the requirements are retained as points for subsequent feature tracking.

[0071] See Figure 3 As shown in FIG. 1 , the process of acquiring static feature points includes:

[0072] Calculate the curvature of a point in point cloud data:

[0073] ;

[0074] in Indicate point The point set in the domain of Indicate point points within the field.

[0075] Points are classified by curvature value. When the curvature value of the point is higher than the given curvature threshold , then the point is a corner point. When the curvature value of the point is lower than the curvature threshold , then the point is a plane point.

[0076] The points belonging to the corner points are temporally registered, the ICP algorithm is used to register two consecutive frames of point clouds, and the current lidar frame point cloud is transformed into the coordinate system of the previous frame.

[0077] It is determined whether points in the area where the corner points are located have spatial displacement exceeding a preset spatial displacement threshold in multiple frames.

[0078] If not, it means that the corner point meets the static feature. The corner points that meet the static feature are retained as static feature points, and the set they form is defined as ,in Indicates the current laser radar frame point cloud extracted Static feature points.

[0079] Perform feature matching and tracking on static feature points, and obtain the static feature point retention rate of the current lidar frame relative to the previous key frame through feature matching and tracking;

[0080] Determine whether the current LiDAR frame is a key frame based on the retention rate of static feature points in the current LiDAR frame relative to the previous key frame, and implement key frame selection. Specifically, among the static feature points extracted in the current LiDAR frame, some have been tracked in multiple previous LiDAR frames. The number of static feature points in the current LiDAR frame that are observed and tracked, and the retention rate of static feature points in the current LiDAR frame relative to the previous key frame is calculated:

[0081] ;

[0082] in, Indicates the retention rate of static feature points in the current lidar frame relative to the previous key frame. Indicates the previous keyframe The total number of static feature points in , Indicates the total number of static feature points in the current lidar frame. Indicates the last key frame The number of static feature points in the current LiDAR frame that are observed and tracked. Less than the pre-set retention rate threshold , it means that the laser radar frame state changes greatly, and the current laser radar frame is used as the new key frame. will be entered into the keyframe array, and the previous keyframe and the current keyframe The lidar frames in between are discarded.

[0083] Selecting key frames in this way not only achieves a balance between mapping density and memory consumption, but also facilitates subsequent state updates, which is suitable for real-time nonlinear optimization.

[0084] Step 4: Calculate the position and posture of the laser radar in the global coordinate system G when the key frame is collected based on the position and posture calculated by the inertial measurement unit and the matching results of the static feature points of each key frame. The process includes:

[0085] According to the matching results of the static feature points of each key frame, the least squares method is used to calculate the rotation matrix and translation vector of the lidar when collecting each lidar frame:

[0086] ;

[0087] in, and are the rotation matrix and translation vector respectively, and are the matching static feature points in the source keyframe and the target keyframe, respectively. Indicates the total number of static feature points.

[0088] When the current lidar frame is collected, the position, rotation matrix and translation vector calculated by the inertial measurement unit are used to estimate the position of the lidar relative to the global coordinate system. G The posture is as follows:

[0089] ;

[0090] ;

[0091] in, Represents the laser radar relative to the global coordinate system G The pose estimation, Represents the laser radar relative to the global coordinate system G position estimate. Represents the coordinate system from the inertial measurement unit I To the global reference frame G The posture information, Represents the position information of the inertial measurement unit coordinate system I in the global reference system G.

[0092] Step 5: Based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain the measurement model, and the following measurement model is obtained:

[0093] ;

[0094] in, represents the projection of the measurement information in the left null space of the measurement matrix, represents the projection of the measurement noise into the left null space of the measurement matrix, Measurement matrix, represents the state error vector, where is the block vector Composition, expressed as follows:

[0095] ;

[0096] in Indicates the total number of static feature points currently detected, block vector For the The projection of static feature points in the left null space, It is expressed as:

[0097] ,in represents the left null space transformation matrix of the measurement matrix, Indicates Static feature points The measurement vector of It is expressed as:

[0098] ;

[0099] in, Indicates The number of laser radar frames in which static feature points are detected. Indicates Static feature points In the LiDAR frames The measurement residual of is expressed as:

[0100] ;

[0101] in Indicates Static feature points In the Observations of lidar frames. Indicates Static feature points In the LiDAR frames The predicted value of can be expressed as:

[0102] ;

[0103] in , and Indicates Static feature points In the LiDAR frames The predicted three-dimensional coordinates of . and Respectively expressed in LiDAR frames The pose estimation results. Indicates Static feature points In the global coordinate system G The location information in.

[0104] Step 6: Update and correct the state. Use the reprojection error in the lidar observation to correct the system state in the Kalman filter update step. The measurement model can be optimized by minimizing the reprojection error, thereby updating the position of the inertial measurement unit and lidar of the entire system.

[0105] Example 2

[0106] See also Figure 4 As shown, an embodiment of the present invention provides a MSCKF laser radar inertial odometer fusion device, including: at least one processing unit, the processing unit, the storage unit, the inertial measurement unit and the laser radar unit are interconnected through a bus unit, and the storage unit is a computer-readable storage medium, which can be used to store software programs, computer executable programs and modules, such as the software programs, computer executable programs and modules corresponding to the laser radar inertial odometer fusion method based on MSCKF in the embodiment of the present invention, which can be used to store data collected by the inertial measurement unit and the radar unit. The processing unit implements the above-mentioned laser radar inertial odometer fusion method based on MSCKF by running the software programs, computer executable programs and modules stored in the storage unit, including:

[0107] The state vector is obtained through the inertial measurement unit and the lidar. The state vector includes: the posture, velocity, gyroscope bias and accelerometer bias calculated by the inertial measurement unit, and the posture state information calculated by the most recent set number of lidar frames.

[0108] The motion model of the IMU uses the accelerometer and gyroscope data of the IMU to make state predictions and update the state vector to the next moment;

[0109] Collect and preprocess the LiDAR point cloud data, extract key frames from the LiDAR frames; calculate the LiDAR position in the global coordinate system G when collecting key frames based on the position calculated by the inertial measurement unit and the matching results of the static feature points of each key frame;

[0110] Based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain a measurement model;

[0111] The reprojection error in the lidar observation is used to correct the system state in the update step of the Kalman filter. The measurement model is optimized by minimizing the reprojection error, thereby updating the position and posture of the inertial measurement unit and lidar of the entire system.

[0112] Of course, the computer program stored in the storage unit of the MSCKF laser radar inertial odometer fusion device provided in an embodiment of the present invention is not limited to the method operations described above, and can also execute related operations in the MSCKF-based laser radar inertial odometer fusion method provided in any embodiment of the present invention.

[0113] Example 3

[0114] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program. When the computer program is executed, the MSCKF-based laser radar inertial odometer fusion method is implemented, including:

[0115] The state vector is obtained through the inertial measurement unit and the lidar. The state vector includes: the posture, velocity, gyroscope bias and accelerometer bias calculated by the inertial measurement unit, and the posture state information calculated by the most recent set number of lidar frames.

[0116] The motion model of the IMU uses the accelerometer and gyroscope data of the IMU to make state predictions and update the state vector to the next moment;

[0117] Collect and preprocess the LiDAR point cloud data, extract key frames from the LiDAR frames; calculate the LiDAR position in the global coordinate system G when collecting key frames based on the position calculated by the inertial measurement unit and the matching results of the static feature points of each key frame;

[0118] Based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain a measurement model;

[0119] The reprojection error in the lidar observation is used to correct the system state in the update step of the Kalman filter. The measurement model is optimized by minimizing the reprojection error, thereby updating the position and posture of the inertial measurement unit and lidar of the entire system.

[0120] A computer-readable storage medium provided in an embodiment of the present invention stores a computer program which is not limited to the method operations described above, but can also execute related operations in a MSCKF-based laser radar inertial odometer fusion method provided in any embodiment of the present invention.

[0121] In the embodiments provided by the present invention, it should be understood that the disclosed structures and methods can be implemented in other ways. For example, the structural embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, structures or units, which can be electrical, mechanical or other forms.

[0122] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0123] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0124] The foregoing is merely a specific embodiment of the present invention, which enables those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A laser radar inertial odometer fusion method based on MSCKF, characterized in that: include: The state vector is obtained through the inertial measurement unit and the lidar. The state vector includes: the posture, velocity, gyroscope bias and accelerometer bias calculated by the inertial measurement unit, and the posture state information calculated by the most recent set number of lidar frames. The motion model of the IMU uses the accelerometer and gyroscope data of the IMU to make state predictions and update the state vector to the next moment; Collect and preprocess the LiDAR point cloud data, extract key frames from the LiDAR frames, including: extract feature points from the preprocessed point cloud data using curvature and a preset curvature threshold, and obtain static feature points that meet the requirements. The process of obtaining static feature points includes: calculating the curvature of the midpoint of the point cloud data: ； in Indicate point The point set in the domain of Indicate point points within the domain of Points are classified by curvature value. When the curvature value of the point is higher than the given curvature threshold , then the point is a corner point, when the curvature value of the point is lower than the curvature threshold , then the point is a plane point; The points belonging to the corner points are sequentially registered, the ICP algorithm is used to register two consecutive frames of point clouds, and the current LiDAR frame point cloud is transformed into the previous frame coordinate system; Determine whether the points in the area where the corner point is located have spatial displacement exceeding the preset spatial displacement threshold in multiple frames; if not, it means that the corner point belongs to a static feature; the corner points that meet the static features are retained as static feature points, and the set composed is defined as ,in Indicates the current laser radar frame point cloud extracted Static feature points; Perform feature matching and tracking on static feature points, obtain the static feature point retention rate of the current laser radar frame relative to the previous key frame through feature matching and tracking, and determine whether the current laser radar frame is a key frame according to the static feature point retention rate of the current laser radar frame relative to the previous key frame; among them, the statistics of the previous key frame The number of static feature points in the current LiDAR frame that are observed and tracked, and the retention rate of static feature points in the current LiDAR frame relative to the previous key frame is calculated: ； in Indicates the retention rate of static feature points in the current lidar frame relative to the previous key frame. Indicates the previous keyframe The total number of static feature points in , Represents the total number of static feature points in the current lidar frame, Indicates the last key frame The number of static feature points in the current LiDAR frame that are observed and tracked. Less than the pre-set retention rate threshold , it means that the laser radar frame state changes greatly, and the current laser radar frame is used as the new key frame. Add to the keyframe array, and the previous keyframe and the current keyframe The lidar frames in between are discarded; The pose calculated based on the inertial measurement unit and the pose of the lidar in the global coordinate system G when the key frame is collected according to the static feature point matching results of each key frame, including: According to the matching results of the static feature points of each key frame, the least squares method is used to calculate the rotation matrix and translation vector of the lidar when collecting each lidar frame: ； in, and are the rotation matrix and translation vector respectively, and are the matching static feature points in the source keyframe and the target keyframe, respectively. Represents the total number of static feature points. When collecting the current lidar frame, the lidar is relative to the global coordinate system G The pose estimate is: ； ； in, Represents the laser radar relative to the global coordinate system G The pose estimation, Represents the laser radar relative to the global coordinate system G The position estimate, Represents the coordinate system from the inertial measurement unit I To the global reference frame G The posture information, Represents the position information of the inertial measurement unit coordinate system I in the global reference system G; Based on the laser radar relative to the global coordinate system G The pose estimation of all detected static feature points is modeled to obtain a measurement model by measuring the residuals, including: Modeling the measurement residuals of all detected static feature points to obtain a measurement model: ； in, represents the projection of the measurement information in the left null space of the measurement matrix, represents the projection of the measurement noise into the left null space of the measurement matrix, Measurement matrix, represents the state error vector, where is the block vector Composition, expressed as follows: ； in Indicates the total number of static feature points currently detected, block vector For the The projection of static feature points in the left null space, It is expressed as: ,in, represents the left null space transformation matrix of the measurement matrix, Indicates Static feature points The measurement vector of It is expressed as: ； in, Indicates The number of laser radar frames where static feature points are detected, Indicates Static feature points In the LiDAR frames The measurement residual of is expressed as: ； in, Indicates Static feature points In the The observation value of the lidar frame, Indicates Static feature points In the LiDAR frames The predicted value of is expressed as: ； in , and Indicates Static feature points In the LiDAR frames The predicted three-dimensional coordinates of and Respectively expressed in LiDAR frames The pose estimation result of Indicates Static feature points In the global coordinate system G Location information in The reprojection error in the lidar observation is used to correct the system state in the update step of the Kalman filter. The measurement model is optimized by minimizing the reprojection error, thereby updating the position and posture of the inertial measurement unit and lidar of the entire system.

2. The MSCKF-based laser radar inertial odometer fusion method according to claim 1, characterized in that: When the motion model of the inertial measurement unit performs motion state propagation prediction, the state vector is updated to the next moment using the discrete integration method.

3. The MSCKF-based laser radar inertial odometer fusion method according to claim 1, characterized in that: Choose the Earth-centered Earth-fixed coordinate system as the global reference system G .

4. A MSCKF laser radar inertial odometer fusion device, characterized in that: include: At least one processing unit, interconnecting the processing unit, the storage unit, the inertial measurement unit and the laser radar unit through a bus unit, wherein the storage unit stores a computer program and data collected by the inertial measurement unit and the laser radar unit, and when the computer program is executed by the processing unit, the laser radar inertial odometer fusion method based on MSCKF as described in any one of claims 1-3 is implemented.

5. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the MSCKF-based laser radar inertial odometer fusion method as described in any one of claims 1 to 3 is implemented.

Images