Indoor positioning method fusing ultra-wideband non-line-of-sight detection and anchor adaptive selection

Abstract

The present application relates to the fusion ultra-wideband non-line-of-sight detection and anchor adaptive selection indoor positioning method, belongs to indoor positioning technical field, solve the problem that the existing positioning method is difficult to detect NLOS error in real time and anchor missing leads to positioning interruption. First, the original ranging data and sampling data are obtained and aligned in time; the three-dimensional relative displacement prediction value is obtained after short-time inertia integration; the ranging difference sequence and direction vector sequence are calculated according to the effective ranging value of each anchor point of the current frame and the last frame; the consistency check is carried out on the three-dimensional relative displacement prediction value and the measured distance change, and the NLOS level of the anchor point is determined; the positioning strategy is adaptively selected; the positioning strategy is executed to carry out positioning calculation, and the target position estimation value is obtained; the final positioning position is output after Kalman filtering smoothing. The present application can detect NLOS error in real time, and dynamically adjust the positioning strategy when part of the anchor points fail, improve the robustness and continuity of indoor positioning in complex shielding environment.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning technology, and specifically to an indoor positioning method that integrates ultra-wideband non-line-of-sight detection and adaptive anchor point selection. Background Technology

[0002] Ultra-wideband (UWB) technology, with its nanosecond-level pulse signals and centimeter-level ranging accuracy, has become one of the mainstream technologies in the field of indoor positioning. Existing UWB positioning systems typically employ ranging methods based on Time of Arrival (TOA) or Time Difference of Arrival (TDOA), combined with multilateral positioning or least squares algorithms, to calculate the distance from the tag to multiple anchor points into three-dimensional coordinates.

[0003] However, in real-world deployment environments, this solution faces two main problems:

[0004] Firstly, there is the non-line-of-sight (NLOS) problem. When obstacles such as people, metal shelves, or concrete walls obstruct the propagation path of UWB signals, signal diffraction or reflection leads to inflated ranging values, resulting in systematic errors and severely affecting positioning accuracy. Existing NLOS detection methods largely rely on Channel Impulse Response (CIR) feature extraction or statistical hypothesis testing, which has high computational complexity, making it difficult to meet the low-latency requirements of real-time positioning, and requiring additional channel estimation hardware support.

[0005] Secondly, there is the issue of missing anchor points. Existing UWB positioning systems typically require all anchor points to be visible simultaneously. For example, in coding practice, there is a common logic of "positioning calculation is only performed when all four anchor points are valid". If any anchor point signal is lost or the ranging is abnormal, the system will discard the entire frame of data, resulting in interruption of positioning output, and real-time performance and continuity cannot be guaranteed.

[0006] The two types of problems mentioned above are particularly prominent in engineering sites with dense crowds and frequent obstructions, severely restricting the practical deployment of UWB positioning systems. Therefore, an indoor positioning method that integrates UWB non-line-of-sight detection and adaptive anchor point selection is needed. This method should be able to identify NLOS errors in real time and dynamically adjust the positioning dimension and strategy when some anchor points fail, thereby significantly improving the robustness and practicality of indoor positioning in complex obstructed environments. Summary of the Invention

[0007] This invention proposes an indoor positioning method that integrates ultra-wideband non-line-of-sight detection and adaptive anchor point selection, aiming to solve the problems of difficulty in real-time detection of NLOS error and positioning interruption caused by missing anchor points in the prior art.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] An indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection includes the following steps:

[0010] Step 1: Obtain the raw ranging data between the UWB tag and each anchor point in the UWB positioning system, as well as the sampled data output by the IMU integrated with the UWB tag, and preprocess the sampled data, and perform time alignment on the raw ranging data and the preprocessed sampled data.

[0011] Step 2: Perform short-time inertial integration on the time-aligned sampled data to obtain the predicted three-dimensional relative displacement of the target in the world coordinate system between two adjacent UWB ranging times, and add a confidence index to the predicted three-dimensional relative displacement.

[0012] Step 3: Calculate the ranging difference sequence and the direction vector sequence of the UWB tag pointing to each anchor point based on the effective ranging values ​​of each anchor point in the current frame and the previous frame;

[0013] Step 4: Verify the consistency between the predicted three-dimensional relative displacement value and the measured distance change at each anchor point, and determine the NLOS level of each anchor point. The NLOS level includes reliable, suspected NLOS, and severe NLOS.

[0014] Step 5: Adaptively select a positioning strategy based on the number of available anchor points with NLOS ratings of trusted and suspected NLOS and the positioning strategy mapping rules;

[0015] Step 6: Execute the selected positioning strategy to perform positioning calculations and obtain the estimated location value;

[0016] Step 7: Perform Kalman filtering on the position estimate to obtain the filtered position estimate and the filtered velocity estimate. The filtered position estimate is sent back to Step 3 as the reference position for calculating the anchor point direction vector in the next frame, and the filtered position estimate is output as the final positioning position. The filtered velocity estimate is sent back to Step 2 as the initial value of the short-time inertial integral in the next frame.

[0017] The indoor positioning method proposed in this invention, which integrates ultra-wideband non-line-of-sight (UWB) detection and adaptive anchor point selection, utilizes an inertial measurement unit (IMU) to perform inertial integration on the target within a short time window to obtain high-frequency three-dimensional relative displacement predictions. These predictions are then compared with the actual UWB distance changes at each anchor point for consistency verification, thereby determining the NLOS level, which reflects the signal reliability of each anchor point. This achieves lightweight real-time NLOS detection without the need for channel feature extraction. Compared with existing technologies, this invention has the following advantages:

[0018] (1) This invention utilizes the short-time inertial integration results of the tag-side IMU between two adjacent UWB ranging moments to construct a three-dimensional relative displacement prediction value, and projects it onto the radial direction of each anchor point. The consistency is verified with the actual UWB distance change, thereby realizing the real-time determination of the NLOS level of each anchor point. This method does not rely on additional hardware information such as channel impulse response, nor does it require complex statistical hypothesis testing. It has the advantages of low computational load, strong real-time performance, and easy engineering deployment.

[0019] (2) This invention classifies anchor points according to their NLOS level: trusted anchor points participate in positioning normally, suspected NLOS anchor points are soft-weighted by dynamically amplifying observation noise, and severely NLOS anchor points are removed in the current frame. Compared with traditional methods that directly remove abnormal anchor points or require all anchor points to be effective at the same time, this invention can suppress the impact of abnormal ranging on positioning results while retaining available ranging information, thereby improving the robustness of positioning in complex occlusion environments.

[0020] (3) The present invention adaptively selects a positioning strategy based on the number of available anchor points: when there are enough available anchor points, full three-dimensional multi-anchor positioning is performed; when the number of available anchor points decreases to 3, IMU height constraints are introduced for positioning; when the number of available anchor points is less than or equal to 2, a short-time inertial estimation strategy based on residual anchor point constraints is performed. Through the above-mentioned hierarchical positioning mechanism, the system can maintain continuous positioning output even when some anchor points fail or are severely interfered with by NLOS, effectively avoiding positioning interruption. It is suitable for indoor positioning scenarios with frequent obstructions, such as industrial plants, underground parking lots, and hospital corridors. Attached Figure Description

[0021] Figure 1 This is a flowchart of the indoor positioning method that integrates ultra-wideband non-line-of-sight detection and adaptive anchor point selection as described in an embodiment of the present invention;

[0022] Figure 2 This is a comparison diagram of three positioning strategies, where A0, A1, A2, and A3 represent anchor point 0, anchor point 1, anchor point 2, and anchor point 3, respectively. Figure 2In the table, (a) represents the complete three-dimensional multilateral positioning strategy, (b) represents the IMU height-constrained three-dimensional positioning strategy, and (c) represents the short-time inertial estimation strategy with residual anchor point constraints. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. These embodiments are only for explaining the invention and do not constitute a limitation on the scope of protection of this invention.

[0024] This embodiment provides an indoor positioning method that integrates ultra-wideband non-line-of-sight detection and adaptive anchor point selection. It is a positioning method based on IMU (accelerometer + gyroscope) assisted UWB NLOS error detection and adaptive anchor point selection, suitable for high-precision real-time indoor positioning in complex, obstructed environments such as industrial plants, underground parking lots, and hospital corridors. Figure 1 As shown, the method specifically includes the following steps:

[0025] Step 1: Obtain the raw ranging data between the UWB tag and each anchor point in the UWB positioning system, as well as the sampled data output by the IMU integrated with the UWB tag. Preprocess the sampled data and perform time alignment between the raw ranging data and the preprocessed sampled data.

[0026] Step 101: Acquisition and frame parsing of raw UWB ranging data.

[0027] UWB tags periodically engage in bilateral, bidirectional ranging interactions with each UWB anchor point to obtain ranging information between the UWB tag and each anchor point. UWB anchor points connected to the host computer act as data aggregation nodes, receiving or summarizing the ranging information between the UWB tag and each anchor point. They then upload raw UWB ranging data frames, containing system timestamps, tag IDs, anchor point counts, distance data between anchor points, and verification information, to the host computer via serial port. The raw UWB ranging data received by the host computer is transmitted in a custom binary frame format. The frame structure includes, in sequence: frame header bytes, function identifier bytes, system timestamp field, tag ID field, anchor point count field, distance data between anchor points field, and frame tail verification bytes.

[0028] The host computer performs frame synchronization detection on the received raw byte stream, scanning byte by byte in the byte buffer, matching consecutive combinations of frame header bytes and function identifier bytes to locate the start position of a valid frame; it reads the data length field within the frame and verifies whether the number of remaining bytes in the buffer meets the requirement for a complete frame length. If not, it waits for subsequent bytes to complete the frame; it increments the valid data bytes within the frame byte by byte, takes the lower 8 bits and compares them with the frame tail check byte. If the verification passes, it continues parsing; if the verification fails, or the number of remaining bytes in the buffer is insufficient to form a complete frame, it discards the first byte of the current suspected frame and continues to slide and scan backward in the buffer until the next valid frame header combination is matched.

[0029] After successful verification, the original distance bytes of each anchor point are parsed sequentially according to the protocol offset. The distance data of each anchor point is encoded in a three-byte fixed-point format, and the conversion formula is as follows:

[0030]

