An underground tunnel inspection robot UWB / IMU fusion positioning device

Abstract

The application relates to the field of underground tunnel inspection robot positioning, and discloses a UWB / IMU fusion positioning device for an underground tunnel inspection robot, which comprises a UWB positioning unit, an IMU inertial measurement unit, a synchronous preprocessing unit, an edge computing processing unit, a storage unit, a communication interface unit, a positioning output unit, a power stabilizing unit, a protection mounting unit and a running state diagnosis unit. When the edge computing processing unit executes a positioning program instruction, a time synchronization and data cleaning module, a coordinate unification and rod arm compensation module, a UWB quality evaluation and available measurement selection module, an LNN physical prior compensation module, a UKF forward fusion positioning module, a VBI dynamic robust measurement module, a mutual covariance tensor cache module, a DN-RTS reverse smoothing module, a residual calibration enhancement module, a safety gate control and trajectory reweaving module and a confidence evaluation and abnormal degradation module are formed, which are used for outputting a fusion positioning result carrying a position, a speed, a positioning confidence, a safety gate control sign and an abnormal degradation sign.

Description

Technical Field

[0001] This invention relates to the field of positioning technology for underground tunnel inspection robots, specifically a UWB / IMU fusion positioning device for underground tunnel inspection robots. Background Technology

[0002] Underground mine tunnels, underground utility tunnels, smart manufacturing workshops, and large-scale warehousing and logistics areas are typically characterized by narrow passages, dense metal structures, strong wall obstructions, complex environmental reflections, significant dynamic changes in work objects, and unsustainable GNSS signal reception. Inspection robots, unmanned inspection vehicles, transport AGVs, or intelligent AMRs in these environments need to continuously obtain position, speed, trajectory reliability, and device health status in order to perform autonomous inspection, path tracking, obstacle avoidance, work scheduling, and remote monitoring tasks.

[0003] UWB technology relies on fixed base stations deployed along the tunnel roof, sidewalls, or node areas to establish a local absolute positioning reference, while IMUs can provide high-frequency acceleration, angular velocity, and timestamp information. Therefore, the complementary integration of UWB and IMUs provides a feasible technical basis for achieving continuous positioning in GNSS-denied environments. However, metal supports, rail vehicles, work equipment, pipelines, cables, damp rock walls, personnel obstruction, and curved structures within the tunnel can easily cause non-line-of-sight propagation, diffraction, and multipath reflection of UWB signals, resulting in local offsets, sudden flypoints, and non-Gaussian heavy-tailed errors in UWB position, ranging, or time-of-arrival observations.

[0004] On the other hand, the IMU exhibits zero-biased random walk, temperature drift, vibration coupling, and installation deviations during long-term operation. The inspection robot displays significant high-dynamic nonlinear maneuvers when navigating sharp bends, emergency braking, traversing ramps, handling load changes, bypassing obstacles, and experiencing localized slippage. If the forward filtering relies solely on a simplified model with constant velocity or constant acceleration, trajectory phase lag, velocity discontinuities, accumulated position drift, and localized trajectory throw-outs may occur at bends.

[0005] In existing technologies, some solutions only use neural networks as independent trajectory regressors, lacking physical constraints and statistical consistency control; some solutions only run filtering algorithms in software, without forming a deployable device that adapts to robot hardware installation relationships, edge computing interfaces, data health detection, failure degradation, on-site calibration, and simulation physical parameter migration.

[0006] Therefore, a UWB / IMU fusion positioning device for underground tunnel inspection robots is needed. Summary of the Invention

[0007] The purpose of this invention is to provide a UWB / IMU fusion positioning device for an underground tunnel inspection robot to solve the problems in the prior art.

[0008] The objective of this invention can be achieved through the following technical solutions: A UWB / IMU fusion positioning device for underground tunnel inspection robots, the fusion positioning device includes a UWB positioning unit, an IMU inertial measurement unit, a synchronous preprocessing unit, an edge computing processing unit, a storage unit, a communication interface unit, a positioning output unit, a power supply voltage regulation unit, a protective installation unit, and an operation status diagnosis unit; The UWB positioning unit, IMU inertial measurement unit, synchronous preprocessing unit, storage unit, communication interface unit, positioning output unit, and operation status diagnosis unit are all communicatively or electrically connected to the edge computing processing unit. The storage unit stores positioning program instructions, UWB base station parameters, coordinate extrinsic parameters, noise parameters, network parameters, threshold parameters, and an independent residual sample library index. When the edge computing processing unit executes the positioning program instructions, it forms a time synchronization and data cleaning module, a coordinate unification and lever compensation module, a UWB quality evaluation and available measurement selection module, an LNN physical prior compensation module, a UKF forward fusion positioning module, a VBI dynamic robust measurement module, a cross-covariance tensor caching module, a DN-RTS inverse smoothing module, a residual calibration enhancement module, a safety gating and trajectory reweaving module, and a confidence assessment and anomaly degradation module, which are used to output the fusion positioning result carrying position, velocity, positioning confidence, safety gating flags, and anomaly degradation flags.

