Shield tunnel laser inertial odometer method

Abstract

The invention discloses a shield tunnel laser inertial odometer method. The method comprises the following specific steps: realizing initial pose estimation and global map construction by using an iterative error state Kalman filter through an odometer front-end pose estimation module; the shield tunnel ring sheet identification module realizes accurate identification of a ring sheet by executing bolt hole extraction; the ring milemeter module is fused with the direction constraint executed by the local IMU sliding window to judge the motion direction of the carrier, so that accurate ring mileage constraint is realized; the rear-end optimization module fuses the odometer constraint provided by the front-end odometer, the prior pose constraint and the ring mileage constraint to realize pose updating, thereby overcoming the interference of the speedometer repeated structure of the shield subway tunnel, realizing high-precision and low-drift unmanned inspection positioning, enabling the autonomous inspection system to be close to disease detection accurately, and improving the inspection efficiency of the shield subway tunnel. And the development of intelligent and unmanned operation and maintenance of the subway tunnel is promoted.

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, spalling, misalignment, and fine 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 fine cracks and initial leaks. The track inspection vehicle is limited by the track laying, and it is difficult to capture small diseases during long-distance scanning, and the inspection range is limited, making it difficult to meet the high-precision and full-coverage detection requirements. In contrast, autonomous inspection systems such as unmanned aerial vehicles and robots can approach the tunnel wall, and through high-resolution vision, lidar, and ultrasonic sensors, accurately identify millimeter-level disease characteristics 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 the unmanned inspection system. The shield segment structure and texture are highly similar, resulting in easy registration errors and "standing still in place" for the method based on laser SLAM. The absence of GNSS signals makes the traditional integrated navigation system ineffective, and the visual odometer is affected by weak light and it is difficult to stably extract feature points. In addition, the existing laser inertial odometer (LIO) has error accumulation during long-distance inspection, and the lack of constraint in the X-axis direction is prone to drift in the X-axis and cannot achieve positioning. Summary of the Invention

[0005] The invention 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 the precise close-range disease detection of the autonomous inspection system, 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, and obtains the final tunnel odometer positioning information; It includes the following steps: 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; 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 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; 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 the instantaneous change of the X-axis forward direction and the slow change of the X-axis, and obtain the segment number statistics of the segment odometer guided by the carrier behavior through judgment; 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, and obtain the final tunnel odometer positioning and the updated global map.

[0007] Further, the specific process of S1 is as follows: S1.1. Acquire 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 time synchronization accuracy of the Lidar point cloud and the IMU measurement The IMU pre-integration is calculated as follows: ; 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 kThe rotation of the IMU at time k; S1.2. Correct the distortion of the Lidar point cloud using the pose obtained from IMU pre-integration: ; Among them, represents the rotational transformation of the LIDAR point cloud within one frame to the end of the frame, represents the translational 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; 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 the IMU measurement is used for attitude propagation. 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, 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 radar frame of the frame is , and the covariance matrix is ; ; ; ; Among them, and respectively represent the estimated state of the i-th frame IMU and the estimated state of the frame, and respectively represent the estimated covariance matrix of the i-th frame IMU and the estimated covariance matrix of the frame, is 's covariance matrix, ; ; where, represents the estimated value of the state variable, and are respectively day Jacobin matrices of the error state and the noise at "0"; When inputting LIDAR frame and IMU frame measurements simultaneously, 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: ; 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 ; ; represents the residual from the error-free point to the elliptical cylinder surface, represents the residual value of the observation 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 rule is as follows: ; ; where, , is the IMU pose provided by the pose propagation model, is the IMU pose of the k-th iteration, is the extrinsic parameters of the IMU and LIDAR provided by the pose propagation model, is the extrinsic parameters of the IMU and LIDAR for the -th iteration; ; Maximum a posteriori estimation of the error state: ; 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 cofactor. 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; ; ; where, represents all the residuals of the -th frame, that is, the residual iteration of points times of updated residuals; Repeat the above process until convergence, , where is the set empirical threshold; ; ; where, 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 .

[0008] Furthermore, the specific process of S2 is as follows: S2.1. Coarse extraction based on the geometric distribution of the bolt - hole point cloud. Extract the bolt - hole point cloud through 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 all greater than a preset screening threshold , then this point is considered as the candidate point cloud of the bolt hole; S2.2. Since the normal vectors of the point cloud on the tunnel wall have strong unity and usually show a consistent normal distribution in a local area, while the normal vectors of the point cloud of the bolt hole show greater discreteness and irregularity, the candidate bolt hole point cloud is further refined 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 , and the partitioning principle is: ; where, is the partitioning index; S2.2-2. Use RANSAC to fit the plane equation of the partitioned point cloud: , satisfying: ; S2.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 formulas are as follows: ; S2.2-4. By setting the angle threshold , and the distance threshold , , judge whether the point cloud belongs to the bolt hole point cloud. The formula is as follows: ; where, is the set empirical threshold, is the point cloud after screening 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: ; where, is the neighborhood radius, is the minimum sample number; S2.2-5-1. Traverse all bolt hole clusters extracted, calculate the geometric center coordinates of each cluster, and record the geometric center coordinates of the bolt hole cluster as: ; where, 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; Analyze the center point spacing 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: ; 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, the bolt hole clusters are divided into their corresponding segment rings: ; Among them, is the set of bolt holes within one ring , is a certain bolt hole within ring 1; S2.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 initial ring joint position, record and set the pose of the moving carrier at this time as the initial pose of the segment odometer.