[0031] in, For the first Distance data for each anchor point; Number the anchor points; This is the original integer ranging value.

[0032] Simultaneously extract the intra-frame system timestamp (Unit: milliseconds) is used for subsequent time alignment, and the parsed distance array is written to the data buffer of the corresponding tag according to the intra-frame tag ID field, so as to realize independent management of concurrent data of multiple tags.

[0033] Step 102: IMU data acquisition and preprocessing.

[0034] The UWB tag integrates an IMU, and the IMU continuously outputs sampled data to the host computer at a sampling rate no lower than the UWB ranging frequency. The sampled data specifically includes triaxial acceleration data. (Unit is) ) and three-axis gyroscope data (Unit is) ),in This represents the machine's coordinate system.

[0035] During the startup phase, the mean IMU output when the UWB tag is stationary is used as a zero-bias estimate, including the accelerometer bias. and gyroscope bias The corresponding bias is subtracted from all subsequent collected data, using the following formula:

[0036]

[0037] in, The data are triaxial accelerations after offset. This is the data from the three-axis gyroscope after deducting the bias.

[0038] Using debiased three-axis gyroscope data The current attitude is updated by integral calculation using quaternion differential equations to obtain the rotation matrix from the body coordinate system to the world coordinate system. Then, the debiased triaxial acceleration data By rotation matrix Transform to world coordinates and subtract the gravitational acceleration vector. To obtain the pure motion acceleration, the formula is as follows:

[0039]

[0040] in, It represents the pure motion acceleration in the world coordinate system.

[0041] Finally, regarding pure motion acceleration... A sliding mean low-pass filter is applied to filter out high-frequency vibration noise. The window length is determined based on the ratio of the IMU sampling rate to the UWB ranging cycle to ensure that the acceleration data in each UWB ranging cycle is sufficiently smoothed.

[0042] Step 103: Time alignment.

[0043] Because the raw UWB ranging data and IMU sampled data differ in sampling rate and time reference, time synchronization and alignment must be performed before proceeding to subsequent processing steps. This is based on the UWB intra-frame system timestamp. As a baseline time, timestamps falling within a preset time window are extracted from the IMU data buffer queue (e.g., ...). All IMU sampling data within the range are used to achieve time alignment between the raw ranging data and the preprocessed sampling data. The duration of one UWB ranging cycle.

[0044] Record the intra-system timestamp of the previous UWB frame. With the current intra-frame system timestamp The difference between the two is the integration time window (in seconds), which can be expressed by the following formula:

[0045]

[0046] This integration time window The IMU data sequence is passed to step 2 for short-time inertial integration calculation. If the system timestamp difference between the current frame and the previous frame is... If the preset maximum tolerance threshold is exceeded, it is determined that there is an abnormal jump in the data or a communication interruption. The initial state of the IMU integration is reset, and the IMU auxiliary weight is reduced for this frame to avoid incorrect predictions caused by long-term integration drift.

[0047] Step 2: Perform short-time inertial integration on the time-aligned sampled data to obtain the predicted three-dimensional relative displacement of the target in the world coordinate system between two adjacent UWB ranging times, and add a confidence index to the predicted three-dimensional relative displacement.

[0048] Step 201, Velocity Integration: Perform time integration on the IMU acceleration sequence within the integration time window to obtain the velocity sequence.

[0049] The IMU acceleration sequence within the integration time window transmitted in step 103 is received, and the IMU acceleration sequence is denoted as... The corresponding timestamp sequence is denoted as ,in This represents the number of IMU sampling points within the window. For index The pure motion acceleration of the sampling point in the world coordinate system. For the first The timestamps corresponding to the sampling points are such that an integral sub-interval is formed between two adjacent sampling points. The actual time interval for the integral sub-interval corresponding to each sampling point is:

[0050]

[0051] The filtered velocity estimate at the previous UWB frame was used as the initial velocity for integration. The velocity sequence is calculated point-by-point using the trapezoidal integral method based on actual time intervals.

[0052]

[0053] in, For the first The velocity estimates for each sampling point are obtained. Finally, the velocity estimate at the end of the integration time window is obtained. and the complete velocity sequence .

[0054] Step 202, Zero-velocity detection and drift correction: Combine the IMU acceleration sequence and gyroscope sequence to determine if the target is stationary. When the stationary determination condition is met, the target is determined to be stationary. At this time, zero-velocity update is performed and drift correction is performed on the velocity sequence.

[0055] Because residual bias and noise from the accelerometer cause continuous drift in the velocity integral, zero-velocity detection needs to be performed within each integration time window to suppress accumulated errors. The zero-velocity detection in this step uses a combination of acceleration amplitude variance, acceleration amplitude mean, angular velocity amplitude mean, and the filtered velocity amplitude from the previous frame. The specific process is as follows:

[0056] The first in the current points time window The pure motion acceleration of each sampling point in the world coordinate system is: , No. The angular velocity of each sampling point in the body coordinate system is ,in First calculate the... acceleration amplitude at each sampling point :

[0057]

[0058] Based on acceleration amplitude Calculate the mean acceleration amplitude within the integration time window :

[0059]

[0060] Then based on the acceleration amplitude and the mean acceleration amplitude Calculate the variance of acceleration amplitude within the integration time window. :

[0061]

[0062] Finally, the mean angular velocity amplitude within the integration time window is calculated. :

[0063]

[0064] When the average acceleration amplitude Acceleration amplitude variance Angular velocity amplitude mean And the filtered velocity estimate returned at the previous UWB time frame. The target is considered to be in a static state when all of the following static determination conditions are met:

[0065]

[0066] in, To preset the static variance threshold, To preset the motion acceleration threshold, To preset the angular velocity threshold, A preset speed threshold is used. If any of the above conditions are not met, the target is determined to be in motion, and zero-speed update is not performed.

[0067] When the target is determined to be stationary, drift correction is applied to the velocity sequence within the integration time window. Considering that the IMU sampling time interval may not be completely uniform, a linear drift correction method based on the actual timestamp is adopted:

[0068]

[0069] in, For the revised first The speed of each sampling point For the first time before the correction The speed of each sampling point For the first The timestamp of each sampling point This is the start timestamp of the integration time window. This is the integration time window determined in step 103.

[0070] After the above corrections, the correction rate at the end of the integration time window satisfies:

[0071]

[0072] Step 203, Displacement Integration: Perform a second integration on the corrected velocity sequence to obtain the predicted three-dimensional relative displacement.

[0073] After the velocity sequence correction is completed, the trapezoidal integral method based on the actual time interval is used to perform a second integration on the corrected velocity sequence to obtain the predicted three-dimensional relative displacement within the integration time window. :

[0074]

[0075] in, and The corrected numbers are respectively the first. The and the first The speed corresponding to each sampling point; This represents the time interval between adjacent sampling points. This is the predicted three-dimensional relative displacement value. This is a displacement vector, representing the change in the target's three-dimensional position in the world coordinate system between two adjacent UWB ranging frames, including... Three components.

[0076] Step 204, Confidence estimation: Estimate the uncertainty covariance of the three-dimensional relative displacement prediction based on the integral time window length and accelerometer noise density, and introduce a correction factor according to the motion state to obtain the final displacement uncertainty as a confidence index.

[0077] Because IMU integration inevitably involves accumulated errors, it is necessary to obtain the predicted values ​​of the three-dimensional relative displacements. An additional confidence index is added for use in the consistency check in step 4.

[0078] Based on the length of the integration time window and accelerometer noise density Estimate the predicted value of three-dimensional relative displacement Uncertainty covariance :

[0079]

[0080] Uncertainty covariance The calculation formula is based on the error propagation model of white noise acceleration after double integration. A correction factor is also introduced based on the target's motion state. The final displacement uncertainty is obtained as a confidence index. Its formula is:

[0081]

[0082] When step 202 determines that the target is stationary... (High confidence); When the target is determined to be in motion, (Standard confidence level).

[0083] Three-dimensional relative displacement prediction values and its final displacement uncertainty This information is then passed to step 4 for consistency verification with the measured UWB distance changes at each anchor point.

[0084] Step 3: Calculate the ranging difference sequence and the direction vector sequence of the UWB tag pointing to each anchor point based on the effective ranging values ​​of each anchor point in the current frame and the previous frame.

[0085] Step 301: Extract the anchor point distance of the current frame.

[0086] From the ranging data buffer parsed in step 101, read the original ranging values ​​of each anchor point in the current frame according to the tag ID, and record them as follows. ,in The total number of anchor points deployed for the UWB positioning system. Number the anchor points. Simultaneously, read the original distance measurements of each anchor point stored in the previous frame. If the ranging value of an anchor point in the current frame is zero or no valid data is received, the anchor point is marked as "no data in this frame" and will not participate in subsequent differential calculations.

[0087] Step 302: Calculate the distance difference.

[0088] For each anchor point that has a valid ranging value in both the current frame and the previous frame Calculate the distance difference value:

[0089]

[0090] in, Indicates the first The distance difference value corresponding to each anchor point; For the current frame number The original distance measurement values ​​of each anchor point; For the previous frame The original distance measurement value of each anchor point.

[0091] The ranging difference value Reflects the label relative to the anchor point The change in radial distance between two adjacent frames; a positive value indicates that the label is moving further away from the anchor point. A negative value indicates that the label is close to the anchor point. .

[0092] Step 303: Calculate the anchor point direction vector.

[0093] The estimated tag position is calculated using the localization solution from the previous frame. With the known coordinates of each anchor point The unit direction vector pointing from the UWB tag to each anchor point is calculated using the following formula:

[0094]

[0095] in, The UWB tag points to the first The unit direction vector of each anchor point. This unit direction vector... This is used in step 4 to project the predicted three-dimensional relative displacement values ​​onto the radial direction of each anchor point, thereby comparing them with the UWB ranging difference values ​​for consistency.

[0096] After completing the above calculations, the resulting ranging difference sequence will be... Direction vector sequence The data validity markers for each anchor point are also passed to step 4.

[0097] Step 4: Verify the consistency between the predicted three-dimensional relative displacement and the measured distance change at each anchor point, and determine the NLOS level of each anchor point, which includes reliable, suspected NLOS, and severe NLOS.