[0009] Furthermore, the UWB positioning unit includes a UWB tag, a UWB transceiver module, an external antenna, and a base station signal receiving interface, and is used to output at least one of UWB location observation data, ranging data, time of arrival data, angle of arrival data, received signal strength, first path power, signal-to-noise ratio, or link quality indicators; The IMU (Inertial Measurement Unit) is fixedly installed in the robot chassis, inertial mounting base, or protective mounting unit, and is used to output three-axis or six-axis acceleration, angular velocity, and timestamp data; The synchronization preprocessing unit establishes a high-frequency buffer queue with the IMU timestamp as the primary time base, and performs first-order interpolation on the UWB observations of two adjacent frames to obtain synchronized UWB observations:

[0010] In the formula, This is the current IMU sampling time. and These are the observation times of two adjacent UWB frames. and For corresponding UWB observations, the synchronous preprocessing unit is also used to perform repair on missing, reversed, or anomalous timestamps based on the median of the valid sampling period, and output a data validity flag.

[0011] Furthermore, the LNN physical prior compensation module inputs the previous state, current IMU acceleration, velocity information, local curvature features, time phase features, UWB mass features, and robot motion constraint features into a pre-trained Lagrange neural network to obtain the unmodeled acceleration residual:

[0012] The LNN physical prior compensation module forms an equivalent effective acceleration based on the small gain coefficient, residual confidence weight, and amplitude limiting threshold:

[0013] In the formula, Acceleration measured by IMU, estimated for Acceleration bias estimate, To meet small gain coefficient, As the confidence weight of the residuals, This is the residual limiting threshold; The UKF forward fusion localization module constructs a state vector containing position, velocity, and IMU acceleration bias:

[0014] The UKF forward fusion localization module constructs Sigma sampling points based on the posterior state and posterior covariance of the previous time step, and performs second-order kinematic propagation on each Sigma sampling point using equivalent effective acceleration to obtain the predicted state, predicted covariance, and predicted measurement.

[0015] Furthermore, the VBI dynamic robust measurement module calculates the Mahalanobis distance of the UWB measurement innovation and adjusts the measurement noise covariance when the Mahalanobis distance exceeds a preset anomaly threshold; the Mahalanobis distance and the scale factor satisfy:

[0016] The VBI dynamic robust measurement module adjusts the nominal measurement noise covariance according to the scaling factor:

[0017] In the formula, To measure the noise covariance of the nominal measurement, For dynamic scaling factor, This is the upper limit for noise expansion; when the measurement innovation does not trigger the abnormal threshold, the nominal measurement noise covariance is maintained.

[0018] Furthermore, the cross-covariance tensor caching module is used to store the forward prediction state, the forward posterior state, the prediction covariance, the posterior covariance, and the nonlinear cross-covariance of adjacent time steps; the DN-RTS inverse smoothing module calculates the smoothing gain based on the nonlinear cross-covariance.

[0019] And the inverse smoothed state is obtained using the smoothing gain:

[0020] The residual calibration enhancement module calls upon an independent residual sample library constructed from total station, laser SLAM, manually calibrated trajectories, or cross-validation residual libraries, and incorporates ridge regression residuals and features. Nearest neighbor residuals and time phase The nearest-neighbor residuals are combined according to confidence weights to obtain the residual correction:

[0021] in, , and For normalized confidence weights; when the running scenario does not have an independent residual calibration library, the residual library coverage is lower than the preset threshold, or the location confidence is lower than the preset threshold, the residual calibration enhancement module is turned off or the residual correction weight is reduced, and an uncalibrated or pre-calibrated status flag is output.

[0022] Furthermore, the safety gating and trajectory reweaving module performs a physical feasibility check on candidate trajectories based on maximum speed, maximum acceleration, maximum jump, minimum turning radius, ramp access restrictions, and roadway safety corridors. When a candidate trajectory point exceeds the safety threshold, it is pulled back to the vicinity of the safety anchor point according to the following formula:

[0023] The security gating and trajectory reweaving module is also used to perform three operations on the trajectory gaps after removing outliers. The spline was rewoven, and rigid lever offsets between the UWB antenna, IMU mount, and robot body coordinate system were compensated using SVD-Kabsch rigid body alignment.

