A Laser Inertial Odometry Method for Shield Tunnels

Abstract

A laser inertial odometer method for shield tunnels, the specific steps are as follows: the odometer front-end pose estimation module uses an iterative error state Kalman filter to achieve initial pose estimation and global map construction; the shield tunnel segment recognition module realizes accurate segment recognition by performing bolt hole extraction; the segment odometer module fuses the direction constraints executed by the local IMU sliding window to judge the movement direction of the carrier and realizes accurate segment mileage constraints; the back-end optimization module fuses the odometer constraints, prior pose constraints and segment mileage constraints provided by the front-end odometer to achieve pose update, overcomes the interference of the repetitive structure of the odometer in shield subway tunnels, realizes high-precision and low-drift unmanned inspection positioning, enables the accurate approach of the autonomous inspection system to disease detection, and promotes the development of intelligent and unmanned operation and maintenance of subway tunnels.

Description

Technical Field

[0001] The present invention relates to a laser inertial odometer method for shield tunnels, belonging to the technical field of underground space navigation and positioning. Background Art

[0002] With the rapid development of rail transit, subway tunnels, as core infrastructure, their long-term safety directly affects the stable operation. Diseases such as water seepage, falling blocks, misalignment, and tiny cracks, if not discovered and treated in time, may lead to structural deterioration and even affect the safety of train operation. Therefore, improving the inspection accuracy and detection efficiency and realizing unmanned and intelligent tunnel monitoring have become an urgent need in the industry.

[0003] The existing inspection methods mainly rely on manual inspection and track inspection vehicles. However, manual inspection has low efficiency and large subjective errors, and it is difficult to accurately identify tiny cracks and initial leaks. Track inspection vehicles are limited by the track laying, and it is difficult to capture minor diseases in long-distance scanning, and the inspection range is limited, making it difficult to meet the requirements of high-precision and full-coverage detection. In contrast, autonomous inspection systems such as drones and robots can approach the tunnel wall, and through high-resolution vision, lidar, and ultrasonic sensors, accurately identify disease characteristics at the millimeter level and achieve autonomous operation, improving the inspection accuracy and efficiency.

[0004] However, the closed environment and repetitive structure of the tunnel pose challenges to the precise positioning of unmanned inspection systems. The shield segment structure and texture are highly similar, resulting in easy registration errors and "standing still in place" for methods based on laser SLAM. The absence of GNSS signals makes traditional integrated navigation systems ineffective, and visual odometry is affected by low light and difficult to stably extract feature points. In addition, existing laser inertial odometers (LIO) have error accumulation in long-distance inspections, lack constraints in the X-axis direction, are prone to drift in the X-axis, and cannot achieve positioning. Summary of the Invention

[0005] The object of the present invention is to provide a laser inertial odometer method for shield tunnels, which can overcome the interference of repetitive structures, achieve high-precision and low-drift unmanned inspection positioning, support precise close-range disease detection of autonomous inspection systems, and promote the development of intelligent and unmanned operation and maintenance of subway tunnels.

[0006] To achieve the above object, the present invention provides a shield tunnel laser inertial odometer method, and the hardware it is based on includes a front-end odometer, a segment odometer module, and a back-end optimization module; the front-end odometer is a carrier pose estimation module based on error state Kalman filter estimation; the segment odometer module uses a bolt hole extraction method to identify segments in the shield tunnel point cloud, and uses local IMU measurement direction constraints to count the number of segments in the shield tunnel, providing segment odometer constraints; the back-end optimization module fuses the front-end odometer constraints, segment odometer constraints, and prior pose constraints to perform factor graph optimization to obtain the final tunnel odometer positioning information;

[0007] It includes the following steps:

[0008] S1. Align the time stamps of the acquired Lidar point cloud data and IMU measurements using the front-end odometer, estimate the local pose change based on point-to-surface registration, and then obtain the carrier's global odometer pose and the constructed global map of the shield tunnel based on the error state Kalman filter;

[0009] S2. Identify the initial shield tunnel segments from the global map of the shield tunnel constructed by the front-end odometer, and obtain the initial positions of the segments. When the carrier moves to the initial segment position, the segment odometer starts counting, and the pose of the carrier at this time is set as the initial pose of the segment odometer;

[0010] S3. Maintain an IMU direction constraint sliding window near the current frame of the carrier pose estimation to monitor whether the carrier makes a U-turn behavior, including instantaneous changes in the X-axis forward direction and slow changes in the X-axis, and obtain the segment number statistics of the segment odometer guided by the carrier behavior through judgment;

[0011] S4. Use the back-end optimization module to perform factor graph optimization using the odometer constraints provided by the front-end of the laser inertial odometer, the odometer constraints provided by the segment odometer, and the prior position constraints to obtain the final tunnel odometer positioning and the updated global map.

[0012] Further, the specific process of S1 is as follows:

[0013] S1.1. Obtain the Lidar point cloud and the IMU measurement and synchronize them based on the time stamp, and use IMU pre-integration for interpolation to improve the accuracy of the time synchronization of the Lidar point cloud and the IMU measurement The IMU pre-integration calculation is as follows:

[0014] ;

[0015] Wherein, is the angular velocity at time k; is the angular velocity at time k; is the IMU measurement interval; represents the angular velocity bias; respectively represent the velocity, rotation, and position at time i; R k is the rotation of the IMU at time k;

[0016] S1.2. Correct the distortion of the Lidar point cloud using the pose obtained from IMU pre-integration: ;

[0017] Among them, represents the rotation transformation of the LIDAR point cloud within one frame to the end of the frame, represents the translation transformation of the LIDAR point cloud within one frame to the end of the frame; P k represents the radar point cloud at time k;

[0018] S1.3. Align the time of the undistorted Lidar point cloud with the IMU measurement, and perform iterative error state Kalman filtering to solve for the real-time pose of the vehicle. Considering that the IMU update frequency is greater than the Lidar update frequency, when there is no Lidar point cloud input, only perform attitude propagation through IMU measurements. Define the following partial variables:

[0019] ; ;

[0020] ;

[0021] Among them, respectively represent the transpose of, the transpose of, the transpose of, the transpose of, the transpose of, the transpose of, represents the angular velocity measurement value, represents the acceleration measurement value, represents the angular velocity measurement noise, represents the acceleration measurement noise, represents the angular velocity bias measurement noise, represents the acceleration bias measurement noise;

[0022] are respectively the rotation, position, velocity, angular velocity bias, acceleration bias, gravitational acceleration, rotation from the LIDAR coordinate system to the IMU coordinate system, and translation from the LIDAR coordinate system to the IMU coordinate system of the IMU in the world coordinate system; Assume the state of the frame of the radar frame is , the covariance matrix is , when the IMU measurement frequency is greater than the radar measurement frequency and there is no point cloud frame input, the state is propagated using IMU measurements:

[0023] ;

[0024] ;

[0025] ;

[0026] ;

[0027] where and represent the estimated state of the i-th IMU frame and the estimated state of the frame respectively, and represent the estimated covariance matrix of the i-th IMU frame and the estimated covariance matrix of the frame respectively, is the covariance matrix of , and are solved by the following formula:

[0028] ; ;

[0029] where represents the estimated value of the state variable, and are respectively day Jacobin matrices of the error state and the noise at "0";

[0030] When both LIDAR frame and IMU frame measurements are input, project the L system to the G system through the following formula, where the L system is the LIDAR system and the G system is the world coordinate system: ;

[0031] where is the unit vector in the direction of the cylinder axis, is the coordinate of a point on the cylinder axis in the global coordinate system, r represents the radius of the cylinder, , is with respect to Jacobin matrix at "0", is related to the original observation noise ; ;

[0032] Represents the residual of the point without error to the elliptical cylinder surface, Represents the residual value of the observed value at the specific 0, is the true value, is the estimated value, is the error value; The error state is iteratively updated as follows:

[0033] ;

[0034] ;

[0035] is an operation, and the operation rule is as follows: ; ;

[0036] Among them, , is the IMU pose provided by the pose propagation model, is the IMU pose of the k-th iteration, is the extrinsic parameter between the IMU and LIDAR provided by the pose propagation model, is the extrinsic parameter between the IMU and LIDAR of the -th iteration; ;

[0037] Maximum a posteriori estimation of the error state: ;

[0038] Among them, represents the two-norm operation, represents the th iteration is the covariance of the error state , represents The covariance of can be obtained by the propagation of the covariance factor. Let The Hessian matrix of all points in the k-th frame, The covariance matrix after linearization of all points in the k-th frame, is the covariance matrix of the error state of all points in the k-th frame, ; Represents the residual of the j-th point to the cylinder surface in the k-th frame;

[0039] ; ;

[0040] Among them, represents all the residuals of the -th frame, that is, the residuals of the points are iteratively updated residuals after times;

[0041] Repeat the above process until convergence, , where is the set empirical threshold;

[0042] ; ;

[0043] Among them, is the posterior pose after Kalman filter update. Finally, according to the obtained pose the current frame point cloud is converted into a global point cloud .

[0044] Furthermore, the specific process of S2 is as follows:

[0045] S2.1. Coarse extraction is performed based on the geometric distribution of the bolt hole point cloud. The bolt hole point cloud is extracted by the height difference threshold. Take an unfolded tunnel inner wall point , calculate the height differences between point M and the four points l above, below, left, and right of it, denoted as , ; for the height differences of the upper and lower parts, take the larger value and denote it as ; for the height differences of the left and right parts, take the larger value and denote it as ; if , are both greater than the preset screening threshold , then this point is considered as a bolt hole candidate point cloud;

