A large-scale underground space positioning method based on tight coupling of map / laser / inertial

Abstract

The present invention discloses a large underground space positioning method based on map / laser / inertial tight coupling, which belongs to the field of large underground space positioning. First, the global three-dimensional point cloud map of the underground space is loaded, and the global map is lightweight processed using a three-dimensional grid arrangement structure; then, the IMU and laser point cloud data at the current moment are collected and the point cloud line and surface features are extracted; a three-dimensional grid structure is adopted and point-to-plane ICP is used to achieve efficient global map initialization. Then, pre-integration factors, global lightweight map matching residuals based on point-to-plane, and local point cloud map matching residuals of point-to-plane and point-to-line are constructed and input into a unified optimization framework for iterative optimization, and finally a high-precision positioning result is output. The present invention realizes high-precision, high-real-time, and high-robustness positioning in large underground space scenarios.

Description

Technical Field

[0001] The present invention belongs to the technical field of underground large space positioning, and in particular relates to a large underground space positioning method based on map / laser / inertial tight coupling. Background Art

[0002] Currently, large underground spaces, such as underground tunnels, logistics corridors, underground cities, and coal mine tunnels, are playing an increasingly important role in transportation, commerce, energy, and other fields. High-precision mobile target positioning technology is not only the foundation for the development and utilization of underground spaces but also the core support for their intelligent and efficient operation, possessing significant social and economic significance.

[0003] Due to the enclosed nature of underground spaces, traditional GNSS cannot achieve effective positioning. Typical positioning methods used in underground spaces include autonomous relative positioning and radio absolute positioning. Laser and inertial positioning are two commonly used autonomous positioning methods. To achieve robust positioning, these two positioning methods are often combined. Laser / inertial positioning offers high autonomy and excellent relative positioning results in feature-rich scenes. However, traditional laser / inertial fusion positioning methods face challenges such as performance degradation and accumulated positioning errors in large-scale underground spaces with single-feature structures, such as garages and tunnels. Compensating for absolute positioning with laser / inertial relative positioning is an effective means of maintaining high-precision positioning capabilities in large underground spaces. Radio absolute positioning achieves high-precision absolute positioning by measuring the signal between the terminal and the base station. However, in underground spaces, it is severely affected by multipath effects and signal obstruction, making it difficult to achieve stable and robust global correction capabilities. High-precision point cloud map matching positioning, with its unique advantage of not relying on external signals, can achieve ultra-high-precision absolute positioning at the centimeter level, demonstrating great technological potential in complex and enclosed underground spaces. However, current map matching and localization methods still face significant challenges in environmental robustness. Furthermore, the high computational complexity of point cloud data processing and matching algorithms in large-scale scenarios limits their real-time performance. Summary of the Invention

[0004] In order to solve the technical problems mentioned in the above background technology, the present invention proposes a large-scale underground space positioning method based on tight coupling of map / laser / inertia. Through lightweight processing of large-scale global point cloud maps, the global point cloud map feature matching, inertial pre-integration factors and laser point cloud map local feature factors are integrated into the factor graph framework to realize tight coupling of global point cloud map / laser / inertia and obtain the globally optimal positioning result.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A large underground space positioning method based on map / laser / inertial tight coupling includes the following steps:

[0007] Step 1: Load the global 3D point cloud map of the underground space and perform a 3D voxel downsampling; calculate the boundary value of the map and divide the downsampled map into A three-dimensional grid; traverse the coordinates of each point in the point cloud map and store them in the corresponding three-dimensional grid according to the grid division rules;

[0008] Step 2, Collection Time IMU (Inertial Measurement Unit) and laser point cloud data , and extract the current point cloud frame Edge characteristics Dough characteristics ;

[0009] Step 3: Determine whether the system has completed map matching initialization. If it has, jump to step 5; otherwise, go to step 4.

