Seamless integrated positioning system and method in complex railway tunnel environment

Abstract

The invention relates to the technical field of rail transit train operation control and navigation positioning, and discloses a seamless integrated positioning system and method in a complex railway tunnel environment, and the system employs a three-dimensional laser radar to construct a local environment map in real time, detects the physical shielding condition of an ultra-wideband signal propagation path through a light tracing technology, and achieves the real-time positioning of the ultra-wideband signal. And the non-line-of-sight propagation state is accurately identified. And based on an identification result, the system dynamically adjusts a measurement noise covariance, reduces the weight of non-line-of-sight measurement under a factor graph optimization framework, and fuses IMU pre-integration and LiDAR odometer data to carry out real-time pose calculation. In addition, high-precision satellite positioning states of the entrance and exit of the tunnel are used as double-end boundary constraints, and global adjustment is carried out on the whole-process track of the tunnel. According to the method, the non-line-of-sight error is effectively inhibited from the physical level, dead reckoning accumulated drifting is eliminated, and the positioning robustness and precision of the train in the complex tunnel environment are improved.

Description

Technical Field

[0001] This invention relates to the field of rail transit train operation control and navigation positioning technology, specifically a seamless integrated positioning system and method in complex railway tunnel environments. Background Technology

[0002] With the rapid development of high-speed railways and urban rail transit, train operation control systems have placed extremely high demands on the accuracy, continuity, and reliability of real-time train location information. In open environments, global navigation satellite systems can provide high-precision absolute position information. However, in the typical scenario of railway tunnels, satellite signals are completely blocked by rock and concrete structures, rendering traditional satellite positioning technologies inoperable.

[0003] To address the positioning challenges in tunnel environments, existing technologies typically employ dead reckoning based on inertial measurement units (IMUs). While inertial navigation offers advantages such as high autonomy and short-term accuracy, its position error tends to diverge over time and with increasing distance traveled. During long-distance tunnel travel, accumulated drift errors often lead to significant deviations from the actual trajectory in positioning results, failing to meet the requirements for precise train stopping or safe interval control. Therefore, introducing external observation sources for auxiliary correction becomes essential. Ultra-wideband (UWB) positioning technology, with its high temporal resolution and resistance to multipath interference, is widely used for precise positioning in indoor and underground spaces.

[0004] However, railway tunnels are long and enclosed environments with numerous overhead contact line supports, power equipment boxes, cable trays, and uneven tunnel walls. During high-speed train operation, the straight-line communication path between onboard tags and fixed base stations is easily blocked by these physical structures, causing ultra-wideband signals to propagate non-line-of-sight (NFS) propagation. NFS propagation introduces forward ranging errors, which typically do not follow a Gaussian distribution and exhibit randomness and abrupt changes. Existing NFS identification methods mainly rely on statistical characteristics of signal strength, channel impulse response characteristics, or residual detection. These methods are mostly based on probabilistic statistical models and lack direct perception of the geometric relationships of the actual physical environment, leading to unstable identification accuracy in complex and dynamically changing tunnel environments. If NFS signals are not accurately identified and eliminated, traditional fusion algorithms such as Kalman filtering will be severely contaminated, causing the positioning solution to diverge.

[0005] Furthermore, existing multi-sensor fusion solutions often employ loosely coupled architectures based on filtering, making it difficult to flexibly handle the confidence weights of different sensor data at the optimization level. They also frequently lack effective utilization of the absolute position information of tunnel entrances and exits, and cannot perform global backtracking correction of accumulated errors generated by long-distance dead reckoning. Therefore, there is an urgent need for a combined positioning technology that can combine physical environment perception for accurate non-line-of-sight identification and effectively eliminate long-distance accumulated errors. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a seamless combined positioning system and method for complex railway tunnel environments. It solves the problems of existing train positioning technologies in tunnel environments, such as the accumulation of dead reckoning errors due to satellite signal rejection, and the fact that ultra-wideband ranging is easily affected by physical obstructions, resulting in non-line-of-sight errors that seriously affect positioning accuracy and reliability.

[0007] To achieve the above objectives, the present invention provides the following technical solution: The first aspect of the present invention provides a seamless combined positioning system in a complex railway tunnel environment, which mainly consists of a data acquisition subsystem installed on a train and a data processing subsystem connected to the data acquisition subsystem.

[0008] The data acquisition subsystem is responsible for acquiring the train's motion status data and observation data of the surrounding environment. Specifically, it includes a three-dimensional lidar, an inertial measurement unit, a global navigation satellite system receiver, and an onboard ultra-wideband tag.

[0009] The data processing subsystem is logically configured with a signal integrity verification module and a multi-sensor fusion module. The core function of the signal integrity verification module is to physically verify the signal propagation state using cross-modal data. Specifically, this module uses point cloud data acquired by 3D LiDAR to construct a local environmental map reflecting the geometric features inside the tunnel. Based on this local environmental map, it performs line-of-sight analysis on the signal propagation path between the vehicle-mounted UWB tag and the external UWB base station, thereby identifying whether the current ranging signal is in a non-line-of-sight propagation state.

[0010] The multi-sensor fusion module is responsible for receiving inertial measurement data from the inertial measurement unit, point cloud data from the 3D lidar, positioning data from the global navigation satellite system receiver, and ranging data from the onboard ultra-wideband tag. This module does not directly use the raw ranging data; instead, it dynamically adjusts the measurement noise covariance of the ranging data based on the identification results from the signal integrity verification module. Subsequently, the module uses a factor graph optimization algorithm to calculate the train's pose using the adjusted data.

[0011] In one specific implementation, in order to ensure the temporal and spatial consistency of multi-source data, the data acquisition subsystem realizes the physical integration and rigid connection of each sensor through the installation components, and unifies the time reference of each sensor through a hardware synchronization controller.

