Underground fusion positioning system based on UWB and inertial navigation

Abstract

The application discloses a kind of based on UWB and inertial navigation's downhole fusion positioning system, belong to computer technology field, by real-time difference calculation inertial data to detect pole arm change, and based on multidimensional confidence evaluation dynamic adjustment factor graph weight is carried out joint optimization, solved the positioning precision and reliability problem caused by pole arm dynamic change between UWB label and target to be positioned in downhole environment.It can overcome the limitations of fixed compensation or segmented constant assumption in the related art, realize real-time detection and adaptive compensation of pole arm dynamic change, so as to continuously output positioning results with high precision and high reliability in downhole dynamic environment, improve the robustness and accuracy of downhole personnel and equipment positioning.

Description

Technical Field

[0001] This application relates to the field of computer technology, and in particular to a downhole fusion positioning system based on UWB and inertial navigation. Background Technology

[0002] In underground environments such as coal mines and metal mines where GPS signals are blocked, the fusion of Ultra Wide Band (UWB) and Inertial Navigation System (INS) positioning is the mainstream technology for ensuring high-precision positioning of personnel and equipment. UWB has high ranging accuracy but is easily affected by non-line-of-sight environments, while INS has strong autonomy but its errors accumulate over time. The fusion of the two can complement each other's advantages.

[0003] In related technologies, there are significant shortcomings in handling the lever arm effect between UWB tags and the target to be located. Some solutions use fixed compensation algorithms, which compensate based on known fixed lever arm values, but cannot cope with real-time changes in the lever arm caused by personnel posture changes, equipment vibration, or loose installation in dynamic downhole environments. Some solutions use dual inertial navigation configurations and assume piecewise constant values ​​for the lever arm effect, but this assumption relies on rotation modulation devices and cannot adapt to scenarios with rapid attitude changes downhole. Other solutions separate confidence assessment from lever arm estimation, lacking a dynamic perception and feedback adjustment mechanism for the reliability of multi-source information.

[0004] It is evident that the relevant technologies lack the ability to detect and adaptively compensate for dynamic changes in the boom in real time, making it difficult to continuously output high-precision and high-reliability positioning results in complex dynamic environments downhole. Summary of the Invention

[0005] This application provides a downhole fusion positioning system based on UWB and inertial navigation, the technical solution of which is as follows: On the one hand, a downhole fusion positioning system based on UWB and inertial navigation is provided, the system including a processor and a memory, the processor being configured to execute: In the downhole environment, differential calculation is performed on the first inertial data collected by the first IMU and the second inertial data collected by the second IMU to obtain differential acceleration characteristics and lever arm change detection results. The first IMU is fixed at the UWB tag and the second IMU is fixed at the target to be located. Based on the boom change detection results, UWB ranging data, and historical trajectory smoothing constraints, a multi-dimensional confidence assessment is performed by fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to obtain a comprehensive confidence and fusion state diagnosis result. Based on the weights of the UWB factor and the inertial factor in the comprehensive confidence dynamic adjustment factor diagram, and by jointly optimizing the UWB ranging data, the first inertial data, and the second inertial data, the estimated value of the time-varying lever arm vector between the target position information of the target to be located and the first IMU and the second IMU is obtained. The objective function of the joint optimization solution includes a regularization term constructed based on the historical trajectory smoothing constraint to suppress abnormal jumps in the estimated value of the time-varying lever arm vector. The attitude change information of the target to be located downhole is determined based on the target location information and the estimated value of the time-varying lever vector. Attached Figure Description

[0006] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0007] Figure 1 This is a flowchart of a downhole fusion positioning method based on UWB and inertial navigation provided in an embodiment of this application; Figure 2 This is a flowchart of another downhole fusion positioning method based on UWB and inertial navigation provided in the embodiments of this application; Figure 3 This is a flowchart of another downhole fusion positioning method based on UWB and inertial navigation provided in the embodiments of this application; Figure 4 This is a flowchart of another downhole fusion positioning method based on UWB and inertial navigation provided in the embodiments of this application; Figure 5 This is a flowchart of another downhole fusion positioning method based on UWB and inertial navigation provided in the embodiments of this application. Detailed Implementation

[0008] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0009] In this application, the terms "first," "second," etc., are used to distinguish identical or similar items with essentially the same function. It should be understood that there is no logical or temporal dependency between "first," "second," and "nth," nor are there any restrictions on quantity or execution order.

[0010] It should be noted that the information (including but not limited to user device information, user personal information, etc.), data (including but not limited to data used for analysis, data stored, data displayed, etc.) and signals involved in this application are all authorized by the user or fully authorized by all parties, and the collection, use and processing of related data must comply with the relevant laws, regulations and standards of the relevant countries and regions.

[0011] The existing downhole UWB and inertial navigation fusion positioning technology has limitations in handling the lever arm effect between UWB tags and the target to be located. Some solutions employ fixed compensation or piecewise constant assumptions, which cannot cope with real-time lever arm changes caused by personnel posture changes, equipment vibration, or loose installation in dynamic downhole environments. Furthermore, the technology lacks a dynamic perception and feedback adjustment mechanism for the reliability of multi-source information, resulting in insufficient real-time detection and adaptive compensation capabilities for dynamic lever arm changes, making it difficult to consistently output high-accuracy and high-reliability positioning results in dynamic downhole environments.

[0012] To address this, this application proposes a downhole fusion positioning system based on UWB and inertial navigation. The system includes a processor and a memory. The processor is configured to perform differential calculations on first inertial data acquired by a first IMU and second inertial data acquired by a second IMU in the downhole environment to obtain differential acceleration characteristics and lever arm change detection results. Based on the lever arm change detection results, UWB ranging data, and historical trajectory smoothing constraints, a multi-dimensional confidence assessment is performed by fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to obtain a comprehensive confidence score and fusion state diagnostic result. The weights of the UWB factor and inertial factor in the factor graph are dynamically adjusted based on the comprehensive confidence score, and a joint optimization solution is performed on the UWB ranging data, the first inertial data, and the second inertial data to obtain the target position information of the target to be located and the estimated value of the time-varying lever arm vector between the first and second IMUs. The objective function of the joint optimization solution includes a regularization term constructed based on historical trajectory smoothing constraints to suppress abnormal jumps in the estimated value of the time-varying lever arm vector. The attitude change information of the target to be located downhole is determined based on the target location information and the estimated value of the time-varying lever arm vector.

[0013] For ease of understanding, the following explains some key terms in this embodiment: UWB, or Ultra Wideband, is a wireless communication technology that uses non-sinusoidal narrow pulses at the nanosecond or microsecond level to transmit data. It has high-precision ranging capabilities and is often used for positioning in environments where GPS signals are limited, such as indoors or underground.

[0014] Inertial navigation is an autonomous navigation technology that calculates the position, velocity, and attitude of a vehicle in space by measuring its acceleration and angular velocity and combining them with its initial position and attitude information.

[0015] An IMU, or inertial measurement unit, typically contains a three-axis accelerometer and a three-axis gyroscope, used to measure the linear acceleration and angular velocity of an object in three-dimensional space.

[0016] Inertial data is raw data collected by the IMU, including acceleration data and angular velocity data.

[0017] UWB tags are devices attached to the target to be located and used for ranging communication with UWB base stations.

[0018] The target to be located is an object or person whose exact position and orientation need to be determined in the downhole environment.

[0019] Differential acceleration characteristics are obtained by comparing the difference in acceleration data acquired by two IMUs, reflecting the relative motion characteristics between the two IMUs.

[0020] The lever arm change detection result is a signal or estimate indicating whether the relative positional relationship (i.e., lever arm) between the UWB tag and the target to be located has changed.

[0021] UWB ranging data is the distance information between a UWB tag and a UWB base station measured using UWB technology.

[0022] Historical trajectory smoothing constraint is a restriction imposed on the current positioning result based on the assumption of the continuity and smoothness of the target's past movement trajectory, in order to improve the stability of positioning.

[0023] Multidimensional confidence assessment is a process that comprehensively considers the reliability of multiple data sources (such as inertial data, UWB data, historical trajectories, etc.) and quantifies them.

[0024] The overall confidence level is the result of a multi-dimensional confidence assessment and is used to characterize the overall reliability level of the current positioning system.

[0025] The fusion status diagnosis results are based on the degree of conflict and convergence status during the confidence assessment process, and are used to diagnose the current operating status of the fusion positioning system, indicating the effectiveness of the assessment.

[0026] Factor graphs are a type of probabilistic graphical model used to represent the probabilistic relationships between variables and between variables and observations. They are often used in multi-sensor fusion and optimization problems.

[0027] In a factor diagram, the UWB factor represents the constraint relationship between UWB ranging data and state variables (such as location).

[0028] The inertia factor in the factor diagram represents the constraint relationship between IMU inertial data and state variables (such as position, velocity, and attitude).

[0029] Joint optimization is a process that integrates multiple sensor data and constraints into a unified optimization framework and finds the optimal state estimate through iterative calculation.

[0030] The target location information is the three-dimensional coordinates of the target to be located in the downhole environment.

[0031] The time-varying lever vector is a vector representation of the relative positional relationship between the UWB tag and the target to be located as time changes.

[0032] Attitude change information describes the rotational and translational motion of the target in the downhole environment relative to a certain reference coordinate system.

[0033] This embodiment provides a downhole fusion positioning system based on UWB and inertial navigation. The system includes a processor and a memory. See [link to documentation]. Figure 1 The processor is configured to perform the following steps: 101. In the downhole environment, differential calculation is performed on the first inertial data collected by the first IMU and the second inertial data collected by the second IMU to obtain differential acceleration characteristics and lever arm change detection results.

[0034] In this system, the first IMU is fixed at the UWB tag, and the second IMU is fixed at the target to be located. For example, the raw acceleration data from the first and second IMUs can be subtracted point-by-point to obtain the differential acceleration, which can then be used as the differential acceleration feature. Lever change detection can be achieved by setting a fixed threshold; when the magnitude of the differential acceleration exceeds this threshold, a lever change is considered to have occurred, and a lever change detection result is generated. Alternatively, the acceleration data from the first and second IMUs can be low-pass filtered to remove high-frequency noise before subtracting to obtain the differential acceleration. Lever change detection can be determined by observing the long-term trend or mean change of the differential acceleration.

[0035] 102. Based on the boom change detection results, UWB ranging data, and historical trajectory smoothing constraints, a multi-dimensional confidence assessment is performed by fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to obtain the comprehensive confidence and fusion state diagnosis results.

[0036] For example, the lever arm change detection result can act as a simple binary switch, triggering a confidence assessment when a change is detected. UWB ranging data, inertial data consistency, UWB signal quality, and geometric constraints can each be calculated as an independent confidence score, which is then averaged using a simple weighted average to obtain the overall confidence score. The fusion state diagnosis result can be determined simply based on whether the overall confidence score exceeds a certain fixed threshold. Alternatively, the lever arm change detection result can serve as a trigger condition for the assessment without affecting the assessment process itself. The confidence scores for each dimension can be linearly combined based on preset fixed weights. The fusion state diagnosis result can be generated solely based on whether the confidence scores for each dimension are within a preset range.

[0037] 103. Based on the comprehensive confidence level, dynamically adjust the weights of the UWB factor and the inertial factor in the factor graph, and perform joint optimization on the UWB ranging data, the first inertial data and the second inertial data to obtain the target position information of the target to be located and the estimated value of the time-varying lever vector between the first IMU and the second IMU.

[0038] The objective function of the joint optimization solution includes a regularization term constructed based on historical trajectory smoothing constraints to suppress anomalous jumps in the time-varying lever arm vector estimate. For example, the overall confidence level can be simply mapped to a fixed weight adjustment coefficient; when the confidence level is high, the weight of the UWB factor increases, and the weight of the inertia factor decreases, and vice versa. The joint optimization solution can employ the standard nonlinear least squares method, and the regularization term can be a fixed quadratic penalty term to constrain the change in the lever arm vector. As an alternative implementation, the overall confidence level can be used to select from several preset weight combinations. The joint optimization solution can be performed within a fixed-size sliding window, and the regularization term can be a simple L2 norm term used to smooth the lever arm vector estimate, but its strength does not dynamically change with the system state.

[0039] 104. Determine the attitude change information of the target to be located downhole based on the target position information and the estimated value of the time-varying lever arm vector.

[0040] For example, the rate of change of position can be directly extracted as velocity from the target position information, and simple rotational information can be inferred from the change of the time-varying lever arm vector. These information can then be combined to form attitude change information. As another approach, the acceleration can be roughly estimated by performing two time derivatives on the target position information, and the attitude angle can be approximated by combining the direction change of the time-varying lever arm vector, thereby generating attitude change information.

[0041] The method proposed in this application detects lever arm changes by calculating inertial data in real time using differential calculations and performs joint optimization based on multi-dimensional confidence assessment and dynamic adjustment of factor map weights. This solves the positioning accuracy and reliability problems caused by dynamic lever arm changes between UWB tags and the target to be located in downhole environments. Therefore, it overcomes the limitations of fixed compensation or piecewise constant assumptions in related technologies, achieving real-time detection and adaptive compensation for dynamic lever arm changes. This results in consistently high-accuracy and high-reliability positioning results in dynamic downhole environments, improving the robustness and accuracy of downhole personnel and equipment positioning.

[0042] In some embodiments described above, this application proposes differential calculation of first inertial data acquired by a first IMU and second inertial data acquired by a second IMU to obtain differential acceleration features and lever arm change detection results, which support subsequent fusion positioning. However, during implementation, the calculation of differential acceleration features may fluctuate significantly due to noise interference, leading to feature instability. The sensitivity of lever arm change detection is insufficient, failing to respond promptly to rapid attitude changes or vibrations in the dynamic downhole environment. The lack of a reliable mechanism for providing initial optimization values ​​makes subsequent estimation of time-varying lever arm vectors prone to jumps or convergence difficulties, thus affecting positioning accuracy and robustness.

[0043] To address this, this application further proposes a method for differentially calculating the differential acceleration characteristics and lever arm change detection results by performing differential calculations on the first inertial data acquired by the first IMU and the second inertial data acquired by the second IMU. (See [link to relevant documentation]). Figure 2 The specific steps include: 201. Based on the first inertial data and the second inertial data, determine the acceleration difference value between the first IMU and the second IMU, and use the acceleration difference value as the differential acceleration feature.

[0044] 202. Perform sliding window statistics on the differential acceleration feature to obtain the standard deviation of the differential acceleration feature within the current time window, and generate a lever arm change trigger signal based on the ratio of the standard deviation within the current time window to the standard deviation at the initial calibration time.

[0045] 203. When the lever arm change trigger signal indicates that the lever arm has changed, the differential acceleration feature is integrated to obtain an estimated value of the lever arm change trend. The lever arm change trigger signal and the estimated value of the lever arm change trend are used together as the lever arm change detection result. The estimated value of the lever arm change trend is used to provide an optimized initial value for the subsequent estimation of the time-varying lever arm vector.

[0046] For example, determining the acceleration difference between the first IMU and the second IMU aims to directly capture the relative motion information between the two inertial measurement units. This can be achieved in several ways. For instance, the instantaneous difference can be obtained by directly subtracting the first acceleration data of the first IMU after time synchronization and coordinate system alignment from the second acceleration data of the second IMU. Alternatively, a state observer containing acceleration data from both IMUs can be constructed, and the acceleration difference between them can be estimated using methods such as Kalman filtering. Furthermore, the raw acceleration data from the two IMUs can be preprocessed, such as through low-pass filtering or bias removal, before performing the difference operation. This difference effectively filters out common motion components, highlighting differences caused by lever effects or relative motion, thus forming the differential acceleration characteristic.

[0047] A sliding window statistical analysis is performed on the differential acceleration characteristic to obtain its dynamic characteristics over a period of time. For example, the standard deviation of the differential acceleration characteristic within the current time window can be calculated, reflecting the degree of fluctuation in the differential acceleration. Besides the standard deviation, other statistical measures such as variance, root mean square (RMS), or mean absolute deviation (MAD) can also be used to characterize its dispersion. The standard deviation within the current time window is compared with the standard deviation obtained beforehand under steady-state conditions (e.g., during the initial calibration phase), for example, by calculating the ratio between the two. When this ratio exceeds a preset threshold, a lever change trigger signal is generated, indicating that a change in the lever may have occurred. This judgment method based on statistical ratios can effectively distinguish between normal motion fluctuations and actual lever changes, improving the sensitivity and robustness of the detection.

[0048] When the lever arm change trigger signal indicates a change in the lever arm, the differential acceleration feature is integrated. Integration accumulates the change in differential acceleration over time, thus obtaining information about the trend of the lever arm change. For example, trapezoidal integration, Simpson's integration, or more complex numerical integration algorithms can be used to integrate the differential acceleration feature to obtain the cumulative displacement or velocity change trend during the change. This integration result is the estimated value of the lever arm change trend, which provides not only the amplitude information of the lever arm change but also its direction information. The lever arm change trigger signal and the estimated value of the lever arm change trend are used together as the detection result of the lever arm change. The estimated value of the lever arm change trend is particularly crucial, as it provides a physically meaningful and relatively accurate initial value for subsequent estimation of the time-varying lever arm vector, reducing the convergence difficulty and computational cost of the optimization algorithm.

[0049] The above technical solution solves the problems of large fluctuations in differential acceleration characteristics due to noise interference, insufficient sensitivity in lever arm change detection, and lack of reliable initial values ​​for optimization. By directly determining the acceleration difference between the first and second IMUs as the differential acceleration characteristic, the redundant information of the two IMUs can be effectively utilized to capture dynamic changes caused by the lever arm effect, thereby avoiding interference from noise from a single data source and generating a more reliable and stable differential acceleration characteristic. A sliding window statistical analysis is performed on this differential acceleration characteristic, and a lever arm change trigger signal is generated based on the ratio of the standard deviation within the current time window to the standard deviation at the initial calibration time. This dynamic comparison mechanism can smooth out instantaneous noise and fluctuations and sensitively identify lever arm change events, ensuring timely and accurate detection of dynamic changes in the lever arm during rapid attitude adjustments or equipment vibrations in the downhole environment. When the lever arm change trigger signal indicates a change in the lever arm, the differential acceleration feature is integrated to obtain an estimate of the lever arm change trend. This estimate serves as the initial value for subsequent estimation of the time-varying lever arm vector, reducing the number of iterations and the risk of jumps in the optimization process, thereby enhancing the convergence efficiency and robustness of the entire positioning system. These synergistic features not only improve the accuracy and real-time performance of lever arm change detection but also provide a solid foundation for subsequent high-precision positioning.

[0050] In some of the embodiments described above in this application, an acceleration difference value is determined based on the first inertial data and the second inertial data for differential calculation. However, in its implementation, due to the possible time asynchrony and lever effect interference of the inertial measurement unit (IMU) data, direct differential calculation will result in inaccurate acceleration difference values, affecting the reliability of subsequent lever change detection.

[0051] In this regard, this application further proposes a method for determining the acceleration differential value between the first IMU and the second IMU, which includes: The first acceleration data in the first inertial data and the second acceleration data in the second inertial data are time-synchronized and aligned to obtain synchronized first acceleration data and synchronized second acceleration data.

[0052] Based on the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data, preliminary lever arm effect compensation is performed on the synchronized first acceleration data and the synchronized second acceleration data to obtain compensated first acceleration data and compensated second acceleration data.

[0053] The difference between the compensated second acceleration data and the compensated first acceleration data is used to obtain the acceleration difference value.

[0054] For example, time synchronization alignment of the first acceleration data in the first inertial data and the second acceleration data in the second inertial data aims to eliminate time inconsistencies caused by sampling clock deviations or data transmission delays between different inertial measurement units (IMUs). One approach is to use hardware synchronization mechanisms, such as sharing an external clock signal or using trigger pulses, to ensure that the two IMUs acquire data at the same time. Another approach is to use software synchronization algorithms, such as timestamp-based interpolation or resampling techniques, to align the data from the two IMUs onto a unified time axis, or to utilize state estimation algorithms such as Kalman filtering to dynamically estimate and compensate for time deviations during the fusion process.

