A robust two-stage laser inertial odometry for non-flat environments

Abstract

The present disclosure provides a robust two-stage laser inertial odometry for non-flat environments, in which the laser odometry module obtains the posture of the unmanned platform based on laser point cloud information by using coarse matching based on NDT and precise matching based on KISS-ICP; NDT solves the precise matching of rotational motion, and the KISS-ICP algorithm solves the precise matching of translational motion. The loop detection and optimization module performs loop detection and compensation on the posture output by the laser odometry module; the multi-sensor data fusion module uses a Kalman filter based on the error state to perform data fusion based on the posture output by the loop detection and optimization module and the IMU data output by the inertial sensor to obtain the final posture result of the unmanned platform at the current moment. The error amount and observation amount of the Kalman filter are added to the speed error of the unmanned platform. The use of the present invention can improve the accuracy and robustness of posture estimation in non-flat environments.

Description

Technical Field

[0001] The present invention relates to the field of positioning, navigation and pose estimation of autonomous driving, and in particular to a robust two-stage laser inertial odometry for non-flat environments. Background Art

[0002] When it comes to autonomous unmanned platforms, pose estimation technology is particularly critical. Laser odometry-based pose estimation technology is gaining popularity due to its high precision, strong reliability, and independence from external lighting conditions. Its core concept is to perform 3D point cloud matching, correlating the data between two frames of point clouds to obtain the correct point cloud transformation relationship. Currently, laser odometry-based pose estimation methods have achieved good results in scenarios with gentle terrain, such as urban roads and highways. This is because when a vehicle is driving on gentle terrain, the point cloud obtained by laser radar scanning includes information such as normal trees and buildings, as well as ground information, and contains a lot of useful information.

[0003] However, in non-flat environments such as unpaved roads in the wild and stairs in buildings, the pitch or roll angle of the vehicle is large. For example, the field of view of the lidar is often significantly offset from that during normal driving. Due to the 30° vertical field of view limit of the lidar, useless ground information or blank areas without point clouds corresponding to the change of direction at the pitch angle (roll angle) increases, while useful scene information such as buildings decreases, resulting in sparser point clouds in non-flat scenes compared to scenes with flat terrain. Currently, mainstream pose estimation methods based on laser odometry often require feature extraction from point clouds. When the two frames of point clouds are relatively sparse, feature-based methods find it difficult to extract enough features and generate incorrect data associations during the feature matching process, ultimately leading to increased errors and even the risk of operational failure. Therefore, existing pose estimation methods based on laser odometry suffer from low accuracy and poor robustness in non-flat scenes. Summary of the Invention

[0004] In view of this, the present invention provides a robust two-stage laser inertial odometry for non-flat environments, which can improve the accuracy and robustness of pose estimation in non-flat environments.

[0005] In order to solve the above technical problems, the present invention is implemented as follows.

[0006] A robust two-stage laser inertial odometry for uneven environments, deployed on an unmanned platform equipped with laser and inertial sensors, includes a laser odometry module combining coarse and fine matching, a loop detection and optimization module, and a multi-sensor data fusion module.

[0007] The laser odometry module is used to obtain the first position of the unmanned platform by using coarse matching and precise matching based on the laser point cloud obtained by laser sensor scanning. t , sent to the loop detection and optimization module;

[0008] The rough matching adopts the point cloud registration algorithm based on normal distribution transformation (NDT) to solve the precise matching of rotational motion; the precise matching uses the matching result of the NDT algorithm as the initial value and adopts the KISS-ICP algorithm to solve the precise matching of translational motion;

[0009] The loop detection and optimization module is used to perform loop detection and compensation on the first pose output by the laser odometry module, and output the processed second pose of the unmanned platform to the multi-sensor data fusion module;

[0010] The multi-sensor data fusion module is used to time-align the second pose from the loop detection and optimization module with the IMU data from the inertial sensor, and use an error-state-based Kalman filter to fuse the data at the same moment to obtain the final pose result of the unmanned platform at the current moment. The error amount and observation amount of the error-state-based Kalman filter are added to the error of the unmanned platform to limit the speed estimate from exceeding the actual speed upper limit of the unmanned platform.

