Laser radar inertial odometer design method and system suitable for complex terrain environment

Abstract

The invention relates to a laser radar inertial odometer design method and system suitable for a complex terrain environment, and belongs to the technical field of robot autonomous navigation and localization, and the method comprises the following steps: S1, obtaining three-dimensional point cloud data of the environment by using laser radar equipment, and obtaining IMU data of each frame; s2, environment analysis and pretreatment; s3, IMU pre-integration processing is carried out; s4, performing scanning matching based on the GICP; and S5, generating a key frame and a map. The method overcomes the defects in the prior art, is different from a traditional odometer, can achieve balance between calculation cost and precision, carries out sub-map generation through a kNN method in a novel key frame and map management module, shows reliable state estimation performance, and provides technical reference for environment recognition and autonomous navigation of an intelligent operation robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation and positioning, and particularly to a design method and system of a lidar inertial odometer applicable to complex terrain environments for an intelligent operation robot. Background Art

[0002] Lidar Inertial Odometry (LIO) is a technology used for the positioning and navigation of unmanned systems (such as robots, autonomous driving vehicles, and drones). It combines data from a Light Detection and Ranging (LIDAR) and an Inertial Measurement Unit (IMU) to estimate the position and attitude of the system in real time. State estimation is the most fundamental element for the operation of unmanned systems such as drones and mobile robots. Algorithms based on lidar have the advantage of accurate state estimation performance and are widely used in autonomous systems. In addition, if tightly coupled with an IMU, by compensating for relative position and orientation changes in a short time, the state estimation accuracy can be greatly improved even during fast movement. Vision-based state estimation algorithms have great advantages in the drone field using stereo or monocular cameras due to their small and lightweight hardware characteristics. However, even with these advantages, vision-based state estimation algorithms have a sensitive problem, that is, feature points cannot be extracted in visually degraded environments (such as dark, foggy, and dusty scenes).

[0003] The academic field has proven that through the use of accurate depth information, lidar state estimation algorithms are more robust than vision algorithms in visually impaired environments. However, the existing lidar inertial odometers currently available have feature extraction methods that do not fully utilize lidar measurements according to different surrounding environments, thus reducing the state estimation performance. In addition, in the case of long-term state estimation, due to the accumulation of local relative attitude estimation drift over time, large inconsistencies occur on a global scale, unable to guarantee the accuracy of state estimation. At the same time, in a large environment, the number of lidar points will be excessive, and the computational cost required to estimate the relative position between two consecutive lidar frames may be very high, which will lead to the loss of lidar frames, meaning a decline in the overall positioning performance of the robot.

[0004] Based on this, there is an urgent need for a design method of a lidar odometer that can still have high precision in complex environmental terrains. Summary of the Invention

[0005] The purpose of the present invention is to provide a design method and system of a lidar inertial odometer applicable to complex terrain environments, and to design a high-precision odometer using an adaptive key frame method.

[0006] To achieve the above object, the present invention provides the following solutions:

[0007] A design method of a lidar odometer that can still have high precision in complex environmental terrains, including:

[0008] Using a lidar device to obtain three-dimensional point cloud data of the environment and obtaining each frame of IMU data;

[0009] An environment analysis and preprocessing module, after obtaining the LiDAR measurement values, defines the breadth of the surrounding environment, and changes the corresponding distance parameters for scanning to submap matching according to the breadth of the surrounding environment. For wide areas, the maximum corresponding distance parameter is set to a larger value, and for narrow areas, the maximum corresponding distance parameter is set to a smaller value, realizing global-scale position compensation in large areas and local-scale position compensation in narrow areas.

[0010] An IMU pre-integration processing module, which is used to calculate the relative position, velocity, and rotational change between the previous IMU frame and the current IMU frame through the original IMU measurement values.

[0011] A scan matching module based on GICP is used to transmit the output of the environment analysis and preprocessing algorithm to a point-based scan matching module to calculate the relative transformation of two consecutive LiDAR frames, where the scan matching module consists of scan-to-scan matching and scan-to-submap matching.