[0012] Regarding the core non-line-of-sight (NOS) recognition and suppression mechanism, the signal integrity verification module constructs a local environment map as follows: Centered on the train's current carrier coordinate system, it fuses the laser point cloud data after motion distortion correction in real time to construct a local 3D point cloud map containing information about the tunnel walls and obstacles. Based on this, the specific process of line-of-sight analysis is as follows: The spatial positions of the onboard ultra-wideband tag and ultra-wideband base station in the local 3D point cloud map coordinate system are determined, and a virtual straight line segment is constructed between the two points. Subsequently, a ray tracing algorithm is used to detect whether this virtual straight line segment spatially intersects with the obstacle point cloud data in the map. If an intersection occurs, it is determined to be a NOS propagation state; if no intersection occurs, it is determined to be a line-of-sight propagation state.

[0013] Accordingly, the dynamic adjustment strategy executed by the multi-sensor fusion module is as follows: when the propagation state is determined to be line-of-sight, a predetermined nominal covariance value is assigned to the corresponding ranging data so that it can play a normal constraint role in the optimization; when the propagation state is determined to be non-line-of-sight, a large covariance value is assigned to the corresponding ranging data to reduce its weight in the factor graph optimization, thereby suppressing the errors caused by multipath effect and occlusion.

[0014] Furthermore, to eliminate the cumulative errors caused by long-distance tunnel travel, the system also employs a global trajectory correction mechanism. The data processing subsystem utilizes the high-precision positioning data from the Global Navigation Satellite System at the tunnel entry and exit times to generate initial and final boundary constraints, respectively. The multi-sensor fusion module constructs a global factor map containing initial prior factors, all measurement factors within the tunnel, and final prior factors, and performs joint optimization using a nonlinear least squares solver to generate a globally consistent train trajectory.

[0015] A second aspect of the present invention provides a seamless combined positioning method in a complex railway tunnel environment, the method mainly comprising the following steps: First, in the open environment before the train enters the tunnel, the system uses a tightly coupled navigation system consisting of a global navigation satellite system, an inertial measurement unit, and a 3D lidar. When the system detects that the satellite signal has failed due to entering the tunnel, it locks the high-precision state of the last moment as the initial boundary constraint.

[0016] Secondly, when the train is running inside the tunnel, the system performs front-end dead reckoning. Pre-integration calculations are performed using data from the inertial measurement unit to obtain the relative motion increment; simultaneously, motion distortion correction and point cloud matching are performed using point cloud data from the 3D lidar to calculate the laser odometer.

[0017] Simultaneously, the system performs signal quality verification based on environmental perception. A local environmental map is constructed in real time using 3D LiDAR, and ray tracing is performed on the connection between the vehicle-mounted UWB tag and the UWB base station within the map space. If an obstacle is detected obstructing the connection, it is determined to be non-line-of-sight propagation, and a large measurement noise covariance is assigned to the ranging data; if no obstruction is detected, it is determined to be line-of-sight propagation, and a nominal covariance is assigned.

[0018] Subsequently, the system constructs a factor graph model for state estimation. This factor graph includes IMU pre-integration factors, LiDAR odometry factors, and UWB ranging factors adjusted for dynamic covariance. Using a least-squares optimization algorithm, the system calculates the train's pose in real time.

[0019] Finally, when the train exits the tunnel and satellite signal recovery is detected, the system locks the state at the recovery moment as the termination boundary constraint. The system constructs a global factor graph containing the initial and termination boundary constraints, performs adjustment optimization on the trajectory throughout the tunnel process, eliminates cumulative drift, and generates a high-precision global trajectory.

[0020] This invention introduces lidar for real-time perception of the physical environment, enabling geometrical identification and elimination of ultra-wideband non-line-of-sight signals, overcoming the limitations of traditional methods that rely solely on signal statistical features. Combined with a factor graph optimization framework based on dynamic covariance adjustment and double-ended boundary constraints, it effectively ensures the continuity, reliability, and global accuracy of train positioning in complex tunnel environments.

[0021] This invention provides a seamless combined positioning system and method for complex railway tunnel environments. It offers the following advantages: 1. This invention utilizes a local environment map constructed by a 3D lidar for ray tracing detection, achieving accurate identification of ultra-wideband non-line-of-sight propagation states from a physical geometry perspective. Compared to traditional methods that rely solely on signal strength or signal-to-noise ratio statistical characteristics, this invention can clearly determine physical occlusion using environmental geometric information, thereby accurately distinguishing and isolating non-line-of-sight signals. This avoids the contamination of positioning solutions by multipath effects and occlusion errors from the source, and improves the system's anti-interference capability in complex tunnel environments.

[0022] 2. This invention employs a factor graph optimization strategy that dynamically adjusts the measurement noise covariance based on line-of-sight analysis results. By assigning extremely large covariance values ​​to non-line-of-sight measurement data, its weight is automatically reduced in the objective function, thus suppressing contaminated data. This mechanism avoids the problem of insufficient system observations caused by directly removing data, ensuring that the multi-sensor fusion system can effectively shield the influence of abnormal observations while maintaining data continuity, and preserving the robustness and smoothness of the positioning results.

[0023] 3. This invention introduces a dual-boundary constraint mechanism based on high-precision satellite positioning at tunnel entrances and exits. By constructing a global factor map using initial and final boundary constraints for adjustment optimization, the accumulated drift error generated by long-term dead reckoning within the tunnel can be smoothly eliminated across the entire trajectory. This not only ensures the geometric consistency of the train's trajectory throughout the entire process in the global coordinate system but also provides a reliable data foundation for the subsequent generation of a high-precision tunnel environment map with absolute geographic coordinate accuracy. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the system hardware architecture of the present invention; Figure 2 This is a flowchart illustrating the overall process of the integrated navigation method of the present invention. Figure 3 This is a schematic diagram of the UWBNLOS identification process assisted by lidar according to the present invention; Figure 4 This is a schematic diagram of the factor graph representation method of the present invention. Detailed Implementation

