A localization and mapping method and system based on lidar and IMU

Abstract

This invention proposes a localization and mapping method and system based on lidar and IMU. The method includes: acquiring raw point cloud data from lidar and IMU data from the IMU; preprocessing the raw point cloud data and IMU data; extracting feature points from the point cloud data of the current lidar frame based on the preprocessed raw point cloud data; constructing a sliding window that forms a local map based on the point cloud data and IMU data of the corresponding lidar frame; constructing an optimization function that uses the extracted feature points of the current lidar frame in the local map for optimization to obtain the lidar pose output of the current lidar frame; and stitching together the pose outputs of lidar from all lidar frames to output a 3D point cloud map. This lidar and IMU-based localization and mapping method enhances the stability of feature point extraction and improves matching robustness.

Description

Technical Field

[0001] This invention relates to the field of robot perception and navigation technology, specifically to a localization and mapping method and system based on lidar and IMU. Background Technology

[0002] LiDAR (LiDAR) is typically used for localization and mapping. LiDAR is an active environmental information sensor that acquires environmental point cloud information by emitting laser beams. It is usually used in conjunction with LiDAR, along with an IMU (Inertial Measurement Unit). The IMU measures the object's three-axis attitude angles (or angular rates) and acceleration, detecting data such as acceleration and angular velocity from the LiDAR; this data is generally referred to as IMU data. Suppose a LiDAR is installed on a mobile device, which is then moved within a certain range in an unknown environment. After collecting point cloud data over a period of time, data processing algorithms can be used to construct an environmental map and generate the mobile device's trajectory from its starting position. This data processing algorithm is commonly known as SLAM (Simultaneous Localization and Mapping). Besides LiDAR, SLAM also commonly uses sensors such as cameras, wheel speedometers, GNSS (GNSS), and inertial navigation systems.

[0003] The SLAM algorithm can be divided into a front-end and a back-end. The front-end obtains pose information with accumulated errors, while the back-end further optimizes the pose obtained by the front-end using loop closure detection or GNSS (Generalized Navigation Skills). In the front-end, the current frame's point cloud information is typically correlated and matched with the point cloud from the previous time period to obtain the relative pose relationship between the current frame's point cloud and the target point cloud. This relative pose, plus the known target point cloud pose, yields the pose of the current frame's point cloud. If this correlation and matching process is not executed well, the obtained relative pose will have a large error, or even a completely wrong result in some difficult scenarios, i.e., a matching failure. Therefore, the correlation and matching process greatly affects the accuracy of the front-end output; conversely, improving the accuracy of correlation and matching plays a crucial role in improving the front-end's accuracy. Especially in some scenarios, conventional algorithms struggle with effective correlation and matching, or cannot utilize GNSS or effective loop closure for back-end optimization. In these cases, the accuracy of the front-end correlation and matching becomes critical.

[0004] There are two main categories of methods for performing association matching: 1. Non-feature-based methods; 2. Feature-based methods.

[0005] Non-feature-based methods do not extract features but instead utilize all point cloud data in the current frame for computation. Taking ICP (Intermediate Point Cloud Matching) as an example, it is a classic point cloud registration algorithm. First, an initial rotation matrix R and translation transformation T are used to rotate the source point cloud to the coordinate system of the target point cloud. Then, based on distance, the corresponding point in the target point cloud is found in the source point cloud. Next, the correspondence between points is used to solve for a better transformation that shortens the distance between corresponding points. This process is repeated until a termination condition is met. The ICP algorithm has a high computational cost. On the one hand, it uses all data points in a frame of point cloud; on the other hand, ICP repeatedly performs corresponding point searches and least-squares optimizations, both of which are computationally intensive operations. Therefore, the ICP algorithm is slow and cannot achieve real-time performance. Furthermore, it has high requirements for the initial predicted pose; if the initial pose deviation is large, the algorithm will not converge. In addition, the ICP algorithm is prone to failure in some challenging scenarios, such as long tunnels or relatively open scenes. This is because the geometric features are similar in such scenarios, and there are fewer points captured in open scenes. Therefore, the ICP algorithm is prone to errors when searching for the correspondence between laser points in two consecutive frames.

