Robust two-stage laser inertial odometer for non-flat environment

Abstract

The invention provides a robust two-stage laser inertial odometer for a non-flat environment, and in the odometer, a laser odometer module obtains the pose of an unmanned platform based on laser point cloud information by adopting rough matching based on NDT and accurate matching based on KISS-ICP; the NDT is used for solving the precise matching of rotary motion, and the KISS-ICP algorithm is used for solving the precise matching of translational motion. The loopback detection and optimization module is used for carrying out loopback detection and compensation on the pose output by the laser odometer module; and the multi-sensor data fusion module carries out data fusion by adopting a Kalman filter based on an error state according to the pose output by the loopback detection and optimization module and IMU data output by an inertial sensor to obtain a final pose result of the unmanned platform at the current moment. The error quantity and the observed quantity of the Kalman filter are added into the speed error of the unmanned platform. By using the method, the accuracy and robustness of pose estimation in the non-flat environment can be improved.

Description

Technical Field

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

[0002] When it comes to autonomous unmanned platforms, pose estimation technology is particularly critical. Among them, pose estimation technology based on laser odometers is becoming increasingly popular due to its high precision, strong reliability, and unaffected by external lighting conditions. Its core idea is to perform 3D point cloud matching, and obtain the correct point cloud transformation relationship by associating the data of two frames of point clouds. At present, the pose estimation method based on laser odometers has achieved good results in scenes with flat terrain such as urban roads and highways. This is because when the carrier is driving in scenes with flat terrain, the point cloud that can be obtained by laser radar scanning includes normal information such as trees and buildings, as well as ground information, and has more useful information.

[0003] However, in non-flat environments such as unpaved roads in the wild and stairs in buildings, the pitch angle or roll angle of the carrier is large. For example, the field of view of the laser radar is often greatly offset from that during normal driving. Due to the 30° vertical field of view limit of the laser radar, useless ground information or blank parts without point clouds corresponding to the change of direction at the pitch angle (roll angle) increases, and useful scene information such as buildings decreases, resulting in sparser point clouds in non-flat scenes compared to scenes with flat terrain. At present, mainstream pose estimation methods based on laser odometers often need to extract features from point clouds. When the two frames of point clouds are relatively sparse, feature-based methods are difficult to extract enough features, and incorrect data associations are generated during the feature matching process, which ultimately leads to increased errors and even the risk of operation failure. Therefore, the existing pose estimation methods based on laser odometers have the problems of 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 odometer 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 odometer for non-flat environments is deployed on an unmanned platform equipped with a laser sensor and an inertial sensor. The laser inertial odometer includes: a laser odometer module combining coarse matching and precise matching, a loop detection and optimization module, and a multi-sensor data fusion module;

[0007] The laser odometer module is used to obtain the first position of the unmanned platform based on the laser point cloud obtained by laser sensor scanning, using coarse matching and precise matching. 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 odometer 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 posture from the loop detection and optimization module 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 time 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 with the speed 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 odometer 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 corrected, then downsampled twice and output to the NDT coarse matching module;

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

[0015] KISS-ICP fine matching module is used to use 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 obtain the matching result T of the laser odometer 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 point cloud matching result of the current frame is optimized and the optimized result is The second pose is output to the multi-sensor data fusion module, and a 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 posture information received from the laser odometer module is directly output to the multi-sensor data fusion module as the second posture, and the optimization link 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 from 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 transfer matrix, represents the coordinate transformation matrix from the carrier coordinate system to the local navigation coordinate system; × represents the antisymmetric matrix; and Respectively 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 state equation is discretized and brought into the basic equation of the ESKF filter with the observation equation 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 estimation 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 The overall 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 odometer for non-flat environments, such as Figure 1 As shown in FIG. 1 , it includes 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. The odometer can be deployed on an unmanned platform equipped with a laser sensor A and an inertial sensor B, such as Figure 2 shown.

[0040] When an unmanned platform equipped with a laser inertial odometer is driving, laser sensor A scans to obtain laser point cloud information, and inertial sensor B feeds back the real-time acceleration and attitude angle of the unmanned platform itself.

[0041] The laser odometer 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, sent to loop detection and optimization module 2. The present invention adopts coarse and fine two-stage matching for the laser odometer module 1. The coarse matching adopts a point cloud registration algorithm based on normal distribution transformation NDT to solve the precise matching of rotational motion, and uses the matching result of the NDT algorithm as the initial value of the precise matching; the precise matching adopts the KISS-ICP algorithm to solve the precise matching of translational motion. The combination of these two matching algorithms can solve the problem of precise matching in both rotation and translation directions.