[0025] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0026] See attached document Figure 1 The present invention provides a seamless combined positioning system 100 in a complex railway tunnel environment. The system 100 includes a data acquisition subsystem 110, a data processing subsystem 120, and an installation component 130.

[0027] The data acquisition subsystem 110 is configured to be installed on the train 20 to acquire real-time motion status data of the train 20 and observation data of the surrounding environment. The data acquisition subsystem 110 includes a three-dimensional lidar 111, an inertial measurement unit 112, a global navigation satellite system receiver 113, an onboard ultra-wideband tag 114, and a hardware synchronization controller 115.

[0028] Mounting assembly 130 includes a protective rigid housing and a mounting bracket. A 3D lidar 111, an inertial measurement unit 112, a global navigation satellite system receiver 113, and an onboard ultra-wideband tag 114 are physically integrated within or on the surface of this rigid housing and rigidly connected to the roof or front of the train 20 via the mounting bracket. This mounting location ensures that the scanning field of view of the 3D lidar 111 covers the tunnel space in front of the train 20, and that the antenna of the global navigation satellite system receiver 113 can receive satellite signals when the train 20 exits the tunnel.

[0029] The 3D lidar 111 is configured to periodically emit laser beams into the surrounding environment and receive reflected signals, outputting 3D point cloud data containing environmental geometric information.

[0030] The inertial measurement unit 112 is configured to measure the three-axis angular velocities of the train 20 in the carrier coordinate system at a predetermined frequency. and triaxial acceleration The inertial measurement unit 112 is rigidly fixed inside the mounting assembly 130, and its coordinate axes maintain a fixed rotational transformation relationship with the carrier coordinate axes of the train 20.

[0031] The Global Navigation Satellite System receiver 113 is configured to calculate the absolute position and speed of the train 20 in the Earth's fixed coordinate system using real-time dynamic differential technology when receiving satellite signals.

[0032] The vehicle-mounted ultra-wideband tag 114 is configured to wirelessly communicate with an ultra-wideband base station 200 deployed in the external environment, acquiring distance observations through time-of-flight ranging. The ultra-wideband base station 200 is a fixed infrastructure installed along the tunnel route, and the three-dimensional position coordinates of each ultra-wideband base station 200 in the global coordinate system are... It is known.

[0033] The hardware synchronization controller 115 is electrically connected to the 3D LiDAR 111, the inertial measurement unit 112, the global navigation satellite system receiver 113, and the vehicle-mounted ultra-wideband tag 114, respectively. The hardware synchronization controller 115 is configured to send a unified trigger signal or time reference signal to each of the above sensors, so that the measurement data output by each sensor has a unified timestamp.

[0034] The data processing subsystem 120 is electrically connected to the data acquisition subsystem 110 and is used to receive and process sensor data. The data processing subsystem 120 includes a processor and a memory. The memory stores computer program instructions, which the processor executes to implement the integrated navigation function. Logically, the data processing subsystem 120 further includes a signal integrity verification module 121 and a multi-sensor fusion module 122.

[0035] The signal integrity verification module 121 is configured to construct a local map using point cloud data collected by the 3D LiDAR 111 and perform line-of-sight analysis on the signal propagation path between the vehicle-mounted ultra-wideband tag 114 and the ultra-wideband base station 200 to identify non-line-of-sight propagation states.

[0036] The multi-sensor fusion module 122 is configured to receive angular velocities from the inertial measurement unit 112. and acceleration The system receives point cloud data from a 3D lidar 111, positioning data from a global navigation satellite system receiver 113, and ranging data from an onboard ultra-wideband tag 114. The multi-sensor fusion module 122 dynamically adjusts the weights of the ultra-wideband ranging data based on the output of the signal integrity verification module 121, and calculates the train 20's pose, speed, and sensor zero bias using a factor graph optimization algorithm.

[0037] The data processing subsystem 120 also stores a database containing location information of all ultra-wideband base stations 200 within the tunnel. The data processing subsystem 120 outputs the calculated train positioning results to the train control system or human-machine interface via a communication interface.

[0038] See attached document Figure 2 The method provided by the present invention is executed by the data processing subsystem 120. The method automatically switches between different working modes according to the availability status of the global navigation satellite system signal, and performs continuous state estimation and error correction during tunnel crossing.

[0039] The method first performs system initialization and status monitoring steps. In the open environment before train 20 enters the tunnel, data processing subsystem 120 receives high-precision positioning data from the global navigation satellite system receiver 113, inertial measurement data from the inertial measurement unit 112, and point cloud data from the 3D lidar 111. During this stage, the system operates in a navigation mode with tight coupling between the global navigation satellite system, the inertial navigation system, and the lidar. Data processing subsystem 120 uses Kalman filtering or factor graph optimization algorithms to estimate the position of train 20 in the Earth's fixed coordinate system in real time. ,speed ,attitude and the accelerometer zero bias of the inertial measurement unit 112 and gyroscope zero bias Meanwhile, the data processing subsystem 120 continuously monitors the positioning solution status flag, positioning accuracy factor, and carrier-to-noise ratio output by the global navigation satellite system receiver 113.

[0040] When the positioning status of the Global Navigation Satellite System receiver 113 degrades from a fixed solution to a floating-point solution or a single-point solution, or when the carrier-to-noise ratio falls below a preset threshold, it is determined that the train 20 is about to enter the tunnel. At this time, the data processing subsystem 120 executes the boundary constraint acquisition step. The system locks and stores the last moment. High-precision state vector The state vector contains the position, velocity, attitude, and sensor zero-bias estimate at the moment of entry into the tunnel. These are marked as initial prior factors, serving as the initial conditions and absolute coordinate references for subsequent dead reckoning within the tunnel.