[0010] Step 4: Use the initial pose setting tool on rviz (visualization tool) in ROS (Robot Operating System) to determine the initial pose according to the current position of the carrier. ; According to the initial posture In step 1, the Find the closest grid in a three-dimensional grid and form a new grid point cloud ; Current frame point cloud Surface features and Matching is performed using the ICP (Iterative Closest Point) algorithm. If the match is successful, the precise initial point pose is obtained. , complete initialization and go to step 5; if the matching is unsuccessful, clear the point cloud and IMU data and jump to step 2 to re-initialize the map;

[0011] Step 5: The gyroscope and acceleration bias optimized in the last iteration are input to the pre-integration calculation module. Time point cloud and The inertial data between the time point clouds are pre-integrated to obtain Time has come The change in posture at the moment , and update the initial pose ;

[0012] Step 6: Construct the pre-integration factor, the global lightweight map matching residual based on point-to-plane, and the local point cloud map matching residuals based on point-to-plane and point-to-line, and enter them into a unified optimization framework for iterative optimization.

[0013] Step 7: Get the optimal global positioning result ; Go to step 2 to enter the next moment calculation process.

[0014] Furthermore, the specific method of step 1 is:

[0015] Traverse each 3D coordinate point in the global point cloud map , is the number of coordinates of the global point graph; then The position of the point in the 3D grid for:

[0016] ;

[0017] in, calculate Specific coordinates in the three-dimensional grid ; In step 1 It can be expressed as:

[0018] ;

[0019] in Respectively represent the global point cloud map in Maximum boundary values ​​in three directions;

[0020] but The position index in the three-dimensional grid is expressed as:

[0021] ;

[0022] Based on the above method, Store into 3D grid .

[0023] Furthermore, the specific method of step 4 is:

[0024] Get the initial pose The three-dimensional position coordinates in , and search the coordinate index of the three-dimensional grid corresponding to the initial position coordinate according to step 1 ;Bundle , , The 3D meshes within the range are fused into a new local point cloud frame , to achieve lightweight processing of the global map to be matched;

[0025] Current frame point cloud Corresponding surface features and The optimal initial pose is obtained by iterative matching through ICP:

[0026] ;

[0027] The above ICP matching algorithm obtains the optimal initialization pose under the condition of meeting the maximum number of iterations or the optimal matching score. And complete the initialization. If the above formula does not converge, jump to step 2 to re-initialize the map.

[0028] Furthermore, the specific process of step 6 is as follows:

[0029] State quantity to be optimized for:

[0030] ;

[0031] in, is the sliding window of the state variables to be optimized. ; 、 、 、 、 Represent three-dimensional position, velocity, quaternion, gyroscope bias and accelerometer bias respectively;

[0032] Construct a residual optimization function that integrates pre-integration factors, global map matching based on point-to-plane, and local point cloud map matching based on point-to-plane and point-to-line:

[0033] ;

[0034] in, is the inertia pre-integration residual function, represent Sliding windows of time intervals; represents the inertial pre-integration between the k-1 time point cloud frame and the k time, is the covariance of the inertia pre-integration factor;

[0035] Matching residuals for the global lightweight point cloud map between points and surfaces; is the covariance of global map matching; Represents the surface features extracted from the point cloud of the current frame The lightweight global map extracted according to the method described in step 4 The point-to-surface error between Every point in , obtained in step 5 Transformed into the global coordinate system, we get ;exist Search and The 5 nearest points are fitted into a plane using the least squares method, and the plane equation is:

[0036] ;

[0037] in is the plane normal vector, is a constant term; then Distance to plane for:

[0038] ;

[0039] Further available Projection point on the plane for:

[0040] ;

[0041] thereby:

[0042] ;

[0043] in, for Moment surface features The number of coordinate points; The state quantity to be optimized The position and posture part of

[0044] For local laser point cloud map matching, Indicates the current Moment surface features and surface features in local point cloud maps The point-to-plane residuals between It is a sliding window map composed of 20 consecutive frames of point cloud. The local map constructed without global correction contains accumulated errors; is the covariance of local map feature matching;

