Positionable sensing laser radar inertial odometer for challenging scene

Abstract

The invention belongs to the technical field of laser sensor positioning, and discloses a laser radar inertial odometer capable of realizing positioning perception for challenging scenes. And the robustness of the system in a challenging scene is improved. The laser radar inertial odometer mainly comprises three parts, namely a degradation detector, a degradation processing subsystem and a high-frequency odometer subsystem, the segmentation of the laser radar point cloud alleviates the problem of excessive distortion of the point cloud in a strenuous motion scene. The laser radar degradation detection can realize stable degradation judgment. According to the ESKF method adaptive to the weight of each point, existing space information is utilized as much as possible, and the stability of the system in a degradation scene is improved. In various experiments, evaluation and comparison of the provided method prove that the performance and reliability of the scheme are improved in a challenging environment.

Description

Technical Field

[0001] The present invention relates to the field of laser sensor positioning technology, and in particular to a laser radar inertial odometer for localizability perception in challenging scenarios. Background Art

[0002] Many excellent LIO systems only discuss scenarios where their own motion is relatively slow. For example, FAST-LIO2 downsamples the original laser points, uses IMU to predict the state, and compensates for point cloud distortion. Then, iKD-tree is used to construct point-to-plane observations, and the state is updated directly in the iterative error state Kalman filter framework. LIO-SAM proposes a tightly coupled laser-inertial odometer method based on factor graph optimization, which optimizes the LiDAR odometer factor, IMU pre-integration factor, GPS factor, and loopback factor to obtain the robot's globally consistent posture. The above methods all require the use of IMU to restore motion during point cloud accumulation. There is a lot of noise in the IMU measurement. The error caused by long-term integration will inevitably affect the accuracy and robustness of the odometer, and also limit the output frequency of the laser odometer. As a result, they cannot be reliably positioned in scenarios with overly aggressive motion. Continuous-time methods such as CLINS can ignore the motion compensation error caused by aggressive motion through non-rigid registration strategies. However, this requires more control points to fit the drastically changing trajectory, which greatly affects the efficiency of the system.

[0003] In order to reduce the cumulative error caused by long-term integration of IMU, there are also many excellent works that use the method of segmenting laser point cloud data to improve robustness. LLOL divides the laser radar points and regards the rotating laser radar as a flow sensor. SR-LIO reduces the IMU pre-integration error by dividing the complete laser frame into 3 segments and then combining them to obtain a new frame. Although these two systems perform segmentation, they still update the pose of the complete point cloud frame and cannot handle more radical movements. The latest Point-LIO proposes a point-by-point pose update method, which fundamentally solves the problem of point cloud distortion, but because there are too few constraints for each update, this method is sensitive to degraded scenarios.

[0004] Regarding localizability in degraded scenarios, coastal navigation is an early attempt to model the information content (i.e., information gain) in the environment. "Censi, Andrea." On achievable accuracy for range-finder localization." Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007." proposed an information matrix method to analyze the degree of constraint in each direction by performing eigenvalue decomposition on the information matrix to evaluate localizability. "Liu, Zhe, et al." Localizability estimation for mobile robots based on probabilistic grid map and its applications to localization." 2012 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI). IEEE, 2012." This method is applied to 2D scenes, aiming to maximize the determinant of the information matrix to estimate localizability.

[0005] In the field of laser radar degradation detection, "Liu, Zhe, et al. "Localizability estimation for mobile robots based on probabilistic grid map and its applications to localization." 2012 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI). IEEE, 2012." By analyzing the optimization problem of state estimation, a new degradation factor is proposed to characterize geometric degradation, aiming to determine the direction of laser degradation, and no radar is used for updating in the degradation direction. This method can only be applied to optimization-based state estimation problems. "Zhang, Ji, et al. "On Degeneracy of Optimization-Based State Estimation Problems." 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016." By performing singular value decomposition on the normal vector, a new method is proposed to determine whether the robot can continue to produce accurate positioning results at its current position. Later, Zhen, Weikun, et al. "Robust Localization and Localizability Estimation with a Rotating Laser Scanner." 2017 IEEE International Conference on Robotics and Automation (ICRA), 2017 proposed the idea of ​​perturbation constraint to judge laser degradation. However, the degradation judgment thresholds calculated by these methods do not have a clear physical meaning, which is not convenient for application on different data sets.

