Railway tunnel laser inertial simultaneous localization and mapping method and system

Abstract

A railway tunnel laser inertial simultaneous positioning and mapping method and system based on track priori constraints and multi-feature enhancement, comprising: S1, multi-sensor data acquisition and data preprocessing, S2, IMU motion state prediction, S3, railway tunnel point cloud region construction, S4, structure enhancement feature construction, S5, laser inertial matching, S6, track priori constraints, S7, nonholonomic constraints, S8, laser reflection intensity auxiliary matching, S9, high-precision output pose and map. According to the method and system of the application, the positioning accuracy and running stability of the laser inertial SLAM system in the weak feature and repeated structure environment of the railway tunnel can be improved by fusing the laser radar, inertial measurement unit, track priori information and environmental structure feature information.

Description

Technical Field

[0001] This invention relates to the field of laser inertial simultaneous localization and mapping (SLAM) for railway tunnels. Specifically, this invention relates to a method and system for laser inertial simultaneous localization and mapping (SLAM) for railway tunnels based on track prior constraints and multi-feature enhancement. Background Technology

[0002] With the development of intelligent inspection robots, track inspection vehicles, and unmanned railway equipment, autonomous localization and mapping (SLAM) technology in railway tunnel environments has become an important foundation for realizing intelligent operation and maintenance. Currently, the mainstream SLAM (Simultaneous Localization and Mapping) systems mainly include laser SLAM, visual SLAM, laser inertial SLAM (LIO), and multi-sensor fusion SLAM.

[0003] Among them, laser inertial SLAM is widely used for positioning in underground space due to its advantages such as high precision, independence from lighting, resistance to obstruction, and long-term stability.

[0004] LiDAR-Inertial Odometry (LIO), based on the fusion of lidar and inertial measurement units, is widely used in the autonomous navigation of robots and mobile platforms due to its advantages such as not requiring external positioning base stations, high positioning accuracy, and adaptability to complex environments. However, the environment of railway tunnels differs significantly from urban roads and open outdoor environments. Railway tunnels typically feature long distances, enclosed spaces, highly repetitive structures, and simple geometric features, making traditional LIO systems prone to severe degradation during operation.

[0005] (1) The weak geometry of the tunnel leads to the degradation of laser matching:

[0006] LiDAR systems typically acquire 3D point clouds of the environment using lidar and estimate the motion state of the vehicle by utilizing geometric constraints between consecutive frame point clouds. However, inside railway tunnels, the side walls are usually continuous smooth curved surfaces, the tunnel cross-section maintains a similar structure over a long period, and the surrounding environment lacks obvious corners and rich textures.

[0007] Therefore, a large number of points in the point cloud acquired by lidar are located in low-curvature regions, which cannot provide sufficient spatial constraints. When the vehicle travels a long distance along the track, lidar matching is prone to problems such as feature degradation, insufficient matching constraints, and unstable pose estimation.

[0008] Especially in straight sections of tunnels, due to the high similarity of environmental geometry in the direction of vehicle movement, the laser scanning results may have multiple similar interpretations, making it difficult for the system to accurately determine its own motion state.

[0009] (2) Degradation leads to trajectory drift and map distortion:

[0010] Due to insufficient laser matching constraints, the position and attitude errors in a LiDAR-Inertial Odometry (LIO) system will accumulate continuously as the operating distance increases.

[0011] Specific manifestations include: trajectory drift: due to the lack of long-term effective constraints, the system's estimated trajectory will gradually deviate from the true orbit centerline, especially manifested as: gradual shift in lateral position; cumulative error in heading angle, resulting in significant trajectory drift after long-distance operation.

[0012] Reverse motion (mirror image solution) problem:

[0013] In tunnel environments with highly repetitive structures, due to the strong similarity of laser observations, the system may be unable to distinguish the true direction of vehicle movement when relying solely on local geometric matching. For example:

[0014] When a vehicle moves along a track and in the opposite direction, the changes in the local point cloud may be similar, leading to incorrect motion estimations during the optimization process—a problem known as the reverse motion mirror solution problem. This results in: incorrect pose estimation direction, mirrored trajectory flipping, and abnormal map construction direction.

[0015] Map Distortion:

[0016] As attitude errors, especially heading errors, accumulate, the final tunnel map will show the following:

[0017] The walls are bent, the track lines are off-center, the repeating structures cannot be closed, and there are local map misalignments.

[0018] (3) Traditional LIO lacks constraints on the motion of railway vehicles:

[0019] Existing LIO algorithms typically assume that the robot can move freely in three-dimensional space, i.e., it has full six degrees of freedom of motion capability.

[0020] However, railway vehicles are constrained by the track structure, and their movement has distinct characteristics:

[0021] The direction of travel is determined by the track;

[0022] Lateral movement is constrained by the track;

[0023] The vehicle must not produce any lateral displacement.

[0024] Therefore, traditional free-space motion models cannot fully utilize the motion laws of railway vehicles themselves, leaving the system without additional constraints in degraded environments.

[0025] (4) Traditional methods lack the utilization of prior information about the railway environment:

[0026] Railway systems possess a wealth of prior information, such as track centerline, curvature, gradient, elevation, and beacon locations. However, existing general-purpose SLAM methods typically rely solely on laser point clouds and IMUs for local state estimation, failing to fully utilize the global constraints provided by railway design data. Consequently, cumulative errors are still prone to occur during long-distance tunnel operations.

[0027] (5) Insufficient utilization of laser reflection information:

[0028] Railway tunnels contain numerous structures with different material properties, such as rails, metal support structures, overhead contact line equipment, and signage equipment. These structures exhibit distinctly different laser reflection intensity characteristics.

[0029] However, existing LIO methods mainly utilize point cloud spatial geometric information, and do not make sufficient use of laser reflection intensity information, resulting in a lack of additional discrimination criteria in environments with highly repetitive geometric structures.

[0030] In addition, the patent "Laser Inertial Odometry Calculation Method and System for Weak Structure Long Tunnel Environment" issued by Wuhan University under the authorization announcement number "CN120411158B" also discloses a laser inertial odometry calculation method and system. However, it does not use region segmentation, cannot remove invalid point clouds, and has a large computational load. It requires deep learning feature extraction, which is not lightweight, not fast, and difficult to deploy. It does not incorporate prior track information, resulting in a large computational load, low accuracy, and difficulty in implementation.

[0031] Therefore, the industry expects a method and system for simultaneous laser inertial positioning and mapping in railway tunnels. This method and system can improve the positioning accuracy and operational stability of laser inertial SLAM systems in railway tunnels with weak features and repetitive structures by fusing lidar, inertial measurement units, prior track information, and environmental structural feature information. Summary of the Invention

[0032] To overcome the above problems, this invention provides a method and system for laser inertial simultaneous localization and mapping (SLAM) of railway tunnels. The method and system are based on track prior constraints and multi-feature enhancement. By fusing lidar, inertial measurement unit, track prior information, and environmental structural feature information, the method and system can improve the positioning accuracy of the laser inertial SLAM system in railway tunnels with weak features and repetitive structures.

[0033] This invention integrates lidar geometric observation, IMU motion prediction, track design priors, track beacon positioning, nonholonomic motion constraints, point cloud structure features, and laser reflection intensity features to construct a multi-constraint laser inertial positioning and mapping method suitable for railway tunnel environments. It employs region segmentation to eliminate invalid point clouds, reducing computational load; and direct intensity matching eliminates the need for deep learning feature extraction. This improves positioning accuracy, long-term stability, and ground motion in complex tunnel environments. Figure 1 To the point of being responsive.

[0034] According to the present invention:

[0035] It can improve the reliability of laser point cloud matching in railway tunnel environments; reduce positioning degradation caused by repetitive structures; reduce invalid motion degrees of freedom by utilizing track motion constraints; suppress long-distance cumulative drift by utilizing track design priors; and improve the integrity and consistency of tunnel environment map construction.

[0036] The technical solution of the laser inertial simultaneous positioning and mapping method for railway tunnels according to the present invention is as follows:

[0037] A method for simultaneous laser inertial positioning and mapping of railway tunnels, the method being based on prior track constraints and multi-feature enhancement, including:

[0038] S1. Multi-sensor data acquisition and data preprocessing:

[0039] Using a solid-state lidar module, a six-axis inertial measurement unit (IMU) module, and a track beacon positioning module, sensor data (raw IMU measurement data) is acquired. This data includes: solid-state lidar point cloud, IMU angular velocity and acceleration, track beacon position, and track design data. The raw point cloud output from the solid-state lidar first undergoes preprocessing, which includes:

[0040] Time synchronization, coordinate transformation, point cloud filtering, and outlier removal are used to subsequently divide the point cloud into regions in step S3.

[0041] S2. IMU motion state prediction:

[0042] The vehicle's continuous motion state prediction is obtained through the IMU pre-integration model, including pose prediction, velocity prediction and attitude prediction. The IMU prediction results are output to S5. Laser inertial matching is used as the initial estimate for laser point cloud matching to improve the efficiency of point cloud matching search.

[0043] S3. Point cloud region segmentation for railway tunnels:

[0044] The point cloud region of the railway tunnel is segmented into the track region, the top structure region, and the weak texture region of the tunnel wall, which is the three-region segmentation of the tunnel and differentiated SLAM feature processing.

[0045] S4. Construction of structural enhancement features:

[0046] For the point cloud data of the track region, top structure region, and weak texture region of the tunnel wall after S3 segmentation, the features of each region are solved, and the local normal vector, curvature, linearity, and flatness geometric features of each region are calculated to construct the enhanced structure of the track region and top structure region.

[0047] S5. Laser inertial matching:

[0048] S6. Prior constraints on the trajectory:

[0049] Input the track design data and establish a globally fixed Frenet track coordinate system.

[0050] Track design data includes: offline import of subway as-built track true value data, which contains four types of fixed engineering parameters: track centerline, point-by-point curvature, longitudinal slope, and absolute elevation; and a globally fixed Frenet coordinate system pre-constructed based on the complete track reference curve of the entire tunnel.

[0051] S7. Nonholonomic constraint:

[0052] The NHC nonholonomic constraint model is adopted to ensure that the robot's instantaneous motion velocity is always along the tangent direction of the vehicle's movement, and there is no instantaneous lateral velocity perpendicular to the direction of movement, as well as no instantaneous vertical jumping velocity.

[0053] S8. Laser reflection intensity assisted matching;

[0054] S9. High-precision output of pose and map updates.

[0055] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0056] S1. The preprocessing of the original point cloud output by the solid-state lidar is as follows:

[0057] S1-1. Perform multi-sensor time synchronization:

[0058] The system collects raw data timestamps from solid-state LiDAR, industrial cameras, and IMU inertial units, using a high-precision system clock as a unified time reference. Hardware timing and software interpolation are used to align the data from each sensor. This eliminates sampling timing discrepancies between LiDAR point clouds, image data, and inertial data, ensuring strict timing matching between the same frame of spatial point cloud data and the corresponding motion state and texture data. This avoids feature matching misalignment and pose calculation errors caused by timing misalignments.

[0059] S1-2. Perform radar and IMU external parameter calibration:

[0060] The IMU extrinsic parameters are fixed relative pose parameters of the solid-state lidar relative to the IMU coordinate system, including three-dimensional translation offset and three-axis rotation angle, which are used to accurately characterize the inherent spatial transformation relationship between the lidar observation coordinate system and the IMU inertial measurement coordinate system.

[0061] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0062] S1-3:

[0063] The IMU extrinsic parameter calibration implementation method is as follows: multiple sets of calibration data are used in conjunction with a hand-eye calibration algorithm to solve the optimal rotation matrix and translation vector of the lidar relative to the IMU offline, obtain fixed extrinsic parameters, and then solidify them into the system.

[0064] By using the precise extrinsic parameters obtained through calibration, each frame of the original laser point cloud is uniformly converted to the IMU reference coordinate system, eliminating system observation errors caused by the installation eccentricity and tilt deviation between the radar and the IMU, and achieving precise spatial alignment between the laser point cloud data and the IMU inertial data.

[0065] This solves the problem of raw data distortion caused by asynchronous timing of multiple sensors and installation and assembly errors, ensuring the spatiotemporal consistency and accuracy reliability of point cloud data input for subsequent region segmentation, structural feature extraction, and backend constraint optimization, and guaranteeing the stability and accuracy of SLAM positioning in tunnel weak texture scenarios from the data base level.

[0066] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0067] S1. Multi-sensor data acquisition and data preprocessing:

[0068] Solid-state lidar is used to continuously acquire 3D point cloud data of the following railway tunnel environment:

[0069] Spatial coordinate information, point cloud geometric distribution information, laser reflection intensity information,

[0070] Among them, spatial coordinate information is used to build tunnel environment maps and perform laser odometry matching; point cloud geometric distribution information is used to analyze the spatial characteristics of different structural regions; and reflection intensity information is used to help distinguish different material structures.

[0071] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0072] The spatial coordinate information includes the three-dimensional Cartesian coordinates and distance information of each laser point. It is mainly used to construct a global three-dimensional environment map of the tunnel, and also participates in the inter-frame matching and pose determination of laser odometry.

[0073] The inter-frame matching is to find the geometric correspondence between point clouds of two adjacent frames through an algorithm, and the pose solution is to construct an optimization problem based on these correspondences and iteratively calculate the relative motion between the two frames.

[0074] This provides basic spatial observation constraints for SLAM front-end localization, enabling real-time robot position calculation and environmental model construction.

[0075] According to the present invention, the task of front-end localization is to extract effective information from the above-mentioned observation data, construct or update the map, and estimate the robot's pose. Front-end localization typically includes the following steps:

[0076] Feature extraction: Extracting useful features from sensor data, such as detecting corners or edges in images, or identifying planes and curves in point clouds.

[0077] Feature matching: Matching the features of the current frame with features in the map to find corresponding relationships. This typically involves calculating the similarity between feature descriptors (such as SIFT, ORB, etc.).

[0078] Pose estimation: Utilizing matched features and sensor readings (such as IMU data), the robot's pose is estimated using optimization algorithms (such as BA-Bundle Adjustment). Map update: Based on new observations and pose estimates, information in the map is updated. This may involve adding new feature points or correcting the positions of existing features.

[0079] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0080] The point cloud geometric distribution information includes the density distribution of a single point's neighborhood, the local spatial undulation morphology, the structural extension trend, and the point cloud aggregation and discrete distribution characteristics, which are used to accurately analyze the spatial feature differences of different structural regions inside the tunnel.

[0081] Based on the inherent structural characteristics of the tunnel, the point cloud is distinguished by geometric distribution differences: dense and regular point cloud in the track area, strip-shaped point cloud in the top structure, and large-area uniform and sparse point cloud on the wall.

[0082] This provides a spatial dimension basis for subsequent physical geometric partitioning and structured feature selection, enabling the identification of low-computing-power regions without semantic networks.

[0083] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0084] The laser reflection intensity information is the echo intensity value returned after laser irradiates different media. It can accurately reflect the differences in material properties on the surface of the tunnel structure and is used to help distinguish different material structures inside the tunnel.

[0085] The reflectance intensity values ​​of different structural materials within the tunnel, including rails, sleepers, metal supports, concrete walls, and tunnel segments, vary significantly. Therefore, this invention utilizes these intensity threshold differences to assist in identifying the rail metal structure, overhead metal components, and weakly textured concrete walls. This compensates for the blind spots in purely geometric partitioning when distinguishing between similar shapes and different material structures, thereby improving the accuracy and robustness of the tunnel's three-zone segmentation.

[0086] According to the present invention, by using the division of labor and collaborative application of multi-dimensional information from lidar, and combining time synchronization and laser-IMU extrinsic parameter calibration preprocessing, spatial positioning, structural partitioning, and material identification multi-task coupling is achieved, thereby improving the SLAM matching accuracy and region partitioning reliability in tunnel weak texture and repetitive structure-like scenes without increasing computational overhead.

[0087] According to the present invention, this multi-parameter joint segmentation method abandons complex semantic reasoning and relies entirely on the inherent physical geometry prior of the tunnel and the intrinsic features of the point cloud to achieve partitioning. The segmentation logic is simple and the amount of computation is small. Through a four-level discrimination mechanism of height coarse segmentation, position positioning, curvature shape determination and linearity shaping, a regular point cloud distribution of the upper power supply contact network area and the lower track sleeper area is accurately formed, which solves the problems of mixed and low structural discrimination of traditional single threshold segmentation. It provides an accurate regional prior basis for subsequent extraction of differentiated structural features of the upper and lower regions, noise filtering, and stable odometer matching.