[0006] Feature-based methods extract certain features from the current point cloud and then use this feature information for association. Currently, this method is mainly used for multi-line mechanical LiDAR. Taking the LoAM series feature extraction algorithm as an example: the LoAM algorithm's front-end registration uses the current frame's point cloud to register with the previous frame's point cloud, then obtains the relative transformation relationship between the current frame and the previous frame. LoAM's feature extraction and matching algorithms have good performance, but like the ICP algorithm, its feature extraction capability still has room for improvement in challenging scenarios. This is because matching between consecutive frames relies on feature point extraction. However, for the same feature point, the curvature value changes when calculating its curvature at different locations, causing instability in feature extraction, especially in challenging scenarios. This instability significantly affects the algorithm's robustness. Summary of the Invention

[0007] In order to overcome the defects existing in the prior art, the purpose of this invention is to provide a positioning and mapping method and system based on lidar and IMU.

[0008] To achieve the above-mentioned objectives of this invention, this invention provides a localization and mapping method based on lidar and IMU, comprising the following steps:

[0009] Acquire raw point cloud data collected by lidar and IMU data collected by IMU;

[0010] The raw point cloud data and IMU data are preprocessed.

[0011] Feature points of the point cloud data of the current laser frame are extracted from the preprocessed raw point cloud data.

[0012] The feature point extraction steps are as follows:

[0013] Find the current calculation point in the current laser frame and several points around the current point. The several points around the current point include several points before and after the scan line of the current calculation point and / or several points at the same position on several scan lines adjacent to the current calculation point.

[0014] Take these points as a point set S, and calculate the covariance matrix M of the point set S;

[0015] Calculate the eigenvalues ​​of the matrix based on the covariance matrix M;

[0016] The curvature of the current calculation point is calculated based on the eigenvalue. If the curvature of the current calculation point reaches the set value, then the current calculation point is a eigenpoint.

[0017] Construct a sliding window, which corresponds to a local map composed of point cloud data and IMU data of laser frames;

[0018] An optimization function is constructed, and the feature points of the current laser frame extracted in the local map are used in the optimization function to obtain the pose output of the lidar in the current laser frame.

[0019] A 3D point cloud map is generated by stitching together the pose outputs of all laser frames and LiDAR sensors.

[0020] This localization and mapping method based on LiDAR and IMU enhances the stability of feature point extraction and improves matching robustness.

[0021] The preferred scheme of the localization and mapping method based on lidar and IMU is as follows: When finding the current calculation point and several points around the current point in the current laser frame, assuming that the current calculation point is point i, several points are selected before and after the scan line where point i is located, and these points are used as point set S1; several points with the same position on several scan lines adjacent to the current calculation point i are selected to obtain point set S2.

[0022] Then calculate the covariance matrix M1 of all points in point set S1 and the covariance matrix M2 of all points in point set S2 respectively;

[0023] Calculate the eigenvalues ​​corresponding to covariance matrices M1 and M2 respectively, and sort the eigenvalues ​​of each matrix from largest to smallest;

[0024] The horizontal curvature of the current calculation point is calculated based on the eigenvalues ​​corresponding to the covariance matrix M1, and the vertical curvature of the current calculation point is calculated based on the eigenvalues ​​corresponding to the covariance matrix M2. When both the horizontal and vertical curvatures reach their respective set values, the current calculation point is a planar feature point. When only the horizontal curvature reaches its respective set value, the calculation point is an edge feature point.

[0025] The horizontal and vertical curvature calculation methods used in this preferred scheme enhance the stability of feature extraction.

[0026] Preferably, the ratio of the largest eigenvalue to the second largest eigenvalue of the covariance matrix M is used as the curvature.

[0027] The preferred scheme for the localization and mapping method based on lidar and IMU: The calculation steps for the covariance matrix M are as follows:

[0028] Let the point set S be S = [P] i-n ,P i-n+1 ,...,P i ,...,P i+n-1 ,P i+n ], P i Let be the i-th point in point set S, where n is a positive number and 2n+1 is the number of points in point set S;

[0029] Calculate the center point of all points in the point set S, i.e., the center point.

[0030] Subtracting the center point of point set S from each point in point set S yields S'.

[0031]

[0032] The covariance matrix corresponding to the point set S is M:

[0033] The preferred scheme of the localization and mapping method based on lidar and IMU: The steps for constructing the optimization function are as follows:

[0034] Find the line corresponding to each edge feature point in the local map, and find the surface corresponding to each planar feature point;

