Indoor three-dimensional positioning method based on ultra-wideband ultrasonic wave homologous arrival time difference ranging

Abstract

An indoor three-dimensional positioning method based on ultra-wideband ultrasonic wave homologous arrival time difference ranging, the method comprising: in the same positioning period, the trigger control modules of m anchor points respectively emit ultra-wideband signals and ultrasonic signals with synchronization or known transmission time delay relationship to the to-be-positioned tag; the to-be-positioned tag receives the ultra-wideband signals and ultrasonic signals, and determines the arrival time of the ultra-wideband signals and the arrival time of the ultrasonic signals of each anchor point, extracts the arrival time difference of the ultra-wideband signals and the ultrasonic signals emitted by the same anchor point at the tag end; the distance estimation module calculates the distance from the to-be-positioned tag to each anchor point according to the signal arrival time difference of each anchor point, and forms a distance observation vector composed of the distances of m anchor points; the unscented Kalman filter module recursively fuses the distance observation vector and the motion state model, and outputs the three-dimensional position, three-dimensional velocity and three-dimensional acceleration of the to-be-positioned tag. Effect: the positioning accuracy of indoor dynamic targets can be improved.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning technology, and in particular to an indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging. Background Technology

[0002] Indoor positioning is a fundamental technology in mobile robots, unmanned vehicles, drones, industrial logistics, smart warehousing, augmented reality, personnel safety management, and automated detection systems. Existing indoor positioning solutions include visual positioning, LiDAR positioning, inertial positioning, magnetic field positioning, WiFi positioning, Bluetooth positioning, ultra-wideband positioning, and acoustic positioning. Different solutions vary in accuracy, cost, deployment complexity, occlusion sensitivity, and real-time performance.

[0003] Ultra-wideband (UWB) positioning technology utilizes wide-bandwidth signals to achieve high time resolution, reaching centimeter-level or decimeter-level positioning accuracy under good line-of-sight conditions. However, common two-way ranging schemes require multiple communication interactions; time difference of origin (TDOA) positioning schemes typically rely on high-precision clock synchronization between anchor points; and personnel positioning and construction AOA schemes usually require antenna arrays and depend on array calibration. In complex indoor environments, non-line-of-sight propagation, multipath interference, and obstructions can still lead to ranging anomalies.

[0004] Ultrasonic positioning utilizes the low speed of sound to achieve higher distance resolution for the same time measurement resolution. However, ultrasound is susceptible to the effects of temperature, humidity, airflow, obstruction, and reflection, and when used alone, it may suffer from problems such as large propagation delay, insufficient concurrent operation capability, and acoustic multipath error.

[0005] Existing integrated positioning schemes typically fuse positioning results from UWB, inertial navigation, ultrasonic, or other sensors at the result level, or perform clock synchronization, TDOA calculation, non-line-of-sight (NLOS) identification, and filtering optimization within the UWB ranging link. These schemes do not fully utilize the physical mechanism by which the time difference of arrival (TDOA) formed at the tag after the same anchor point transmits electromagnetic and acoustic signals within the same positioning cycle directly calculates the distance.

[0006] The shortcomings of existing technologies: Existing indoor dynamic target positioning methods do not fully utilize the physical mechanism of directly calculating the distance by using the time difference of arrival formed at the tag after the same anchor point emits electromagnetic and acoustic signals in the same positioning cycle. This results in poor stability and low positioning accuracy. Summary of the Invention

[0007] This invention provides an indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging, which can improve the positioning accuracy of dynamic targets indoors.

[0008] To achieve the above objectives, this invention provides an indoor three-dimensional positioning method based on ultra-wideband ultrasonic time-difference-of-arrival ranging, which, as a key feature, includes the following steps:

[0009] Step 1: Construct an indoor 3D positioning system based on ultra-wideband ultrasonic co-source time difference of arrival ranging and unscented Kalman filtering. The indoor 3D positioning system is equipped with anchor point infrastructure, tag hardware front-end and algorithm back-end connected in sequence.

[0010] The anchor infrastructure includes m anchor points deployed within the indoor positioning area, each anchor point being equipped with a trigger control module, an ultra-wideband transmission module, and an ultrasonic transmission module;

[0011] The tag-end hardware front end is a mobile device equipped with a tag to be located. The tag to be located is equipped with an ultra-wideband receiving module, an ultrasonic receiving module, and a time difference of arrival extraction unit. The ultra-wideband receiving module and the ultrasonic receiving module are respectively connected to the time difference of arrival extraction unit.

[0012] The algorithm backend is provided with a sound speed compensation module, a distance estimation module and an unscented Kalman filter module connected in sequence. The sound speed compensation module is also connected to a temperature and humidity sensor deployed in the indoor positioning area. The distance estimation module is also connected to the time difference of arrival extraction unit. The unscented Kalman filter module establishes a motion state model of the tag to be located.