[0024] Furthermore, the confidence assessment and anomaly degradation module calculates the location confidence based on the number of effective base stations, measurement quality indicators, IMU data validity, prediction covariance trace, communication link status, and security gating results. When the location confidence is less than a preset confidence threshold, the device enters at least one of the following modes: inertial prediction-dominated mode, nearest reliable trajectory holding mode, speed limit suggestion mode, or safe parking suggestion mode.

[0025] An underground tunnel inspection robot is provided, which uses a UWB / IMU fusion positioning device for underground tunnel inspection robots. The underground tunnel inspection robot includes a mobile carrier body and a chassis controller. The fusion positioning device is installed in the main control cabin, top equipment cabin, chassis rigid mounting base or independent protective computing box of the mobile carrier body, and outputs the fusion positioning trajectory, speed, positioning reliability, safety gate control flag and abnormal degradation flag to the chassis controller through a communication interface unit. Multiple UWB base stations are deployed along the top slab, sidewalls, intersections, bends, slopes, or working areas of underground tunnels. These multiple UWB base stations, together with the fusion positioning device, form a local absolute positioning reference within the underground tunnels.

[0026] A fusion positioning method employs a UWB / IMU fusion positioning device for an underground tunnel inspection robot, which sequentially performs UWB / IMU data acquisition and synchronization, data validity check, coordinate unification, available measurement selection, LNN physical prior compensation, UKF forward fusion positioning, VBI dynamic robust measurement update, cross-covariance tensor caching, DN-RTS inverse smoothing, independent residual calibration enhancement, safety gating and trajectory reweaving, location reliability calculation, and anomaly degradation output, and outputs the final absolute positioning trajectory.

[0027] A computer-readable storage medium storing program instructions for implementing the aforementioned fusion positioning method, wherein when executed by a processor, the program instructions enable the processor to perform time synchronization, coordinate unification, available measurement selection, LNN physical prior compensation, UKF forward fusion positioning, VBI dynamic robust measurement update, DN-RTS inverse smoothing, residual calibration enhancement, safety gating and trajectory reweaving, location reliability calculation, and anomaly degradation output functions in the fusion positioning device.

[0028] The beneficial effects of this invention are: 1. This invention integrates UWB, IMU, edge computing, storage, communication, power supply, protection, diagnostics, and positioning outputs into an installable device structure, supporting on-site deployment of underground tunnel inspection robots, industrial mobile platforms, and warehouse logistics robots; 2. This invention uses LNN as a restricted physical prior residual compensation layer, rather than directly replacing the filter, to improve the adaptability of state propagation under conditions such as sharp bends, braking, slopes, load changes, and local slippage. 3. This invention uses the VBI dynamic robust measurement mechanism to perform flexible weighting on heavy-tailed anomaly UWB measurements, reducing over-updates caused by NLOS and multipath contamination; 4. This invention uses future information to correct the current state by using covariance tensor caching and DN-RTS inverse smoothing, thereby reducing phase lag and local jitter caused by forward causal filtering; 5. This invention improves trajectory continuity, controllability, interpretability, and engineering applicability through an independent residual calibration library, physical security gating, outlier handling, B-spline trajectory reweaving, and SVD-Kabsch alignment. 6. This invention generates confidence and degradation flags by combining the number of effective base stations, measurement quality, prediction covariance, communication status and security gating results, so that the chassis controller can execute speed limiting, holding or safe stopping strategies according to the positioning reliability. 7. This invention supports the migration of network parameters, noise parameters, threshold parameters, and scenario priors through simulated physical collaborative deployment and feedback update mechanisms, thereby reducing the cost of repeated downhole debugging; 8. This invention can switch between uncalibrated operation mode and fixed route calibration enhancement mode, avoiding the direct use of the true value to be evaluated for online compensation, and enhancing the reliability of the final positioning result. Attached Figure Description

[0029] The invention will now be further described with reference to the accompanying drawings.

[0030] Figure 1 This is a basic flowchart of the fusion positioning algorithm of the present invention; Figure 2 This is a schematic diagram of the positioning device of the present invention installed on an underground tunnel inspection robot; Figure 3 This is a flowchart of the UKF-LNN fusion localization process of the present invention; Figure 4 This is a flowchart of the VBI dynamic robust measurement, cross-covariance tensor caching, DN-RTS inverse smoothing and physical security gated trajectory pullback of the present invention; Figure 5 This is a flowchart of the simulated physical collaborative deployment process of this invention; Figure 6 This is a schematic diagram of the hardware composition and signal connection relationship of the fusion positioning device of the present invention; Figure 7 This is a schematic diagram of IMU multi-source data time synchronization and coordinate unification according to the present invention; Figure 8 This is a simulation diagram of the positioning trajectory of the present invention; Figure 9 This is a simulation diagram of the positioning trajectory of the present invention. Detailed Implementation

