Laser radar-inertial navigation tight coupling SLAM method using ground constraint

Abstract

The invention relates to a laser radar-inertial navigation tight coupling SLAM (Simultaneous Localization and Mapping) method using ground constraint, belongs to the field of mapping of autonomous mobile equipment, and aims to solve the problems of relatively large Z-axis height positioning error, inaccurate point cloud map construction and low rear-end matching speed existing in long-time operation of a laser inertial SLAM method in the prior art. The method comprises the following steps: S100, constructing a laser inertial navigation SLAM system, wherein the construction is divided into IMU pre-integration, laser radar odometer and rear-end sliding window optimization; and S200, adding a functional module for simulating and extracting ground point cloud in the laser radar odometer, and using the functional module to restrain the posture of the robot / unmanned vehicle. And S300, constructing a self-adaptive voxel map by using the pose optimized and output by the rear end. According to the method, pose estimation is optimized by adopting a cloth simulation technology, Z-axis errors are reduced, an adaptive voxel map is introduced, and the matching efficiency and precision of the SLAM rear end are improved in combination with a hash table and a K-D tree.

Description

Technical Field

[0001] The invention relates to a laser radar-inertial navigation tightly coupled SLAM method using ground constraints, and belongs to the field of autonomous mobile device mapping. Background Art

[0002] SLAM technology is a requirement and necessary condition for unmanned equipment to cope with complex mission scenarios. Its positioning accuracy will affect a series of subsequent upper-level tasks such as navigation, obstacle avoidance, coordination and formation. At present, the application scenarios of unmanned equipment are becoming increasingly complex. In order to ensure the accuracy and robustness of autonomous positioning of unmanned vehicles, 3D laser radar, IMU and other sensors are generally used for fusion positioning. At present, the laser inertial SLAM method still has problems such as large Z-axis height positioning error, inaccurate point cloud map construction and slow back-end matching speed after long-term operation.

[0003] In 2014, Zhang Ji from Cameron NeXT University proposed LOAM, the cornerstone of 3D laser SLAM algorithm. A series of excellent works were subsequently improved and expanded on the basis of LOAM. LOAM divides the 3D laser SLAM system into the front-end and back-end. LOAM estimates the pose by matching the line features and surface features of the laser radar point cloud. The front-end outputs a rough estimate of the pose with high frequency and low precision, and the back-end outputs a high-precision pose with a lower frequency and establishes a point cloud map. This architecture ensures the real-time and high-precision of LOAM. In 2018, Tixiao Shan from the School of Mechanical Engineering at Stevens Institute of Technology proposed LeGo-LOAM based on LOAM. The algorithm adds ground point cloud constraints to the pose estimation and pre-processes the point cloud, reducing the point cloud noise and dividing the laser radar pose solution into two steps to reduce the algorithm's computational complexity. It extracts ground features to optimize the Z-axis positioning error. This method is a lightweight algorithm that can run in embedded systems. The judgment of ground point cloud in LeGo-LOAM is too simple and easy to misjudge ground point cloud. The method is as follows: two adjacent laser beams hit the ground to form two points A and B. Their vertical height difference is h. This value is close to 0 in an ideal situation (the radar is installed horizontally and the ground is horizontal). The angle of the horizontal plane is also close to 0 in an ideal situation, but the radar will not be installed completely horizontally, and the ground is not flat, so this angle will be greater than 0. LeGO-LOAM is set to 10°, that is, less than 10° is judged as a ground point. In 2020, Tixiao Shan proposed LIO-SAM, which tightly couples the lidar and IMU data for positioning and adopts factor graph optimization during global optimization. The optimization process adds IMU factors and GPS factors. Compared with traditional graph optimization, it reduces the computational complexity while ensuring the same order of magnitude of accuracy. Summary of the invention

[0004] The present invention proposes a laser radar-inertial navigation tightly coupled SLAM method using ground constraints, which uses a cloth simulation method to extract high-quality ground point cloud features for constraining the posture of a robot / unmanned vehicle, thereby reducing the Z-axis error, and uses an adaptive voxel map to model the measurement error of the laser radar and use a hash table plus a KD tree data structure to jointly store the point cloud map, thereby improving the real-time and accuracy of SLAM back-end matching, thereby solving the problems in the prior art that the laser inertial SLAM method still has a large Z-axis height positioning error, inaccurate point cloud map construction and slow back-end matching speed after long-term operation.

[0005] A laser radar-inertial navigation tightly coupled SLAM method using ground constraints, the laser radar-inertial navigation tightly coupled SLAM method using ground constraints comprising the following steps:

[0006] S100, build a laser inertial navigation SLAM system, which includes three sub-modules: IMU pre-integration, laser radar odometer and back-end sliding window optimization;

[0007] S200, firstly extracting ground point cloud from the laser radar point cloud information by using cloth filtering, then adding the ground point cloud information to the laser radar odometer, and then simulating cloth covering the point cloud under the action of gravity to identify and separate the ground point cloud, and constraining the posture by building a constraint relationship between the unmanned vehicle and the ground;

[0008] S300, further optimizing the constrained posture through the back-end sliding window optimization submodule in S100, and constructing an adaptive voxel map using the optimized posture information.