[0013] Step 2: Within the same positioning cycle, the trigger control modules of m anchor points respectively control the ultra-wideband transmission module and ultrasonic transmission module of the corresponding anchor point to transmit ultra-wideband signals and ultrasonic signals with synchronous or known transmission delay relationships to the tag to be positioned;

[0014] Within the same positioning cycle, m anchor points transmit ultrasonic signals using time-division polling, frequency-division multiplexing, code-division multiplexing, or a combination thereof, in order to reduce aliasing or echo overlap between ultrasonic signals from different anchor points.

[0015] Step 3: The ultra-wideband receiving module and ultrasonic receiving module in the tag to be located receive ultra-wideband signals and ultrasonic signals from m anchor points respectively, and then the arrival time of the ultra-wideband signal at each anchor point is determined by the time difference of arrival extraction unit. and the arrival time of the ultrasonic signal The time difference of arrival of ultra-wideband signals and ultrasonic signals emitted from the same anchor point at the tag end is extracted and sent to the algorithm backend.

[0016] Step 4: The sound speed compensation module in the algorithm backend acquires temperature and humidity data from the temperature and humidity sensor and calculates the current ambient sound speed. And pass it to the distance estimation module;

[0017] The distance estimation module is based on the signal arrival time difference at each anchor point and the current ambient sound speed. The distance from the tag to be located to each anchor point is calculated. This forms a distance observation vector consisting of the distances between m anchor points. And pass it to the unscented Kalman filter module;

[0018] Step 5: The unscented Kalman filter module processes the distance observation vector. The three-dimensional position of the tag to be located is output by recursively fusing the motion state model. 3D velocity and three-dimensional acceleration .

[0019] Through the above design, this invention deploys multiple anchor points within the positioning area. Each anchor point emits ultra-wideband (UWB) and ultrasonic signals with synchronous or known transmission delay relationships within the same positioning cycle. The tag to be positioned determines the arrival time of the UWB and the arrival time of the ultrasonic signal from the same anchor point, and calculates the distance from the tag to the corresponding anchor point based on their arrival time difference, known transmission delay, fixed hardware delay offset, and ambient sound velocity. The ambient sound velocity can be updated online according to temperature and humidity. The distances of multiple anchor points constitute a distance observation vector. By establishing a motion state model including three-dimensional position, velocity, and acceleration, an unscented Kalman filter module is used to recursively fuse the distance observation vector and the motion state model, ultimately outputting the three-dimensional position, velocity, and acceleration of the tag to be positioned.

[0020] This invention utilizes the high-speed propagation of electromagnetic signals to provide a local time reference, and the low-speed propagation of ultrasonic waves to improve the resolution of time difference relative to distance. Through sound speed compensation, hardware delay calibration, and abnormal measurement processing, it effectively improves the accuracy, robustness, and stability of indoor dynamic target positioning.

[0021] Preferably, in step 3, the arrival time difference extraction unit performs at least one of the following processing on the ultrasonic signal: bandpass filtering, envelope detection, threshold detection, correlation detection, and matched filtering, to determine the arrival time of the ultrasonic wave; and performs at least one of the following processing on the ultra-wideband signal: timestamp reading, preamble detection, first path detection, or arrival time estimation, to determine the arrival time of the ultra-wideband signal.

[0022] Preferably, in step 4, the sound speed compensation module calculates the current ambient sound speed. The expression is:

[0023] ;

[0024] in, For the current ambient speed of sound, The current ambient temperature. This represents the current relative humidity.

[0025] Current ambient speed of sound Based on the current ambient temperature and relative humidity Updated online.

[0026] Environmental sensors can be installed at locations such as tags, anchor points, or representative locations within a defined area. For large spaces with significant temperature gradients, multiple environmental sensors can be configured, and sound velocity can be estimated in zones or spatially interpolated.

[0027] Preferably, in step 4, the distance estimation module calculates the distance from the tag to be located to each anchor point. The expression is:

[0028] ;

[0029] in, For from the Ultrasonic arrival time at each anchor point For from the Ultra-wideband arrival time of each anchor point For the first The known transmission delay of the ultrasonic signal relative to the ultra-wideband signal at each anchor point For the first The fixed hardware delay bias corresponding to each anchor point For the current ambient speed of sound, The speed of electromagnetic wave propagation. To locate the tag to the The distance between the anchor points.

[0030] because ≫ Therefore, in the calculation of the distance from the tag to be located to each anchor point, the denominator mainly consists of 1 / Therefore, a small change in the arrival time of the ultrasound will correspond to a recognizable change in distance; at the same time, the arrival time of the ultra-wideband signal provides a local time reference at the tag end, which helps to reduce the explicit clock synchronization dependency between the tag and the anchor point.