[0055] Based on the first angular velocity data from the first inertial data and the second angular velocity data from the second inertial data, preliminary compensation for the lever effect is performed on the synchronized first and second acceleration data. The aim is to eliminate or reduce the additional acceleration components experienced by the non-rotation center IMU when rotational motion occurs due to the physical distance (i.e., lever) between the two IMUs. One approach is to directly calculate the centripetal and tangential accelerations caused by rotation using kinematic formulas based on the known or preliminarily estimated lever vector and the IMU's angular velocity, and then subtract these components from the original acceleration data. Another approach is to utilize a model-based prediction compensation method. By establishing a dynamic model of the relative motion of the IMUs and combining it with angular velocity data, the deviation introduced by the lever effect in the acceleration data is predicted and corrected.

[0056] The difference between the compensated second acceleration data and the compensated first acceleration data is used to obtain purely relative linear acceleration information between the two IMUs. This step is a direct vector subtraction operation. By subtracting the two acceleration data after initial compensation for time synchronization and lever effect, a differential acceleration value that accurately reflects the relative motion state between the two IMUs is obtained.

[0057] The above technical solution solves the problems of time asynchrony and lever arm effect interference encountered when performing differential calculations on inertial measurement unit (IMU) data. For example, synchronizing the first and second acceleration data ensures data consistency in the time dimension and eliminates errors caused by mismatched acquisition timing. Based on the first and second angular velocity data, preliminary lever arm effect compensation is performed on the synchronized acceleration data. The angular velocity information accurately corrects the additional acceleration component caused by rotational motion, making the compensated acceleration data more realistically reflect the translational motion of the IMU. Subtracting the compensated second acceleration data from the compensated first acceleration data improves the accuracy of the resulting differential acceleration value. Therefore, this technical solution provides high-precision and high-reliability input for subsequent lever arm change detection, thereby improving the overall downhole fusion positioning method's ability to perceive dynamic lever arm changes and its positioning accuracy.

[0058] In some of the solutions mentioned above in this application, a preliminary compensation for the lever effect based on angular velocity data is proposed to compensate for acceleration data. However, in this process, since the motion state of the target to be located is not distinguished, the compensation may be inaccurate. This is because the lever effect has different physical characteristics and influence mechanisms under rotational and translational motion, which makes it impossible for the compensation process to specifically handle the acceleration components under different motion modes, thereby introducing errors and affecting the calculation accuracy of acceleration difference values ​​and the reliability of subsequent positioning results.

[0059] To address this, this application proposes a method for performing preliminary lever arm effect compensation on synchronized first and second acceleration data based on first and second angular velocity data to obtain compensated first and second acceleration data. The method includes: determining the motion state of the target to be located based on the first and second angular velocity data, whereby the motion state includes rotational and translational motion. In the case of rotational motion, the centripetal acceleration component and tangential acceleration component are separated from differential acceleration features, and lever arm effect compensation is performed based on these components to obtain the compensated first and second acceleration data. In the case of translational motion, the synchronized first and second acceleration data are corrected using differential acceleration features to obtain the compensated first and second acceleration data.

[0060] The process involves determining the motion state of the target to be located, including rotational and translational motion. This step aims to identify the target's current motion pattern, i.e., whether it is in a rotational or translational motion state. Since the lever effect exhibits different physical characteristics and influence mechanisms in different motion modes, accurate identification of the motion state is a prerequisite for targeted compensation. For example, this can be determined by analyzing the differences, rates of change, or magnitudes of the first and second angular velocity data. For instance, if the angular velocity data of both IMUs are greater than a certain preset threshold and there is a certain difference between them, it can be determined as a rotational motion state. If both are close to zero or fluctuate within a small threshold, it can be determined as a translational motion state. Alternatively, the cross-correlation or covariance of the two IMU angular velocity data can be calculated. When the cross-correlation is low or the covariance is large, it indicates that there is relative rotation between the two IMUs, thus identifying rotational motion. Conversely, when the cross-correlation is high and the covariance is small, it indicates that the two IMUs are relatively stationary or undergoing translational motion, thus identifying translational motion.

[0061] When the target is in a rotational motion state, the centripetal acceleration component and the tangential acceleration component are separated from the differential acceleration feature. Lever effect compensation is then performed based on these components to obtain the compensated first and second acceleration data. When the target is in a rotational motion state, the lever effect mainly manifests as centripetal and tangential acceleration. This step aims to accurately identify and compensate for these acceleration components caused by the lever arm, eliminating their influence on the differential acceleration value. One implementation method is to use the first and second angular velocity data, combined with known or estimated lever vectors, to calculate the theoretical centripetal and tangential acceleration components using kinematic formulas. These calculated components are then subtracted from the differential acceleration feature to achieve separation and compensation. Another implementation method is to use adaptive filtering or state estimation methods, such as constructing a Kalman filter containing centripetal and tangential acceleration state variables, using the differential acceleration feature as observations to estimate and separate these components, and then using them for compensation.

[0062] When the motion is translational, the differential acceleration feature is used to correct the synchronized first and second acceleration data, resulting in compensated first and second acceleration data. When the target is in pure translational motion, theoretically, the acceleration data from the two IMUs should be identical (ignoring noise and small lever effects). In this case, the differential acceleration feature mainly reflects sensor noise, incompletely eliminated small lever effects, or measurement errors. This step aims to use this differential information to correct the original acceleration data, improving its accuracy. One implementation is to add half of the differential acceleration feature as a correction to the synchronized first acceleration data and subtract the other half from the synchronized second acceleration data, making them more consistent. Another implementation is to use a weighted average correction based on the noise characteristics or reliability of the two IMUs. For example, if a certain IMU is known to have a lower noise level, it can be given a higher weight, making its acceleration data closer to the true value after correction.

[0063] The above technical solution enables dynamic adjustment of the lever arm effect compensation strategy based on the actual motion pattern of the target to be located. This targeted compensation avoids the errors introduced by using a single compensation model under different motion patterns, improving the calculation accuracy of acceleration difference values. Especially in complex dynamic environments downhole, where the motion state of the target (such as personnel or equipment) frequently changes, this solution ensures the accuracy and robustness of lever arm effect compensation, thus providing more reliable inertial data input for subsequent lever arm change detection and joint optimization solutions, improving the accuracy and reliability of downhole positioning results. This adaptive motion state differentiation mechanism enhances the targeting and accuracy of compensation, laying a reliable data foundation for subsequent positioning.

[0064] In some of the solutions mentioned above in this application, lever arm effect compensation based on motion state is proposed to adapt to the dynamic environment downhole. However, in the rotational motion state, how to accurately separate the centripetal acceleration component and the tangential acceleration component to eliminate their interference on the differential acceleration characteristics and avoid introducing additional errors in the compensation process, which would lead to inaccurate lever arm effect correction and reduced positioning accuracy, is a problem that needs to be solved.

[0065] To address this, this application further proposes lever effect compensation based on centripetal and tangential acceleration components to obtain compensated first and second acceleration data. Specifically, this includes: removing the centripetal and tangential acceleration components from the differential acceleration features to obtain the remaining acceleration difference; correcting the synchronized first and second acceleration data based on the remaining acceleration difference to obtain compensated first and second acceleration data; and further correcting the synchronized first and second acceleration data using differential acceleration features to obtain compensated first and second acceleration data, including: adding half of the differential acceleration features as a correction to the synchronized first acceleration data; and subtracting half of the differential acceleration features as a correction from the synchronized second acceleration data to obtain compensated second acceleration data.

[0066] For example, removing the centripetal and tangential acceleration components from the differential acceleration features yields the remaining acceleration difference, aiming to accurately separate the pure relative acceleration components caused by the lever effect. During rotational motion, the first and second IMUs on the target are affected not only by translational acceleration but also by centripetal and tangential acceleration. If these rotation-related acceleration components are not effectively removed, they will obscure the true relative acceleration caused by the lever effect, leading to inaccurate subsequent compensation. This step ensures that the remaining acceleration difference more accurately reflects changes in lever length or relative position by identifying and removing these rotational components. For instance, the theoretical centripetal and tangential acceleration components can be calculated based on the relative attitude, angular velocity, and estimated lever vector between the first and second IMUs, and then removed by vector subtraction from the original differential acceleration features. Alternatively, signal processing techniques, such as designing specific filters, can be used to separate and remove these periodic or quasi-periodic rotational acceleration components from the differential acceleration features when the rotation frequency or pattern is known.

[0067] The residual acceleration difference is used to correct the synchronized first and second acceleration data, resulting in compensated first and second acceleration data. The purpose is to finely adjust the acceleration data of the two IMUs by utilizing the relative acceleration difference caused by the pure lever effect after eliminating interference from rotational motion, thus achieving theoretical consistency. This helps eliminate the lever effect in acceleration measurement, providing more accurate inertial data for subsequent positioning fusion. For example, the residual acceleration difference can be evenly distributed between the two IMUs, with half added to the synchronized first acceleration data and the other half subtracted from the synchronized second acceleration data. Alternatively, a weighted average can be used to proportionally distribute the residual acceleration difference between the two IMUs for correction, based on their installation location, sensor characteristics, or historical error statistics.

[0068] The differential acceleration characteristics are used to correct the synchronized first and second acceleration data, resulting in compensated first and second acceleration data. This step primarily addresses the initial compensation for lever arm effects during translational motion. In pure translational motion, theoretically, the accelerations of the two IMUs should be identical, and the differential acceleration characteristic should be zero. However, in actual measurements, slight lever arm effects, sensor noise, or installation errors may cause non-zero differential values. This step achieves symmetrical correction of the acceleration data from the two IMUs by adding half of the differential acceleration characteristic as a correction to the synchronized first acceleration data, and subtracting half of the differential acceleration characteristic as a correction from the synchronized second acceleration data, thus obtaining the compensated second acceleration data. This correction method makes the acceleration data of the two IMUs more consistent, effectively distributing the influence of the differential acceleration characteristic evenly across the two sensors, thereby initially eliminating or reducing measurement deviations caused by lever arm effects during translational motion.

[0069] The above technical solution enables the dynamic selection and execution of corresponding lever arm effect compensation strategies based on the specific motion state (rotation or translation) of the target to be located. In rotational motion, by accurately eliminating centripetal and tangential acceleration components, the interference of these inherent rotational accelerations on lever arm effect correction is avoided, ensuring that the remaining acceleration difference more accurately reflects the true changes in the lever arm, thereby improving compensation accuracy. In translational motion, by symmetrically distributing the differential acceleration characteristics to the two IMUs for correction, the compensation intensity is effectively balanced, avoiding over- or under-compensation, ensuring high consistency of acceleration data from the two IMUs during translational motion. Overall, this method enhances the adaptability and accuracy of lever arm effect correction, solves the problem of decreased positioning accuracy caused by dynamic changes in the lever arm in complex downhole environments, and provides more reliable and accurate inertial measurement data for subsequent UWB and inertial navigation fusion positioning.

[0070] In some of the embodiments described above in this application, a multi-dimensional confidence assessment is proposed to dynamically adjust the weights of UWB and inertia factors. However, in its implementation, it is necessary to more accurately integrate multiple dimensions (including IMU motion consistency, UWB signal quality, downhole positioning geometric constraints, and historical trajectory consistency) to ensure the reliability and effectiveness of the assessment, especially when the boom changes, in order to provide more accurate condition diagnosis and avoid assessment bias caused by information separation.

[0071] To address this, this application further proposes a multi-dimensional confidence assessment method, which includes: based on the boom change detection results, UWB ranging data, and historical trajectory smoothing constraints, fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to perform a multi-dimensional confidence assessment, obtaining a comprehensive confidence score and a fused state diagnosis result. For example, see... Figure 3 The method includes the following steps: 301. Based on the first inertial data and the second inertial data, determine the IMU motion consistency confidence level.

[0072] IMU motion consistency confidence is an indicator that measures the data synchronization and physical correlation between the first and second IMUs during motion. Its function is to evaluate the data reliability of the two IMUs under specific motion states, especially when there are lever changes or external disturbances, to determine whether the measurements of the two IMUs conform to the expected relative motion pattern. One implementation method is to determine this by comparing the differences in acceleration or angular velocity data of the two IMUs after transformation in their respective coordinate systems. For example, the measurement data of one IMU can be transformed to the coordinate system of the other IMU through relative attitude transformation, and then the Euclidean distance, covariance, or correlation coefficient between the two can be calculated as the confidence level. Another implementation method is to use state estimation algorithms such as Kalman filtering or extended Kalman filtering, taking the measurements of the two IMUs as observation inputs, estimating the relative state (such as relative position, relative attitude) between them, and evaluating their consistency based on the diagonal elements of the estimated residuals or covariance matrices.

[0073] 302. Based on the UWB ranging data and the geometric layout information of the underground base station, determine the confidence level of UWB signal quality and the confidence level of underground positioning geometric constraints.

[0074] The UWB signal quality confidence score is used to assess the reliability of UWB ranging data in the current downhole environment. The downhole environment is complex, and factors such as multipath effects and non-line-of-sight propagation can severely impact UWB ranging accuracy. This confidence score aims to quantify the degree of impact of these adverse factors on ranging data quality. One approach is to determine this by analyzing the channel impulse response (CIR) characteristics of the UWB signal. For example, parameters such as first-path power, total power, signal-to-noise ratio (SNR), and the number and intensity of multipath components can be extracted, and the signal quality can be evaluated based on the combined values ​​of these parameters. Another approach is to determine this by statistically analyzing the historical fluctuations of UWB ranging data or the deviation from the expected distance. For example, given the location of base stations, the ranging residual can be calculated based on the distance between the coarse location estimate of the UWB tag and each base station, and the signal quality can be evaluated based on the root mean square error (RMSE) or standard deviation of the residual. The downhole positioning geometric constraint confidence score reflects the degree of impact of the geometric distribution of UWB base stations on positioning accuracy. In UWB positioning, the geometric distribution of base stations (such as the number of base stations and the relative positions between base stations and the target) directly affects the accuracy of the positioning solution, known as the Geometric Precision Factor (GDOP). This confidence level aims to quantify the impact of this geometric configuration on the reliability of the positioning results. One approach is to determine this by calculating the GDOP or its variants. A smaller GDOP value indicates a better geometric configuration and higher positioning accuracy; the confidence level can be inversely proportional to the GDOP value. Another approach is to construct a Jacobian matrix for the positioning equation by analyzing distance measurements between UWB tags and each base station, combined with the known locations of the base stations, and then assess the strength of the geometric constraints based on the condition number or eigenvalue distribution of this matrix.

[0075] 303. Based on the historical trajectory smoothing constraint and historical positioning results, determine the historical trajectory consistency confidence level. The historical positioning results are obtained by dead reckoning based on the first inertial data and the second inertial data.

[0076] The historical trajectory consistency confidence score is used to assess whether the current positioning result matches the smoothness and continuity of the historical motion trajectory. While inertial navigation systems (INS) offer high autonomy, errors accumulate over time, whereas UWB positioning achieves high accuracy under favorable geometric configurations. By comparing with historical trajectories, abnormal positioning jumps or unreasonable motion patterns can be detected, thereby improving the robustness of positioning. One approach is to compare the current positioning result with the predicted position obtained through a motion model based on historical trajectory data (such as historical position and velocity). If the deviation between the current positioning result and the predicted position is within a reasonable range, the consistency is considered high. Another approach is to analyze the rate of change of velocity and acceleration in historical positioning results to determine whether they conform to the laws of physical motion. For example, if the velocity or acceleration change caused by the current positioning result is too large and exceeds physical limits, the historical trajectory consistency confidence score is considered low.

[0077] 304. Using the arm change detection result as the trigger condition for confidence assessment, the confidence of the IMU motion consistency, the confidence of the UWB signal quality, the confidence of the downhole positioning geometric constraint, and the confidence of the historical trajectory consistency are fused to obtain the comprehensive confidence and the fused state diagnosis result.

[0078] Using the lever arm change detection result as the trigger condition for confidence assessment means that the confidence assessment process is not continuous or performed at a fixed frequency, but is only activated or reinitialized when a potential change in the lever arm is detected. Its purpose is that when the lever arm dynamically changes, the original fixed lever arm assumption no longer holds, requiring a reassessment of the reliability of UWB and IMU data to avoid positioning errors caused by lever arm changes. One implementation is to immediately activate or reset the multi-dimensional confidence assessment module when the lever arm change detection result indicates a change, comprehensively calculating and fusing the IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence. Another implementation is to treat the lever arm change detection result as a binary signal (changed / unchanged). When the signal is "changed," the output weights or fusion strategy of the confidence assessment module will be adjusted; for example, it may reduce the dependence on certain confidence levels or trigger more frequent assessment cycles. This approach integrates confidence scores from multiple dimensions to comprehensively assess the reliability of UWB and IMU data, outputting a unified overall confidence score and a diagnostic result reflecting the state of the fusion process itself. The overall confidence score is a weighted average of all evaluation dimensions, guiding the weight adjustment of UWB and inertia factors in subsequent factor graph optimization. The fusion state diagnostic result indicates the validity of the current confidence score assessment or the existence of conflicts, providing a deeper level of decision-making support for the system. One implementation method is to use information fusion frameworks such as Dempster-Shafer evidence theory or Bayesian networks. Confidence scores from each dimension are used as evidence input, and the overall confidence score is iteratively synthesized by defining a basic probability assignment function and fusion rules. The fusion state diagnostic result is generated by analyzing the degree of conflict between evidence or the convergence of the fusion process. Another implementation method is to use weighted averaging or fuzzy logic. A weight is assigned to each confidence score, and then a weighted sum is performed to obtain the overall confidence score. The fusion state diagnostic results can be generated by monitoring the differences between individual confidence levels or the rate of change of the overall confidence level. For example, if there are contradictions among the confidence levels, the diagnostic results may indicate "assessment conflict".

[0079] The above technical solutions address the shortcomings of dynamic perception and feedback adjustment mechanisms for the reliability of multi-source information in complex and dynamic downhole environments. By introducing lever arm change detection results as the trigger condition for confidence assessment, it ensures timely and proactive comprehensive and multi-dimensional evaluation of the reliability of UWB ranging and inertial data when the lever arm undergoes dynamic changes. For example, IMU motion consistency confidence effectively reflects the reliability of dual IMU data in relative motion, avoiding the accumulation of errors from a single IMU. UWB signal quality confidence and downhole positioning geometric constraint confidence comprehensively evaluate the availability and accuracy potential of UWB ranging data from both the signal itself and base station layout perspectives, effectively addressing the impact of complex environments such as multipath and non-line-of-sight conditions in downhole environments. Historical trajectory consistency confidence utilizes historical motion patterns to smooth and constrain the current positioning results, enhancing the continuity and rationality of the trajectory. These confidence scores are dynamically triggered and fused when the boom changes, resulting not only in a quantified comprehensive confidence score for adaptive adjustment of the UWB and inertia factor weights in subsequent factor graph optimization, but also in a fusion state diagnostic result, providing the system with in-depth feedback on the effectiveness of the current assessment. This dynamic, multi-dimensional confidence assessment mechanism enhances the robustness and adaptability of positioning in complex downhole environments, ensuring high-precision and high-reliability positioning results even with dynamic boom changes, thus overcoming the problem of decreased positioning accuracy caused by the lack of dynamic sensing and feedback adjustment mechanisms in related technologies.

[0080] In some of the embodiments described above in this application, a method is proposed to determine the IMU motion consistency confidence level based on the first inertial data and the second inertial data to evaluate the consistency of the two IMU data. However, in its implementation, since the relative attitude change between the IMUs is not considered, the comparison of acceleration data may be carried out in their respective independent coordinate systems, which makes the difference measurement unable to accurately reflect the true motion consistency, thereby affecting the evaluation accuracy and reliability of the comprehensive confidence level.

[0081] To address this, this application further proposes a method for determining the IMU motion consistency confidence level, comprising: determining the IMU motion consistency confidence level based on the first inertial data and the second inertial data, specifically including: constructing a relative attitude matrix between the first IMU and the second IMU based on the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data; using the relative attitude matrix, transforming the first acceleration data in the first inertial data to the coordinate system of the second IMU to obtain the transformed first acceleration data; calculating the difference metric between the transformed first acceleration data and the second acceleration data in the second inertial data, and using the difference metric as the IMU motion consistency confidence level.