[0046] S2.2. Since the point cloud normal vectors of the tunnel wall have strong unity and usually show a consistent normal distribution in a local area, while the point cloud normal vectors of the bolt holes show greater discreteness and irregularity, the candidate bolt hole point cloud is further refined based on the normal vector characteristics:

[0047] S2.2-1. First, divide the unfolded point cloud into small point clouds of 1.2M × 1.2M for subsequent processing; let the original point cloud , and the block division principle is: ;

[0048] Among them, is the block index;

[0049] S2.2-2. Use RANSAC to fit the plane equation of the block point cloud: , satisfying: ;

[0050] S2.2-3. Calculate the normal vector of each point and its angle with the plane normal vector , and the distance of each point from the fitted plane , the calculation formula is as follows: ;

[0051] S2.2-4. By setting the angle threshold , and the distance threshold , , to determine whether the point cloud belongs to the bolt hole point cloud, the formula is as follows: ;

[0052] Among them, is the set empirical threshold, is the point cloud filtered by the angle and distance thresholds;

[0053] S2.2-5. Use the density-based clustering algorithm DBSCAN to perform clustering analysis on the extracted bolt hole point cloud, and clean the noise points in the bolt hole point cloud by setting appropriate radius and minimum sample number parameters:

[0054] ;

[0055] Among them, is the neighborhood radius, is the minimum sample number;

[0056] S2.2-5-1. Traverse all the bolt hole clusters extracted, calculate the geometric center coordinates of each cluster, and record the geometric center coordinates of the bolt hole cluster as: ;

[0057] Among them, is the center coordinate of the rth bolt hole cluster, is the coordinate of the sth bolt hole point in the rth cluster, is the number of points in this cluster;

[0058] Analyze the distance between the center points of the bolt hole clusters along the mileage direction of the shield tunnel. According to the structural characteristics of the segment ring, the distance between the center points of adjacent bolt holes inside a segment ring is greater than the distance between the center points of bolt holes between adjacent segment rings, that is, the following relationship is satisfied: ;

[0059] Among them, is the distance between the center points of adjacent bolt hole clusters within the same segment ring, is the distance between the center points of bolt hole clusters between adjacent segment rings; According to the distribution characteristic that the distance between the center points of bolt hole clusters between adjacent segment rings is greater than the distance between the center points of adjacent bolt hole clusters within the same segment ring, divide the bolt hole clusters into their respective corresponding segment rings: ;

[0060] Among them, The set of bolt holes within one ring , is a certain bolt hole within Ring 1;

[0061] S2.2-5-2. Based on the bolt hole positions of the identified first and second ring segments, determine the global coordinate positions of the initial circumferential seam in the advancing direction of the first ring segment; when the mobile carrier moves to the position of the initial circumferential seam, record and set the pose of the mobile carrier at this time as the initial pose of the ring segment odometer.

[0062] Furthermore, the specific process of S3 is as follows:

[0063] S3.1. Maintain the local IMU sliding window W, use the first frame within the window as the local coordinate system, integrate the IMU measurements within the window, and obtain the direction change of the carrier during the local IMU sliding window time: ; ;

[0064] wherein, is the angular velocity of the IMU measurement at time i, is the IMU measurement interval, and N is the number of IMU frames maintained within the local IMU sliding window;

[0065] S3.2. Detect the turning behavior of the carrier in a short period of time, including yaw and pitch angle constraints: ; ;

[0066] wherein, is the change in the yaw angle within the local IMU sliding window, is the change in the pitch angle within the local IMU sliding window, is the empirical threshold for angular turning, used to determine whether reverse movement occurs;

[0067] S3.3. Combine the "zero speed" detection in the X-axis direction, and the formula is as follows: ;

[0068] wherein, is the speed in the advancing direction, is the empirical threshold for judging the linear velocity at rest, used to determine whether the movement stops;

[0069] S3.4. Judge the movement direction of the carrier according to the detection result, represented by the symbol , and dynamically update the shield ring segment statistics : ; ;

[0070] wherein, is the number of identified ring segments, It is a flag for judging whether reverse movement occurs according to the local IMU sliding window, and no reverse movement occurs This operation is performed whenever a ring block is recognized.

[0071] Furthermore, the specific process of S4 is as follows: using the odometry constraint provided by the front end of the laser inertial odometer which is obtained through an iterative error state Kalman filter and the odometry constraint provided by the ring slice odometer By multiplying the number of recognized ring slices by the width of the ring slice (usually 1.2 meters) and the prior position constraint which is the global pose obtained in the previous cycle, after obtaining each constraint factor, perform back-end factor graph optimization to finally obtain the optimized tunnel odometer positioning and the updated global map .