[0031] This invention extracts the arrival time difference between the ultra-wideband signal and the ultrasonic signal emitted from the same anchor point at the tag end, and combines the known transmission delay, fixed hardware delay offset, and ambient sound speed to realize single anchor point distance estimation and the construction of multi-anchor point distance observation vectors.

[0032] As a preferred option: when the first When the ultra-wideband signal and ultrasonic signal are transmitted synchronously at each anchor point =0; when the first When the ultra-wideband signal and ultrasonic signal at each anchor point are transmitted according to a known time delay relationship... The transmit delay is either pre-calibrated or recorded by the trigger control module.

[0033] Preferably, the fixed hardware delay bias includes, but is not limited to, UWB timestamp bias, ultrasonic emission drive delay, electroacoustic conversion delay, analog front-end delay, sampling delay, threshold detection delay, and related detection bias.

[0034] The fixed hardware delay offset can be calibrated at known distance points during the installation or maintenance phase. During calibration, the ultra-wideband arrival time and ultrasonic arrival time are collected at each calibration point, and the fixed hardware delay offset is calculated back based on the known distance, the current ambient sound speed, and the electromagnetic wave propagation speed. Alternatively, the fixed hardware delay offset and the proportional error correction parameter can be determined simultaneously through multi-calibration point fitting.

[0035] Preferably, in step 5, the state vector of the motion state model is:

[0036] ;

[0037] in, For three-dimensional position, For three-dimensional velocity, For three-dimensional acceleration, superscript Indicates matrix transpose;

[0038] The motion state model is a constant acceleration model, and its state update expression is:

[0039] ;

[0040] The constant acceleration model satisfies:

[0041] ;

[0042] in, Let be the motion state transition matrix. For a moment, For process noise, satisfy normal distribution;

[0043] ;

[0044] in, It is a third-order identity matrix. It is a third-order zero matrix. The sampling period;

[0045] At time k, the distance observation vector is composed of the distances of m anchor points. The expression is:

[0046] ;

[0047] The distance observation function expression is:

[0048] ;

[0049] ;

[0050] in, This is the distance observation vector. For distance observation function, To measure noise, satisfy normal distribution; Let k be the three-dimensional position of the label to be located at time k. Let m be the three-dimensional position of the m-th anchor point.

[0051] Preferably, in step 5, the unscented Kalman filter module applies the distance observation vector... The recursive fusion with the motion state model is performed as follows:

[0052] Step 5.1: The unscented Kalman filter module first uses the average state value from the previous time step... State covariance structure There are Sigma state points, expressed as:

[0053] ;

[0054] ;

[0055] ;

[0056] in, This represents the mean state at time k-1. Let be the state covariance at time k-1. For the state dimension, This represents the j-th Sigma state point. For scale parameters, Here are the scale parameters for the Sigma state point distribution. This is a second-order scale adjustment parameter;

[0057] Step 5.2: Each Sigma state point is propagated through the motion model to obtain the predicted Sigma state points. The expression is:

[0058] , ;

[0059] And based on the predicted Sigma state points The mean of the predicted state is calculated. and predicted state covariance The expression is:

[0060] ;

[0061] ;

[0062] ;

[0063] ;

[0064] ;

[0065] in, Weighted by mean, For covariance weights, These are the prior parameters of the state distribution. For process noise covariance, This is the motion state transition matrix;

[0066] Step 5.3: Then, input the predicted Sigma state point into the distance observation function to obtain the predicted Sigma measurement point. The expression is:

[0067] , ;

[0068] And based on the predicted Sigma measurement points The predicted mean value was calculated. Innovation Covariance State-Measurement Cross-Covariance The expression is:

[0069] ;

[0070] ;

[0071] ;

[0072] in, To measure the noise covariance, For distance observation function;

[0073] The parameters of the unscented Kalman filter module can be adjusted based on the motion intensity of the tag to be located, the variance of the ranging noise, and the sampling period. , , , , The value of .

[0074] Step 5.4: Finally, the mean of the predicted state is calculated using Kalman gain. and predicted state covariance Make corrections to obtain the mean state at the current time. State covariance The expression is:

[0075] ;

[0076] ;

[0077] ;

[0078] in, For Kalman gain, For state-measurement cross-covariance, To innovate covariance, To predict the measurement mean, This is the distance observation vector;

[0079] mean of the current state The three-dimensional location of the label to be located 3D velocity and three-dimensional acceleration ,Right now: .

[0080] Before the measurement update of the unscented Kalman filter module, abnormal observation processing is performed on the distance of one or more anchor points in the distance observation vector. When one or more anchor points have observation residuals exceeding the threshold, ultrasonic correlation peak values ​​below the threshold, ultra-wideband first diameter quality below the threshold, non-line-of-sight criteria being met, or anchor point geometry not meeting preset conditions, the corresponding anchor point distances are processed by elimination, weight reduction, gating, or increase of measurement noise variance to ensure the quality of the distance observation vector and ensure the positioning accuracy of the subsequent tags to be located.