[0035] Calculate the distance from each edge feature point to its corresponding line. The formula for calculating the distance from a point to a line is: in, Let be the distance from the i-th edge feature point to the corresponding straight line in the local map. Let the coordinates of two points on the line be... The coordinates of the i-th edge feature point. The pose to be estimated for the current laser frame;

[0036] Calculate the distance from each planar feature point to its corresponding plane. The formula for calculating the distance from a point to a plane is: Let be the distance expression from the j-th planar feature point within the current laser frame to the corresponding plane in the local map. The coordinates of the i-th planar feature point. These are the coordinates of three points on a plane in a local map.

[0037] The formula for the optimization function is: Where α and β are the number of edge feature points and planar feature points, respectively.

[0038] The preferred embodiment of the localization and mapping method based on lidar and IMU includes the following preprocessing:

[0039] IMU and LiDAR time synchronization: Find the IMU data at the corresponding time based on the LiDAR timestamp, as well as all IMU data between two LiDAR frames; if there is no corresponding IMU data at the LiDAR timestamp, calculate the corresponding IMU data using linear interpolation to achieve IMU and LiDAR time synchronization.

[0040] Point cloud distortion correction: unifying all points in a frame of LiDAR to the coordinate system of the first point in that frame of LiDAR;

[0041] IMU pre-integration: The relative motion information between two frames of LiDAR is calculated using IMU data. This serves as the initial pose for pose matching and as a pre-integration factor in sliding window optimization.

[0042] The preferred scheme of the localization and mapping method based on lidar and IMU is as follows: when extracting feature points, the 360-degree angular direction is divided into several regions according to the angle, and the same or similar number of feature points are extracted in each region.

[0043] The preferred embodiment of the localization and mapping method based on lidar and IMU is: using the sliding window to optimize the preprocessed IMU data;

[0044] During sliding window optimization, if the current laser frame meets the key laser frame condition, the current laser frame is added to the sliding window for joint optimization to obtain the key laser frame pose after sliding window optimization. At the same time, the key laser frames are used to form a local map for odometry matching.

[0045] The preferred embodiment of this localization and mapping method based on lidar and IMU also includes the following steps:

[0046] Loop closure detection: As the lidar and IMU work, the above steps are repeated. When the pose difference between the current laser frame and a previous laser frame K is less than the threshold, a loop closure is considered to be detected.

[0047] Global optimization: Construct a global optimization function, using the pose of the corresponding time of the historical key laser frame as the optimization variable, the pose of the key laser frame after sliding window optimization as the initial value of global optimization, and the loop closure detected as the constraint condition. Then, perform least squares optimization to obtain the final optimized pose of the lidar at each time. Then, stitch together the poses of the lidar at each time to form a global map.

[0048] This invention also proposes a positioning and mapping system based on lidar and IMU, characterized by comprising a lidar, an IMU, a processing module, and a storage module. The lidar acquires raw point cloud data, the IMU acquires IMU data, the lidar is connected to the processing module and sends its acquired raw point cloud data to the processing module, the IMU is connected to the processing module and sends its acquired IMU data to the processing module, the processing module is communicatively connected to the storage module, and the storage module is used to store at least one executable instruction. The executable instruction causes the processing module to perform operations corresponding to the aforementioned lidar and IMU-based positioning and mapping method based on the raw point cloud data and IMU data.

[0049] The beneficial effects of this invention are: based on this invention, it can more accurately utilize lidar and IMU for positioning and mapping. In particular, the horizontal curvature and vertical curvature calculation in this invention are independent of the observer's location and can stably reflect the geometric characteristics of the environment. Therefore, it can often achieve more robust results in challenging environments and extract more effective features, making the algorithm more effective in challenging environments, easier to converge, and less prone to divergence.

[0050] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0051] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which:

[0052] Figure 1 This is a flowchart illustrating a localization and mapping method based on lidar and IMU. Detailed Implementation

[0053] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0054] In the description of this invention, unless otherwise specified and limited, it should be noted that the terms "installation", "connection" and "linking" should be interpreted broadly. For example, they can refer to mechanical or electrical connections, or internal connections between two components. They can be direct connections or indirect connections through an intermediate medium. Those skilled in the art can understand the specific meaning of the above terms according to the specific circumstances.

[0055] like Figure 1 As shown, this invention provides an embodiment of a localization and mapping method based on lidar and IMU. This embodiment uses a multi-line lidar as the primary sensor, which can acquire accurate environmental information. The method specifically includes the following steps:

[0056] Acquire raw point cloud data from lidar and IMU data from IMU, including acceleration and angular velocity data.

[0057] Then, the original point cloud data and IMU data are preprocessed.

[0058] In this embodiment, the preprocessing mainly includes three parts: IMU and lidar time synchronization processing, point cloud distortion correction processing, and IMU pre-integration processing.

[0059] IMU and LiDAR time synchronization: Find the IMU data at the corresponding time based on the LiDAR timestamp, as well as all IMU data between two LiDAR frames; if there is no corresponding IMU data at the LiDAR timestamp, calculate the corresponding IMU data using linear interpolation to achieve IMU and LiDAR time synchronization.

[0060] Point cloud distortion correction: Because the points in a LiDAR frame are affected by the LiDAR's own movement within a given time frame, distortion correction is required. Specifically, all points in a LiDAR frame are unified to the coordinate system of the first point in that frame.

[0061] IMU pre-integration: The relative motion information between two frames of LiDAR is calculated using IMU data. On the one hand, it serves as the initial pose for pose matching, and on the other hand, it serves as a pre-integration factor in sliding window optimization. For details on how it participates in sliding window optimization, please refer to existing well-known technologies, such as lio-mapping and lio-sam, which are all used in these schemes.

[0062] Feature point extraction: Extract feature points from the point cloud data of the current laser frame based on the preprocessed raw point cloud data.

[0063] In this embodiment, the specific steps for extracting feature points are as follows:

[0064] Find the current calculation point and several points around the current calculation point in the current laser frame; then take these points as a point set S and calculate the covariance matrix M of the point set S; calculate the eigenvalues ​​and eigenvectors of the matrix based on the covariance matrix M, and sort them in descending order of eigenvalues; calculate the curvature of the current calculation point based on the eigenvalues. If the curvature of the current calculation point reaches a set value, then the current calculation point is a feature point.

[0065] Specifically, when finding the current calculation point and several points around the current point in the current laser frame, assuming the current calculation point is point i, several points are selected before and after the scan line where point i is located, and these points are used as point set S1; several points with the same position on several scan lines adjacent to the current calculation point i are selected to obtain point set S2. Here, the several scan lines are preferably, but not limited to, three scan lines above and below the current calculation point, and the several points are preferably, but not limited to, six points. Preferably, one point is taken from each of the above six scan lines. If six points cannot be taken, at least three points are taken above or below the current point.

[0066] The steps for calculating the covariance matrix M are as follows: Let the point set S be S = [P λ-n ,P λ-n+1 ,...,P λ ,...,P λ+n-1 ,P λ+n ], P λ Let λ be the λ-th point in the point set S, where n is a positive number and 2n+1 is the number of points in the point set S. First, calculate the center point of all points in the point set S, i.e., the center point. Then, subtract the center point of point set S from each point in the point set S to obtain S'. Finally, the covariance matrix corresponding to the point set S is M: Since we have point sets S1 and S2, we calculate the covariance matrix M1 of all points in point set S1 and the covariance matrix M2 of all points in point set S2, respectively. When calculating the covariance matrix M1 of all points in point set S1, we use point set S1; when calculating the covariance matrix M2 of all points in point set S2, we use point set S2.

[0067] Calculate the eigenvalues ​​and eigenvectors of covariance matrices M1 and M2 respectively, and sort the eigenvalues ​​of each matrix from largest to smallest. The specific method for calculating eigenvalues ​​and eigenvectors is SVD matrix decomposition, which will not be explained in detail here. The three eigenvalues ​​[γ1, γ2, γ3] and the corresponding eigenvectors [υ1, υ2, υ3] of covariance matrices M1 and M2 respectively are obtained.

[0068] The magnitude of the eigenvalue reflects the distribution of points in the direction of the eigenvector within a local region of the point set. If the eigenvalue is large, it indicates that the points are more dispersed in that direction. Conversely, if the eigenvalue is small, the points are more concentrated in the corresponding direction. Therefore, the magnitude of the eigenvalue reflects the local geometric characteristics of the point set.

[0069] Therefore, the horizontal curvature of the current calculation point is calculated based on the eigenvalues ​​corresponding to the covariance matrix M1. Calculate the vertical curvature of the current calculation point based on the eigenvalues ​​corresponding to the covariance matrix M2.