[0088] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0089] S2. IMU motion state prediction:

[0090] The pre-integration of the IMU pre-integration model adopts the inter-frame fixed incremental integration paradigm, with the trigger time of the lidar frame as the start and end point of integration.

[0091] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0092] S2. IMU motion state prediction:

[0093] The IMU pre-integration model employs a six-degree-of-freedom inertial navigation model, utilizing gyroscope integration and acceleration integration to predict attitude / velocity / position.

[0094] An IMU and a lidar are combined to form a laser inertial odometry system, which uses IMU motion constraints and laser geometric constraints.

[0095] Together, we estimate the vehicle's motion state.

[0096] According to the present invention S2:

[0097] The key execution process is as follows:

[0098] 2.1. Raw data preprocessing: Collect raw inertial data from high-frequency gyroscopes and accelerometers, perform sliding window denoising and real-time zero-bias compensation, filter out high-frequency random noise and temperature drift zero-bias errors, and ensure that the inertial data input for integration calculation is pure and reliable.

[0099] 2.2. Inter-frame data alignment and truncation: Using the laser point cloud times of the two consecutive frames as the start and end times of integration, all continuous IMU sampling data within this time period are truncated to achieve precise temporal matching between inertial data and laser frames.

[0100] 2.3. Recursive Incremental Integral Calculation: Based on the differential equation of inertial motion, the angular velocity and specific force data are solved by recursive integration at each time step. The inter-frame rotation increment, velocity increment, and position increment are calculated sequentially, and the high-frequency discrete inertial observations are accumulated into unified motion increment observations between laser frames.

[0101] 2.4. Pre-integration residual and covariance update: The pre-integration residual equation and error covariance matrix are updated synchronously to quantify the observation confidence of the pre-integration increment and provide a weighted inertial constraint residual term for back-end graph optimization.

[0102] 2.5. Incremental Output and Iterative Reuse: Output stable inter-frame attitude, velocity, and displacement pre-integral increments as initial values ​​for laser matching and constraints for joint optimization at the back end; when the next laser frame arrives, iteratively update the integration window to achieve continuous and uninterrupted motion state prediction output.

[0103] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0104] S3. The tunnel scene is divided into a track area, a top structure area, and a tunnel wall weak texture area. Specifically, based on the fixed standard lining size and spatial physical geometric distribution characteristics of subway tunnels, a preset fixed physical geometric interval is used to achieve rapid area division, avoiding the high computing power consumption of complex semantic reasoning and adapting to industrial real-time application scenarios. Among them, the track laying area within the preset physical height interval at the bottom of the tunnel is determined as the track area, the fixed space area containing tunnel auxiliary structures within the preset physical height interval at the top of the tunnel is determined as the top structure area, and the fixed physical interval between the two, with continuous and smooth tunnel sidewalls, is determined as the tunnel wall weak texture area.

[0105] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0106] S3. Point cloud region segmentation for railway tunnels:

[0107] Using a multi-dimensional joint discrimination criterion of height, geometric position, curvature, and linearity, a refined regional classification of railway tunnel point clouds was performed, dividing the upper power supply contact network region, the lower track sleeper region, and the weak texture region of the tunnel wall.

[0108] The specific classification principles and implementation methods are as follows:

[0109] First, the tunnel space is coarsely divided based on absolute height thresholds:

[0110] Based on the standard lining dimensions of railway tunnels and a preset fixed height boundary threshold, the point cloud in the upper part of the tunnel is initially divided into the upper region, and the point cloud in the lower part of the tunnel near the ground is initially divided into the lower region, thus achieving rapid coarse segmentation of the upper and lower regions;

[0111] Secondly, the region can be accurately located by combining geometric location distribution characteristics:

[0112] Based on the overall spatial distribution of the point cloud, the point cloud in the upper region extends laterally in the center of the tunnel top, corresponding to the location of the tunnel arch, power supply bracket, and suspended equipment.

[0113] The point cloud in the lower region is distributed in a regular strip at the bottom of the tunnel, corresponding to the locations of the track, sleepers, and track bed. By analyzing the spatial clustering patterns, stray noise point clouds across regions can be eliminated.

[0114] Then, structural properties are identified through local curvature features:

[0115] The upper power supply contact network area includes a large number of suspended cables, metal supports, and insulators with fragmented and irregular structures. The local point cloud curvature is relatively large and the structure undulation is obvious.

[0116] The lower track and sleeper area is generally regular and flat, with small local surface fitting residuals and a generally low curvature. The curvature threshold can effectively distinguish the irregularly shaped auxiliary structure at the top from the regular track structure at the bottom.

[0117] Finally, accurate classification and shaping are achieved by relying on local linearity features.

[0118] According to this invention, accurate classification and shaping are finally achieved based on local linearity features:

[0119] The cables and supports in the upper contact network area are mostly slender and elongated structures, with high linearity and significant directionality in the local point cloud; the rails in the lower track area are long-distance continuous linear structures, which also have high linearity characteristics, but can be distinguished from the top structure by combining height and position.

[0120] The sleeper and track bed areas have moderate linearity and stronger planar features. The internal structure of the upper and lower areas can be finely classified by the difference in linearity gradient.

[0121] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0122] S4. Structural Enhancement Feature Construction: A lightweight geometric selection criterion of low curvature, high linearity / high flatness is adopted to selectively retain stable structural points. The specific selection requirements are as follows:

[0123] Track area: Select points with low curvature and high linearity, retain continuous rigid edge points of rails and sleepers, and eliminate undulating and noisy points.

[0124] Top structural area: Select points with medium to low curvature and high linearity, and retain continuous linear structural points such as segment joints and metal supports.

[0125] For areas with weak texture on the wall: select points with extremely low curvature and high flatness, discard invalid pseudo-feature points, and retain only the structural points of the stable surface of the flat wall.

[0126] According to the present invention, the core rule for constructing structural enhancement features is to filter out high curvature abrupt change points and low linearity scattered points throughout the process, and retain only structural feature points with tunnel rigidity, low repeatability, and high distinguishability. This reduces the amount of computation while ensuring a small number of highly reliable features, thus adapting to industrial real-time SLAM.

[0127] According to the present invention S4:

[0128] For the point cloud data of the segmented track region, top structure region, and weak texture region of the tunnel wall, local neighborhood feature solving is performed respectively, and four types of geometric structure features, namely local normal vector, curvature, linearity, and flatness, are calculated point by point.

[0129] The local normal vector is obtained through eigenvalue decomposition of the covariance matrix of the point cloud neighborhood, and is used to characterize the spatial orientation of the local point cloud. Curvature is calculated using the residual from fitting the surface of the neighboring point cloud, and is used to characterize the degree of local structural undulation. Linearity and flatness are calculated based on the normalization of the three-dimensional eigenvalues ​​of the covariance matrix, respectively characterizing the saliency of linear structures and the flatness of planar structures in the local point cloud. Based on the calculated geometric parameters, a fixed industrial threshold is set to screen for high linearity and high stability structural points: in the track and top structure areas, low-curvature, high-linearity edge, support, and segment joint linear structural points are screened; in the weakly textured area of ​​the tunnel wall, low-curvature, high-flatness stable planar structural points are screened.

[0130] By using the above structured screening method, disordered, repetitive, and unstable noise feature points are eliminated, and only structural feature points with high scene recognition, strong stability, and significant spatial distinguishability are retained. This effectively avoids the problem of feature homogenization and feature repetition matching in different locations, and greatly improves the distinguishability and matching reliability of inter-frame features in narrow and repetitive tunnel scenes.

[0131] According to the present invention S4:

[0132] Threshold values ​​for track area (rails, sleepers):

[0133] The curvature threshold is set to 0.05m. -1 The linearity threshold is set to 0.85, the number of neighborhood points is 30, and the search radius is 0.4m.

[0134] For example, the local curvature C: C <= 0.05m -1 (Low curvature)

[0135] Local linearity L: L>=0.85 (high linearity)

[0136] According to the present invention S4:

[0137] Curvature range for the top structural region: 0.03~0.15m -1 The linearity threshold is 0.80, the number of neighborhood points is 35, and the search radius is 0.6m.

[0138] According to the present invention S5:

[0139] Using the pre-integrated output of the IMU as the motion prior and incorporating laser point cloud registration observation constraints, a tightly coupled back-end optimization framework is constructed. This model utilizes the high-frequency output of the IMU to compensate for laser motion distortion and uses laser observations to suppress IMU cumulative drift, outputting high-frequency and stable carrier pose estimation results.

[0140] Point cloud matching (geometric constraints):

[0141] By performing scan matching (such as ICP, NDT, GICP) between the current frame and the local map / point cloud of the previous frame, point-to-surface / point-to-line residuals are constructed to provide high-precision relative pose estimation.

[0142] It corrects cumulative drift, but is prone to failure in scenarios with missing features (empty spaces, repetitive structures).

[0143] IMU Motion Constraints (High-Frequency Priors):

[0144] Pre-integrate the IMU data between adjacent laser frames to generate a relative pose prior after angular velocity / acceleration integration;

[0145] ① Provides high-frequency motion prediction for distortion correction (compensating for carrier motion during laser scanning); ② Maintains short-term trajectory continuity when point cloud matching fails; ③ Estimates and compensates for IMU bias online.

[0146] Pose optimization (global consistency):

[0147] The two types of constraints (point cloud matching factor + IMU pre-integration factor) are uniformly incorporated into the factor map (or extended Kalman filter) to jointly optimize the trajectory nodes and sensor extrinsic parameters / biases.

[0148] According to S6 of the present invention:

[0149] By importing authoritative trajectory design data, a globally unified and permanently fixed Frenet trajectory benchmark is constructed, mapping the robot's SLAM pose to a trajectory-specific global curve coordinate system, thus achieving three core functions:

[0150] First, the tangential direction of the track centerline is used as the global heading reference to supplement fixed heading observation constraints for long straight tunnel sections with no features and high symmetry;

[0151] Secondly, by using lateral offset constraints (l) and elevation constraints (h), continuous anchoring correction is performed on the lateral offset and vertical scale drift of SLAM.

[0152] Third, a globally unified benchmark is established that does not depend on environmental textures and does not accumulate with frames, upgrading traditional local inter-frame optimization to global engineering truth value constraint optimization.

[0153] According to S6 of the present invention:

[0154] Track design data includes: centerline; curvature; gradient; elevation. Vehicle position is represented as (s, l, h).

[0155] Where: s: track direction; l: lateral offset; h: height. Constraint: Limitation: l≈0

[0156] That is, the vehicle always stays close to the center of the track. Solution: Long-distance drift makes the heading unobservable. Reason:

[0157] Straight sections of tunnels: Laser cannot provide directional constraints, but the track direction provides additional constraints, effectively suppressing cumulative errors.

[0158] It effectively solves the core pain points of traditional tunnel SLAM in long, straight, and weakly textured scenes, such as unobservable heading, feature degradation and matching failure, and long-distance cumulative drift; it completely gets rid of the limitation that positioning accuracy depends entirely on the quality of environmental features.

[0159] According to the present invention:

[0160] The IMU pre-integration model is used to provide motion priors; the point cloud structure feature model is used to construct effective observations of the railway environment; the track Frenet constraint model is used to provide global track priors; the NHC model is used to constrain the degrees of freedom of railway vehicle motion; the laser reflection intensity model is used to enhance the matching of repeated environments; and the multi-constraint optimization model is used to achieve the final pose estimation.

[0161] Through the above-mentioned standardized pre-integration process, high-frequency and high-continuity robot motion states can be continuously output without relying on effective laser features, thus completely solving the problems of laser matching failure in weak texture and featureless regions of tunnels and pose gaps and jumps.

[0162] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0163] S6. Prior constraints on the trajectory:

[0164] Import prior data for orbit design and construct a globally fixed Frenet orbit coordinate system;

[0165] The aforementioned prior track design data directly reads the alignment parameters from the as-built CAD of the railway tunnel, serving as true-value data for track mapping. Specifically, it includes:

[0166] The data includes the station number, centerline coordinate sequence, radius of curvature / curvature value, transition length (total length of transition curve), long transition (parameters of long transition curve), circular curve length, longitudinal slope, vertical curve parameters, and design elevation for each mileage; based on the complete track CAD baseline shape of the entire tunnel.

[0167] A globally fixed Frenet-Serret coordinate system is pre-constructed in one go. The global coordinate system datum, tangent, normal, and elevation datum are fixed and unchanged. Only the robot's (s,l,h) pose parameters under the global datum are calculated in real time, where s is the longitudinal mileage station of the track, l is the lateral offset of the track, and h is the elevation deviation of the track.

[0168] According to the present invention:

[0169] Specific definitions of prior parameters for each orbit and their role in SLAM;

[0170] 1. Curvature parameters (CAD plane alignment parameters): The curvature radius and curvature value are output by the CAD plane alignment for each mileage, which characterizes the degree of curvature of the track plane. It is used for iterative calculation of the tangential and normal directions in the Frenet coordinate system, accurately matching the heading characteristics of straight, gentle curve and circular curve sections, and correcting the cumulative heading deviation of SLAM in curve segments.

[0171] 2. Transition Length and Long Transition (CAD Transition Curve Professional Parameters): Transition length refers to the full length of the standard transition curve, while long transition refers to the design parameter of the transition curve for a longer interval, corresponding to the length of the characteristic pile interval of CAD straight transition (ZH), transition circle (HY), circle transition (YH), ​​and transition straight (HZ); used to accurately divide the straight section, the gradual transition section of the transition curve, and the steady-state section of the circular curve, ensuring the smooth continuity of the Frenet coordinate system in the linear transition section and eliminating abrupt changes in heading constraints.

[0172] 3. Circular curve length: The design length of the pure circular arc section of the track, used for global heading steady-state constraints of the curve section to improve the consistency of the curve trajectory.

[0173] 4. Longitudinal gradient and vertical curve parameters (CAD longitudinal profile parameters): Read the design gradient, vertical curve radius, and vertical curve gradient change point mileage of the CAD longitudinal profile drawing for each mileage, construct the longitudinal undulation benchmark of the track, and constrain the longitudinal scale expansion and mileage drift of SLAM.

[0174] 5. Mileage-by-mile design elevation (CAD elevation true value): The absolute as-built design elevation corresponding to each mileage station of the track centerline serves as the unique true value benchmark for the tunnel's vertical orientation, solving the problem of long-term elevation drift in weakly textured tunnels due to the lack of vertical observation constraints.

[0175] Therefore, Frenet track coordinates and elevation information are established (retaining (s,d) to describe the plane position, and recording the absolute elevation value of the projection point or the vehicle's centroid separately), forming long-term global constraints and improving long-distance operation stability, that is: the vehicle always stays close to the track center, solving the problems of long-distance drift and unobservable heading, because: the straight section of the tunnel provides additional constraints from the track direction.

[0176] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0177] S8. Laser reflection intensity assisted matching:

[0178] By utilizing steel rails, metal structures, and reflective markings, a strength-assisted matching system is formed to improve the ability to identify positions in repetitive environments.

[0179] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0180] S8. Fully utilize the differences in laser echo reflection characteristics among different structures within railway tunnels to screen three types of high-stability, high-discrimination intensity characteristic targets:

[0181] Metal rail treads, top-mounted metal supports, and reflective signage on tunnel sidewalls;

[0182] Among them, the metal material and the engineering reflective coating have significant strong reflective properties to lasers, while the concrete pipe segments and wall track bed have weak reflective properties, forming a stable and reproducible intensity gradient difference.

[0183] In the process of inter-frame matching and loop closure matching, not only does it rely on point cloud geometric structure matching, but it also superimposes intensity feature constraints.

[0184] For the extracted high linear geometric structure points, their reflection intensity labels are simultaneously bound; the continuous high reflection strip features of the rail, the discrete high reflection spot features of the top metal support, and the point-like strong reflection features of the side wall reflective markers are used as exclusive location fingerprints to construct a global intensity feature fingerprint database for the tunnel.

[0185] A method for simultaneous laser inertial positioning and mapping of railway tunnels according to the present invention:

[0186] To address the issues of highly repetitive tunnel geometry and homogeneous segment morphology leading to erroneous matching and location confusion in pure geometric matching, a secondary verification is performed using the uniqueness of strength fingerprints.

[0187] Regions with similar geometric structures but inconsistent intensity distributions are identified as different locations. Only when the geometric structures match and the intensity texture distributions are consistent are they considered valid matches. False matches caused by geometric repetitions are eliminated.