[0081] After obtaining the distance observation vectors of multiple anchor points, a motion state model and an unscented Kalman filter module are used for prediction and measurement updates, and the target's three-dimensional position, velocity and acceleration are recursively output.

[0082] Preferably, the tag terminal hardware front end is either a mobile robot operating indoors, an unmanned vehicle operating indoors, an unmanned aerial vehicle operating indoors, or any device equipped with a tag to be located and whose location information is to be obtained.

[0083] The beneficial effects of this invention are:

[0084] (1) Reduced Synchronization Dependence. This invention utilizes the time difference of arrival of two signals from the same source, namely ultra-wideband signals and ultrasonic signals, for one-way ranging. Among them, the ultra-wideband signals propagate at near the speed of light and can provide a local time reference at the tag end; the ultrasonic signals, due to their low propagation speed, are more sensitive to distance changes in their arrival delay, effectively solving the problem of high dependence on precise explicit clock synchronization between the tag to be located and the anchor point in traditional indoor positioning methods.

[0085] (2) Improved ranging resolution. Under the same time-resolution conditions, the ultrasonic wave propagation speed is relatively low, which amplifies the distance change due to the arrival time difference of the ultra-wideband signal and the ultrasonic signal, greatly improving the ranging resolution of the system and making it easier to obtain millimeter-level distance resolution.

[0086] (3) Reduced system deviation. This invention reduces system errors introduced by hardware link and environmental changes by correcting known delay at the transmitter, calibrating fixed hardware delay, and compensating for temperature, humidity and sound speed, thereby improving the system positioning accuracy and stability.

[0087] (4) Enhanced dynamic tracking capability. By integrating a motion state model that includes position, velocity and acceleration with the unscented Kalman filter (UKF) recursive fusion, continuous state estimation can be maintained when there is noise, occlusion or short-term anomalies in the distance observations at multiple anchor points, thereby improving the dynamic target localization capability of the system.

[0088] (5) The system can be implemented using mature UWB devices, ultrasonic transducers, environmental sensors and embedded processors. It is suitable for various indoor positioning application scenarios such as mobile robots, AGV / AMR, drones, industrial warehousing and laboratory automation, and has strong practicality. Attached Figure Description

[0089] Figure 1 This is a flowchart of the method of the present invention;

[0090] Figure 2 The diagram shows the structural block diagram of the indoor three-dimensional positioning system based on ultra-wideband ultrasonic co-source time difference of arrival ranging and unscented Kalman filtering in the embodiment.

[0091] Figure 3 For the example

[0092] Figure 4 This is a timing comparison diagram of positioning errors in the embodiment;

[0093] Figure 5 This is a comparison chart of the empirical distribution function of positioning error in the embodiments;

[0094] Figure 6 This is a triaxial residual plot from an example embodiment;

[0095] Figure 7 The image shows the three-dimensional trajectory estimation results in the example. Detailed Implementation

[0096] The present invention will be further described in detail below with reference to the accompanying drawings and specific examples. The following embodiments or drawings are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0097] like Figure 1 As shown, an indoor three-dimensional positioning method based on ultra-wideband ultrasonic time-difference-of-arrival ranging includes the following steps:

[0098] Step 1: Construct an indoor 3D positioning system based on ultra-wideband ultrasonic co-source time difference of arrival ranging and unscented Kalman filtering. The indoor 3D positioning system is equipped with anchor point infrastructure, tag hardware front-end and algorithm back-end connected in sequence.

[0099] The anchor infrastructure includes m anchor points deployed within the indoor positioning area, each anchor point being equipped with a trigger control module, an ultra-wideband transmission module, and an ultrasonic transmission module;

[0100] The tag-end hardware front end is a mobile device equipped with a tag to be located. The tag to be located is equipped with an ultra-wideband receiving module, an ultrasonic receiving module, and a time difference of arrival extraction unit. The ultra-wideband receiving module and the ultrasonic receiving module are respectively connected to the time difference of arrival extraction unit.

[0101] The algorithm backend is configured with a sound speed compensation module, a distance estimation module, and an unscented Kalman filter module connected in sequence. The sound speed compensation module is also connected to a temperature and humidity sensor deployed within the indoor positioning area. The distance estimation module is also connected to the time difference of arrival extraction unit. The unscented Kalman filter module establishes a motion state model of the tag to be located, such as... Figure 2 , Figure 3 As shown;

[0102] The algorithm backend can be integrated into the tag to be located, or it can be deployed on edge computing nodes, host computers, or servers.

[0103] Step 2: Within the same positioning cycle, the trigger control modules of m anchor points respectively control the ultra-wideband transmission module and ultrasonic transmission module of the corresponding anchor point to transmit ultra-wideband signals and ultrasonic signals with synchronous or known transmission delay relationships to the tag to be positioned;