[0082] In this process, a relative attitude matrix is ​​constructed between the first and second IMUs based on the first angular velocity data from the first inertial data and the second angular velocity data from the second inertial data. This aims to accurately capture the dynamically changing rotational relationship between the first and second IMUs. In complex downhole environments, the attitude of the target (such as personnel or equipment) may change rapidly, causing continuous changes in the relative attitude between the first IMU fixed at the UWB tag and the second IMU fixed at the target. Constructing the relative attitude matrix lays the foundation for subsequent comparison of acceleration data in a unified coordinate system. This construction process can be implemented in various ways. For example, the attitudes of the two IMUs relative to a common reference coordinate system can be estimated by integrating the first and second angular velocity data, and then the relative attitude matrix can be calculated through attitude transformation. Alternatively, optimization or filtering methods can be used to directly utilize the angular velocity measurements of the two IMUs, combined with other possible constraints, to estimate and update the relative attitude matrix between them in real time.

[0083] Using the relative attitude matrix, the first acceleration data in the first inertial data is transformed to the coordinate system of the second IMU, obtaining the transformed first acceleration data. The purpose is to project the acceleration data measured by the first IMU into the local coordinate system of the second IMU. Since the physical positions and orientations of the two IMUs may differ, their measured acceleration data reside in different coordinate systems. Coordinate transformation using the relative attitude matrix eliminates errors introduced by coordinate system inconsistencies, allowing direct comparison of the acceleration data from the two IMUs within a unified reference frame. For example, this is typically achieved through matrix multiplication, where the relative attitude matrix is ​​multiplied by the first acceleration data vector to obtain the equivalent acceleration data in the second IMU's coordinate system.

[0084] The difference metric between the transformed first acceleration data and the second acceleration data in the second inertial data is calculated and used as the IMU motion consistency confidence level. This aims to quantify the similarity between acceleration measurements from two IMUs in a unified coordinate system. A smaller difference metric indicates more consistent motion between the two IMUs, higher data reliability, and higher confidence. Conversely, a larger difference metric indicates poorer motion consistency and lower confidence. This difference metric can be calculated using various mathematical methods. For example, the Euclidean distance (L2 norm) between the two acceleration vectors can be calculated, i.e., the magnitude of the vector difference. Alternatively, the statistical difference between the two acceleration vectors over a time window, such as the mean square error or covariance, can be calculated to reflect their dynamic consistency.

[0085] By explicitly constructing a relative attitude matrix between the first and second IMUs and using this matrix to accurately transform the acceleration data of the first IMU into the coordinate system of the second IMU, the inaccurate acceleration data comparison problem caused by the failure to consider the relative attitude changes of the IMUs in traditional methods is solved. This measurement of acceleration data difference in a unified coordinate system can more realistically and accurately reflect the motion consistency of the two IMUs, thereby improving the accuracy and reliability of the IMU motion consistency confidence assessment. This high-precision confidence assessment provides a more reliable input for subsequent multi-dimensional confidence assessments (such as the multi-dimensional confidence assessment that integrates the measurement consistency of the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints), thus making the comprehensive confidence assessment more accurate, optimizing the weight adjustment of the UWB factor and the inertial factor in the factor diagram, and improving the accuracy and robustness of the downhole positioning results.

[0086] In some of the embodiments described above in this application, a method is proposed to determine the confidence level of UWB signal quality and the confidence level of underground positioning geometric constraints based on UWB ranging data and the geometric layout information of underground base stations. However, in its implementation, the UWB signal quality is affected by the channel impulse response, and the geometric layout information may lead to a decrease in positioning accuracy, resulting in inaccurate confidence assessment and affecting the reliability of fused positioning.

[0087] To address this, this application further proposes a method for determining the confidence level of UWB signal quality and the confidence level of downhole positioning geometric constraints based on UWB ranging data, the first inertial data, and the second inertial data. The method includes: acquiring the channel impulse response corresponding to the UWB ranging data; extracting the first-path power ratio, signal-to-noise ratio, and multipath number based on the channel impulse response to obtain UWB signal quality features; performing weighted fusion on the UWB signal quality features to obtain the UWB signal quality confidence level; determining the geometric accuracy factor based on the geometric layout information and the UWB ranging data; and determining the confidence level of the downhole positioning geometric constraints based on the ratio of the geometric accuracy factor to a preset geometric accuracy threshold.

[0088] For example, in the step of acquiring the channel impulse response corresponding to the UWB ranging data, this step aims to directly obtain the physical channel characteristics experienced by the UWB signal during propagation. The channel impulse response is a function describing how the amplitude, phase, and time delay of a signal change as it propagates through the channel; it includes key information such as signal attenuation, multipath effects, and noise. Acquiring the channel impulse response provides the most original and comprehensive data foundation for subsequent signal quality assessment, avoiding potential errors caused by indirect assessments relying solely on ranging values. In practice, UWB transceivers typically have a built-in digital signal processor (DSP) that can directly demodulate and output an estimated value of the channel impulse response from the received UWB signal. Alternatively, the channel impulse response can be inverted by performing signal processing operations such as Fourier transform on the received raw UWB signal samples.

[0089] In the step of extracting the first-path power ratio, signal-to-noise ratio (SNR), and multipath quantity based on the channel impulse response to obtain UWB signal quality characteristics, this step aims to quantify key features closely related to UWB signal quality from the original channel impulse response. The first-path power ratio reflects the strength of the direct-path signal and its proportion in the total received power, and is an important indicator of signal obstruction and attenuation. The SNR directly quantifies the ratio of signal strength to noise strength, reflecting the signal's anti-interference capability. The multipath quantity indicates the complexity of the signal's propagation through effects such as reflection and refraction; excessive multipath effects can lead to increased ranging errors. By extracting these features, the quality changes of UWB signals in complex downhole environments can be comprehensively and accurately captured. Specifically, a peak detection algorithm can be used to identify the first-path component in the channel impulse response and calculate its power ratio to the total power to obtain the first-path power ratio. The SNR can be calculated by estimating the signal power and noise power (e.g., during periods without signal or through statistical methods). The multipath quantity can be obtained by counting the effective paths in the channel impulse response by setting a threshold.

[0090] In the step of weighted fusion of the UWB signal quality features to obtain the UWB signal quality confidence score, the aim is to integrate the multiple extracted UWB signal quality features into a single, physically meaningful confidence index. Since different signal quality features may have varying degrees of impact on positioning accuracy, weighted fusion allows for the reasonable allocation of weights to each feature, resulting in a more representative and robust UWB signal quality confidence score. Specifically, a linear weighted summation method can be used, assigning a weight coefficient to the first-path power ratio, signal-to-noise ratio, and multipath number, and then summing the weighted feature values ​​to obtain the UWB signal quality confidence score. The weight coefficients can be set based on experience, experimental data, or expert knowledge. Alternatively, a fuzzy logic system or machine learning-based methods can be used to train a model to learn how to map these features to the UWB signal quality confidence score.

[0091] In the step of determining the geometric accuracy factor based on the geometric layout information and the UWB ranging data, this step aims to evaluate the impact of the geometric distribution of UWB base stations on positioning accuracy. In underground environments, the layout of base stations may be irregular, and the geometric configuration of base stations in certain areas may amplify positioning errors, a phenomenon known as geometrical dilution of accuracy (GDOP). By determining the geometric accuracy factor, the potential impact of this geometric constraint on positioning accuracy can be quantified. Specifically, based on the UWB ranging data and known base station locations, positioning algorithms such as least squares or Kalman filtering can be used to calculate the covariance matrix of the position estimate during the positioning process, and factors related to geometric accuracy, such as the GDOP value, can be extracted from this covariance matrix. Alternatively, an observation matrix can be constructed, the elements of which are related to the relative positions between the base stations and the target to be located, and then the geometric accuracy factor can be obtained by calculating the pseudo-inverse of the observation matrix or the condition number of its correlation matrix.

[0092] In the step of determining the confidence level of the downhole positioning geometric constraint based on the ratio of the geometric accuracy factor to a preset geometric accuracy threshold, this step aims to transform the calculated geometric accuracy factor into a standardized confidence index. By comparing it with the preset geometric accuracy threshold, it can be determined whether the impact of the current base station geometric layout on positioning accuracy is within an acceptable range, and its confidence level can be quantified. In practice, the ratio of the geometric accuracy factor to the preset threshold can be directly linearly mapped; for example, the smaller the ratio (the better the geometric accuracy), the higher the confidence level. Alternatively, a non-linear mapping function, such as the sigmoid function or a piecewise function, can be used to map the ratio to a confidence score between 0 and 1, making the confidence level more sensitive to changes as it approaches the threshold.

[0093] The above technical solutions enable a more accurate and dynamic assessment of UWB signal quality and downhole positioning geometric constraints. Directly acquiring the channel impulse response and extracting UWB signal quality features such as first-path power ratio, signal-to-noise ratio, and multipath quantity provides a comprehensive and detailed reflection of the actual propagation of UWB signals in complex downhole environments. This effectively avoids errors caused by missing information or indirect assessments in traditional methods, thus improving the accuracy of UWB signal quality confidence assessment. Weighted fusion of these multi-dimensional features comprehensively considers the impact of different factors on signal quality, making UWB signal quality confidence assessment more robust and reliable. Determining the geometric accuracy factor based on geometric layout information and UWB ranging data, and comparing it with a preset threshold, dynamically quantifies the impact of base station geometric distribution on positioning accuracy. This allows for timely detection and mitigation of the risk of positioning accuracy degradation due to poor geometric configurations, thereby enhancing the effectiveness of downhole positioning geometric constraint confidence assessment. Overall, these accurate confidence assessment results provide high-quality input for subsequent fusion positioning algorithms, enabling dynamic adjustment of the weight of the UWB factor in joint optimization based on the actual quality of the UWB signal and the strength of geometric constraints. This enhances the robustness, adaptability, and accuracy of downhole fusion positioning, especially in complex downhole environments where UWB signals are susceptible to interference and base station deployment is limited. It can suppress abnormal fluctuations in positioning results and ensure the continuous reliability of positioning.

[0094] In some of the solutions described above in this application, a geometric accuracy factor is proposed to be determined based on geometric layout information and UWB ranging data to calculate the confidence level of downhole positioning geometric constraints. However, in the process of its implementation, how to accurately calculate the geometric accuracy factor to ensure the accuracy of the confidence level assessment, thereby improving the positioning reliability in complex downhole environments, is a problem that needs to be solved.

[0095] To address this, this application further proposes a method for determining the confidence level of UWB signal quality and the confidence level of underground positioning geometric constraints based on UWB ranging data and the geometric layout information of underground base stations. The geometric layout information includes the location coordinates of each base station. Determining the geometric accuracy factor based on the geometric layout information and the UWB ranging data includes: determining the measurement distance between the UWB tag and each base station based on the UWB ranging data; constructing an observation matrix based on the location coordinates of each base station and the measurement distance; determining the covariance matrix of the location estimation based on the observation matrix; extracting diagonal elements from the covariance matrix and calculating the geometric accuracy factor based on the diagonal elements.

[0096] For example, "the geometric layout information includes the location coordinates of each base station" refers to the spatial three-dimensional coordinates of each UWB base station that have been accurately mapped and recorded in the underground environment beforehand. These coordinates are the basic reference for UWB ranging and positioning, and their accuracy directly affects the accuracy of subsequent positioning calculations. For instance, the location coordinates of the base stations can be obtained in various ways. For example, the underground base stations can be accurately mapped in advance using high-precision measuring equipment (such as a total station or laser scanner), and these coordinates can be stored in the positioning system database for real-time retrieval. Alternatively, self-calibration or SLAM (Simultaneous Localization and Mapping) technology can be used to estimate and optimize the base station positions during the initial stage of system operation or periodically to adapt to possible changes in the underground environment.

[0097] "Determining the measured distance between the UWB tag and each base station based on the UWB ranging data" refers to using UWB technology to obtain the actual distance information between the UWB tag and each UWB base station deployed in the underground environment. These measured distances are key inputs for constructing the positioning equation set, directly reflecting the real-time spatial relationship between the UWB tag and each base station. Specifically, this measured distance can be obtained by the UWB tag actively sending a ranging request to the base station. After the base station responds, the tag calculates the distance based on the signal's Time of Flight (ToF) or Round-Trip Time (TWR). Alternatively, the base station can actively send a ranging signal to the tag, and after the tag receives it, the base station calculates the distance based on the signal propagation time.

[0098] "Constructing an observation matrix based on the location coordinates of each base station and the measured distance" refers to transforming the known location coordinates of UWB base stations and the measured distances between UWB tags and each base station into a mathematical matrix form to describe the positioning observation model. This observation matrix is ​​the foundation for solving the linear or nonlinear equations for UWB tag positions and can effectively capture the dynamic influence of geometric relationships on positioning accuracy. For example, for a three-dimensional positioning problem, each row of the observation matrix typically corresponds to the observation equation of a base station, and its elements can be represented as the partial derivatives of the tag position to be estimated with respect to the base station coordinates and the measured distance. For instance, in a nonlinear least squares framework, the observation matrix can be represented as the Jacobian matrix of the measurement equation with respect to the state variable (tag position).

[0099] "Determining the covariance matrix of the position estimate based on the observation matrix" refers to using the constructed observation matrix, combined with a measurement noise model, to calculate the uncertainty or error distribution of the position estimate of the target to be located. The diagonal elements of the covariance matrix represent the variance along each coordinate axis, while the off-diagonal elements represent the covariance between different coordinate axes. It directly quantifies the impact of geometric configuration and measurement noise on the accuracy of the position estimate. Specifically, in classic linear least squares problems, the covariance matrix of the position estimate is usually calculated by multiplying the transpose of the observation matrix by itself (the inverse matrix), and may be weighted by incorporating the covariance matrix of the measurement noise. In nonlinear optimization, the covariance matrix can be approximated by the inverse of the Hessian matrix of the objective function, thus reflecting the local uncertainty of the positioning solution.

[0100] "Extracting diagonal elements from the covariance matrix and calculating the geometric precision factor based on these diagonal elements" refers to obtaining variance information in each coordinate direction from the covariance matrix of the position estimation and synthesizing it into a single scalar index, namely the geometric precision factor. The diagonal elements of the covariance matrix directly represent the magnitude of uncertainty in the position estimation in each independent direction. Specifically, the geometric precision factor can be defined as the sum of the square roots of the diagonal elements of the covariance matrix (i.e., the sum of the standard deviations in each direction), or the square root of the sum of the diagonal elements (i.e., the root mean square of the position error), to provide a comprehensive assessment of geometric accuracy. Alternatively, the trace (the sum of the diagonal elements) or its determinant of the covariance matrix can be calculated and then mapped to the geometric precision factor using a specific function, such as the reciprocal of the trace or a power of the reciprocal of the determinant, to more intuitively reflect the degree of influence of the geometric configuration on positioning accuracy.

[0101] The above technical solution enables accurate calculation of the geometric precision factor, thus solving the problem of inaccurate geometric constraint confidence assessment in complex downhole environments. For example, by accurately obtaining the location coordinates of each base station and the measured distance between the UWB tag and each base station, an accurate observation matrix can be constructed, thereby reliably determining the covariance matrix of the location estimate. Extracting diagonal elements from the covariance matrix and calculating the geometric precision factor allows for a comprehensive evaluation of the impact of the current UWB positioning geometry on positioning accuracy. This accurate geometric precision factor calculation method makes the assessment of UWB signal quality confidence and downhole positioning geometric constraint confidence more accurate and reliable. Therefore, in subsequent multi-dimensional confidence assessments, it can more accurately reflect the reliability of UWB positioning, providing a more accurate basis for the dynamic adjustment of the weights of the UWB factor and inertia factor in the factor graph, improving the overall accuracy and robustness of downhole fusion positioning. Especially in complex downhole environments where UWB signals are greatly affected by the environment or where base station geometry is not ideal, it can effectively avoid the degradation of positioning performance caused by inaccurate geometric constraint assessment.

[0102] In some of the embodiments described above in this application, a confidence level for determining the consistency of historical trajectories based on historical trajectory smoothing constraints and historical positioning results is proposed to evaluate the consistency of historical trajectories. However, in its implementation, historical positioning results may be deviated due to the accumulation of errors in inertial navigation or interference from the dynamic environment downhole, resulting in inaccurate confidence level evaluation, which in turn affects the reliability of multi-dimensional fusion and reduces positioning accuracy.

[0103] To address this, this application further proposes that historical values ​​include historical position sequences and historical velocity sequences. Based on historical trajectory smoothing constraints and target position information, the historical trajectory consistency confidence level is determined. Specifically, this includes: acquiring historical positioning results obtained through dead reckoning based on the first inertial data and the second inertial data, thus obtaining the historical position sequence and the historical velocity sequence; performing state prediction on the historical position sequence and the historical velocity sequence based on the historical trajectory smoothing constraints to obtain the predicted position at the current moment; obtaining the estimated position at the current moment through dead reckoning based on the first inertial data and the second inertial data; determining the deviation between the predicted position and the estimated position, and mapping the deviation to the historical trajectory consistency confidence level.

[0104] Here, "historical value" refers to data about the motion state of the target to be located that the system has calculated or recorded before the current moment. The historical position sequence is a set of the target's positions at different historical moments, typically represented as a series of coordinate points, such as three-dimensional coordinates X, Y, and Z. The historical velocity sequence is a set of the target's velocities at different historical moments, typically represented as a series of velocity vectors, such as Vx, Vy, and Vz. This historical data provides the basis for evaluating the continuity and smoothness of the trajectory and is a necessary input for state prediction and consistency checks. In practical applications, the position and velocity information obtained from each positioning calculation can be stored in a circular buffer or database and sorted and retrieved according to timestamps to form a sequence. Alternatively, the historical position and velocity components can be directly extracted by maintaining historical estimates of the state vectors in the positioning filter (such as a Kalman filter or particle filter).

[0105] The steps of obtaining historical positioning results based on the first and second inertial data through dead reckoning, and obtaining historical position and velocity sequences, aim to obtain a relatively reliable historical trajectory benchmark. By fusing the first and second inertial data for dead reckoning, the complementarity of the two IMUs can be utilized to suppress the error accumulation of a single IMU and improve the accuracy of historical positioning results. In specific implementation, a dead reckoning algorithm based on dual IMU fusion can be used. For example, by pre-integrating the acceleration and angular velocity data of the first and second IMUs and combining them with relative attitude estimation, the position and velocity of the target to be located can be jointly calculated. More specifically, the relative motion information of the first IMU (fixed at the UWB tag) and the second IMU (fixed at the target to be located) can be used, and techniques such as lever arm compensation can be used to unify the measurements of the two IMUs to the same reference frame before dead reckoning. Another implementation method is to perform dead reckoning mainly based on the second IMU data when the lever arm change detection result indicates that the lever arm has not changed. When the lever arm changes, the lever arm effect compensation of the second IMU data is performed by using the relative attitude change and differential acceleration characteristics of the first and second IMUs, and then dead reckoning is performed to obtain a more accurate historical positioning result.

[0106] Based on the historical trajectory smoothing constraint, state prediction is performed on the historical position sequence and the historical velocity sequence to obtain the predicted position at the current moment. This historical trajectory smoothing constraint refers to the restriction imposed on the continuity, smoothness, and physical reachability of the target's trajectory. It reflects that the target's motion state will not undergo drastic or physically incompatible changes within a short period. State prediction uses known historical motion states and motion models to infer the target's motion state at a future moment (the current moment). This step, by introducing a smoothing constraint, allows the predicted position to better reflect the target's true motion trend, suppressing potential noise and instantaneous errors in historical data, and improving the robustness of the prediction. For example, kinematic model-based prediction methods can be used, such as uniform linear motion models, uniformly accelerated motion models, or more complex state estimation algorithms like Kalman filters and extended Kalman filters (EKF). The historical position sequence and historical velocity sequence are used as input, combined with a preset motion model and system noise covariance, to predict the position at the current moment. Alternatively, data-driven methods based on spline interpolation or Gaussian process regression can be used to fit and extrapolate the historical trajectory to obtain the predicted position at the current moment, while considering the smoothness of the trajectory.