[0009] Furthermore, the specific process of the above S3 is as follows: S3.1. Maintain the local IMU sliding window W. 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: ; ; Among them, 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; S3.2. Detect the turning behavior of the carrier in a short time, including 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 of the angle turn, used to judge 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 speed in the advancing direction, The linear velocity static judgment empirical threshold is used to judge whether to stop the movement; S3.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 : ; ; Among them, is the number of segments that have been recognized, is the flag for judging whether reverse movement occurs based on the local IMU sliding window. If reverse movement does not occur , this operation is performed whenever a segment is recognized.

[0010] Furthermore, the specific process of S4 is as follows: Utilize 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 segment odometer , multiply the number of recognized segments by the segment width (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 .

[0011] 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 segment recognition module realizes accurate segment recognition by performing bolt hole extraction; the segment odometer module fuses the direction constraint executed by the local IMU sliding window to judge the movement direction of the carrier and realizes accurate segment odometry constraint; the back-end optimization module fuses the odometry constraint provided by the front-end odometer, the prior pose constraint and the segment odometry constraint to realize pose update, overcomes the interference of the repeated 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

[0012] Figure 1 is the workflow diagram of the present invention; Figure 2 is the single-ring segment recognition flowchart of the shield tunnel of the present invention; Figure 3 is the schematic diagram of the local point cloud projection expansion of the present invention. Among them, Figure 3 in (a) is a plane screenshot and (b) is a three-dimensional diagram; Figure 4 is the schematic diagram of bolt hole extraction of the present invention; Figure 5 It is a schematic diagram of the existing odometer method in the positioning of subway shield tunnels. Specific implementation mode

[0013] The present invention will be further described below in conjunction with the accompanying drawings.

[0014] As Figure 1 shown, 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; It includes the following steps: 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 global odometer pose and the constructed global map of the shield tunnel based on error-state Kalman filter; 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; 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 the instantaneous change of the X-axis forward direction and the slow change of the X-axis, and obtain the segment number statistics of the segment odometer guided by the carrier behavior through judgment; 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.

[0015] The specific process of S1 is as follows: 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 Lidar point cloud and the accuracy of the IMU measurement time synchronization. The IMU pre-integration calculation is 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 obtained from IMU pre-integration: ; where, 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; 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 carrier. Considering that the IMU update frequency is higher than the Lidar update frequency, when there is no Lidar point cloud input, only perform attitude propagation using IMU measurements. Define the following variables: ; ; ; where, 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; 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 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 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: ; ; 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 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: ; 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 the Jacobin matrix of at "0", is related to the original observation noise ; ; represents the residual of the error-free point 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: ; ; is an operation, and the operation rules are as follows: ; ; where, , 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; ; Maximum a posteriori estimation of the error state: ; 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 cofactor. 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; ; ; where, represents all residuals of the -th frame, that is, residual iterations of points times of updated residuals; Repeat the above process until convergence, , where is the set empirical threshold; ; ; where, 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 .

[0016] Such as Figure 2As shown in the figure, the process of shield tunnel segment recognition is as follows: In the initial stage, since there is no serious error accumulation in the inertial measurement unit (IMU) sensor, 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 cloud 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: (1) Coarse extraction based on the geometric distribution of the bolt hole point cloud. Extract the bolt hole point cloud through the height difference threshold, take an unfolded inner wall point of the tunnel , 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 candidate point cloud of the bolt hole; (2) Since the normal vectors of the point cloud 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 cloud show large discreteness and irregularity, further refine the extraction of the candidate bolt hole point cloud based on the normal vector characteristics: (2-1) As shown in (a) and (b) of Figure 3 , first divide the unfolded point cloud into small point clouds of 1.2M × 1.2M for subsequent processing; let the original point cloud be , and the partitioning principle is: ; Among them, is the partitioning index; (2-2) Use RANSAC to fit the plane equation of the partitioned point cloud: , satisfying: ; (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 formulas are as follows: ; (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: ; Among them, is the set empirical threshold, is the point cloud filtered by the angle and distance thresholds; (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 sample number; (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: ; 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; As Figure 4 shown, analyze the center point spacing 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: ; 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: ; Among them, is the set of bolt holes within one ring , is a certain bolt hole within the 1st ring; (2-5-2), based on the bolt hole positions of the identified first segment ring and the second segment ring, determine the global coordinate position of the initial ring joint in the forward direction of the first segment ring; when the moving carrier moves to the initial ring joint position, record and set the pose of the moving carrier at this time as the initial pose of the segment ring odometer.

[0017] The specific process of monitoring whether the carrier has a U-turn behavior and obtaining the segment ring number statistics of the carrier behavior guiding segment ring odometer is as follows: (1) Maintain the local IMU sliding window. Use the first frame within the sliding window as the local coordinate system, integrate the IMU measurements within the window, and obtain the orientation change of the carrier during the local IMU sliding window time: ; ; Among them, 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; S3.2. Detect the turning 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, used to determine whether reverse movement occurs; (3) Combine the "zero speed" detection in the X-axis direction. The formula is as follows: ; Among them, is the speed in the forward direction, is the empirical threshold for judging the linear velocity at rest, used to determine whether the movement stops; (4) Determine the movement direction of the carrier according to the detection result, represented by the symbol , and dynamically update the shield segment statistics : ; ; Among them, is the number of segments that have been recognized, is the flag for judging whether reverse movement occurs according to the local IMU sliding window. If reverse movement does not occur , this operation is performed whenever a segment is recognized.

[0018] The specific process of obtaining the tunnel odometer positioning and the updated global map is as follows: Utilize the odometer constraints provided by the front end of the laser inertial odometer , which is obtained through the iterative error state Kalman filter, and the odometer constraints provided by the segment odometer , which is obtained through the iterative error state Kalman filter, and the odometer constraints 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 .

[0019] Such asFigure 5 The figure shows a schematic diagram of an odometer method in the prior art for positioning in a subway shield tunnel. By comparing it with Figure 4 the schematic diagram of bolt hole extraction of the present invention shown, it can be seen that the positioning of the subway shield tunnel by the present invention is clearer, overcomes the interference of repetitive structures, and realizes unmanned inspection positioning with high precision and low drift.

Claims

1. A laser inertial odometer method for shield tunnels, characterized in that, It includes the following steps: 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 vehicle's global odometer pose and the constructed global shield tunnel map based on the error-state Kalman filter; S2. Identify the initial shield tunnel segment from the global shield tunnel map constructed by the front-end odometer, and obtain the initial position of the segment. When the vehicle moves to the initial segment position, the segment odometer starts counting, and the pose of the vehicle 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 vehicle pose estimation to monitor whether the vehicle has a U-turn behavior, including the instantaneous change and slow change of the X-axis forward direction. Obtain the segment count of the segment odometer guided by the vehicle behavior through judgment; 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.

2. The shield tunnel laser inertial odometer method according to claim 1, wherein 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 vehicle 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 shield tunnel segments, 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.

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. Distort the Lidar point cloud using the pose obtained by 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 time of the undistorted LIDAR point cloud and IMU measurements, and perform iterative error-state Kalman filtering to solve 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: ; ; ; 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; They 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 the Jacobin matrices of the error state and the noise at "0"; When simultaneously inputting LIDAR frame and IMU frame measurements, 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 with respect to the Jacobin matrix at "0", is related to the original observation noise ; ; Indicates the residual from a point without error to the elliptic cylindrical surface, Indicates 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: ； ； is an operation, and the operation rules are as follows: ; ; Among them, , is the IMU pose provided for the pose propagation model, is the IMU pose at the k-th iteration, is the extrinsic parameter of the IMU and LIDAR provided for the pose propagation model, is the extrinsic parameter of the IMU and LIDAR at the -th iteration; ; Maximum a posteriori (MAP) estimation of error state: ; Among them, represents the second norm operation, represents the covariance of the error state at the -th iteration, represents that the covariance of can be obtained by the propagation of cofactors. 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; ； ； Among them, represents all the residuals of the \(i\)-th frame, that is, the residual iteration of \(N\) points updated \(M\) 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 bolt hole candidate point cloud; 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 , and the chunking principle is: ; Among them, is the block index; S2.2-2. Fit the plane equation of the segmented point cloud using RANSAC: , satisfying: ; S2.2-3. Calculate each point of the normal vector 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: ; ; S2.2-4. By setting the angle threshold , and the distance threshold , , it is determined 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 screening 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 clusters 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 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: ; 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 feature 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. Based on the bolt hole positions of the identified first and second segments, determine the global coordinate position of the initial segment seam in the forward direction of the first segment; when the moving vehicle moves to the initial segment seam position, record and set the pose of the moving vehicle at this time as the initial pose of the segment odometer.

5. The shield tunnel laser inertial odometer method according to claim 1, wherein The specific process of S3 is as follows: S3.

1. Maintain the local IMU sliding window W. Using the first frame within the sliding 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 turning behavior of the carrier within a short time, including the constraints of yaw and pitch angles: ; ; 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 speed stillness judgment, used to judge whether to stop moving; S3.

4. Determine the movement direction of the carrier based on the detection results, which is represented by the symbol , and dynamically update the shield segment statistics : ; ; Among them, is the number of identified ring pieces, is the flag for judging whether reverse movement occurs based on the local IMU sliding window. If reverse movement does not occur , this operation is performed whenever a ring block is identified.

6. The shield tunnel laser inertial odometer method according to claim 1, characterized in that, 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 the 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