[0104] Within the same positioning cycle, m anchor points transmit ultrasonic signals using time-division polling, frequency-division multiplexing, code-division multiplexing, or a combination thereof, in order to reduce aliasing or echo overlap between ultrasonic signals from different anchor points.

[0105] Step 3: The ultra-wideband receiving module and ultrasonic receiving module in the tag to be located receive ultra-wideband signals and ultrasonic signals from m anchor points respectively, and then the arrival time of the ultra-wideband signal at each anchor point is determined by the time difference of arrival extraction unit. and the arrival time of the ultrasonic signal The time difference of arrival of ultra-wideband signals and ultrasonic signals emitted from the same anchor point at the tag end is extracted and sent to the algorithm backend.

[0106] Step 4: The sound speed compensation module in the algorithm backend acquires temperature and humidity data from the temperature and humidity sensor and calculates the current ambient sound speed. And pass it to the distance estimation module;

[0107] The distance estimation module is based on the signal arrival time difference at each anchor point and the current ambient sound speed. Based on the known transmission delay relationship and fixed hardware delay offset, the distance from the tag to be located to each anchor point is calculated. This forms a distance observation vector consisting of the distances between m anchor points. And pass it to the unscented Kalman filter module;

[0108] Step 5: The unscented Kalman filter module processes the distance observation vector. The three-dimensional position of the tag to be located is output by recursively fusing the motion state model. 3D velocity and three-dimensional acceleration .

[0109] In this embodiment, the ultra-wideband transmitting module and the ultra-wideband receiving module adopt UWB devices that conform to IEEE 802.15.4z; the ultrasonic transmitting module adopts a 40kHz ultrasonic transducer; and the ultrasonic receiving module consists of a microphone, a bandpass filter circuit and an analog-to-digital converter.

[0110] In step 3, the arrival time difference extraction unit performs at least one of the following processing on the ultrasonic signal: bandpass filtering, envelope detection, threshold detection, correlation detection, and matched filtering, to determine the arrival time of the ultrasonic wave; and performs at least one of the following processing on the ultra-wideband signal: timestamp reading, preamble detection, first path detection, or arrival time estimation, to determine the ultra-wideband arrival time.

[0111] Ultrasonic arrival time It can be obtained through one or a combination of the following methods:

[0112] 1. Perform bandpass filtering on the received ultrasonic signal;

[0113] 2. Calculate the signal envelope or energy curve;

[0114] 3. Perform threshold detection to obtain the initial arrival time;

[0115] 4. Perform cross-correlation or matched filtering with the preset ultrasonic emission sequence;

[0116] 5. Interpolate the relevant peaks to improve the resolution of arrival time estimation.

[0117] In step 4, the sound speed compensation module calculates the current ambient sound speed. The expression is:

[0118] ;

[0119] in, The current ambient sound speed is expressed in m / s. The current ambient temperature is expressed in °C. The relative humidity is the current ambient humidity, expressed as a percentage. For large spaces with significant temperature gradients, multiple environmental sensors can be configured, and sound velocity can be estimated or spatially interpolated for different zones.

[0120] For the For each anchor point, the corrected arrival time difference obtained from the tag end is:

[0121] ;

[0122] If the positioning tag is to be reached at the The geometric distance between the anchor points is The speed of electromagnetic wave propagation is The current ambient speed of sound is In the ideal case:

[0123] ;

[0124] Considering fixed hardware delay bias Then, the distance is estimated as follows:

[0125] ;

[0126] Therefore, in step 4, the distance estimation module calculates the distance from the tag to be located to each anchor point. The expression is:

[0127] ;

[0128] in, For from the Ultrasonic arrival time at each anchor point For from the Ultra-wideband arrival time of each anchor point For the first The known transmission delay of the ultrasonic signal relative to the ultra-wideband signal at each anchor point For the first The fixed hardware delay bias corresponding to each anchor point For the current ambient speed of sound, The speed of electromagnetic wave propagation. To locate the tag to the The distance between the anchor points.

[0129] When the When the ultra-wideband signal and ultrasonic signal are transmitted synchronously at each anchor point =0; when the first When the ultra-wideband signal and ultrasonic signal at each anchor point are transmitted according to a known time delay relationship... The transmit delay is either pre-calibrated or recorded by the trigger control module.

[0130] The fixed hardware delay bias includes, but is not limited to, UWB timestamp bias, ultrasonic emission drive delay, electroacoustic conversion delay, analog front-end delay, sampling delay, threshold detection delay, and related detection bias.

[0131] This fixed hardware delay bias is calibrated at a known distance point during the installation or maintenance phase. The first place to proceed Then, for the second calibration:

[0132] ;

[0133] in, This represents the fixed hardware delay bias at the nth calibration. The fixed bias and scaling error can be determined by averaging multiple calibration points or by least-squares fitting to correct the parameters.