[0107] Based on the first and second inertial data, the steps of obtaining the estimated position at the current moment through dead reckoning provide a current position directly calculated from real-time inertial data as a benchmark for comparison with the predicted position. By utilizing dual IMU data again for dead reckoning, the accuracy of the estimated position and the consistency with the calculation method of historical positioning results are ensured, enhancing the effectiveness of the comparison. For example, a dual IMU fusion dead reckoning algorithm similar to that used to obtain historical positioning results can be employed to process the first and second inertial data at the current moment in real time, directly calculating the estimated position at the current moment through steps such as pre-integration, attitude calculation, and position update. Alternatively, an IMU-based inertial navigation system (INS) module can be used, which receives the first and second inertial data and combines them with a lever compensation model to output a real-time position estimate for the current moment.

[0108] The process involves determining the deviation between the predicted and estimated positions and mapping this deviation to a confidence level for historical trajectory consistency. The deviation refers to the spatial distance or difference between the predicted and estimated positions. Mapping converts this deviation into a numerical value representing the level of confidence, typically ranging from 0 to 1. A higher value indicates higher consistency and confidence. This step quantifies the difference between prediction and estimation, intuitively reflecting the degree of conformity between the current trajectory and the smoothing constraints of historical trajectories, thus generating an objective and quantifiable confidence level for historical trajectory consistency. For example, the Euclidean distance between the predicted and estimated positions can be calculated as the deviation, and then mapped to a 0-1 confidence level range using a pre-defined nonlinear function (such as the Sigmoid function, Gaussian function, or piecewise linear function). Smaller distances indicate higher confidence. When the distance exceeds a certain threshold, the confidence level drops rapidly. Another approach is to consider the covariance information of the predicted and estimated positions, calculating the Mahalanobis distance between them as the deviation, and then similarly converting it to a confidence level using a mapping function.

[0109] The above technical solutions address the problem of inaccurate confidence assessments of historical positioning results due to accumulated inertial navigation errors or interference from the dynamic downhole environment. For example, by acquiring historical positioning results based on dead reckoning using dual IMU data, a more reliable and accurate historical trajectory benchmark is provided for subsequent trajectory consistency assessments, suppressing the accumulated errors of single inertial navigation. Introducing historical trajectory smoothing constraints for state prediction of historical position and velocity sequences effectively filters out instantaneous noise and abnormal fluctuations in historical data, making the predicted position more consistent with the target's actual motion. By again calculating the estimated position based on dual IMU data and comparing it with the predicted position, the degree of conformity between the current motion state and the historical trajectory smoothing constraints can be quantified in real time and dynamically. Mapping the deviation between the predicted and estimated positions to historical trajectory consistency confidence makes the assessment of historical trajectory consistency more objective, accurate, and robust. This accurate confidence assessment provides high-quality input for subsequent multi-dimensional confidence fusion, thereby improving the reliability and accuracy of overall fused positioning, especially in complex dynamic downhole environments, better addressing the challenges posed by dynamic changes in the boom arm.

[0110] In some of the embodiments described above in this application, a method for determining historical positioning results is proposed to provide historical positioning results for calculating the confidence level of historical trajectory consistency. However, in its implementation, if the lever arm changes and lever arm effect compensation is not performed, the historical positioning results will be inaccurate, thereby affecting the reliability of confidence level assessment and overall fusion positioning accuracy.

[0111] In this regard, this application further proposes a method for determining historical positioning results, including: If the stick arm change detection result indicates that the stick arm has not changed, dead reckoning is performed based on the second acceleration data and the second angular velocity data in the second inertial data to obtain the historical positioning result.

[0112] When the stick arm change detection result indicates a change in the stick arm, the relative attitude change between the first IMU and the second IMU is determined using the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data. The stick arm effect is compensated using the relative attitude change and the differential acceleration characteristics of the second acceleration data in the second inertial data. Dead reckoning is then performed based on the compensated second acceleration data to obtain the historical positioning result.

[0113] For example, the lever change detection result is a signal or state variable used to indicate whether the lever between the first and second IMUs has changed. It can be generated based on a sliding window statistical analysis of differential acceleration characteristics. When the detection result indicates that the lever has not changed, it means that the relative positional relationship between the first and second IMUs remains stable. In this case, dead reckoning can be performed directly based on the second acceleration and second angular velocity data collected by the second IMU fixed at the target location. Dead reckoning is a navigation technique that estimates position, velocity, and attitude by integrating motion sensor data. For example, an extended Kalman filter (EKF) or an unscented Kalman filter (UKF) can be used, with the second acceleration and second angular velocity data as observation inputs, combined with a motion model to iteratively estimate the target's position, velocity, and attitude. Another implementation is to use quaternions to represent attitude, update the attitude by integrating the second angular velocity data, and then transform the second acceleration data from the IMU coordinate system to the navigation coordinate system and perform two integrations to obtain velocity and position. In this way, historical positioning results can be obtained efficiently under stable lever conditions.

[0114] When the stick change detection result indicates a change in the stick arm, it means that the relative positional relationship between the first and second IMUs is no longer fixed, possibly due to changes in personnel attitude, equipment vibration, or loose installation. In this case, directly using the second IMU data for dead reckoning will lead to errors. Therefore, it is necessary to use the first angular velocity data from the first inertial data and the second angular velocity data from the second inertial data to determine the relative attitude change between the first and second IMUs. This can be achieved by comparing the angular velocity vectors of the two IMUs, for example, by calculating the rotation matrix or quaternion between the two angular velocity vectors to represent the relative attitude. Alternatively, the angular velocity data of the first and second IMUs can be integrated separately to obtain their respective attitude estimates, and then the relative attitude change can be calculated by comparing these two attitude estimates.

[0115] After determining the relative attitude change, the lever arm effect is compensated for in the second acceleration data of the second inertial data using this relative attitude change and the differential acceleration characteristics. The purpose of lever arm effect compensation is to eliminate additional acceleration components (such as centripetal and tangential acceleration) introduced into the second IMU acceleration measurement due to dynamic changes in the lever arm (e.g., rotation). For example, a kinematic model can be built to describe the motion of the lever arm based on the known relative attitude change and differential acceleration characteristics, and the additional acceleration components caused by the lever arm motion can be calculated and then subtracted from the second acceleration data. Another compensation method is to model the lever arm effect compensation problem as an optimization or filtering problem. In a factor graph or Kalman filter framework, the relative attitude change and differential acceleration characteristics are used as observations to estimate and compensate for the lever arm effect in the second acceleration data. After compensation, the second acceleration data will more accurately reflect the actual translational motion of the target to be located. Dead reckoning is then performed based on the compensated second acceleration data to obtain more accurate historical positioning results. The dead reckoning method is similar to that when the lever arm remains unchanged, but the input data has been corrected for the lever arm effect.

[0116] The above technical solution enables the adaptive selection of different historical positioning results to determine strategies based on whether the boom arm changes. When the boom arm remains unchanged, dead reckoning is performed directly using the second IMU data, simplifying the calculation process and maintaining efficiency. When the boom arm changes, the relative attitude change between the first and second IMUs is accurately determined, and boom arm effect compensation is performed on the second acceleration data using differential acceleration characteristics. This effectively eliminates errors introduced by dynamic boom arm changes, ensuring the accuracy of the data used for dead reckoning. This dynamic adjustment strategy improves the reliability and accuracy of historical positioning results, providing more accurate input for subsequent multi-dimensional confidence assessments such as IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence. This, in turn, enhances the robustness of the entire fusion positioning system and its positioning accuracy in complex dynamic downhole environments.

[0117] In some of the embodiments described above in this application, the results of lever arm change detection are proposed for confidence assessment. However, in its implementation, when lever arm change is triggered, there is a lack of an effective mechanism to reinitialize the fusion process and handle conflicts between multiple confidence levels, which may result in inaccurate or invalid comprehensive confidence assessment, affecting the reliability of subsequent positioning.

[0118] In response, this application further proposes a method for fusion confidence assessment, which uses the lever arm change detection result as the trigger condition for confidence assessment, and fuses the IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence to obtain a comprehensive confidence and fusion state diagnosis result.

[0119] For example, the method includes the following steps: When the lever arm change detection result triggers a confidence reinitialization, a framework for fusing the IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence is established. The lever arm change detection result serves as a key triggering mechanism, enabling a timely reassessment of the confidence levels of each sensor input when the relative position or attitude (i.e., the lever arm) between the UWB tag and the target changes. This triggering can be based on a preset threshold, such as when the lever arm change amplitude exceeds a certain set value. Alternatively, it can be based on a dynamic judgment of the estimated lever arm change trend, such as when the trend is sustained. This approach ensures that the subsequent confidence fusion process is not based on outdated or inaccurate confidence information. Once reinitialization is triggered, the IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence calculated at the current moment are aggregated as independent input data streams into a specially designed confidence fusion framework. This input process can be achieved by encapsulating these confidence values ​​into a unified data structure (e.g., a vector or list containing all confidence values) and passing it to the interface function of the fusion framework, or by sending these confidence information to the fusion processing module through a message queue in a distributed system.

[0120] In this confidence fusion framework, basic probability assignments are performed on each confidence level to obtain the corresponding basic probability assignment value. Basic probability assignment is a crucial step in confidence fusion, aiming to unify confidence information from different sources and with different dimensions into a standardized probabilistic representation. For example, the IMU motion consistency confidence level (which might be a similarity score between 0 and 1) can be converted into basic probability assignment values ​​for the propositions "IMU data is reliable" and "IMU data is unreliable" through a predefined mapping function. Similarly, the UWB signal quality confidence level, downhole positioning geometric constraint confidence level, and historical trajectory consistency confidence level are also converted into corresponding basic probability assignment values ​​through their respective mapping rules or statistical models. These assignment values ​​quantify the degree to which each confidence source supports specific hypotheses (such as data validity, environmental stability, etc.), laying the foundation for subsequent fusion calculations.

[0121] The comprehensive confidence level is obtained by iteratively synthesizing the basic probability assignment values. Iterative synthesis is the core step in achieving effective fusion of multi-source confidence information. This process combines the independent basic probability assignment values ​​through a gradual accumulation and update. For example, the combination rule in Dempster-Shafer evidence theory can be used to initially fuse the basic probability assignment values ​​corresponding to the IMU motion consistency confidence level and the UWB signal quality confidence level, obtaining an intermediate fusion result. This intermediate fusion result is then used as prior information and fused with the basic probability assignment values ​​corresponding to the downhole positioning geometric constraint confidence level, and so on, until the basic probability assignment values ​​corresponding to all confidence levels are included in the fusion calculation. During each iterative fusion process, the fusion weights can be dynamically adjusted based on the difference between the current fusion result and the new input confidence level to more effectively handle information conflicts and uncertainties, generating a comprehensive confidence level that fully reflects the overall reliability of the system.

[0122] Based on the degree of conflict between each confidence level and the convergence state of the overall confidence level, a fusion state diagnostic result is generated. This fusion state diagnostic result characterizes the effectiveness of the current confidence level assessment. To ensure the reliability of the overall confidence level, this application introduces a fusion state diagnostic mechanism. This mechanism assesses whether there are conflicts between the various confidence level sources, for example, by calculating the difference or inconsistency coefficient between the IMU motion consistency confidence level and the UWB signal quality confidence level. The system also monitors the change sequence of the overall confidence level during the iterative synthesis process to determine whether it has reached a stable state (i.e., convergence). For example, if the overall confidence level changes only slightly in several consecutive iterations, it is considered to have converged. Conversely, if the fluctuations are drastic, it may not have converged. Taking into account both the degree of conflict between each confidence level and the convergence state of the overall confidence level, the system generates a fusion state diagnostic result. This diagnostic result can be a discrete label (such as "Evaluation Effective", "Data Conflict", "Fusion Unstable") or a continuous validity index, used to clearly indicate the reliability level of the current overall confidence level assessment.

[0123] The above technical solution addresses the lack of an effective re-initialization mechanism and conflict handling capability in confidence assessment during dynamic changes in the lever arm. When the lever arm change detection triggers confidence re-initialization, all relevant confidence information can be promptly and proactively input into the fusion framework, avoiding the use of outdated or inaccurate confidence data for subsequent processing. By performing basic probability assignment on each confidence level, heterogeneous confidence information is uniformly quantified, providing a unified mathematical foundation for subsequent iterative synthesis. The iterative synthesis process effectively integrates multi-source information, generating a more stable and reliable comprehensive confidence level while handling uncertainties and conflicts. More importantly, by evaluating the degree of conflict between each confidence level and the convergence state of the comprehensive confidence level, this application can generate fusion state diagnostic results, thereby self-diagnosing the effectiveness of the current confidence assessment. This diagnostic result provides crucial feedback information for the subsequent positioning system, enabling adaptive adjustments to the positioning strategy based on the effectiveness of the assessment, such as reducing reliance on certain sensor data or triggering further calibration when the assessment is ineffective. This dynamic and adaptive confidence assessment and diagnostic mechanism improves the accuracy and reliability of positioning results in complex and dynamic downhole environments. Especially in scenarios where the boom changes frequently, it can effectively avoid the accumulation of positioning errors caused by inaccurate confidence assessment, thereby ensuring the accuracy of the target position information and the time-varying boom vector estimation value.

[0124] In some of the embodiments described above in this application, a basic probability allocation is proposed for each confidence level to fuse the confidence levels. However, in this process, since the difference between confidence levels is not considered, the basic probability allocation may be inaccurate, affecting the reliability of the fusion result.

[0125] To address this, this application further proposes a method for optimizing basic probability allocation, aiming to improve the accuracy and adaptability of confidence fusion. The method includes: determining an initial allocation value corresponding to the IMU motion consistency confidence level based on the IMU motion consistency confidence level; determining an initial allocation value corresponding to the UWB signal quality confidence level based on the UWB signal quality confidence level; determining an initial allocation value corresponding to the downhole positioning geometric constraint confidence level based on the downhole positioning geometric constraint confidence level; determining an initial allocation value corresponding to the historical trajectory consistency confidence level based on the historical trajectory consistency confidence level; determining a dynamic correction coefficient corresponding to each confidence level based on the degree of difference between each confidence level; and multiplying the initial allocation value corresponding to each confidence level by the corresponding dynamic correction coefficient to obtain the basic probability allocation value corresponding to each confidence level.

[0126] For example, when determining the initial assignment value corresponding to each confidence level, the initial assignment value is determined based on the IMU motion consistency confidence level value. This step aims to assign a basic weight to the IMU in confidence level fusion based on the degree of consistency of motion data between the first and second IMUs. For instance, the IMU motion consistency confidence level value can be directly converted into the initial assignment value using a linear mapping function, meaning the initial assignment value is directly proportional to the confidence level value. Alternatively, a non-linear function, such as the sigmoid function or an exponential function, can be used to make the initial assignment value more sensitive to changes within a specific confidence level range, thus highlighting its importance.

[0127] Based on the numerical value of the UWB signal quality confidence score, an initial allocation value corresponding to that confidence score is determined. The purpose of this is to quantify the initial contribution of the UWB signal reliability to the overall confidence assessment. Specifically, a series of UWB signal quality thresholds can be set, and the corresponding initial allocation value can be determined using a piecewise linear function or a lookup table method based on the interval into which the UWB signal quality confidence score falls. Alternatively, the deviation between the UWB signal quality confidence score and a preset ideal value can be mapped using a Gaussian function with equal probability density function to reflect the rationality of the initial allocation.

[0128] Based on the confidence level of the downhole positioning geometric constraints, an initial allocation value corresponding to this confidence level is determined. This initial allocation reflects the fundamental impact of the downhole base station's geometric layout on positioning accuracy. One approach is to inversely map the confidence level of the downhole positioning geometric constraints to the initial allocation value; that is, the better the geometric constraint (higher confidence level), the larger the initial allocation value. Another approach is to use a step function or a piecewise constant function, based on the range of the geometric precision factor (e.g., GDOP), to map different geometric constraint levels to different initial allocation values.

[0129] Furthermore, based on the numerical value of the historical trajectory consistency confidence level, an initial assignment value corresponding to this historical trajectory consistency confidence level is determined. This step aims to incorporate the smoothness and continuity of the historical trajectory as prior information into the confidence level assessment. For example, a Gaussian function can be designed to assign a higher initial assignment value when the historical trajectory consistency confidence level is close to the ideal value, and to gradually decrease the initial assignment value when it deviates from the ideal value. Alternatively, fuzzy logic rules can be used to divide the historical trajectory consistency confidence level into multiple levels such as "high," "medium," and "low," and a corresponding initial assignment value can be preset for each level.

[0130] After determining the initial assignment values ​​for each confidence level, this application further determines the dynamic correction coefficient corresponding to each confidence level based on the degree of difference between them. The core of this step is to capture inconsistencies or conflicts between different confidence level sources and make dynamic adjustments accordingly. One implementation method is to calculate the absolute or relative difference between any two confidence levels (e.g., IMU motion consistency confidence level and UWB signal quality confidence level), and then map this difference to a correction coefficient. The larger the difference, the smaller the correction coefficient may be, and vice versa. Another implementation method is to calculate the variance or standard deviation of all confidence level values, and then dynamically generate the corresponding correction coefficient based on the degree of deviation of each confidence level value from the mean, so as to reflect its relative reliability in the overall confidence level set.

[0131] Multiplying the initial allocation value corresponding to each confidence level by the corresponding dynamic adjustment coefficient yields the basic probability allocation value for each confidence level. This is a crucial step in combining basic assessment with dynamic adjustment. Typically, this step is achieved through direct multiplication, where the basic probability allocation value equals the initial allocation value multiplied by the dynamic adjustment coefficient. In some cases, additional weighting factors can be introduced to weight the dynamic adjustment coefficient itself, further refining the adjustment effect.

[0132] The above technical solution addresses the problem of inaccurate basic probability allocation during confidence fusion due to insufficient consideration of the differences between confidence levels. For example, by determining initial allocation values ​​for IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence, this application provides a quantitative basis for assessing the reliability of each information source. Furthermore, by introducing a dynamic correction coefficient based on the differences between confidence levels, the initial allocation value can be adaptively adjusted when inconsistencies or conflicts exist between different confidence sources, thus avoiding deviations that may arise from static allocation. Multiplying the initial allocation value by the dynamic correction coefficient yields the basic probability allocation value corresponding to each confidence level, which not only includes the inherent reliability of each information source but also reflects the degree of coordination or conflict between them. This dynamic adjustment mechanism improves the accuracy and robustness of the basic probability allocation, enabling more accurate and reliable comprehensive confidence and fusion state diagnostic results when iteratively synthesizing these basic probability allocation values ​​within the confidence fusion framework. This is crucial for dynamically adjusting the weights of the UWB factor and the inertial factor in the factor diagram based on the comprehensive confidence level, and for jointly optimizing the UWB ranging data, the first inertial data, and the second inertial data to obtain more accurate target position information and time-varying lever vector estimates, ensuring high accuracy and high reliability of positioning results in complex dynamic environments downhole.

[0133] In some of the solutions mentioned above in this application, multiple confidence levels are fused to obtain a comprehensive confidence level. However, in this process, due to the lack of a dynamic adjustment mechanism, when the input data fluctuates due to changes in the downhole environment (such as lever effect or signal interference), the fusion result may not be accurate or stable enough, and cannot effectively reflect the real-time reliability of multi-source information, leading to inaccurate weight allocation in subsequent positioning optimization.

[0134] To address this, this application further proposes an iterative synthesis of the basic probability assignment values ​​to obtain a comprehensive confidence level. Specifically, this involves: using the basic probability assignment values ​​corresponding to the IMU motion consistency confidence level and the UWB signal quality confidence level as initial inputs, performing a first-round fusion through an iterative fusion function to obtain an intermediate fusion result. This intermediate fusion result is then used as prior information for the next round of fusion, and iteratively fused sequentially with the basic probability assignment values ​​corresponding to the downhole positioning geometric constraint confidence level and the historical trajectory consistency confidence level. In each round of iterative fusion, the fusion weights are dynamically adjusted based on the difference between the current round's input data and the previous round's fusion result. Once the basic probability assignment values ​​corresponding to all confidence levels have been fused, the fusion result is used as the comprehensive confidence level.