[0006] After determining the degree of degradation, there are many processing methods, such as "Shan, Tixiao, et al. "LVI-SAM: Tightly-Coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping." 2021IEEE International Conference on Robotics and Automation (ICRA), 2021." A loosely coupled system between the visual system and the laser system is proposed. The system will detect the degradation of the vision and laser. When a problem occurs in one system, the system will automatically switch to the other system to ensure the stability of the system. "Zhen, Weikun, et al. "Robust Localization and Localizability Estimation with a Rotating LaserScanner." 2017IEEE International Conference on Robotics and Automation (ICRA), 2017." It is proposed to use UWB sensors to provide another effective positioning information during the laser degradation process. At present, many SLAM solutions for multi-sensor fusion positioning have degradation judgments for single sensors, but most of these methods only judge the degradation of the sensor and then abandon the information of the sensor, but some of the effective information is also discarded. Summary of the invention

[0007] The purpose of the present invention is to propose a degradation-aware laser inertial odometer that is highly robust to extreme scenarios such as degradation and violent motion.

[0008] The technical solution of the present invention is as follows: a laser radar inertial odometer for localizability perception in challenging scenarios, which determines the current constraint state of the overall odometer system through a degradation detector, and divides the overall odometer system into two subsystems according to the constraint state, namely a degradation processing subsystem and a high-frequency odometer subsystem; enters the degradation processing subsystem when insufficient constraints are detected and degradation occurs, and enters the high-frequency odometer subsystem when the constraints are good; forms an adaptive weight in the degradation processing subsystem to update the laser radar frequency state; in the high-frequency odometer subsystem, after dividing the time frame into subframes, the IMU frequency state of the subframe is updated.

[0009] The degradation detector is specifically:

[0010] Extract the z points of the local map corresponding to each point in the current frame, fit the z points of the local map into a plane, each plane has a normal vector, normalize the normal vector of the local map plane corresponding to each point in this frame, move the starting points of all plane normal vectors corresponding to this frame to the origin of the world coordinate system, and construct a constraint sphere matrix, as shown in the following formula:

[0011] N pre =[n1,n2,n3,...,n m ]

[0012] Among them, N pre Represents the constraint sphere matrix composed of the plane normal vectors corresponding to the current frame point, n i represents the plane normal vector corresponding to the i-th point, and m represents the number of plane normal vectors. For each plane normal vector constraint, take the negative number to get a new normal vector. For a plane, there are two normal vectors. The final constraint matrix N is as follows:

[0013] N=[N pre ,-N pre ]

[0014] The constraint ball matrix reflects all the constraints of this frame. The more uniform the constraint ball is, the better the constraints of this frame are. Each normalized plane normal vector is regarded as a plane point, and the principal component analysis PCA is used to perform constraint analysis on these plane points. First, the mean is subtracted from these plane points. Since each plane point is negatively counted and added to the constraint ball matrix, the mean is always zero. Then, the oblique variance matrix of these plane points is calculated. Perform eigenvalue decomposition on this covariance matrix:

[0015]

[0016] Three eigenvalues ​​{λ1, λ2, λ3} are obtained, and the eigenvalues ​​are sorted. The eigenvector corresponding to the largest eigenvalue represents the main direction of these plane points; the degenerate condition number is defined as λ min is the minimum eigenvalue, λ max is the maximum eigenvalue. The larger the degenerate condition number, the better the constraint, and the smaller the degenerate condition number, the worse the constraint.

[0017] The constraint state is determined by comparing the degradation condition number with a threshold value;

