Underground non-gnss roadway laser inertial navigation cooperative measurement method and system

Abstract

This invention discloses a laser inertial navigation collaborative measurement method and system for underground roadways without GNSS coverage, belonging to the field of underground positioning, navigation, and measurement technology. The method includes acquiring inertial sensor data of the vehicle to be tracked and roadway environmental observation data within the current time window, performing inertial navigation calculations to obtain a preliminary trajectory estimate; constructing geometrically constrained observations based on roadway geometric features and performing filtering estimation to obtain the error state adjustment amplitude; selecting linear correction or nonlinear filtering based on the error state adjustment amplitude to obtain the current corrected trajectory; generating a measurement reliability score based on multi-segment trajectory continuity data and evaluation features, and determining the usable trajectory or performing backtracking recalculation; performing stability analysis on the usable trajectory, and outputting trajectory verification results with reliability labels. This invention can reduce the impact of inertial error accumulation and environmental interference on trajectory measurement results in roadway environments without GNSS coverage, improving the reliability of trajectory results when used for control or monitoring judgments.

Description

Technical Field

[0001] This invention relates to the field of downhole positioning, navigation and measurement technology, specifically to a laser inertial navigation collaborative measurement method and system for GNSS-free downhole tunnels. Background Technology

[0002] In enclosed environments such as mine roadways and underground tunnels, GNSS signals are difficult to maintain stable coverage, making it difficult for tracking vehicles such as mine trucks, inspection robots, or handheld measuring terminals to continuously obtain absolute position references. Existing underground positioning and measurement methods typically rely on inertial navigation, laser observation, or a combination of both. Inertial sensors are used to continuously calculate the vehicle's attitude and position, and environmental information such as roadway boundaries, orientation, or slope is used to correct the trajectory.

[0003] However, when relying solely on inertial navigation, inertial sensor errors accumulate over time, causing the trajectory to gradually drift. When relying solely on general environmental observation corrections, external constraints are easily weakened or distorted under conditions such as long tunnels, repetitive textures, dusty and slippery surfaces, undulating roads, or strong vibration interference, resulting in insufficient stability of the trajectory results and making them difficult to directly use for control, monitoring, or safety assessment of downhole equipment. Therefore, existing technologies still lack a measurement method and system capable of collaboratively correcting inertial calculation results and tunnel environment observation results in GNSS-free tunnel environments, and assessing trajectory reliability.

[0004] Currently, there is a lack of an effective method to solve the problems in existing technologies.

[0005] The information disclosed in this background section is intended only to enhance the understanding of the overall background of the invention and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0006] The purpose of this invention is to provide a laser inertial navigation collaborative measurement method and system for underground roadways without GNSS, so as to solve the above-mentioned defects.

[0007] On the one hand, to solve the above-mentioned technical problems, this invention provides a method for laser inertial navigation cooperative measurement in underground GNSS-free tunnels, including: acquiring inertial sensor data and tunnel environment observation data of the vehicle to be tracked within the current time window, and performing inertial navigation calculations to obtain a preliminary trajectory estimate of the vehicle to be tracked; extracting geometric features reflecting changes in tunnel heading or slope based on the tunnel environment observation data to construct geometric constraint observations; performing filtering estimation based on the preliminary trajectory estimate as a priori state and the geometric constraint observations as observation update quantities to obtain an error state adjustment amplitude; performing linear correction on the preliminary trajectory estimate using the geometric constraint observations based on the comparison result of the error state adjustment amplitude and a first preset threshold to obtain the current corrected trajectory and a first evaluation feature; or performing environmental interference assessment to generate... The parameter compensation model and attitude angle correction are used to update the trajectory state through nonlinear filtering, resulting in the current corrected trajectory and the convergence feature as the second evaluation feature. Based on the multi-segment trajectory continuity data formed by the current corrected trajectory and adjacent trajectory segments, the multi-segment trajectory continuity data is fused with the first evaluation feature or the second evaluation feature to generate a measurement reliability score. Based on the comparison result of the measurement reliability score and a second preset threshold, the current corrected trajectory is determined to be a usable trajectory. Alternatively, based on the preliminary trajectory estimation, backtracking recalculation is performed on abnormal trajectory segments to obtain the optimized overall trajectory path and determine it as a usable trajectory. The residual error of the usable trajectory is subjected to stability analysis, and a reliability label is generated based on the analysis results. The trajectory verification result with the reliability label is then output.

[0008] Optionally, the step of extracting geometric features reflecting changes in the roadway heading or slope from the roadway environment observation data and constructing geometric constraint observations includes: extracting turning points and undulating segments from the roadway environment observation data within the current trajectory window; forming geometric constraint quantities related to changes in heading and / or slope based on the extracted turning points and undulating segments, wherein the geometric constraint observations include the geometric constraint quantities.

[0009] Optionally, the comparison result between the error state adjustment amplitude and the first preset threshold includes: when the error state adjustment amplitude does not exceed the first preset threshold, the geometric constraint observation is applied as an update quantity to the preliminary trajectory estimation using Kalman filtering, and the linear correction is performed; when the error state adjustment amplitude exceeds the first preset threshold, the environmental disturbance assessment is triggered, and the nonlinear filtering is performed using particle filtering combined with the parameter compensation model and the attitude angle correction quantity.

[0010] Optionally, the step of performing environmental interference assessment to generate parameter compensation model and attitude angle correction includes: calculating at least one of high-frequency vibration energy, attitude change amount, and geometric residual, and outputting interference assessment features; determining the interference level based on the interference assessment features according to preset rules or historical mapping relationships; generating the parameter compensation model by calling the corresponding compensation mapping table or piecewise linear mapping relationship according to the interference level, and outputting the attitude angle correction through the parameter compensation model.

[0011] Optionally, the method for extracting the convergence feature as the second evaluation feature includes: during the process of updating the trajectory state by nonlinear filtering, extracting the convergence feature based on at least one of particle weight concentration, effective particle number, and cumulative deviation decreasing trend.

[0012] Optionally, the step of combining the multi-segment trajectory continuity data with the first evaluation feature or the second evaluation feature to generate a measurement reliability score includes: generating a first score representing trajectory continuity based on the multi-segment trajectory continuity data; generating a second score representing table convergence based on the first evaluation feature or the second evaluation feature; and normalizing and weighting the first score and the second score according to preset dynamic weights to obtain the measurement reliability score.

[0013] Optionally, the step of performing backtracking recalculation on abnormal trajectory segments to obtain the optimized overall trajectory path includes: when the measurement reliability score is lower than the second preset threshold, performing the backtracking recalculation on the abnormal trajectory segments, and fusing the preliminary trajectory estimation, the error state adjustment amplitude, and the nonlinear filtering posterior result within the corresponding time window; when the recalculation process reaches a preset stopping condition, ending the backtracking and outputting the optimized overall trajectory path; wherein, the stopping condition includes reaching the maximum number of iterations or the recalculation benefit being lower than the minimum improvement amount.