[0009] Furthermore, in S100, the following steps are included:

[0010] S110, the IMU pre-integration module processes the IMU data and integrates the acceleration and angular velocity data of the IMU to predict the posture change of the unmanned vehicle between two frames of laser radar scanning, that is, to obtain the posture estimation between the laser radar scanning data and provide an initial value;

[0011] S120, using the IMU data obtained by pre-integration and the laser radar scanning data, performing laser radar odometer, and estimating the posture change by matching the point cloud frames;

[0012] S130, controlling the scale of graph optimization through back-end sliding window optimization to ensure the real-time performance of the SLAM system.

[0013] Furthermore, in S110,

[0014] The output of the IMU in the IMU pre-integration module is modeled as:

[0015]

[0016] Where a and w are the acceleration and angular velocity output by the IMU, respectively, and b a and b w The zero bias during IMU measurement is a deterministic error, n a and n w The random walk during IMU measurement belongs to Gaussian white noise. The linear acceleration and angular velocity of the unmanned vehicle are directly measured by IMU.

[0017] The discrete time recursive formula of IMU pre-integration quantity is as follows:

[0018]

[0019] Pre-integrated observations are used as factors or edges for graph optimization to constrain the posture of the SLAM system. In IMU-related applications, the state at each moment is modeled as a variable containing rotation, translation, linear velocity, and IMU zero bias to form a state variable set c. The formula is as follows:

[0020]

[0021] A nine-dimensional residual variable is defined to represent the difference in rotation, translation, linear velocity and angular velocity of the SLAM system at two consecutive moments. The IMU pre-integrated observation value is associated with the nine-dimensional residual variable, where the IMU pre-integrated observation value includes the IMU zero bias at moment i. When the IMU zero bias at moment i is adjusted, the pre-integrated observation value is updated accordingly to change the value of the nine-dimensional residual variable in a linear manner. Through linear transformation, the nine-dimensional residual variable is linked to the constraint factor of the IMU zero bias to consider the update of the zero bias in the optimization process.

[0022] Furthermore, in S120,

[0023] The core of the LiDAR odometer module is point cloud frame matching. Point cloud frame matching is to match two frames of point cloud data to obtain the posture change relationship between the two LiDAR frames. Point cloud frame matching is modeled as the following problem: Given two point cloud sets X = {x1, K, x n}, Y={y1,K,y n}, x i ,y i The coordinates of the point cloud are obtained by solving the rotation matrix R and the translation vector t between X and Y so that the following equation holds:

[0024]

[0025] The above formula can be transformed into the following form:

[0026]

[0027] where u x 、u y are the centroids of point sets X and Y respectively,

[0028]

[0029] Assumptions

[0030]

[0031] Then, for any rotation matrix R, we can find a displacement vector t such that E2(R, t) = 0.

[0032]

[0033] Assuming that equation (10) holds, minE1(R, t) is equivalent to maxE1'(R, t). According to the mathematical theorem, the trace of a constant is equal to itself.

[0034]

[0035] The problem is transformed into finding a suitable R to maximize Trace(RH). According to the theorem, if there is a positive definite matrix AA T , then for any orthogonal matrix B, Trace(AA T )>Trace(BAA T ), we know that if we can find an R that converts Trace(RH) into Trace(AA T ) form to obtain the required rotation matrix R, and then perform SVD decomposition on the H matrix:

[0036] H=U∑V T (13)

[0037] Take R = VU T but:

[0038]

[0039] After the rotation matrix R is determined, the translation vector t can be determined by the following formula.

[0040] t=u x -Ru y (15).

[0041] Furthermore, in S130, the following steps are included:

[0042] S131. Define optimization variables to represent the positions and postures of the laser radar at different times;

[0043] S132, establishing a transformation relationship between optimization variables, and determining posture transformation parameters through the optimization process;

[0044] S133, introducing the prior map as an optimization variable and establishing a constraint relationship with the laser radar posture;

[0045] S134, taking the IMU's posture at different times as optimization variables, and establishing a fixed transformation relationship with the laser radar posture to achieve mutual optimization of posture estimation errors;

[0046] S135, defining the deterministic error of the IMU as a constant to simplify the processing during system operation;

[0047] S136, controlling the scale of graph optimization, retaining only the data of the most recent N laser radar key frames, and performing marginalization processing on the data beyond the time window;

[0048] S137. By utilizing the sparsity and Schur complement of the matrix, the constraint information of the marginalized data is retained without optimizing it to maintain the real-time performance of the system.

[0049] Furthermore, in S200, the following steps are included:

[0050] S210, integrating a cloth simulation method in a laser radar odometer module to extract ground point clouds;

[0051] S220, identifying and separating the ground point cloud by simulating the cloth covering the point cloud under the action of gravity;

[0052] S230: Use the extracted ground point cloud to construct a constraint relationship between the unmanned vehicle and the ground, and constrain the posture.

[0053] Furthermore, in S220, the cloth simulation method includes the following steps:

[0054] S221, setting an initial position of the simulated cloth above the inverted laser radar point cloud;