[0018] Two thresholds are defined. When the degradation condition number is less than the threshold t1, it means that the overall odometer system enters the degradation processing subsystem from the high-frequency odometer subsystem. When the degradation condition number is greater than the threshold t2, it means that the overall odometer system will enter the high-frequency odometer subsystem from the degradation processing subsystem. When t1 is less than t2 and the degradation condition number is between the threshold t1 and the threshold t2, the system state at the previous moment is still maintained to prevent the overall system from frequently switching between the two subsystems.

[0019] The z is 5.

[0020] The high-frequency odometer subsystem is specifically:

[0021] According to the IMU data, the laser radar point cloud data is segmented to obtain sub-data frames. After obtaining the sub-data frames, the ESKF algorithm is used to update the state. First, the system state quantity is defined as follows:

[0022]

[0023] in Indicates its own position in the world system, Indicates the speed of the world system. Represents the rotation in the world coordinate system, represents the acceleration of the IMU, represents the offset of the angular velocity meter, G g T represents the acceleration due to gravity;

[0024] Using the error state Kalman filter for state estimation, the new state quantity is

[0025]

[0026] First, the state transfer equation of the system error state in discrete time is:

[0027] δx k+1 =F x δx k +F i n

[0028] where F x represents the state transfer matrix of the linearized system, F i represents the control matrix of the linearized system.

[0029] Then calculate the prior error state covariance matrix as:

[0030] P k+1 =F x P k F x T +F i Qk F i T

[0031] Where P k represents the posterior error covariance matrix at the previous moment, Q k is the covariance matrix of the system noise at the previous moment.

[0032] Using the above point-to-plane distance as observation, iterative state updates are performed. In each round of state update, the point-to-plane association is re-performed, and the state is updated using the following formula:

[0033] K=(H T R -1 H+P k -1 ) -1 H T R -1

[0034]

[0035] Where H is the mapping matrix of the observation equation, R is the covariance matrix of the observation noise, is the posterior estimate, which is the current optimal estimate.

[0036] Finally, the covariance matrix of the system state is updated:

[0037] P k =(I-KH)P k

[0038] The point cloud of each subframe after each state update is accumulated, and when a complete frame is accumulated, the point cloud of the complete laser frame is sent to the degradation detector.

[0039] In order to reduce repeated point cloud dedistortion operations, the dedistorted points of the current point cloud frame are used for adaptive weight allocation in the degradation processing subsystem and sent to the degradation detector for degradation detection.

[0040] The degradation processing subsystem is specifically:

[0041] The constraint sphere matrix is ​​divided into q surface patches according to the longitude and latitude of the earth, and the area of ​​each surface patch is used as the weight value of the surface patch; the area of ​​each surface patch and the number of normal vectors in each surface patch are as follows:

[0042] S={s1,s2,s3,...,s q}

[0043] P={p1,p2,p3,...,p q}

[0044] The points in the patch divide the weight of the patch equally; the weight of each point is calculated as follows to obtain the importance of other points with respect to that point:

[0045]

[0046] The state estimation is also performed according to the ESKF algorithm. The covariance of each point is adaptively allocated through the weight of each point, so as to obtain the final covariance matrix; the calculated weight and the actual covariance are unified to the same scale; the variance of each point is obtained using the following formula:

[0047]

[0048] Among them, a represents the commonly used inverse covariance of each point, and 2m represents the number of constraints. Since each point is independent of each other, the covariance between every two points is zero, and the final R matrix is ​​a diagonal matrix with the diagonal as the variance of each point. Similarly, the ESKF algorithm is used for state estimation, and the observation covariance matrix is ​​replaced by the calculated new covariance matrix R when calculating the Kalman gain.

[0049] The beneficial effects of the present invention are as follows: by using the degradation detector to judge the current state of the system, the overall system can switch between the degradation processing subsystem and the high-frequency odometer subsystem. Finally, the system can achieve robust positioning and mapping in violent motion and degradation scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 The present invention is a flow chart of the lidar inertial odometer for localizability perception in challenging scenes.