[0072] In the present invention, the initial pose estimation and global map construction are realized by the odometry front-end pose estimation module using an iterative error state Kalman filter; the shield tunnel ring slice recognition module realizes accurate recognition of the ring slice by performing bolt hole extraction; the ring slice odometer module fuses the direction constraint executed by the local IMU sliding window to judge the movement direction of the carrier, realizing accurate ring slice odometry constraint; the back-end optimization module fuses the odometry constraint, prior pose constraint and ring slice odometry constraint provided by the front-end odometer to realize pose update, overcomes the interference of the repetitive structure of the odometer in the shield subway tunnel, realizes high-precision and low-drift unmanned inspection positioning, enables the accurate approach of the autonomous inspection system to disease detection, and promotes the development of intelligent and unmanned operation and maintenance of the subway tunnel. Brief Description of the Drawings

[0073] Figure 1 is the workflow diagram of the present invention;

[0074] Figure 2 is the flow chart of single-ring slice recognition of the shield tunnel of the present invention;

[0075] Figure 3 is the schematic diagram of the local point cloud projection expansion of the present invention, where Figure 3 in (a) is a plane screenshot and (b) is a three-dimensional view;

[0076] Figure 4 is the schematic diagram of bolt hole extraction of the present invention;

[0077] Figure 5 is the schematic diagram of the existing odometer method in the positioning of the subway shield tunnel. Detailed Embodiment

[0078] The present invention will be further described below with reference to the accompanying drawings.

[0079] AsFigure 1 As shown in Figure 1 , a laser inertial odometer method for shield tunnels, the hardware it is based on includes a front-end odometer, a segment odometer module, and a back-end optimization module; the front-end odometer is a carrier pose estimation module based on error-state Kalman filter estimation; the segment odometer module uses a bolt hole extraction method to identify segments in the shield tunnel point cloud, and uses local IMU measurement direction constraints to count the number of segments in the shield tunnel, providing segment odometer constraints; the back-end optimization module fuses the front-end odometer constraints, segment odometer constraints, and prior pose constraints to perform factor graph optimization to obtain the final tunnel odometer positioning information;

[0080] It includes the following steps:

[0081] S1. Align the time stamps of the acquired Lidar point cloud data and IMU measurements using the front-end odometer, estimate the local pose change based on point-to-surface registration, and then obtain the carrier's global odometer pose and the constructed global map of the shield tunnel based on error-state Kalman filter;

[0082] S2. Identify the initial shield tunnel segments from the global map of the shield tunnel constructed by the front-end odometer, and obtain the initial positions of the segments. When the carrier moves to the initial segment position, the segment odometer starts counting, and the pose of the carrier at this time is set as the initial pose of the segment odometer;

[0083] S3. Maintain an IMU direction constraint sliding window near the current frame of the carrier pose estimation to monitor whether the carrier makes a U-turn behavior, including instantaneous changes in the X-axis forward direction and slow changes in the X-axis, and obtain the segment number statistics of the segment odometer guided by the carrier behavior through judgment;

[0084] S4. Use the back-end optimization module to perform factor graph optimization using the odometer constraints provided by the front-end of the laser inertial odometer, the odometer constraints provided by the segment odometer, and the prior position constraints to obtain the final tunnel odometer positioning and the updated global map.

[0085] The specific process of S1 is as follows:

[0086] S1.1. Obtain the Lidar point cloud and the IMU measurement and synchronize them based on the time stamp. Use IMU pre-integration for interpolation to improve the accuracy of the time synchronization between the Lidar point cloud and the IMU measurement The IMU pre-integration is calculated as follows:

[0087] ;

[0088] where, is the angular velocity at time k; is the angular velocity at time k; is the IMU measurement interval; represents the angular velocity bias; respectively represent the velocity, rotation, and position at time i; R k is the rotation of the IMU at time k;

[0089] S1.2. Distort the Lidar point cloud through the pose of IMU pre-integration: ;

[0090] Among them, represents the rotation transformation of the LIDAR point cloud within one frame to the end of the frame, represents the translation transformation of the LIDAR point cloud within one frame to the end of the frame; P k represents the radar point cloud at time k;

[0091] S1.3. Align the time of the undistorted Lidar point cloud and the IMU measurement, and perform iterative error state Kalman filtering to solve the real-time pose of the carrier. Considering that the IMU update frequency is greater than the Lidar update frequency, when there is no Lidar point cloud input, only perform attitude propagation through IMU measurement. Define the following partial variables:

[0092] ; ;

[0093] ;

[0094] Among them, respectively represent the transpose of, the transpose of, the transpose of, the transpose of, the transpose of, the transpose of, represents the angular velocity measurement value, represents the acceleration measurement value, represents the angular velocity measurement noise, represents the acceleration measurement noise, represents the angular velocity bias measurement noise, represents the acceleration bias measurement noise;

[0095] are respectively the rotation, position, velocity, angular velocity bias, acceleration bias, gravitational acceleration, rotation from the LIDAR coordinate system to the IMU coordinate system, and translation from the LIDAR coordinate system to the IMU coordinate system of the IMU in the world coordinate system;