[0012] A key frame and map management module is used to extract key frames and generate submaps at the same time. Using a submap generation method based on the kNN algorithm, when the key frame generation interval is satisfied, the kNN algorithm is used to extract key frames and generate submaps at the same time. Finally, scan matching is performed between the submap and the current filtered scan for position compensation. Brief Description of the Drawings

[0013] Figure 1 The principle block diagram of the lidar inertial odometer designed by the present invention Detailed Embodiment

[0014] Next, the technical solutions in the embodiments of the present invention will be described in detail in conjunction with the drawings and embodiments.

[0015] The purpose of the present invention is to provide a design method and system of a lidar inertial odometer suitable for complex terrain environments, and a high-precision odometer is obtained based on an adaptive key frame generation algorithm.

[0016] Different from existing odometers, the present invention can achieve a balance between computational cost and accuracy. In the new key frame and map management module, submap generation is performed through the kNN method, showing reliable state estimation performance.

[0017] Design method of a lidar odometer with high precision in complex environmental terrains, comprising the following steps:

[0018] Step 1: Use a lidar device to obtain three-dimensional point cloud data of the environment and obtain each frame of IMU data;

[0019] In the real-time acquired data, the robot state X can be expressed as R T is the rotation matrix of the IMU, p T is the position vector of the IMU, v T is the velocity vector of the IMU, is the accelerometer of the IMU, is the gyroscope bias vector of the IMU.

[0020] Step 2: Environmental analysis and preprocessing

[0021] After obtaining the lidar (LiDAR) measurement data, first define the breadth of the surrounding environment. In a broad environment, even if large voxel parameters are used for LiDAR point cloud downsampling, sufficient geometric features can still be maintained. Therefore, even if the key frame generation interval is large, sparse point clouds can still be used for accurate state estimation. On the other hand, in a narrow environment, due to the existence of blind spots, the environment may change rapidly, and large voxelization parameters may distort the geometric features. Therefore, the key frame generation interval must be made more dense. At the same time, change the corresponding distance parameter used for scan-to-submap matching according to the breadth of the surrounding environment. For broad areas, the maximum corresponding distance parameter is set to a larger value because the key frame generation interval is large and the common geometric features are sufficient for a relatively long time. On the other hand, in narrow areas such as stairs and narrow corners, the maximum corresponding distance parameter is set to a smaller value because the common geometric features may be insufficient due to sudden environmental changes. Therefore, it is also necessary to define the parameters for the scan matching module according to the surrounding environment, so that the proposed method can perform global-scale position compensation in large areas and local-scale position compensation in narrow areas.

[0022] In the environmental analysis and preprocessing module algorithm, the input data are: the current lidar scan voxel size V env , maximum number of points maximum voxelization size maximum key frame spacing maximum corresponding distance criterion exponential decay values α, β, γ, and μ. The output data are: the voxelization parameter V for scan matching scan , key frame generation criterion d key , maximum corresponding distance d corr and filtered scan

[0023] Step 3: IMU Pre-integration Processing

[0024] Use the raw IMU measurement data to calculate the relative position, velocity, and rotational changes between the previous frame IMU and the current frame IMU. Define the raw IMU measurement model as follows:

[0025]

[0026]

[0027] In this model, and are the raw IMU measurement values in the B coordinate system at time t, and g is the gravity vector in the W coordinate system. and are the biases of the gyroscope and accelerometer respectively, which can be modeled as a random walk process. and are the Gaussian white noises of the gyroscope and accelerometer respectively. After receiving the IMU measurement data, use the IMU pre-integration technique to obtain the relative motion between the i-th frame and the j-th frame. The IMU pre-integration measurement values Δp ij , Δv ij and Δq ij are calculated by the following formulas:

[0028]

[0029]

[0030]

[0031] The IMU pre-integration measurement is used as an initial guess when performing scan matching based on GICP to calculate the relative transformation between two consecutive lidar frames in the scan matching module. The IMU bias is optimized by non-linear optimization with the lidar odometry obtained from the scan matching module.

[0032] Step 4: Scan Matching Based on GICP