[0051] Figure 2 Schematic diagram of degradation analysis. (a)-(c) are three points on the plane, and (d)-(f) are the corresponding Figure 2 The constraint sphere after normalization of all normal vectors in (a)-(c). The arrows in the sphere are the eigenvectors corresponding to the three eigenvalues ​​obtained by principal component analysis of these constraints.

[0052] Figure 3 This is a schematic diagram of segmenting sub-data frames. The dotted line shows a complete lidar frame and multiple data frames of the IMU in a lidar frame, and the dotted line shows the sub-data frames obtained by segmenting the lidar frame according to the IMU data frame.

[0053] Figure 4 Schematic diagram of the face division of the degradation processing subsystem; on the left, all normal vectors are divided according to the longitude and latitude curves, and on the right, the details are enlarged. The same color indicates that they belong to the same face.

[0054] Figure 5 Visualization of the point cloud map constructed by the dynamic_spinning sequence of the Newer College dataset, (a) is the map constructed by the FAST-LIO2 system, and (b) is the point cloud map constructed by the system of the present invention.

[0055] Figure 6 Maps constructed by different systems on the same recorded intense motion dataset, where (a) is FAST-LIO2, (b) is Faster-LIO, (c) is Point-LIO, and (d) is the point cloud map constructed by the present invention.

[0056] Figure 7 The open source data packets for FAST-LVIO are compared with the data packets recorded by the present invention; wherein (a1), (b1), (c1) and (d1) are respectively the point cloud maps constructed by FAST-LIO2, Faster-LIO, Point-LIO and the present invention on the laser degradation data packets of FAST-LVIO; (a2), (b2), (c2) and (d2) are respectively the point cloud maps constructed by FAST-LIO2, Faster-LIO, Point-LIO and the present invention on the data packets recorded by themselves. DETAILED DESCRIPTION

[0057] This invention introduces a degradation detection method, which aims to determine whether a complete frame of point cloud data contains sufficient information to produce accurate and reliable positioning results. The technical framework is shown in the figure Figure 1 .

[0058] First, no matter whether the high-frequency odometer subsystem or the degradation processing subsystem is used, the distance from the current frame point to the plane fitted by the local map is used as the residual to update the state. Figure 2 As shown in the figure, if the robot is slightly disturbed in a certain direction and the residual does not change much, the constraint in that direction is weak; otherwise, the constraint is strong. The residual changes along the normal vector movement the most, while the residual changes perpendicular to the normal vector movement the least (changes to 0). This shows that all normal vectors of a complete frame can reflect the constraints of the current frame. These constraints effectively limit the current position. When there are fewer or even no constraints in some directions, the information of the point cloud of the frame is not enough to estimate the accurate robot state.

[0059] In order to analyze whether the constraints of the current frame are sufficient, each normal vector of this frame is normalized, and then the starting points of all vectors are moved to the origin of the world coordinate system to construct a constraint sphere matrix, as shown in the following formula:

[0060] N pre =[n1,n2,n3,…,n m ]

[0061] Here, n1 represents the matrix composed of the normal vector of the current frame, n i Represents the ith normal vector, and m represents the number of normal vectors. It should be noted that for each constraint, each normal vector needs to be negated to obtain a new normal vector. This operation is not only beneficial for the subsequent degenerate direction analysis, but also conforms to the actual constraint situation. Because for a plane, the residual is the distance from the point to the plane, and this distance has no positive or negative sign, so a plane should have two normal vectors. Therefore, the final constraint matrix N is as follows:

[0062] N=[N pre ,-N pre ]

[0063] This constraint sphere reflects all the constraints of this frame. The more uniform the sphere is, the better the constraints of this frame are. Take each normalized normal vector as a point and use principal component analysis (PCA) to perform constraint analysis on these points. First, you need to subtract the mean from these points, but since each point is taken inversely and added to the constraint sphere, the mean is always zero. Then, calculate the oblique variance matrix of these points Then perform eigenvalue decomposition on this covariance matrix:

[0064]