[0096] Assume The state of the frame radar frame is , and the covariance matrix is . When the IMU measurement frequency is greater than the radar measurement frequency and there is no point cloud frame input, the state is propagated using IMU measurements:

[0097] ;

[0098] ;

[0099] ;

[0100] ;

[0101] Among them, and respectively represent the estimated state of the i-th frame of IMU and the estimated state of the -th frame, and respectively represent the estimated covariance matrix of the i-th frame of IMU and the estimated covariance matrix of the -th frame, is the covariance matrix, and are solved by the following formula:

[0102] ; ;

[0103] Among them, represents the estimated value of the state variable, and are respectively day Jacobin matrices of the error state and the noise at "0";

[0104] When both LIDAR frame and IMU frame measurements are input, project the L system to the G system through the following formula, where the L system is the LIDAR system and the G system is the world coordinate system: ;

[0105] Among them, is the unit vector in the direction of the cylinder axis, is the coordinate of a point on the cylinder axis in the global coordinate system, r represents the radius of the cylinder, , is with respect to Jacobin matrix at "0", and the original observation noise related to; ;

[0106] represents the residual from the error - free point to the elliptical cylinder surface, represents the residual value of the observed value at a specific 0, is the true value, is the estimated value, is the error value; The error state is iteratively updated as follows:

[0107] ;

[0108] ;

[0109] is an operation, and the operation rule is as follows: ; ;

[0110] where, , is the IMU pose provided by the pose propagation model, is the IMU pose at the k - th iteration, is the extrinsic parameter between the IMU and LIDAR provided by the pose propagation model, is the extrinsic parameter between the IMU and LIDAR at the k - th iteration; ;

[0111] Maximum a posteriori estimation of the error state: ;

[0112] where, represents the two - norm operation, represents the th iteration is the covariance of the error state , represents that the covariance of can be obtained by the propagation of the covariance factor. Let be the Hessian matrix of all points in the k - th frame, be the covariance matrix after linearization of all points in the k - th frame, be the covariance matrix of the error state of all points in the k - th frame, ; represents the residual from the j - th point in the k - th frame to the cylinder surface;

[0113] ; ;

[0114] where, represents all the residuals in the th frame, that is, the residuals of points are iteratively updated times;

[0115] Repeat the above process until convergence, , where is the set empirical threshold;

[0116] ; ;

[0117] Among them, is the posterior pose after Kalman filter update. Finally, according to the obtained pose transform the current frame point cloud into the global point cloud .

[0118] As Figure 2 shown, the process of shield tunnel segment recognition is as follows: In the initial stage, since the inertial measurement unit (IMU) sensor does not have serious error accumulation, accurate mapping of the shield tunnel can be achieved. According to the current frame pose cut the global map to obtain the global point cloud map of the current frame position, and retain the point clouds of two rings of shield tunnels , and realize the initial segment recognition and subsequent segment recognition based on the bolt hole point cloud. The specific steps are as follows:

[0119] (1), Coarse extraction is carried out based on the geometric distribution of the bolt hole point cloud. The bolt hole point cloud is extracted by the height difference threshold. Take an unfolded tunnel inner wall point , calculate the height differences between point M and the four points at a distance of l above, below, left, and right of it, denoted as , ; For the height differences of the upper and lower parts, take the larger value and denote it as ; For the height differences of the left and right parts, take the larger value and denote it as ; If , are both greater than the preset screening threshold , then this point is considered as a bolt hole candidate point cloud;

[0120] (2), Since the normal vectors of the point clouds on the tunnel wall have strong unity and usually show a consistent normal distribution in the local area, while the normal vectors of the bolt hole point clouds show large discreteness and irregularity, further fine extraction of the candidate bolt hole point clouds is carried out based on the normal vector characteristics:

[0121] (2-1), As Figure 3 shown in (a) and (b) of, first divide the unfolded point cloud into small point clouds of 1.2M × 1.2M for subsequent processing; Let the original point cloud , and the block division principle is: ;

[0122] Among them, is the block index;

[0123] (2-2), Use RANSAC to fit the plane equation of the block point cloud: , satisfying: ;

[0124] (2-3), Calculate the normal vector of each point and its included angle with the plane normal vector , as well as the distance of each point from the fitted plane. The calculation formula is as follows: ;

[0125] (2-4), By setting the angle threshold , and the distance threshold , , determine whether the point cloud belongs to the bolt hole point cloud. The formula is as follows: ;

[0126] Among them, is the set empirical threshold, is the point cloud filtered by the angle and distance thresholds;

[0127] (2-5), Use the density-based clustering algorithm DBSCAN to perform clustering analysis on the extracted bolt hole point cloud, and clean the noise points in the bolt hole point cloud by setting appropriate radius and minimum sample number parameters:

[0128] ;

[0129] Among them, is the neighborhood radius, is the minimum sample number;