[0055] S222, simulating the displacement of cloth particles under the action of gravity to identify particles that may intersect with the ground;

[0056] S223. For the simulated cloth particles, if the displacement of the simulated cloth particles causes the position of the simulated cloth particles to be lower than the ground, reposition the simulated cloth particles to the ground level, and mark the simulated cloth particles as being in a fixed state;

[0057] S224. For particles that are not in contact with the ground and are still in a movable state, their positions are adjusted according to the internal force interaction between the particles.

[0058] Furthermore, in S220, in order to adapt the cloth simulation to the ground filtering,

[0059] The particles are restricted to move only in the vertical direction to detect the ground point cloud by comparing the height values ​​of the particles and the terrain;

[0060] When particles reach the ground under the action of gravity, they are set to be immovable to prevent them from moving laterally due to internal forces, which would result in inaccurate ground point clouds.

[0061] The force acting on the particle is decomposed into two forces. First, the effect of gravity on the particle is considered. When the particle reaches the ground under the action of gravity, the particle becomes immovable. At this time, the collision force is ignored and the position of the particle is changed according to the internal force.

[0062] Furthermore, in S230, extracting the ground point cloud includes the following steps:

[0063] S231, invert the original radar point cloud;

[0064] S232, initializing the cloth grid, setting the number of particles according to the grid resolution, and setting the initial position of the cloth above the highest point of the point cloud;

[0065] S233, projecting all the laser radar point clouds and mesh particles onto a horizontal plane, finding corresponding points of the mesh particles on the horizontal plane, and recording intersection height values;

[0066] S234. For each mesh particle, if the particle is movable, the position affected by gravity is calculated, and the height of the particle is compared with the intersection height value; if the height of the particle is equal to or less than the intersection height value, the particle is placed at the intersection height value position and is set to be immovable;

[0067] S235, calculating the state change of each grid particle under the action of internal force;

[0068] S236, repeat S234 and S235 until the maximum height change of all particles is less than the threshold or the number of iterations reaches the maximum number of iterations, and the cloth simulation process is terminated;

[0069] S237, calculating the distance from the point cloud to the mesh particles;

[0070] S238. Classify the laser radar point cloud smaller than the threshold as a ground point cloud, and classify the point cloud larger than the threshold as a non-ground point.

[0071] Furthermore, in S300, constructing the adaptive voxel map includes the following steps:

[0072] S310, analyzing the uncertainty of the laser radar scanning point, including the range uncertainty and the orientation uncertainty, and describing them using a Gaussian model;

[0073] S320, uncertainty modeling of a planar feature composed of a set of laser radar scanning points, taking into account measurement noise and pose estimation errors;

[0074] S330, constructing data types for map storage, and using hash tables and octrees to organize and store map data;

[0075] S340, dividing the space into voxels, and filling the voxels according to the laser radar scanning points to construct a voxel map, and updating the map information when new point cloud data conflicts with point clouds in existing voxels;

[0076] S350, when the SLAM system is running, registering the newly scanned point cloud into the global map, and updating the map if the new point cloud causes a change in the plane parameters;

[0077] S360, when a new point cloud is detected to be significantly different from the old one, the planar uncertainty is remodeled and the map is updated to reflect the environmental changes.

[0078] Beneficial effects of the present invention:

[0079] 1. The present invention improves the positioning accuracy of unmanned vehicles and robots in complex environments and enhances the robustness of the SLAM system by using a ground-constrained lidar-inertial navigation tightly coupled SLAM method.

[0080] 2. The method of the present invention extracts ground point cloud through a cloth simulation method, which improves the system in a ground environment, especially in Z-axis height positioning.

[0081] 3. The use of adaptive voxel maps models the measurement errors of lidar, improving the accuracy and real-time performance of mapping.

[0082] 4. The present invention reduces the amount of calculation through back-end sliding window optimization and adaptive voxel map, so that the algorithm can run in resource-constrained embedded systems.

[0083] 5. The sliding window method controls the scale of graph optimization, ensuring the real-time performance of the SLAM system and maintaining high efficiency even after long-term operation.

[0084] 6. The precise extraction of ground point clouds through cloth simulation methods improves the construction quality of point cloud maps and improves the positioning accuracy.

[0085] 7. Constructing the constraint relationship between the unmanned vehicle and the ground effectively reduces the errors of the unmanned vehicle in the Z-axis height value, pitch angle and roll angle.

[0086] 8. The adaptive voxel map can be dynamically updated to adapt to environmental changes, allowing the SLAM system to maintain high performance in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 This is a schematic diagram of IMU pre-integration;

[0088] Figure 2 Optimize models for backend graphs;

[0089] Figure 3 Optimization diagram for sliding window;

[0090] Figure 4 Extract ground point cloud diagram for cloth simulation;

[0091] Figure 5 Simulate schematics for cloth;

[0092] Figure 6 This is a diagram of cloth simulation steps, where: Figure 6 (a) Schematic diagram of the initial morphology of the particles. Figure 6 (b) is a schematic diagram of the displacement of particles under the action of gravity; Figure 6 (c) is a schematic diagram of particles in the cross-check state; Figure 6 (d) is a schematic diagram showing the movement of the remaining movable particles according to the internal forces between the particles;