[0011] Preferably, the laser odometry module includes a uniform motion prediction model, a point cloud correction and downsampling module, an NDT coarse matching module, and a KISS-ICP fine matching module;

[0012] The uniform motion prediction model is used to obtain the pose estimation value T according to the input laser point cloud. pred,t ;

[0013] The point cloud correction and downsampling module is used to use the pose estimation value T pred,t The laser point cloud is deflected, then downsampled twice and output to the NDT coarse matching module;

[0014] NDT rough matching module is used to obtain the rough matching pose T using the NDT algorithm ndt ;

[0015] KISS-ICP fine matching module is used to utilize the coarse matching pose T ndt As the initial pose of the KISS-ICP algorithm, fine matching is performed on the point cloud of the same frame to obtain the pose increment ΔT kiss-icp , and finally get the matching result T of the laser odometry module t =ΔT kiss-icp ·T ndt .

[0016] Preferably, the loop detection and optimization module uses Scan Context loop detection based on point cloud descriptors to perform closed loop detection; if a loop is detected between the current frame and the historical frame, the relative pose ΔT between the two frames is calculated. ij As the loop constraint factor is passed into the pose factor graph, the pose of the current frame point cloud matching result is optimized and the optimized result is The second pose is output to the multi-sensor data fusion module, and the local point cloud map M is constructed. local , and is retransmitted to the NDT coarse matching module and the KISS-ICP fine matching module as the target point cloud for subsequent frame matching; if no loop is detected, the pose information received from the laser odometry module is directly output to the multi-sensor data fusion module as the second pose, and the optimization step is not performed.

[0017] Preferably, the multi-sensor data fusion module uses the error amount of the posture information as a state quantity to construct the Kalman filter based on the error state; the state quantity includes the position error, velocity error, attitude error, and zero bias error of the accelerometer and gyroscope of the unmanned platform;

[0018]

[0019] Among them, δx t is the state quantity of the filter, δp t is the position error of the unmanned platform, δv t is the speed error of the unmanned platform, δθ t is the attitude error of the unmanned platform, δb α(t) and δb ω(t) They represent the error of the accelerometer and gyroscope zero bias noise in the inertial sensor at time t; w is the process noise, n α is the accelerometer white noise, n ω is the gyroscope white noise, b α(t) and b ω(t) They represent the zero bias noise corresponding to the accelerometer and gyroscope at time t respectively;

[0020] The state equation of the filter is derived based on the differential equation of the error:

[0021]

[0022]

[0023]

[0024]

[0025]

[0026] Among them, F t represents the process noise state transfer matrix, B t represents the process noise state transition matrix, Represents the coordinate transformation matrix from the carrier coordinate system to the local navigation coordinate system; × represents the antisymmetric matrix; and Represent the measurement values ​​of the accelerometer and gyroscope at time t, and represents the acceleration and angular velocity after removing the zero bias noise at time t;

[0027] The observation equation is:

[0028]

[0029] Among them, y t is the observed quantity; are the observed values ​​of position error, velocity error, and attitude error of the unmanned platform respectively;

[0030] Using the state equation and observation equation based on the error quantity, the basic equation of the Kalman filter ESKF based on the error state is constructed. The discretized state equation and the observation equation are substituted into the basic equation of the ESKF filter to obtain the complete ESKF.

[0031] The IMU data is used to perform inertial calculation to obtain the estimated values ​​of the current position, speed and attitude information as the IMU settlement result. The speed observation value is calculated based on the position observation value output by the laser odometer module (1) and the timestamps of two adjacent frames of data. The difference between the position observation value, speed observation value and attitude observation value and the IMU solution result is taken as δp t , δv, δθ, perform ESKF update; obtain the final pose T through ESKF iterative solution curr .