[0014] Optionally, the step of performing stability analysis on the residual error of the available trajectory, generating a reliability label based on the analysis results, and outputting the trajectory verification results with the reliability label includes: performing stability analysis on the residual angle error, residual cumulative deviation, and short-term jitter of the available trajectory; if the analysis results are stable, then attaching a high-confidence reliability label to the available trajectory and outputting it; if the analysis results are unstable, then attaching a low-confidence reliability label to the available trajectory and simultaneously outputting control signals for triggering alarms, speed limits, or degraded operation.

[0015] On the other hand, the present invention also provides a GNSS-free underground tunnel laser inertial navigation cooperative measurement system, comprising: a data acquisition module configured to acquire inertial sensor data and tunnel environment observation data of the vehicle to be tracked within the current time window, and perform inertial navigation calculation to obtain a preliminary trajectory estimate of the vehicle to be tracked; a geometric constraint construction module configured to extract geometric features reflecting tunnel heading or slope changes based on the tunnel environment observation data, construct geometric constraint observations, and perform filtering estimation based on the preliminary trajectory estimate as a priori state and the geometric constraint observations as observation update quantities to obtain an error state adjustment amplitude; and a graded correction module configured to perform linear correction on the preliminary trajectory estimate using the geometric constraint observations based on the comparison result of the error state adjustment amplitude and a first preset threshold to obtain the current corrected trajectory and a first evaluation feature; or perform an environmental interference assessment to generate parameter compensation module. The system comprises: a trajectory state update module for type and attitude angle correction, which updates the trajectory state through nonlinear filtering to obtain the current corrected trajectory and a convergence feature as the second evaluation feature; a reliability scoring module configured to generate a measurement reliability score by combining the multi-segment trajectory continuity data formed by the current corrected trajectory and adjacent trajectory segments with the first or second evaluation feature; a trajectory optimization module configured to determine the current corrected trajectory as a usable trajectory based on the comparison result of the measurement reliability score and a second preset threshold; or to perform backtracking recalculation on abnormal trajectory segments based on the preliminary trajectory estimation to obtain the optimized overall trajectory path and determine it as a usable trajectory; and a stability verification output module configured to perform stability analysis on the residual error of the usable trajectory, generate a reliability label based on the analysis result, and output the trajectory verification result with the reliability label.

[0016] Beneficial effects:

[0017] This invention acquires inertial sensor data and tunnel environment observation data of the vehicle to be tracked within the current time window. It first performs inertial navigation calculation to obtain a preliminary trajectory estimate, then constructs geometric constraint observations based on geometric features reflecting changes in tunnel heading or slope, and adjusts the amplitude according to the error state to select linear correction or nonlinear filtering correction after environmental interference assessment. This solves the problems of continuous accumulation of inertial errors in tunnel environments without GNSS coverage and the easy weakening and distortion of tunnel environment constraints under strong vibration or complex tunnel conditions, resulting in insufficient reliability of trajectory measurement results. It realizes graded correction of the preliminary trajectory, backtracking optimization of abnormal trajectory segments, and marking of the reliability of the output trajectory, thereby improving the reliability of trajectory measurement results when used for downhole equipment control, monitoring, or safety judgment. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the method flow provided in the embodiment of the present invention;

[0019] Figure 2 This is a schematic diagram of the geometric constraint construction and hierarchical correction process provided in an embodiment of the present invention;

[0020] Figure 3 This is a schematic diagram illustrating the interaction between environmental interference assessment and nonlinear filtering provided in an embodiment of the present invention;

[0021] Figure 4 This is a schematic diagram of the measurement reliability scoring and backtracking optimization data flow provided in an embodiment of the present invention;

[0022] Figure 5 This is a schematic diagram of stability analysis and output feedback provided in an embodiment of the present invention;

[0023] Figure 6 This is a schematic diagram of the system module structure provided in an embodiment of the present invention. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only for explaining the invention and are not intended to limit the invention; that is, the described embodiments are merely some embodiments of the invention, and not all embodiments. The components of the embodiments of the invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0025] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0026] It should be noted that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0027] As mentioned earlier, in enclosed environments without GNSS coverage, such as mine roadways and underground tunnels, it is difficult for the tracked vehicle to continuously obtain a stable absolute position reference. If only inertial navigation is relied upon for continuous calculation, the error of the inertial sensor will accumulate over time. If only general environmental observations are relied upon for correction, external constraints may be weakened or distorted under conditions such as long corridor-like roadways, repetitive textures, dusty and slippery surfaces, undulating roads, or strong vibration interference, making it difficult to directly use the trajectory results for control or monitoring judgment.

[0028] To address this, the present invention provides a laser inertial navigation collaborative measurement method and system for GNSS-free underground roadways. By acquiring inertial sensor data and roadway environmental observation data within the current time window, inertial navigation calculations are first performed to form a preliminary trajectory estimate. Then, geometric constraint observations reflecting changes in roadway heading or slope are used for filtering correction. The amplitude is adjusted according to the error state, selecting either a linear correction link or a nonlinear filtering link after environmental interference assessment. Subsequently, a measurement reliability score is generated based on the current corrected trajectory, multi-segment trajectory continuity data, and evaluation characteristics. If the score is unsatisfactory, backtracking and recalculation are performed. Finally, stability analysis is conducted on the available trajectories, and trajectory verification results with reliability labels are output.

[0029] In embodiments of the present invention, the carrier to be tracked may specifically be a mining truck, an inspection robot, or a handheld navigation terminal, etc. The following is in conjunction with... Figures 1-6 This invention is described in detail.

[0030] Example 1:

[0031] like Figure 1 As shown in the embodiment of the present invention, a laser inertial navigation cooperative measurement method for underground roadways without GNSS can be executed by an underground positioning and navigation control terminal, a processor, or a measurement control unit mounted on the carrier to be tracked. The method mainly includes the following steps:

[0032] S100: Obtain the inertial sensor data and tunnel environment observation data of the carrier to be tracked within the current time window, and perform inertial navigation calculation to obtain the preliminary trajectory estimate of the carrier to be tracked.

[0033] In this step, inertial sensor data provides motion state information such as acceleration and angular velocity of the tracked vehicle within the current time window, while tunnel environment observation data provides environmental references related to tunnel boundaries, orientation, or undulations. The processor performs attitude recursion, velocity updates, and position updates based on the data within the current time window, forming the prior trajectory required for subsequent geometric constraint construction.

[0034] S200. Based on the tunnel environment observation data, extract geometric features reflecting changes in tunnel heading or slope, construct geometric constraint observations, and perform filtering estimation based on the preliminary trajectory estimation and the geometric constraint observations to obtain the error state adjustment amplitude.