[0070] When both the horizontal and vertical curvatures reach their respective set values, the current calculation point is a planar feature point; when only the horizontal curvature reaches its respective set value, the calculation point is an edge feature point.

[0071] When extracting feature points, to ensure that the selected feature points are distributed as evenly as possible along the 360-degree direction, the 360-degree angle is divided into several regions according to the angle, and the same or similar number of feature points are extracted in each region. For example, the 360-degree angle is divided into 6 regions, with each region being 60 degrees. In each region, a maximum of 30 edge feature points and 100 planar feature points can be selected.

[0072] In actual calculations, a frame of point cloud can be considered as arranged sequentially, from the first point to the last point. When extracting feature points, it is not necessary to calculate the covariance matrix for all points. For example, when calculating the point set of 5 points to the left and right of each point, the first 5 points do not have 5 points to their left, so the covariance matrix is ​​not calculated for the first 5 points. The calculation starts from the 6th point. Similarly, the covariance matrix is ​​not calculated for the last 5 points.

[0073] sliding window

[0074] Similar to the lio-mapping project, a sliding window with a width of q laser frames is constructed. The laser data of the q laser frames corresponding to this sliding window constitute a local map for pose matching. Here, the laser data refers to the preprocessed point cloud data and the IMU data corresponding to each frame of point cloud data within the sliding window. Knowing the pose allows the point cloud to be represented uniformly in the world coordinate system. The method for generating the local map is an existing method and will not be elaborated here. The sliding window constructs a nonlinear optimization problem with optimization factors including an edge-triggered factor, a laser odometry factor, and a pre-integration factor. Each time a new laser frame enters the window, the oldest data frame is edge-triggered, and then an optimization is performed to obtain the optimized pose corresponding to q laser data frames. Therefore, this sliding window is used to perform sliding window optimization on the preprocessed IMU data.

[0075] In this embodiment, during sliding window optimization, if the current laser frame meets the key laser frame condition, it is added to the sliding window for joint optimization. The key laser frame pose after sliding window optimization is output to the global optimization module. Simultaneously, the sliding window laser frames form a local map for odometry matching. The condition for determining whether the current laser frame meets the key laser frame condition is that if the pose of the current laser frame differs from the pose of the most recent key laser frame by more than a set range, the current laser frame is considered a key laser frame. The first key laser frame is directly designated by the user or the system, for example, the first laser frame.

[0076] Pose estimation

[0077] In this embodiment, pose estimation is performed by constructing an optimization function. The steps for constructing the optimization function are as follows:

[0078] Find the line corresponding to each edge feature point in the local map, and find the surface corresponding to each planar feature point.

[0079] The method for finding the corresponding lines and surfaces in the map is the same as the method described in loam. The general process is to construct a kd-tree for the local map, then find the points closest to the current edge feature point or the points closest to the current planar feature point, and then determine whether these points in the map can be fitted into a line or a surface. If they can be fitted, then the fitted line or surface is considered to be the line or surface corresponding to the current edge feature point or planar feature point.

[0080] Calculate the distance from each edge feature point to its corresponding line. The formula for calculating the distance from a point to a line is: in, Let be the distance from the i-th edge feature point to the corresponding straight line in the local map. Let the coordinates of two points on the line be... The coordinates of the i-th edge feature point. This represents the pose to be estimated for the current laser frame. It is a variable, which is the value that the optimization function wants to solve. This value can be obtained through calculation. At the beginning of the calculation, the IMU data provides an initial value for this value before optimization for calculation.

[0081] Calculate the distance from each planar feature point to its corresponding plane. The formula for calculating the distance from a point to a plane is: Let be the distance expression from the i-th planar feature point in the current laser frame to the corresponding plane in the local map. The coordinates of the i-th planar feature point. The coordinates are three points on a plane in the local map, and · represents the dot product between vectors.

[0082] The formula for the optimization function is: Where α and β are the number of edge feature points and planar feature points, respectively, the meaning of this optimization function is to sum up the distance residuals from all edge feature points to the plane of the line and planar feature points and optimize them together.

[0083] The feature points of the current laser frame extracted from the local map are used in the optimization function to obtain the pose output of the lidar for the current laser frame, thus achieving localization.

[0084] A 3D point cloud map is generated by stitching together the pose outputs of all laser frames and LiDAR sensors.