[0188] According to the present invention, a railway tunnel laser inertial simultaneous positioning and mapping system is provided.

[0189] A laser inertial simultaneous positioning and mapping system for railway tunnels, the system comprising:

[0190] Solid-state lidar modules and six-axis IMU modules for multi-sensor data acquisition and output data preprocessing;

[0191] The orbit beacon module is used to collect prior orbit parameters, and the orbit parameter constraint module is used to output prior orbit constraints.

[0192] An IMU pre-integration model is used for predicting the continuous motion state of a vehicle. The prediction of the continuous motion state of a vehicle includes: pose prediction, velocity prediction and attitude prediction. The IMU prediction results are output to S5. Laser inertial matching is used as the initial estimate for laser point cloud matching.

[0193] A point cloud structure segmentation module used to construct point cloud regions and structural enhancement features for railway tunnels;

[0194] Laser inertial odometry module for predicting IMU motion state;

[0195] NHC nonholonomic constraint module that ensures that the vehicle body always moves along the forward tangent direction;

[0196] A laser intensity matching module for reflection intensity-assisted matching; a map building module and a pose optimization module.

[0197] According to the present invention, the deficiency of pure geometric SLAM in the recognition capability of long tunnel repetitive structure scenarios is compensated by the intensity-assisted matching mechanism. By utilizing the natural strong reflective characteristics of rails, metal components and reflective signs, unique intensity features are given to each location in the tunnel, which significantly improves the reliability of inter-frame matching and global positioning recognition in degraded repetitive scenarios. Moreover, the intensity calculation is based on the native attribute reading of point cloud, without adding extra computational overhead, and is suitable for real-time industrial deployment.

[0198] This invention integrates lidar geometric observation, IMU motion prediction, track design priors, track beacon positioning, nonholonomic motion constraints, point cloud structure features, and laser reflection intensity features to construct a multi-constraint laser inertial positioning and mapping method suitable for railway tunnel environments. It employs region segmentation to eliminate invalid point clouds, reducing computational load; and direct intensity matching eliminates the need for deep learning feature extraction. This improves positioning accuracy, long-term stability, and ground motion in complex tunnel environments. Figure 1 To the point of being responsive.

[0199] This invention is applicable to: subway tunnel positioning, railway inspection vehicles, unmanned rail vehicles, digital tunnel mapping, railway asset inspection, automatic rail transit, underground space positioning and navigation, and tunnel maintenance robots, etc. Attached Figure Description

[0200] Figure 1 , Figure 2 The flowcharts for the method and system of the present invention are shown below.

[0201] Figure 3 This is a schematic diagram of the tunnel point cloud structure segmentation.

[0202] Figure 4 This is a schematic diagram of the Frenet orbital coordinate system.

[0203] Figure 5-1 This invention provides a visualized global trajectory map after positioning in a subway tunnel.

[0204] Figure 5-2 This is an enlarged view of the visualization trajectory details after positioning in a subway tunnel, based on the present invention.

[0205] Figure 6-1 This is a schematic diagram of the tunnel constructed in a subway tunnel according to the present invention.

[0206] Figure 6-2 This is a magnified schematic diagram of the map details constructed in a subway tunnel according to the present invention.

[0207] Figure 7 This is a schematic diagram of the attitude, including roll angle and pitch angle.

[0208] Figure 8 This is a schematic diagram showing the location of the invention in a subway tunnel.

[0209] Figure 9 This is a schematic diagram illustrating the speed of the present invention in a subway tunnel.

[0210] Figure 10 This is a schematic diagram of the system of the present invention.

[0211] like Figure 3 As shown, the tunnel body is divided into four regions. During the localization phase, only the point cloud information of the feature-rich regions C and D is used. During the mapping phase, in order to construct a complete internal environment of the tunnel, all point cloud regions A, B, C, and D are used for mapping.

[0212] Figure 4 In the middle: L-shaped route: straight line -> gentle turn -> straight line, with a gentle slope ascent overall.

[0213] Figure 7 In the diagram, the upper line (red) represents the pitch angle, and the lower line (blue) represents the roll angle. The horizontal axis represents time in seconds (s), and the vertical axis represents the angle in degrees (°).

[0214] Figure 8 In the diagram, the three-dimensional position coordinates are as follows: the X (blue) line represents the position coordinate along the X-axis, the Y (orange) line represents the position coordinate along the Y-axis, and the Z (purple) line represents the position coordinate along the Z-axis. The horizontal axis represents time in seconds (s); the vertical axis represents distance in meters (m).

[0215] Figure 9 In the diagram, the three-dimensional velocity is represented by the following lines: X (green) line represents the velocity along the X-axis, Y (purple) line represents the velocity along the Y-axis, and Z (orange) line represents the velocity along the Z-axis. The horizontal axis represents time in seconds (s), and the vertical axis represents the velocity value in meters per second (m). Detailed Implementation

[0216] Example 1:

[0217] This embodiment addresses the problems of traditional SLAM localization drift, feature matching failure, and low pose calculation accuracy caused by weak texture, extremely uneven texture distribution, simple structure, and scarce features in the subway tunnel environment. It proposes a SLAM localization optimization scheme based on adaptive segmentation of the tunnel region. The core innovation is to accurately segment the subway tunnel laser / visual acquisition scene into three independent regions: a high-texture track region, a structured region on the tunnel roof, and a weak-texture tunnel wall region. Different feature extraction, matching weights, and pose constraint strategies are designed for the texture features and geometric characteristics of different regions, solving the industry pain points of poor adaptability and insufficient localization robustness of traditional unified SLAM algorithms in weak-texture tunnel regions.

[0218] 1. Hardware and Experimental Scenario Setup:

[0219] 1.1) Hardware platform configuration:

[0220] This embodiment uses a subway tunnel inspection robot as the carrier, equipped with SLAM data acquisition hardware, to adapt to the low-speed inspection conditions of real subway tunnels. The hardware parameters are fixed as follows:

[0221] LiDAR: 16-line mechanical LiDAR with a horizontal field of view of 360°, a vertical field of view of ±15°, a ranging range of 0.1~150m, a ranging accuracy of ±2cm, and a frame rate of 10Hz. It is used to collect three-dimensional point cloud structure information of tunnels.

[0222] Inertial Measurement Unit (IMU): Six-axis IMU, sampling frequency 100Hz, gyroscope zero bias error ≤0.1° / h, accelerometer zero bias ≤50μg, used for high-frequency pose pre-integration to compensate for visual / laser low-frequency positioning defects;

[0223] Computing unit: Industrial embedded industrial control computer, equipped with Intel i7 processor, 16G memory, 512G solid-state drive, to meet the real-time SLAM computing requirements;

[0224] Positioning accuracy device: High-precision total station for tunnels, with static positioning accuracy of ±1mm, used to calibrate the positioning error of the algorithm.

[0225] 1.2) Track area (rails, sleepers):

[0226] Selection criteria: low curvature + high linearity points; retain continuous rigid edge points of rails and sleepers; eliminate undulating and noisy points.

[0227] Threshold values ​​for track area (rails, sleepers):

[0228] The curvature threshold is set to 0.05m. -1 The linearity threshold is set to 0.85, the number of neighborhood points is 30, and the search radius is 0.4m.

[0229] For example, the local curvature C: C <= 0.05m -1 (Low curvature)

[0230] Local linearity L: L>=0.85 (high linearity)

[0231] Linearity definition: Let the eigenvalues ​​of the covariance matrix of the neighborhood of a point be:

[0232] λ1>=λ2>=λ3, linearity L=(λ1-λ2) / λ1, take the value of L[0,1], the closer the value of L is to 1, the stronger the linear feature.

[0233] Neighborhood search: Assume the number of neighborhood points N in the KD-Tree is 20~40, and the search radius r is 0.3~0.6m.

[0234] Additional geometric constraints: Point cloud height constraint: The distance between points in the track area and the track surface reference height is 0~0.45m;

[0235] Remove:

[0236] Local curvature C > 0.08m -1 The fluctuation noise is directly removed;

[0237] Discrete random points with linearity L < 0.7 are eliminated;

[0238] Isolated points: If the number of valid points in the neighborhood is less than 8, it is considered noise and is removed.

[0239] 1.3) Top structural area (tunnel arch, segment joints, metal supports):

[0240] Curvature range for the top structural region: 0.03~0.15m -1 The linearity threshold is 0.80, the number of neighborhood points is 35, and the search radius is 0.6m.