[0035] In this step, geometric features are used to describe the changes in roadway direction or undulation within the current trajectory window. Geometrically constrained observations are used as observation updates in the filtering estimation, ensuring that the initial trajectory estimation is constrained by the physical morphology of the roadway. The error state adjustment magnitude obtained after filtering estimation is used to characterize the error scale that needs to be adjusted in this round of linear correction and is then used in subsequent graded correction judgments.

[0036] S300. Based on the comparison result between the error state adjustment amplitude and the first preset threshold, perform linear correction on the preliminary trajectory estimation, or perform environmental interference assessment and update the trajectory state through nonlinear filtering to obtain the current corrected trajectory and corresponding evaluation features.

[0037] In this step, when the error state adjustment amplitude does not exceed the first preset threshold, it indicates that the current error is within the range that can be handled by geometric constraints and linear filtering. Therefore, linear correction is performed using geometric constraint observation, and the first evaluation feature is formed. When the error state adjustment amplitude exceeds the first preset threshold, it indicates that there may be strong vibration, sideslip, or geometric matching anomalies in the current time window. Therefore, environmental interference assessment is performed first to generate parameter compensation model and attitude angle correction amount. Then, the trajectory state is updated through nonlinear filtering, and the convergence feature is formed as the second evaluation feature.

[0038] S400: Based on the multi-segment trajectory continuity data formed by the current corrected trajectory and adjacent trajectory segments, the multi-segment trajectory continuity data is combined with the first evaluation feature or the second evaluation feature to perform fusion calculation and generate a measurement reliability score.

[0039] In this step, multiple trajectory continuity data are used to reflect whether the changes in position, heading, or slope between the current corrected trajectory and adjacent trajectory segments are continuous. The first or second evaluation feature is used to reflect the convergence of linear correction or nonlinear filtering. After the measurement reliability score is formed through fusion calculation, the score is sent to the subsequent threshold determination stage.

[0040] S500. Based on the comparison result between the measurement reliability score and the second preset threshold, determine that the current corrected trajectory is a usable trajectory, or perform backtracking recalculation on the abnormal trajectory segment to obtain the optimized overall trajectory path and determine it as a usable trajectory.

[0041] In this step, when the measurement reliability score reaches the second preset threshold, the current corrected trajectory can be used as the available trajectory for the current time window and enter the stability analysis; when the measurement reliability score is lower than the second preset threshold, the abnormal trajectory segments are backtracked and recalculated, and the preliminary trajectory estimation, error state adjustment results and nonlinear filtering posterior results are fused to obtain the optimized overall trajectory path.

[0042] S600. Perform stability analysis on the residual error of the available trajectory, generate reliability labels based on the analysis results, and output the trajectory verification results with reliability labels.

[0043] In this step, the available trajectory is not directly output as the final result. Instead, it is further combined with residual angle error, residual cumulative deviation, and short-term jitter for stability analysis. When the analysis result is stable, a high-confidence reliability label is attached to the available trajectory; when the analysis result is unstable, a low-confidence reliability label is attached to the available trajectory, and control signals that can be used to trigger alarms, speed limits, or degraded operation are output simultaneously.

[0044] Based on the above steps, the collaborative measurement method provided by this invention explicitly extracts roadway geometric features as constraint observations, and then combines error state adjustment amplitude and scoring mechanisms to construct a hierarchical correction link with adaptive switching between linear and nonlinear modes, as well as a recalculation and backtracking mechanism. This method, while retaining efficient calculation within small error intervals, enables the system to effectively cope with trajectory divergence caused by strong nonlinear disturbances, ultimately outputting a trajectory with a reliability label, thereby providing a safety judgment basis for the scheduling and control of end-point equipment.

[0045] Example 2:

[0046] To provide a more detailed explanation of the technical solutions provided in the above embodiments, the present invention also provides another preferred embodiment. This preferred embodiment proceeds in the order of data acquisition within the current time window, geometric constraint construction, hierarchical correction, reliability scoring, trajectory optimization, and stability verification.

[0047] In another embodiment of the present invention, step S100, acquiring the inertial sensor data and tunnel environment observation data of the carrier to be tracked within the current time window, and performing inertial navigation calculation, may include the following steps:

[0048] S110. Obtain the inertial sensor data of the carrier to be tracked within the current time window.

[0049] In this step, the inertial sensor data may include the acceleration and angular velocity data of the carrier to be tracked within the current time window. The data is arranged in time sequence according to the sampling time and serves as the basis for inertial navigation calculation.

[0050] S120. Obtain the tunnel environment observation data within the current time window.

[0051] In this step, the tunnel environment observation data is used to reflect observation information related to the physical morphology of the tunnel, such as tunnel boundaries, centerline, direction changes, or elevation changes. This information is then used to extract geometric features that reflect changes in tunnel heading or slope.

[0052] S130. Time synchronization and coordinate system integration of inertial sensor data and tunnel environment observation data.

[0053] In this step, the processor can map the inertial sensor data and the tunnel environment observation data to the same time window according to the sampling time, and unify them according to the pre-established transformation relationship between the carrier coordinate system, the sensor coordinate system and the tunnel reference coordinate system, so that the subsequent geometric constraint observation can correspond to the preliminary trajectory estimation.

[0054] S140. Based on the synchronized inertial sensor data, perform inertial navigation calculations to obtain a preliminary trajectory estimate.

[0055] In this step, the processor performs attitude recursion, velocity update, and position update based on the acceleration and angular velocity within the current time window, forming a sequence of position, velocity, and attitude of the vehicle to be tracked within the current time window. This sequence of position, velocity, and attitude constitutes a preliminary trajectory estimate.

[0056] S150. When a short-term data gap is detected within the current time window, perform short-term extrapolation and mark the low-confidence interval.

[0057] In this step, if there is a short-term missing data in the inertial sensor data or tunnel environment observation data within the current time window, the trajectory state of the previous moment is used for short-term extrapolation to fill in the trajectory state corresponding to the missing data, and the trajectory segment corresponding to the extrapolation is marked as a low confidence interval.

[0058] For example, steps S110 to S150 may further include the following process: When the vehicle to be tracked enters a new time window, the processor acquires the continuously collected angular velocity, acceleration, and tunnel environment observation data within that time window; then, the above data is aligned according to the sampling time, and the tunnel observation results are converted to a coordinate expression comparable to the preliminary trajectory estimation; after synchronization is completed, the processor uses inertial navigation to calculate the preliminary trajectory estimation within the current time window, which serves as the prior state for geometric constraint construction and filtering estimation in step S200. If a short-term sampling segment is missing, a short-term extrapolation is first performed using the trajectory state at the previous moment, and the low-confidence marker of the extrapolated segment is passed to the subsequent scoring steps along with the preliminary trajectory estimation.