[0065] Here is a matrix composed of decomposed eigenvectors. Three eigenvalues ​​{λ1, λ2, λ3} will be obtained. These eigenvalues ​​are sorted, and the eigenvector corresponding to the largest eigenvalue represents the main direction of these points. Define a degenerate condition number as Define the minimum eigenvalue λ min and the maximum eigenvalue λ max The ratio of is the degenerate condition number. The smaller the condition number, the worse the pose constraint, indicating that the point cloud information at the current position can no longer continuously produce stable pose results.

[0066] Two thresholds are defined, threshold t1 represents the threshold for entering the degradation processing subsystem from the framing subsystem, and threshold t2 represents the threshold for entering the framing subsystem from the degradation processing subsystem. In the present invention, it is considered that degradation occurs continuously, so t1 is generally set to be slightly smaller than t2, thereby ensuring that the system does not repeatedly switch between the two subsystems frequently.

[0067] A complete data frame should include an IMU frame for pose prediction and laser points for pose update. Based on the continuous acquisition characteristics of LiDAR, the original scan is regarded as a continuous point cloud data stream, and a frame scan is divided according to the IMU data to obtain subframes, thereby achieving a higher frequency of sub-scans. An IMU data and the laser point cloud within the corresponding range are regarded as a new data frame. Figure 3 The core idea of ​​segmentation scanning is explained. The lidar frame is segmented according to the interval of IMU frame.

[0068] Due to the small number of sub-frame point clouds, degradation may occur due to insufficient constraints of the sub-frame point clouds. However, this is not a big deal, because after the degradation detection of the full frame, it can be ensured that the point cloud constraints of the full frame are sufficient. Although several sub-frames may be degraded, other sub-frames can immediately provide constraint information for the state quantity.

[0069] In order to reduce repeated calculations of the degradation detector, the point cloud and corresponding normal vector of each subframe after the state update are accumulated. When a complete frame is accumulated, the normal vector of the complete frame is sent to the degradation detector.

[0070] The adaptive weight allocation method in the degradation processing subsystem can emphasize the importance of a few non-centralized constraints.

[0071] First, in the degenerate case, many normal vector constraints are in one direction. For example, if the current frame is illuminated on a relatively flat ground, then if there is a small part of the feature perpendicular to this plane, then this feature will be a very effective constraint, and the constraint of this feature needs to be magnified. Therefore, the constraint sphere is divided into multiple surface patches according to the longitude and latitude of the earth, such as Figure 4 , the area of ​​each patch is used as the weight of the patch. The area of ​​each patch and the number of normal vectors in each patch are as follows:

[0072] S={s1,s2,s3,…,s n}

[0073] P={p1,p2,p3,…,p n}

[0074] The points in the patch share the weight of the patch equally, so that some scattered points will get higher weights, while some points that are concentrated together will have their weights reduced. The weight of each point is calculated as follows. The weight here has no unit, but only expresses the importance of this point to other points:

[0075]

[0076] The ESKF method is used for state estimation. This method performs optimal state estimation by minimizing the covariance of the posterior state. The observation is the residual from each point to the plane. The covariance of the observation indicates the uncertainty of the residual. The common method now is to fix the covariance of all points. The covariance of each point is adaptively allocated through the weight obtained for each point to obtain the final covariance matrix. Since the calculated weights have no units, in order to apply the weights to the covariance, the formula is used to unify the calculated weights and the actual covariance to the same scale. Use the following formula to get the covariance of each point:

[0077]

[0078] Here a represents the inverse of the covariance of each point, 2m represents the number of constraints, and represents the covariance of each point. Since each point is independent of each other, the covariance between each two points is zero, and the R matrix is ​​finally obtained as a diagonal matrix with a diagonal of. The ESKF method is used for state estimation. It is only necessary to replace the observation covariance matrix with the new covariance matrix when calculating the Kalman gain in the following formula:

[0079] K=(H T R -1 H+P -1 ) -1 H T R -1

[0080] The following is a specific application example of the odometer of the present invention.