[0241] Selection criteria: Medium to low curvature + high linearity points; retain continuous linear structural points such as segment joints and metal supports. Optimal parameter range:

[0242] Local curvature C: 0.03 <= C <= 0.15m -1 (Medium to low curvature)

[0243] The arch itself has the inherent curvature of the tunnel arc, so it is permissible to have a curvature slightly larger than that of the track area; excessive curvature indicates damage and protrusions.

[0244] Local linearity L: L>= 0.80 (high linearity, identifying seams and support linear features).

[0245] Neighborhood search: KD-Tree, with N = 25~50 neighborhood points and r = 0.5~0.8m.

[0246] Additional geometric constraints (height constraints): The arch area with a distance greater than 4.2m from the rail surface reference height;

[0247] Removal criteria: C > 0.20m -1 Elimination of abrupt protrusions and peeling noise;

[0248] Clustered scattered points with linearity L < 0.65 were removed;

[0249] Line segment continuity check: Feature points with a continuous length of less than 0.6m are considered fragment noise and filtered out.

[0250] 1.4) Experimental Scenario and Data Acquisition Conditions:

[0251] The experimental scenario was a one-way tunnel in an operating urban subway system. The tunnel was 800m long, a standard circular shield tunnel, and had an inner diameter of 5.4m, perfectly simulating the real subway operating environment. The scenario included three typical regional characteristics:

[0252] Track area: The double track bed at the bottom of the tunnel includes rails, sleepers, ballast, and drainage ditches. It has rich texture, regular geometric features, and dense feature points, making it a high-reliability texture area.

[0253] The tunnel roof structure area includes ventilation ducts, lighting fixtures, cable supports, and shield segment splicing joints. The geometric structure is regular, the structured features are significant, the texture is uniform and stable, and there are no large areas without features.

[0254] Tunnel wall area: The shield precast concrete wall is smooth, undecorated, and unmarked, with large areas of weak or no texture and sparse feature points. It is the main area for traditional SLAM positioning drift.

[0255] Data acquisition conditions: The robot travels at a low and constant speed of 0.5 m / s to simulate routine inspection operations in a subway tunnel. There is no strong light, no obstruction, and no dynamic pedestrian interference throughout the process. It continuously collects complete laser point cloud and IMU inertial data of 800m tunnel to build a dedicated subway tunnel dataset.

[0256] 2. Implementation details of the core algorithm (region segmentation + differentiated SLAM localization):

[0257] 2.1): Point cloud geometric constraint correction: Combining the height threshold of the 3D point cloud from the LiDAR and the features of the plane normal vector, misjudged areas are eliminated. Based on prior knowledge of the fixed dimensions of the tunnel, the height of 0~1.2m is defined as the track area, the height of 3.8~5.4m as the top structure area, and the height of 1.2~3.8m as the wall area.

[0258] 2.2) Regional Differentiated SLAM Feature Extraction Strategy:

[0259] Based on the texture and structural characteristics of the three types of regions, feature extraction thresholds, feature types, and filtering rules are set differently to maximize the use of effective features and suppress interference from invalid features in weak texture regions.

[0260] 2.2.1): High-texture region of the track: high-density feature extraction, used as the main constraint for localization.

[0261] The track region, with its rich texture, stable features, and low repeatability, is a core and effective area for tunnel SLAM localization. Refined feature points such as rail edges, sleeper corners, and drainage ditch inflection points are extracted; simultaneously, structured features of track lines and rectangles are extracted to construct a point-line fusion feature set. The feature weight for this region is set to 1.0 as the primary constraint for pose calculation, ensuring basic positioning accuracy.

[0262] 2.2.2): Tunnel roof structural region: Prioritize structured features and suppress texture noise.

[0263] The tunnel ceiling lacks rich textures, but its geometry is regular and its layout is fixed, without any random interference.

[0264] In this embodiment, sparse texture points in this region are discarded, and the focus is on extracting geometric structural features: stable geometric features such as the outline of ventilation ducts, the center point of lamps, the annular joints of pipe segments, and the parallel structure of cable supports are extracted first to construct a structural feature dictionary; weak and unstable texture noise points at the top are filtered out to avoid matching errors caused by invalid features. The feature weight of this region is set to 0.8 as an auxiliary positioning constraint to supplement the feature loss in the track region.

[0265] 2.2.3): Weak texture areas on tunnel walls: Invalid feature extraction is prohibited, and planar constraint compensation is introduced.

[0266] The tunnel walls are large areas of smooth, weakly textured surfaces. Traditional algorithms will forcibly extract false and noisy features, which is the core reason for positioning drift.

[0267] 2.3): Regional Feature Matching and Pose Optimization

[0268] 2.3.1) Front-end matching: High-weight features in the track area are matched first to complete the coarse localization between frames; top structural features are used for secondary precise correction; wall plane constraints eliminate the cumulative error between frames and avoid the failure of single feature matching;

[0269] 2.3.2) Backend optimization: Based on the graph optimization SLAM framework, the feature constraints, planar constraints, and IMU pre-integration constraints of the three types of regions are fused to construct a joint optimization objective function, iteratively optimize the robot pose, and suppress the cumulative drift of localization during long-distance tunnel travel;

[0270] 3. Loop Detection: To address the issue of high repeatability in tunnel structures, this method combines region segmentation results and prioritizes using unique texture features of the track with features of the top structure for loop closure matching, discarding invalid matches in areas with weak texture on the walls, thus significantly improving the accuracy of loop closure detection.

[0271] 3.1): Map building and global positioning output:

[0272] Based on the pose calculation of the sub-regional SLAM, the 3D point cloud and texture image of the tunnel are stitched together frame by frame to construct a globally consistent dense map of the subway tunnel. The track area retains fine texture details, the top area retains the complete structural outline, and the wall area retains the flat planar structure. The final output is a high-precision global map of the tunnel and real-time robot positioning coordinates, with the positioning frequency stabilized at 10Hz, meeting the real-time inspection and positioning requirements of the tunnel.

[0273] 4. Comparative Experiment Setup (to verify the effectiveness of innovation):

[0274] To verify the superiority of the proposed region segmentation SLAM algorithm, two sets of control experiments were set up under identical conditions: an 800m subway tunnel, identical hardware, and identical driving conditions.

[0275] Control group 1: Traditional visual SLAM algorithm, which extracts features uniformly for the entire frame of image, does not perform region segmentation, and uses the same feature threshold and matching weight for all regions.

[0276] Control group 2: Traditional laser SLAM algorithm, which relies solely on point cloud geometric features and lacks a texture region differentiation strategy;

[0277] Experimental group: This invention is based on a differentiated SLAM localization algorithm that segments the track, top, and wall areas.

[0278] 5. Experimental Results and Effect Analysis of the Example:

[0279] 5.1) Improved positioning accuracy:

[0280] Traditional unified SLAM algorithms suffer from severe feature mismatches in areas with weak texture on walls, with an average localization error of 12.6cm and a cumulative drift error of 28.3cm at the end of an 800m tunnel. Conventional laser SLAM has an average error of 8.9cm and a drift of 21.5cm at the end. The algorithm of this invention avoids interference from weak texture areas and strengthens the feature constraints of effective areas through differentiated processing of region segmentation. The average localization error is reduced to 3.2cm and the maximum drift error at the end is only 6.8cm, improving the localization accuracy by more than 60%.

[0281] Example 2:

[0282] 6. To address the issues of missing heading constraints and long-distance cumulative drift in straight tunnel sections, this invention introduces prior data from subway track engineering to construct a tunnel-specific Frenet-Serret curve coordinate system. This applies long-term, consistent, and global prior track constraints to the robot's pose, supplementing the heading constraints missing in traditional SLAM from an observational perspective and significantly improving the positioning stability in long-distance tunnels.

[0283] 6.1) Prior Track Input Data (Engineering True Value Data):

[0284] This module imports structured prior data of subway as-built design lines offline. All data are fixed engineering true values, unaffected by sensor observations and without cumulative errors. It includes four core input types:

[0285] Track centerline: The three-dimensional discrete point sequence of the tunnel design reference track centerline constitutes the global reference curve, which is the reference reference line of the Frenet coordinate system;

[0286] Point-by-point curvature: The curvature value at each mileage point of the track centerline, used to distinguish between straight segments (curvature ≈ 0) and curved segments, and to adapt to coordinate projections of different track shapes;