[0059] For example, the vehicle-mounted control terminal acquires IMU and LiDAR data at fixed intervals and outputs a preliminary trajectory for the current time window through integration. If the LiDAR data is lost due to dust obstruction within a certain second, the terminal will extrapolate and fill in the missing frame using the IMU and the effective velocity and heading data from the previous second, and mark this 1-second trajectory segment as a "low confidence interval" to prevent it from being used as a high-precision reference during subsequent filtering.

[0060] In another embodiment of the present invention, in step S200, geometric features reflecting changes in the tunnel's heading or slope are extracted based on tunnel environment observation data, geometric constraint observations are constructed, and filtered estimation is performed based on preliminary trajectory estimation and geometric constraint observations. This may include the following steps:

[0061] S210. Within the current trajectory window, extract turning points and undulating sections from the tunnel environment observation data.

[0062] In this step, turning points are used to reflect locations or trajectory segments where the tunnel's heading changes, while undulating segments are used to reflect locations or trajectory segments where the tunnel's slope or elevation changes significantly. Turning points and undulating segments together form the basis for subsequent geometrically constrained observations.

[0063] S220. Based on the extracted turning points and undulating segments, geometric constraints related to changes in heading and / or slope are formed.

[0064] In this step, geometric constraints can be used to describe the heading change relationship, slope change relationship, or a combination of both that the tracked vehicle should satisfy within the current trajectory window. The geometric constraints are organized into geometric constraint observations and correspond to the preliminary trajectory estimation.

[0065] S230. Using the preliminary trajectory estimate as the prior state and the geometric constraint observation as the observation update quantity, perform filtered estimation.

[0066] In this step, the filtered estimation can introduce geometrically constrained observations into the initial trajectory estimation update process to estimate the error state associated with the inertial sensor error and obtain the error state adjustment magnitude.

[0067] Specifically, the error state vector is typically defined as: ,in, Indicates positional error. Indicates speed error, Indicates attitude error. and These represent the zero-bias error of the gyroscope and accelerometer, respectively. The processor uses an extended Kalman filter (EKF) to calculate the error state update vector introduced by geometrically constrained observations, and calculates the error state adjustment amplitude using the following formula. :

[0068] ;

[0069] in, Represents the geometrically constrained observation vector. Represents the observation matrix. This represents the prior error state estimate before the update. Represents the Kalman gain matrix. The L2 norm, or Euclidean distance, represents the magnitude of the physical distance correction made by a single filter on the trajectory position (e.g., the observation vector when undulations are identified ahead). A residual of 0.5 meters appears relative to the prior elevation, which is then corrected by Kalman gain. After weight allocation, the error state adjustment magnitude is calculated. 0.4 meters).

[0070] S240, The error state adjustment amplitude is sent to the graded correction judgment.

[0071] In this step, the error state adjustment amplitude is used to characterize the error scale that can be corrected solely by linear correction within the current time window. This amplitude is compared with a first preset threshold to determine whether to subsequently perform linear correction or trigger environmental interference assessment and nonlinear filtering.

[0072] For example, steps S210 to S240 can further include the following process: The processor analyzes the tunnel environment observation data within the current trajectory window. If a continuous turning point is detected in the direction of the tunnel centerline, the corresponding position is designated as a turning point. If a change in tunnel elevation or pitch direction is detected to meet geometric observation conditions, the corresponding trajectory segment is designated as an undulating segment. Subsequently, the processor generates heading change constraints based on the turning points and slope change constraints based on the undulating segments. These constraints are then used as observation update quantities for filtering estimation to obtain the error state adjustment amplitude corresponding to the current time window. This error state adjustment amplitude is not directly used as the final output but is instead used in step S300 for graded correction.

[0073] For example, the terminal detects continuous changes in the direction of the tunnel centerline and records them as turning points; it detects changes in the elevation gradient and records them as undulating sections. The physical boundaries of these turns and undulations constitute the geometric constraint that "the carrier cannot penetrate the wall." This constraint is compared with the initial trajectory input filter. If the filter calculates that the original trajectory needs to be adjusted back by 0.6 meters, then this "0.6 meters" is the error state adjustment amplitude.

[0074] In another embodiment of the present invention, step S300, performing graded correction based on the comparison result between the error state adjustment amplitude and the first preset threshold, may include the following steps:

[0075] S310. Compare the error state adjustment amplitude with the first preset threshold.

[0076] In this step, the first preset threshold is used to determine whether the current error state is still within the range that linear correction can handle. The first preset threshold can be determined based on the inertial sensor calibration results, equipment trial operation results, or historical operation data statistics, and can be updated during equipment maintenance or on-site operation.

[0077] S320. When the error state adjustment amplitude does not exceed the first preset threshold, linear correction is performed on the preliminary trajectory estimation using geometric constraint observation.

[0078] In this step, the processor uses Kalman filtering to apply geometrically constrained observations as update quantities to the initial trajectory estimation, obtains the current corrected trajectory, and forms the first evaluation feature based on the filter consistency or residual changes during the linear correction process.

[0079] S330. When the error state adjustment amplitude exceeds the first preset threshold, an environmental interference assessment is triggered.

[0080] In this step, the processor calculates interference assessment features based on inertial sensor data, geometric residuals, or attitude changes within the current time window, and determines the interference level according to preset rules or historical mapping relationships. In a preferred embodiment, the interference assessment features utilize high-frequency vibration energy. (i.e., the acceleration variance characterization value) indicates that the processor calculates the high-frequency vibration energy within the current time window using the following formula:

[0081] ;

[0082] in, This indicates the total number of accelerometer sampling points within the current time window. Indicates the first The three-axis acceleration vector at each sampling time point, This represents the average acceleration vector within the time window.

[0083] S340. Generate a parameter compensation model based on the interference level, and output the attitude angle correction amount through the parameter compensation model.

[0084] In this step, the parameter compensation model is used to describe the correspondence between the disturbance level, the parameter to be compensated, and the attitude angle correction. To enable the system to adaptively compensate under strong vibrations, the parameter compensation model can employ a piecewise linear mapping model, outputting the attitude angle correction for heading or pitch using the following formula. :

[0085] ;

[0086] in, and This represents the slope coefficient and bias term corresponding to the i-th interference level. These coefficients constitute a compensation coefficient set, which can be determined based on equipment calibration results, historical operating data, or field configuration. (Processor judgment) The corresponding interval level is then used to calculate... (For example, high-frequency vibration energy is detected in the current time window) This triggered Level 3 "Strong Interference", invoking calibration parameters. The attitude angle correction amount is calculated. ).