[0032] Preferably, the method further comprises: when performing ESKF updating, if there is no laser odometer observation data, skipping the updating step and directly using the prior information of the state quantity as the posterior information.

[0033] Beneficial effects:

[0034] (1) The present invention first designs a two-stage matching algorithm for non-flat environments, using NDT for coarse matching and KISS-ICP for fine matching. The KISS-ICP algorithm often requires a good initial pose value to obtain an accurate matching result, and its matching accuracy for translational motion is higher; while the NDT algorithm does not have high requirements for the initial pose, and the NDT algorithm has higher matching accuracy and better robustness for rotational motion. Using its matching result as the initial value of KISS-ICP can significantly improve the robustness of the KISS-ICP algorithm in non-flat environments. By combining the NDT and KISS-ICP algorithms in a combination of coarse matching and fine matching, a simple and accurate pure laser odometer can be constructed.

[0035] (2) When constructing the Kalman filter, the present invention uses the error quantity as the state quantity, and adds the speed error of the unmanned platform to the state quantity and the observation quantity to limit the speed estimate value from exceeding the actual speed upper limit of the unmanned platform, thereby ensuring the validity of the Kalman filter result. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 Schematic diagram of the laser inertial odometer

[0037] Figure 2 Schematic diagram of the location of unmanned system sensors. DETAILED DESCRIPTION

[0038] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0039] The present invention provides a robust two-stage laser inertial odometry for non-flat environments, such as Figure 1 As shown in FIG, it includes a laser odometer module 1 that combines coarse matching and precise matching, a loop detection and optimization module 2, and a multi-sensor data fusion module 3. The odometer can be deployed on an unmanned platform equipped with a laser sensor A and an inertial sensor B, as shown in FIG. Figure 2 shown.

[0040] When an unmanned platform equipped with a laser inertial odometry is driving, laser sensor A scans and obtains laser point cloud information, while inertial sensor B provides feedback on the unmanned platform's real-time acceleration and attitude angle.

[0041] The laser odometry module 1 obtains laser point cloud information based on the laser sensor scanning, and uses a combination of coarse matching and precise matching to obtain a set of unmanned platform postures T at the current moment. t, and sent to loop detection and optimization module 2. This invention employs a two-stage coarse-fine matching approach for this laser odometry module 1. The coarse matching approach uses a point cloud registration algorithm based on normal distribution transform (NDT) to achieve precise matching in rotational motion, using the NDT algorithm matching results as the initial value for exact matching. The exact matching approach uses the KISS-ICP algorithm to achieve precise matching in translational motion. The combination of these two matching algorithms can solve the problem of precise matching in both rotational and translational directions.

[0042] The loop detection and optimization module 2 performs loop detection and compensation on the pose information output by the laser odometry module 1 and outputs the processed pose of the unmanned platform to the multi-sensor data fusion module 3. The loop detection and optimization module 2 detects in real time whether the unmanned platform has passed through a previously passed location. If a loop is present, loop compensation is performed on the pose information output by the laser odometry module 1. If no loop is present, no compensation is performed and the pose result of the laser odometry module 1 is directly output.

[0043] The multi-sensor data fusion module 3 uses the timestamp to time-align the unmanned platform posture from the loop detection and optimization module 2 and the IMU data from the inertial sensor, and then inputs the laser odometer posture and IMU data at the same moment into the multi-sensor data fusion module 3; for the data at the same moment, the error-state Kalman filter (ESKF) is used to perform data fusion to obtain the final posture result of the unmanned platform at the current moment. The present invention improves the Kalman filter and adds the speed error δv of the unmanned platform to the error amount. t , to limit the speed estimate from exceeding the actual speed limit of the unmanned platform.

[0044] The specific implementation of each module is described in detail below.

[0045] The laser odometry module 1 includes a uniform motion prediction model, a point cloud correction and downsampling module, an NDT coarse matching module, and a KISS-ICP fine matching module.