[0041] Subsequently, the system enters the real-time relative pose calculation and multi-source fusion step within the tunnel. During this stage, the data processing subsystem 120 executes front-end odometry processing and back-end optimization processing in parallel. The front-end odometry utilizes the high-frequency angular velocity of the inertial measurement unit 112. and acceleration The data undergoes pre-integration calculations to generate relative motion constraints between adjacent time points. Simultaneously, the point cloud data acquired by the 3D LiDAR 111 is used for motion distortion correction based on the pre-integration results, and laser odometry constraints are calculated using a point cloud registration algorithm.

[0042] In parallel with the front-end odometer, the data processing subsystem 120 performs ultra-wideband signal integrity verification and dynamic weighting steps. For each distance observation acquired by the vehicle-mounted ultra-wideband tag 114, the signal integrity verification module 121 performs physical ray detection using a local environment map constructed in real time by the 3D LiDAR 111. Based on the detection results, the observation is labeled as line-of-sight propagation or non-line-of-sight propagation, and a corresponding measurement noise covariance matrix is ​​assigned accordingly. .

[0043] The data processing subsystem 120 further constructs and maintains a factor graph model within a sliding window. This factor graph uses the state variables of the train 20 at different times as nodes, and the initial prior factors, inertial pre-integration factors, laser odometer factors, and dynamically weighted ultra-wideband ranging factors as edges connecting the state nodes. The multi-sensor fusion module 122 employs a nonlinear least squares solver to jointly optimize all constraints in the factor graph, outputting the optimal pose estimate of the train 20 within the tunnel in real time. The process continued until train 20 exited the tunnel.

[0044] When the data processing subsystem 120 detects that the Global Navigation Satellite System receiver 113 has regained its fixed depositioning state, it determines that the train 20 has exited the tunnel. At this time, the system executes a global trajectory correction step. The system records the moment the signal is restored. High-precision absolute state vector As a termination prior factor, the data processing subsystem 120 constructs a global factor map containing the initial prior factor, all historical measurement factors within the tunnel, and the termination prior factor, and performs offline batch optimization. This optimization process utilizes two high-precision boundary constraints to eliminate accumulated dead reckoning drift within the tunnel, generating a globally consistent train trajectory. Finally, the system uses this global trajectory to stitch together the point cloud data collected within the tunnel, generating a 3D tunnel map with absolute geographic coordinates.

[0045] See attached document Figure 3 In the open environment before train 20 enters the tunnel, the data processing subsystem 120 operates in a tightly coupled navigation mode based on Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and LiDAR. In this mode, the GNSS receiver 113 in the data acquisition subsystem 110 is within good satellite signal coverage and continuously outputs absolute position information, including longitude, latitude, and elevation, as well as three-dimensional velocity information. The data processing subsystem 120 receives the aforementioned absolute position and velocity information and, combining it with the angular velocity and acceleration output by the inertial measurement unit 112 and the environmental point cloud features output by the three-dimensional LiDAR 111, performs joint state estimation. At this time, the system's state vector... The state vector is updated in real time. Defined as: ; in, This indicates the three-dimensional position of the train in a fixed Earth coordinate system. Represents the three-dimensional speed of the train. This represents the attitude rotation matrix (or quaternion) of the train relative to the navigation coordinate system. This indicates the zero bias of the accelerometer. This indicates the zero bias of the gyroscope.

[0046] During the operation of the aforementioned tightly coupled navigation mode, the data processing subsystem 120 executes a signal quality monitoring program in real time. This program continuously reads the quality index data output by the Global Navigation Satellite System receiver 113. The quality index data includes positioning solution status flags (e.g., Fixed, Float, Single, or None), position accuracy factors, and the carrier-to-noise ratio for each satellite channel. The data processing subsystem 120 sets predetermined signal quality thresholds to determine the availability and reliability of the current GNSS observation data.

[0047] As train 20 approaches and is about to enter the tunnel, the data processing subsystem 120 detects changes in the aforementioned quality indicators. Specifically, when it detects that the positioning solution state degenerates from a fixed solution to a non-fixed solution, or the position accuracy factor exceeds a preset threshold, or the number of effective satellites is lower than the minimum number required for the solution, the data processing subsystem 120 determines that the GNSS signal is about to be interrupted or is no longer reliable.

[0048] Upon determining that the GNSS signal has failed, the data processing subsystem 120 immediately performs a state latching operation. The system extracts the state estimate value that meets the high-precision requirements from the last frame before the signal failure and marks it as the initial boundary constraint. The initial boundary constraint Includes the train's precise absolute position at the moment of entry into the tunnel. ,speed ,attitude And the zero bias of the inertial sensor, which has converged at this moment. and .

[0049] The data processing subsystem 120 sets the initial boundary constraints. This is converted into an initial prior factor in subsequent factor graph optimization. This initial prior factor is assigned a very small covariance matrix, which can be mathematically used as a strong constraint to anchor the starting point of subsequent relative navigation calculations within the tunnel. Once the initial boundary constraints are acquired and locked, the system automatically switches to the tunnel navigation mode in the GNSS denied environment and begins to rely on the inertial measurement unit 112, the three-dimensional lidar 111, and the vehicle-mounted ultra-wideband tag 114 for subsequent pose recursion.

[0050] See attached document Figure 3 When the system switches to the tunnel working mode, the data processing subsystem 120 starts the front-end relative pose real-time calculation program. This program mainly processes high-frequency inertial measurement data from the inertial measurement unit 112 and point cloud data from the three-dimensional lidar 111.

[0051] First, the data processing subsystem 120 performs pre-integration processing by the inertial measurement unit 112. The inertial measurement unit 112 outputs the raw angular velocity measurements in the carrier coordinate system at a predetermined high sampling frequency (e.g., 100Hz or 200Hz). and acceleration measurement values To effectively integrate high-frequency inertial measurement data into the keyframe-based low-frequency factor graph optimization framework, while avoiding recalculation of integrals with each linearization point update, the data processing subsystem 120 employs pre-integration techniques to calculate two discrete time points (e.g., two scanning frames of a lidar sensor). and The relative motion increment between them.