[0135] For example, the basic probability allocation values ​​corresponding to the IMU motion consistency confidence score and the UWB signal quality confidence score are used as initial inputs. An iterative fusion function is used for the first round of fusion to obtain intermediate fusion results. The basic probability allocation value corresponding to the IMU motion consistency confidence score is determined based on the first and second inertial data, and then obtained through a basic probability allocation method. It quantifies the degree of consistency between the motion data of the first and second IMUs. The basic probability allocation value corresponding to the UWB signal quality confidence score is determined based on the UWB ranging data and the geometric layout information of the underground base station, and then obtained through a basic probability allocation method. It reflects the reliability of the UWB signal. The iterative fusion function can be implemented using various methods such as the combination rules of Dempster-Shafer evidence theory, Bayesian fusion algorithms, or weighted averaging. Its function is to mathematically combine two or more confidence scores to obtain a more comprehensive evaluation. The purpose of the first round of fusion is to prioritize the processing of core sensor data, ensure the reliability of the basic fusion results, and provide a stable starting point for subsequent steps. The intermediate fusion result is the output of the first round of fusion, representing the comprehensive evaluation after partial confidence fusion.

[0136] The intermediate fusion results are used as prior information for the next round of fusion, and are iteratively fused sequentially with the basic probability allocation values ​​corresponding to the confidence levels of the downhole positioning geometric constraints and the historical trajectory consistency confidence levels. Prior information plays a crucial role in iterative fusion; it is knowledge about the system state or confidence levels obtained before the current fusion step, guiding subsequent fusion processes and making them more accurate and stable. The basic probability allocation value corresponding to the confidence level of the downhole positioning geometric constraints is determined based on UWB ranging data and the geometric layout information of the downhole base stations, and then obtained through the basic probability allocation method. It reflects the impact of the UWB base station's geometric layout on positioning accuracy. The basic probability allocation value corresponding to the confidence level of historical trajectory consistency is determined based on historical trajectory smoothness constraints and historical positioning results, and then obtained through the basic probability allocation method. It quantifies the degree of conformity between the current positioning result and the historical trajectory smoothness. This iterative fusion approach helps to gradually accumulate information, avoid information conflicts, and improve fusion efficiency.

[0137] Furthermore, in each iteration of fusion, the fusion weights are dynamically adjusted based on the difference between the current round's input data and the previous round's fusion result. The difference can be measured by calculating metrics such as the Euclidean distance, KL divergence, and information entropy difference between the current round's input data (e.g., a basic probability assignment value to be fused) and the previous round's fusion result (e.g., an intermediate fusion result). The fusion weights determine the proportion of each input confidence level in the fusion result. Dynamic adjustment means that these weights are not fixed but change in real-time according to the degree of matching between the input data and the existing fusion results. For example, if a certain input confidence level differs significantly from the existing fusion results, it may mean that the current reliability of that confidence level is low, and its weight can be reduced accordingly; conversely, it can be increased. This can be achieved through methods such as fuzzy logic, adaptive Kalman gain, or information entropy-based weight allocation.

[0138] When the basic probability assignment values ​​corresponding to all confidence levels have been fused, the fusion result is used as the comprehensive confidence level. This comprehensive confidence level is the output after iterative fusion of all relevant confidence levels, and it comprehensively and dynamically reflects the overall reliability of the multi-source information of the current positioning system.

[0139] The above technical solution addresses the lack of a dynamic adjustment mechanism in the fusion process by dynamically generating a comprehensive confidence score through iterative synthesis of basic probability allocation values. Specifically, the first round of fusion prioritizes core sensor data, ensuring the reliability of the basic fusion result and providing a stable starting point for subsequent steps. Using this intermediate result as prior information, the basic probability allocation values ​​corresponding to downhole positioning geometric constraint confidence and historical trajectory consistency confidence are fused sequentially. Ordered iteration avoids information conflicts and improves fusion efficiency. Crucially, in each iteration, the fusion weights are dynamically adjusted based on the difference between the current round's input data and the previous round's fusion result. This allows the fusion process to adapt to data fluctuations, effectively reducing noise impact and enhancing the robustness of the fusion result. After all confidence scores are fused, a comprehensive confidence score is output, ensuring comprehensive coverage of multi-source information and providing a high-precision, high-reliability basis for subsequent positioning optimization. This effectively avoids inaccurate weight allocation in positioning optimization caused by inaccurate or unstable fusion results.

[0140] In some of the solutions mentioned above in this application, multiple confidence levels are fused based on the lever arm change detection results as trigger conditions to generate a comprehensive confidence level and a fusion state diagnosis result, which is used to evaluate the effectiveness of multi-source information fusion. However, in this process, due to the lack of quantitative evaluation of the degree of conflict between confidence levels and dynamic monitoring of the convergence state of the comprehensive confidence level, the fusion state diagnosis may be inaccurate or unstable, and may not be able to reliably reflect the reliability of the current fusion process, thereby affecting the accuracy of subsequent positioning weight adjustment.

[0141] In response, this application further proposes to generate a diagnostic result for the fusion state based on the degree of conflict between each confidence level and the convergence state of the overall confidence level, including: Calculate the pairwise differences between the IMU motion consistency confidence, the UWB signal quality confidence, the downhole positioning geometric constraint confidence, and the historical trajectory consistency confidence.

[0142] Based on the degree of difference between each pair, the degree of conflict between each confidence level is determined.

[0143] Obtain the sequence of changes in the overall confidence level during the iterative synthesis process within the confidence fusion framework.

[0144] Based on this change sequence, the convergence state of the comprehensive confidence level is determined.

[0145] The degree of conflict and the convergence state are mapped to the set of diagnostic results to obtain the diagnostic results of the fusion state.

[0146] For example, when generating the fusion state diagnostic results, it is necessary to calculate the pairwise differences between the IMU motion consistency confidence score, the UWB signal quality confidence score, the downhole positioning geometric constraint confidence score, and the historical trajectory consistency confidence score. This step aims to quantify the degree of consistency or inconsistency between different confidence score sources. In multi-sensor fusion systems, the confidence scores provided by various sensors or data sources may have biases or even conflicts, and direct fusion may lead to inaccurate results. By calculating the pairwise differences, it is possible to identify which confidence scores have significant discrepancies, providing basic data for subsequent assessment of the degree of conflict. For example, mathematical metrics such as Euclidean distance, Manhattan distance, or cosine similarity can be used to calculate the differences between pairwise confidence score values. For two confidence score values ​​C1 and C2, the difference can be defined as |C1-C2| or (C1-C2)^2. Alternatively, the confidence score values ​​can be discretized by setting thresholds, for example, dividing them into three levels: "high," "medium," and "low," and then the difference can be determined by comparing the differences between the levels; for example, the greater the difference between the levels, the higher the difference.

[0147] Based on the pairwise differences, the degree of conflict between each confidence level is determined. This step transforms the specific difference measure calculated in the previous step into a more interpretable "conflict level" index. The degree of conflict reflects the intensity of inconsistency in the judgments of different information sources regarding the current positioning status and is key to assessing the reliability of fusion. For example, all pairwise differences can be weighted and averaged or the maximum value can be taken to obtain a comprehensive conflict level index. The weights can be preset according to the importance or reliability of different confidence sources. If the difference between the IMU motion consistency confidence level and the UWB signal quality confidence level is large, the conflict level is high. Another approach is to use fuzzy logic or evidence theory (such as Dempster-Shafer theory) to handle these differences. Using the differences as evidence, the conflict factor between each confidence level set is calculated through rule reasoning or combination functions, thereby obtaining a quantified degree of conflict.

[0148] The step of obtaining the change sequence of the overall confidence score during the iterative synthesis process within the confidence fusion framework aims to dynamically monitor the evolution trajectory of the overall confidence score during the fusion process. The overall confidence score is not calculated all at once, but is gradually formed by iteratively fusing multiple basic confidence scores. Obtaining its change sequence can reveal the stability of the fusion process, the convergence speed, and whether there are oscillations or divergences, providing temporal information for judging the reliability of the fusion result. During iterative synthesis within the confidence fusion framework (e.g., based on Dempster-Shafer evidence theory or Bayesian networks), the overall confidence score value at the current moment is recorded after each iteration. These overall confidence score values ​​arranged in chronological order constitute a time series. Alternatively, in each iteration, not only the overall confidence score value can be recorded, but also its intermediate state values ​​during the iteration process, forming a more detailed change sequence for more accurate analysis of its convergence characteristics.

[0149] Based on the change sequence, the convergence state of the overall confidence score is determined. This step analyzes the change sequence of the overall confidence score to determine whether it has reached a stable state or is approaching a stable value. The convergence state is an important indicator for evaluating the effectiveness of the fusion result. A non-convergent or unstable overall confidence score may indicate a problem in the fusion process. For example, the difference or rate of change between adjacent elements in the change sequence can be calculated and compared with a preset convergence threshold. If the rate of change is less than the threshold for N consecutive time points, the overall confidence score is considered to have converged. Statistical methods, such as moving averages and exponential smoothing, can also be used to process the change sequence and observe its trend. Alternatively, time series analysis models (such as the ARIMA model) can be used to predict its future trend, thereby determining its convergence.

[0150] Mapping the conflict level and convergence state to the diagnostic result set yields the fusion state diagnostic result. This step is crucial for generating the fusion state diagnostic result, as it integrates the two core indicators—conflict level and convergence state—obtained through prior quantification, forming a clear diagnostic conclusion. This diagnostic result directly guides subsequent adjustments to the localization strategy, such as factor weight allocation. A pre-defined diagnostic rule table or decision tree can be used. For example, if the conflict level is low and the convergence state is good, the diagnostic result is "Effective Fusion." If the conflict level is high but the convergence state is good, the diagnostic result is "Local Conflict Exists, but Fusion is Stable." If the conflict level is high and the convergence state is poor, the diagnostic result is "Abnormal Fusion, Reassessment Required." Alternatively, a machine learning classifier (such as a support vector machine or neural network) can be used for training. Using the conflict level and convergence state as input features and the pre-defined diagnostic result as a label, the model is trained to automatically map new conflict levels and convergence states to the corresponding diagnostic result set. The diagnostic result set can include different levels such as "Normal," "Warning," and "Abnormal."

[0151] By introducing a quantitative assessment of the conflict degree between various confidence levels and dynamic monitoring of the convergence state of the comprehensive confidence level, the aforementioned technical solution addresses the problems of inaccurate or unstable fusion state diagnosis in related technologies. For example, by calculating the pairwise differences between IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence, inconsistencies between different information sources can be comprehensively and meticulously captured, avoiding information loss or misjudgment that may result from simple fusion. Based on these differences, the conflict degree between each confidence level is further determined, providing clear quantitative indicators for potential discrepancies during the fusion process. By acquiring the change sequence of the comprehensive confidence level during the iterative synthesis process within the fusion framework and determining its convergence state based on this sequence, dynamic and real-time monitoring of the fusion process is achieved, ensuring the stability and reliability of the fusion results. Mapping the quantified conflict degree and the dynamically monitored convergence state to a preset diagnostic result set generates accurate and reliable fusion state diagnostic results. This diagnostic result accurately characterizes the effectiveness of the current multi-source information fusion, providing a solid and reliable basis for the dynamic adjustment of the weights of UWB factors and inertia factors in the subsequent factor graph. For example, when the diagnostic result indicates a good fusion status, the current weight allocation can be maintained or optimized. Conversely, when the diagnostic result indicates high conflict or poor convergence, the weights can be adjusted in a timely manner, such as reducing the weight of data sources with significant conflicts or triggering reinitialization. This improves the adaptability and positioning accuracy of the downhole fusion positioning system, especially in complex and variable downhole environments, enabling it to more effectively cope with challenges such as UWB signal quality fluctuations and IMU data drift, ensuring the output of highly reliable target location information.

[0152] In some of the embodiments described above in this application, the weights of the UWB factor and the inertia factor in the factor graph are dynamically adjusted based on the comprehensive confidence level to perform joint optimization. However, in this process, the weight adjustment may lack the ability to respond to the dynamic environment in real time and cannot accurately adapt to the changes in the lever arm, resulting in unreliable positioning results.

[0153] To address this, this application further proposes a downhole fusion positioning method based on UWB and inertial navigation. The factor graph includes state nodes and observation nodes. The state nodes include the position node, velocity node, and time-varying lever vector node of the target to be located. The observation nodes include UWB ranging nodes, first IMU pre-integration nodes, and second IMU pre-integration nodes. The weights of the UWB factor and inertial factor in the factor graph are dynamically adjusted based on the comprehensive confidence level, and the UWB ranging data, first inertial data, and second inertial data are jointly optimized to obtain the target position information of the target to be located and the estimated value of the time-varying lever vector between the first IMU and the second IMU. For example, see... Figure 4 The method includes the following steps: 401. Based on the overall confidence level, determine the UWB factor weights corresponding to the UWB ranging node and the inertia factor weights corresponding to the first IMU pre-integration node and the second IMU pre-integration node, respectively.

[0154] 402. Based on the UWB ranging data, the first inertial data, and the second inertial data, construct a joint optimization objective function that includes the UWB factor weight and the inertial factor weight. The joint optimization objective function also includes a regularization term constructed based on the historical trajectory smoothing constraint.

[0155] 403. Iterate the solution of the joint optimization objective function within the sliding window to obtain the target position information and the estimated value of the time-varying lever arm vector.

[0156] To better understand the above technical solutions, the technical features involved will be described in detail below.

[0157] The factor graph is a probabilistic graphical model whose core lies in representing the conditional dependencies between random variables through nodes and edges. In this factor graph, variable nodes represent the state variables to be estimated, while factor nodes represent the observations or constraints on these state variables. For example, a state node can represent key state parameters such as the position and velocity of the target in three-dimensional space, and the time-varying lever vector between the first and second IMUs. An observation node corresponds to the distance observation information provided by UWB ranging data, the relative motion constraints provided by the pre-integrated data of the first IMU, and the relative motion constraints provided by the pre-integrated data of the second IMU. Through this modeling approach, the factor graph provides a unified and flexible framework that can effectively integrate data from different sensors and the complex relationships between various state variables into a graph structure, thus laying the foundation for subsequent joint optimization solutions. In another implementation, the state node can be further refined. For example, the position node can contain position information in the global coordinate system, the velocity node can contain velocity information of the target in the carrier coordinate system, the time-varying lever vector node represents the relative displacement between the first IMU and the second IMU, and the observation node can correspond to the UWB ranging value, the attitude and displacement change of the first IMU over a period of time (pre-integration), and the attitude and displacement change of the second IMU over a period of time (pre-integration).

[0158] Based on the overall confidence level, the UWB factor weights corresponding to the UWB ranging node and the inertial factor weights corresponding to the first and second IMU pre-integration nodes are determined respectively. This aims to dynamically adjust the contribution of different sensor data to joint optimization based on the multi-dimensional confidence level assessment results (i.e., the overall confidence level). When the confidence level of a sensor data source is high, its corresponding factor weight will increase accordingly, and vice versa, thus making the optimization process more inclined to adopt reliable data. In one implementation, the overall confidence level can be a value between 0 and 1, associated with the UWB factor weights and inertial factor weights through direct mapping or a preset mapping function. For example, the higher the overall confidence level, the larger the UWB factor weight, while the inertial factor weight may be adjusted according to the inverse or complementary relationship of the overall confidence level. In another implementation, a weighting function can be designed, taking the overall confidence level as input and outputting the UWB factor weights and inertial factor weights. This function can be non-linear, such as using a sigmoid function or a piecewise linear function, to better reflect the influence of confidence level on the weights. In addition, the rate of change of overall confidence can be considered to smooth or accelerate the adjustment of weights.

[0159] Based on the UWB ranging data, the first inertial data, and the second inertial data, a joint optimization objective function is constructed, incorporating the weights of the UWB factor and the inertial factor. This joint optimization objective function also includes a regularization term based on the historical trajectory smoothing constraint, which is the core component of the joint optimization solution. By constructing a comprehensive objective function, UWB ranging observations, IMU motion constraints, and historical trajectory smoothing constraints are integrated, and dynamically adjusted weights are used to balance the contributions of different data sources. Simultaneously, the regularization term suppresses unreasonable estimation results. In one implementation, the joint optimization objective function can be expressed as a weighted sum of squares of the residuals. The UWB factor residual term measures the difference between the distance calculated from the UWB ranging value and the current state estimate. The first IMU pre-integration factor residual term and the second IMU pre-integration factor residual term measure the difference between the IMU pre-integration value and the current state estimate. This regularization term can be constructed based on the smoothness or continuity of the historical trajectory; for example, penalizing the rate of change or second derivative of the time-varying lever arm vector to ensure the stationarity of the lever arm estimation. In another implementation, the objective function can employ a robust kernel function (such as the Huber kernel or Cauchy kernel) to handle possible outlier observations and reduce their impact on the optimization results. In addition to the smoothing constraint, the regularization term can also introduce sparsity constraints or prior distribution constraints to further improve the stability and accuracy of the estimation.

[0160] The joint optimization objective function is iteratively solved within a sliding window to obtain the target position information and the estimated value of the time-varying lever vector. The aim is to improve positioning accuracy and the robustness of lever estimation by using historical data over a period of time for joint optimization while maintaining computational efficiency. Iterative solving ensures that the objective function converges to the optimal solution. In one implementation, the sliding window typically contains a fixed number of state variables and observation data from the latest time. During each optimization, all state variables within the window (including position, velocity, lever vector, etc.) are optimized simultaneously. Iterative solving can employ nonlinear optimization methods such as the Levenberg-Marquardt algorithm and the Gauss-Newton algorithm. In another implementation, the size of the sliding window can be dynamically adjusted according to computational resources and positioning accuracy requirements. During the iterative solving process, a convergence threshold or a maximum number of iterations can be set to balance computational efficiency and optimization accuracy. The optimization result includes not only the target position information at the current time but also the estimated values ​​of state variables at all times within the window, as well as the estimated value of the time-varying lever vector.

[0161] The above technical solution introduces a factor graph model, using the position and velocity of the target to be located, as well as the time-varying lever arm vector between the first and second IMUs, as state nodes. UWB ranging data, first inertial data, and second inertial data are used as observation nodes. Based on the comprehensive confidence level obtained in the preceding steps, this solution dynamically adjusts the UWB factor weights corresponding to the UWB ranging nodes and the inertial factor weights corresponding to the IMU pre-integration nodes. This means that when the UWB signal quality is high and the IMU motion consistency is good, the corresponding weights will increase, and vice versa, thus enabling the optimization process to adopt more reliable data sources in real time. The joint optimization objective function also includes a regularization term constructed based on historical trajectory smoothing constraints. This regularization term can suppress abnormal jumps in the time-varying lever arm vector estimate, ensuring the stability of the lever arm estimate. By iteratively solving the constructed joint optimization objective function within a sliding window, the position information of the target to be located at the current moment and the accurate estimate of the time-varying lever arm vector between the first and second IMUs can be obtained efficiently. This dynamic weight adjustment and joint optimization mechanism solves the problems of inaccurate weight adjustment and unreliable positioning results caused by changes in the reliability of sensor data and real-time changes in the boom arm in dynamic downhole environments. Compared with fixed-weight or simple separate processing schemes, this scheme dynamically weights UWB and inertial data by integrating confidence levels, enabling real-time perception and response to changes in the reliability of different sensor data, thus prioritizing the use of high-quality data during the optimization process. The regularization term of the historical trajectory smoothing constraint further enhances the robustness and stability of the time-varying boom arm vector estimation, avoiding estimation jumps caused by rapid boom arm changes or data noise. Iterative solving within a sliding window ensures real-time performance and computational efficiency. Overall, this scheme improves the accuracy and reliability of positioning in complex dynamic downhole environments, especially in scenarios with frequent boom arm changes, continuously outputting high-precision target position information and time-varying boom arm vector estimates.