[0287] Track design gradient: The longitudinal gradient parameter of the track, used to describe the vertical variation of the track along the direction of travel;

[0288] Track design elevation: refers to the absolute altitude corresponding to each mileage point on the track centerline. It defines the vertical reference plane of the tunnel track and is used to constrain the vertical height deviation of the robot, solving the problems of elevation drift and vertical scale instability in traditional SLAM.

[0289] 6.2) Constructing the Frenet-Serret dynamic coordinate system for the tunnel (core modeling):

[0290] This invention abandons the traditional follower-local Frenet coordinate system and the globally fixed Cartesian coordinate system, and pre-constructs a globally fixed Frenet-Serret coordinate system based on the reference curve of the entire tunnel track. This coordinate system is a global reference coordinate system built offline in one go, which remains fixed throughout the entire process and does not update the reference shape in real time with the robot's movement, providing a unified and consistent global coordinate constraint reference for the entire tunnel.

[0291] The coordinate system is essentially a globally fixed orthogonal reference coordinate system constructed offline based on the reference curve of the tunnel's complete track centerline. The reference curve, global tangent vector, normal vector, and elevation reference of the coordinate system are all predefined fixed true values. During the robot's movement, only its own projected coordinates under the global Frenet reference are solved in real time. The overall structure of the coordinate system remains fixed throughout the entire process, with no dynamic sliding or reference offset.

[0292] Coordinate axis definition:

[0293] Longitudinal S-axis: The direction of the tangent vector of the reference curve at the projection point, representing the direction of track travel;

[0294] Horizontal L-axis: The direction of the principal normal vector of the reference curve at the projection point, which is orthogonal to the direction of travel and represents the lateral offset direction perpendicular to the center of the track.

[0295] Vertical H-axis: Matches the direction of the track design elevation normal, representing the vertical height dimension.

[0296] 6.3) Unified representation of robot pose (s, l, h):

[0297] The robot's global pose is mapped to the Frenet coordinate system of the track, and the motion state inside the tunnel is uniformly expressed using three-dimensional parameters:

[0298] s (longitudinal mileage): The cumulative mileage traveled by the robot along the centerline of the track, describing its longitudinal position along the track.

[0299] l (lateral offset): The vertical normal distance from the robot's current position to the center line of the track, that is, the left and right offset relative to the center of the track.

[0300] h (vertical elevation deviation): The difference between the robot's current measured height and the design elevation of the corresponding track mileage, used to characterize the vertical positioning deviation.

[0301] 6.4) Prior constraints on the orbit (core innovation constraints):

[0302] Based on prior knowledge of subway inspection robot operations: the equipment always travels directly above the track and will not deviate significantly from the track center. Therefore, a global, permanent hardware-software combined constraint is established:

[0303] Core constraint: l ≈ 0.

[0304] Forced constraints in backend optimization: The robot's lateral offset l always approaches 0, limiting the lateral offset to within the minimum allowable threshold range (example threshold: ±5cm).

[0305] Simultaneously, elevation constraints are applied: the robot's vertical height h follows the changes in elevation and slope of the track design, suppressing random drift in elevation direction and scale drift.

[0306] 6.5) Key issues to be addressed by constraints (corresponding innovation mechanisms).

[0307] 6.6) Constraint Integration Implementation Process (Implementation Steps):

[0308] Step 1: Offline loading of prior engineering files for track centerline, curvature, slope, and elevation, completing the global Frenet coordinate system pre-construction and parameter solidification for the entire tunnel line in one go, and generating a globally fixed SLH datum mapping table;

[0309] Step 2: Project the robot's global pose calculated in real time by SLAM onto the orbit reference curve frame by frame, and calculate the current (s,l,h) state variables;

[0310] Step 3: Construct the constraint equations for lateral offset residuals and elevation residuals, and add the prior constraint l≈0 to the objective function of the graph optimization backend;

[0311] Step 4: Automatically increase the weight of prior constraints on the track in straight sections and weaken the constraints of failed environmental features; in curved sections, use environmental features as the main factor and prior constraints as the secondary factor to achieve adaptive fusion.

[0312] Step 5: Output the stable pose after the orbital prior global correction, completely suppressing long-distance drift and heading deviation.

[0313] Figures 3-9 The visualization results are based on data obtained according to embodiments of the present invention. It can be seen that the three-dimensional position and three-dimensional velocity of the robot carrier in the subway tunnel are consistent with the physical reality of train operation, which is continuous and stable.

[0314] According to the present invention, by constructing a globally fixed Frenet orbit prior constraint, the inherent defects of traditional tunnel SLAM in that the heading is unobservable in straight sections and there is no global reference for local matching are compensated. By introducing a fixed engineering-level global truth constraint throughout the entire process, which does not depend on environmental texture and structural features and has no reference accumulation error, it can stably constrain the lateral, vertical and heading accumulation errors in the long term, completely solve the reference drift problem of the following coordinate system, and significantly improve the positioning robustness and global trajectory consistency in long-distance weak texture scenarios in subway tunnels.

Claims

1. A method for simultaneous laser inertial positioning and mapping of railway tunnels, the method being based on prior track constraints and multi-feature enhancement, characterized in that... include: S1, Multi-sensor data acquisition and data preprocessing: A solid-state LiDAR module, a six-axis inertial measurement unit (IMU) module, and a track beacon positioning module were used to collect sensor data, including: 3D point cloud data from the solid-state LiDAR, angular velocity and acceleration data from the IMU, track beacon position, and track design data. 3D point cloud data includes: spatial coordinate information, point cloud geometric distribution information, and laser reflection intensity information. The raw point cloud output by the solid-state lidar first undergoes preprocessing, which includes: Time synchronization, coordinate transformation, point cloud filtering, and outlier removal are used to subsequently divide the point cloud into regions in step S3. S2. IMU motion state prediction: The vehicle's continuous motion state prediction is obtained through the IMU pre-integration model, including pose prediction, velocity prediction and attitude prediction. The IMU prediction results are output to S5. Laser inertial matching is used as the initial estimate for laser point cloud matching to improve the efficiency of point cloud matching search. S3. Point cloud region segmentation for railway tunnels: The point cloud region of the railway tunnel is segmented into the track region, the top structure region, and the weak texture region of the tunnel wall, which is the three-region segmentation of the tunnel and differentiated SLAM feature processing. S4. Construction of structural enhancement features: For the point cloud data of the track region, top structure region, and weak texture region of the tunnel wall after S3 segmentation, the features of each region are solved separately. The local normal vector, curvature, linearity, and flatness geometric features of each region are calculated point by point to construct the enhanced structure of the track region and top structure region. S5. Laser inertial matching, including: point cloud matching, IMU motion constraints and pose optimization; S6. Prior constraints on the track; Input the track design data and establish a globally fixed Frenet track coordinate system. Track design data includes: offline import of subway as-built track true value data, which includes four types of fixed engineering parameters: track centerline, point-by-point curvature, longitudinal slope, and absolute elevation; and a globally fixed Frenet coordinate system pre-constructed in one go based on the complete track reference curve of the entire tunnel. S7. Nonholonomic constraint: The NHC nonholonomic constraint model is adopted to ensure that the robot's instantaneous motion velocity is always along the tangent direction of the vehicle's movement, and there is no instantaneous lateral velocity perpendicular to the direction of movement, as well as no instantaneous vertical jumping velocity. S8. Laser reflection intensity assisted matching; S9. High-precision output of pose and map updates.

2. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S1: The preprocessing of the original point cloud output by the solid-state lidar is as follows: S1-1. Perform multi-sensor time synchronization: The system collects raw data timestamps from solid-state LiDAR, industrial cameras, and IMU inertial units, using a high-precision system clock as a unified time reference. Hardware timing and software interpolation are used to align the data from each sensor. This eliminates sampling timing discrepancies between LiDAR point clouds, image data, and inertial data, ensuring strict timing matching between the same frame of spatial point cloud data and the corresponding motion state and texture data. This avoids feature matching misalignment and pose calculation errors caused by timing misalignments. S1-2. Perform radar and IMU external parameter calibration: The IMU extrinsic parameters are fixed relative pose parameters of the solid-state lidar relative to the IMU coordinate system, including three-dimensional translation offset and three-axis rotation angle, which are used to accurately characterize the inherent spatial transformation relationship between the lidar observation coordinate system and the IMU inertial measurement coordinate system.

3. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 2, characterized in that: S1-3. The implementation method for IMU external parameter calibration is as follows: By employing multiple sets of calibration data and a joint hand-eye calibration algorithm, the optimal rotation matrix and translation vector of the lidar relative to the IMU are solved offline, resulting in fixed extrinsic parameters that are then stored in the system. By using the precise extrinsic parameters obtained through calibration, each frame of the original laser point cloud is uniformly converted to the IMU reference coordinate system, eliminating system observation errors caused by the installation eccentricity and tilt deviation between the radar and the IMU, and achieving precise spatial alignment between the laser point cloud data and the IMU inertial data.

4. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S1, Multi-sensor data acquisition and data preprocessing: The spatial coordinate information includes the three-dimensional Cartesian coordinates and distance information of each laser point. It is mainly used to construct a global three-dimensional environment map of the tunnel, and also participates in the inter-frame matching and pose determination of laser odometry. The inter-frame matching is to find the geometric correspondence between point clouds of two adjacent frames through an algorithm, and the pose solution is to construct an optimization problem based on these correspondences and iteratively calculate the relative motion between the two frames.

5. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 4, characterized in that: The point cloud geometric distribution information includes the density distribution of a single point's neighborhood, the local spatial undulation morphology, the structural extension trend, and the point cloud aggregation and discrete distribution characteristics, which are used to accurately analyze the spatial feature differences of different structural regions inside the tunnel. Based on the inherent structural characteristics of the tunnel, the point cloud is distinguished by geometric distribution differences: dense and regular point cloud in the track area, strip-shaped point cloud in the top structure, and large-area uniform and sparse point cloud on the wall.

6. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: Laser reflection intensity information refers to the echo intensity value returned after laser irradiates different media. It can accurately reflect the differences in material properties on the surface of tunnel structures and is used to help distinguish different material structures inside the tunnel.

7. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S2. IMU motion state prediction: The pre-integration of the IMU pre-integration model adopts the inter-frame fixed incremental integration paradigm, with the trigger time of the lidar frame as the start and end point of integration.

8. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S2. IMU motion state prediction: The IMU pre-integration model employs a six-degree-of-freedom inertial navigation model, utilizing gyroscope integration and acceleration integration to predict attitude / velocity / position. An IMU and a lidar are combined to form a laser inertial odometry system, which uses IMU motion constraints and laser geometric constraints. Together, we estimate the vehicle's motion state.

9. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S3. Divide the tunnel scene into the track area, the top structure area, and the tunnel wall weak texture area. Specifically, based on the fixed standard lining size and spatial physical geometric distribution characteristics of subway tunnels, a preset fixed physical geometric interval is used to achieve rapid area division, avoiding the high computing power consumption of complex semantic reasoning and adapting to industrial real-time application scenarios. Specifically, the area where the track is laid within the preset physical height range at the bottom of the tunnel is defined as the track area; the fixed space area containing the tunnel's auxiliary structures within the preset physical height range at the top of the tunnel is defined as the top structure area; and the fixed physical range between the two, where the tunnel sidewall is continuous and smooth, is defined as the tunnel wall weak texture area.

10. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 9, characterized in that: S3. Point cloud region segmentation for railway tunnels: Using a multi-dimensional joint discrimination criterion of height, geometric position, curvature, and linearity, the point cloud of railway tunnels is classified into refined regions, dividing the upper power supply contact network region, the lower track sleeper region, and the weak texture region of the tunnel wall.

11. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 10, characterized in that: S3. Point cloud region segmentation for railway tunnels: The specific classification principles and implementation methods are as follows: First, the tunnel space is coarsely divided based on absolute height thresholds: Based on the standard lining dimensions of railway tunnels and a preset fixed height boundary threshold, the point cloud in the upper part of the tunnel is initially divided into the upper region, and the point cloud in the lower part of the tunnel near the ground is initially divided into the lower region, thus achieving rapid coarse segmentation of the upper and lower regions; Secondly, the region can be accurately located by combining geometric location distribution characteristics: Based on the overall spatial distribution of the point cloud, the point cloud in the upper region extends laterally in the center of the tunnel top, corresponding to the location of the tunnel arch, power supply bracket, and suspended equipment. The point cloud in the lower region is distributed in a regular strip at the bottom of the tunnel, corresponding to the location of the track, sleepers, and track bed. Cross-regional stray noise point clouds are eliminated by the spatial location aggregation pattern.

12. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 11, characterized in that: S3. Point cloud region segmentation for railway tunnels: Then, structural properties are identified through local curvature features: The upper power supply contact network area includes a large number of suspended cables, metal supports, and insulators with fragmented and irregular structures. The local point cloud curvature is relatively large and the structure undulation is obvious. The lower track and sleeper area is generally regular and flat, with small local surface fitting residuals and a generally low curvature. The curvature threshold can effectively distinguish the irregularly shaped auxiliary structure at the top from the regular track structure at the bottom. Finally, accurate classification and shaping are achieved by relying on local linearity features.

13. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S4. Structural Enhancement Feature Construction: A lightweight geometric selection criterion of low curvature, high linearity / high flatness is adopted to selectively retain stable structural points. The specific selection requirements are as follows: Track area: Select points with low curvature and high linearity, retain continuous rigid edge points of rails and sleepers, and eliminate undulating and noisy points. Top structural area: Select points with medium to low curvature and high linearity, and retain continuous linear structural points such as segment joints and metal supports.

14. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S6. Prior constraints on the trajectory: Import prior data for orbit design and construct a globally fixed Frenet orbit coordinate system; The aforementioned prior track design data directly reads the alignment parameters from the as-built CAD of the railway tunnel, serving as true-value data for track mapping. Specifically, it includes: The data includes the station number, centerline coordinate sequence, radius of curvature / curvature value, transition length (total length of transition curve), long transition curve (parameters of long transition curve), circular curve length, longitudinal slope, vertical curve parameters, and design elevation per kilometer; based on the complete track CAD baseline shape of the entire tunnel. A globally fixed Frenet-Serret coordinate system is pre-constructed in one go. The global coordinate system datum, tangent, normal, and elevation datum are fixed and unchanged. Only the robot's (s,l,h) pose parameters under the global datum are calculated in real time, where s is the longitudinal mileage station of the track, l is the lateral offset of the track, and h is the elevation deviation of the track.

15. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 1, characterized in that: S8. Laser reflection intensity assisted matching: By fully utilizing the differences in laser echo reflection characteristics among different structures within railway tunnels, three types of high-stability, high-discrimination intensity characteristic targets were selected: Metal rail treads, top-mounted metal supports, and reflective signage on tunnel sidewalls; Among them, the metal material and the engineering reflective coating have significant strong reflective properties to lasers, while the concrete pipe segments and wall track bed have weak reflective properties, forming a stable and reproducible intensity gradient difference.

16. The method for simultaneous laser inertial positioning and mapping of railway tunnels as described in claim 9 or 11, characterized in that: To address the issues of highly repetitive tunnel geometry and homogeneous segment morphology leading to erroneous matching and location confusion in pure geometric matching, a secondary verification is performed using the uniqueness of strength fingerprints. Regions with similar geometric structures but inconsistent intensity distributions are identified as different locations. Only when the geometric structures match and the intensity texture distributions are consistent are they considered valid matches. False matches caused by geometric repetitions are eliminated.

17. A simultaneous laser inertial positioning and mapping system for railway tunnels, the system being based on track prior constraints and multi-feature enhancement, characterized in that, The system includes: Solid-state lidar modules and six-axis IMU modules for multi-sensor data acquisition and output data preprocessing; The orbit beacon module is used to collect prior orbit parameters, and the orbit parameter constraint module is used to output prior orbit constraints. An IMU pre-integration model is used for predicting the continuous motion state of a vehicle. The prediction of the continuous motion state of a vehicle includes: pose prediction, velocity prediction and attitude prediction. The IMU prediction results are output to S5. Laser inertial matching is used as the initial estimate for laser point cloud matching. A point cloud structure segmentation module used to construct point cloud regions and structural enhancement features for railway tunnels; Laser inertial odometry module for predicting IMU motion state; NHC nonholonomic constraint module that ensures that the vehicle body always moves along the forward tangent direction; A laser intensity matching module for reflection intensity-assisted matching; a map building module and a pose optimization module.

Images