[0087] To enable the model to have a callable internal structure in the engineering code, the processor can pre-store the historical mapping relationship between interference levels and compensation coefficient sets. These compensation coefficient sets are used at least to determine the generation method of the heading angle correction or pitch angle correction. Once the interference level is determined, the processor reads the compensation coefficient set corresponding to that interference level and, combined with the geometric residual, attitude mutation, or high-frequency vibration energy within the current time window, generates the attitude angle correction. The historical mapping relationship can be stored in the form of a compensation mapping table. When a smooth transition is required between adjacent interference levels, the compensation coefficient set can be determined according to a piecewise linear mapping relationship. The source of the compensation mapping table or piecewise linear mapping relationship can be determined based on equipment calibration results, historical operating data, or on-site configuration. The calculation result of the attitude angle correction enters the particle state prediction stage in step S350.

[0088] S350. A nonlinear filter is performed using particle filtering combined with the parameter compensation model and the attitude angle correction to obtain the current corrected trajectory and convergence characteristics.

[0089] In this step, particle filtering can initialize particles and predict their states based on the current trajectory state. The attitude angle correction is injected into the particle prediction step as a pre-compensation amount during the state transition process. That is, based on the particle state at the previous moment, the processor first recursively derives the predicted attitude of the particles from the inertial sensor data, then superimposes the attitude angle correction output by the parameter compensation model to form the corrected predicted particle attitude, and updates the particle position state based on the corrected predicted particle attitude. Subsequently, the processor calculates the particle weights based on the matching residuals between the corrected predicted particle state and the tunnel environment observation data. This weight update process follows the following Gaussian observation likelihood probability:

[0090] ;

[0091] in, Indicates the first At the current moment, each particle The weight, Indicates the current direction of environmental observation. This represents the mapping function from the state space to the observation space. This represents the prior position state of the particle. This is the amount of attitude angle correction injected. To determine the observation noise covariance matrix, we introduce... This allows the particle swarm to be forced to converge toward a realistic physical boundary when it experiences nonlinear sideslip or severe turbulence (for example, a particle whose observation residual was originally as high as 2 meters caused its weight to decay to 0.01, but after injection...). After correction and compensation, its mapping residual dropped sharply to 0.2 meters, and the weight was boosted and updated to 0.85, thus effectively avoiding particle degradation.

[0092] Therefore, the attitude angle correction is not directly involved in the weight calculation as an independent weight term, but rather indirectly affects the particle weight update by changing the particle prediction state. During the update process, the processor extracts convergence features based on at least one of the following: particle weight concentration, effective particle count, and cumulative deviation decreasing trend.

[0093] Specifically, step S330 may further include: S331, calculating at least one of high-frequency vibration energy, attitude change amount, and geometric residual, and outputting interference evaluation features; S332, determining the interference level based on the interference evaluation features according to preset rules or historical mapping relationships; S333, calling the corresponding compensation mapping table or piecewise linear mapping relationship according to the interference level; S334, determining the compensation coefficient group corresponding to the current interference level from the compensation mapping table or piecewise linear mapping relationship; S335, generating attitude angle correction amount based on the compensation coefficient group and the geometric residual, attitude change amount, or high-frequency vibration energy within the current time window.

[0094] The step S350 may further include: S351, initializing particles based on the current trajectory state; S352, recursively calculating the predicted attitude of particles based on inertial sensor data; S353, injecting the attitude angle correction into the state transition process to generate the corrected predicted particle attitude and predicted particle position; S354, updating particle weights based on the matching residual between the corrected predicted particle state and the tunnel environment observation data; S355, performing resampling when particle degradation meets the resampling condition; and S356, extracting convergence features based on the particle weight concentration, effective particle count, or cumulative deviation decreasing trend.

[0095] For example, steps S310 to S350 can further include the following process: If the error state adjustment amplitude obtained by the processor after filtering based on geometric constraint observations and preliminary trajectory estimation does not exceed the first preset threshold, it indicates that the current trajectory deviation can be corrected by linear filtering and geometric constraint observations. Therefore, the processor directly outputs the current corrected trajectory and the first evaluation feature. If the error state adjustment amplitude exceeds the first preset threshold, it indicates that there may be strong disturbances in the current time window. The processor further calculates the high-frequency vibration energy, attitude change amount, or geometric residual to form interference evaluation features and determines the interference level according to the historical mapping relationship. Subsequently, the processor calls the compensation mapping table or piecewise linear mapping relationship according to the interference level to determine the corresponding compensation coefficient group and outputs the attitude angle correction amount based on the compensation coefficient group. The attitude angle correction amount is first injected into the state transition process of particle filtering to correct the particle predicted attitude and particle predicted position. The particle weight is updated based on the matching residual between the corrected particle predicted state and the tunnel environment observation data. Thus, the current corrected trajectory and convergence feature after nonlinear filtering enter step S400.

[0096] If the error state adjustment amplitude is 1.5 meters (exceeding the first preset threshold of 1.0 meter), the terminal senses a strong external disturbance (such as severe sideslip caused by running over gravel). It immediately calculates the high-frequency vibration energy and maps it as a "high-level" disturbance. The system calls the corresponding compensation model to generate new pitch and heading attitude angle corrections, which are then input into a more robust particle filter stage. During particle filter resampling, if particles rapidly converge towards the true position (the effective particle count remains high), the convergence feature reflecting this health state is extracted and passed to the next stage along with the trajectory.

[0097] In another embodiment of the present invention, step S400, generating a measurement reliability score based on the current corrected trajectory, multi-segment trajectory continuity data, and evaluation features, may include the following steps:

[0098] S410. Based on the current corrected trajectory and adjacent trajectory segments, form multiple segments of trajectory continuity data.

[0099] In this step, the multi-segment trajectory continuity data can reflect whether the positional changes, heading changes, or gradient changes between the current corrected trajectory and the adjacent trajectory segments before and after it are continuous.

[0100] S420. Generate a first score representing the continuity of the trajectory based on the multi-segment trajectory continuity data.

[0101] In this step, the processor can normalize multiple segments of trajectory continuity data to form a trajectory continuity score that can be used for fusion calculation.

[0102] S430. Generate a second score for table convergence based on the first evaluation feature or the second evaluation feature.

[0103] In this step, when a linear correction link is used in step S300, the processor generates a second score based on the first evaluation feature; when a nonlinear filtering link is used in step S300, the processor generates a second score based on the convergence feature, which is the second evaluation feature.

[0104] The first evaluation feature includes at least one of the following: the magnitude of the decrease in filter residuals, the filter consistency index, and the change in geometric constraint residuals.

[0105] S440. The first score and the second score are normalized and weighted according to the preset dynamic weights to obtain the measurement reliability score.

[0106] In this step, dynamic weights are used to balance the contributions of trajectory continuity and filter convergence to measurement reliability. These dynamic weights can be determined based on historical operational data statistics, pilot or intermediate test results, and downhole operating condition configurations.

[0107] To ensure that the measurement reliability score has a clear calculation relationship, in one implementation method, the following normalized weighted form can be used:

[0108]