[0045] For the current Time edge characteristics and edge features of local point cloud maps The point-to-line residuals constructed between ; is the covariance of local map feature matching; Each edge feature point , obtained in step 5 Transformed into the global coordinate system, we get ,exist Search and The last 5 points ; Calculate the covariance matrix of these 5 points :

[0046] ;

[0047] in is the center of mass;

[0048] right Perform matrix decomposition, , ; ,and .

[0049] Maximum eigenvalue If it is greater than the next largest eigenvalue 3 times, then The corresponding eigenvector Point Cloud The direction of the fitted line in ; the two endpoints on the line can be expressed as:

[0050] ;

[0051] ;

[0052] ;

[0053] in, for Time edge characteristics The number of coordinate points;

[0054] Based on the Gauss-Newton iterative optimization method, the objective function is gradually optimized. When the error in the iterative process meets the convergence condition or reaches the pre-set maximum number of iterations, the optimization process is terminated. At this time, the optimal solution of the optimized state variable is was determined.

[0055] Due to the adoption of the above technical solution, the present invention has the following beneficial effects compared with the prior art:

[0056] 1. This paper proposes a large-scale underground space positioning method based on tight coupling of map / laser / inertial. It effectively solves the problems of high computational complexity and insufficient real-time performance of traditional global map matching positioning in large-scale spaces. It realizes the real-time loading and matching positioning of large-scale three-dimensional point cloud global maps in any large-scale scene, significantly improving map processing efficiency.

[0057] 2. The present invention proposes a large-scale underground spatial positioning method based on tight coupling of map / laser / inertial, which overcomes the problems of insufficient robustness and hardware adaptability of traditional methods based on NDT and point-line-surface matching, and is also applicable to offline global maps with changed scan line order.

[0058] 3. The present invention proposes a large underground space positioning method based on tight coupling of map / laser / inertial. By tightly coupling the point-to-plane feature residuals of the global point cloud map, the inertial pre-integration residuals, and the point-to-plane and point-to-plane feature residuals of the local point cloud map into a unified optimization framework, it solves the problems of insufficient positioning robustness and performance degradation of traditional fusion positioning methods in complex underground environments, and realizes high-precision, high-real-time, and high-robustness positioning in large underground space scenarios, providing a reliable solution for high-precision positioning in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 This is a positioning flow chart of a large underground space positioning method based on map / laser / inertial tight coupling in an embodiment of the present invention.

[0060] Figure 2 This is a three-dimensional grid division diagram of a large underground space positioning method based on map / laser / inertial tight coupling in an embodiment of the present invention.

[0061] Figure 3 This is a positioning trajectory diagram of a large underground space positioning method based on map / laser / inertial tight coupling in an embodiment of the present invention. DETAILED DESCRIPTION

[0062] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0063] This embodiment designs a large underground space positioning method based on map / laser / inertial tight coupling, such as Figure 1 As shown, the following steps are included:

[0064] Step 1: Load the global 3D point cloud map of the underground space and perform a 3D voxel downsampling. Calculate the boundary value of the map and divide the downsampled map into indivual The coordinates of each point in the point cloud map are traversed and stored in the corresponding three-dimensional grid according to the grid division rules to achieve lightweight processing of the global map, such as Figure 2 shown.

[0065] Step 2, Collection Time IMU (Inertial Measurement Unit) and laser point cloud data , and extract the current point cloud frame Edge characteristics Dough characteristics .

[0066] Step 3: Determine whether the system has completed map matching initialization. If it has, jump to step 5; otherwise, go to step 4.

[0067] Step 4: Enter the global map lightweight initialization phase. According to the current position of the carrier, use the initial pose setting tool on the rviz (visualization tool) in ROS (Robot Operating System) to determine the initial pose According to the initial pose In step 1, the Find the closest grid in a three-dimensional grid and form a new grid point cloud . Current frame point cloud Surface features and Matching is performed using the ICP (Iterative Closest Point) algorithm. If the match is successful, the precise initial point pose is obtained. , complete initialization and go to step 5. If the matching is unsuccessful, clear the point cloud and IMU data and jump to step 2 to re-initialize the map.