[0081] Step 1. System operating environment: ROS (Robot Operating System): select the appropriate ROS distribution according to the Ubuntu version, Eigen: an advanced C++ library for linear algebra, matrix and vector operations, and related mathematical operations, Ceres Solver: an open source C++ library for modeling and solving large and complex nonlinear minimization problems, glog: Google's log library, Ceres uses it for logging, gflags: Google's flag library for defining command line parameters;

[0082] Step 2: System operation test: In order to verify the effectiveness of the algorithm in intense motion and degradation scenarios, the present invention has been tested on a variety of public data sets and some self-recorded data sets.

[0083] Here we mainly use M2DGR, Newer College dataset, HILTI Challenge Dataset, laser degradation challenge dataset contributed by FAST-LVIO algorithm and self-recorded dataset to evaluate system performance. The first three are rotating laser radars, and the last two are Livox solid-state laser radars. The algorithm of the present invention is compared with the most advanced LIO systems: FAST-LIO2, Faster-LIO, LIO-SAM and Point-LIO. In all experiments, FAST-LIO2 disables online external parameter estimation, LIO-SAM disables closed-loop optimization, and the others use default parameters to provide the best results for each algorithm. All experiments were performed on an Intel i7-11700H CPU, using the Robot Operating System (ROS) in Ubuntu20.04 and 32GB of RAM.

[0084] In order to verify the accuracy of the proposed LIO in conventional datasets. The following table shows the absolute trajectory error (APE), where the red font indicates the highest accuracy and the blue font indicates the second. A total of 4 sequences in the M2DGR dataset and 3 sequences in the Newer College dataset were used to evaluate the positioning accuracy. In the following experiments, FAST-LIO2, Faster-LIO, Point-LIO and the system of the present invention all use a 4-point skip operation and a 0.5m voxel filter to filter the point cloud. As can be seen from Table 1, in general scenarios, due to good constraints, the high-frequency odometer subsystem of the present invention plays a major role, and the multi-frame integral cumulative error of the IMU is not large, which cannot reflect our advantages, but in this general scenario, our system is still comparable to the existing state-of-the-art LIO system in terms of accuracy. Among them, ncd_1 represents the quad_with_dynamics sequence in the Newer College dataset, ncd_2 represents the dynamic_spinning sequence in the Newer College dataset, and ncd_3 represents the parkland_mound sequence in the Newer College dataset.

[0085] Table 1

[0086] Seq FAST-LIO2 Faster-lio LIO-SAM Point-lio Ours street_01 0.373 0.341 0.604 0.491 0.342 street_02 2.826 2.915 3.097 2.986 2.806 street_03 0.138 0.136 0.143 0.130 0.134 street_04 0.512 0.652 1.242 0.561 0.517 street_05 0.347 0.350 0.373 0.468 0.355 short_experiment 0.438 0.397 0.381 0.401 0.410 quad_with_dynamics 0.141 0.112 0.127 0.131 0.116 dynamic_spinning 0.093 0.089 0.121 0.098 0.082 parkland_mound 0.158 0.160 0.173 0.147 0.136

[0087] The dynamic_spinning sequence in Newer College is used to verify the robustness of aggressive movements. In this sequence, the operator manually flips the sensor system, and the maximum angular velocity reaches 183deg / s. In this scenario of intense movement, the integration of multiple frames of IMU will lead to a large cumulative error. The LIO system of the present invention uses a sub-frame generation module to allow the system to perform an observation correction immediately after each frame of IMU integration, greatly reducing the cumulative error of multiple frame IMU integration. The true value in the dynamic_spinning sequence is basically fixed in a very small range, but the LIO of the present invention can still have a small range of accuracy improvement. From Figure 5 From the visualization effect, we can see that our LIO method has higher accuracy in this kind of violent motion, and the reconstructed point cloud is clearer and more complete.

[0088] In the dataset recorded by the present invention, the robustness of different systems in the intense motion scene is verified. We use AVIA radar in the corridor facing the other wall to ensure that the point cloud is not degraded. We hold the device and shake the radar quickly to record the data. Figure 6 It can be seen that the point cloud maps constructed by FAST-LIO2 and Faster-LIO in our recorded data are relatively fuzzy. Since both Point-LIO and our system are frame-based systems, they can significantly reduce the error of IMU pre-integration and can both construct high-precision point cloud maps.