[0162] In some embodiments described above in this application, a method for dynamically adjusting the weights of UWB factors and inertial factors based on comprehensive confidence is proposed. However, in its implementation, how to more accurately combine comprehensive confidence and the historical rate of change of the time-varying lever arm vector to determine the weights in order to enhance adaptability and positioning accuracy is a problem that needs to be solved. To address this, this application further proposes a specific method for jointly optimizing the solution of UWB factors and inertial factors in the factor graph based on comprehensive confidence, and for jointly optimizing the UWB ranging data, the first inertial data, and the second inertial data. Specifically, this method for dynamically adjusting the weights of UWB factors and inertial factors in the factor graph based on comprehensive confidence determines the UWB factor weights corresponding to the UWB ranging nodes and the inertial factor weights corresponding to the first and second IMU pre-integration nodes, including the following steps: Obtain the historical rate of change of the overall confidence score and the estimated value of the time-varying lever arm vector. This step aims to provide dynamic, real-time input for subsequent weight calculations, reflecting the reliability of the current fusion state and the trend of lever arm changes. For example, the historical rate of change of the overall confidence score can be obtained by calculating the difference or percentage change between adjacent time points after applying a moving average or exponential smoothing to the historical overall confidence score sequence. The historical rate of change of the estimated value of the time-varying lever arm vector can be calculated by recording its historical estimated value sequence and performing differencing operations or using the slope of a fitted curve.

[0163] Based on this overall confidence level, the basic weight values ​​for the UWB factors are determined. This step establishes a fundamental correlation between the UWB factor weights and the overall system confidence level, ensuring that UWB data receives higher initial weights when reliability is high. For example, a monotonically increasing mapping function, such as a linear function, a sigmoid function, or a piecewise function, can be designed to map the overall confidence level to basic weight values ​​between 0 and 1; that is, the higher the overall confidence level, the larger the basic weight value. Alternatively, a lookup table can be pre-defined to directly assign corresponding basic weight values ​​based on different intervals of the overall confidence level.

[0164] Based on the historical rate of change of the estimated time-varying lever arm vector, a dynamic correction coefficient for the UWB factor weights is determined. This step introduces sensitivity to dynamic changes in the lever arm, enabling the UWB factor weights to adaptively respond to rapid changes in the lever arm and avoid positioning errors caused by these changes. For example, a function can be designed to map the historical rate of change of the estimated lever arm vector to a dynamic correction coefficient; when the rate of change is small, the correction coefficient is close to 1. When the rate of change is large, the correction coefficient may be less than 1 to reduce the weight of UWB data during periods of drastic lever arm changes. Another approach is to determine the correction coefficient based on the absolute value of the rate of change or its statistical characteristics (such as variance); the larger the rate of change, the smaller the correction coefficient.

[0165] The base weight value is multiplied by the dynamic correction coefficient to obtain the UWB factor weight. This step comprehensively considers the impact of overall system confidence and lever dynamic changes on the reliability of UWB data, thus forming the UWB factor weight. The most direct way is to perform a multiplication operation, that is, the UWB factor weight equals the base weight value multiplied by the dynamic correction coefficient.

[0166] The inertia factor weight is obtained by multiplying the preset inertia factor baseline weight by an inverse proportional function of the overall confidence level. This step achieves complementary adjustment of the UWB and inertial data weights: when the reliability of UWB data (reflected by the overall confidence level) decreases, the weight of the inertial data is increased, and vice versa. The inverse proportional function can be in the form of 1 divided by (overall confidence level plus a small positive number), or in the form of (1 minus the overall confidence level). For example, the inertia factor weight can be equal to the preset inertia factor baseline weight multiplied by (1 minus the overall confidence level) to the power of k, where k is a positive number used to adjust the steepness of the decrease.

[0167] The above technical solution provides a specific mechanism to accurately determine the weights of UWB factors and inertia factors. By combining comprehensive confidence and historical change rate, the adaptability problem in weight adjustment is solved. For example, obtaining the comprehensive confidence and the historical change rate of the estimated time-varying lever arm vector provides dynamic input for weight calculation. The comprehensive confidence assesses the overall reliability of multi-source information, while the historical change rate reflects the changing trend of the lever arm vector, ensuring that weight determination is based on real-time and historical context. The base weight value of the UWB factor is determined based on the comprehensive confidence, allowing the base value to directly reflect the confidence level of the current fusion state, avoiding the blind setting of weights. The dynamic correction coefficient of the UWB factor weight is determined based on the historical change rate of the estimated time-varying lever arm vector. The historical change rate captures the rate and direction of lever arm change, and the dynamic correction coefficient is adjusted accordingly, enabling the weight to adaptively respond to rapid changes in the lever arm and preventing positioning deviations caused by fixed weights. The UWB factor weight is obtained by multiplying the base weight value by the dynamic correction coefficient. By combining the base value and the correction coefficient through multiplication, the UWB factor weight considers both the overall confidence level and the dynamic characteristics of lever arm changes, thus improving the accuracy and robustness of weight adjustment. The inertial factor weight is obtained by multiplying the preset inertial factor baseline weight by an inverse proportional function of the overall confidence level. The design of the inverse proportional function ensures that the inertial weight is relatively reduced when the overall confidence level is high, and increased when it is low. This balances the influence of UWB and inertial data. At high confidence levels, UWB data is prioritized to utilize its ranging accuracy, while at low confidence levels, inertial data is relied upon to compensate for UWB signal deficiencies, thereby optimizing the accuracy of fused positioning.

[0168] In some of the solutions mentioned above in this application, a joint optimization objective function is proposed to optimize the position and lever vector. However, in this process, without an adaptive constraint mechanism, the optimization result may be unstable when the lever vector changes abruptly, affecting the positioning accuracy.

[0169] To address this challenge, this application further proposes a method for constructing a joint optimization objective function. This method, based on UWB ranging data, first inertial data, and second inertial data, constructs a joint optimization objective function that includes UWB factor weights and inertial factor weights. For example, the method includes the following steps: Based on UWB ranging data, a UWB factor residual term is constructed. UWB ranging data refers to distance information obtained by a UWB positioning system by measuring the propagation time of the UWB signal between the tag and the base station. This can be achieved through techniques such as one-way ranging (OWR) or two-way ranging (TWR). For example, in TWR mode, the UWB tag and the base station make two round trips, and the distance is calculated by accurately measuring the signal flight time, effectively eliminating clock synchronization errors. The UWB factor residual term represents the difference between the UWB ranging data and the predicted distance calculated based on the current estimated state (such as the UWB tag location and the base station location). For example, if the measured distance between the UWB tag and the base station is d_m, and the predicted distance calculated based on the current estimated location is d_p, then the UWB factor residual term can be expressed as (d_m - d_p). This residual term is typically normalized and may be squared for least-squares optimization.

[0170] Based on the first and second inertial data, first and second IMU pre-integration factor residual terms are constructed, respectively. The first and second inertial data refer to the raw inertial sensor data collected by the first IMU fixed at the UWB tag and the second IMU fixed at the target to be located, respectively. These data typically include triaxial acceleration and triaxial angular velocity data. For example, acceleration data reflects the linear acceleration of the IMU in space and can be used to estimate velocity and position. Angular velocity data reflects the rotational motion of the IMU and can be used to estimate attitude changes. The first and second IMU pre-integration factor residual terms represent the differences between the pre-integrated measurements (such as changes in position, velocity, and attitude) of the first and second IMUs over a period of time and the predicted changes calculated based on the current estimated state (such as the initial and current states of the IMU), respectively. The pre-integration technique integrates the raw IMU data over a period of time to obtain the relative motion within that time period, thus avoiding re-integrating the raw data in each optimization iteration. For example, pre-integrated residuals may include attitude error, velocity error, and position error, which are typically represented based on Lie groups or quaternions.

[0171] An adaptive regularization term is constructed based on the historical trajectory smoothing constraint and the historical rate of change of the estimated time-varying lever arm vector. This adaptive regularization term is used to strengthen the constraint strength when the estimated time-varying lever arm vector value jumps. The historical trajectory smoothing constraint is a constraint condition used to ensure the temporal continuity and smoothness of the estimated trajectory of the target to be located. This constraint aims to prevent drastic jumps or irregular jitters in the trajectory that do not conform to physical laws. For example, this can be achieved by imposing a penalty term on the acceleration or higher-order derivative of the trajectory, i.e., limiting the rate of change of acceleration or the curvature of the trajectory. The historical rate of change of the estimated time-varying lever arm vector describes how quickly the estimated lever arm vector value between the first IMU and the second IMU changes over a past period of time. This rate of change can serve as an indicator of whether the lever arm has changed drastically. For example, the instantaneous rate of change can be obtained by calculating the Euclidean distance between the current lever arm estimate and the previous lever arm estimate and dividing by the time interval. The adaptive regularization term is an additional term in the joint optimization objective function. Its role is to dynamically adjust the constraint strength on certain state variables (such as the time-varying lever arm vector) based on specific conditions (such as the historical rate of change of the estimated value of the time-varying lever arm vector). For example, when an abnormal jump in the lever arm vector is detected, the weight of this regularization term will increase, thereby penalizing the drastic change in the lever arm vector more strongly and making its estimate tend to be smoother.

[0172] The UWB factor residuals are weighted using UWB factor weights, and the residuals of the first and second IMU pre-integration factors are weighted using inertia factor weights. The weighted UWB factor residuals, the weighted first IMU pre-integration factor residuals, the weighted second IMU pre-integration factor residuals, and the adaptive regularization term are then summed to obtain the joint optimization objective function. The UWB factor weights and inertia factor weights are coefficients used in the joint optimization process to measure the reliability or importance of UWB ranging data and inertial data (including first and second IMU pre-integration). These weights determine the degree of influence of different types of data on the optimization results. For example, when the UWB signal quality is poor, the UWB factor weight can be reduced to decrease its contribution to the optimization. Conversely, when the inertial data exhibits significant drift, the inertia factor weight can be reduced. Weighting refers to multiplying different residual terms by a coefficient (i.e., a weight) when constructing the joint optimization objective function to reflect the relative importance of these residual terms in the optimization process or the inverse variance of their measurement noise. For example, a residual term can be multiplied by the inverse of its corresponding measurement covariance matrix to achieve the best linear unbiased estimate. The joint optimization objective function is a mathematical expression whose minimum value corresponds to the optimal solution of the state variable to be estimated (such as target position information, time-varying lever vector, etc.). This function typically consists of multiple residual terms (representing the difference between measured and predicted values) and their corresponding weights.

[0173] Through the above technical solution, a joint optimization objective function including an adaptive regularization term was constructed, solving the problem of instability in the optimization process when the lever arm vector undergoes abrupt changes. For example, by integrating UWB ranging data and first and second inertial data, comprehensive measurement information is provided. More importantly, the introduced adaptive regularization term can dynamically strengthen the constraint strength on the lever arm vector estimation based on historical trajectory smoothing constraints and the historical rate of change of the estimated value of the time-varying lever arm vector. When an abnormal jump in the lever arm vector is detected, it can suppress abnormal jumps in the estimated value of the lever arm vector during the optimization process, avoiding the decrease in positioning accuracy and system instability caused by dynamic changes in the lever arm. By using UWB factor weights and inertial factor weights to weight each residual term, the contribution of different sensors in the optimization can be dynamically adjusted according to their reliability, further enhancing the robustness and adaptability in complex downhole environments. This dynamic adjustment and adaptive constraint mechanism enables the continuous output of high-precision and high-reliability positioning results in dynamic downhole environments, even if changes in personnel posture, equipment vibration, or loose installation cause real-time changes in the lever arm, thus improving the performance of the positioning system.

[0174] In some of the above-mentioned solutions in this application, it is proposed to iteratively solve the joint optimization objective function within a sliding window to estimate the target position information and the time-varying lever arm vector. However, in its implementation, the solution process may result in slow convergence speed, numerical instability or large result deviation due to improper initial value selection or nonlinear optimization problems, which affects the positioning accuracy and real-time performance. Especially in the dynamic environment of the well, when the lever arm changes frequently, it is easy to cause abnormal jumps in the estimated value.

[0175] To address this, this application further proposes a method for iteratively solving the joint optimization objective function within a sliding window to obtain the target position information and the estimated value of the time-varying lever arm vector. The specific steps include: Obtain the initial position value and the initial value of the time-varying lever arm vector at each moment within the sliding window.

[0176] Based on the joint optimization objective function, the initial values ​​of the position and the time-varying lever vector are linearized to obtain the linearized optimization subproblem.

[0177] The linearized optimization subproblem is solved iteratively to obtain the position increment and the increment of the time-varying lever arm vector.

[0178] The initial value of the position is updated based on the position increment to obtain the target position information, and the initial value of the time-varying lever arm vector is updated based on the time-varying lever arm vector increment to obtain the estimated value of the time-varying lever arm vector.

[0179] For example, there are several ways to obtain the initial position and the initial value of the time-varying lever vector at each moment within the sliding window. One approach is to use the optimized position information and the estimated time-varying lever vector from the previous moment, and then predict the position using dead reckoning by the inertial navigation system (INS), as the initial value for the current moment. Another approach is to use UWB ranging data during system startup or reinitialization to perform a preliminary position estimate using methods such as least squares or extended Kalman filtering, and combine this with a preset or roughly calibrated lever value as the initial value of the time-varying lever vector. Obtaining these initial values ​​aims to provide a reasonable starting point for the subsequent optimization process, avoiding divergence or slow convergence due to random initialization.

[0180] When linearizing the initial values ​​of the position and the time-varying lever arm vector based on the joint optimization objective function to obtain a linearized optimization subproblem, methods such as Taylor expansion are commonly used. For example, a first-order Taylor expansion can be performed on the joint optimization objective function around the current initial values ​​of the position and the time-varying lever arm vector, thus approximating the nonlinear optimization problem as a linear least-squares problem. This linearization transforms the complex nonlinear optimization problem into an easily solvable linear subproblem, reducing computational complexity. Another approach is to use numerical differentiation to approximate the Jacobian and Hessian matrices when the analytical derivative of the objective function is difficult to obtain, thereby constructing the linearized optimization subproblem.

[0181] When iteratively solving the linearized optimization subproblem to obtain the position increment and the increment of the time-varying lever arm vector, various numerical optimization algorithms can be employed. For example, a commonly used method is the Gauss-Newton method or the Levenberg-Marquardt method, which obtains the increment of the current iteration step by solving a system of linear equations. Specifically, a normal equation can be constructed for the linearized objective function and solved using direct methods such as Cholesky decomposition or QR decomposition to obtain the position increment and the increment of the time-varying lever arm vector. For large-scale sparse problems, iterative methods such as the conjugate gradient method can also be used to improve computational efficiency.

[0182] When updating the initial value of the target position based on the position increment to obtain the target position information, and updating the initial value of the time-varying lever arm vector based on the increment of the time-varying lever arm vector to obtain the estimated value of the time-varying lever arm vector, an iterative update strategy is usually adopted. For example, the solved position increment can be directly superimposed on the current initial position value to obtain a new position estimate, which is then used as the initial value for the next iteration until convergence. Similarly, the increment of the time-varying lever arm vector is updated in a similar manner. Another implementation is to introduce strategies such as line search or trust region during the update process to ensure that the objective function value decreases effectively with each update and to prevent instability or escape from local optima due to excessively large step sizes.

[0183] The above technical solution addresses the problems of slow convergence, numerical instability, or large result deviations that may occur during iterative solutions to the joint optimization objective function within a sliding window. By obtaining reasonable initial values, a reliable starting point is provided for the optimization process, avoiding the risk of divergence caused by random initialization and reducing the number of iterations. Linearizing the nonlinear optimization problem into subproblems reduces computational complexity, improves solution efficiency, and preserves the constraint characteristics of the objective function. Iteratively solving the linearized subproblems gradually optimizes the increments of the position increment and the time-varying lever arm vector, ensuring that each iteration approaches the optimal solution and suppressing abnormal fluctuations in the estimated values. Based on incremental updates of the initial values, stable convergence and accurate output are achieved, continuously providing high-precision and high-reliability positioning results in dynamic downhole environments. Especially when lever arm changes frequently, it effectively avoids abnormal jumps in the estimated values, enhancing the robustness of the positioning system.

[0184] In some of the solutions mentioned above in this application, the attitude change information of the target to be located in the well is determined based on the target position information and the estimated value of the time-varying lever vector to output attitude information. However, in this process, the attitude change information may be inaccurate due to the accumulation of errors in the position sequence or the instability of the lever vector estimation, and it cannot adapt to the scenario of rapid attitude change in the well. Specifically, the derivation of velocity sequence and acceleration sequence is affected by position data noise, the calculation of attitude angle sequence lacks effective integration of dynamic angular velocity data, and the generation of attitude change information does not fully combine motion mode characteristics, thereby reducing the reliability and adaptability of attitude estimation.

[0185] To address this, this application further proposes a method for determining the attitude change information of the target to be located downhole, see [link to relevant documentation]. Figure 5 The specific steps include: 501. Obtain the time series of target location information to obtain the location sequence.

[0186] 502. Based on the position sequence, determine the velocity sequence and acceleration sequence of the target to be located.

[0187] 503. Based on the estimated value of the time-varying lever arm vector, the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data, determine the attitude angle sequence of the target to be located.

[0188] 504. Generate attitude change information based on velocity sequence, acceleration sequence and attitude angle sequence.

[0189] The first step, acquiring the time series of target location information to obtain a location sequence, aims to establish a record of the target's position at different times, providing continuous and reliable basic data for subsequent kinematic analysis. For example, after each positioning update, the target location information output by the joint optimization solution process and its corresponding timestamp can be stored to form a location dataset arranged in chronological order. Alternatively, the location data stream output by the positioning system can be received in real time and cached or recorded to construct a dynamically updated location sequence, ensuring the timeliness and integrity of the data.

[0190] Based on the position sequence, the velocity and acceleration sequences of the target to be located are determined. This step involves mathematically processing the acquired position sequence to extract the velocity and acceleration information of the target. The velocity sequence can be obtained by performing a first-order difference operation on the position sequence, for example, calculating the ratio of the difference in position between adjacent time points to the time interval. The acceleration sequence can be obtained by performing a first-order difference operation on the velocity sequence or a second-order difference operation on the position sequence. To improve the robustness of the calculation, methods such as sliding window averaging, Kalman filtering, or least squares fitting can be used to smooth the position sequence before differencing to suppress the influence of measurement noise on the velocity and acceleration estimation.

[0191] Based on the estimated value of the time-varying lever arm vector, the first angular velocity data from the first inertial data, and the second angular velocity data from the second inertial data, the attitude angle sequence of the target to be located is determined. This step is one of the key aspects of this application, as it utilizes the accurately estimated time-varying lever arm vector and angular velocity data from two IMUs to infer the target's attitude. For example, the angular velocity data from the first and second IMUs, combined with the estimated value of the time-varying lever arm vector, can be used to estimate the target's attitude in real time through an attitude fusion algorithm (such as extended Kalman filtering, unscented Kalman filtering, or complementary filtering based on quaternions or rotation matrices). Another implementation approach is to construct a nonlinear optimization problem containing attitude state variables, using the first angular velocity data, the second angular velocity data, and the estimated value of the time-varying lever arm vector as observations, and solving the problem iteratively to obtain a series of continuous attitude angles, thus forming an attitude angle sequence.

[0192] Based on velocity, acceleration, and attitude angle sequences, attitude change information is generated. This step integrates the previously obtained kinematic and attitude information to form a comprehensive description of the target's attitude changes. Attitude change information can include parameters directly derived from the attitude angle sequence, such as the rate of change of attitude angles, angular velocity, and angular acceleration. Furthermore, the velocity and acceleration sequences can be combined to analyze the target's motion pattern, such as determining whether the target is in linear motion, turning, stationary, or other complex motion patterns, and incorporating this motion pattern information as part of the attitude change information. For example, by analyzing the magnitude and direction of velocity and acceleration, it can be identified that the target is turning, and its turning radius and angular velocity can be further calculated.