[0033] The output of the environmental analysis and preprocessing module algorithm is transmitted to the scan matching module to calculate the relative transformation between two consecutive lidar frames, where the scan matching module is implemented using GICP and consists of scan-to-scan matching and scan-to-submap matching. Two consecutive lidar measurements and can be represented by the following probability model: where C i,t-1 and C i,t are covariance matrices. Assume there is no noise and only perfect correspondences, and the relative transformation T between * satisfies Define the Euclidean distance dT under any transformation T as follows:

[0034]

[0035]

[0036] The relative transformation between two consecutive LiDAR frames is calculated by iterative calculation using maximum likelihood estimation (MLE) as follows:

[0037]

[0038] The scan matching module includes scan-to-scan matching and scan-to-submap matching. The scan-to-scan matching calculates the relative transformation T between two consecutive LiDAR frames; when the d defined in the algorithm of the environmental analysis and preprocessing module is satisfied key Scan-to-submap matching is performed. The state estimated by the previous scan-to-submap matching is set as the reference state in the W coordinate system And the state between consecutive key frames is estimated by accumulating the results of scan-to-scan matching. When the relative transformation between the last key frame and time t is ΔT t the robot state T at time t t in the W coordinate system can be expressed as follows:

[0039]

[0040]

[0041]

[0042] where C i,L and C i,M are the covariance matrices of the LiDAR measurements of the last key frame and the submap points respectively. For scan-to-scan matching and scan-to-submap matching the error models are as follows:

[0043]

[0044]

[0045] where and are the LiDAR measurement points and submap points of the last key frame screened by the algorithm of the environmental analysis and preprocessing module respectively. When ΔT is obtained tWhen, T will be used t Optimize the IMU bias in IMU pre-integration measurement.

[0046] Step 5: Key frame and map generation

[0047] Use the sub-map generation method based on the kNN algorithm. When the key frame generation interval is satisfied, use the kNN algorithm to extract key frames, and at the same time generate sub-maps. Finally, perform scan matching between the sub-mapping and the current filtered scan for position compensation.

[0048] Invention effect

[0049] The present invention overcomes the deficiencies of the prior art. Different from traditional odometers, it can achieve a balance between computational cost and accuracy. In the new key frame and map management module, sub-map generation is performed by the kNN method, showing reliable state estimation performance, providing a technical reference for intelligent operation robot environment recognition and autonomous navigation.

[0050] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes, but is not limited to, the embodiments described in the specific implementation manners. Any other implementation manners obtained by those skilled in the art according to the technical solutions of the present invention also belong to the scope of protection of the present invention.

Claims

1. A design method and system of lidar inertial odometer applicable to complex terrain environments, which uses the adaptive keyframe theory to achieve fast and accurate state estimation of ground robots. It includes the following steps: Step 1: Use lidar equipment to obtain the three-dimensional point cloud data of the environment and obtain each frame of IMU data. Step 2: Environmental analysis and preprocessing Step 3: IMU pre-integration processing Step 4: Perform scan matching based on GICP Step 5: Generate keyframes and maps.

2. A lidar inertial odometer method using adaptive keyframe generation according to claim 1. It is characterized in that After obtaining the LiDAR measurement value, define the breadth of the surrounding environment, and change the corresponding distance parameter for scanning to submap matching according to the breadth of the surrounding environment. For wide areas, the maximum corresponding distance parameter is set to a larger value, and for narrow areas, the maximum corresponding distance parameter is set to a smaller value, so as to achieve global-scale position compensation in large areas and local-scale position compensation in narrow areas.

3. A lidar inertial odometer method using adaptive keyframe generation according to claim 1. It is characterized in that Use the original IMU measurement value to calculate the relative position, velocity and rotation change between the previous IMU frame and the current IMU frame.

4. A lidar inertial odometer method using adaptive keyframe generation according to claim 1. It is characterized in that Transmit the output of the environmental analysis and preprocessing algorithm to the point-based scan matching module to calculate the relative transformation of two consecutive LiDAR frames, where the scan matching module consists of scan-to-scan matching and scan-to-submap matching.

5. A lidar inertial odometer method using adaptive keyframe generation according to claim 1. It is characterized in that Use the submap generation method based on the kNN algorithm. When the keyframe generation interval is satisfied, use the kNN algorithm to extract keyframes and generate submaps at the same time. Finally, perform scan matching between the submap and the current filtered scan for position compensation.

Images