[0093] Figure 7 Flowchart for adaptive voxel map;

[0094] Figure 8 It is the flow chart of SLAM algorithm;

[0095] Fig. 9 This is a diagram showing the effect of using the method of the present invention to build a point cloud map of the KITTI dataset, where: Fig. 9 (a) is the global point cloud map of the KITTI09 sequence; Fig. 9 (b) is the global point cloud map of the KITTI10 sequence; Fig. 9 (c) is the local point cloud map of the KITTI09 sequence; Fig. 9 (d) is the local point cloud map of the KITTI10 sequence. DETAILED DESCRIPTION

[0096] This method uses a cloth simulation method to extract high-quality ground point cloud features to constrain the position and posture of the robot / unmanned vehicle and reduce the Z-axis error. Compared with the LeGo-LOAM method for extracting ground point clouds, this method can extract ground point clouds more accurately; this method also uses an adaptive voxel map, which models the measurement error of the laser radar and uses a hash table plus a KD tree data structure to jointly store the point cloud map, thereby improving the real-time performance and accuracy of SLAM backend matching. The present invention includes the following steps:

[0097] S100, constructing a laser inertial navigation SLAM system, wherein the laser inertial navigation SLAM system includes three submodules: IMU pre-integration, laser radar odometer, and back-end sliding window optimization;

[0098] S200, firstly, extracting ground point cloud using cloth filtering on the laser radar point cloud information, then adding the ground point cloud information to the laser radar odometer, and then simulating cloth covering the point cloud under the action of gravity, thereby identifying and separating the ground point cloud, and constraining the posture by building a constraint relationship between the unmanned vehicle and the ground;

[0099] S300, further optimizing the constrained posture through the back-end sliding window optimization submodule in S100, and constructing an adaptive voxel map using the optimized posture information.

[0100] In the S100 IMU pre-integration module, the output of the IMU can be modeled as:

[0101]

[0102] Where a and w are the acceleration and angular velocity output by the IMU, respectively, and b a and b w The zero bias during IMU measurement is a deterministic error, n a and n w The random walk during IMU measurement belongs to Gaussian white noise. IMU can directly measure the linear acceleration and angular velocity of the unmanned vehicle. Figure 1 As shown in the figure, since the output frequency of the IMU is much higher than the output frequency of the LiDAR frame, if you want to use the IMU and LiDAR to jointly estimate the pose, you need to integrate the IMU data in the time period between the two frames of radar scanning images to obtain the pose transformation of the unmanned vehicle measured by the IMU during this period. However, if the IMU data is directly integrated, when the pose is optimized at the back end of the SLAM system, the IMU needs to be re-integrated after each pose optimization, which is very time-consuming. The introduction of pre-integration solves this problem. In fact, the SLAM system obtains IMU data in discrete time. The system needs to obtain the integral result of the IMU data between the two LiDAR frames. Therefore, the pre-integration quantity is updated every time a new IMU data is received. Since it is discrete data, summing it is equivalent to continuous time integration. The discrete time recursive formula of the IMU pre-integration quantity is as follows:

[0103]

[0104] Pre-integrated observations can be used as factors or edges in graph optimization to constrain the posture of the SLAM system. In IMU-related applications, the state at each moment is usually modeled as variables including rotation, translation, linear velocity, and IMU zero bias, forming a state variable set c. The specific formula is as follows:

[0105]

[0106] Usually, the residual in the SLAM system is uniformly written as a 9-dimensional residual variable. It can relate the rotation, translation, linear velocity and angular velocity of the SLAM system at two moments, but because the pre-integrated observation contains the IMU zero bias, it is actually related to the two zero biases at moment i. During the optimization process, if the zero bias at moment i is updated, the pre-integrated observation should also change linearly to affect the value of the residual term. Therefore, there will be a constraint factor for the zero bias of the IMU at different moments, which can be obtained by expanding it.

[0107] The core of the laser radar odometer module in S100 is point cloud frame matching. Point cloud frame matching is to obtain the posture change relationship between the two laser radar frames by matching the point cloud data of two frames. Point cloud frame matching can be modeled as the following problem: Given two point cloud sets X = {x1, K, x n}, Y={y1,K,y n}, x i ,y i Represents the coordinates of the point cloud. By solving the rotation matrix R and translation vector t between X and Y, the following equation is established:

[0108]

[0109] The above formula can be resolved into the following form:

[0110]

[0111] where u x 、u y are the centroids of point sets X and Y respectively,

[0112]

[0113] Assumptions

[0114]

[0115] Then, for any rotation matrix R, we can find a displacement vector t that makes E2(R, t) = 0.

[0116]

[0117] Assuming that equation (10) holds, minE1(R, t) is equivalent to maxE1'(R, t). According to the mathematical theorem, the trace of a constant is equal to itself.

[0118]

[0119]

[0120] This transforms the problem into finding a suitable R to maximize Trace(RH). According to the theorem, if there is a positive definite matrix AA T , then for any orthogonal matrix B, Trace(AA T )>Trace(BAA T ), we know that if we can find an R that converts Trace(RH) into Trace(AA T ) form to obtain the required rotation matrix R, and then perform SVD decomposition on the H matrix:

[0121] H=U∑V T (13)

[0122] Take R = VU T but:

[0123]

[0124] After the rotation matrix R is determined, the translation vector t can be determined by the following formula.

[0125] t=u x -Ru y (15)

[0126] The optimization problem of laser inertial navigation SLAM in S100 can be modeled as follows:

[0127]

[0128] in is the lidar constraint, is the IMU constraint, The prior constraints can be a priori maps, GPS or wireless sensor base stations, etc.

[0129] like Figure 2The figure shows the graph optimization model of laser inertial navigation SLAM, corresponding to equation (16), where the graph above represents the optimization variable, and the edges connecting the graphs represent the transformation relationship between different variables. The blue circle represents the position and posture of the laser radar at different times, and the line between two adjacent blue circles represents the position and posture transformation relationship at two different times; the pink diamond represents the prior map, that is, the point cloud map of the current location and surrounding environment stored in the SLAM system. The prior point cloud map and the position and posture of the laser radar can also be constrained by ICP, NDT and other methods. The red square represents the position and posture of the IMU at different times; the position and posture transformation relationship between the laser radar and the IMU in the laser inertial navigation SLAM system is fixed, so the two can optimize each other's position estimation error; the yellow square is the deterministic error of the IMU, which can be regarded as a constant when the system is running.

[0130] As the system runs longer, the number of optimized variables and the scale of optimization will increase, making the SLAM system unable to run in real time. To ensure the real-time performance of the system, the scale of graph optimization needs to be controlled. The sliding window method can be used to control the scale of graph optimization. The idea of ​​the sliding window method is to only retain the N lidar key frames closest to the current time and remove the data at earlier times, such as Figure 3 As shown in the figure, the graph optimization will be fixed within a time window, and the optimization variables leaving this window will be marginalized, that is, the sparsity and Schur complement of the matrix are utilized to retain the constraint information but not optimize it, so as to control the scale of the graph optimization and ensure the real-time performance of the SLAM system.

[0131] The basic idea of ​​ground point cloud extraction based on cloth simulation in S200 is as follows Figure 4 As shown in the figure: the point cloud scanned by the laser radar is inverted, and then a piece of cloth is placed on the inverted point cloud and allowed to fall to the surface of the inverted point cloud under the action of gravity. The final shape of the cloth is obtained through the interaction between the cloth and the laser radar point cloud, so as to separate the ground part and the non-ground part of the laser point cloud. After separating the ground point cloud, the Z-axis error is constrained by building a constraint relationship between the unmanned vehicle and the ground to reduce the Z-axis error.

[0132] Cloth simulation is a technique used in 3D computer graphics to simulate clothing fabrics using computer programs. In computer graphics, a piece of cloth can be considered as a network of interconnected particles with mass, i.e., a mass spring model. Figure 5 As shown, particles at mesh nodes have no size but do have mass.

[0133] The position of each particle in the cloth simulation in three-dimensional space constitutes the position and shape of the cloth, and the virtual spring refers to the interconnection model between particles, which follows Hooke's law. Figure 5 The cloth simulation shown defines three types of springs to fully simulate the properties of cloth: shear springs, traction springs, and flexion springs. The position and velocity of a particle depend on the force each particle is subject to. According to Newton's law of acceleration, the position of a particle and the force it is subject to are related as follows:

[0134]

[0135] Where X represents the position of the particle at time t, F ext (X, t) represents the external force, which is composed of gravity and collision force, f int (X,t) is the internal force, which is generated by the connection between particles.

[0136] In order to adapt the cloth simulation to the ground filtering, the present invention modifies the cloth simulation as follows:

[0137] (1) Restricting particles to move only in the vertical direction, so that the height values ​​of particles and terrain can be compared to detect ground point clouds;

[0138] (2) When the particles reach the ground under the action of gravity, the particles are set to be immovable to prevent the particles from moving laterally due to internal forces, which would result in inaccurate separated ground point clouds;

[0139] (3) Decompose the force acting on the particle into two forces. First, consider the effect of gravity on the particle. When the particle reaches the ground under the action of gravity, the particle becomes immobile. At this time, the collision force can be ignored to change the particle position according to the internal force.

[0140] The cloth simulation process is as follows Figure 6 As shown:

[0141] (1) Initial state: The simulated cloth is initially set above the inverted lidar point cloud.

[0142] (2) Calculate the displacement of each particle under the action of gravity, where some particles may appear below the ground.

[0143] (3) Cross-check: Move particles below the ground to the ground and set the status of these particles to immovable.

[0144] (4) The remaining movable particles move according to the internal forces between the particles.

[0145] Cloth simulation filtering can be divided into two steps. First, consider the effect of external forces. Ignore the collision force and only consider the gravity factor to calculate the displacement of each particle, that is, consider the displacement when the internal force between particles is 0. The following solution equation can be obtained:

[0146]