[0085] To reduce errors and improve the accuracy of 3D point cloud maps, the following two steps can be added to the above embodiments to obtain a more accurate global map.

[0086] Loopback detection

[0087] Loop closure detection involves detecting a return to a previously reached position as the lidar moves. This process is repeated with the lidar and IMU, and a loop closure is considered detected when the pose difference between the current lidar frame and a previous lidar frame K is less than a threshold.

[0088] Specifically, loop closure detection uses a kd-tree to store the pose nodes after the sliding window edge. This kd-tree forms a pose graph. When a new frame arrives, the kd-tree is used to find a past frame whose pose difference from the current frame is less than a threshold. If a loop closure constraint is found, a loop closure constraint is established by mapping the laser frame K to the corresponding lidar pose of the current laser frame and outputting this constraint to the global optimization module.

[0089] Global optimization

[0090] Global optimization does not run in real time. In the aforementioned steps, the system saves the key laser frame poses after sliding window optimization, the corresponding point cloud data, and the detected loop closure information. If needed, the user can trigger global optimization offline. The main consideration of this step is to use loop closure constraints to reduce the cumulative error of the system. Because global optimization involves a large amount of data, it is not performed in real time, but is triggered offline to ensure that the system can run in real time and obtain the final globally optimized map.

[0091] The global optimization constraints consist of two parts. The first part is the relative pose constraints of adjacent key laser frames, which are composed of historical key laser frames optimized by the sliding window module. The second part is the loop closure constraints found through loop closure detection.

[0092] The global optimization module accepts the pose of key laser frames and loop closure constraints, performs global consistency optimization, and finally stitches together and outputs a global map.

[0093] The steps for global optimization are: construct a global optimization function. The optimization variable is the pose of the key laser frames at the corresponding time points in the historical calculations. The key laser frame poses output after sliding window optimization are used as the initial values ​​for global optimization. Loop closures detected are used as constraints. Then, least squares optimization is performed to obtain the final optimized poses of the LiDAR at each time point. The poses of the LiDAR at each time point are stitched together to form a global map, where r ρ，σ The residual constraint for adjacent laser frames represents the relative pose of two adjacent frames. H is the set of all laser frames, and ρ and σ are two adjacent key laser frames. The loop closure detection residual represents the relative pose between the two frames that constitute the loop, and L is the set of all loop closure frames. τ and τ are the two frames that form a loop.

[0094] This application also proposes an embodiment of a positioning and mapping system based on lidar and IMU. The system in this embodiment includes a lidar, an IMU, a processing module, and a storage module. The lidar acquires raw point cloud data, and the IMU acquires IMU data. The lidar is connected to the processing module and sends its acquired raw point cloud data to the processing module. The IMU is also connected to the processing module and sends its acquired IMU data to the processing module. The processing module is communicatively connected to the storage module, which stores at least one executable instruction. This executable instruction causes the processing module to perform operations corresponding to the lidar and IMU-based positioning and mapping method described above, based on the raw point cloud data and the IMU data.

[0095] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0096] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A method for localization and mapping based on lidar and IMU, characterized in that, The method comprises the following steps: acquiring original point cloud data collected by a laser radar and IMU data collected by an IMU; performing data preprocessing on the original point cloud data and the IMU data; extracting feature points of point cloud data of a current laser frame according to the preprocessed original point cloud data; the step of extracting the feature points comprises: finding a current calculation point in the current laser frame and a plurality of points around the current point, wherein the plurality of points around the current point comprise a plurality of points before and after a scanning line of the current calculation point and a plurality of points at the same position on a plurality of scanning lines adjacent to the current calculation point; assuming that the current calculation point is point i, a plurality of points before and after the scanning line of point i are selected as a point set S1; a plurality of points at the same position on a plurality of scanning lines adjacent to point i are selected as a point set S2; covariance matrices M1 of all points in the point set S1 and covariance matrices M2 of all points in the point set S2 are calculated respectively; feature values corresponding to the covariance matrices M1 and the covariance matrices M2 are calculated respectively; horizontal curvature of the current calculation point is calculated according to the feature values corresponding to the covariance matrices M1, vertical curvature of the current calculation point is calculated according to the feature values corresponding to the covariance matrices M2, when the horizontal curvature and the vertical curvature both reach corresponding set values, the current calculation point is a plane feature point, and when only the horizontal curvature reaches the corresponding set value, the calculation point is an edge feature point; a sliding window is constructed, the sliding window corresponds to point cloud data of a laser frame and IMU data, and forms a local map; an optimization function is constructed, the extracted feature points of the current laser frame are used in the optimization function for optimization in the local map, and a pose output of the laser radar of the current laser frame is obtained; a 3D point cloud map is spliced according to pose outputs of laser radars of all laser frames.