[0031] The technical solutions of 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. Example

[0032] A UWB / IMU fusion positioning device for underground tunnel inspection robots, based on Figures 1 to 9 As shown, the UWB / IMU fusion positioning device can be installed in the main control cabin, top equipment cabin, chassis rigid mounting base, or independent protective computing box of the underground tunnel inspection robot.

[0033] The UWB / IMU fusion positioning device includes a UWB positioning unit, an IMU inertial measurement unit, a synchronization preprocessing unit, an edge computing processing unit, a storage unit, a communication interface unit, a positioning output unit, a power supply stabilization unit, a protective installation unit, and an operational status diagnostic unit. When the edge computing processing unit executes program instructions in the storage unit, it forms the following modules: time synchronization and data cleaning module, coordinate unification and lever compensation module, UWB quality evaluation and available measurement selection module, LNN physical prior compensation module, UKF forward fusion positioning module, VBI dynamic robust measurement module, cross-covariance tensor caching module, DN-RTS inverse smoothing module, residual calibration enhancement module, safety gating and trajectory reweaving module, and confidence assessment and anomaly degradation module.

[0034] The protective mounting unit includes a housing, vibration damping pads, mounting brackets, fasteners, shielding grounding terminal, power supply voltage regulation circuit, and heat dissipation components.

[0035] The casing preferably adopts a dustproof, moisture-proof, impact-resistant, and electromagnetic shielding structure; The device is powered by the robot's battery system, which is then converted into a stable operating voltage by a power supply regulator.

[0036] The UWB positioning unit includes a UWB tag, a UWB transceiver module, and an external antenna. The UWB positioning unit is installed on an underground tunnel inspection robot.

[0037] Multiple UWB base stations are installed in the underground tunnel, and these base stations are deployed in sections along the roof, side walls, bends, branches, slopes and working surfaces of the underground tunnel.

[0038] The IMU (Inertial Measurement Unit) is rigidly mounted on the robot chassis or inertial navigation mount and establishes extrinsic parameters, lever arm parameters, and installation deviation parameters with the UWB tag.

[0039] Edge computing processing units can employ ARM processors, industrial computing boards, DSPs, FPGAs, GPU edge computing modules, or combinations thereof.

[0040] The edge computing processing unit outputs position, speed, confidence level, and safety gating flags to the chassis controller via the communication interface unit; The communication interface unit can be one or more of a serial communication interface or an industrial Ethernet interface.

[0041] The host computer or remote monitoring terminal obtains operation logs, abnormal alarms, trajectory playback and parameter configuration results through wired or wireless interfaces.

[0042] In this embodiment, the operation method for deploying and starting self-testing of UWB base stations is as follows: Before equipment deployment, the roadway is divided into straight sections, curved sections, branch sections, slope sections, and working face areas.

[0043] In this section, straight sections are laid out at intervals, while curves, forks, areas with dense metal support, and work surfaces are laid out with increased density.

[0044] After the device is powered on, it sequentially completes the power supply self-test, storage space self-test, communication interface self-test, UWB tag online status check, IMU timestamp check, external parameter file verification, and parameter version verification.

[0045] In this embodiment, the IMU timestamp is used as the primary time base for synchronizing UWB observations.

[0046] Let the current IMU sampling time be t_k, and the UWB observation times of two adjacent frames be t_i and t_i+1, respectively. Then the synchronous UWB observation value can be obtained by the following formula:

[0047] In the formula, These are the synchronized UWB observations.

[0048] For data with missing timestamps, reversed order, or anomalous jumps, the synchronous preprocessing unit calculates the median of historical valid sampling intervals and repairs the time series; for data with missing values... Data or The data can perform at least one of the following operations: recent confidence value preservation, linear imputation, measurement-free prediction, or fault log recording.

[0049] The physics prior compensation module does not directly output the robot's position. Instead, it uses state, inertial input, velocity, curvature, time phase, measurement mass, and motion constraints as features to predict the unmodeled acceleration residuals.

[0050] The unmodeled acceleration residuals, after being processed with small gain, confidence weights, and component limiting, are compared with the original... Acceleration together form an equivalent effective acceleration:

[0051] The UKF forward fusion localization module constructs a state vector containing two-dimensional position, two-dimensional velocity, and two-dimensional IMU acceleration bias:

[0052] The VBI dynamic robust measurement module identifies multipath abrupt changes based on the Mahalanobis distance of measurement innovations and adjusts the measurement noise covariance when anomalous innovations occur.

[0053]

[0054] The DN-RTS inverse smoothing module reuses the nonlinear cross-covariance stored during the forward UKF filtering process and obtains the smoothing gain and inverse smoothing state according to the following formula:

[0055]

[0056] The residual calibration enhancement module calls independent calibration trajectories or cross-validation residual libraries to compensate for systematic deviations in fixed scenarios:

[0057] The safety gating and trajectory reweaving module performs a physical feasibility check on candidate trajectories based on lane boundaries, obstacle boundaries, robot shape envelope, velocity, acceleration, jump, turning radius, and ramp constraints. When a candidate trajectory point exceeds the safety threshold, the following formula is used to pull the candidate trajectory point back to the vicinity of the safety anchor point: Let the first The availability of each base station at the current moment is indicated by: As shown in the following formula:

[0058] in, For link quality metrics; For the current distance measurement or equivalent position observation, estimate For predicting distance measurement; Due to data delay; This is an indicator function.

[0059] The number of effective base stations is calculated as follows:

[0060] When the number of effective base stations is less than the preset number, the operation status diagnosis unit outputs a base station shortage flag and increases the measurement noise covariance or switches to the inertial prediction-dominated mode in subsequent measurement updates.

[0061] In this embodiment, clock correction, time synchronization and data cleaning: the clocks of UWB and IMU may have fixed bias and frequency drift; The synchronization preprocessing unit can perform affine correction on the original timestamp t as follows:

[0062] in, For clock ratio correction parameters; This is the clock bias parameter.

[0063] For the historical valid sampling interval, the median sampling period is defined as follows:

[0064] The synchronization preprocessing unit is synchronized according to equation (1); for missing, reversed or abrupt data, the time series is repaired according to equation (14); for invalid UWB or IMU values, the most recent reliable value is preserved, linear imputation is performed or a no-measurement flag is set.

[0065] In this embodiment, coordinate unification, lever compensation, and motion state definition are as follows: the UWB tag coordinate system is defined as U, the IMU coordinate system as I, the robot body coordinate system as B, and the alleyway global coordinate system as G.

[0066] The UWB tag position conversion relationship is as follows:

[0067] in, This is the rotation matrix from the body coordinate system to the global coordinate system; It is a translation vector; This is the lever vector of the UWB label relative to the IMU.

[0068] If UWB outputs the first The distance measurement between a base station and a tag is calculated using the following model:

[0069] In the formula, For the first Location of each UWB base station This is a non-line-of-sight offset term. This is random measurement noise.

[0070] In this embodiment, UWB quality evaluation, NLOS identification, and available measurement selection: The UWB quality evaluation and available measurement selection module calculates the quality score of each measurement by comprehensively considering the received signal strength, first-path power, innovation residual, base station geometric distribution, and number of effective base stations.

[0071] No. The quality score of each base station is as follows:

[0072] in, The received signal strength; The power index is the first diameter. To innovate statistical measures; Geometric precision factor; This is the Sigmoid function.

[0073] The set of available base stations is defined as follows:

[0074] When the geometric accuracy factor exceeds a threshold or the number of available base stations is insufficient, the device removes base stations with lower geometric contributions, switches to alternative base station combinations, or increases the measurement noise covariance. The aforementioned quality score can also serve as a priori input to the VBI dynamic robustness module.

[0075] In this embodiment, LNN physical prior compensation: combined with Figure 3 As shown, the LNN module does not directly output the robot position, but predicts the unmodeled acceleration residual based on the previous state, IMU raw acceleration, velocity, local trajectory curvature, time phase, UWB quality features, and robot motion constraint features.

[0076] To improve the stability of the input values, the feature vectors are normalized as follows:

[0077] The residual confidence weights can be defined based on the network output amplitude and uncertainty estimates as follows:

[0078] The offline training loss of LNN consists of data fitting loss, physical consistency loss, smoothing loss, and parameter regularization term:

[0079] The supervision labels are obtained from independently calibrated trajectories, cross-validation data, or high-precision reference equipment, and the true values ​​of the trajectories to be evaluated are not used as online labels.

[0080] In this embodiment, UKF forward fusion localization is implemented as follows: the state dimension is n, and the posterior state mean is the estimated state. The posterior covariance is .

[0081] First, the 0th Sigma point is constructed; the remaining Sigma points are generated in pairs with positive and negative biases based on the square root matrix of the posterior covariance and the unscented transformation scaling parameter, as shown in the following equation:

[0082] The Sigma point propagates through a state function, as shown in the following equation:

[0083] The predicted state mean and predicted covariance are as follows:

[0084]

[0085] in, and These are the mean weights and covariance weights of the unscented transformation, respectively. Let be the process noise covariance.

[0086] In this embodiment, VBI dynamic robust measurement update involves mapping the predicted Sigma points to the UWB observation space to obtain the predicted measurement points, and constructing the measurement prediction covariance.