[0098] Step 401, Radial Projection of IMU Displacement Prediction Values: The three-dimensional relative displacement prediction values ​​are scalar projected along the unit direction vector of each anchor point in the direction vector sequence to obtain the expected value of the IMU predicted distance change at each anchor point.

[0099] In this step, the predicted three-dimensional relative displacement output from step 204, with an added confidence index, is used. The unit direction vector of each anchor point calculated in step 303 By performing scalar projection, the expected values ​​of the radial distance changes predicted by the IMU at each anchor point are obtained, which is the expected value of the IMU predicted distance change at each anchor point. The projection process is expressed by the formula:

[0100]

[0101] The negative sign is because of the unit direction vector. Defined as the direction the label points to the anchor point, when the label is along the unit direction vector The distance between the label and the anchor point decreases as the direction of movement increases. This projection value... This indicates the label to the anchor point under the motion trajectory predicted by the IMU. The amount of change that the distance should produce.

[0102] Step 402, Residual Calculation: Determine the change in UWB measured distance at each anchor point based on the distance difference sequence, and calculate the residual between the change in UWB measured distance and the expected value of the IMU predicted distance change.

[0103] For each valid anchor point Calculate the residual between the measured UWB distance change and the expected IMU distance change:

[0104]

[0105] in, For the first The ranging difference value corresponding to the nth anchor point, i.e. the nth anchor point The variation in UWB measured distance at each anchor point. This residual. This reflects the degree of inconsistency between UWB ranging results and IMU inertial predictions. Under line-of-sight conditions, UWB ranging accuracy is high, and the residual... The value should be close to zero and randomly distributed; under non-line-of-sight conditions, UWB ranging values ​​are larger due to signal diffraction or reflection, resulting in a higher residual. A significant positive bias will occur.

[0106] Step 403, Sliding window statistics update: Maintain a residual history sliding window for each anchor point, and calculate the residual moving mean and moving standard deviation for that anchor point after each frame update, and set a lower limit for the moving standard deviation.

[0107] To establish a dynamic residual discrimination criterion, for each anchor point Maintain a fixed length of residual history sliding window Calculate the anchor point after each frame update. residual moving mean and sliding standard deviation The calculation formula is as follows:

[0108]

[0109]

[0110] To prevent the standard deviation from approaching zero when the target is stationary for an extended period or the anchor point remains within line of sight, thus preventing overly sensitive subsequent judgments, a lower limit is set for the moving standard deviation. :

[0111]

[0112] in, This represents the function that takes the maximum value.

[0113] Step 404, Mahalanobis distance calculation and NLOS level determination: Calculate the normalized Mahalanobis distance of each valid anchor point based on the residual and sliding statistics of the current frame, and compare the normalized Mahalanobis distance with the preset first threshold and second threshold respectively. Based on the comparison results, the anchor point is determined as credible, suspected NLOS or severe NLOS.

[0114] Based on the residual of the current frame Using the sliding window statistics, calculate the normalized Mahalanobis distance for each valid anchor point, as shown in the following formula:

[0115]

[0116] in, For the first The normalized Mahalanobis distance of each anchor point. Since the residual corresponding to each anchor point in this embodiment... Since it is a one-dimensional scalar residual, the normalized Mahalanobis distance is equivalent to a one-dimensional standardized residual based on the moving mean and moving standard deviation of the residual at that anchor point, which is used to characterize the degree of deviation of the current frame residual from the historical residual distribution at that anchor point.

[0117] According to the normalized Mahalanobis distance Compared with the preset two-level threshold, i.e., the first threshold Second threshold (Assuming) The NLOS level of each anchor point is determined. The NLOS level is divided into three levels, as shown in Table 1.

[0118] Table 1 NLOS Levels

[0119]

[0120] Furthermore, to avoid misjudgments due to insignificant changes in ranging differences between adjacent frames under continuous NLOS conditions, this embodiment imposes a time continuity constraint on the NLOS determination results of each anchor point. Specifically, a continuous suspected NLOS counter is maintained for each anchor point. When the first When an anchor point is identified as a suspected NLOS in the current frame, its consecutive suspected NLOS counter is updated according to the following formula:

[0121]

[0122] in, For the first The anchor point in the current frame (i.e., the first anchor point) The consecutive suspected NLOS counters corresponding to the frames) For the first The anchor point is in the previous frame (i.e., the first anchor point). The consecutive suspected NLOS counters corresponding to the frames.

[0123] When the When an anchor point is determined to be trustworthy in the current frame, its consecutive suspected NLOS counters are reset to zero:

[0124]

[0125] If the first If an anchor point is in a suspected NLOS state for multiple consecutive frames, and the moving average of its residuals consistently exceeds a preset mean offset threshold, then the anchor point's state will be maintained or its NLOS level will be upgraded. Specifically, the NLOS level of the anchor point will be upgraded from suspected NLOS to severe NLOS when the following conditions are met:

[0126]

[0127] in, The threshold for the number of consecutive suspected NLOS frames. A preset mean offset threshold is set. Through the above processing, even when the anchor point is in NLOS for a long time but the ranging difference between adjacent frames does not change significantly, the anchor point state can be maintained or improved by combining the continuous offset of the residual moving mean, thereby enhancing the robustness of NLOS detection.

[0128] Step 405, Observation noise adjustment for suspected NLOS anchor points: For anchor points whose NLOS level is determined to be suspected NLOS, the observation noise variance of the anchor point in the positioning solution is dynamically amplified based on its normalized Mahalanobis distance.

[0129] Anchor points that are suspected of being NLOS in NLOS rating According to its normalized Mahalanobis distance The observation noise variance of the anchor point is dynamically amplified in the positioning solution, so that its weight is automatically reduced in the weighted least squares or Kalman filter update, and the adjusted observation noise variance is... for:

[0130]

[0131] in, The default observation noise variance; This is the amplification factor. The larger the normalized Mahalanobis distance, the greater the observation noise variance of the anchor point, and the lower its contribution weight in the final positioning solution, thus achieving soft weighting rather than hard rejection of suspicious ranging data.

[0132] Step 406, Summary of Anchor Point Reliability Status: Count the number of all available anchor points and record the observation noise variance of each available anchor point.

[0133] Iterate through all anchor points, and group the anchor points marked as "No data in this frame" in step 301 and the anchor points judged as "Severe NLOS" in step 404 into the unusable set. The remaining anchor points are grouped into the usable set. Count the number of usable anchor points in the usable set. Simultaneously, the adjusted observation noise variance of each available anchor point is recorded. The results are then passed to step 5 for location strategy selection.

[0134] Step 5: Adaptively select a positioning strategy based on the number of available anchor points with NLOS ratings of trusted and suspected NLOS and the positioning strategy mapping rules.

[0135] Step 501: Determine the number of available anchor points and map the strategy.

[0136] Receive the number of available anchor points output in step 406 and the set of available anchor point numbers The corresponding positioning strategy is adaptively selected according to the positioning strategy mapping rules shown in Table 2.

[0137] Table 2 Location Strategy Mapping Rules

[0138]

[0139] Step 502: Optimize geometric accuracy factor (Strategy A enabled).

[0140] When the number of available anchor points exceeds four, the differences in spatial geometric configuration of different anchor point subsets will lead to different degrees of amplification of positioning errors. It is necessary to select the anchor point combination with the optimal geometric distribution.

[0141] For the set of available anchor points All 4 anchor point combinations Based on the tag location estimate of the previous frame Calculate the geometrical dilution of precision (GDOP) separately. For the combination... The four anchor points are numbered sequentially as follows: Constructing a geometric matrix :

[0142]

[0143] Geometric matrix Each row in the text is a unit direction vector pointing to the corresponding anchor point. , .

[0144] Next, based on the geometric matrix Calculate the corresponding GDOP value The calculation formula is as follows:

[0145]

[0146] in, Represents the trace operation of a matrix. Selects the anchor combination with the smallest GDOP value. As the subset of anchor points that ultimately participate in the positioning solution, the geometric configuration corresponding to this combination minimizes the amplification effect of the positioning error.

[0147] If all combinations If the matrix determinants are all close to zero (i.e., the anchor points are approximately coplanar or collinear), then GDOP optimization is abandoned, and all available anchor points are used directly in the positioning solution.

[0148] Step 503: IMU inertial navigation timing management (Policy C enabled).

[0149] when When strategy C is triggered, the inertial navigation cumulative timer is started. Record the starting frame number since entering strategy C. It saves the most recent valid positioning position output by strategy A or strategy B as the reference position. Simultaneously, the cumulative displacement under strategy C is initialized as a three-dimensional zero vector:

[0150]

[0151] in, This represents the initial cumulative displacement value before entering the starting frame of strategy C; since no three-dimensional relative displacement prediction values ​​have been accumulated at this time, it is set as the three-dimensional zero vector.

[0152] During the continuous execution of Strategy C, the position calculated by Strategy C in each frame is no longer used as the new reference position. Instead, the predicted three-dimensional relative displacement values ​​of each frame since entering Strategy C are continuously accumulated.

[0153] Because the short-term inertial estimation error under strategy C accumulates rapidly with the duration of the operation, a maximum permissible duration for inertial navigation is set. :when When normal IMU inertial navigation calculations are performed, the estimated tag position is output; when When the location result is marked as "low confidence", it indicates to the upper layer application that the location result is not reliable enough. At the same time, it stops updating the location output and keeps the output of the previous valid location value to avoid the application layer being misled by severely drifted integration results.

[0154] When the number of available anchor points recovers to 3 or more, immediately exit strategy C and reset the inertial navigation cumulative timer. The IMU integration state is then recalibrated based on the current UWB positioning results.

[0155] The selected positioning strategy identifier, the corresponding set of anchor points, and the observation noise parameters are passed to step 6 to perform the positioning calculation.

[0156] Step 6: Execute the selected positioning strategy to perform positioning calculation and obtain the position estimate.

[0157] Step 601: Strategy A - Complete 3D Multilateral Positioning Strategy.

[0158] When strategy A is selected, the subset of anchor points (or all available anchor points) determined in step 502 and the adjusted observation noise variance output in step 405 are used. The weighted Levenberg-Marquardt iterative algorithm is used for three-dimensional polygon localization.

[0159] The weighted centroids of all available anchor points are used as the initial values ​​for the iteration. The formula is as follows:

[0160]