[0046] The uniform motion prediction model is based on the input laser point cloud information P src , get the pose estimate T pred,t .

[0047] Point cloud correction and downsampling module, used to use the pose estimate T pred,t Point cloud P src Perform motion correction; point cloud P after correction * Downsampling is performed twice to reduce the computational complexity of the point cloud matching algorithm and minimize the impact of scanning error points, and then the data is output to the NDT coarse matching module.

[0048] NDT rough matching module is used to obtain the rough matching pose T using the NDT algorithm ndt .

[0049] KISS-ICP fine matching module, used to use the rough matching result T ndt As the initial pose of the KISS-ICP algorithm, fine matching is performed on the point cloud of the same frame to obtain the pose increment ΔT kiss-icp , and finally obtain the matching result T of laser odometry module 1 t =ΔT kiss-icp ·T ndt .

[0050] This paper selects the NDT algorithm as the coarse matching algorithm and KISS-ICP as the fine matching algorithm. This is because the ICP algorithm and its variants often require a good initial pose value to obtain accurate matching results, and they have higher matching accuracy for translational motion. However, the NDT algorithm has a more relaxed initial pose requirement and offers higher matching accuracy and better robustness for rotational motion. Using these matching results as the initial values ​​for KISS-ICP can significantly improve the robustness of the KISS-ICP algorithm in non-flat environments. By combining the NDT and KISS-ICP algorithms through coarse and fine matching, a simple and accurate pure laser odometry can be constructed.

[0051] Loop detection and optimization module 2 uses the Scan Context loop detection method based on point cloud descriptors for closed loop detection. If a loop is detected between the current frame and the historical frame, the relative pose ΔT between the two frames is calculated. ij As the loop constraint factor is passed into the pose factor graph, the pose of the current frame point cloud matching result is optimized and the optimized result is The obtained unmanned platform posture is output to the multi-sensor data fusion module 3, and the optimization result is used Build a local point cloud map M local ; The local point cloud map M local As the target point cloud of the subsequent frame, it is re-input into the NDT coarse matching module and KISS-ICP fine matching module of the laser odometry module 1. If no loop is detected, the pose provided by the laser odometry module 1 is directly output without performing the optimization step.

[0052] In scenarios without closed loops, loop closure detection and back-end optimization cannot be used to optimize the front-end odometry. Furthermore, in uneven environments, due to the unmanned platform's own violent movements, the point cloud information input by the lidar may be subject to significant motion distortion, resulting in unreliable data. Therefore, to further improve the robustness of the system, it is necessary to integrate data from other sensors.

[0053] Multi-sensor data fusion module 3 uses an error-state Kalman filter (ESKF) as its multi-sensor data fusion module. This filter uses the errors in the unmanned platform's position, velocity, and attitude as the system's state variables. The filter's state equation can be derived from the differential equations for the errors, which include position error, velocity error, attitude error, and the bias errors of the accelerometer and gyroscope in the IMU.

[0054]

[0055] Among them, δx t is the state quantity of the filter, δp t is the position error of the unmanned platform, δv t is the speed error of the unmanned platform, δθ t is the attitude error of the unmanned platform, δb α(t) and δb ω(t) They represent the error of the accelerometer and gyroscope zero bias noise in the inertial sensor at time t; w is the process noise, n α is the accelerometer white noise, n ω is the gyroscope white noise, b α(t) and b ω(t) They represent the zero bias noise corresponding to the accelerometer and gyroscope at time t respectively.

[0056] Rewrite the error equation in the general form of the state equation:

[0057]

[0058]

[0059]

[0060]

[0061]

[0062] Among them, F t represents the process noise state transfer matrix, B t represents the process noise state transition matrix, represents the coordinate transformation matrix from the carrier coordinate system (b system) to the local navigation coordinate system (n system); × represents the antisymmetric matrix; and Represent the measurement values ​​of the accelerometer and gyroscope at time t, and represents the acceleration and angular velocity after removing the zero bias noise at time t;