[0109] in, To measure reliability scores; The first score, obtained from multiple segments of trajectory continuity data, is used to characterize trajectory continuity. The second score, obtained based on the first or second evaluation feature, is used to characterize the convergence of linear correction or nonlinear filtering. The preset dynamic weights can be determined based on equipment calibration results, historical operating data, or on-site configuration. The value of is not the only limitation of this embodiment. The input of the formula is the first score and the second score, and the output is the measurement reliability score. The calculation result is then compared with the second preset threshold in step S500.

[0110] For example, steps S410 to S440 may further include the following process: The processor selects the preceding and following adjacent trajectory segments of the current corrected trajectory, calculates the continuity of position change, heading change, and slope change among the three trajectory segments, and forms a first score accordingly; simultaneously, the processor forms a second score based on the first evaluation feature in the linear correction process or the convergence feature in the particle filtering process. Subsequently, the processor fuses the first score and the second score according to preset dynamic weights to obtain the measurement reliability score corresponding to the current time window, and sends this score to step S500.

[0111] The terminal scores the continuity of the current segment into 80 points (i.e., The particle filter convergence health score is 90 points (i.e., The system retrieves dynamic weights obtained based on historical calibration. (For example Substituting these values ​​into the above formula yields the final measurement reliability score. It is 84 points (i.e. This score will directly determine whether the trajectory segment can be allowed to proceed.

[0112] In another embodiment of the present invention, step S500, determining the available trajectory or performing backtracking recalculation based on the comparison result of the measurement reliability score and the second preset threshold, may include the following steps:

[0113] S510. Compare the measurement reliability score with a second preset threshold.

[0114] In this step, the second preset threshold is used to determine whether the current corrected trajectory meets the availability requirements. The second preset threshold can be determined based on historical trajectory error samples, on-site pre-test results, or statistical results from the operation and maintenance phase (for example, setting the second preset threshold to 75 points, with only scoring). (Only then is the trajectory considered reliable).

[0115] S520. When the measurement reliability score reaches the second preset threshold requirement, the current corrected trajectory is determined as an available trajectory.

[0116] In this step, the current corrected trajectory is deemed to meet the entry conditions for subsequent stability analysis and continues to be passed to step S600.

[0117] S530. When the measurement reliability score is lower than the second preset threshold, backtracking and recalculation are performed on the abnormal trajectory segment.

[0118] In this step, the processor can determine anomalous trajectory segments based on the time window location with lower scores or low confidence interval markers, and establish a local backtracking window centered on the time window where the anomalous trajectory segment is located. The local backtracking window may include the time window where the anomalous trajectory segment is located, adjacent trajectory segments that are located before the anomalous trajectory segment and have already formed reliability labels, and adjacent trajectory segments that are located after the anomalous trajectory segment and need to be re-verified (for example, setting the local backtracking window to a data window that extends 3 seconds before and after the anomalous segment, and reallocating observation noise within this 6-second range for global graph optimization or smooth recalculation).

[0119] Within the local backtracking window, the processor fuses the preliminary trajectory estimation, error state adjustment amplitude, and nonlinear filtering posterior result for the corresponding time window to correct the trajectory continuity within that window.

[0120] For example, using the nonlinear filtering posterior result as a high-confidence node, constructing relative pose constraint edges between nodes with the initial trajectory estimation and error state adjustment amplitude, and then optimizing the global graph by constructing a pose graph and using the least squares method, thus achieving trajectory fusion.

[0121] S540. When the recalculation process reaches the preset stopping condition, the backtracking ends and the optimized overall trajectory path is output.

[0122] In this step, the stopping conditions include reaching the maximum number of iterations or the recalculation benefit being lower than the minimum improvement. The recalculation benefit can be determined based on the improvement in measurement reliability score, the decrease in residual error, or the improvement in trajectory continuity after two adjacent local backtracking steps. After recalculation, the processor connects the corrected trajectory within the local backtracking window with the determined trajectory segments outside the window according to the continuity of adjacent endpoints to form an optimized overall trajectory path, and then determines it as a usable trajectory before proceeding to step S600.

[0123] For example, steps S510 to S540 can further include the following process: When the measurement reliability score corresponding to a certain time window reaches the second preset threshold requirement, the processor takes the current corrected trajectory of that time window as the usable trajectory and continues to perform stability analysis; when the score is lower than the second preset threshold, the processor determines the abnormal trajectory segment based on the low score position and establishes a local backtracking window centered on the time window where the abnormal trajectory segment is located. Subsequently, the processor backtracks to the corresponding preliminary trajectory estimation point within the local backtracking window, and re-integrates the preliminary trajectory estimation, error state adjustment amplitude, and nonlinear filtering posterior result to improve the trajectory continuity and reliability score within the window. If the recalculated score meets the requirements, or the recalculation has reached the maximum number of iterations, or the improvement amount of two adjacent recalculations is lower than the minimum improvement amount, the backtracking ends, and the corrected trajectory within the local backtracking window is connected with the determined trajectory segment outside the window to output the optimized overall trajectory path.

[0124] When the recalculation process reaches a preset stopping condition, the backtracking ends and the optimized overall trajectory path is output as a usable trajectory. The stopping condition includes reaching the maximum number of iterations (e.g., 5 times) or the recalculation benefit being lower than the minimum improvement (e.g., the trajectory offset correction between two iterations is less than 0.05 meters). The trajectory offset correction is the L2 norm average of the position coordinate differences of all trajectory points within the local backtracking window in two adjacent iterations.

[0125] If the fusion score is 60 (below the second preset threshold of 75), the terminal will lock the abnormal time window, retrieve the previous solution state from the cache, and perform multiple rounds of filtering and recalculation by redistributing observation noise. If, in the third iteration, the trajectory correction amount no longer changes significantly (below the minimum improvement amount), the terminal will stop iterating and output the relatively optimal overall trajectory path that can be obtained at this time.

[0126] In another embodiment of the present invention, step S600, performing stability analysis on the residual error of the available trajectory, generating a reliability label based on the analysis result, and outputting the trajectory verification result with the reliability label, may include the following steps:

[0127] S610. Perform stability analysis on the residual error of the available trajectory.

[0128] In this step, residual errors may include residual angle errors, residual cumulative deviations, and short-term jitter. The processor analyzes these residual errors to determine whether the available trajectory meets the stable output conditions within the current time window.

[0129] S620. If the analysis results are stable, then attach a high-confidence reliability label to the available trajectory and output it.

[0130] In this step, the trajectory verification results with high confidence labels can be used as the effective output of the current time window and as a reference trajectory state for the next time window processing.

[0131] S630. If the analysis results are unstable, a low-confidence reliability label is attached to the available trajectory, and a control signal is output synchronously.

[0132] In this step, the low-confidence reliability tag is used to indicate that the availability of the current trajectory results is limited in subsequent control or monitoring stages; the synchronously output control signal can be used to trigger alarms, speed limits, or degraded operation (for example, when a low-confidence tag is output for 2 consecutive seconds, the system sends a speed limit of 5 to the mining truck chassis via the vehicle control bus interface). The control commands will continue until the trajectory state returns to a high confidence level.

[0133] S640, Use the trajectory verification results with reliability labels as reference information for the next time window.

[0134] In this step, the output of this round not only constitutes the result of the closed loop of this round, but can also serve as a historical reference for data acquisition, trajectory calculation and reliability judgment in the next time window.

[0135] For example, steps S610 to S640 may further include the following process: After obtaining the available trajectory, the processor calculates its residual angle error, residual cumulative deviation, and short-term jitter; if the residual error meets the stability condition, a trajectory verification result with a high-confidence reliability label is output; if the residual error does not meet the stability condition, a trajectory verification result with a low-confidence reliability label is output, and control signals required for alarm, speed limit, or degraded operation are sent to the control or monitoring link. This output result is simultaneously recorded as the reference trajectory and reliability status for the next time window processing.

[0136] The terminal performs end-point smoothness verification on the final output trajectory and finds that although the trajectory has been optimized, there are still dense short-term jitters in the yaw angle direction. Based on this, the system adds a "low confidence" tag to the data frame header and synchronously sends a degradation command to the mine truck chassis control bus. After receiving this signal, the mine truck continues to use coordinates for driving while automatically limiting its speed below a safe threshold until it receives "high confidence" trajectory data.

[0137] Example 3:

[0138] Based on the same inventive concept, and where this invention also relates to system implementation, the present invention can also provide the following system embodiments. For example... Figure 6As shown, a laser inertial navigation collaborative measurement system for GNSS-free underground tunnels can include a data acquisition module, a geometric constraint construction module, a hierarchical correction module, a reliability scoring module, a trajectory optimization module, and a stability verification output module. Each module can be implemented by a processor calling programs stored in memory, or it can be deployed within the same measurement and control terminal according to its function.

[0139] The data acquisition module is configured to acquire inertial sensor data and tunnel environment observation data of the vehicle to be tracked within the current time window, and perform inertial navigation calculations to obtain a preliminary trajectory estimate of the vehicle to be tracked. The preliminary trajectory estimate output by this module and the synchronized tunnel environment observation data are then transmitted to the geometric constraint construction module.

[0140] The geometric constraint construction module is configured to extract geometric features reflecting changes in roadway heading or slope based on roadway environmental observation data, construct geometric constraint observations, and perform filtering estimation based on preliminary trajectory estimation as the prior state and geometric constraint observations as the observation update quantity to obtain the error state adjustment amplitude. This module then passes the geometric constraint observations and error state adjustment amplitude to the hierarchical correction module.

[0141] The graded correction module is configured to select between executing a linear correction link or a nonlinear filtering link based on the comparison result between the error state adjustment amplitude and a first preset threshold. When executing the linear correction link, the module outputs the current correction trajectory and the first evaluation feature. When executing the nonlinear filtering link, the module performs an environmental interference assessment, calls a compensation mapping table or piecewise linear mapping relationship according to the interference level to generate a parameter compensation model, and outputs the attitude angle correction amount. The attitude angle correction amount is injected into the state transition process of the nonlinear filtering to form the corrected particle prediction state. The graded correction module then updates the particle weights according to the matching residual between the corrected particle prediction state and the roadway environmental observation data, and outputs the current correction trajectory and the convergence feature as the second evaluation feature.

[0142] The reliability scoring module is configured to generate a measurement reliability score by combining multi-segment trajectory continuity data formed by the current corrected trajectory and adjacent trajectory segments with a first evaluation feature or a second evaluation feature. This module then transmits the measurement reliability score to the trajectory optimization module.

[0143] The trajectory optimization module is configured to determine the current corrected trajectory as a usable trajectory based on a comparison between the measurement reliability score and a second preset threshold; or, when the measurement reliability score is lower than the second preset threshold, to establish a local backtracking window centered on the time window where the abnormal trajectory segment is located, and to perform backtracking recalculation on the abnormal trajectory segment within the local backtracking window based on the preliminary trajectory estimation, thereby obtaining the optimized overall trajectory path and determining it as a usable trajectory. This module outputs the usable trajectory to the stability verification output module.

[0144] The stability verification output module is configured to perform stability analysis on the residual errors of available tracks, generate reliability labels based on the analysis results, and output track verification results with reliability labels. When the analysis results are unstable, this module can also output control signals to trigger alarms, speed limits, or degraded operation.

[0145] For example, during system operation, the data acquisition module first outputs preliminary trajectory estimation and tunnel environment observation data to the geometric constraint construction module; the geometric constraint construction module further outputs geometric constraint observations and error state adjustment amplitudes; the hierarchical correction module outputs the current corrected trajectory and corresponding evaluation features based on the first preset threshold judgment result. Specifically, when entering the nonlinear filtering link, the hierarchical correction module first calls the compensation mapping table or piecewise linear mapping relationship to generate attitude angle correction based on the interference level, then injects the attitude angle correction into the particle state prediction process, and updates the particle weights based on the corrected particle prediction state; after the reliability scoring module generates a measurement reliability score, the trajectory optimization module determines whether to perform backtracking recalculation within the local backtracking window corresponding to the abnormal trajectory segment based on the second preset threshold; the stability verification output module finally outputs the trajectory verification result with a reliability label, and feeds back alarm, speed limit, or degraded operation signals to the control or monitoring link under low confidence conditions. Thus, a closed-loop data flow is formed within the system, from data acquisition, trajectory correction, scoring judgment, local backtracking optimization to output feedback.

[0146] It should be understood that, in various embodiments of the present invention, the order of the above-mentioned process numbers does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0147] Furthermore, the terms "system" and "network" are often used interchangeably in this paper. The term "and / or" in this paper merely describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, and B alone. Additionally, the character " / " in this paper generally indicates that the preceding and following related objects have an "or" relationship.

[0148] It should be understood that, in the embodiments of the present invention, "B corresponding to A" means that B is associated with A, and B can be determined based on A. However, it should also be understood that determining B based on A does not mean that B is determined solely based on A; B can also be determined based on A and / or other information.

[0149] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution.

[0150] In the embodiments provided in this application, the disclosed systems and methods can be implemented in other ways. For example, the module division described above is only a logical functional division, and there may be other division methods in actual implementation; multiple modules can be combined or integrated into the same processing unit, or some or all of the modules can be selected to achieve the purpose of the embodiments of the present invention according to actual needs.

[0151] In summary, the above description is merely a preferred embodiment of the technical solution of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for downhole GNSS-free tunnel laser inertial navigation cooperative measurement, characterized in that, include: Acquire inertial sensor data and tunnel environment observation data of the carrier to be tracked within the current time window, and perform inertial navigation calculations to obtain a preliminary trajectory estimate of the carrier to be tracked; Based on the tunnel environment observation data, geometric features reflecting changes in tunnel heading or slope are extracted to construct geometric constraint observations; based on the preliminary trajectory estimation as the prior state and the geometric constraint observations as the observation update quantity, a filtering estimation is performed to obtain the error state adjustment amplitude; Based on the comparison between the error state adjustment amplitude and the first preset threshold, the geometric constraint observation is used to perform linear correction on the preliminary trajectory estimation to obtain the current corrected trajectory and the first evaluation feature; Alternatively, an environmental disturbance assessment can be performed to generate a parameter compensation model and attitude angle correction, and the trajectory state can be updated through nonlinear filtering to obtain the current corrected trajectory and the convergence feature as the second evaluation feature. Based on the current corrected trajectory and the multi-segment trajectory continuity data formed by adjacent trajectory segments, the multi-segment trajectory continuity data is combined with the first evaluation feature or the second evaluation feature to perform fusion calculation and generate a measurement reliability score. Based on the comparison between the measurement reliability score and the second preset threshold, the current corrected trajectory is determined to be a usable trajectory; or based on the preliminary trajectory estimation, the abnormal trajectory segment is backtracked and recalculated to obtain the optimized overall trajectory path and determine it as a usable trajectory. The residual error of the available trajectory is subjected to stability analysis. Based on the analysis results, a reliability label is generated, and the trajectory verification results with the reliability label are output.

2. The method of claim 1, wherein, The steps for extracting geometric features reflecting changes in tunnel heading or slope from the tunnel environmental observation data and constructing geometrically constrained observations include: Within the current trajectory window, extract turning points and undulating sections from the tunnel environment observation data; Based on the extracted turning points and undulating segments, geometric constraints are formed that are related to changes in heading and / or slope, and the geometric constraint observations include the geometric constraints.

3. The method of claim 1, wherein, The comparison result between the error state adjustment amplitude and the first preset threshold includes: When the error state adjustment amplitude does not exceed the first preset threshold, the geometric constraint observation is used as an update quantity by Kalman filtering and applied to the preliminary trajectory estimation, and the linear correction is performed. When the error state adjustment amplitude exceeds the first preset threshold, the environmental interference assessment is triggered, and the nonlinear filtering is performed by combining particle filtering with the parameter compensation model and the attitude angle correction.

4. The method of claim 1, wherein, The execution environment interference assessment generates a parameter compensation model and attitude angle correction, including: Calculate at least one of the high-frequency vibration energy, attitude change, and geometric residual, and output the disturbance assessment characteristics; The interference level is determined based on the interference assessment features according to preset rules or historical mapping relationships. Based on the interference level, the corresponding compensation mapping table or piecewise linear mapping relationship is called to generate the parameter compensation model, and the attitude angle correction amount is output through the parameter compensation model.

5. The method of claim 1, wherein, The extraction method of the convergence feature, which serves as the second evaluation feature, includes: During the process of updating the trajectory state through nonlinear filtering, the convergence feature is extracted based on at least one of particle weight concentration, effective particle number, and cumulative deviation decreasing trend.

6. The method of claim 1, wherein, The step of combining the multi-segment trajectory continuity data with the first evaluation feature or the second evaluation feature to generate a measurement reliability score includes: A first score characterizing trajectory continuity is generated based on the multi-segment trajectory continuity data; A second score for table convergence is generated based on the first evaluation feature or the second evaluation feature; The first score and the second score are normalized and weighted according to preset dynamic weights to obtain the measurement reliability score.

7. The method of claim 1, wherein, The steps involved in performing backtracking recalculation on abnormal trajectory segments to obtain the optimized overall trajectory path during the initial trajectory estimation include: When the measurement reliability score is lower than the second preset threshold, the backtracking recalculation is performed on the abnormal trajectory segment, and the preliminary trajectory estimation, the error state adjustment amplitude and the nonlinear filtering posterior result within the corresponding time window are fused. When the recalculation process reaches the preset stopping condition, the backtracking ends and the optimized overall trajectory path is output. The stopping conditions include reaching the maximum number of iterations or the recalculated benefit being lower than the minimum improvement amount.

8. The method according to claim 1, characterized in that, The steps of performing stability analysis on the residual error of the available trajectory, generating reliability labels based on the analysis results, and outputting trajectory verification results with reliability labels include: Stability analysis was performed on the residual angle error, residual cumulative deviation, and short-term jitter of the available trajectory; If the analysis results are stable, a high-confidence reliability label is attached to the available trajectory and output; If the analysis results are unstable, a low-confidence reliability label is attached to the available trajectory, and a control signal for triggering alarms, speed limiting, or degraded operation is output synchronously.

9. The method according to claim 1, characterized in that, When acquiring the inertial sensor data and tunnel environment observation data of the carrier to be tracked within the current time window, the method further includes: If a short-term data gap is detected within the current time window, the trajectory state from the previous moment is used for short-term extrapolation to fill in the missing data, and the trajectory segment corresponding to the extrapolation is marked as a low-confidence interval.

10. A laser inertial navigation cooperative measurement system for GNSS-free underground tunnels, used to implement the method described in any one of claims 1 to 9, characterized in that, Includes the following modules: The data acquisition module is configured to acquire inertial sensor data and tunnel environment observation data of the carrier to be tracked within the current time window, and perform inertial navigation calculations to obtain a preliminary trajectory estimate of the carrier to be tracked. The geometric constraint construction module is configured to extract geometric features reflecting changes in roadway heading or slope based on the roadway environment observation data, construct geometric constraint observations, and perform filtering estimation based on the preliminary trajectory estimation as the prior state and the geometric constraint observations as the observation update amount to obtain the error state adjustment amplitude. The graded correction module is configured to perform linear correction on the preliminary trajectory estimation based on the comparison result of the error state adjustment amplitude and the first preset threshold, using the geometric constraint observation, to obtain the current corrected trajectory and the first evaluation feature; Alternatively, an environmental disturbance assessment can be performed to generate a parameter compensation model and attitude angle correction, and the trajectory state can be updated through nonlinear filtering to obtain the current corrected trajectory and the convergence feature as the second evaluation feature. The reliability scoring module is configured to generate a measurement reliability score by combining the multi-segment trajectory continuity data formed by the current corrected trajectory and adjacent trajectory segments with the first evaluation feature or the second evaluation feature. The trajectory optimization module is configured to determine the current corrected trajectory as a usable trajectory based on the comparison result between the measurement reliability score and the second preset threshold; or to perform backtracking recalculation on abnormal trajectory segments based on the preliminary trajectory estimation to obtain the optimized overall trajectory path and determine it as a usable trajectory. The stability verification output module is configured to perform stability analysis on the residual error of the available trajectory, generate reliability labels based on the analysis results, and output the trajectory verification results with reliability labels.

Images