[0161] in, The set of anchor points participating in the positioning solution; anchor point The known coordinates; anchor point The weight.

[0162] In each iteration, the positioning residual between the geometric distance from the current estimated location to each anchor point and the measured UWB distance in the current frame is calculated. For the first iteration involved in the positioning calculation... Each anchor point has a positioning residual. Defined as:

[0163]

[0164] in, This is the estimated label position for the current iteration. For the first The known coordinates of the anchor points For the current frame number The measured UWB distances at each anchor point are calculated. The distance residual vector is composed of the positioning residuals of each participating anchor point. :

[0165]

[0166] in, The number of anchor points involved in the solution. This is the corresponding anchor index.

[0167] Construct the Jacobian matrix, where the Jacobian matrix contains elements that are related to the first... The behavior corresponding to each anchor point:

[0168]

[0169] in, For the Jacobian matrix and the first The row vector corresponding to each anchor point Let T denote the Euclidean norm, and T denote the matrix transpose operation.

[0170] Introducing regularization parameters to solve for position correction :

[0171]

[0172] in, This is a weighted diagonal matrix. This is the distance residual vector in the current positioning solution. It is the identity matrix. For regularization parameters, This represents the matrix transpose operation. It should be noted that... Unlike the residual used for NLOS detection in step 402 residual This is used to determine the consistency between the UWB ranging change and the IMU predicted distance change, and the distance residual vector in this step... Used for Levenberg-Marquardt localization solutions.

[0173] Step 602: Strategy B - IMU height-constrained 3D localization strategy.

[0174] When strategy B is selected, only 3 anchor points are available. Direct 3D polygonal positioning results in redundant degrees of freedom in the vertical direction, leading to unstable solutions in the height direction. Therefore, using an IMU to provide height constraints transforms the problem into a constrained optimization problem.

[0175] First, based on the predicted three-dimensional relative displacement values ​​output in step 204... of Components, calculate the height prediction value of the current frame. The calculation formula is as follows:

[0176]

[0177] in, Position the height value output from the previous frame; Three-dimensional relative displacement prediction value of Quantity.

[0178] Predict the height value of the current frame As a virtual observation, a location solution is introduced to construct the augmented observation equation. Based on the weighted Levenberg-Marquardt iterative framework of step 601, a height constraint equation is added:

[0179]

[0180] in, For location estimation of Quantity, Let be the height constraint function, representing the height residual between the z-component of the position estimate and the IMU height prediction. The Jacobian row vector corresponding to this constraint equation is [0,0,1], and the observation noise variance is set to the final displacement uncertainty in step 204. .

[0181] Augmented Jacobian matrix Add a height constraint row below the original 3-anchor Jacobian matrix, and add a weight matrix. Add height constraint weights to the diagonal lines accordingly, and the iterative solution process is the same as in step 601, outputting a 3D position estimate with height constraints. .

[0182] Step 603: Strategy C – Short-term inertia calculation strategy for residual anchor point constraints.

[0183] When strategy C is selected, the number of available UWB anchor points is less than or equal to 2, which cannot directly support a complete 3D positioning solution. In this case, a short-time inertial estimation strategy with residual anchor point constraints is executed. Let the starting frame for entering strategy C be... The most recent valid positioning position output by strategy A or strategy B is used as the reference position. During the continuous execution of strategy C, the predicted three-dimensional relative displacement values ​​of each frame since entering strategy C are accumulated to obtain the current frame. Cumulative displacement:

[0184]

[0185] in, This represents the cumulative three-dimensional relative displacement up to the current k-th frame; k is the index of the current frame. The starting frame index for entering strategy C; The summation index has a range of values. ; For the first Frame to the Predicted three-dimensional relative displacement between frames.

[0186] Based on the reference position and cumulative displacement Get the initial estimated position of the current frame This can be expressed as a formula:

[0187]

[0188] If one or two available anchor points still exist in the current frame, the available anchor points are used as residual constraint anchor points. For the first... For each residual constraint anchor point, calculate the residual between the unit direction vector pointing from the anchor point to the initially calculated position and the measured UWB distance in the current frame. :

[0189]

[0190] in, For the first The known coordinates of the anchor points For the current frame number UWB measured distance of each anchor point.

[0191] Based on the radial direction of the residual constraint anchor points, a residual radial constraint is constructed, and the initial calculated position is weakly corrected, as expressed by the formula:

[0192]

[0193] in, Calculate the position for the current frame; For the set of residual constraint anchor points; To correct the step size factor; To point from the initially calculated position to the first Unit direction vector of each anchor point; The weights are set based on the NLOS level of the residual constraint anchors; for example, the weights corresponding to trusted anchors. Larger, likely the weight corresponding to the NLOS anchor point. Smaller. When no available anchor point exists in the current frame, let:

[0194]

[0195] Simultaneously calculate the cumulative position uncertainty under strategy C. :

[0196]

[0197] in, Due to the uncertainty of the benchmark position, For the first Uncertainty corresponding to the frame's three-dimensional relative displacement prediction value This is the uncertainty inflation factor when strategy C is executed continuously. The accumulated uncertainty at this position... The number of consecutive execution frames of strategy C increases monotonically, reflecting the continuous decay of positioning accuracy under the condition of insufficient residual anchor points.

[0198] Step 604: Output the positioning results uniformly.

[0199] The three-dimensional position estimates output by strategy A, strategy B, and strategy C respectively. 3D position estimation with height constraints Calculate the location Unified as location estimate This will include location estimates. The location results, including the associated metadata, are passed to step 7. The output fields of the location results are shown in Table 3.

[0200] Table 3. Description of Output Fields for Location Results

[0201]

[0202] Step 7: Estimate the location Perform Kalman filtering to smooth the image and output the final location.

[0203] Step 701: Define the state vector and system model.

[0204] A 9-dimensional state vector is used to model the label's motion state. The definition of the 9-dimensional state vector is as follows:

[0205]

[0206] in, For three-dimensional position components; Three-dimensional velocity components; This represents three-dimensional acceleration components. Each of the three coordinate axes is modeled independently, and each axis uses a uniformly accelerated motion model.

[0207] The state transition equation is:

[0208]

[0209] in, for The state vector at any given time; for The state vector at any given time; The process noise vector follows a zero-mean Gaussian distribution. ; It is a 9×9 state transition matrix, which consists of three identical 3×3 diagonal blocks. Composed of, each diagonal piece A uniformly accelerated model corresponding to a single coordinate axis:

[0210]

[0211] in, The time interval between two adjacent UWB frames calculated in step 103 is the integration time window.

[0212] Step 702: Construction of process noise covariance matrix.

[0213] Process noise covariance matrix It is constructed using a continuous noise discretization model based on acceleration jitter (Jerk), and also consists of three independent 3×3 diagonal blocks. Composed of, each diagonal piece for:

[0214]

[0215] in, This is a process noise intensity parameter, reflecting the unpredictability of the target's motion. The strategy identifier passed in step 604 is dynamically adjusted to reflect the rapid increase in uncertainty of state prediction during short-term inertial extrapolation of strategy C.

[0216] Step 703: Construction of observation model and observation noise matrix.

[0217] Observation matrix Given a 3×9 matrix, extract the three-dimensional position components from the 9-dimensional state vector as observations:

[0218]

[0219] Observation noise covariance matrix Dynamically set based on the strategy identifier and location uncertainty transmitted in step 604:

[0220] When using strategy A, the observation noise covariance matrix The baseline observation noise value is taken to reflect the standard accuracy of UWB multilateral positioning when all anchor points are available; the observation noise covariance matrix at this time is... for:

[0221]

[0222] in, The reference observation noise value.

[0223] When using strategy B and Directional observation noise remains at the baseline value. The direction is appropriately increased due to the IMU height constraint, and the observation noise covariance matrix at this time... for:

[0224]

[0225] in, This represents the uncertainty of the final displacement in step 204.

[0226] When using strategy C, the observation noise covariance matrix is ​​set based on the cumulative uncertainty of the position calculated in step 603. This matrix reflects the uncertainty of the short-time inertial estimation results under the condition of insufficient residual anchor points. The formula is as follows:

[0227]

[0228] in, This refers to the cumulative uncertainty of the location calculated in step 603. This value increases monotonically with the duration of strategy C, causing the filter to gradually reduce its confidence in the IMU's estimated location.

[0229] Step 704: Kalman filter prediction step.

[0230] Using the state transition matrix defined in step 701 The process noise covariance matrix constructed in step 702 The formulas for execution state prediction and covariance prediction are as follows:

[0231]

[0232]

[0233] in, For one-step prediction of the state vector; To predict the covariance matrix in one step; and These are the posterior state estimate and posterior covariance matrix of the previous frame, respectively.

[0234] Step 705: Kalman filter update step.

[0235] The position estimate of the current frame output in step 604 As observation vector Calculate the observation residuals :

[0236]

[0237] Calculate the observation residual covariance matrix :

[0238]

[0239] Calculate the Kalman gain matrix:

[0240]

[0241] Execution status update and covariance update:

[0242]

[0243]

[0244] in, It is a 9×9 identity matrix; This is the updated state vector; This is the updated covariance matrix. When the observation noise covariance matrix... When the noise covariance matrix is ​​relatively small (Strategy A), the Kalman gain is large, the filter trusts the current observation more, and the output position quickly tracks the UWB localization result; when the observation noise covariance matrix is ​​small... When the value is large (Strategy C), the Kalman gain decreases, and the filter relies more on motion model prediction, effectively suppressing the impact of low-quality observation data on the positioning output.

[0245] Step 706: Final location output and status feedback.

[0246] From the updated state vector Extract the three-dimensional position and three-dimensional velocity to obtain the filtered position estimate. and filtered velocity estimate :

[0247]

[0248]

[0249] Filter position estimate The final location of the current frame is output to the upper-layer application, and the filtered velocity estimate is also output. The value is fed back to step 201 as the initial value for the short-time inertial integral of the next frame, and the filtered position estimate is used. The value is sent back to step 303 as the reference position for calculating the anchor point direction vector in the next frame, i.e., the estimated tag position. This forms a complete inter-frame recursive closed loop.