[0134] The fixed hardware delay bias is obtained through one or more known distance calibration points. During the calibration process, the arrival time of the ultra-wideband and the arrival time of the ultrasonic wave at each calibration point are collected, and the fixed hardware delay bias is calculated back based on the known distance, the current ambient sound speed and the electromagnetic wave propagation speed, or the fixed hardware delay bias and the proportional error correction parameter are determined simultaneously through multi-calibration point fitting.

[0135] In step 5, the state vector of the motion state model is:

[0136] ;

[0137] in, For three-dimensional position, For three-dimensional velocity, For three-dimensional acceleration, superscript Indicates matrix transpose;

[0138] The motion state model is a constant acceleration model, and its state update expression is:

[0139] ;

[0140] in, Let be the motion state transition matrix. For a moment, For process noise, satisfy normal distribution;

[0141] ;

[0142] in, It is a third-order identity matrix. It is a third-order zero matrix. The sampling period;

[0143] At time k, the distance observation vector is composed of the distances of m anchor points. The expression is:

[0144] ;

[0145] The distance observation function expression is:

[0146] ;

[0147] ;

[0148] in, This is the distance observation vector. For distance observation function, To measure noise, satisfy normal distribution; Let k be the three-dimensional position of the label to be located at time k. Let m be the three-dimensional position of the m-th anchor point.

[0149] In step 5, the unscented Kalman filter module applies the distance observation vector. The recursive fusion with the motion state model is performed as follows:

[0150] Step 5.1: The unscented Kalman filter module first uses the mean state value from the previous time step... State covariance structure There are Sigma state points, expressed as:

[0151] ;

[0152] ;

[0153] ;

[0154] in, This represents the mean state at time k-1. Let be the state covariance at time k-1. For the state dimension, , This represents the j-th Sigma state point. For scale parameters, Here are the scale parameters for the Sigma state point distribution. This is a second-order scale adjustment parameter;

[0155] Step 5.2: Each Sigma state point is propagated through the motion model to obtain the predicted Sigma state points. The expression is:

[0156] , ;

[0157] And based on the predicted Sigma state points The mean of the predicted state is calculated. and predicted state covariance The expression is:

[0158] ;

[0159] ;

[0160] ;

[0161] ;

[0162] ;

[0163] in, Weighted by mean, For covariance weights, These are the prior parameters of the state distribution. For process noise covariance, This is the motion state transition matrix; in this embodiment... , and .

[0164] Step 5.3: Then, input the predicted Sigma state point into the distance observation function to obtain the predicted Sigma measurement point. The expression is:

[0165] , ;

[0166] And based on the predicted Sigma measurement points The predicted mean value was calculated. Innovation Covariance State-Measurement Cross-Covariance The expression is:

[0167] ;

[0168] ;

[0169] ;

[0170] in, To measure the noise covariance, For distance observation function;

[0171] Step 5.4: Finally, the mean of the predicted state is calculated using Kalman gain. and predicted state covariance Make corrections to obtain the mean state at the current time. State covariance The expression is:

[0172] ;

[0173] ;

[0174] ;

[0175] in, For Kalman gain, For state-measurement cross-covariance, To innovate covariance, To predict the measurement mean, This is the distance observation vector;

[0176] mean of the current state The three-dimensional location of the label to be located 3D velocity and three-dimensional acceleration ,Right now: .

[0177] Anomaly Handling: Before updating measurements, anomaly handling can be performed on the distance observation vector. If an anchor point meets one of the following conditions, the corresponding distance observation will be removed, downweighted, gated, or have its measurement noise variance increased:

[0178] (1) The observation residual exceeds the preset threshold;

[0179] (2) The ultrasonic correlation peak value is lower than the preset threshold;

[0180] (3) The quality index of the first diameter of the ultra-wideband is lower than the preset threshold;

[0181] (4) The non-line-of-sight criterion holds;

[0182] (5) The geometry of the anchor point results in insufficient contribution of this observation to the current three-dimensional positioning;

[0183] (6) The ultrasonic signal of the anchor point may be superimposed with the echoes of other anchor points or the environment during the current positioning cycle.

[0184] The measurement noise covariance R can be adaptively adjusted based on anchor quality, correlation peak value, temperature and humidity changes, UWB reception quality, and NLOS discrimination results.

[0185] The tag terminal hardware front end can be a mobile robot operating indoors, an unmanned vehicle operating indoors, a drone operating indoors, or any device equipped with a tag to be located and whose location information is to be obtained.

[0186] In one simulation embodiment, the positioning area was 15m × 15m × 5m, with m = 6 anchor points deployed. The target moved along a 60s three-dimensional trajectory at a sampling rate of 50Hz. The UWB+US+UKF method yielded an RMSE of 1.655mm and an R95 of 3.120mm.

[0187] In one experimental embodiment, the positioning area was 15m × 15m × 3m, with N = 6 anchor points deployed. The tag to be positioned was installed on the unmanned vehicle, and the external optical tracking system served as the reference ground truth. The UWB+US+UKF method yielded an RMSE of 1.8mm and an R95 of 3.5mm, as shown in Table 1.