[0068] Step 5, after the last iteration optimization ( arrive The gyroscope and acceleration zero bias at the moment are input to the pre-integration calculation module. Time point cloud and The inertial data between the time point clouds are pre-integrated to obtain Time has come The change in posture at the moment , and update the initial pose .

[0069] Step 6: Construct the pre-integration factor, the global lightweight map matching residual based on point-to-plane, and the local point cloud map matching residual based on point-to-plane and point-to-line, and enter them into a unified optimization framework for iterative optimization.

[0070] Step 7: Get the optimal global positioning result Go to step 2 to enter the next moment calculation process.

[0071] The specific method of step 1 is:

[0072] Traverse each 3D coordinate point in the global point cloud map , is the number of coordinates of the global point graph. The position of the point in the 3D grid for:

[0073] ;

[0074] in, calculate Specific coordinates in the three-dimensional grid In step 1 It can be expressed as:

[0075] ;

[0076] in Respectively represent the global point cloud map in The maximum boundary value in three directions.

[0077] but The position index in the three-dimensional grid is expressed as:

[0078] ;

[0079] Based on the above method, Stored in a three-dimensional grid , completing lightweight storage.

[0080] The specific method of step 2 is:

[0081] Point cloud frame The edge and surface feature extraction method is:

[0082] ;

[0083] in, for A coordinate point in ; For The interval consisting of the left and right neighboring points on the same scan line is centered. is the calculated curvature value.

[0084] like is greater than the corresponding threshold, then It is judged as an edge point and stored in On the contrary, if If it is less than a certain threshold, the point is determined to be a plane point and stored in middle.

[0085] Furthermore, the specific method of step 4 is:

[0086] Get the initial pose The three-dimensional position coordinates in , and search for the coordinate index of the three-dimensional grid corresponding to the initial position coordinate according to the method in step 1 .Bundle , , The 3D meshes within the range are fused into a new local point cloud frame , to achieve lightweight processing of the global map to be matched. For the sake of explanation, Figure 2 A schematic diagram of the fusion principle of two-dimensional grids is given, and the principle of three-dimensional grids is similar.

[0087] Current frame point cloud Corresponding surface features and The optimal initial pose is obtained by iterative matching through ICP:

[0088] ;

[0089] The above ICP matching algorithm obtains the optimal initialization pose under the condition of meeting the maximum number of iterations or the optimal matching score. And complete the initialization. If the above formula does not converge, jump to step 2 to re-initialize the map.

[0090] The specific method of step 6 is:

[0091] State quantity to be optimized for:

[0092] ;

[0093] in, is the sliding window of the state variables to be optimized. . 、 、 、 、 They represent three-dimensional position, velocity, quaternion, gyroscope bias, and accelerometer bias respectively.

[0094] Construct a residual optimization function that integrates pre-integration factors, global map matching based on point-to-plane, and local point cloud map matching based on point-to-plane and point-to-line:

[0095] ;

[0096] in, is the inertia pre-integration residual function, represent Sliding window of time intervals. represents the inertial pre-integration between the k-1 time point cloud frame and the k time, is the covariance of the inertia pre-integration factor;

[0097] Global lightweight point cloud map matching residuals for point-to-plane. is the covariance of global map matching; Represents the surface features extracted from the point cloud of the current frame The lightweight global map extracted according to the method described in step 4 The point-to-surface error between Every point in , obtained in step 5 Transformed into the global coordinate system, we get .exist Search and The 5 nearest points are fitted into a plane using the least squares method, and the plane equation is:

[0098] ;

[0099] in is the plane normal vector, is a constant term. Then Distance to plane for:

[0100] ;

[0101] Further available Projection point on the plane for:

[0102] ;

[0103] thereby:

[0104] ;

[0105] in, for Moment surface features The number of coordinate points. The state quantity to be optimized The position and posture part of the .

[0106] For local laser point cloud map matching, Indicates the current Moment surface features and surface features in local point cloud maps The point-to-plane residuals between . It is a sliding window map composed of 20 consecutive frames of point cloud. It is important to distinguish and the above mentioned The distinction, is a local map constructed without global correction, containing accumulated errors, while It is a high-precision point cloud map with no global drift. is the covariance of local map surface feature matching. This is similar to the point-to-plane residual formula in the global map above and will not be repeated here.

[0107] For the current Time edge characteristics and edge features of local point cloud maps The point-to-line residuals constructed between . is the covariance of local map feature matching. Each edge feature point , obtained in step 5 Transformed into the global coordinate system, we get ,exist Search and The last 5 points . Calculate the covariance matrix of these 5 points :

[0108] ;

[0109] in is the center of mass.

[0110] right Perform matrix decomposition, , . ,and .

[0111] Maximum eigenvalue If it is greater than the next largest eigenvalue 3 times, then The corresponding eigenvector Point Cloud The direction of the fitted line in . The two endpoints on the line can be expressed as:

[0112] ;

[0113] ;

[0114] ;

[0115] in, for Time edge characteristics The number of coordinate points.

[0116] Based on the Gauss-Newton iterative optimization method, the objective function is gradually optimized. When the error in the iterative process meets the convergence condition or reaches the pre-set maximum number of iterations, the optimization process ends. At this point, the optimal solution of the optimized state variable is was determined.

[0117] Specifically, such as Figure 3 As shown, this is the test result of this embodiment in a large underground space. The trajectory length exceeds 5 km. It can be seen from the figure that the starting point and the end point trajectory of this embodiment completely coincide with each other, and the error is at the centimeter level.

[0118] In summary, the present invention proposes a large-scale underground space positioning method based on tight coupling of map / laser / inertial. By lightweight processing of large-scale global point cloud maps and tightly coupling pre-integration factors, global lightweight map matching residuals based on point-to-plane, and local point cloud map matching residuals based on point-to-plane and point-to-line into a unified optimization framework, the present invention solves the problems faced by traditional fusion positioning methods based on global point cloud maps, such as high large-scale spatial calculation complexity, insufficient hardware applicability, and positioning performance degradation in complex underground environments, and achieves high-precision, high-real-time, and high-robust positioning in large underground space scenarios.

[0119] Those skilled in the art will appreciate that the embodiments described are intended to help readers understand the principles of the present invention and should be understood that the scope of protection of the present invention is not limited to the embodiments described. It will be apparent to those skilled in the art that various modifications and variations are possible in the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention are intended to be included within the scope of the claims of the present invention.

Claims

1. A large underground space positioning method based on map / laser / inertial tight coupling, characterized in that: The following steps are involved: Step 1: Load the global 3D point cloud map of the underground space and perform a 3D voxel downsampling; calculate the boundary value of the map and divide the downsampled map into A three-dimensional grid; traverse the coordinates of each point in the point cloud map and store them in the corresponding three-dimensional grid according to the grid division rules; Step 2, Collection Real-time IMU and laser point cloud data , and extract the current point cloud frame Edge characteristics Dough characteristics ; Step 3: Determine whether the system has completed map matching initialization; If initialization is completed, jump to step 5, otherwise go to step 4; Step 4: Use the initial pose setting tool on rviz in ROS to determine the initial pose according to the current position of the carrier ; According to the initial posture In step 1, the Find the closest grid in a three-dimensional grid and form a new grid point cloud ; Current frame point cloud Surface features and Matching is performed through the ICP algorithm. If the match is successful, the accurate initial point pose is obtained. , complete initialization and go to step 5; if the matching is unsuccessful, clear the point cloud and IMU data and jump to step 2 to re-initialize the map; Step 5: The gyroscope and acceleration bias optimized in the last iteration are input to the pre-integration calculation module. Time point cloud and The inertial data between the time point clouds are pre-integrated to obtain Time has come The change in posture at the moment , and update the initial pose ; Step 6: Construct the pre-integration factor, the global lightweight map matching residual based on point-to-plane, and the local point cloud map matching residuals based on point-to-plane and point-to-line, and enter them into a unified optimization framework for iterative optimization. Step 7: Get the optimal global positioning result ; Go to step 2 to enter the next moment calculation process; The specific method of step 1 is: traverse each three-dimensional coordinate point in the global point cloud map , is the number of coordinates of the global point graph; then The position of the point in the 3D grid for: ； in, calculate Specific coordinates in the three-dimensional grid ; In step 1 It can be expressed as: ； in Respectively represent the global point cloud map in Maximum boundary values ​​in three directions; but The position index in the three-dimensional grid is expressed as: ； Based on the above method, Store into 3D grid .

2. The underground large space positioning method based on map / laser / inertial tight coupling according to claim 1 is characterized in that: The specific method of step 4 is: Get the initial pose The three-dimensional position coordinates in , and search the coordinate index of the three-dimensional grid corresponding to the initial position coordinate according to step 1 ;Bundle , , The 3D meshes within the range are fused into a new local point cloud frame , to achieve lightweight processing of the global map to be matched; Current frame point cloud Corresponding surface features and The optimal initial pose is obtained by iterative matching through ICP: ； The above ICP matching algorithm obtains the optimal initialization pose under the condition of meeting the maximum number of iterations or the optimal matching score. And complete the initialization. If the above formula does not converge, jump to step 2 to re-initialize the map.

3. The underground large space positioning method based on map / laser / inertial tight coupling according to claim 2 is characterized in that: The specific process of step 6 is as follows: State quantity to be optimized for: ； in, is the sliding window of the state variable to be optimized; ; 、 、 、 、 Represent three-dimensional position, velocity, quaternion, gyroscope bias and accelerometer bias respectively; Construct a residual optimization function that integrates pre-integration factors, global map matching based on point-to-plane, and local point cloud map matching based on point-to-plane and point-to-line: ； in, is the inertia pre-integration residual function, represent Sliding windows of time intervals; represents the inertial pre-integration between the k-1 time point cloud frame and the k time, is the covariance of the inertia pre-integration factor; Matching residuals for the global lightweight point cloud map between points and surfaces; is the covariance of global map matching; Represents the surface features extracted from the point cloud of the current frame The lightweight global map extracted according to the method described in step 4 The point-to-surface error between Every point in , obtained in step 5 Transformed into the global coordinate system, we get ;exist Search and The 5 nearest points are fitted into a plane using the least squares method, and the plane equation is: ； in is the plane normal vector, is a constant term; then Distance to plane for: ； Further available Projection point on the plane for: ； thereby: ； in, for Moment surface features The number of coordinate points; The state quantity to be optimized The position and posture part of For local laser point cloud map matching, Indicates the current Moment surface features and surface features in local point cloud maps The point-to-plane residuals between It is a sliding window map composed of 20 consecutive frames of point cloud. The local map constructed without global correction contains accumulated errors; is the covariance of local map feature matching; For the current Time edge characteristics and edge features of local point cloud maps The point-to-line residuals constructed between ; is the covariance of local map feature matching; Each edge feature point , obtained in step 5 Transformed into the global coordinate system, we get ,exist Search and The last 5 points ; Calculate the covariance matrix of these 5 points : ； in is the center of mass; right Perform matrix decomposition, , ; ,and ; Maximum eigenvalue If it is greater than the next largest eigenvalue 3 times, then The corresponding eigenvector Point Cloud The direction of the fitted line in ; the two endpoints on the line can be expressed as: ； ； ； in, for Time edge characteristics The number of coordinate points; Based on the Gauss-Newton iterative optimization method, the objective function is gradually optimized. When the error in the iterative process meets the convergence condition or reaches the pre-set maximum number of iterations, the optimization process is terminated. At this time, the optimal solution of the optimized state variable is was determined.

Images