[0250] The indoor positioning method proposed in this embodiment, which integrates ultra-wideband non-line-of-sight detection and adaptive anchor point selection, does not rely on additional hardware information such as channel impulse response or complex statistical hypothesis testing. It only uses the inertial integration of the IMU integrated on the tag side within a short time window to obtain the predicted three-dimensional relative displacement value. By verifying the consistency with the measured distance change in UWB, the NLOS level of each anchor point can be determined in real time. It has low computational load, does not increase hardware cost, and can meet the stringent requirements for positioning latency in dynamic occlusion environments. It effectively solves the problem that existing NLOS detection methods cannot balance real-time performance and accuracy, and achieves lightweight, low-latency real-time detection of non-line-of-sight errors. At the same time, this method abandons the requirement of all anchor points being simultaneously detectable in traditional UWB positioning systems. To overcome the limitations of existing methods, this method dynamically calculates the number of available anchor points based on the NLOS level of each anchor point and designs an adaptive positioning strategy based on the number of available anchor points. This ensures continuous positioning results even when some anchor point signals are lost or severely interfered with by NLOS, avoiding positioning interruptions or data frame drops. It is particularly suitable for complex environments with frequent occlusion, such as industrial plants, underground parking lots, and hospital corridors, significantly improving the continuity and robustness of positioning. Furthermore, this method does not require modification of UWB hardware and only relies on tag-side IMU data, offering advantages such as low computational cost, strong real-time performance, and smooth degradation. It is suitable for continuous indoor positioning scenarios in dynamically occluded environments and can be easily integrated into existing UWB positioning systems, demonstrating high engineering practicality.

[0251] The technical solution of the present invention will be described in detail below with a specific embodiment deployed in an industrial plant.

[0252] In this embodiment, the UWB positioning system is deployed in an industrial plant environment to track the three-dimensional position of mobile workers in real time. Four UWB anchor points are deployed within the plant, with coordinates in the indoor positioning space Cartesian coordinate system as follows: Anchor 0 (-2.1m, 2.6m, 1.2m), Anchor 1 (-2.1m, -2.6m, 1.2m), Anchor 2 (2.1m, -2.6m, 1.2m), and Anchor 3 (2.1m, 2.6m, 2.0m). Workers wear positioning tag devices integrating UWB tags and IMUs. The ranging period of the UWB tag is 33ms (approximately 30Hz), and the sampling rate of the IMU is 100Hz.

[0253] Step 1: Collect raw UWB ranging data and IMU sampling data.

[0254] Step 101: Acquisition and frame parsing of raw UWB ranging data.

[0255] In this embodiment, the UWB anchor points use LinkTrack PB modules, and the UWB tags use LinkTrack P-BT2 modules. The UWB tags interact with each UWB anchor point at a 33ms interval to perform bilateral bidirectional ranging (DS-TWR) to obtain the raw integer ranging values ​​between the UWB tags and each anchor point. The UWB anchor points connected to the host computer serve as data aggregation anchor points, sending the ranging data to the host computer via a serial port (115200bps baud rate). The data frame structure uploaded from the UWB anchor points connected to the host computer to the host computer is shown in Table 4.

[0256] Table 4. Data frame structure definition for uploading from the UWB anchor point connected to the host computer.

[0257]

[0258] The host computer performs frame synchronization detection on the received raw byte stream. It scans the byte buffer byte by byte, matching consecutive combinations of the frame header byte (0x55) and the function identifier byte (0x06) to locate the start of a valid frame. It reads the data length field within the frame and verifies whether the remaining bytes in the buffer meet the full frame length requirement. If not, it waits for subsequent bytes to complete the frame. It increments bytes 3-24 byte by byte, compares the lower 8 bits of the sum with the frame end checksum byte. If the checksum passes, parsing continues; otherwise, the current frame is discarded. The host computer's receiving program maintains a 1024-byte circular buffer, and the serial port receiving thread polls the data at a frequency of 100Hz.

[0259] After successful verification, the original distance bytes of each anchor point are parsed sequentially according to the protocol offset. The distance data of each anchor point is encoded in a three-byte fixed-point format, and the conversion formula is as follows:

[0260]

[0261] in, For the first Anchor point distance data; Number the anchor points; This is the original integer ranging value.

[0262] Simultaneously extract the intra-frame system timestamp (Unit: milliseconds) is used for subsequent time alignment, and the parsed distance array is written to the data buffer of the corresponding tag according to the intra-frame tag ID field, realizing independent management of concurrent data for multiple tags. In this embodiment, taking a certain frame as an example, its parsing result is: Tag ID = 1, the distances of the 4 anchor points are respectively , , , .

[0263] Step 102: IMU data acquisition and preprocessing.

[0264] The IMU continuously outputs sampled data, including triaxial accelerometer data, at a sampling rate no lower than the UWB ranging frequency. (Unit is) ) and three-axis gyroscope data (Unit is) ),in This represents the machine's coordinate system. In this embodiment, the IMU uses a WT901SDCL-BT50 module to continuously output data at a sampling rate of 100Hz.

[0265] During the startup phase, the mean IMU output when the UWB tag is stationary is used as a zero-bias estimate, including the accelerometer bias. and gyroscope bias Subtract the corresponding bias from all subsequent collected data:

[0266]

[0267] in, The data are triaxial accelerations after offset. This is the data from the three-axis gyroscope after deducting the bias.

[0268] Using debiased three-axis gyroscope data The current attitude is updated by integral calculation using quaternion differential equations to obtain the rotation matrix from the body coordinate system to the world coordinate system. The debiased triaxial acceleration data By rotation matrix Transform to world coordinates and subtract the gravitational acceleration vector. To obtain pure motion acceleration:

[0269]

[0270] in, It represents the pure motion acceleration in the world coordinate system.

[0271] Finally, the pure motion acceleration in the world coordinate system... A moving average low-pass filter is applied to filter out high-frequency vibration noise. The window length is determined based on the ratio of the IMU sampling rate to the UWB ranging period, ensuring that the acceleration data within each UWB ranging period is sufficiently smoothed. In this embodiment, the moving average window length is 3 sampling points (corresponding to 3 IMU samples within a 33ms UWB period). Assume the raw IMU acceleration at a certain moment is... After debiasing, coordinate transformation, and gravity compensation, the pure motion acceleration is obtained. .

[0272] Step 103: Time alignment of UWB and IMU data.

[0273] Because the raw UWB ranging data and the IMU sampled data differ in sampling rate and time reference, time synchronization and alignment must be completed before entering the subsequent processing flow.

[0274] Using UWB intra-frame system timestamps Using the base time, extract the timestamp falling within the interval from the IMU data buffer queue. All IMU sampling data within the system are used to achieve time alignment between the raw ranging data and the preprocessed sampling data. The duration of one UWB ranging cycle.

[0275] Record the intra-system timestamp of the previous UWB frame. With the current intra-frame system timestamp The difference between the two is the integration time window (in seconds):

[0276]

[0277] In this embodiment, the current intra-frame system timestamp is The system timestamp in the previous frame is The difference between the two is Extract sampled data with timestamps in the range [4628ms, 4661ms] from the IMU buffer queue, obtaining a total of 4 IMU sampling points (corresponding to timestamps 4628ms, 4638ms, 4649ms, and 4661ms). Pass the IMU data sequence within this time window to step 2 for short-time inertial integral calculation.

[0278] If the difference between the system timestamp of the current frame and the previous frame is... If the preset maximum tolerance threshold is exceeded, it is determined that there is an abnormal jump in the data or a communication interruption. The initial state of the IMU integration is reset, and the IMU auxiliary weight for this frame is reduced to avoid incorrect predictions caused by long-term integration drift. In this embodiment, the maximum tolerance threshold is set to 500ms, and the IMU auxiliary weight is reduced to 0.5 times.

[0279] Step 2: Short-time inertial integration to obtain the predicted three-dimensional relative displacement value, and add a confidence index.

[0280] Step 201: Velocity integration.

[0281] The IMU acceleration sequence within the integration time window transmitted in step 103 is received. In this embodiment, the system timestamp within the current frame is 4661ms, and the system timestamp within the previous frame is 4628ms. The difference between the two is:

[0282]

[0283] Sample data with timestamps in the range [4628ms, 4661ms] were extracted from the IMU buffer queue, resulting in a total of 4 IMU sampling points with corresponding timestamps as follows: , , , The above four sampling points form three integration sub-intervals, and the actual time intervals of each integration sub-interval are as follows:

[0284]

[0285]

[0286] The acceleration data at the four sampling points are as follows:

[0287]

[0288]

[0289]

[0290] The filtered velocity estimate at the UWB time of the above frame As the initial velocity of integration The velocity sequence is calculated point-by-point using the trapezoidal integral method based on actual time intervals.

[0291]

[0292] in, For the first The velocity estimates for each sampling point are obtained. Finally, the velocity estimate at the end of the integration time window is obtained. and the complete velocity sequence In this embodiment, the estimated filtered velocity value of the previous frame is... The velocity sequence was obtained by calculating point by point using the trapezoidal integral method: , , , .

[0293] Step 202: Zero speed detection and drift correction.

[0294] In this embodiment, the acceleration amplitudes at four sampling points are calculated, and the mean acceleration amplitude, variance of acceleration amplitude, and mean angular velocity amplitude are further obtained. A preset rest variance threshold is set to... The preset motion acceleration threshold is The preset angular velocity threshold is The preset speed threshold is According to the combined static criterion, only when the variance of the acceleration amplitude is less than... The average acceleration amplitude is lower than The average amplitude of angular velocity is lower than And the filtering speed amplitude of the previous frame is lower than Only when the target is stationary is it determined that it is stationary.

[0295] In this embodiment, the filtered velocity amplitude of the previous frame is higher than the preset velocity threshold, so it is determined that the target is in motion. Zero velocity update is not performed, the velocity integral result obtained in step 201 is retained, and the velocity sequence is passed to step 203 for displacement integration.

[0296] Step 203: Displacement Integral.

[0297] After completing the motion state determination in step 202, if the target is determined to be stationary within the current integration time window, the displacement integration is performed using the drift-corrected velocity sequence; if the target is determined to be moving within the current integration time window, zero-velocity update is not performed, and the following is set: The velocity integral result obtained in step 201 is used to perform displacement integration.

[0298] In this embodiment, step 202 determines that the target is in motion, therefore zero-velocity update is not performed, and the velocity sequence remains as follows:

[0299]

[0300] in, The first term used for displacement integration Speed ​​per sampling point This indicates the result obtained in step 201. The sampling rate per sampling point.

[0301] In this embodiment, the four IMU sampling points form three integration sub-intervals, and the actual time intervals of the three integration sub-intervals are respectively... , , Therefore, the trapezoidal integral method based on the actual time interval is used to calculate the predicted three-dimensional relative displacement:

[0302]

[0303] Expanding the three integral subintervals above, we have:

[0304]

[0305] The final predicted three-dimensional relative displacement values ​​are:

[0306]

[0307] That is, within a 33ms time window, the target is at the lower edge of the world coordinate system. The axis moves 17mm, along The axis moves 9mm, along The axis moves 1mm.

[0308] Step 204: Confidence estimation of displacement prediction values.

[0309] Since IMU integration inevitably involves cumulative errors, a confidence index needs to be added to the three-dimensional relative displacement prediction values ​​for consistency verification in subsequent step 4.

[0310] It is important to note that and The meanings are different. Among them, This represents the actual time interval between adjacent sampling points inside the IMU, used for velocity integration in step 201 and displacement integration in step 203. This represents the total time interval between two adjacent UWB ranging frames, which is the length of the entire IMU integration time window, used for displacement uncertainty estimation.

[0311] In this embodiment, the 4 IMU sampling points form 3 integral sub-intervals, therefore:

[0312]

[0313] In step 204, the length of the entire integration time window is used for displacement uncertainty estimation. Instead of the time interval of any single integral subinterval. .

[0314] Based on the length of the integration time window and accelerometer noise density Estimate the predicted value of three-dimensional relative displacement Uncertainty covariance:

[0315]

[0316] Uncertainty covariance The calculation formula is based on the error propagation model of white noise acceleration after double integration.

[0317] In this embodiment, the accelerometer noise density Time window The displacement uncertainty covariance was calculated. Simultaneously, a correction factor is introduced based on the target's motion state. When step 202 determines that the target is stationary... (High confidence) When the target is determined to be in motion (Standard confidence level), the final displacement uncertainty is:

[0318]

[0319] In this embodiment, since step 202 determines that the target is in motion, the correction factor is... The final displacement uncertainty is The corresponding standard deviation is approximately 0.069 mm. The predicted three-dimensional relative displacement values... and its final displacement uncertainty This data is also passed to step 4 for consistency verification with the UWB measured distance changes at each anchor point. In this embodiment, the data passed to step 4 includes: predicted three-dimensional relative displacement values. The final displacement uncertainty .

[0320] Step 3: Calculate the ranging difference sequence and the direction vector sequence.

[0321] Step 301: Extract the anchor point distance of the current frame.