[0089] We use the laser challenge dataset of the FAST-LVIO algorithm and our own recorded scenes with degradation to verify the robustness of our system to scenes with degradation and intense motion. Figure 7 This is the laser degradation challenge data package contributed by the FAST-LVIO algorithm. This data segment is a Livox laser radar shining on a wall, floor and ceiling in the hall. The system always has insufficient constraints in the left and right directions. The fast-lio2, faster-lio and point-lio algorithms will drift significantly when the laser is degraded, resulting in a large end-to-end posture deviation. Our adaptive weight module emphasizes the importance of a small number of constraints in the degradation direction. For this degradation situation, it can alleviate the problem of error accumulation and propagation of the positioning system in the degradation direction. In the end, our system can still maintain the stability of the system, and the constructed point cloud map conforms to the actual situation and returns to the initial position.

[0090] In order to verify the robustness of the system of the present invention in data with both violent motion and degraded scenes, the AVIA radar was shaken quickly in an empty hall using its own data set, and there were several sections in which all radar points were irradiated on the ground. In this extreme scenario, the FAST-LIO2 system could not maintain stable positioning, causing the system to completely run away. Faster-LIO will not completely run away, but the positioning will drift due to the appearance of degraded scenes, and ultimately a distorted point cloud map will be constructed. Point-LIO is sensitive to degradation, resulting in the construction of multiple point cloud maps, but the system of the present invention can perform robust positioning in such extreme scenarios with both violent motion and degradation by adaptively switching between the point cloud segmentation subsystem and the degradation processing subsystem, and ultimately reconstruct a high-precision point cloud map.

Claims

1. A laser radar inertial odometer for localizability perception in challenging scenarios, characterized in that: The current constraint state of the overall odometer system is determined by a degradation detector, and the overall odometer system is divided into two subsystems according to the constraint state, namely a degradation processing subsystem and a high-frequency odometer subsystem; when insufficient constraints are detected and degradation occurs, the degradation processing subsystem is entered, and when the constraints are good, the high-frequency odometer subsystem is entered; adaptive weights are formed in the degradation processing subsystem to update the lidar frequency state; in the high-frequency odometer subsystem, after the time frame is divided into subframes, the IMU frequency state of the subframe is updated.

2. The laser radar inertial odometer for localizability perception in challenging scenarios according to claim 1, characterized in that: The degradation detector is specifically: Extract the z points of the local map corresponding to each point in the current frame, fit the z points of the local map into a plane, each plane has a normal vector, normalize the normal vector of the local map plane corresponding to each point in this frame, move the starting points of all plane normal vectors corresponding to this frame to the origin of the world coordinate system, and construct a constraint sphere matrix, as shown in the following formula: Among them, N pre Represents the constraint sphere matrix composed of the plane normal vectors corresponding to the current frame point, n i represents the plane normal vector corresponding to the i-th point, and m represents the number of plane normal vectors. For each plane normal vector constraint, take the negative number to get a new normal vector. For a plane, there are two normal vectors. The final constraint matrix N is as follows: N=[N pre ,-N pre ] The constraint ball matrix reflects all the constraints of this frame. The more uniform the constraint ball is, the better the constraints of this frame are. Each normalized plane normal vector is regarded as a plane point, and the principal component analysis PCA is used to perform constraint analysis on these plane points. First, the mean is subtracted from these plane points. Since each plane point is negatively counted and added to the constraint ball matrix, the mean is always zero. Then, the oblique variance matrix of these plane points is calculated. Perform eigenvalue decomposition on this covariance matrix: Three eigenvalues ​​{λ1, λ2, λ3} are obtained, and the eigenvalues ​​are sorted. The eigenvector corresponding to the largest eigenvalue represents the main direction of these plane points; the degenerate condition number is defined as λ min is the minimum eigenvalue, λ max is the maximum eigenvalue. The larger the degenerate condition number, the better the constraint, and the smaller the degenerate condition number, the worse the constraint.