[0188]

[0189] In Table 1, UWB represents ultra-wideband, US represents ultrasound, UKF represents Kalman filter, EKF represents extended Kalman filter, PF represents particle filter, RMSE represents root mean square error, and R95 represents tail error.

[0190] Combination Figures 4 to 7 As can be seen, under the conditions of the above embodiments, UWB+US+UKF has a lower mean error and a smaller tail error compared to UWB-only and US-only baselines.

[0191] in, Figure 4This is a time-series comparison chart of positioning errors, with the horizontal axis representing time and the vertical axis representing positioning error. It is used to illustrate the error changes of different positioning methods on the same trajectory.

[0192] Figure 5 This is a comparison chart of the empirical distribution function of positioning errors. The horizontal axis represents the positioning error, and the vertical axis represents the empirical distribution function. It is used to illustrate the differences in error distribution and tail error.

[0193] Figure 6 This is a three-axis residual plot, with time on the horizontal axis and residuals on the vertical axis, used to illustrate the error fluctuations after fixed hardware delay calibration and sound velocity compensation.

[0194] Figure 7 This is a diagram showing the results of the three-dimensional trajectory estimation, used to illustrate the consistency between the estimated trajectory and the reference trajectory under multi-anchor-point conditions.

[0195] The present invention may also include the following modified embodiments:

[0196] 1. Ultrasonic signals can use different center frequencies, swept signals, coded pulses, or spread spectrum sequences;

[0197] 2. UWB signals can be extracted using different UWB chips, frame formats, or timestamp extraction methods;

[0198] 3. The motion model can be replaced by a uniform velocity model, a constant speed model, an interactive multi-model, or a state model fused with an IMU and wheel speed meter;

[0199] 4. UKF can be combined with anomaly observation gating, adaptive measurement noise estimation, NLOS detection, or robust loss functions;

[0200] 5. The algorithm backend can be deployed on the tag end, anchor end, edge server, or cloud;

[0201] 6. For multi-label scenarios, resources can be allocated through time division, frequency division, code division, spatial multiplexing, or communication scheduling.

[0202] This invention can be applied to scenarios such as indoor mobile robots, unmanned vehicles, drones, automated guided vehicles, smart warehousing, industrial assembly, laboratory automation, medical equipment positioning, personnel safety management, augmented reality, and high-precision trajectory recording. The system can be composed of UWB devices, ultrasonic devices, environmental sensors, and embedded processors, and is feasible for manufacturing, deployment, and reuse.

[0203] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An indoor three-dimensional positioning method based on ultra-wideband ultrasonic time-difference-of-arrival ranging, characterized in that, Includes the following steps: Step 1: Construct an indoor 3D positioning system based on ultra-wideband ultrasonic co-source time difference of arrival ranging and unscented Kalman filtering. The indoor 3D positioning system is equipped with anchor point infrastructure, tag hardware front-end and algorithm back-end connected in sequence. The anchor infrastructure includes m anchor points deployed within the indoor positioning area, each anchor point being equipped with a trigger control module, an ultra-wideband transmission module, and an ultrasonic transmission module; The tag-end hardware front end is a mobile device equipped with a tag to be located. The tag to be located is equipped with an ultra-wideband receiving module, an ultrasonic receiving module, and a time difference of arrival extraction unit. The ultra-wideband receiving module and the ultrasonic receiving module are respectively connected to the time difference of arrival extraction unit. The algorithm backend is provided with a sound speed compensation module, a distance estimation module and an unscented Kalman filter module connected in sequence. The sound speed compensation module is also connected to a temperature and humidity sensor deployed in the indoor positioning area. The distance estimation module is also connected to the time difference of arrival extraction unit. The unscented Kalman filter module establishes a motion state model of the tag to be located. Step 2: Within the same positioning cycle, the trigger control modules of m anchor points respectively control the ultra-wideband transmission module and ultrasonic transmission module of the corresponding anchor point to transmit ultra-wideband signals and ultrasonic signals with synchronous or known transmission delay relationships to the tag to be positioned; Step 3: The ultra-wideband receiving module and ultrasonic receiving module in the tag to be located receive ultra-wideband signals and ultrasonic signals from m anchor points respectively, and then the arrival time of the ultra-wideband signal at each anchor point is determined by the time difference of arrival extraction unit. and the arrival time of the ultrasonic signal The time difference of arrival of ultra-wideband signals and ultrasonic signals emitted from the same anchor point at the tag end is extracted and sent to the algorithm backend. Step 4: The sound speed compensation module in the algorithm backend acquires temperature and humidity data from the temperature and humidity sensor and calculates the current ambient sound speed. And pass it to the distance estimation module; The distance estimation module is based on the signal arrival time difference at each anchor point and the current ambient sound speed. The distance from the tag to be located to each anchor point is calculated. This forms a distance observation vector consisting of the distances between m anchor points. And pass it to the unscented Kalman filter module; Step 5: The unscented Kalman filter module processes the distance observation vector. The three-dimensional position of the tag to be located is output by recursively fusing the motion state model. 3D velocity and three-dimensional acceleration .

2. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 1, characterized in that: In step 3, the arrival time difference extraction unit performs at least one of the following processing on the ultrasonic signal: bandpass filtering, envelope detection, threshold detection, correlation detection, and matched filtering, to determine the arrival time of the ultrasonic wave; and performs at least one of the following processing on the ultra-wideband signal: timestamp reading, preamble detection, first path detection, or arrival time estimation, to determine the ultra-wideband arrival time.

3. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 1, characterized in that: In step 4, the sound speed compensation module calculates the current ambient sound speed. The expression is: ； in, For the current ambient speed of sound, The current ambient temperature. This represents the current relative humidity.

4. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic homogeneous time difference of arrival ranging according to claim 1, characterized in that: In step 4, the distance estimation module calculates the distance from the tag to be located to each anchor point. The expression is: ； in, For from the Ultrasonic arrival time at each anchor point For from the Ultra-wideband arrival time of each anchor point For the first The known transmission delay of the ultrasonic signal relative to the ultra-wideband signal at each anchor point For the first The fixed hardware delay bias corresponding to each anchor point For the current ambient speed of sound, The speed of electromagnetic wave propagation. To locate the tag to the The distance between the anchor points.

5. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 4, characterized in that: When the When the ultra-wideband signal and ultrasonic signal are transmitted synchronously at each anchor point =0; when the first When the ultra-wideband signal and ultrasonic signal at each anchor point are transmitted according to a known time delay relationship... The transmit delay is either pre-calibrated or recorded by the trigger control module.

6. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 4, characterized in that: The fixed hardware delay bias includes, but is not limited to, UWB timestamp bias, ultrasonic emission drive delay, electroacoustic conversion delay, analog front-end delay, sampling delay, threshold detection delay, and related detection bias.

7. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 1, characterized in that: In step 5, the state vector of the motion state model is: ； in, For three-dimensional position, For three-dimensional velocity, For three-dimensional acceleration, superscript Indicates matrix transpose; The motion state model is a constant acceleration model, and its state update expression is: ； in, Let be the motion state transition matrix. For a moment, For process noise, satisfy normal distribution; ； in, It is a third-order identity matrix. It is a third-order zero matrix. The sampling period; At time k, the distance observation vector is composed of the distances of m anchor points. The expression is: ； The distance observation function expression is: ； ； in, This is the distance observation vector. Let be the distance observation function. To measure noise, satisfy normal distribution; Let k be the three-dimensional position of the label to be located at time k. Let m be the three-dimensional position of the m-th anchor point.

8. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 7, characterized in that: In step 5, the unscented Kalman filter module applies the distance observation vector. The recursive fusion with the motion state model is performed as follows: Step 5.1: The unscented Kalman filter module first uses the mean state value from the previous time step... State covariance structure There are Sigma state points, expressed as: ； ； ； in, This represents the mean state at time k-1. Let be the state covariance at time k-1. For the state dimension, This represents the j-th Sigma state point. For scale parameters, Here are the scale parameters for the Sigma state point distribution. This is a second-order scale adjustment parameter; Step 5.2: Each Sigma state point is propagated through the motion model to obtain the predicted Sigma state points. The expression is: ， ； And based on the predicted Sigma state points The mean of the predicted state is calculated. and predicted state covariance The expression is: ； ； ； ； ； in, Weighted by mean, For covariance weights, These are the prior parameters of the state distribution. For process noise covariance, This is the motion state transition matrix; Step 5.3: Then, input the predicted Sigma state point into the distance observation function to obtain the predicted Sigma measurement point. The expression is: ， ； And based on the predicted Sigma measurement points The predicted mean value was calculated. Innovation Covariance State-Measurement Cross-Covariance The expression is: ； ； ； in, To measure the noise covariance, For distance observation function; Step 5.4: Finally, the mean of the predicted state is calculated using Kalman gain. and predicted state covariance Make corrections to obtain the mean state at the current time. State covariance The expression is: ； ； ； in, For Kalman gain, For state-measurement cross-covariance, To innovate covariance, To predict the measurement mean, This is the distance observation vector; mean of the current state The three-dimensional location of the label to be located 3D velocity and three-dimensional acceleration ,Right now: .

9. The indoor three-dimensional positioning method based on ultra-wideband ultrasonic time difference of arrival ranging according to claim 1, characterized in that: The tag terminal hardware front end can be a mobile robot operating indoors, an unmanned vehicle operating indoors, a drone operating indoors, or any device equipped with a tag to be located and whose location information is to be obtained.

Images