[0322] From the ranging data buffer parsed in step 101, read the original ranging values ​​of each anchor point in the current frame according to the tag ID, and record them as follows. ,in The total number of anchor points deployed for the UWB positioning system. Number the anchor points. Simultaneously, read the original distance measurements of each anchor point stored in the previous frame. If an anchor point has a ranging value of zero or has not received valid data in the current frame, then that anchor point is marked as "no data in this frame" and will not participate in subsequent differential calculations. In this embodiment, the system deploys a total of [number missing] anchor points. Read the current frame from the tag data buffer ( The original distance measurement values ​​for each anchor point are: , , , Simultaneously read the previous frame ( The stored distance measurement values ​​for each anchor point are: , , , Since all four anchor points have valid ranging values ​​in both the current and previous frames, they are all marked as "valid data" and will participate in subsequent differential calculations.

[0323] Step 302: Calculate the distance difference.

[0324] For each anchor point that has a valid ranging value in both the current frame and the previous frame Calculate the distance difference value:

[0325]

[0326] in, Indicates the first The distance difference value corresponding to each anchor point; For the current frame number The original distance measurement values ​​of each anchor point; For the previous frame The original distance measurement value of each anchor point.

[0327] The ranging difference value Reflects the label relative to the anchor point The change in radial distance between two adjacent frames. A positive value indicates that the tag is moving away from the anchor point, and a negative value indicates that the tag is moving closer to the anchor point. In this embodiment, the ranging difference values ​​for the four anchor points are calculated as follows:

[0328]

[0329]

[0330]

[0331]

[0332] Step 303: Calculate the anchor point direction vector.

[0333] The estimated tag position is calculated using the localization solution from the previous frame. With the known coordinates of each anchor point Calculate the unit direction vector from which the UWB tag points to each anchor point:

[0334]

[0335] in, The UWB tag points to the first The unit direction vector of each anchor point.

[0336] The unit direction vector This is used in step 4 to project the predicted three-dimensional relative displacement values ​​onto the radial direction of each anchor point, thereby comparing their consistency with the UWB ranging differential values. In this embodiment, the estimated tag position value output from the previous frame's positioning solution is... Based on the known coordinates of each anchor point, calculate the unit direction vector from the label to each anchor point. Taking anchor point 0 as an example:

[0337]

[0338]

[0339]

[0340] And so on, , , .

[0341] After completing the above calculations, the resulting ranging difference sequence will be... Direction vector sequence The data validity markers for each anchor point are also passed to step 4. In this embodiment, the data passed to step 4 includes: the ranging differential sequence. , direction vector sequence , and the “valid data” markers for the four anchor points.

[0342] Step 4: Verify consistency and determine the NLOS level of each anchor point.

[0343] Step 401: Radial projection of IMU displacement prediction values.

[0344] The predicted three-dimensional relative displacement output from step 204, with an added confidence index. By performing a scalar projection along the unit direction vector of each anchor point calculated in step 303, the expected value of the radial distance change of each anchor point predicted by the IMU is obtained, i.e., the expected value of the IMU predicted distance change of each anchor point. Taking anchor point 0 as an example:

[0345]

[0346] And so on, , , .

[0347] Step 402: Residual calculation.

[0348] Combining the UWB measured distance change calculated in step 302 and the expected IMU predicted distance change calculated in step 401, calculate the distance residual corresponding to each anchor point:

[0349]

[0350]

[0351]

[0352]

[0353] The negative residuals at anchor points 0, 1, and 2 indicate that the reduction in the measured distance by UWB is greater than the IMU prediction, suggesting a possible slight ranging error. The negative residual at anchor point 3 indicates that the UWB ranging is consistent with the IMU prediction.

[0354] Step 403: Update sliding window statistics.

[0355] To establish a dynamic residual discrimination criterion, for each anchor point Maintain a fixed length of (For example, 20 to 50 frames can be used; in this embodiment, 30 frames are used) residual history sliding window Calculate the anchor point after each frame update. residual moving mean and sliding standard deviation :

[0356]

[0357]

[0358] To prevent the standard deviation from approaching zero when the target is stationary for an extended period or the anchor point remains within line of sight, thus preventing overly sensitive subsequent judgments, a lower limit is set for the moving standard deviation. :

[0359]

[0360] in, This represents the function that takes the maximum value.

[0361] In this embodiment, The value is set to 0.05 meters. For anchor point 0, its residual history sliding window contains residual data from the past 30 frames, and the moving average is calculated. Sliding standard deviation .because Correct the sliding standard deviation to Similarly, calculate the statistics for other anchor points: anchor point 1's... , Anchor point 2 , Anchor point 3 , .

[0362] Step 404: Normalized Mahalanobis distance calculation and NLOS level determination.

[0363] In this embodiment, The value is 2.0. A value of 4.0 corresponds to approximately the 95% and 99.99% confidence interval boundaries under a normal distribution. The normalized Mahalanobis distance for each anchor point is calculated sequentially, and the NLOS level is determined.

[0364] Anchor point 0: It is determined to be trustworthy (LOS);

[0365] Anchor point 1: It is determined to be trustworthy (LOS);

[0366] Anchor point 2: It is determined to be trustworthy (LOS);

[0367] Anchor point 3: It is determined to be trustworthy (LOS).

[0368] In this embodiment, the normalized Mahalanobis distances of all four anchor points are less than the threshold. All were determined to be in a reliable state, indicating that the current frame UWB ranging data is highly consistent with the IMU prediction and there is no NLOS error interference.

[0369] Step 405: Adjusting observation noise for suspected NLOS anchor points.

[0370] Anchor points that are suspected of being NLOS in NLOS rating According to its normalized Mahalanobis distance The observation noise variance of the anchor point in the positioning solution is dynamically amplified, so that its weight is automatically reduced in weighted least squares or Kalman filter updates.

[0371] In this embodiment, a default observation noise variance is set. Magnification factor Since step 404 determines that all four anchor points are in a reliable state, no observation noise adjustment is required, and the variance of the observation noise for each anchor point remains at the default value after adjustment. .

[0372] Step 406: Summarize the trusted status of anchor points.

[0373] Iterate through all anchor points, and group the anchor points marked as "No data in this frame" in step 301 and the anchor points determined as "Severe NLOS" in step 404 into the unusable set. The remaining anchor points are grouped into the usable set. In this embodiment, all four anchor points are marked as valid data and determined to be in a trusted (LOS) state, and are all grouped into the usable set. Count the number of usable anchor points in the usable set. The variance of the observed noise at each anchor point after adjustment is 1. Set the available anchor points Number of available anchor points The NLOS level of each anchor point (all are reliable) and the adjusted observation noise variance are passed to step 5.

[0374] Step 5: Select a positioning strategy based on the number of available anchor points.

[0375] Step 501: Determine the number of available anchor points and map the strategy.

[0376] Receive the number of available anchor points output in step 406 and the set of available anchor point numbers Based on the positioning strategy mapping rules, strategy A is selected: execute the complete three-dimensional multilateral positioning strategy and perform standard three-dimensional positioning calculation using all available anchor points.

[0377] Step 502: GDOP Preferred (Strategy A Enabled).

[0378] In this embodiment, the number of available anchor points is 4, and there is only one unique combination of 4 anchor points. No GDOP optimization is required.

[0379] Step 503: IMU inertial navigation timing management (Policy C enabled).

[0380] when When strategy C is triggered, the inertial navigation cumulative timer is started. Record the continuous duration since entering strategy C. Because short-term inertial estimation errors accumulate rapidly with duration under strategy C, a maximum permissible duration for inertial navigation is set. (For example, it could be 3 to 5 seconds): When When normal IMU inertial navigation calculations are performed, the position estimate is output; when When this happens, the positioning result is marked as "low confidence," indicating to the upper-layer application that the positioning result lacks confidence. Simultaneously, the output remains unchanged from the previous valid positioning value to prevent severely drifted integration results from misleading the application layer. In this embodiment, since strategy A is selected and strategy C is not triggered, there is no need to start inertial navigation timing management. The inertial navigation cumulative timer is maintained. state.

[0381] The selected positioning strategy identifier, the corresponding set of anchor points, and the observation noise parameters are passed to step 6 to perform the positioning calculation. In this embodiment, the data passed to step 6 includes: the positioning strategy identifier is "Strategy A", and the set of anchor points participating in the calculation. The variance of the observed noise at each anchor point after adjustment is 1. and the current frame ranging value of each anchor point. .

[0382] Step 6: Execute the positioning solution according to the corresponding strategy to obtain the position estimate.

[0383] Step 601: Strategy A - Complete 3D Multilateral Positioning Strategy.

[0384] When strategy A is selected in step 501, such as Figure 2 As shown in (a), the number of available anchor points is greater than or equal to 4, which can support a complete 3D ranging and positioning solution. At this point, step 502 is used to determine the set of anchor points participating in the solution for weighted Levenberg-Marquardt positioning. In this embodiment, the set of anchor points participating in the solution is... Since all four anchor points in this embodiment are determined to be reliable anchor points and have the same observation noise variance, each anchor point has the same weight. The weighted centroids involved in solving the anchor point positions are used as the initial position estimates for the Levenberg-Marquardt iterative algorithm. :

[0385]

[0386] in, For the first The known coordinates of the anchor points For the first The weights of the anchor points are calculated. In each iteration, the positioning residuals between the geometric distance from the current estimated location to each anchor point and the measured UWB distance in the current frame are calculated. For the nth anchor point involved in the positioning calculation... Each anchor point has a positioning residual. Defined as:

[0387]

[0388] in, This is the estimated label position for the current iteration. For the first The known coordinates of the anchor points For the current frame number The measured UWB distances at each anchor point. The distance residual vector is composed of the positioning residuals of each participating anchor point. :

[0389]

[0390] It should be noted that, This is the geometric distance residual vector in the positioning solution of Strategy A, whose elements are the difference between the geometric distance from the current estimated position to the corresponding anchor point and the UWB measured distance of that anchor point in the current frame; this distance residual vector Unlike the residual used for NLOS detection in step 402 residual This represents the difference between the measured distance change by UWB and the expected distance change predicted by the IMU, and is used to determine the consistency between the UWB distance change and the IMU motion prediction.

[0391] Constructing the Jacobian matrix The Jacobian matrix is ​​related to the first... The behavior corresponding to each anchor point:

[0392]

[0393] Introducing regularization parameters Solve for the position correction:

[0394]

[0395] in, This is a weighted diagonal matrix. This is the distance residual vector in the current positioning solution. It is the identity matrix. For regularization parameters, This represents the matrix transpose operation. By introducing a weighted diagonal matrix, the ranging data from reliable anchor points contributes a larger weight in the solution, while the ranging data from suspected NLOS anchor points have their weight automatically reduced.

[0396] In this embodiment, the regularization parameter is set to The convergence threshold is 0.0001m, and the maximum number of iterations is 300. The iteration process is as follows:

[0397] First iteration: The weighted centroid is used as the current estimated position. Since all four anchor points are reliable anchor points and have the same observation noise variance, each anchor point has the same weight, and the weighted centroid is equivalent to the ordinary geometric centroid. Based on the coordinates of anchor points 0, 1, 2, and 3, the initial estimated position is obtained:

[0398]

[0399] Based on the current frame ranging result input in step 5, the initial estimated position is... Substituting into the geometric distance calculation formula, the geometric distances to the four anchor points are calculated to be 3.348m, 3.348m, 3.348m, and 3.396m, respectively. Therefore, the initial positioning residual vector is obtained as follows: The process then iterates based on the Jacobian matrix and the Levenberg-Marquardt update formula. The position estimate after the first iteration is:

[0400]

[0401] The position estimate after the second iteration is:

[0402]

[0403] After continuing the iteration, the estimated position after the 5th iteration is:

[0404]

[0405] When the position correction is less than the preset convergence threshold, the iteration is considered to have converged, and the final output of the 3D position estimate of strategy A is: Substituting the final position estimates into the geometric distance calculation formulas for each anchor point, the final geometric distances are obtained as 4.102m, 4.336m, 4.080m, and 4.365m, respectively, corresponding to the final positioning residual vectors. Based on the root mean square error calculated from the positioning residuals, the position uncertainty index under strategy A is obtained as follows: The corresponding positional uncertainty variance is Finally, the position estimate output by strategy A is... and its location uncertainty Proceed to step 604.

[0406] Step 602: Strategy B - IMU height-constrained 3D localization strategy.

[0407] In this embodiment, since strategy A is selected, the IMU height-constrained 3D localization process of strategy B is not executed. To illustrate how strategy B works, it is assumed that anchor point 2 is determined to be severely NLOS and is removed, leaving only anchor point 2. Available, such as Figure 2As shown in (b) in the figure, 3. If so, strategy B is triggered. The height value of the previous frame will then be... IMU displacement prediction value Quantity Calculate the height prediction value The predicted height is used as a virtual fourth observation, and its observation noise variance is... The corresponding weights are much larger than the UWB anchor weights, which strongly constrains the solution result in the height direction to near the IMU prediction value. The augmented Jacobian matrix is ​​a 4×3 matrix (where the 4 rows include 3 rows of anchor direction vectors and 1 row [0,0,1]), and the output after iterative solution is... .

[0408] Step 603: Strategy C – Short-term inertia calculation strategy for residual anchor point constraints.

[0409] In this embodiment, since the main process selects strategy A, strategy C is not executed. To illustrate how strategy C works, a separate frame-based anchor point degradation scenario will be used for explanation. This degradation scenario is different from the frame in which the main process of strategy A is located.

[0410] In this degradation scenario, assuming anchor 1 and anchor 2 are determined to be severely NLOS and are removed, only anchor 1 remains. It is available, and anchor point 0 is a trusted anchor point, while anchor point 3 is a suspected NLOS anchor point, such as... Figure 2 As shown in (c) above. Assume the most recent valid location output by strategy A is... The predicted three-dimensional relative displacement output in step 204 of the current frame is The initial estimated position of the IMU in the current frame is then... .

[0411] The coordinates of anchor point 0 and anchor point 3 are respectively , The measured UWB distances of anchor point 0 and anchor point 3 in the current frame are respectively set as follows: Calculate the geometric distances from the initial estimated position to anchor point 0 and anchor point 3. , Therefore, the residual radial residuals of the anchor points are as follows: , Calculate the radial unit vector from the anchor point to the initially calculated position. , .

[0412] In this embodiment, anchor point 0 is a trusted anchor point, and anchor point 3 is a suspected NLOS anchor point. Therefore, anchor point 3 is downweighted, and the following weights are set: Let the correction step size coefficient be... Then the residual anchor point constraint correction result of strategy C is:

[0413]

[0414] Substituting the values ​​into the equation:

[0415]

[0416] Therefore, when the number of available anchor points is insufficient to complete the full 3D positioning, strategy C can still use the UWB ranging of the remaining anchor points to make a weak correction to the initial estimated position of the IMU, thereby reducing the drift caused by short-time inertial estimation under conditions without sufficient anchor point constraints.

[0417] Simultaneously calculate the cumulative position uncertainty under strategy C. Let the reference position uncertainty be... The uncertainty corresponding to the IMU displacement prediction value in the current frame is The uncertainty inflation coefficient under strategy C is If the current frame is the first frame after entering strategy C, then the cumulative uncertainty of the position in strategy C is: This value will be passed to step 604 as the uncertainty of the output position of strategy C, and used in step 7 to dynamically set the Kalman filter observation noise covariance matrix.

[0418] Step 604: Output the positioning results uniformly.

[0419] The 3D position estimate output by strategy A Recorded as The strategy identifier is denoted as A, and the number of available anchor points participating in the solution is denoted as A. Location uncertainty is denoted as Each anchor point's NLOS level is recorded as a trusted state and passed to step 7 for Kalman filtering smoothing.

[0420] Step 7: Smooth the image using Kalman filtering and output the final location.

[0421] Step 701: Define the state vector and system model.

[0422] A 9-dimensional state vector is used to model the label's motion state. The definition of the 9-dimensional state vector is as follows:

[0423]

[0424] in, For three-dimensional position components (unit: m); Three-dimensional velocity components (unit: m / s); The acceleration components are in three dimensions (unit: m / s²). Each of the three coordinate axes is modeled independently, and each axis uses a uniform acceleration motion model.

[0425] The state transition equation is:

[0426]

[0427] in, for The state vector at any given time; for The state vector at any given time; The process noise vector follows a zero-mean Gaussian distribution. ; The state transition matrix is ​​9×9. In this embodiment, the time interval between two adjacent UWB frames is... Construct the diagonal blocks of the state transition matrix:

[0428]

[0429] Complete 9×9 state transition matrix Three identical Arranged diagonally. The posterior state estimate of the previous frame is... .

[0430] Step 702: Construction of process noise covariance matrix.

[0431] Process noise covariance matrix The model is constructed using a continuous noise discretization model based on acceleration jitter (Jerk), and also consists of three independent 3×3 diagonal blocks, each of which is:

[0432]

[0433] in, This is a process noise intensity parameter, reflecting the unpredictability of the target's motion. Dynamic adjustments are made based on the policy identifier passed in step 604: under policy A and policy B. Taking the standard value of 0.1, increase it under strategy C. The value is increased to 0.8 to reflect the rapid increase in state prediction uncertainty during short-time inertial extrapolation of strategy C. Therefore, the calculation... The result is:

[0434]

[0435] The complete 9×9 process noise covariance matrix Q consists of three identical It is composed of elements arranged along the diagonal.

[0436] Step 703: Construction of observation model and observation noise matrix.

[0437] Observation matrix Given a 3×9 matrix, extract the three-dimensional position components from the 9-dimensional state vector as observations:

[0438]

[0439] In this embodiment, the strategy identifier passed in step 604 is "Strategy A", and the position uncertainty is... Set the baseline observation noise value. Construct the observation noise covariance matrix :

[0440]

[0441] Step 704: Kalman filter prediction step.

[0442] In this embodiment, the posterior state estimate from the previous frame remains unchanged. (The last part, "from the previous frame state vector," appears to be a separate, unrelated statement and is left untranslated.) and the UWB time interval between two adjacent frames ( After performing state prediction, the following is obtained:

[0443]

[0444] The three-dimensional predicted location is:

[0445]

[0446] The posterior covariance matrix of the previous frame is a 9×9 diagonally dominant matrix. After performing covariance prediction, the predicted covariance matrix is ​​obtained, and its diagonal elements are slightly increased.

[0447] Step 705: Kalman filter update step.

[0448] The position estimate of the current frame output in step 604 As observation vector Calculate the observation residuals:

[0449]

[0450] Calculate the observation residual covariance matrix After inverting the Kalman matrix, calculate the Kalman gain matrix. Perform state updates and covariance updates:

[0451]

[0452] Performing covariance update yields the posterior covariance matrix. .

[0453] Step 706: Final location output and status feedback.

[0454] In this embodiment, from the updated state vector Extract the three-dimensional position and three-dimensional velocity to obtain the filtered position estimate. and filtered velocity estimate :

[0455]

[0456]

[0457] Filter position estimate The final location of the current frame is output to the upper-layer application, and the filtered velocity estimate is also output. The value is fed back to step 201 as the initial value for the short-time inertial integral of the next frame, and the filtered position estimate is used. The value is sent back to step 303 as the reference position for calculating the anchor point direction vector in the next frame, i.e., the estimated tag position. This forms a complete inter-frame recursive closed loop.

[0458] This embodiment fully demonstrates the entire process from UWB / IMU data acquisition, IMU short-time integration, UWB ranging differential, NLOS detection and anchor point classification, positioning strategy selection, hierarchical positioning calculation, to Kalman filter smoothing output. Through the IMU-assisted NLOS detection mechanism, the system outputs high-precision positioning results under ideal conditions where all four anchor points are reliable. When some anchor points experience NLOS or signal loss, the system can automatically switch to IMU height-constrained positioning or short-time inertial estimation mode, maintaining the continuity and robustness of the positioning output. This effectively solves the problems of difficult real-time detection of NLOS errors and positioning interruptions caused by missing anchor points in existing technologies.

[0459] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0460] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. An indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection, characterized in that, Includes the following steps: Step 1: Obtain the raw ranging data between the UWB tag and each anchor point in the UWB positioning system, as well as the sampled data output by the IMU integrated with the UWB tag, and preprocess the sampled data, and perform time alignment on the raw ranging data and the preprocessed sampled data. Step 2: Perform short-time inertial integration on the time-aligned sampled data to obtain the predicted three-dimensional relative displacement of the target in the world coordinate system between two adjacent UWB ranging times, and add a confidence index to the predicted three-dimensional relative displacement. Step 3: Calculate the ranging difference sequence and the direction vector sequence of the UWB tag pointing to each anchor point based on the effective ranging values ​​of each anchor point in the current frame and the previous frame; Step 4: Verify the consistency between the predicted three-dimensional relative displacement value and the measured distance change at each anchor point, and determine the NLOS level of each anchor point. The NLOS level includes reliable, suspected NLOS, and severe NLOS. Step 5: Adaptively select a positioning strategy based on the number of available anchor points with NLOS ratings of trusted and suspected NLOS and the positioning strategy mapping rules; Step 6: Execute the selected positioning strategy to perform positioning calculations and obtain the estimated location value; Step 7: Perform Kalman filtering on the position estimate to obtain the filtered position estimate and the filtered velocity estimate. The filtered position estimate is sent back to Step 3 as the reference position for calculating the anchor point direction vector in the next frame, and the filtered position estimate is output as the final positioning position. The filtered velocity estimate is sent back to Step 2 as the initial value of the short-time inertial integral in the next frame.

2. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 1, characterized in that, In step 1, based on the UWB intra-frame system timestamp, sampled data with timestamps within a preset time window are extracted from the IMU data buffer queue to achieve time alignment between the original ranging data and the preprocessed sampled data. If the difference between the system timestamps of two adjacent frames exceeds the preset maximum tolerance threshold, the initial state of the IMU integration will be reset.

3. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 1 or 2, characterized in that, Step 2 includes the following steps: Step 201: Integrate the IMU acceleration sequence within the integration time window to obtain the velocity sequence; Step 202: Calculate the mean and variance of acceleration amplitude within the integration time window based on the IMU acceleration sequence, and calculate the mean angular velocity amplitude within the integration time window based on the IMU gyroscope sequence; determine whether the mean acceleration amplitude, the variance of acceleration amplitude, the mean angular velocity amplitude, and the filtered velocity amplitude of the previous frame are each less than the corresponding preset threshold. If so, determine that the target is in a stationary state, perform zero-velocity update, and correct the drift of the velocity sequence. Step 203: Perform a second integration on the corrected velocity sequence to obtain the predicted three-dimensional relative displacement value; Step 204: Estimate the uncertainty covariance of the predicted three-dimensional relative displacement based on the integral time window length and accelerometer noise density, and introduce a correction factor according to the motion state to obtain the final displacement uncertainty as the confidence index.

4. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 1 or 2, characterized in that, Step 4 includes the following steps: Step 401: Project the predicted three-dimensional relative displacement value along the unit direction vector of each anchor point in the direction vector sequence to obtain the expected value of the IMU predicted distance change of each anchor point; Step 402: Determine the UWB measured distance change at each anchor point based on the ranging difference sequence, and calculate the residual between the UWB measured distance change and the expected value of the IMU predicted distance change; Step 403: Maintain a residual history sliding window for each anchor point, and calculate the residual sliding mean and sliding standard deviation of the anchor point after each frame update, and set a lower limit value for the sliding standard deviation; Step 404: Calculate the normalized Mahalanobis distance of each valid anchor point based on the residual and sliding statistics of the current frame, and compare the normalized Mahalanobis distance with the preset first threshold and second threshold respectively. Based on the comparison results, determine the anchor point as reliable, suspected NLOS or severe NLOS.

5. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 4, characterized in that, Step 4 also includes the following steps: Step 405: For anchor points identified as suspected NLOS, dynamically amplify the observation noise variance of the anchor point in the positioning solution based on its normalized Mahalanobis distance; Step 406: Count the number of all available anchor points and record the observation noise variance of each available anchor point.

6. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 1 or 2, characterized in that, The positioning strategy mapping rule is as follows: When the number of available anchor points is greater than or equal to 4, a complete three-dimensional polygonal positioning strategy is executed, and the geometric accuracy factor of the available anchor points is optimized. When the number of available anchor points is equal to 3, the IMU height-constrained 3D positioning strategy is executed. When the number of available anchor points is less than or equal to 2, the short-term inertia estimation strategy of the residual anchor point constraint is executed, and the maximum allowable duration of the short-term inertia estimation strategy of the residual anchor point constraint is set.

7. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 6, characterized in that, In the complete three-dimensional multilateral positioning strategy, the process of optimizing the geometric precision factor includes: calculating the geometric precision factor of all 4 anchor point combinations based on the tag position estimate of the previous frame, and selecting the anchor point combination with the smallest geometric precision factor to participate in the positioning solution; When all the anchor points of the combination are approximately coplanar or collinear, the geometric accuracy factor optimization is abandoned and all available anchor points are used in the positioning solution.

8. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 6, characterized in that, In the IMU height-constrained three-dimensional positioning strategy, the height prediction value of the current frame is calculated based on the three-dimensional relative displacement prediction value. The height prediction value is used as a virtual observation to be introduced into the positioning solution. An augmented observation equation and a Jacobian matrix are constructed to form a constraint in the height direction, and the target position estimate with height constraints is output. In the short-time inertial estimation strategy constrained by residual anchor points, when switching to this strategy for the first time, the most recent effective positioning position output by the full 3D multilateral positioning strategy or the IMU height-constrained 3D positioning strategy is recorded as the reference position. The predicted 3D relative displacement values ​​of each frame since entering the short-time inertial estimation strategy constrained by residual anchor points are accumulated to obtain the initial estimated position for the current frame. If there are still one or two available anchor points in the current frame, the UWB measured distance of the available anchor points and the distance from the initial estimated position to the corresponding... The deviation between the geometric distances of the anchor points is used to construct residual radial constraints, which are then used to weakly correct the initial estimated position to obtain the estimated position for the current frame. When there are no available anchor points in the current frame, the initial estimated position is directly used as the estimated position for the current frame. Simultaneously, the cumulative uncertainty of the position is calculated, which monotonically increases with the number of consecutive frames of the short-time inertial estimation strategy of the residual anchor point constraint. When the duration of the short-time inertial estimation strategy of the residual anchor point constraint exceeds the maximum allowable duration, the positioning result is marked as low confidence, and position updates are stopped.

9. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 1 or 2, characterized in that, In step 7, the Kalman filter uses a 9-dimensional state vector to model the tag motion state. The 9-dimensional state vector specifically includes a three-dimensional position component, a three-dimensional velocity component, and a three-dimensional acceleration component. The state transition matrix is ​​constructed based on a uniform acceleration model. The process noise covariance matrix is ​​constructed using a continuous noise discretization model based on acceleration jitter and consists of three identical 3×3 diagonal blocks along the diagonal.

10. The indoor positioning method integrating ultra-wideband non-line-of-sight detection and adaptive anchor point selection according to claim 9, characterized in that, The observation noise covariance matrix of the Kalman filter is dynamically set according to the positioning strategy selected in step 5: When a complete three-dimensional multilateral positioning strategy is adopted, the observation noise covariance matrix is ​​taken as the baseline observation noise value; When using an IMU-height-constrained 3D localization strategy, the observation noise covariance matrix is ​​in and Maintaining the reference observation noise value in the direction, Increase the observation noise value in the direction; When a short-time inertia estimation strategy with residual anchor point constraints is adopted, the observation noise covariance matrix is ​​set according to the position cumulative uncertainty under this strategy.

Images