[0193] The above technical solution addresses the problem of inaccurate attitude estimation in dynamic downhole environments. By acquiring continuous time series of target position information, a smooth and reliable input is provided for subsequent velocity and acceleration calculations, reducing the impact of position data noise on motion parameter derivation. Crucially, the solution integrates the accurate time-varying lever arm vector estimate obtained from joint optimization with angular velocity data from dual IMUs to determine the attitude angle sequence. This enables real-time and accurate capture of the dynamic attitude changes of the target in complex downhole environments (such as personnel posture changes and equipment vibrations), overcoming the limitations of traditional methods that rely on fixed or piecewise constant lever arm assumptions, and significantly improving the adaptability and accuracy of attitude estimation. By comprehensively utilizing velocity, acceleration, and attitude angle sequences, comprehensive attitude change information is generated. This fusion of multi-source information not only enhances the robustness of attitude estimation and suppresses noise interference from single data sources but also more accurately identifies and describes the target's motion patterns, providing a more reliable attitude perception capability for refined management and safety assurance of downhole personnel and equipment. Compared with the basic solution, this application further refines the generation process of attitude change information based on obtaining high-precision target position information and time-varying lever vector estimation, so that it can still output highly reliable attitude information in the scenario of rapid attitude change downhole.

[0194] In some embodiments described above in this application, attitude change information is generated based on the target position information and the estimated value of the time-varying lever vector to determine the attitude change of the target to be located. However, in the downhole environment, the target to be located may exhibit multiple motion modes such as linear motion and turning motion. If a uniform attitude change information generation method is used, the generated attitude change information may be inaccurate and unable to effectively distinguish the motion characteristics under different motion modes, thereby affecting the subsequent lever estimation and positioning accuracy.

[0195] To address this, this application further proposes a method for generating attitude change information based on velocity sequences, acceleration sequences, and attitude angle sequences. Specifically, this includes: extracting attitude angles at each moment from the attitude angle sequence, and calculating the attitude angle change rate based on attitude angles at adjacent moments to obtain an attitude angle change rate sequence. Based on the velocity and acceleration sequences, the motion mode of the target to be located is determined, including linear motion and turning motion modes. In the case of a turning motion mode, the turning angular velocity and turning radius of the target to be located are determined based on the attitude angle change rate sequence and acceleration sequence, and these are used as attitude change information. In the case of a linear motion mode, the motion direction and velocity of the target to be located are determined based on the velocity sequence, and these are used as attitude change information.

[0196] For example, attitude angles at each moment are extracted from the attitude angle sequence, and the rate of change of attitude angles is calculated based on the attitude angles at adjacent moments to obtain the attitude angle rate of change sequence. The attitude angle sequence is a dataset describing the attitude (such as pitch, roll, and yaw) of the target to be located at different moments. The attitude angle rate of change sequence reflects how quickly these attitudes change over time and is key to understanding the dynamic characteristics of the target's motion. By calculating the attitude angle rate of change, the instantaneous angular velocity information of the target during rotation or turning can be captured, providing basic data for subsequent motion pattern judgment and parameter extraction. In practice, the attitude angle rate of change can be calculated using the difference method. For example, for continuous attitude angle data, the attitude angle rate of change can be approximated by calculating the ratio of the difference between two adjacent attitude angles to the time interval. Alternatively, methods such as sliding window averaging or Kalman filtering can be used to smooth the attitude angle sequence before performing the difference calculation to reduce the impact of noise on the rate of change calculation and improve the accuracy of the rate of change estimation.

[0197] Based on velocity and acceleration sequences, the motion pattern of the target to be located is determined, including linear motion and turning motion. Determining the motion pattern aims to distinguish whether the target is currently in a relatively stable linear motion state or in a turning motion state undergoing a change of direction. This distinction is crucial for subsequently employing different attitude change information generation strategies for different motion patterns, improving the accuracy and specificity of attitude description. For example, this can be determined by analyzing the amplitude changes of the velocity sequence and the lateral component of the acceleration sequence. When the velocity amplitude change is small and the lateral component of acceleration (perpendicular to the velocity direction) is close to zero, it can be identified as a linear motion pattern. When the lateral component of acceleration is present and persistent, it may be a turning motion pattern. Another approach is to combine angular velocity information for judgment. For example, when the absolute value of the angular velocity is below a certain preset threshold, it is determined as linear motion. When the absolute value of the angular velocity is above the threshold, it is determined as turning motion. Furthermore, machine learning classifiers can be used to automatically identify motion patterns by training historical velocity, acceleration, and angular velocity data.

[0198] When the motion mode is turning, the turning angular velocity and turning radius of the target to be located are determined based on the attitude angle change rate sequence and acceleration sequence, and these parameters are used as attitude change information. When the target is in turning motion mode, its motion characteristics are mainly described by the turning angular velocity and turning radius. The turning angular velocity reflects the speed of the target's turn, while the turning radius describes the curvature of the turning path. Combining these parameters with the attitude angle change rate sequence can comprehensively and accurately characterize the target's attitude dynamics during the turning process, providing more accurate input for subsequent lever vector estimation. The turning angular velocity can be directly extracted from the attitude angle change rate sequence, especially the yaw rate change. The turning radius can be calculated through kinematic relationships, for example, by using the ratio of the velocity amplitude to the turning angular velocity (R=V / ω). Alternatively, the turning radius can be determined by analyzing the centripetal acceleration component in the acceleration sequence (a_c=V^2 / R), and then the turning angular velocity can be calculated by combining the velocity information. In addition, methods such as least squares or Kalman filtering can be used to fuse the attitude angle change rate sequence, velocity sequence and acceleration sequence to estimate the turning angular velocity and turning radius more robustly.

[0199] In the case of linear motion, the direction and velocity of the target to be located are determined based on the velocity sequence, and the attitude angle change rate sequence, direction, and velocity are used as attitude change information. When the target is in linear motion, its motion characteristics are mainly described by its direction and velocity. The direction of motion indicates the target's orientation, while the velocity indicates how fast it moves. Combining these parameters with the attitude angle change rate sequence allows for a concise and accurate description of the target's attitude dynamics in linear motion, avoiding the introduction of unnecessary turning parameters and improving the efficiency and accuracy of information generation. The velocity can be directly obtained from the amplitude of the velocity sequence. The direction of motion can be determined by normalizing the velocity vector; for example, dividing the velocity vector by its amplitude yields a unit direction vector. In some cases, the velocity sequence can also be smoothed before extracting its amplitude and direction.

[0200] The above technical solution enables the adaptive generation of more accurate and targeted attitude change information based on the actual motion state of the target in the downhole environment. For example, by extracting the rate of change of attitude angles from the attitude angle sequence, the dynamic details of the target's attitude can be captured. Based on the velocity and acceleration sequences, it is possible to accurately distinguish whether the target is in a straight-line motion mode or a turning motion mode. In the turning motion mode, by determining the turning angular velocity and turning radius, the motion characteristics of the target during the turning process can be comprehensively described, avoiding the information loss and inaccuracy caused by using only a single parameter to describe the attitude change during turning. In the straight-line motion mode, the focus is on the direction and velocity of motion, avoiding the introduction of unnecessary turning parameters, making the attitude change information more concise and efficient. This method of dynamically adjusting the attitude change information generation strategy according to the motion mode solves the problems of inaccurate attitude change information and inability to effectively distinguish motion characteristics caused by a uniform generation method. Therefore, the generated attitude change information can more accurately reflect the true motion state of the target to be located, thus providing a more reliable input for subsequent time-varying lever vector estimation, thereby improving the overall accuracy and reliability of downhole fusion positioning. Especially in complex and ever-changing downhole environments, it can better adapt to the dynamic changes of the lever caused by personnel attitude changes and equipment movements.

[0201] In some embodiments described above in this application, attitude change information is determined based on target location information and time-varying lever vector estimates. However, during implementation, when the observability of the lever vector is insufficient, direct online estimation may lead to inaccurate or abnormal jumps in the estimated values, thereby affecting the stability and accuracy of the positioning results. The lack of a dynamic adjustment mechanism for the estimation strategy prevents adaptive optimization based on real-time motion states and confidence assessment results, resulting in reduced positioning reliability in complex downhole environments.

[0202] In response, this application further proposes that, after determining the attitude change information of the target to be located downhole based on the target location information and the estimated value of the time-varying lever arm vector, the method further includes: determining the observability of the time-varying lever arm vector based on the attitude change information. Based on the observability, the lever arm change detection result, and the fused state diagnosis result, the estimation strategy for the time-varying lever arm vector in the joint optimization solution process is adaptively updated to output optimized target location information.

[0203] Determining the observability of the time-varying lever vector refers to assessing whether the system state variables can be completely determined through measurement output. For a time-varying lever vector, its observability reflects whether the lever vector can be accurately and stably estimated under the current motion state. High observability means that the lever vector's influence on the system output (such as the first inertial data acquired by the first IMU, the second inertial data acquired by the second IMU, and UWB ranging data) is sufficient and distinguishable, thus enabling effective estimation. Conversely, low observability indicates that the lever vector's influence on the system output is weak or heavily coupled with other state variables, making accurate estimation difficult. Its purpose is to assess the reliability of lever vector estimation and avoid invalid or harmful estimations under poor observation conditions. For example, one implementation method is to assess the observability of the lever vector under the current motion state by analyzing the rank or singular values ​​of the system's Jacobian matrix. For example, an Extended Kalman Filter (EKF) or factor graph optimization model can be constructed, and its linearized system matrix can be analyzed. The observability can be determined by calculating the rank or condition number of the submatrices related to the lever arm vector. Another approach is based on information matrix analysis. For instance, a Fisher information matrix about the lever arm vector can be constructed, and the observability of the lever arm vector can be quantified by analyzing its eigenvalues ​​or determinant. Larger eigenvalues ​​or determinants generally indicate higher observability.

[0204] The adaptive update strategy for estimating the time-varying lever arm vector during the joint optimization process refers to dynamically adjusting how to handle the estimation problem of the time-varying lever arm vector based on real-time system state, environmental information, and evaluation results. This includes whether to perform online estimation, how to set the initial values ​​for the estimation, and how to adjust the update step size or weights. Its purpose is to ensure that the lever arm vector estimation process maintains optimal performance under different operating conditions, avoids introducing errors under adverse conditions, and fully utilizes information for accurate estimation under favorable conditions. For example, one implementation is to switch between online and offline estimation based on the observability level. For instance, when the observability is below a certain threshold, the online estimation of the lever arm vector can be temporarily frozen, and historical or preset values ​​can be used instead. When the observability recovers above the threshold, online estimation is reactivated. Another implementation is to dynamically adjust the parameters of the optimization algorithm based on the lever arm change detection results and the fused state diagnosis results. For instance, when a change in the lever arm is detected, the weight of the lever arm vector estimation can be increased or its state covariance can be relaxed to enable it to respond to changes more quickly. When the fusion state diagnosis results show that the confidence assessment is effective and stable, the update step size can be appropriately reduced to improve the stability of the estimation.

[0205] The optimized target location information refers to the location data of the target to be located, obtained through a joint optimization solution process after adaptively adjusting the lever arm vector estimation strategy, resulting in higher accuracy and reliability. The "optimization" here is reflected in the more accurate handling of the lever arm effect, thereby reducing the interference of lever arm changes on the positioning results. Its role is to provide more accurate and stable target location information to meet the needs of high-precision downhole positioning. For example, one implementation is to directly output the optimized target location information as the positioning result for use in upper-level applications (such as personnel trajectory tracking and equipment scheduling). Another implementation is to use the optimized target location information as input to subsequent data processing or decision-making systems, such as for generating high-precision maps, providing safety warnings, or assisting navigation.

[0206] By introducing an assessment of the observability of the time-varying lever arm vector, this application can intelligently determine whether the lever arm vector can be reliably estimated under the current motion state. When the observability is low, it avoids forcing online estimation when information is insufficient, thereby suppressing inaccurate or abnormal jumps in the estimated lever arm vector value and improving the stability and accuracy of the positioning results. Combining the lever arm change detection results and the fusion state diagnosis results, this application can adaptively adjust the estimation strategy for the time-varying lever arm vector during the joint optimization solution process. For example, when the lever arm change detection results indicate that the lever arm has changed and the observability is high, online estimation can be promptly activated or adjusted to quickly respond to the actual changes in the lever arm. However, when the observability is insufficient or the fusion state diagnosis results show problems with the confidence assessment, a conservative strategy can be adopted, such as using historical estimates, to avoid introducing new errors. This dynamic and intelligent estimation strategy adjustment mechanism enables flexible optimization of the lever arm vector estimation process based on the complex and ever-changing downhole environment and sensor data quality, thereby outputting more accurate and reliable target position information. Overall, this solution addresses the positioning challenges posed by the lever arm effect in dynamic downhole environments through refined management and adaptive control of the lever arm vector estimation process, thereby enhancing the robustness and adaptability of the entire fusion positioning system.

[0207] In some of the embodiments described above in this application, a strategy is proposed to determine the observability of the time-varying lever arm vector based on attitude change information in order to adaptively update the estimation strategy of the time-varying lever arm vector during the joint optimization solution process. However, in its implementation, attitude change information contains a variety of dynamic parameters such as attitude angle change rate and motion mode. How to effectively integrate these parameters to construct a robust observation model and accurately quantify the observability to avoid inaccurate evaluation due to parameter noise or motion state complexity, thereby affecting the reliability and adaptability of the estimation strategy, is a key challenge.

[0208] To address this, this application further proposes methods for determining the observability of the time-varying lever arm vector based on the attitude change information, specifically including the following steps: Based on the attitude angle change rate sequence and the turning angular velocity or direction of motion in the attitude change information, an observation matrix corresponding to the time-varying lever vector is constructed. The attitude angle change rate sequence refers to a series of data consisting of the derivative or difference values ​​of the attitude angles of the target at consecutive time points, reflecting the speed and direction of the target's rotational motion in space. This sequence can be obtained by performing first-order difference or numerical differentiation on the attitude angle sequence, which can be calculated from inertial measurement unit (IMU) data integration or fusion positioning results. Alternatively, it can be extracted directly from the IMU's angular velocity data and combined with the attitude calculation results for transformation and filtering to obtain a smoother and more accurate attitude angle change rate. The turning angular velocity refers to the rate at which the target's attitude angle changes with time during turning motion, typically specifically referring to the rotational speed around a certain axis. The direction of motion refers to the instantaneous direction of the target's motion vector in space. These two factors together characterize the target's motion pattern. By analyzing the attitude angle change rate sequence and velocity sequence, it can be determined whether the target is turning, and the turning angular velocity can be calculated. If it is moving in a straight line, the direction of the velocity vector is extracted as the direction of motion. Alternatively, the turning angular velocity can be directly calculated using IMU angular velocity data, and the direction of motion can be determined by fusing the velocity information output by the positioning system. The observation matrix is ​​a matrix that describes the linear or approximately linear relationship between the observed values ​​and the state variables to be estimated. Its construction aims to capture how the time-varying lever vector affects the observable kinematic quantities when the target is moving. This observation matrix can be obtained based on a kinematic model, using the time-varying lever vector as a state variable, and the attitude angle change rate and turning angular velocity as inputs, deriving the observation equation, and then linearizing it. Alternatively, a numerical method can be used, by making small perturbations to the time-varying lever vector and observing its impact on relevant quantities (such as relative acceleration) in the attitude change information, thereby approximating its structure.

[0209] Based on the observation matrix, the Fisher information matrix of the time-varying arm vector is determined. The Fisher information matrix is ​​a statistical measure of the amount of information about unknown parameters contained in observation data. Here, it reflects the extent to which the arm vector can be accurately estimated given attitude change information. This matrix can be calculated using a formula based on the classic definition of the Fisher information matrix, combined with the constructed observation matrix and the covariance matrix of observation noise (e.g., IMU measurement noise, ultra-wideband (UWB) ranging noise, etc.). Alternatively, in a factor graph-based optimization framework, the Fisher information matrix can be extracted or approximated from the Hessian matrix of the factor graph.

[0210] The observability of the time-varying lever arm vector is determined based on the ratio of the determinant to the trace of the Fisher information matrix. The determinant is a scalar value of the matrix, reflecting the "volume" or "information" of the linear transformation it represents. The trace is the sum of the diagonal elements of the matrix, reflecting its "size" or "total variance." Using the ratio of the determinant to the trace as observability aims to comprehensively evaluate the effectiveness of the observed data in estimating the time-varying lever arm vector. This ratio can be calculated directly; to avoid numerical problems, the logarithm of the determinant can be taken, or the ratio can be normalized.

[0211] The above technical solutions address the challenges of constructing observation models and the inaccuracy of observability quantification caused by the inclusion of multiple dynamic parameters in attitude change information. For example, by systematically utilizing the attitude angle change rate sequence, turning angular velocity, or direction of motion within the attitude change information, the dynamic characteristics of the target under different motion modes can be comprehensively captured. This allows for the construction of a more robust time-varying lever vector observation matrix, which fully reflects the intrinsic relationship between the observation data and the time-varying lever vector, avoiding observation model bias caused by a single motion state or parameter. By determining the Fisher information matrix, the amount of information about the time-varying lever vector contained in the observation data is accurately quantified, enhancing the mathematical rigor of observability assessment and resolving the evaluation fluctuation problem caused by data noise or model imperfections in traditional methods. Determining observability based on the ratio of the determinant to the trace of the Fisher information matrix comprehensively considers the richness of information and the scale of estimation uncertainty, providing a more comprehensive and accurate quantitative indicator. This multi-dimensional and robust observability assessment provides a reliable basis for the estimation strategy of time-varying lever vectors in the subsequent adaptive adjustment of the joint optimization solution process, ensuring that even in the complex dynamic environment downhole, the system can continuously output high-precision and high-reliability positioning results, thereby improving the adaptability and stability of the entire fusion positioning system.

[0212] In some of the solutions described above in this application, an adaptive update estimation strategy based on observability, lever change detection results, and fusion state diagnosis results was proposed. However, this process simply shuts down estimation when observability is insufficient and simply turns it on when lever changes, lacking a fine-grained adjustment mechanism for the estimation strategy under different conditions. A key challenge is how to further optimize the estimation process using fusion state diagnosis results, avoiding slow convergence or estimation oscillations caused by inappropriate estimation update step sizes, and thus improving positioning accuracy while ensuring stability, when confidence assessment is effective and observability is sufficient.

[0213] To address this, this application further proposes a method for adaptively updating the estimation strategy of the time-varying lever arm vector during the joint optimization process to output optimized target position information. The method includes: when the observability is lower than a preset observability threshold, disabling online estimation of the time-varying lever arm vector during the joint optimization process and using the previous time-varying lever arm vector estimate as the current estimate. When the observability is not lower than the preset observability threshold, and the lever arm change detection result indicates a change in the lever arm, using the lever arm change trend estimate from the lever arm change detection result as the initial optimization value for the time-varying lever arm vector during the joint optimization process, and enabling online estimation of the time-varying lever arm vector. When the observability is not lower than the preset observability threshold, and the fusion state diagnosis result indicates a valid confidence assessment, maintaining the current online estimation state of the time-varying lever arm vector, and adjusting the estimation update step size of the time-varying lever arm vector during the joint optimization process based on the fusion state diagnosis result. Based on the updated estimation strategy, the joint optimization process is re-executed to obtain optimized target position information.

[0214] For example, observability refers to the ability of a positioning system to accurately estimate system state variables (such as time-varying lever vectors) using sensor measurement data. In inertial navigation and UWB fusion positioning, the observability of time-varying lever vectors is usually closely related to the motion pattern of the target being located. For instance, when the target is moving in a straight line, the lever effect may be difficult to distinguish from the translational acceleration of the IMU, resulting in low observability of the lever vector. However, when the target is rotating, the centripetal and tangential accelerations generated by the lever effect enhance the observability of the lever vector. Observability can be quantified by analyzing the linearized models of the system state equations and observation equations, and calculating the rank of the Fisher information matrix or observability matrix. Another approach is to indirectly measure observability by analyzing the sensitivity to perturbations of state variables and assessing the degree of impact of perturbations on the observations.

[0215] The preset observability threshold is a pre-defined value used to determine whether the observability of the time-varying lever arm vector is sufficient for reliable online estimation. This threshold can be set according to the actual application scenario, sensor noise level, desired positioning accuracy, and requirements for lever arm estimation stability. For example, offline experiments or simulation analysis can be used to determine at what level of observability the online estimation results begin to deviate or diverge, and this level can then be set as the threshold. Alternatively, the threshold can also be set based on empirical values ​​or industry standards; for example, when the minimum eigenvalue of the Fisher information matrix is ​​below a certain value, the observability is considered insufficient.