[0147] Then consider the internal force part. You only need to traverse all adjacent particles and compare the height difference of adjacent particles. When the particles start to move under the action of internal force, two particles with different heights will try to move to the same height under the influence of internal force. If two adjacent particles are movable, they will move in opposite directions to each other; if one of the particles is immovable, the other movable particle will move in the opposite direction; if the two particles are at the same height, both particles will remain stationary. From this, the displacement formula of each particle can be obtained:

[0148]

[0149] in Represents the particle displacement vector, b represents whether the particle can move, b is 1 when it can move, and 0 when it cannot move. is the coordinate vector of the currently traversed particle, is a unit vector in the vertical direction. The particle movement process under the action of internal force is an iterative process. The distance moved by the particle in each iteration is half of the previous one.

[0150] The specific process is as follows:

[0151] (1) Invert the original radar point cloud.

[0152] (2) Initialize the cloth mesh. Set the number of particles according to the mesh resolution and set the initial position of the cloth above the highest point of the point cloud.

[0153] (3) Project all lidar point clouds and mesh particles onto the horizontal plane, find the corresponding points of the mesh particles on the horizontal plane, and record the intersection height values.

[0154] (4) For each mesh particle, if the particle is movable, the position affected by gravity is calculated, and the height of the particle is compared with the intersection height value. If the height of the particle is equal to or less than the intersection height value, the particle is placed at the intersection height value position and set to be immovable.

[0155] (5) Calculate the state changes of each grid particle under the action of internal forces.

[0156] (6) Repeat steps (4) and (5) until the maximum height change of all particles is less than the threshold or the number of iterations reaches the maximum number of iterations, and the cloth simulation process terminates.

[0157] (7) Calculate the distance from the point cloud to the mesh particles.

[0158] (8) The lidar point cloud with a value smaller than the threshold is classified as a ground point cloud, and the point cloud with a value larger than the threshold is classified as a non-ground point.

[0159] By using the separated ground point cloud features and constructing point-to-ground constraints, the Z-axis height value, pitch angle and roll angle of the SLAM system unmanned vehicle can be optimized.

[0160] The adaptive voxel map in S300 is divided into three aspects: uncertainty analysis of lidar point cloud, planar uncertainty modeling and voxel map construction.

[0161] The uncertainty of the LiDAR scan point in the LiDAR coordinate system is given by the range uncertainty (δd i ) and position uncertainty (dω i ) is composed of two parts. Its Gaussian model is as follows:

[0162] d ωi :N(0 2′1 ,ω i ),δ di :N(0,d i )(20)

[0163] is the measured axial direction, d ωi :N(0 2′1 ,∑ω i ) is ω i Axial noise on the tangent plane, d i is the depth measurement value, i.e. the distance from the point to the lidar, δ di :N(0,∑d i ) is the ranging noise. The measurement point is L pi ,∑L pi is the measurement point covariance, is the measurement point noise, ∑W pi is the covariance of the point. They have the following relationship:

[0164]

[0165]

[0166] The coordinates of a point in the world coordinate system and its uncertainty relationship are shown below:

[0167]

[0168] Where ∑R is The uncertainty in the tangent space, ∑t, is Uncertainty in the tangent plane. Considering the uncertainty of LiDAR measurement and attitude estimation, the covariance of point clouds at different locations is quite different. The noise of points closer to the target is mainly caused by ranging noise, while the noise of points farther away is mainly caused by azimuth noise. The uncertainty analysis of single-point LiDAR is also the basis of the uncertainty model of plane features.

[0169] Next, we model the plane uncertainty. A plane feature consists of a set of LiDAR scan points. Each scan point has uncertainty due to measurement noise and pose estimation error. W ∑p i . Let the covariance of this set of laser radar scanning points be A, then,

[0170]

[0171] A plane can be represented by a normal vector n, which is the eigenvector associated with the smallest eigenvalue of A. There is a point on the plane Because A and It all depends on W p i , so the plane parameters (n, q) can be expressed as W p i express.

[0172] [n,q] T =f( W p1, W p2,K, W p N ) (26)

[0173] Based on the previous analysis of uncertainty, each lidar point There is a noise Then the plane n in the real world gt and position q gt It can be estimated as follows:

[0174]

[0175] here Assume that A has an eigenvector matrix U, a minimum eigenvalue λ3, and a corresponding eigenvector u3. For each point W p i Find the derivatives of n and q as follows:

[0176]

[0177] Then the covariance matrix ∑n,q is

[0178]

[0179] According to equation (30), since n and q are calculated from the same set of noise points, n and q are not independent.

[0180] The data types stored in the map are composed of hash tables and octrees; the hash table manages the top-level map structure, and each node of the octree stores a point on a plane. If the points in a node cannot be represented as a plane, the node will continue to be split until each octree node stores a plane feature; if a new point cloud conflicts with an old point cloud in the map, the old point cloud will be deleted and the new point cloud will be retained. The specific process of map construction is as follows: first, the space is cut into voxels, and the size of each voxel is the maximum size that the system can set (set to 1m in this method), and the lidar scanning points are continuously assigned to the voxels and the voxels filled with lidar observation points are indexed into the hash table; if all the points filled in each voxel are located on a plane, that is, the minimum eigenvalue of the covariance matrix of the point is less than the specified threshold, these plane points are stored and their plane parameters (n, q) and uncertainty are calculated; if the points in the voxel are located in different planes, the current voxel will be divided into 8 smaller voxels and the plane check and voxel cutting will be repeated in each voxel until the preset maximum number of layers of the KD tree is reached (set to 3 here). The size of the voxels can be different, but each voxel contains only one plane feature fitted by the original lidar point.