2. The method of claim 1, wherein, the step of calculating the covariance matrix M comprises: Let S be a point set , is the i-th point in the point set S, n is a positive number, and 2n+1 is the number of points in the point set S. calculating a center point of all points within the point set S, i.e. the center point ; Subtracting the center point of the point set S from each point in the point set S, we get , ， The covariance matrix corresponding to the point set S is M: .

3. The method of claim 1, wherein, a ratio of a largest feature value to a second largest feature value of the covariance matrix M is taken as curvature.

4. The lidar and IMU based localization and mapping method of claim 1, wherein, the step of constructing the optimization function comprises: a line corresponding to each edge feature point and a surface corresponding to each plane feature point are found in the local map. The distance of each edge feature point to the straight line corresponding thereto is calculated, and the calculation formula of the point-to-straight line distance is as follows: wherein, is the distance of the ith edge feature point to the straight line corresponding thereto in the local map, are the coordinates of two points on the straight line, denotes the coordinates of the ith edge feature point, is the to-be-estimated pose of the current laser frame; The distance of each planar feature point to the plane corresponding thereto is calculated, and the calculation formula of the point-to-plane distance is as follows: , is an expression of the distance of the jth planar feature point in the current laser frame to the plane corresponding thereto in the local map, denotes the coordinate of the jth planar feature point, , are the coordinates of three points on the plane in the local map; The formula of the optimization function is: wherein α and β are the number of edge feature points and plane feature points, respectively.

5. The lidar and IMU based localization and mapping method of claim 1, wherein, the preprocessing comprises: time synchronization of the IMU and the laser radar: corresponding IMU data at a corresponding moment and all IMU data between two laser radars are found according to a laser radar timestamp, and if there is no corresponding IMU data at the laser radar timestamp, corresponding IMU data at the moment is calculated by linear interpolation to realize time synchronization of the IMU and the laser radar; point cloud distortion removal: all points of a laser radar are unified to a coordinate system of a first point of the laser radar; IMU pre-integration: relative motion information between two laser radars is calculated by using the IMU data, which is used as an initial pose for pose matching and a pre-integration factor for participating in sliding window optimization in sliding window optimization.

6. The lidar and IMU based localization and mapping method of claim 1, wherein, when the feature points are extracted, an angle direction of 360 degrees is evenly divided into a plurality of regions, and the same or similar number of feature points are extracted in each region.

7. The method of claim 1, wherein: the sliding window is used to perform sliding window optimization on the preprocessed IMU data. If the current laser frame meets the key laser frame condition, the current laser frame is added to the sliding window for joint optimization to obtain the key laser frame pose after sliding window optimization, and a local map composed of the key laser frame is used for matching of the odometer.

8. The method of claim 7, wherein, Further comprising the following steps: Loop detection: the above steps are repeatedly executed as the laser radar and the IMU work, and when the pose difference between the current laser frame and a previous laser frame K is less than a threshold, it is considered that a loop is detected; Global optimization: a global optimization function is constructed, the poses of the key laser frames at the corresponding time are taken as optimization variables, the key laser frame pose after sliding window optimization is taken as the initial value of global optimization, the detected loop is taken as a constraint condition, then least square optimization is performed to obtain the optimized poses of the laser radar at each time, and the poses of the laser radar at each time are spliced to form a global map.

9. A laser-radar and IMU based localization and mapping system, characterized by The laser radar, the IMU, the processing module and the storage module are included, the laser radar acquires original point cloud data, the IMU acquires IMU data, the laser radar is connected with the processing module, and the original point cloud data acquired by the laser radar is sent to the processing module, the IMU is connected with the processing module, and the IMU data acquired by the IMU is sent to the processing module, the processing module is in communication connection with the storage module, the storage module is used for storing at least one executable instruction, and the executable instruction makes the processing module execute the operation corresponding to the positioning and mapping method based on the laser radar and the IMU according to the original point cloud data and the IMU data.

Images