[0052] definition Let be the sampling time interval of the inertial measurement unit 112. Time's up The time interval includes multiple inertial measurement sampling steps. The data processing subsystem 120 recursively calculates the attitude pre-integration using numerical integration methods (such as Euler integration or median integration). Velocity pre-integral quantity and position pre-integral quantity Within each sampling step, the update formula for the pre-integral is as follows: ; ; ; In the above formula, , and These represent the updated pre-integral increments for attitude, velocity, and position, respectively. , and This indicates the pre-integration state at the previous sampling time. This represents the angular velocity vector measured by the inertial measurement unit 112 at the current moment; This represents the acceleration vector measured by the inertial measurement unit 112 at the current moment; This represents the bias estimate of the gyroscope; This represents the bias estimate of the accelerometer; This represents the exponential mapping operation from Lie algebras to Lie groups, used to handle pose updates on rotating manifolds.

[0053] The pre-integral quantity obtained by calculation ( The corresponding covariance matrix is ​​encapsulated into a relative motion constraint factor, called the IMU pre-integration factor, and transmitted to the subsequent multi-sensor fusion module 122.

[0054] Secondly, the data processing subsystem 120 uses the aforementioned IMU pre-integration results to correct motion distortion in the data from the 3D lidar 111. Because the train 20 is moving at high speed, within the time it takes for the 3D lidar 111 to complete one frame scan (e.g., 100ms), the train 20 has already undergone displacement and rotation, resulting in geometric distortion in the acquired original point cloud. Based on the timestamp of each laser point, the data processing subsystem 120 uses the small relative pose calculated by the IMU pre-integration relative to the frame start time at that moment, and projects the laser point onto the coordinate system of the frame start time, thereby generating an effective point cloud free of motion distortion.

[0055] Finally, the data processing subsystem 120 inputs the distortion-corrected point cloud into a scanning matching algorithm (e.g., a point-to-map matching algorithm). This algorithm registers the point cloud of the current frame with the local map or the point cloud of the previous keyframe, and calculates the relative pose transformation in the LiDAR coordinate system. This relative pose transformation and its uncertainty are encapsulated as LiDAR odometry factors, which are also transmitted to the multi-sensor fusion module 122 for constructing a factor map.

[0056] This section details the specific implementation methods for local environment perception and map construction. This process is executed by the signal integrity verification module 121 running within the data processing subsystem 120, and is processed synchronously and in parallel with the relative pose calculation process. In order to accurately determine the propagation path state of ultra-wideband signals at the physical level, the system first needs to construct a digital model that accurately reflects the current geometric environment surrounding the train.

[0057] The signal integrity verification module 121 is configured to continuously receive high-density laser point cloud data collected by the 3D LiDAR 111. During the operation of the train 20, the 3D LiDAR 111 performs a comprehensive scan of the tunnel interior space, acquiring the 3D coordinates of the tunnel walls, track bed, overhead contact line supports, equipment boxes mounted on the tunnel walls, and other potential obstacle surfaces.

[0058] Signal integrity verification module 121 uses the aforementioned point cloud data to construct and maintain a local 3D point cloud map (denoted as ) in real time. This local 3D point cloud map The map is established with the current carrier coordinate system of train 20 as the center, or with a reference frame within the sliding window calculated by the front-end odometer as the reference. The map covers a certain spatial range centered on train 20, and the scale of this range is set to be sufficient to cover the line-of-sight direction between the onboard UWB tag 114 and the UWB base station 200 within the current communication range.

[0059] In constructing a local 3D point cloud map During the process, the signal integrity verification module 121 executes a dynamic update strategy. As the train 20 moves forward, the module integrates the new frame of point cloud data, corrected for motion distortion (based on pre-integration results from the inertial measurement unit), into the map, while simultaneously removing historical point cloud data that is outside the scope of interest. This sliding window-style map maintenance mechanism ensures the real-time performance and computational efficiency of the map data, enabling... It accurately reflects the distribution of physical obstacles around the train at any given moment.

[0060] This local 3D point cloud map It not only contains macroscopic geometric information about the tunnel structure but also preserves the geometric features of minute structures such as signal lights, fans, and cable trays. These features are the main physical sources of ultra-wideband signal obstruction or multipath effects. Through high-precision laser point cloud reconstruction, the system digitizes the physical environment inside the tunnel, providing the necessary geometric data foundation for subsequent physical analysis of signal propagation paths using ray tracing technology. The data processing subsystem 120 will update the data in real time. Stored in a high-speed cache for later use in subsequent line-of-sight analysis steps.

[0061] The signal integrity verification module 121 establishes and maintains a real-time updated local 3D point cloud map. Based on this, the system performs geometric analysis of the signal propagation path for each ranging action between the vehicle-mounted ultra-wideband tag 114 and the ultra-wideband base station 200 in the tunnel. This analysis aims to determine whether there are physical obstructions in the signal propagation path through geometric calculations.

[0062] Specifically, for any time In a single ultra-wideband ranging event, the signal integrity verification module 121 first determines the coordinates of the two geometric endpoints for this line-of-sight analysis. The first endpoint is the current position of the vehicle-mounted ultra-wideband tag 114. The data processing subsystem 120 calculates the following based on the current pose estimate of the train 20 (obtained by pre-integration of the inertial measurement unit 112 and recursion from the laser odometer), combined with the mounting arm parameters of the onboard ultra-wideband tag 114 relative to the inertial measurement unit 112: Local 3D point cloud map Three-dimensional coordinates in a coordinate system. The second endpoint is the location of the target ultra-wideband base station 200. The data processing subsystem 120 retrieves the known global coordinates of the target base station from a pre-stored database and transforms them to match the current train pose. Under the same coordinate system.

[0063] After confirming and After determining the spatial coordinates, the signal integrity verification module 121 constructs a virtual straight line segment in three-dimensional space connecting the two points. This straight line segment represents the line-of-sight (LOS) propagation path of the ultra-wideband signal under ideal conditions.

[0064] Subsequently, the signal integrity verification module 121 executes a ray tracing algorithm. This algorithm uses the aforementioned virtual straight line segment as a probe ray on a local 3D point cloud map. The algorithm performs a traversal search. It checks whether the virtual line segment spatially intersects with any obstacle point cloud data in the map. This intersection determination is typically based on voxel grid occupancy detection or a point-to-line distance threshold; that is, if there is an obstacle on the virtual line segment's path or in its smallest neighborhood that belongs to the point cloud data, then the intersection is considered. If the point cloud data is not available, a collision is determined to have occurred.

[0065] Based on the collision detection results from ray tracing, the signal integrity verification module 121 classifies and determines the propagation state of the ultra-wideband ranging signal: If the virtual straight line segment is on the propagation path and intersects with the local 3D point cloud map The intersection of point clouds indicates that the line-of-sight path between the vehicle-mounted ultra-wideband tag 114 and the ultra-wideband base station 200 is blocked by a physical obstacle. The ranging signal must have arrived through reflection or diffraction, and the system clearly identifies it as a non-line-of-sight (NLOS) signal.

[0066] Conversely, if the virtual straight line segment does not intersect with any point cloud data on the propagation path, it indicates that the space between the two points is unobstructed, and the ranging signal arrives directly through propagation. The system then identifies it as a line-of-sight (LOS) signal.

[0067] The classification result is generated as a status flag, which is directly transmitted to the subsequent dynamic weighting process as the physical basis for adjusting the weights of the measurement factors.

[0068] See attached document Figure 4 After obtaining the line-of-sight (LOS) and non-line-of-sight (NLOS) classification results, the multi-sensor fusion module 122 in the data processing subsystem 120 performs a multi-sensor fusion analysis on the current time. The ultra-wideband ranging factor performs real-time dynamic assignment of the measurement covariance. This strategy modifies the measurement noise covariance matrix. The magnitude of this value directly controls the weight contribution of the measurement data in the subsequent factor graph optimization objective function. In specific implementation, for the first... In sub-ultra-wideband ranging observations, the multi-sensor fusion module 122 reads the classification flag bit output by the signal integrity verification module 121 and sets its corresponding measurement noise covariance according to the following logic branches. : The first scenario: When the classification result is a line-of-sight (LOS) signal, it indicates that the ranging value directly reflects the straight-line geometric distance between the vehicle-mounted ultra-wideband tag 114 and the ultra-wideband base station 200, unaffected by multipath effects or path extension caused by physical obstacles. In this case, the multi-sensor fusion module 122 will... Set to a predetermined nominal covariance value, denoted as . .Should It is a small scalar or matrix, the value of which is determined based on the physical calibration accuracy of the ultra-wideband sensor. A small covariance value is assigned. Mathematically, this is equivalent to assigning a very high information weight to the measurement factor, forcing the subsequent optimization solver to converge the train state estimate to the vicinity of the geometric surface defined by the distance measurement constraint.

[0069] The second scenario: When the classification result is a non-line-of-sight (NLOS) signal, it indicates that the ranging value is actually the path length of the signal after reflection or diffraction, containing positive systematic errors. In this case, the multi-sensor fusion module 122 will... The dynamic setting is a covariance value that is extremely large, denoted as . .Should The value was set to be much greater than (For example, approaching infinity within the limits allowed by numerical computation). Assigning extremely large covariance values. Mathematically, this is equivalent to assigning the measurement factor an information weight that is extremely low or even close to zero.

[0070] By employing the aforementioned dynamic adjustment strategy, the objective function can be constructed subsequently. When a measurement item is determined to be NLOS, its weighted residual term Because the denominator (i.e., covariance) is extremely large, the calculated result will approach zero. This means that no matter how large the actual residual of the measurement is, its contribution to the growth of the overall objective function is negligible. This approach achieves suppression and isolation of NLOS contamination data at the optimization algorithm level, ensuring that even in the presence of a large number of non-line-of-sight distance measurements, the train's pose estimation will not be skewed by erroneous distance information, thereby maintaining the robustness and accuracy of the positioning solution.

[0071] The multi-sensor fusion module 122 in the data processing subsystem 120 is configured to build and maintain a factor graph model in real time, and to optimize the solution of the model using a nonlinear least squares solver.

[0072] First, the multi-sensor fusion module 122 defines the state variable nodes to be estimated. At discrete time State variable node Includes the three-dimensional position of train 20 in a fixed Earth coordinate system. 3D velocity 3D pose and the accelerometer zero bias of the inertial measurement unit 112 and gyroscope zero bias All the state variable nodes are arranged in a time series, forming the trajectory to be optimized.

[0073] Secondly, the multi-sensor fusion module 122 converts measurement information from different sensors into factor nodes that connect these variable nodes: IMU pre-integration factor: connects two adjacent state variable nodes and This factor includes the calculated attitude increment. Speed ​​increment and position increment This is used to constrain the relative changes in the train's state between two moments.

[0074] LiDAR Odometer Factor: This factor also connects adjacent or cross-time state variable nodes. It provides geometric constraints for train motion using the relative pose transformation output by the scan-matching algorithm.

[0075] Dynamically weighted UWB ranging factor: connects the state variable nodes at a certain moment. The factor provides absolute distance constraints for trains using generated, dynamically covariance-adjusted UWB base station nodes at known locations. It utilizes generated, dynamically covariance-adjusted ultrawideband ranging data. The factor has a high weight (small covariance) for LOS measurements and a very low weight (maximum covariance) for NLOS measurements.

[0076] The multi-sensor fusion module 122 constructs a nonlinear least squares optimization problem from the set of all the above factors. The mathematical objective of this problem is to find an optimal sequence of state variables. This optimizes the system to minimize the sum of squared Mahalanobis distances of all measurement residuals. The objective function is specifically expressed as: ; In this formula, It represents a set containing the state variables at all times within the sliding window. Indicates the first The residual function of each measurement factor. For example, for the UWB ranging factor, the residual function is the measured distance and the residual function based on the current state. The difference between the predicted distances is calculated; for the IMU factor, the residual function is the difference between the pre-integral quantity and the estimated quantity of the state variables. Indicates the first The noise covariance matrix of each measurement factor. For the UWB factor, this matrix is ​​dynamically assigned based on the LOS / NLOS determination results. . The Markov norm is expressed as follows: .because This is the information matrix (weight matrix), which ensures that high-precision measurements (small covariance) dominate the total error, thereby guiding the optimization direction.

[0077] The multi-sensor fusion module 122 employs an iterative optimization algorithm (such as the Gauss-Newton method or the Levenberg-Marquardt method) to numerically solve the objective function. In each iteration, the algorithm calculates the Jacobian matrix and updates the state variables. The process continues until the objective function converges. Through this process, the system calculates the optimal train pose estimate in real time, which integrates information from all sensors and effectively suppresses the influence of NLOS error.

[0078] This section details how to use double-ended boundary constraints to eliminate accumulated errors and generate an environmental map after the train exits the tunnel.

[0079] When train 20 exits the tunnel area and enters open space, data processing subsystem 120 continuously monitors the signal status of global navigation satellite system receiver 113. Once it detects that the positioning solution output by the receiver has returned to a fixed solution and remains stable within a predetermined time window, data processing subsystem 120 determines that the train has completed tunnel crossing and triggers a global correction procedure.

[0080] First, the data processing subsystem 120 performs termination boundary constraint acquisition. The system immediately acquires and locks the recovery signal time. High-precision absolute pose data. This data is encapsulated as termination boundary constraints. This constraint includes the train's precise three-dimensional position, three-dimensional velocity vector, and attitude angles in a fixed Earth coordinate system at the moment of exiting the tunnel. At the mathematical model level, this... It is transformed into a terminating prior with a minimal covariance matrix, used to provide strong geometric constraints at the end of the optimization graph.

[0081] Secondly, the data processing subsystem 120 constructs a global factor graph and performs adjustment optimization. The system calls historical data cached in memory, covering the entire tunnel crossing process, to construct a globally consistent factor graph model. This global factor graph connects the following three types of constraints in its topological structure: Initial constraint: The initial boundary constraint obtained in step one at the moment of entry into the hole. Prior factors of transformation.

[0082] Intermediate process constraints: pre-integration factors of all inertial measurement units covering the entire length of the tunnel generated in steps two and three, laser rotameter factors, and ultra-wideband measurement factors after dynamic weighting.

[0083] Termination constraint: Termination boundary constraint obtained from the exit time in this step. Prior factors of transformation.

[0084] The data processing subsystem 120 utilizes a nonlinear least squares solver to perform offline or batch joint optimization of the global factor map. During the optimization process, since the start and end points of the factor map are anchored by high-precision GNSS prior factors, the solver forces the endpoints of the entire estimated trajectory to align with the real geographic coordinates. According to the least squares principle, the cumulative drift error generated during dead reckoning within the tunnel is smoothly distributed and amortized to each state node in the middle of the trajectory. After optimization convergence, the system outputs an optimal train trajectory that is closed at both ends, smooth internally, and geometrically consistent in the global coordinate system.

[0085] Finally, the data processing subsystem 120 uses the globally corrected optimal trajectory to generate a high-precision map. The system sequentially reads all the raw point cloud frame data collected by the 3D LiDAR 111 inside the tunnel. For the first... The system optimizes the high-precision pose (position) of the original point cloud at that moment. and posture A rigid body transformation matrix is ​​constructed. Using this transformation matrix, the system accurately transforms the point cloud data from the train carrier coordinate system to a fixed Earth coordinate system (such as the WGS84 coordinate system or a local tangent plane coordinate system). All point cloud data that have undergone coordinate transformation are superimposed and fused to form a 3D tunnel point cloud map with absolute geographic coordinate accuracy, and then output to an external storage or display device through a communication interface.

Claims

1. A seamless combined positioning system for complex railway tunnel environments, characterized in that, include: The data acquisition subsystem is configured to be installed on the train to acquire the train's motion status data and observation data of the surrounding environment. The data acquisition subsystem includes a three-dimensional lidar, an inertial measurement unit, a global navigation satellite system receiver, and an on-board ultra-wideband tag. A data processing subsystem, including a processor and a memory, is logically configured to include a signal integrity verification module and a multi-sensor fusion module. The signal integrity verification module is configured to construct a local environment map using the point cloud data collected by the three-dimensional lidar, and to perform line-of-sight analysis on the signal propagation path between the vehicle-mounted ultra-wideband tag and the external ultra-wideband base station based on the local environment map, and to identify non-line-of-sight propagation states. The multi-sensor fusion module is configured to receive inertial measurement data from the inertial measurement unit, point cloud data from the three-dimensional lidar, positioning data from the global navigation satellite system receiver, and ranging data from the vehicle-mounted ultra-wideband tag; and is configured to dynamically adjust the measurement noise covariance of the ranging data based on the identification result of the signal integrity verification module, and then calculate the train's pose through a factor graph optimization algorithm.

2. The seamless combined positioning system for complex railway tunnel environments according to claim 1, characterized in that, The data acquisition subsystem also includes installation components and a hardware synchronization controller; The mounting assembly includes a rigid housing, in which the three-dimensional lidar, the inertial measurement unit, the global navigation satellite system receiver, and the vehicle-mounted ultra-wideband tag are physically integrated within or on the surface of the rigid housing and rigidly connected to the train roof via the mounting assembly. The hardware synchronization controller is connected to the three-dimensional lidar, the inertial measurement unit, the global navigation satellite system receiver, and the vehicle-mounted ultra-wideband tag, respectively, and is used to send a unified time reference signal to unify the timestamps of the data from each sensor.

3. The seamless combined positioning system in a complex railway tunnel environment according to claim 1, characterized in that, The signal integrity verification module constructs a local environment map in the following ways: Centered on the current carrier coordinate system of the train, the point cloud data collected by the three-dimensional lidar and corrected for motion distortion are fused in real time to construct a local three-dimensional point cloud map containing geometric information of the tunnel wall and obstacles. The local 3D point cloud map is updated using a sliding window mechanism, covering the spatial range between the vehicle-mounted ultra-wideband tag and the ultra-wideband base station within the current communication range.

4. The seamless combined positioning system in a complex railway tunnel environment according to claim 3, characterized in that, The specific methods by which the signal integrity verification module performs line-of-sight analysis include: Determine the first coordinates of the vehicle-mounted ultra-wideband tag in the local three-dimensional point cloud map, and the second coordinates of the ultra-wideband base station; A virtual straight line segment is constructed between the first coordinate and the second coordinate, and a ray tracing algorithm is used to detect whether the virtual straight line segment spatially intersects with the obstacle point cloud data in the local 3D point cloud map; If the virtual straight line segment intersects with the obstacle point cloud data, the current signal propagation state is determined to be a non-line-of-sight propagation state. If they do not intersect, it is determined to be a line-of-sight propagation state.

5. The seamless combined positioning system in a complex railway tunnel environment according to claim 4, characterized in that, The specific strategy for the multi-sensor fusion module to dynamically adjust the measurement noise covariance is as follows: When the signal propagation state is determined to be line-of-sight propagation state, the measurement noise covariance of the corresponding ranging data is set to a predetermined nominal covariance value. When the signal propagation state is determined to be a non-line-of-sight propagation state, the measurement noise covariance of the corresponding ranging data is set to a maximum covariance value that is greater than the nominal covariance value, and the weight of the ranging data in the factor graph optimization is reduced.

6. The seamless combined positioning system in a complex railway tunnel environment according to claim 1, characterized in that, The factor graph model constructed by the multi-sensor fusion module includes state variable nodes and factor nodes; The state variable nodes include the train's three-dimensional position, three-dimensional velocity, three-dimensional attitude, accelerometer zero bias, and gyroscope zero bias at discrete moments; The factor nodes include: an IMU pre-integration factor connecting state variable nodes at adjacent time points, used to constrain relative motion increments; a LiDAR odometry factor connecting state variable nodes at adjacent time points, used to provide geometric motion constraints; and a dynamically weighted UWB ranging factor connecting state variable nodes and ultra-wideband base station nodes, wherein the weight of the dynamically weighted UWB ranging factor is determined by the dynamically adjusted measurement noise covariance.

7. The seamless combined positioning system for complex railway tunnel environments according to claim 1, characterized in that, The multi-sensor fusion module is also configured to perform IMU pre-integration processing, specifically including: Using the angular velocity and acceleration output by the inertial measurement unit, the attitude pre-integral, velocity pre-integral, and position pre-integral between adjacent time points are recursively calculated through numerical integration. The motion distortion correction of the original point cloud data of the three-dimensional lidar is performed using the result of the IMU pre-integration processing, and the corrected point cloud data is input into the scanning matching algorithm to generate the LiDAR odometry factor.

8. The seamless combined positioning system for complex railway tunnel environments according to claim 1, characterized in that, The data processing subsystem is also configured to perform boundary constraint acquisition, specifically including: When the signal quality index of the global navigation satellite system receiver is detected to be lower than a preset threshold, the state estimate value at the moment before the signal failure is locked as the initial boundary constraint and converted into the initial prior factor. When the signal quality index of the global navigation satellite system receiver is detected to recover to above the preset threshold, the state estimate at the time of signal recovery is locked as the termination boundary constraint and converted into a termination prior factor.

9. A seamless combined positioning system for complex railway tunnel environments according to claim 8, characterized in that, The multi-sensor fusion module is also configured to perform global trajectory correction, specifically including: Construct a global factor graph that includes the initial prior factor, all measurement factors during tunnel passage, and the termination prior factor; The global factor graph is jointly optimized using a nonlinear least squares solver. The accumulated dead reckoning error in the tunnel is eliminated by using the initial boundary constraints and the termination boundary constraints, and a globally consistent train trajectory is generated.

10. A seamless combination positioning method in a complex railway tunnel environment, characterized in that, The application of a seamless combined positioning system in a complex railway tunnel environment as described in any one of claims 1-9 includes the following steps: Before the train enters the tunnel, tight-coupled navigation is performed using the Global Navigation Satellite System, inertial measurement unit, and three-dimensional lidar. When satellite signal failure is detected, the state at the last moment is locked as the starting boundary constraint. When the train is running in the tunnel, the data from the inertial measurement unit is used for pre-integration calculation, and the motion distortion correction and odometer calculation are performed in combination with the point cloud data of the three-dimensional lidar. A local environment map is constructed using 3D LiDAR, and ray tracing detection is performed on the connection between the vehicle-mounted UWB tag and the UWB base station. If physical obstruction is detected, it is determined to be non-line-of-sight propagation and a large measurement noise covariance is assigned. If no physical obstruction is detected, it is determined to be line-of-sight propagation and a nominal covariance is assigned. A factor graph containing IMU pre-integration factors, LiDAR odometer factors, and UWB ranging factors adjusted by dynamic covariance is constructed, and the train pose is solved in real time by least squares optimization. When the train exits the tunnel and the satellite signal is detected to be restored, the state at the time of restoration is locked as the termination boundary constraint. A global factor graph containing the initial boundary constraint and the termination boundary constraint is constructed for adjustment optimization to generate the global trajectory of the train.

Images