[0181] Reference Figure 8 As shown in the figure, when the laser-inertial navigation SLAM system is running, new point clouds scanned by the lidar are continuously registered, and then these new point clouds are registered to the global map based on the poses of these point clouds estimated by the SLAM system. When there is no voxel in the space where the new point cloud is located, the map will construct a new voxel; when a new point cloud is added to an existing voxel, the parameters and uncertainty of the plane in the voxel need to be updated. Since the plane parameters and uncertainties converge quickly, after the uncertainty converges, all historical lidar points are discarded, and the estimated plane parameters (n, q) and covariance ∑n,q are retained. Once a new point appears, we only retain the 10 most recent points and calculate the normal vector of the new plane composed of these 10 points. If the new normal vector continues to differ greatly from the previously converged normal vector, it means that the scene in this area has changed, and the plane uncertainty needs to be remodeled and the map updated. The specific update, construction, and maintenance process of the adaptive voxel map is as follows. Figure 7 As shown in FIG. 1 , the effect diagram of the point cloud map construction of the KITTI dataset using the method of the present invention is as follows: Fig. 9 shown.

Claims

1. A laser radar-inertial navigation tightly coupled SLAM method using ground constraints, characterized in that: The laser radar-inertial navigation tightly coupled SLAM method using ground constraints comprises the following steps: S100, constructing a laser inertial navigation SLAM system, wherein the laser inertial navigation SLAM system includes three submodules: IMU pre-integration, laser radar odometer, and back-end sliding window optimization; S200, firstly, extracting ground point cloud using cloth filtering on the laser radar point cloud information, then adding the ground point cloud information to the laser radar odometer, and then simulating cloth covering the point cloud under the action of gravity, thereby identifying and separating the ground point cloud, and constraining the posture by building a constraint relationship between the unmanned vehicle and the ground; S300, further optimizing the constrained posture through the back-end sliding window optimization submodule in S100, and constructing an adaptive voxel map using the optimized posture information.

2. A laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 1, characterized in that: In S100, the following steps are included: S110, the IMU pre-integration module processes the IMU data and integrates the acceleration and angular velocity data of the IMU to predict the posture change of the unmanned vehicle between two frames of laser radar scanning, that is, to obtain the initial value of the posture estimation of the two frames of laser radar scanning data; S120, using the IMU data obtained by pre-integration to estimate the initial value of the pose and the laser radar scanning data, and estimating the pose change by matching the point cloud frames; S130, controlling the scale of graph optimization through back-end sliding window optimization to ensure the real-time performance of the SLAM system.

3. A laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 2, characterized in that: In S110, The output of the IMU in the IMU pre-integration module is modeled as: Where a and w are the acceleration and angular velocity output by the IMU, respectively, and b a and b w The zero bias during IMU measurement is a deterministic error, n a and n w The random walk during IMU measurement belongs to Gaussian white noise. The linear acceleration and angular velocity of the unmanned vehicle are directly measured by IMU. The discrete time recursive formula of IMU pre-integration quantity is as follows: Pre-integrated observations are used as factors or edges for graph optimization to constrain the posture of the SLAM system. In IMU-related applications, the state at each moment is modeled as a variable containing rotation, translation, linear velocity, and IMU zero bias to form a state variable set c. The formula is as follows: A nine-dimensional residual variable is defined to represent the difference in rotation, translation, linear velocity and angular velocity of the SLAM system at two consecutive moments, and an IMU pre-integrated observation value is associated with the nine-dimensional residual variable, wherein the IMU pre-integrated observation value includes the IMU zero bias at moment i. When the IMU zero bias at moment i is adjusted, the pre-integrated observation value is updated accordingly to change the value of the nine-dimensional residual variable in a linear manner. Through a linear transformation, the nine-dimensional residual variable is linked to the constraint factor of the IMU zero bias to consider the update of the zero bias during the optimization process.