[0063] Compared with other methods using Kalman filters, the observation quantity used in this invention is the position error Speed ​​error and misalignment angle This is because if the speed error is not taken into account The velocity estimate of the ESKF may exceed the actual velocity limit of the unmanned platform, resulting in a large error in the pose estimation structure. The observation quantity is expressed as follows:

[0064]

[0065] Using the state equation and observation equation based on the error quantity, the basic equation of ESKF can be obtained. The discretized state equation and the observation equation are brought into the basic equation of Kalman filtering to obtain the complete ESKF

[0066]

[0067] Among them, ∨ represents the predicted state of the filter, and ∧ represents the posterior state of the filter. The subscript k represents the kth moment, P k represents the state transition covariance matrix, G k represents the observation vector state transfer matrix, C k represents the observation noise matrix, Q k represents the noise covariance matrix, K k represents the Kalman gain, R k represents the observation vector covariance matrix, B k-1 represents the system noise matrix.

[0068] During the actual operation, first, the ESKF is initialized, and the state vector at the initial moment is set to the zero vector, and the covariance matrix of the initial state, the system process noise and the observation noise are given; and the carrier posture is initialized at the initial moment. The second step is the IMU inertial solution. According to the posture state of the previous moment, the estimated value of the current position, speed and attitude information is obtained by solving the IMU data at the current moment, which is used as the IMU settlement result. The third step is the prediction step of the ESKF. When there is laser odometer observation data, the speed observation value is calculated according to the position observation value output by the laser odometer module 1 and the timestamps of the two adjacent frames of data. The difference between the position observation value, speed observation value and attitude observation value and the IMU solution result is used as the observation vector to update the ESKF; if there is no odometer data, the update step is skipped, and the prior information of the state quantity is directly used as the posterior information. Finally, the state quantity is used to compensate for the posture information. After the state quantity updated by the ESKF is compensated to the IMU solution result, it can be obtained

[0069]

[0070] in, are the posterior position, posterior velocity and posterior attitude matrices respectively. are the prior position, prior velocity and prior attitude matrices respectively. They are the a posteriori position error, the a posteriori velocity error and the a posteriori attitude error respectively. are the a posteriori accelerometer bias noise and the a posteriori gyroscope bias noise, respectively. are the prior accelerometer bias noise and the prior gyroscope bias noise, respectively. are the a posteriori accelerometer bias noise error and the a posteriori gyroscope bias noise error, respectively.

[0071] After the compensation is completed, the state quantity is reset to 0, and the posterior variance Keep it unchanged and iterate the next round of filters. As the final pose T at the current moment curr Output.