[0087] This yields the Kalman gain; the posterior state is updated by adding the product of the Kalman gain and the measurement innovation to the predicted state.

[0088] To improve the numerical stability of covariance, the posterior covariance is updated using the Joseph form:

[0089] When the measurement innovation Mahalanobis distance is greater than the anomaly threshold, the VBI dynamic robust measurement module performs a gentle expansion of the measurement noise covariance according to equations (5) and (6), and controls the scale factor to change within the preset upper and lower limits, so as to avoid excessive rejection of effective measurements when the original UWB observations are relatively clean.

[0090] In this embodiment, the cross-covariance tensor buffer and DN-RTS inverse smoothing are used: during the forward UKF filtering process, the predicted state, posterior state, predicted covariance, posterior covariance, and nonlinear cross-covariance between adjacent time steps are buffered. The cross-covariance between adjacent time steps can be obtained by the following formula:

[0091] The DN-RTS reverse smoothing module calculates the smoothing gain according to equation (7) and corrects the state from back to front according to equation (8); The smoothed covariance is as follows:

[0092] Reverse smoothing can be performed when the sliding window ends, the task segment ends, or the device enters the background optimization state, to output a window-level smooth trajectory; the real-time control interface can still output the UKF forward fusion result.

[0093] In this embodiment, residual calibration enhancement, physical security gating, and trajectory reweaving are performed: The residual calibration enhancement module uses UWB micro-smoothed trajectory, DN-RTS smoothed trajectory, forward filtered trajectory, velocity, acceleration, time phase, and local curvature as input features, and uses the difference between the independent calibration trajectory and the pre-calibrated trajectory as the residual label to construct a ridge regression residual model, a feature K-nearest neighbor residual model, and a time phase K-nearest neighbor residual model. The residual correction amount is generated according to equation (9).

[0094] For physical security gating, the second-order difference judder index of the candidate trajectory is defined as:

[0095] When the jump exceeds the preset threshold or the distance between the candidate trajectory point and the roadway boundary, obstacle boundary or safety corridor boundary is less than the preset safety distance, the corresponding trajectory point is determined to be a non-physical mutation point or a boundary crossing point, and abnormal point elimination, weight reduction or trajectory pullback according to formula (10) is performed.

[0096] After removing the abnormal points, the trajectory gaps are rewoven using cubic B-splines:

[0097] To eliminate rigid lever offset between the UWB antenna, IMU mount, and robot body coordinate system, a covariance matrix of a decentralized point set is constructed:

[0098] Performing singular value decomposition on the covariance matrix, the optimal rotation matrix and translation vector are:

[0099] In this embodiment, location-based confidence, anomaly degradation, and simulated physical objects are deployed collaboratively: The confidence assessment and anomaly degradation module calculates location confidence based on the number of effective base stations, average measurement quality, posterior covariance trace, communication anomaly indicators, and security gating results, using the following formula:

[0100] Among them, average To average the measured mass, This is an indicator of communication anomalies. For security gating results.

[0101] If the location confidence level is lower than a preset threshold, the device enters at least one of the following modes: inertial prediction-dominated mode, nearest reliable trajectory hold mode, speed limit suggestion mode, or safe parking suggestion mode.

[0102] During the collaborative deployment phase of the simulation and physical equipment, standard reference trajectories and UWB / IMU simulation data can be generated based on the digital scene of the underground tunnel to obtain network parameters, filtering parameters, measurement noise parameters and threshold parameters, which can then be used as the initial parameters of the physical equipment.

[0103] To address UWB transmission delay, the device can use motion compensation to map the delayed observations to the current time, as shown in the following formula:

[0104] During operation, the UKF forward fusion localization module can adaptively adjust the process noise covariance based on the LNN residual confidence weights, IMU data validity, and localization reliability.

[0105] The residual calibration enhancement module can calculate the residual library coverage based on the nearest neighbor distance between the current feature vector and the residual library samples, the base station geometric similarity, and the temporal phase similarity.

[0106] When the coverage of the residual library is lower than the preset threshold, the residual calibration enhancement module outputs a low coverage flag and reduces the residual correction amount, thereby avoiding the excessive application of the residual model in the uncovered environment to online localization.

[0107] In operation, the fusion positioning device is installed in the main control compartment of the underground tunnel inspection robot, the UWB tag is installed on the antenna mount on the top of the vehicle, and the IMU is installed on a rigid mounting base near the center of gravity of the vehicle. UWB base stations are deployed along the tunnel roof or sidewalls, with increased density in curves, forks, ramps, and areas with dense metal supports. The inspection robot operates continuously in straight sections, curves, ramps, and areas obstructed by equipment. The edge computing processing unit outputs the UKF forward fusion trajectory according to the control cycle and outputs the DN-RTS smooth trajectory within a set sliding window.