[0216] Disabling the online estimation of the time-varying lever arm vector during the joint optimization process means that the time-varying lever arm vector will no longer be considered as a state variable to be estimated and updated during the joint optimization process. This is to avoid large deviations, divergences, or anomalous jumps in the estimation results due to insufficient observability, where the observation information cannot effectively constrain the lever arm vector, thus affecting the stability and accuracy of the overall positioning. One implementation is to set the weights of factors related to the time-varying lever arm vector (e.g., lever arm residual factors) to zero in the factor graph optimization framework, or to directly remove the time-varying lever arm vector node from the set of state variables to be optimized. Another implementation is to freeze the update of the time-varying lever arm vector during the iteration of the optimization algorithm, that is, to set its increment to zero, keeping it unchanged.

[0217] Using the time-varying lever arm vector estimate from the previous moment as the estimate for the current moment means that when online estimation is disabled, an estimate of the lever arm vector is needed to maintain the continuity of system operation. Using the estimate from the previous moment is a simple and effective strategy. Its purpose is to use historical information to compensate for insufficient observations at the current moment, ensuring that a relatively reasonable lever arm vector value can still be provided even when sufficient observation information is lacking, thereby avoiding sudden interruptions or large fluctuations in the positioning results. One implementation is to directly copy the time-varying lever arm vector estimate obtained from the previous moment's optimization to the lever arm vector state at the current moment during system state updates. Another implementation is to set the prior information of the lever arm vector as the estimate from the previous moment in the optimizer and assign it a very high confidence level (i.e., a very small covariance), so that it hardly changes in the current optimization.

[0218] The lever arm change detection result determines whether there has been a change in the lever arm between the UWB tag and the target to be located. Its function is to provide a trigger condition for adjusting the lever arm estimation strategy, enabling timely response to dynamic changes in the lever arm. This result typically includes a signal indicating whether a change in the lever arm has occurred (e.g., a Boolean value or status flag) and an estimated trend of the lever arm change. One implementation method is to analyze the differential acceleration characteristics between the first and second IMUs, such as their standard deviation or variance; when these exceed a preset threshold, a lever arm change is determined. Another implementation method is to combine information such as UWB ranging residuals and IMU pre-integration residuals, and use statistical methods or machine learning models for anomaly detection to determine whether a change in the lever arm has occurred.

[0219] The estimated trend of lever arm change is a preliminary prediction of the direction and magnitude of the lever arm vector change. Its purpose is to provide an optimized initial value closer to the true value for subsequent online estimation, thereby accelerating convergence and improving estimation accuracy. For example, when the lever arm changes, the displacement trend can be preliminarily estimated by integrating the differential acceleration characteristics. One implementation is to integrate the differential acceleration data over a period of time after detecting a lever arm change to obtain the relative displacement vector of the lever arm, which serves as the estimated trend of lever arm change. Another implementation is to use state estimation algorithms such as Kalman filters or particle filters to predict the motion state of the lever arm based on IMU data after detecting a lever arm change, thus obtaining its changing trend.

[0220] The initial optimization value is the initial guess of the state variable (in this case, the time-varying lever arm vector) when the optimization algorithm begins its iterative solution. Its role is to influence the convergence speed of the optimization algorithm and the probability of convergence to the global optimum. A good initial optimization value can reduce the number of iterations, improve computational efficiency, and avoid getting trapped in local optima. One implementation is to directly input the estimated lever arm change trend as the initial value of the time-varying lever arm vector into the optimizer. Another implementation is to take a weighted average of the estimated lever arm change trend and the lever arm estimate from the previous time step, using this as the initial optimization value to take into account both historical information and the current change trend.

[0221] Enabling online estimation of the time-varying lever arm vector means that during the joint optimization solution process, the time-varying lever arm vector is treated as a state variable to be estimated, and the optimization algorithm is allowed to update it. Its purpose is to enable real-time tracking of the dynamic changes in the lever arm, thereby accurately estimating the new lever arm vector when changes occur, ensuring positioning accuracy. One implementation is to set the weights of factors related to the time-varying lever arm vector to non-zero values ​​in the factor graph optimization framework and include its nodes in the set of state variables to be optimized. Another implementation is to allow the time-varying lever arm vector to be updated based on gradient information during the iteration process of the optimization algorithm.

[0222] The fusion status diagnostic result is a judgment of the effectiveness of the multi-dimensional confidence assessment process. Its role is to reflect the overall health and data reliability of the current fusion localization system, providing macro-level guidance for subsequent estimation strategy adjustments. This result typically includes an indicator of the effectiveness of the confidence assessment, as well as information that may reflect the degree of conflict or convergence status between different confidence levels. One approach is to determine the fusion status by analyzing the degree of conflict between different confidence levels (e.g., using the conflict coefficient in Dempster-Shafer evidence theory) and the convergence speed or stability of the overall confidence. Another approach is to utilize a machine learning classifier, taking the confidence levels of each dimension and their changing trends as input, and outputting a fusion status diagnostic result, such as "effective," "partially effective," or "ineffective."

[0223] The estimated update step size refers to the magnitude by which the state variable (in this case, the time-varying lever vector) is updated along the gradient direction in each iteration of the optimization algorithm. Its role is to balance the convergence speed and stability of the optimization algorithm. A larger step size can accelerate convergence but may lead to oscillations or divergence. A smaller step size can improve stability but may lead to slow convergence. One implementation method, as in optimization algorithms such as Levenberg-Marquardt, is to control the step size by adjusting the damping factor. Another implementation method is to use an adaptive step size strategy, such as the Armijo criterion or the Wolfe criterion, to dynamically adjust the step size based on changes in the objective function.

[0224] The updated estimation strategy refers to a new set of estimation rules formed by adjusting the online estimation process of time-varying lever arm vectors based on observability, lever arm change detection results, and fusion state diagnosis results. Its purpose is to flexibly select the most suitable lever arm estimation method according to the actual situation of the current environment and data quality, thereby achieving optimal positioning performance in different scenarios. This strategy can be a logical judgment flow containing multiple conditional branches, selecting different operations (disabling estimation, enabling estimation, adjusting step size, etc.) based on different input conditions (observability, lever arm change, fusion state diagnosis results). Another implementation method is to use a strategy controller to dynamically adjust the optimizer's parameter configuration based on the input state, such as whether to activate the lever arm state variable, set the prior covariance of the lever arm state variable, and adjust the damping coefficient of the optimization algorithm.

[0225] Re-executing the joint optimization process means running the entire factor graph optimization or similar joint optimization algorithm again after adjusting the lever vector estimation strategy. Its purpose is to ensure that the new estimation strategy takes effect immediately and affects the positioning result at the current moment. By re-executing the optimization, the updated lever estimation strategy, combined with UWB ranging data, first inertial data, and second inertial data, can be used to recalculate the target position information and the estimated value of the time-varying lever vector, thereby outputting a more accurate and reliable positioning result. One implementation is to trigger the optimizer to perform a complete iterative solution process after the strategy adjustment. Another implementation is to pass the updated strategy parameters (such as the weights of the lever factors, the initial values ​​of the lever states, or the step size control parameters) to the optimizer during the continuously running optimization loop, so that it solves according to the new strategy in the next iteration.

[0226] By introducing the aforementioned refined, multimodal estimation strategy and adaptive update mechanism, the problems of slow convergence or oscillation caused by the lack of refined adjustment of the lever arm estimation strategy and inappropriate estimation update step size in complex dynamic environments downhole are solved, thereby improving positioning accuracy while ensuring system stability. For example, when the observability is lower than a preset observability threshold, online estimation of the time-varying lever arm vector can be intelligently turned off, and the previous estimate can be used. This mechanism avoids the risk of abnormal jumps or divergence that may be introduced by forcibly performing online estimation when the observation conditions are insufficient (e.g., the target to be located is in a linear motion state, and the lever arm effect is difficult to distinguish), thereby enhancing the stability of the positioning results. When the observability is sufficient and the lever arm change detection result indicates that the lever arm has changed, this application can use the estimated value of the lever arm change trend in the lever arm change detection result as the initial value for optimizing the time-varying lever arm vector in the joint optimization solution process and start online estimation. By providing an optimized initial value that is closer to the true value, the optimizer can converge to the new lever state more quickly, avoiding the convergence delay caused by estimating from zero, and greatly improving the efficiency of lever estimation and its responsiveness to dynamic changes. Furthermore, provided that the observability is not lower than the preset observability threshold and the confidence assessment indicated by the fusion state diagnosis results is valid, this application can maintain the current online estimation state of the time-varying lever vector and dynamically adjust the estimation update step size based on the fusion state diagnosis results. This means that when the conflicts between the confidence levels of various dimensions (such as IMU motion consistency confidence, UWB signal quality confidence, downhole positioning geometric constraint confidence, and historical trajectory consistency confidence) are small and the fusion state is good, a larger step size can be used to accelerate convergence. Conversely, when the conflicts are large and the fusion state is poor, a smaller step size is used to ensure the stability of the estimation, avoiding the problem of insufficient adaptability of a fixed step size in complex and changing environments. By adaptively adjusting the above strategy and re-executing the joint optimization solution process, this application ensures that the lever estimation strategy is closely linked with the actual environment, enabling the positioning to continuously output high-precision and high-reliability target position information in the dynamic environment downhole. Its performance advantage is even greater, especially in scenarios where the lever changes dynamically or observation conditions are limited.

[0227] The following example will provide a more detailed explanation of the above technical solution: In an underground mine environment, high-precision positioning of a miner is required. The miner wears a safety helmet with a second IMU fixed to it, and a UWB tag is attached to its belt. The first IMU is fixed to the UWB tag. Because the miner's body posture changes frequently underground, the relative position (i.e., lever arm) between the UWB tag on the belt and the safety helmet is not constant and may even change dynamically due to slight equipment loosening. Traditional positioning methods struggle to handle this time-varying lever arm effect, leading to decreased positioning accuracy and reliability. This method aims to solve this technical challenge.

[0228] During the miner's movement, the first IMU continuously collects first inertial data, and the second IMU continuously collects second inertial data. To detect lever arm changes in real time and perform preliminary compensation, differential calculations are performed on these inertial data. For example, the first acceleration data in the first inertial data and the second acceleration data in the second inertial data are first synchronized in time. Based on the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data, preliminary lever arm effect compensation is performed on the synchronized acceleration data. For example, when the system determines that the miner is in a rotational motion, it separates the centripetal acceleration component and the tangential acceleration component from the differential acceleration features for compensation. When the miner is in a translational motion, the differential acceleration features are used to correct the synchronized acceleration data. For example, half of the differential acceleration features is added as a correction to the synchronized first acceleration data and subtracted from the synchronized second acceleration data. In this way, the compensated first acceleration data and compensated second acceleration data are obtained. The difference between the compensated second acceleration data and the compensated first acceleration data yields the differential acceleration value, which serves as the differential acceleration feature. A sliding window statistical analysis is performed on this differential acceleration feature, calculating its standard deviation within the current time window and comparing it with the standard deviation at the initial calibration time to generate a lever arm change trigger signal. If the trigger signal indicates a lever arm change, the system integrates the differential acceleration feature to obtain an estimate of the lever arm change trend. The lever arm change trigger signal and the estimated lever arm change trend together constitute the lever arm change detection result. The estimated lever arm change trend provides an optimized initial value for subsequent estimation of the time-varying lever arm vector. This differs from related technologies that simply assume the lever arm is fixed or has piecewise constant values; this method can perceive and preliminarily quantify the dynamic changes of the lever arm in real time.

[0229] All of the above-mentioned optional technical solutions can be combined in any way to form the optional embodiments of this application, and will not be described in detail here.

[0230] Those skilled in the art will understand that all or part of the steps of the above embodiments can be implemented by hardware or by a program instructing related hardware. The program can be stored in a computer-readable storage medium, such as a read-only memory, a disk, or an optical disk.

[0231] The above are merely optional embodiments of this application and are not intended to limit this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A downhole fusion positioning system based on UWB and inertial navigation, characterized in that, The system includes a processor and memory, the processor being configured to execute: In the downhole environment, differential calculation is performed on the first inertial data collected by the first IMU and the second inertial data collected by the second IMU to obtain differential acceleration characteristics and lever arm change detection results. The first IMU is fixed at the UWB tag and the second IMU is fixed at the target to be located. Based on the boom change detection results, UWB ranging data, and historical trajectory smoothing constraints, a multi-dimensional confidence assessment is performed by fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to obtain a comprehensive confidence and fusion state diagnosis result. Based on the weights of the UWB factor and the inertial factor in the comprehensive confidence dynamic adjustment factor diagram, and by jointly optimizing the UWB ranging data, the first inertial data, and the second inertial data, the estimated value of the time-varying lever arm vector between the target position information of the target to be located and the first IMU and the second IMU is obtained. The objective function of the joint optimization solution includes a regularization term constructed based on the historical trajectory smoothing constraint to suppress abnormal jumps in the estimated value of the time-varying lever arm vector. The attitude change information of the target to be located downhole is determined based on the target location information and the estimated value of the time-varying lever vector.

2. The system according to claim 1, characterized in that, The differential calculation of the first inertial data acquired by the first IMU and the second inertial data acquired by the second IMU to obtain differential acceleration characteristics and lever arm change detection results includes: Based on the first inertial data and the second inertial data, the acceleration difference value between the first IMU and the second IMU is determined, and the acceleration difference value is used as the differential acceleration feature; The differential acceleration feature is statistically analyzed using a sliding window to obtain the standard deviation of the differential acceleration feature within the current time window. Based on the ratio of the standard deviation within the current time window to the standard deviation at the initial calibration time, a lever arm change trigger signal is generated. When the lever arm change trigger signal indicates a change in the lever arm, the differential acceleration feature is integrated to obtain an estimated value of the lever arm change trend. The lever arm change trigger signal and the estimated value of the lever arm change trend are used together as the lever arm change detection result. The estimated value of the lever arm change trend is used to provide an optimized initial value for the subsequent estimation of the time-varying lever arm vector.

3. The system according to claim 2, characterized in that, Determining the acceleration difference between the first IMU and the second IMU based on the first inertial data and the second inertial data includes: The first acceleration data in the first inertial data and the second acceleration data in the second inertial data are time-synchronized and aligned to obtain synchronized first acceleration data and synchronized second acceleration data; Based on the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data, preliminary lever arm effect compensation is performed on the synchronized first acceleration data and the synchronized second acceleration data to obtain compensated first acceleration data and compensated second acceleration data. The difference between the compensated second acceleration data and the compensated first acceleration data is used to obtain the acceleration difference value.

4. The system according to claim 1, characterized in that, Based on the boom change detection results, UWB ranging data, and historical trajectory smoothing constraints, a multi-dimensional confidence assessment is performed by fusing the measurement consistency between the first and second inertial data, UWB signal quality, and downhole positioning geometric constraints to obtain a comprehensive confidence and fusion state diagnosis result, including: Based on the first inertial data and the second inertial data, determine the IMU motion consistency confidence level; Based on the UWB ranging data and the geometric layout information of the underground base station, the confidence level of UWB signal quality and the confidence level of underground positioning geometric constraints are determined. Based on the historical trajectory smoothing constraints and historical positioning results, the historical trajectory consistency confidence level is determined. The historical positioning results are obtained by dead reckoning based on the first inertial data and the second inertial data. The lever arm change detection result is used as the trigger condition for confidence assessment. The confidence of IMU motion consistency, UWB signal quality, downhole positioning geometric constraint, and historical trajectory consistency are fused to obtain the comprehensive confidence and the fused state diagnosis result.

5. The system according to claim 4, characterized in that, The process of determining the confidence level of UWB signal quality and the confidence level of underground positioning geometric constraints based on the UWB ranging data and the geometric layout information of the underground base station includes: Obtain the channel impulse response corresponding to the UWB ranging data; Based on the channel impulse response, the first path power ratio, signal-to-noise ratio, and number of multipaths are extracted to obtain the UWB signal quality characteristics. The UWB signal quality features are weighted and fused to obtain the UWB signal quality confidence level; Based on the geometric layout information and the UWB ranging data, the geometric accuracy factor is determined; The confidence level of the downhole positioning geometric constraint is determined based on the ratio of the geometric accuracy factor to the preset geometric accuracy threshold.

6. The system according to claim 4, characterized in that, The method of using the lever arm change detection result as the trigger condition for confidence assessment, and fusing the IMU motion consistency confidence, the UWB signal quality confidence, the downhole positioning geometric constraint confidence, and the historical trajectory consistency confidence to obtain the comprehensive confidence and the fused state diagnosis result, including: When the arm change detection result triggers confidence reinitialization, the IMU motion consistency confidence, the UWB signal quality confidence, the downhole positioning geometric constraint confidence, and the historical trajectory consistency confidence are input into the confidence fusion framework. In the confidence fusion framework, basic probability assignments are performed on each confidence level to obtain the basic probability assignment value corresponding to each confidence level. The comprehensive confidence level is obtained by iteratively synthesizing the basic probability allocation values. Based on the degree of conflict between each confidence level and the convergence state of the overall confidence level, the fusion state diagnostic result is generated, which is used to characterize the effectiveness of the current confidence level assessment.

7. The system according to claim 1, characterized in that, The factor graph includes state nodes and observation nodes. The state nodes include the position node, velocity node, and time-varying lever arm vector node of the target to be located. The observation nodes include UWB ranging nodes, first IMU pre-integration nodes, and second IMU pre-integration nodes. The weights of the UWB factor and inertia factor in the factor graph are dynamically adjusted based on the comprehensive confidence level, and the UWB ranging data, the first inertial data, and the second inertial data are jointly optimized to obtain the target position information of the target to be located and the estimated value of the time-varying lever arm vector between the first IMU and the second IMU. This includes: Based on the comprehensive confidence level, the UWB factor weights corresponding to the UWB ranging node and the inertia factor weights corresponding to the first IMU pre-integration node and the second IMU pre-integration node are determined respectively. Based on the UWB ranging data, the first inertial data, and the second inertial data, a joint optimization objective function is constructed, which includes the UWB factor weights and the inertial factor weights. The joint optimization objective function also includes a regularization term constructed based on the historical trajectory smoothing constraint. The joint optimization objective function is iteratively solved within a sliding window to obtain the target position information and the estimated value of the time-varying lever arm vector.

8. The system according to claim 7, characterized in that, The step of determining the UWB factor weights corresponding to the UWB ranging node and the inertial factor weights corresponding to the first IMU pre-integration node and the second IMU pre-integration node based on the comprehensive confidence level includes: Obtain the historical rate of change of the overall confidence level and the estimated value of the time-varying lever arm vector; Based on the comprehensive confidence level, the basic weight values ​​of the UWB factor weights are determined; Based on the historical rate of change of the estimated value of the time-varying lever arm vector, the dynamic correction coefficient of the UWB factor weight is determined; Multiply the base weight value by the dynamic correction coefficient to obtain the UWB factor weight; The inertia factor weight is obtained by multiplying the preset inertia factor benchmark weight by the inverse proportional function of the comprehensive confidence level.

9. The system according to claim 1, characterized in that, The step of determining the attitude change information of the target to be located downhole based on the target position information and the estimated value of the time-varying lever vector includes: Obtain the time series of the target location information to obtain the location sequence; Based on the position sequence, determine the velocity sequence and acceleration sequence of the target to be located; Based on the estimated value of the time-varying lever arm vector, the first angular velocity data in the first inertial data and the second angular velocity data in the second inertial data, the attitude angle sequence of the target to be located is determined; The attitude change information is generated based on the velocity sequence, the acceleration sequence, and the attitude angle sequence.

10. The system according to claim 1, characterized in that, After determining the attitude change information of the target to be located downhole based on the target location information and the estimated value of the time-varying lever vector, the processor is further configured to execute: The observability of the time-varying lever arm vector is determined based on the attitude change information; Based on the observability, the lever arm change detection results, and the fusion state diagnosis results, the estimation strategy for the time-varying lever arm vector during the joint optimization solution is adaptively updated to output optimized target position information.

Images