4. The method of tightly coupled SLAM using laser radar and inertial navigation using ground constraints according to claim 3, characterized in that: In S120, The core of the LiDAR odometer module is point cloud frame matching. Point cloud frame matching is to match two frames of point cloud data to obtain the posture change relationship between the two LiDAR frames. Point cloud frame matching is modeled as the following problem: Given two point cloud sets X = {x1, K, x n }, Y={y1,K,y n }, x i ,y i The coordinates of the point cloud are obtained by solving the rotation matrix R and the translation vector t between X and Y so that the following equation holds: The above formula can be transformed into the following form: where u x 、u y are the centroids of point sets X and Y respectively, Assumptions Then, for any rotation matrix R, we can find a displacement vector t such that E2(R, t) = 0. Assuming that equation (10) holds, minE1(R, t) is equivalent to maxE1'(R, t). According to the mathematical theorem, the trace of a constant is equal to itself. The problem is transformed into finding a suitable R to maximize Trace(RH). According to the theorem, if there is a positive definite matrix AA T , then for any orthogonal matrix B, Trace(AA T )>Trace(BAA T ), we know that if we can find an R that converts Trace(RH) into Trace(AA T ) form to obtain the required rotation matrix R, and then perform SVD decomposition on the H matrix: H=U∑V T (13) Take R = VU T but: After the rotation matrix R is determined, the translation vector t can be determined by the following formula. t=u x -Ru y (15)。 5. The method of tightly coupled laser radar-inertial navigation SLAM using ground constraints according to claim 4, characterized in that: In S130, the following steps are included: S131. Define optimization variables to represent the positions and postures of the laser radar at different times; S132, establishing a transformation relationship between optimization variables, and determining posture transformation parameters through the optimization process; S133, introducing the prior map as an optimization variable and establishing a constraint relationship with the laser radar posture; S134, taking the IMU's posture at different times as optimization variables, and establishing a fixed transformation relationship with the laser radar posture to achieve mutual optimization of posture estimation errors; S135, defining the deterministic error of the IMU as a constant to simplify the processing during system operation; S136, controlling the scale of graph optimization, retaining only the data of the most recent N laser radar key frames, and performing marginalization processing on the data beyond the time window; S131. Utilizing the sparsity and Schur complement of the matrix, the constraint information of the marginalized data is retained without optimizing it, so as to maintain the real-time performance of the system.

6. A laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 5, characterized in that: In S200, the following steps are included: S210, integrating a cloth simulation method in a laser radar odometer module to extract ground point clouds; S220, identifying and separating the ground point cloud by simulating the cloth covering the point cloud under the action of gravity; S230: Use the extracted ground point cloud to construct a constraint relationship between the unmanned vehicle and the ground, and constrain the posture.

7. A laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 6, characterized in that: In S220, the cloth simulation method includes the following steps: S221, setting an initial position of the simulated cloth above the inverted laser radar point cloud; S222, simulating the displacement of cloth particles under the action of gravity to identify particles that may intersect with the ground; S223. For the simulated cloth particles, if the displacement of the simulated cloth particles causes the position of the simulated cloth particles to be lower than the ground, reposition the simulated cloth particles to the ground level, and mark the simulated cloth particles as being in a fixed state; S224. For particles that are not in contact with the ground and are still in a movable state, their positions are adjusted according to the internal force interaction between the particles.

8. The laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 7, characterized in that: In S220, in order to adapt the cloth simulation to the ground filtering, The particles are restricted to move only in the vertical direction to detect the ground point cloud by comparing the height values ​​of the particles and the terrain; When particles reach the ground under the action of gravity, they are set to be immovable to prevent them from moving laterally due to internal forces, which would result in inaccurate ground point clouds. The force acting on the particle is decomposed into two forces. First, the effect of gravity on the particle is considered. When the particle reaches the ground under the action of gravity, the particle becomes immovable. At this time, the collision force is ignored and the position of the particle is changed according to the internal force.

9. The method of tightly coupled SLAM using laser radar and inertial navigation using ground constraints according to claim 8, characterized in that: In S230, extracting the ground point cloud includes the following steps: S231, invert the original radar point cloud; S232, initializing the cloth grid, setting the number of particles according to the grid resolution, and setting the initial position of the cloth above the highest point of the point cloud; S233, projecting all the laser radar point clouds and mesh particles onto a horizontal plane, finding corresponding points of the mesh particles on the horizontal plane, and recording intersection height values; S234. For each mesh particle, if the particle is movable, the position affected by gravity is calculated, and the height of the particle is compared with the intersection height value; if the height of the particle is equal to or less than the intersection height value, the particle is placed at the intersection height value position and is set to be immovable; S235, calculating the state change of each grid particle under the action of internal force; S236, repeat S234 and S235 until the maximum height change of all particles is less than the threshold or the number of iterations reaches the maximum number of iterations, and the cloth simulation process is terminated; S237, calculating the distance from the point cloud to the mesh particles; S238. Classify the laser radar point cloud smaller than the threshold as a ground point cloud, and classify the point cloud larger than the threshold as a non-ground point.

10. The laser radar-inertial navigation tightly coupled SLAM method using ground constraints according to claim 9, characterized in that: In S300, constructing an adaptive voxel map includes the following steps: S310, analyzing the uncertainty of the laser radar scanning point, including the range uncertainty and the orientation uncertainty, and describing them using a Gaussian model; S320, uncertainty modeling of a planar feature composed of a set of laser radar scanning points, taking into account measurement noise and pose estimation errors; S330, constructing data types for map storage, and using hash tables and octrees to store map data; S340, dividing the space into voxels, and filling the voxels according to the laser radar scanning points to construct a voxel map, and updating the map information when new point cloud data conflicts with point clouds in existing voxels; S350, when the SLAM system is running, registering the newly scanned point cloud into the global map, and updating the map if the new point cloud causes a change in the plane parameters; S360, when a new point cloud is detected to be significantly different from the old one, the planar uncertainty is remodeled and the map is updated to reflect the environmental changes.

Images