[0072] The above specific embodiments merely illustrate the design principles of the present invention. The shapes and names of the components described herein may vary and are not limiting. Therefore, those skilled in the art may modify or substitute equivalents for the technical solutions described in the above embodiments. Such modifications and substitutions, without departing from the inventive spirit and technical solutions of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A robust two-stage laser inertial odometry for non-flat environments, characterized by: Deployed on an unmanned platform equipped with a laser sensor and an inertial sensor, the laser inertial odometer comprises: a laser odometer module (1) combining coarse matching and precise matching, a loop detection and optimization module (2), and a multi-sensor data fusion module (3); Laser odometry module (1) is used to obtain the first position of the unmanned platform by using coarse matching and precise matching based on the laser point cloud obtained by laser sensor scanning. , sent to the loop detection and optimization module (2); The rough matching adopts the point cloud registration algorithm based on normal distribution transformation (NDT) to solve the precise matching of rotational motion; the precise matching uses the matching result of the NDT algorithm as the initial value and adopts the KISS-ICP algorithm to solve the precise matching of translational motion; A loop detection and optimization module (2) is used to perform loop detection and compensation on the first pose output by the laser odometer module (1), and output the processed second pose of the unmanned platform to the multi-sensor data fusion module (3); The multi-sensor data fusion module (3) is used to time-align the second posture from the loop detection and optimization module (2) and the IMU data from the inertial sensor, and use the error state-based Kalman filter to perform data fusion on the data at the same moment to obtain the final posture result of the unmanned platform at the current moment; the error amount and observation amount of the error state-based Kalman filter are added to the speed error of the unmanned platform to limit the speed estimate from exceeding the actual speed upper limit of the unmanned platform; The multi-sensor data fusion module (3) uses the error amount of the posture information as a state quantity to construct the error state-based Kalman filter; the state quantity includes the position error, velocity error, attitude error, and zero bias error of the accelerometer and gyroscope of the unmanned platform; in, is the state quantity of the filter, is the position error of the unmanned platform, is the speed error of the unmanned platform, is the attitude error of the unmanned platform, and Respectively t The error amount of the accelerometer and gyroscope zero bias noise in the inertial sensor at this moment; w is the process noise, is the accelerometer white noise, is the gyroscope white noise, and They represent the zero bias noise corresponding to the accelerometer and gyroscope at time t respectively; The state equation of the filter is derived from the differential equation of the error: in, represents the process noise state transition matrix, represents the process noise state transition matrix, Represents the coordinate transformation matrix from the carrier coordinate system to the local navigation coordinate system; × represents the antisymmetric matrix; and Respectively t The measurements of the accelerometer and gyroscope at all times, and represents the acceleration and angular velocity after removing the zero bias noise at time t; The observation equation is: in, is the observed quantity; 、 、 are the observed values ​​of position error, velocity error, and attitude error of the unmanned platform respectively; Using the state equation and observation equation based on the error quantity, the basic equation of the Kalman filter ESKF based on the error state is constructed. The discretized state equation and the observation equation are substituted into the basic equation of the ESKF filter to obtain the complete ESKF. The IMU data is used to perform inertial calculation to obtain the estimated values ​​of the current position, speed and attitude information as the IMU settlement result. The speed observation value is calculated based on the position observation value output by the laser odometer module (1) and the timestamps of two adjacent frames of data. The difference between the position observation value, speed observation value and attitude observation value and the IMU solution result is used as 、 、 , perform ESKF update; obtain the final pose through ESKF iterative solution .

2. The robust two-stage laser inertial odometry for non-flat environments according to claim 1, wherein: The laser odometer module (1) includes a uniform motion prediction model, a point cloud correction and downsampling module, an NDT coarse matching module, and a KISS-ICP fine matching module; The uniform motion prediction model is used to obtain the pose estimation value based on the input laser point cloud ; The point cloud correction and downsampling module is used to use the pose estimation value The laser point cloud is deflected, then downsampled twice and output to the NDT coarse matching module; NDT rough matching module, used to obtain rough matching pose using NDT algorithm ; KISS-ICP fine matching module, used to utilize the coarse matching pose As the initial pose of the KISS-ICP algorithm, fine matching is performed on the point cloud of the same frame to obtain the pose increment , and finally obtain the matching result of the laser odometry module (1) .

3. The robust two-stage laser inertial odometry for non-flat environments according to claim 1, wherein: The loop detection and optimization module (2) uses Scan Context loop detection based on point cloud descriptors to perform closed loop detection; if a loop is detected between the current frame and the historical frame, the relative pose between the two frames is calculated. As the loop constraint factor is passed into the pose factor graph, the pose of the current frame point cloud matching result is optimized and the optimized result is The second pose is output to the multi-sensor data fusion module (3) and a local point cloud map is constructed. , and retransmitted to the NDT coarse matching module and the KISS-ICP fine matching module as the target point cloud for subsequent frame matching; if no loop is detected, the posture information received from the laser odometer module (1) is directly output as the second posture to the multi-sensor data fusion module (3) without performing the optimization step.

4. The robust two-stage laser inertial odometry for non-flat environments according to claim 1, wherein: The two-stage laser inertial odometry further performs the following operations: when performing ESKF update, if there is no laser odometry observation data, the update step is skipped and the prior information of the state quantity is directly used as the posterior information.

Images