[0042] The loop detection and optimization module 2 performs loop detection and compensation on the pose information output by the laser odometer 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 the previous path position. If there is a loop, the pose information output by the laser odometer module 1 is loop compensated. If there is no loop, no compensation is performed and the pose result of the laser odometer 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 (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 method of each module is described in detail below.

[0045] 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.

[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; the point cloud P after correction * Downsampling is performed twice to reduce the computational complexity of the point cloud matching algorithm and the impact of scanning error points, and then output to the NDT coarse matching module.

[0048] NDT rough matching module, used to obtain rough matching pose T using 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 odometer module 1 t =ΔT kiss-icp ·T ndt .

[0050] The present invention selects the NDT algorithm as the coarse matching algorithm and KISS-ICP as the precise matching algorithm. The reason for this is that the ICP algorithm and its variants often require a good initial value of posture to obtain accurate matching results, and its matching accuracy for translational motion is higher; while the NDT algorithm does not have high requirements for the initial posture, 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 coarse matching and precise matching manner, a simple and accurate pure laser odometer 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 point cloud matching result of the current frame 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 Construct 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 odometer module 1. If no loop is detected, the pose provided by the laser odometer module 1 is directly output without performing the optimization step.

[0052] In the absence of a closed loop, loop detection and back-end optimization cannot be used to optimize the front-end odometer. In addition, in a non-flat environment, due to the violent movement of the unmanned platform itself, the point cloud information input by the lidar may have large motion distortion, which leads to unreliable data. Therefore, in order to further improve the robustness of the system, it is necessary to integrate data from other sensors.

[0053] The multi-sensor data fusion module 3 uses an error-state Kalman filter (ESKF) as a multi-sensor data fusion module. The filter uses the error of the position, velocity and attitude of the unmanned platform as the state of the system. The state equation of the filter can be derived from the differential equation of the error, where the state includes the position error, velocity error, attitude error, and the zero bias error 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] Rewriting the error equation into 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 transfer 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 Respectively 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 upper limit of the unmanned platform, resulting in a large error in the pose estimation structure. The observation expression method is 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 state equation is discretized and brought into the basic equation of Kalman filtering with the observation equation 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 values ​​of the current position, speed and attitude information are 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, They are the posterior position, posterior velocity and posterior attitude matrices respectively. They 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. They 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. They 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 only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. A robust two-stage laser inertial odometer for non-flat environments, characterized in that: 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); The laser odometer 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. t , 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 posture output by the laser odometer module (1), and output the processed second posture 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 a Kalman filter based on the error state to perform data fusion on the data at the same time to obtain the final posture result of the unmanned platform at the current time; the error amount and observation amount of the Kalman filter based on the error state are added to the speed error of the unmanned platform to limit the speed estimation value from exceeding the actual speed upper limit of the unmanned platform.

2. The robust two-stage laser inertial odometer for non-flat environments as claimed in claim 1, characterized in that The laser odometer module (1) comprises 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 T according to the input laser point cloud. pred,t ; The point cloud correction and downsampling module is used to use the pose estimation value T pred,t The laser point cloud is corrected, then downsampled twice and output to the NDT coarse matching module; NDT rough matching module, used to obtain rough matching pose T using NDT algorithm ndt ; KISS-ICP fine matching module is used to use 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 Finally, the matching result T of the laser odometer module (1) is obtained t =ΔT kiss-icp ·T ndt .

3. The robust two-stage laser inertial odometer for non-flat environments as claimed in claim 1, characterized in that 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 position ΔT between the two frames is calculated. ij As the loop constraint factor is passed into the pose factor graph, the pose of the point cloud matching result of the current frame 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 M is constructed. local , 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 odometer for non-flat environments as claimed in claim 1, characterized in that The multi-sensor data fusion module (3) uses the error amount of the position and posture information as a state amount to construct the Kalman filter based on the error state; the state amount includes the position error, velocity error, attitude error, and zero bias error of the accelerometer and gyroscope of the unmanned platform; 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; The state equation of the filter is derived from the differential equation of the error: Among them, F t represents the process noise state transfer matrix, B t represents the process noise state transfer matrix, represents the coordinate transformation matrix from the carrier coordinate system to the local navigation coordinate system; × represents the antisymmetric matrix; and Respectively 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; The observation equation is: 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; 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 state equation is discretized and brought into the basic equation of the ESKF filter with the observation equation 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 T through ESKF iterative solution curr .

5. The robust two-stage laser inertial odometer for non-flat environments as claimed in claim 4, characterized in that The method further includes: 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.

Images