[0130] (2-5-1), Traverse all the bolt hole clusters extracted, calculate the geometric center coordinates of each cluster, and record the geometric center coordinates of the bolt hole cluster as: ;

[0131] Among them, is the center coordinate of the r-th bolt hole cluster, is the coordinate of the s-th bolt hole point in the r-th cluster, is the number of points in this cluster;

[0132] Such as Figure 4As shown in the figure, the center point spacing of the bolt holes is analyzed along the mileage direction of the shield tunnel. According to the structural characteristics of the segment ring, the distance between the center points of adjacent bolt holes inside a segment ring is greater than the distance between the center points of bolt holes between adjacent segment rings, that is, the following relationship is satisfied: ;

[0133] Among them, is the distance between the clustering centers of adjacent bolt holes within the same segment ring, is the distance between the clustering centers of bolt holes between adjacent segment rings; according to the distribution characteristic that the distance between the clustering centers of bolt holes between adjacent segment rings is greater than the distance between the clustering centers of adjacent bolt holes within the same segment ring, the bolt hole clustering is divided into their respective corresponding segment rings: ;

[0134] Among them, is the set of bolt holes within one ring , is a certain bolt hole within ring 1;

[0135] (2-5-2) Based on the bolt hole positions of the identified first segment and the second segment, determine the global coordinate position of the initial ring joint in the advancing direction of the first segment; when the moving carrier moves to the position of the initial ring joint, record and set the pose of the moving carrier at this time as the initial pose of the segment odometer.

[0136] The specific process of monitoring whether the carrier has a U-turn behavior and obtaining the segment number statistics of the carrier behavior guiding segment odometer is as follows:

[0137] (1) Maintain the local IMU sliding window. Taking the first frame in the sliding window as the local coordinate system, integrate the IMU measurements within the window to obtain the direction change of the carrier within the local IMU sliding window time: ; ;

[0138] Among them, is the IMU measurement angular velocity at time i, is the IMU measurement interval, and N is the number of IMU frames maintained within the local IMU sliding window;

[0139] S3.2. Detect the U-turn behavior of the carrier in a short time, including the yaw and pitch angle constraints: ; ;

[0140] Among them, is the change in the course deviation angle within the local IMU sliding window, is the change in the pitch angle within the local IMU sliding window, is the empirical threshold of the angle turn, used to judge whether reverse movement occurs;

[0141] (3) Combine the "zero speed" detection in the X-axis direction, and the formula is as follows: ;

[0142] Among them, is the speed in the forward direction, is the empirical threshold for judging the linear speed at rest, used to judge whether the movement has stopped;

[0143] (4) Judge the movement direction of the carrier according to the detection result, which is represented by the symbol , and dynamically update the shield segment statistics : ; ;

[0144] Among them, is the number of segments that have been recognized, is the flag for judging whether reverse movement has occurred based on the local IMU sliding window. If no reverse movement occurs, this operation is performed every time a segment is recognized.

[0145] The specific process of obtaining the tunnel odometer positioning and the updated global map is as follows: Utilize the odometer constraint provided by the front end of the laser inertial odometer , which is obtained through the iterative error state Kalman filter, and the odometer constraint provided by the segment odometer , which is obtained through the iterative error state Kalman filter, and the odometer constraint provided by the segment odometer , which is the global pose obtained in the previous cycle. After obtaining each constraint factor, perform the back-end factor graph optimization to finally obtain the optimized tunnel odometer positioning and the updated global map .

[0146] As Figure 5 shows, it is a schematic diagram of the odometer method of the existing technology in the positioning of the subway shield tunnel. By comparing it with the schematic diagram of the bolt hole extraction of the present invention shown in Figure 4 , it can be seen that the positioning of the present invention for the subway shield tunnel is clearer, overcomes the interference of the repetitive structure, and realizes the unmanned inspection positioning with high precision and low drift.

Claims

1. A shield tunnel laser inertial odometer method, characterized in that, It includes the following steps: S1. Align the timestamp of the acquired Lidar point cloud data and the IMU measurement using the front-end odometer, estimate the local pose change based on point-to-surface registration, and then obtain the global odometry pose of the carrier and the constructed global map of the shield tunnel based on the error-state Kalman filter; S2. Identify the initial segments of the shield tunnel from the global map of the shield tunnel constructed by the front-end odometer, and obtain the initial positions of the segments. When the carrier moves to the initial position of a segment, the segment odometer starts counting, and the pose of the carrier at this time is set as the initial pose of the segment odometer; S3. Maintain an IMU direction constraint sliding window near the current frame of the carrier pose estimation to monitor whether the carrier makes a U-turn, including the instantaneous change and slow change of the X-axis forward direction. The number of segments counted by the segment odometer is obtained by judgment to guide the behavior of the carrier; S4. The back-end optimization module performs factor graph optimization using the odometry constraints provided by the front end of the laser inertial odometer, the odometry constraints provided by the segment odometer, and the prior position constraints to obtain the final tunnel odometry positioning and the updated global map; The specific process of S3 is as follows: S3.