[0108] When the robot enters areas with dense metal supports, equipment obstructions, or personnel obstructions, UWB observations may exhibit non-line-of-sight offsets and transient flypoints. In such cases, the VBI dynamic robust measurement module calculates the measurement innovation Mahalanobis distance; if it exceeds a preset threshold, it triggers dynamic measurement noise updates to reduce the impact of abnormal UWB measurements on the posterior state.

[0109] When the robot turns, brakes, crosses a ramp, or performs obstacle avoidance maneuvers, the LNN predicts the unmodeled acceleration residuals based on the previous state, velocity information, and current IMU input. After small gain, limiting, and confidence decay, the residuals are used in the UKF time update to compensate for the deficiencies of constant velocity or constant acceleration models.

[0110] After completing a patrol task or sliding window positioning, the device performs DN-RTS inverse smoothing using the nonlinear cross-covariance tensor stored by forward filtering to reduce the phase lag caused by forward causal filtering. Subsequently, through independent residual calibration enhancement, physical safety gating, outlier removal, B-spline trajectory reweaving, and SVD-Kabsch coordinate alignment, it outputs a continuous and smooth absolute positioning trajectory that meets the robot chassis control requirements.

[0111] It should be noted that the residual calibration enhancement results are obtained based on pre-established independent calibration trajectories or cross-validated residual libraries. In the absence of complete field calibration, the device outputs LNN-UKF-DNRTS pre-calibration results; after residual library calibration is completed along a fixed roadway inspection route, the residual library can be further invoked to obtain a more accurate final positioning result.

[0112] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the claimed invention.

Claims

1. A UWB / IMU fusion positioning device for underground tunnel inspection robots, characterized in that, The fusion positioning device includes a UWB positioning unit, an IMU inertial measurement unit, a synchronous preprocessing unit, an edge computing processing unit, a storage unit, a communication interface unit, a positioning output unit, a power supply stabilization unit, a protective installation unit, and an operation status diagnostic unit; The UWB positioning unit, IMU inertial measurement unit, synchronous preprocessing unit, storage unit, communication interface unit, positioning output unit, and operation status diagnosis unit are all communicatively or electrically connected to the edge computing processing unit. The storage unit stores positioning program instructions, UWB base station parameters, coordinate extrinsic parameters, noise parameters, network parameters, threshold parameters, and an independent residual sample library index. When the edge computing processing unit executes the positioning program instructions, it forms a time synchronization and data cleaning module, a coordinate unification and lever compensation module, a UWB quality evaluation and available measurement selection module, an LNN physical prior compensation module, a UKF forward fusion positioning module, a VBI dynamic robust measurement module, a cross-covariance tensor caching module, a DN-RTS inverse smoothing module, a residual calibration enhancement module, a safety gating and trajectory reweaving module, and a confidence assessment and anomaly degradation module, which are used to output the fusion positioning result carrying position, velocity, positioning confidence, safety gating flags, and anomaly degradation flags.

2. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The UWB positioning unit includes a UWB tag, a UWB transceiver module, an external antenna, and a base station signal receiving interface, and is used to output at least one of UWB location observation data, ranging data, time of arrival data, angle of arrival data, received signal strength, first path power, signal-to-noise ratio, or link quality indicators. The IMU (Inertial Measurement Unit) is fixedly installed in the robot chassis, inertial mounting base, or protective mounting unit, and is used to output three-axis or six-axis acceleration, angular velocity, and timestamp data; The synchronization preprocessing unit establishes a high-frequency buffer queue with the IMU timestamp as the primary time base, and performs first-order interpolation on the UWB observations of two adjacent frames to obtain synchronized UWB observations: In the formula, This is the current IMU sampling time. and These are the observation times of two adjacent UWB frames. and For corresponding UWB observations, the synchronous preprocessing unit is also used to perform repair on missing, reversed, or anomalous timestamps based on the median of the valid sampling period, and output a data validity flag.

3. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The LNN physical prior compensation module inputs the previous time-step state, current IMU acceleration, velocity information, local curvature features, time phase features, UWB mass features, and robot motion constraint features into a pre-trained Lagrange neural network to obtain the unmodeled acceleration residual: The LNN physical prior compensation module forms an equivalent effective acceleration based on the small gain coefficient, residual confidence weight, and amplitude limiting threshold: In the formula, Acceleration measured by IMU, estimated for Acceleration bias estimate, To meet Small gain coefficient, As the confidence weight of the residuals, The residual limiting threshold is used; the UKF forward fusion localization module constructs a state vector containing position, velocity, and IMU acceleration bias: The UKF forward fusion localization module constructs Sigma sampling points based on the posterior state and posterior covariance of the previous time step, and performs second-order kinematic propagation on each Sigma sampling point using equivalent effective acceleration to obtain the predicted state, predicted covariance, and predicted measurement.

4. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The VBI dynamic robust measurement module calculates the Mahalanobis distance of the UWB measurement innovation and adjusts the measurement noise covariance when the Mahalanobis distance exceeds a preset anomaly threshold; the Mahalanobis distance and scale factor satisfy: The VBI dynamic robust measurement module adjusts the nominal measurement noise covariance according to the scaling factor: In the formula, To measure the noise covariance of the nominal measurement, For dynamic scaling factor, This represents the upper limit of noise expansion. When the measurement information does not trigger the abnormal threshold, the nominal measurement noise covariance is maintained.

5. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The cross-covariance tensor caching module is used to store the forward prediction state, the forward posterior state, the prediction covariance, the posterior covariance, and the nonlinear cross-covariance of adjacent time steps; the DN-RTS inverse smoothing module calculates the smoothing gain based on the nonlinear cross-covariance. And the inverse smoothed state is obtained using the smoothing gain: The residual calibration enhancement module calls upon an independent residual sample library constructed from total station, laser SLAM, manually calibrated trajectories, or cross-validation residual libraries, and incorporates ridge regression residuals and features. Nearest neighbor residuals and time phase The nearest-neighbor residuals are combined according to confidence weights to obtain the residual correction: in, , and For normalized confidence weights; when the running scenario does not have an independent residual calibration library, the residual library coverage is lower than the preset threshold, or the location confidence is lower than the preset threshold, the residual calibration enhancement module is turned off or the residual correction weight is reduced, and an uncalibrated or pre-calibrated status flag is output.

6. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The safety gating and trajectory reweaving module performs a physical feasibility check on candidate trajectories based on maximum speed, maximum acceleration, maximum jump, minimum turning radius, ramp access restrictions, and roadway safety corridors. When a candidate trajectory point exceeds the safety threshold, it is pulled back to the vicinity of the safety anchor point according to the following formula: The security gating and trajectory reweaving module is also used to perform three operations on the trajectory gaps after removing outliers. spline reweaving and compensation for UWB antenna and IMU via SVD-Kabsch rigid body alignment. Rigid lever offset between the mounting base and the robot body coordinate system.

7. The UWB / IMU fusion positioning device for an underground tunnel inspection robot according to claim 1, characterized in that, The confidence assessment and anomaly degradation module calculates the location confidence based on the number of effective base stations, measurement quality indicators, IMU data validity, prediction covariance trace, communication link status, and security gating results. When the location confidence is less than a preset confidence threshold, the device enters at least one of the following modes: inertial prediction-dominated mode, nearest reliable trajectory holding mode, speed limit suggestion mode, or safe parking suggestion mode.

8. An underground tunnel inspection robot, employing a UWB / IMU fusion positioning device for underground tunnel inspection robots as described in any one of claims 1-7, the underground tunnel inspection robot comprising a mobile carrier body and a chassis controller, characterized in that, The fusion positioning device is installed in the main control cabin, top equipment cabin, chassis rigid mounting base or independent protective computing box of the mobile carrier body, and outputs the fusion positioning trajectory, speed, positioning reliability, safety gate control flag and abnormal degradation flag to the chassis controller through the communication interface unit; Multiple UWB base stations are deployed along the top slab, sidewalls, intersections, bends, slopes, or working areas of underground tunnels. These multiple UWB base stations, together with the fusion positioning device, form a local absolute positioning reference within the underground tunnels.

9. A fusion positioning method, employing the methods described in claim 1 -7. A UWB / IMU fusion positioning device for an underground tunnel inspection robot, characterized in that... The following steps are executed sequentially: UWB / IMU data acquisition and synchronization, data validity check, coordinate unification, available measurement selection, LNN physical prior compensation, and UKF. Forward fusion localization, VBI dynamic robust measurement update, cross-covariance tensor caching, DN-RTS inverse smoothing, independent residual calibration enhancement, safety gating and trajectory reweaving, localization reliability calculation and anomaly degradation output, and output the final absolute localization trajectory.

10. A computer-readable storage medium storing program instructions thereon for implementing the fusion positioning method as described in claim 9, characterized in that, When the program instructions are executed by the processor, the processor integrates time synchronization, coordinate unification, available measurement selection, LNN physical prior compensation, and UKF from the positioning device. Features include forward fusion localization, VBI dynamic robust measurement update, DN-RTS inverse smoothing, residual calibration enhancement, safety gating and trajectory reweaving, location reliability calculation, and anomaly degradation output.

Images