3. The laser radar inertial odometer for localizability perception in challenging scenarios according to claim 2, characterized in that: The constraint state is determined by comparing the degradation condition number and the threshold; two thresholds are defined. When the degradation condition number is less than the threshold t1, it means that the overall odometer system enters the degradation processing subsystem from the high-frequency odometer subsystem. When the degradation condition number is greater than the threshold t2, it means that the overall odometer system will enter the high-frequency odometer subsystem from the degradation processing subsystem; t1 is less than t2; when the degradation condition number is between the threshold t1 and the threshold t2, the system state at the previous moment is still maintained.

4. The laser radar inertial odometer for localizability perception in challenging scenarios according to claim 2, characterized in that: The z is 5.

5. The laser radar inertial odometer for localizability perception in challenging scenarios according to any one of claims 1 to 4, characterized in that: The high-frequency odometer subsystem is specifically: According to the IMU data, the laser radar point cloud data is segmented to obtain sub-data frames. After obtaining the sub-data frames, the ESKF algorithm is used to update the state. First, the system state quantity is defined as follows: in Indicates its own position in the world system, Indicates the speed of the world system. Represents the rotation in the world coordinate system, represents the acceleration of the IMU, represents the offset of the angular velocity meter, represents the acceleration due to gravity; Using the error state Kalman filter for state estimation, the new state quantity is First, the state transfer equation of the system error state in discrete time is: δx k+1 =F x δx k +F i n Among them, F x represents the state transfer matrix of the linearized system, F i represents the control matrix of the linearized system; Then calculate the prior error state covariance matrix as: P k+1 =F x P k F x T +F i Q k F i T Where P k represents the posterior error covariance matrix at the previous moment, Q k is the covariance matrix of the system noise at the previous moment; Using the above point-to-plane distance as observation, iterative state updates are performed. In each round of state update, the point-to-plane association is re-performed, and the state is updated using the following formula: Among them, H is the mapping matrix of the observation equation, R is the covariance matrix of the observation noise, is the posterior estimate, which is the current optimal estimate; Finally, the covariance matrix of the system state is updated: P k =(I-KH)P k The point cloud of each subframe after each state update is accumulated, and when a complete frame is accumulated, the point cloud of the complete laser frame is sent to the degradation detector.

6. The laser radar inertial odometer for localizability perception in challenging scenarios according to any one of claims 1 to 4, characterized in that: After the degradation detector detects the degradation, it enters the degradation processing subsystem, and uses the points after the current point cloud frame is dedistorted for adaptive weight allocation in the degradation processing subsystem, and is sent to the degradation detector for degradation detection. The degradation processing subsystem is specifically: The constraint sphere matrix is ​​divided into q surface patches according to the longitude and latitude of the earth, and the area of ​​each surface patch is used as the weight value of the surface patch; the area of ​​each surface patch and the number of normal vectors in each surface patch are as follows: <h2 style=";text-align:left;direction:ltr">S = {s1,s2,s3,...,s<h2 style=";text-align:left;direction:ltr"> q <h2 style=";text-align:left;direction:ltr">} P={p1,p2,p3,...,p q } The points in the patch divide the weight of the patch equally; the weight of each point is calculated as follows to obtain the importance of other points with respect to that point: The state estimation is also performed according to the ESKF algorithm. The covariance of each point is adaptively allocated through the weight of each point, so as to obtain the final covariance matrix; the calculated weight and the actual covariance are unified to the same scale; the variance of each point is obtained using the following formula: Among them, a represents the commonly used inverse covariance of each point, and 2m represents the number of constraints. Since each point is independent of each other, the covariance between every two points is zero, and the final R matrix is ​​a diagonal matrix with the diagonal as the variance of each point. Similarly, the ESKF algorithm is used for state estimation, and the observation covariance matrix is ​​replaced by the calculated new covariance matrix R when calculating the Kalman gain.

Images