1. Maintain the local IMU sliding window W. Taking the first frame in the window as the local coordinate system, integrate the IMU measurements within the window to obtain the direction change of the carrier during the local IMU sliding window time: ； ； Among them, is the angular velocity measured by the IMU at time i, is the IMU measurement interval, and N is the number of IMU frames maintained within the local IMU sliding window; S3.

2. Detect the U-turn behavior of the carrier in a short time, including the yaw and pitch angle constraints: ； ； Among them, is the change in the yaw angle within the local IMU sliding window, is the change in the pitch angle within the local IMU sliding window, is the empirical threshold for angular turning to determine whether reverse movement occurs; S3.

3. Combine the "zero speed" detection in the X-axis direction, and the formula is as follows: ; Among them, is the forward direction speed, is the empirical threshold for linear velocity stillness judgment, used to judge whether to stop moving; S3.

4. Determine the movement direction of the carrier according to the detection result, which is represented by the symbol and dynamically update the shield segment statistics : ; ； Among them, is the number of ring pieces that have been recognized, is a flag for judging whether reverse movement has occurred based on the local IMU sliding window. If reverse movement has not occurred, this operation is performed whenever a ring block is recognized.

2. The shield tunnel laser inertial odometer method according to claim 1, characterized in that, The hardware it is based on includes a front-end odometer, a segment odometer module, and a back-end optimization module; the front-end odometer is a carrier pose estimation module based on the error-state Kalman filter; the segment odometer module uses the bolt hole extraction method to identify the segments in the shield tunnel point cloud, and uses the local IMU measurement direction constraint to count the number of segments in the shield tunnel and provide the segment odometer constraint; the back-end optimization module fuses the front-end odometer constraint, the segment odometer constraint, and the prior pose constraint to perform factor graph optimization to obtain the final tunnel odometry positioning information.

3. The shield tunnel laser inertial odometer method according to claim 1, characterized in that, The specific process of S1 is as follows: S1.

1. Obtain Lidar point cloud and IMU measurements and synchronize them based on timestamps. Use IMU pre-integration for interpolation to improve the Lidar point cloud and IMU measurements The accuracy of time synchronization. The IMU pre-integration is calculated as follows: ； Among them, is the angular velocity at time k; is the angular velocity at time k; is the IMU measurement interval; represents the angular velocity bias; respectively represent the velocity, rotation, and position at time i; R k is the rotation of the IMU at time k; S1.

2. Correct the distortion of the Lidar point cloud using the pose of the IMU pre-integration: ； Among them, represents the rotational transformation of the LIDAR point cloud within one frame to the end moment of one frame, represents the translational transformation of the LIDAR point cloud within one frame to the end moment of one frame; P k represents the radar point cloud at time k; S1.

3. Align the timestamp of the undistorted LIDAR point cloud and the IMU measurement, and perform iterative error-state Kalman filtering to solve the real-time pose of the carrier. Considering that the IMU update frequency is greater than the LIDAR update frequency, when there is no LIDAR point cloud input, only the attitude propagation is performed through the IMU measurement. Define the following partial variables: ； ； ； Among them, respectively represent transpose of, transpose of, transpose of, transpose of, transpose of, transpose of, represents the angular velocity measurement value, represents the acceleration measurement value, represents the angular velocity measurement noise, represents the acceleration measurement noise, represents the angular velocity bias measurement noise, represents the acceleration bias measurement noise; are respectively the rotation, position, velocity, angular velocity bias, acceleration bias, gravitational acceleration of the IMU in the world coordinate system, the rotation from the LIDAR coordinate system to the IMU coordinate system, and the translation from the LIDAR coordinate system to the IMU coordinate system; Hypothesis The state of the frame radar frame is , and the covariance matrix is , when the IMU measurement frequency is greater than the radar measurement frequency and there is no point cloud frame input, the state is propagated using IMU measurements: ； ； ； ； Among them, and respectively represent the estimated state of the i-th frame of IMU and the estimated state of the frame, and respectively represent the estimated covariance matrix of the i-th frame of IMU and the estimated covariance matrix of the frame, is 's covariance matrix, and are solved by the following formula: ； ； Among them, represents the estimated value of the state variable, and are respectively day Jacobin matrices of the error state and the noise at "0"; When the LIDAR frame and the IMU frame measurements are input simultaneously, project the L frame to the G frame through the following formula, where the L frame is the LIDAR frame and the G frame is the world coordinate system: ； Among them, is the unit vector in the direction of the cylinder axis, is the coordinate of a point on the cylinder axis in the global coordinate system, r represents the radius of the cylinder, , is about the Jacobin matrix at "0", is related to the original observation noise ; ； Represents the residual of the point without error to the elliptic cylindrical surface, Represents the residual value of the observed value at the specific 0, is the true value, is the estimated value, is the error value; The error state is iteratively updated as follows: ; ； is an operation, and the operation rules are as follows: ； ； Among them, ; The IMU pose provided for the pose propagation model, Is the IMU pose of the k-th iteration, The extrinsic parameters of the IMU and LIDAR provided for the pose propagation model, Is the extrinsic parameters of the IMU and LIDAR of the i-th iteration; ; Maximum a posteriori estimation of the error state: ； Among them, represents the second norm operation, represents the th iteration as the covariance of the error state . represents The covariance of can be obtained by the propagation of covariance factors. Let be the Hessian matrix of all points in the k-th frame, be the covariance matrix after linearization of all points in the k-th frame, be the covariance matrix of the error state of all points in the k-th frame, represents the residual of the j-th point in the k-th frame to the cylindrical surface; ; ; wherein, represents all the residuals of the \(i\)-th frame, that is, the residuals of \(N\) points after \(i\) iterations of update; the updated residuals after \(i\) times. Repeat the above process until convergence, , where is the set empirical threshold; ； ； Among them, is the posterior pose after Kalman filter update. Finally, according to the obtained pose the current frame of point cloud is converted into the global point cloud .

4. The shield tunnel laser inertial odometer method according to claim 1, characterized in that The specific process of S2 is as follows: S2.

1. Coarse extraction is performed based on the geometric distribution of the bolt hole point cloud. The bolt hole point cloud is extracted through the height difference threshold, and an unfolded inner wall point of the tunnel is selected. , calculate the height differences between point M and the four points l above, below, left, and right of it, denoted as , ; for the height differences of the upper and lower parts, take the larger value and denote it as ; for the height differences of the left and right parts, take the larger value and denote it as ; if , are both greater than the preset screening threshold , then this point is considered a candidate point cloud for the bolt hole; S2.

2. Further refine the extraction of the candidate bolt hole point cloud based on the normal vector characteristics: S2.2-1. First, divide the expanded point cloud into small point clouds of 1.2M × 1.2M for subsequent processing. Let the original point cloud be subject to the following partitioning principle: ; Among them, is the block index; S2.2-2. Fit the plane equation of the segmented point cloud using RANSAC: , Satisfy: ; S2.2-3. Calculate each point normal vector and its included angle with the plane normal vector included angle , and the distance of each point from the fitted plane . The calculation formula is as follows: ； S2.2-4. By setting an angle threshold , and a distance threshold , , determine whether the point cloud belongs to the bolt hole point cloud. The formula is as follows: ； Among them, is a set empirical threshold, is the point cloud after filtering by the angle and distance thresholds; S2.2-5. Use the density-based clustering algorithm DBSCAN to perform clustering analysis on the extracted bolt hole point cloud, and clean the noise points in the bolt hole point cloud by setting appropriate radius and minimum sample number parameters: ； Among them, is the neighborhood radius, is the minimum number of samples; S2.2-5-1. Traverse all the bolt hole clusters extracted, calculate the geometric center coordinates of each cluster, and record the geometric center coordinates of the bolt hole cluster as: ； wherein, is the central coordinate of the r-th bolt hole cluster, is the coordinate of the s-th bolt hole point in the r-th cluster, is the number of points in this cluster; Analyze the center point spacing of bolt holes clustered along the mileage direction of the shield tunnel. According to the structural characteristics of the segment ring, the distance between the center points of adjacent bolt holes within a segment ring is greater than the distance between the center points of bolt holes between adjacent segment rings, that is, the following relationship is satisfied: ; Among them, is the distance between the clustering centers of adjacent bolt holes within the same segment ring, is the distance between the clustering centers of bolt holes between adjacent segment rings; according to the distribution characteristic that the distance between the clustering centers of bolt holes between adjacent segment rings is greater than the distance between the clustering centers of adjacent bolt holes within the same segment ring, the bolt hole clustering is divided into their respective corresponding segment rings: ； Among them, is a set of bolt holes within one ring , is a certain bolt hole within one ring; S2.2-5-2. Determine the global coordinate position of the initial circumferential seam in the advancing direction of the first ring piece based on the bolt hole positions of the identified first and second ring pieces; when the mobile carrier moves to the position of the initial circumferential seam, record and set the pose of the mobile carrier at this time as the initial pose of the ring piece odometer.

5. The shield tunnel laser inertial odometer method according to claim 1, wherein The specific process of S4 is as follows: using the odometry constraint provided by the front end of the laser inertial odometer , which is the odometry constraint obtained through an iterative error state Kalman filter and provided by the ring slice odometer , multiplying the number of identified ring slices by the ring slice width and the prior position constraint , which is the global pose obtained in the previous cycle. After obtaining each constraint factor, perform back-end factor graph optimization to finally obtain the optimized tunnel odometer positioning and the updated global map .

Images