Three-dimensional spatial positioning method, mobile device, and computer-readable storage medium

Abstract

The application discloses a three-dimensional space positioning method, a mobile device and a computer readable storage medium, relates to the technical field of device navigation, and is applied to the mobile device which is integrated with a UWB positioning device and a Kalman filter. The method comprises the following steps: acquiring a UWB positioning result of the mobile device and UWB measurement data detected by the UWB positioning device in a sliding time window; performing statistical analysis on the UWB measurement data to determine the fluctuation degree of the UWB measurement data; dynamically adjusting the measurement noise parameter of the Kalman filter according to the fluctuation degree; and fusing and optimizing the UWB positioning result through the Kalman filter which is dynamically adjusted by the measurement noise parameter to obtain a target UWB positioning result. The application can improve the positioning accuracy of UWB positioning.

Description

Technical Field

[0001] This application relates to the field of device navigation technology, and in particular to three-dimensional spatial positioning methods, mobile devices, and computer-readable storage media. Background Technology

[0002] With the increasing demand for indoor positioning and navigation in complex scenarios, UWB (Ultra Wide Band) based 3D positioning technology has attracted attention due to its strong real-time performance and flexible deployment.

[0003] In scenarios without GNSS (Global Navigation Satellite System) signals, UWB positioning directly measures the distance between the mobile device and the UWB base station using the time-of-flight method. It then combines multi-antenna phase difference information to determine the azimuth and elevation angles of the mobile device relative to the UWB base station, thereby calculating the three-dimensional coordinates of the mobile device and obtaining the positioning result.

[0004] However, in practical applications, UWB signals are susceptible to multipath effects, dynamic occlusion, and environmental reflection interference, resulting in a large amount of random fluctuation noise in UWB measurement data, which seriously affects the accuracy of UWB positioning. Summary of the Invention

[0005] The main purpose of this application is to provide a three-dimensional spatial positioning method, a mobile device, and a computer-readable storage medium, aiming to solve the technical problem that the UWB measurement data has a lot of random fluctuation noise, which leads to insufficient UWB positioning accuracy.

[0006] To achieve the above objectives, this application provides a three-dimensional spatial positioning method applied to a mobile device, the mobile device integrating a UWB positioning device and a Kalman filter, the method comprising:

[0007] The device acquires the UWB positioning result of the mobile device and the UWB measurement data detected by the UWB positioning device within a sliding time window, wherein the window length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device, and the faster the movement speed, the shorter the window length.

[0008] Statistical analysis is performed on the UWB measurement data to determine the degree of fluctuation of the UWB measurement data, wherein the degree of fluctuation is characterized based on at least one of variance, standard deviation and range;

[0009] Based on the degree of fluctuation, the measurement noise parameters of the Kalman filter are dynamically adjusted, wherein the measurement noise parameters include azimuth noise figure, pitch noise figure and distance noise figure;

[0010] The UWB positioning results are fused and optimized by using a Kalman filter whose noise parameters are dynamically adjusted to obtain the target UWB positioning result.

[0011] In some embodiments, the mobile device further integrates an inertial sensor, and the step of fusing and optimizing the UWB positioning results by using a Kalman filter with dynamically adjusted noise parameters to obtain the target UWB positioning result includes:

[0012] The UWB positioning results are fused and optimized by using a Kalman filter whose noise parameters are dynamically adjusted to obtain fused and optimized UWB positioning results.

[0013] The motion data of the mobile device is acquired through the inertial sensor.

[0014] Based on the motion data, the optimized UWB positioning results are weighted and fused to obtain the target UWB positioning result.

[0015] In some embodiments, the UWB measurement data includes azimuth measurement data, pitch measurement data, and distance measurement data, and the degree of fluctuation includes a first degree of fluctuation of the azimuth measurement data, a second degree of fluctuation of the pitch measurement data, and a third degree of fluctuation of the distance measurement data;

[0016] The step of dynamically adjusting the measurement noise parameters of the Kalman filter according to the degree of fluctuation includes:

[0017] The azimuth noise figure of the Kalman filter is dynamically adjusted according to the first fluctuation level.

[0018] The pitch noise figure of the Kalman filter is dynamically adjusted according to the second fluctuation level.

[0019] The distance noise figure of the Kalman filter is dynamically adjusted based on the third fluctuation level.

[0020] In some embodiments, the UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, wherein the first UWB antenna, the second UWB antenna, and the third UWB antenna are arranged in an equilateral triangle, and the UWB measurement data further includes first phase difference measurement data between the first UWB antenna and the second UWB antenna, and second phase difference measurement data between the first UWB antenna and the third UWB antenna;

[0021] Before the step of dynamically adjusting the azimuth noise coefficient of the Kalman filter according to the first fluctuation level, the method further includes:

[0022] The first phase difference measurement value and the second phase difference measurement value at the current moment are obtained from the first phase difference measurement data and the second phase difference measurement data, respectively.

[0023] The step of dynamically adjusting the azimuth noise figure of the Kalman filter based on the first fluctuation level further includes:

[0024] From the azimuth measurement data, the azimuth measurement value at the current moment is obtained, and the azimuth error propagation coefficient is calculated based on the first partial derivative of the azimuth measurement value with respect to the first phase difference measurement value and the first partial derivative of the azimuth measurement value with respect to the second phase difference measurement value.

[0025] The azimuth noise coefficient of the Kalman filter is dynamically adjusted based on the first fluctuation level and the azimuth error propagation coefficient.

[0026] In some embodiments, the step of dynamically adjusting the pitch noise figure of the Kalman filter according to the second fluctuation level further includes:

[0027] From the pitch angle measurement data, obtain the pitch angle measurement value at the current moment, and calculate the pitch angle error propagation coefficient based on the first partial derivative of the pitch angle measurement value with respect to the first phase difference measurement value and the first partial derivative of the pitch angle measurement value with respect to the second phase difference measurement value.

[0028] The pitch noise coefficient of the Kalman filter is dynamically adjusted based on the second fluctuation level and the pitch angle error propagation coefficient.

[0029] In some embodiments, the degree of fluctuation further includes a fourth degree of fluctuation of the first phase difference measurement data and a fifth degree of fluctuation of the second phase difference measurement data, and the method further includes:

[0030] The azimuth noise coefficient of the Kalman filter is dynamically adjusted based on the first fluctuation degree, the fourth fluctuation degree, the fifth fluctuation degree, and the azimuth error propagation coefficient.

[0031] The pitch noise figure of the Kalman filter is dynamically adjusted based on the second fluctuation level, the fourth fluctuation level, the fifth fluctuation level, and the pitch angle error propagation coefficient.

[0032] The pitch noise figure of the Kalman filter is dynamically adjusted based on the third, fourth, and fifth fluctuation levels.

[0033] In some embodiments, the UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, wherein the first UWB antenna, the second UWB antenna, and the third UWB antenna are arranged in an equilateral triangle. The UWB measurement data further includes first phase difference measurement data between the first UWB antenna and the second UWB antenna, and second phase difference measurement data between the first UWB antenna and the third UWB antenna. The fluctuation level further includes a fourth fluctuation level of the first phase difference measurement data and a fifth fluctuation level of the second phase difference measurement data. The method further includes:

[0034] The azimuth noise figure of the Kalman filter is dynamically adjusted based on the first fluctuation level, the fourth fluctuation level, and the fifth fluctuation level.

[0035] The pitch noise figure of the Kalman filter is dynamically adjusted based on the second fluctuation level, the fourth fluctuation level, and the fifth fluctuation level.

[0036] The distance noise figure of the Kalman filter is dynamically adjusted based on the third, fourth, and fifth fluctuation levels.

[0037] In some embodiments, the step of performing statistical analysis on the UWB measurement data to determine the degree of fluctuation of the UWB measurement data includes:

[0038] Statistical analysis is performed on the azimuth measurement data to obtain the first standard deviation, first range, and first extreme range of the azimuth measurement data, and the first standard deviation, first range, and first extreme range are used to characterize the first fluctuation degree;

[0039] Statistical analysis is performed on the pitch angle measurement data to obtain the second standard deviation, second range, and second range of the pitch angle measurement data, and the second standard deviation, second range, and second range are used to characterize the second fluctuation degree;

[0040] Statistical analysis is performed on the distance measurement data to obtain the third standard deviation, third range, and third range of the distance measurement data, and the third fluctuation degree is characterized by the third standard deviation, the third range, and the third range.

[0041] Statistical analysis is performed on the first phase difference measurement data to obtain the fourth standard deviation, fourth range, and fourth order range of the first phase difference measurement data, and the fourth standard deviation, the fourth range, and the fourth order range are used to characterize the fourth fluctuation degree;

[0042] Statistical analysis is performed on the second phase difference measurement data to obtain the fifth standard deviation, fifth range, and fifth order range of the second phase difference measurement data, and the fifth standard deviation, the fifth range, and the fifth order range are used to characterize the degree of the fifth fluctuation.

[0043] In addition, to achieve the above objectives, this application also provides a mobile device, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the steps of the three-dimensional spatial positioning method as described above.

[0044] In addition, to achieve the above objectives, this application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the three-dimensional spatial positioning method described above.

[0045] In addition, to achieve the above objectives, this application also provides a computer program product, which includes a computer program that, when executed by a processor, implements the steps of the three-dimensional spatial positioning method described above.

[0046] This application provides a three-dimensional spatial positioning method, a mobile device, and a computer-readable storage medium. The three-dimensional spatial positioning method is applied to a mobile device that integrates a UWB positioning device and a Kalman filter. This application introduces a sliding time window to dynamically analyze the real-time fluctuation characteristics of UWB measurement data. Based on the fluctuation level of the UWB measurement data, the measurement noise parameters of the Kalman filter are adaptively adjusted, dynamically matching the measurement noise parameters with the current signal quality. This automatically balances the weights of the predicted and observed values ​​of the UWB positioning results during the Kalman filtering process, suppressing the interference of random fluctuation noise on UWB positioning. Ultimately, this achieves a simultaneous improvement in positioning accuracy and robustness in complex scenarios, solving the technical problem of insufficient UWB positioning accuracy caused by excessive random fluctuation noise in UWB measurement data. Attached Figure Description

[0047] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

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

[0049] Figure 1 This is a flowchart illustrating the first embodiment of the three-dimensional spatial positioning method of this application.

[0050] Figure 2 This is a flowchart illustrating the second embodiment of the three-dimensional spatial positioning method of this application.

[0051] Figure 3 This is a flowchart illustrating the third embodiment of the three-dimensional spatial positioning method of this application.

[0052] Figure 4 This is a flowchart illustrating the fourth embodiment of the three-dimensional spatial positioning method of this application.

[0053] Figure 5 This is a flowchart illustrating the fifth embodiment of the three-dimensional spatial positioning method of this application.

[0054] Figure 6 This is a schematic diagram of UWB signal transmission provided in a specific embodiment of this application;

[0055] Figure 7 A schematic diagram of the UWB antenna and UWB coordinate system in a UWB base station provided in a specific embodiment of this application;

[0056] Figure 8 A schematic diagram of a communication scenario based on UWB signals provided in a specific embodiment of this application;

[0057] Figure 9 This is a schematic diagram of the device structure of the hardware operating environment involved in the three-dimensional spatial positioning method in the embodiments of this application.

[0058] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0059] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application as detailed in the appended claims.

[0060] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.

[0061] Currently, ultra-wideband (UWB) technology is often affected by random fluctuation noise caused by environmental and equipment factors in practical applications. These factors mainly include multipath effects, non-line-of-sight propagation, antenna performance limitations, and external radio signal interference. These factors result in a large amount of noise in UWB measurement data, directly affecting the positioning accuracy based on time difference of arrival or angle of arrival. Even small positioning errors can be amplified during the position calculation process, especially in scenarios requiring high-precision positioning, which severely impacts the positioning accuracy of UWB systems.

[0062] The main solution of this application embodiment is a three-dimensional spatial positioning method applied to a mobile device. The mobile device integrates a UWB positioning device and a Kalman filter. The method includes: acquiring the UWB positioning result of the mobile device and the UWB measurement data detected by the UWB positioning device within a sliding time window, wherein the window length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device, and the faster the movement speed, the shorter the window length; performing statistical analysis on the UWB measurement data to determine the fluctuation degree of the UWB measurement data, wherein the fluctuation degree is characterized based on at least one of variance, standard deviation, and range; dynamically adjusting the measurement noise parameters of the Kalman filter according to the fluctuation degree, wherein the measurement noise parameters include azimuth noise figure, pitch noise figure, and distance noise figure; and fusing and optimizing the UWB positioning result through the Kalman filter with dynamically adjusted measurement noise parameters to obtain the target UWB positioning result.

[0063] This application's embodiments introduce a sliding time window to dynamically analyze the real-time fluctuation characteristics of UWB measurement data. Based on the fluctuation level of the UWB measurement data, the measurement noise parameters of the Kalman filter are adaptively adjusted, dynamically matching the measurement noise parameters with the current signal quality. This automatically balances the weights of the predicted and observed values ​​of the UWB positioning results during the Kalman filtering process, suppressing the interference of random fluctuation noise on UWB positioning. Ultimately, this achieves a simultaneous improvement in positioning accuracy and robustness in complex scenarios, solving the technical problem of insufficient UWB positioning accuracy caused by excessive random fluctuation noise in UWB measurement data.

[0064] It should be noted that the execution subject of the embodiments of this application is a mobile device, which may include, but is not limited to, mobile terminals such as mobile phones, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Description), PMPs (Portable Media Players), in-vehicle terminals (such as in-vehicle navigation terminals), etc., or any mobile device capable of realizing the above functions. The embodiments of this application do not specifically limit this. The following uses a mobile device as an example to describe the following embodiments of this application.

[0065] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.

[0066] This application proposes a three-dimensional spatial positioning method according to a first embodiment.

[0067] Please refer to Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the three-dimensional spatial positioning method of this application.

[0068] In this embodiment, the three-dimensional spatial positioning method is applied to a mobile device, which integrates a UWB positioning device and a Kalman filter. The method includes steps S100 to S400:

[0069] Step S100: Obtain the UWB positioning result of the mobile device and the UWB measurement data detected by the UWB positioning device within the sliding time window. The window length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device, and the faster the movement speed, the shorter the window length.

[0070] As those skilled in the art will know, UWB positioning is a spatial positioning method based on UWB technology. Its core principle is to establish a communication connection based on UWB signals between a mobile terminal and a UWB base station through a UWB antenna, thereby completing three-dimensional spatial calculation by measuring data such as the flight time and time difference of the UWB signal, and finally outputting a UWB positioning result containing the spatial coordinates of the mobile terminal.

[0071] It should be noted that the UWB positioning device in this embodiment includes at least one UWB antenna, which is responsible for the communication connection between the mobile terminal and the UWB base station to realize UWB positioning.

[0072] It should also be noted that a sliding time window is a method for processing time-series data. It involves collecting and analyzing data within a fixed-length time window, continuously updating the window content as new data arrives. UWB measurement data refers to various data obtained through UWB technology between mobile devices and UWB base stations, primarily including distance measurement data, azimuth measurement data, elevation measurement data, and phase difference measurement data. Specifically, distance measurement data refers to the measured distance between the mobile device and the UWB base station; azimuth measurement data refers to the measured azimuth angle of the mobile device relative to the UWB base station; elevation measurement data refers to the measured azimuth angle of the mobile device relative to the UWB base station; and phase difference measurement data refers to the phase difference between the UWB antennas on the mobile device or the UWB base station.

[0073] In this embodiment, the length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device. When the mobile device moves at high speed and changes position quickly, a shorter sliding time window can capture more immediate data changes, thereby responding to transient changes more quickly. When the mobile device moves at low speed and changes position slowly, a longer sliding time window can include more historical data, thereby capturing the changing trend of UWB measurement data more stably, which helps to smooth the processing and reduce the impact of noise.

[0074] Step S200: Perform statistical analysis on the UWB measurement data to determine the degree of fluctuation of the UWB measurement data, wherein the degree of fluctuation is characterized based on at least one of variance, standard deviation and range;

[0075] It should be noted that the degree of fluctuation refers to the dispersion of UWB measurement data within the sliding time window. It is used to describe the stability and consistency of the data and reflects the changes in noise and error during the measurement process.

[0076] In this embodiment, the degree of fluctuation of a set of data can be characterized by parameters such as variance, standard deviation, range, and second range. The range refers to the difference between the maximum and minimum values ​​in the set of data, and the second range refers to the difference between the second largest and second smallest values ​​in the set of data.

[0077] For example, a set of data is [1, 2, 3, 4, 5], where the maximum value is 5, the second largest value is 4, the minimum value is 1, and the second smallest value is 2. Then the range is 4 and the second largest range is 2.

[0078] This embodiment statistically analyzes the UWB measurement data detected by the UWB positioning device within a dynamically changing sliding time window to obtain parameters such as variance, standard deviation, and range, which can reflect the degree of data fluctuation. This determines the degree of fluctuation of the UWB measurement data, providing an accurate basis for the subsequent dynamic adjustment of measurement noise parameters and ensuring that the dynamically adjusted measurement noise parameters are dynamically matched with the degree of fluctuation of the UWB measurement data.

[0079] Step S300: Dynamically adjust the measurement noise parameters of the Kalman filter according to the degree of fluctuation. The measurement noise parameters include azimuth noise figure, pitch noise figure and distance noise figure.

[0080] As those skilled in the art will know, the Kalman filter is a recursive algorithm used to estimate system states, particularly suitable for processing noisy data. The Kalman filter optimizes the estimation results through two steps: prediction and update, combining the system's dynamic model and measurement data. Measurement noise parameters are an important parameter in the Kalman filter, used to describe the uncertainty or error present in the measurement data, reflecting the degree of deviation between the measured value and the true value.

[0081] In this embodiment, the measurement noise parameters mainly include azimuth noise figure, pitch noise figure, and distance noise figure. The azimuth noise figure describes the angle measurement error of the mobile device relative to the UWB base station on the horizontal plane, reflecting the degree of deviation between the measured azimuth value and the true value. The pitch noise figure describes the angle measurement error of the mobile device relative to the UWB base station on the vertical plane, reflecting the degree of deviation between the measured pitch angle value and the true value. The distance noise figure describes the spatial distance measurement error between the mobile device and the UWB base station, reflecting the degree of deviation between the measured distance value and the true distance value.

[0082] This embodiment dynamically adjusts the measurement noise parameter of the Kalman filter based on the fluctuation level determined by real-time data. When the fluctuation level of UWB measurement data is large, the measurement noise parameter is increased, making the Kalman filter more dependent on the predicted value, suppressing the impact of random fluctuation noise on the UWB positioning result, and improving the robustness of the system. Conversely, when the fluctuation level of UWB measurement data is small, the measurement noise parameter is reduced, making the Kalman filter more dependent on the accurate observation value, improving the positioning accuracy. This achieves dynamic matching between the measurement noise parameter and the current signal quality, ensuring that the weight between the predicted value and the observed value can be automatically balanced during the Kalman filtering process, regardless of the noise environment, to obtain the optimal positioning result.

[0083] Step S400: The UWB positioning results are fused and optimized by measuring the Kalman filter after the noise parameters are dynamically adjusted to obtain the target UWB positioning results.

[0084] It should be noted that the target UBW localization result is a UBW localization result that has been optimized by Kalman filter fusion, which significantly reduces the impact of random fluctuation noise.

[0085] This embodiment utilizes a dynamically adjusted Kalman filter to fuse and optimize UWB positioning results. This reduces the confidence level of observations in high-noise environments and increases the confidence level in low-noise environments, effectively suppressing the impact of random fluctuation noise on UWB positioning results and significantly improving the positioning accuracy of UWB positioning.

[0086] This embodiment introduces a sliding time window to dynamically analyze the real-time fluctuation characteristics of UWB measurement data. Based on the fluctuation level of the UWB measurement data, it adaptively adjusts the measurement noise parameters of the Kalman filter, making the measurement noise parameters dynamically match the current signal quality. In the Kalman filtering process, it automatically balances the weights of the predicted and observed values ​​of the UWB positioning results, suppresses the interference of random fluctuation noise on UWB positioning, and ultimately achieves simultaneous improvement in positioning accuracy and robustness in complex scenarios. This solves the technical problem of insufficient UWB positioning accuracy caused by a large amount of random fluctuation noise in UWB measurement data.

[0087] In one feasible implementation, the mobile device also integrates an inertial sensor, and after step S400, steps S500 to S700 may be included:

[0088] Step S500: The UWB positioning results are fused and optimized by measuring the Kalman filter after the noise parameters are dynamically adjusted, and the fused and optimized UWB positioning results are obtained.

[0089] Step S600: Obtain motion data of the mobile device using an inertial sensor;

[0090] It should be noted that inertial sensors refer to sensors such as accelerometers and gyroscopes used to measure acceleration and angular velocity. Commonly used inertial sensors include inertial measurement units (IMUs). Motion data refers to data collected by inertial sensors, including information such as acceleration and angular velocity, used to describe the dynamic behavior of mobile devices.

[0091] Step S700: Based on the motion data, perform weighted fusion on the optimized UWB positioning results to obtain the target UWB positioning result.

[0092] It should also be noted that weighted fusion refers to combining data from different sources according to certain weights.

[0093] In this embodiment, after the fusion and optimization of the UWB positioning results is completed through the Kalman filter, motion data acquired by the inertial sensor is further introduced to perform weighted fusion of the UWB positioning results, thereby obtaining a more comprehensive and accurate target UWB positioning result and enhancing the robustness and reliability of the system.

[0094] Based on the first embodiment described above, a three-dimensional spatial positioning method according to the second embodiment of this application is proposed.

[0095] In the second embodiment of this application, the same or similar content as in the above embodiments can be referred to the above description, and will not be repeated hereafter.

[0096] Please refer to Figure 2 , Figure 2 This is a flowchart illustrating the second embodiment of the three-dimensional spatial positioning method of this application.

[0097] In this embodiment, the UWB measurement data includes azimuth measurement data, pitch measurement data, and distance measurement data, and the fluctuation level includes a first fluctuation level of the azimuth measurement data, a second fluctuation level of the pitch measurement data, and a third fluctuation level of the distance measurement data;

[0098] Step S300 may include steps S310 to S330:

[0099] Step S310: Dynamically adjust the azimuth noise figure of the Kalman filter according to the first fluctuation level;

[0100] In this embodiment, the first degree of fluctuation can be determined by calculating the statistical characteristics (such as variance, standard deviation, range, etc.) of the azimuth measurement data within the sliding time window.

[0101] When the initial fluctuation increases (e.g., the variance of the azimuth measurement data exceeds a preset threshold or the range significantly expands), it indicates that the azimuth measurement is affected by environmental interference or sudden motion changes, resulting in decreased data stability. In this case, the azimuth noise figure can be increased, causing the Kalman filter to reduce the weight of the azimuth observations during the update phase and rely more on the prediction results of the system dynamics model, thereby suppressing random noise in the azimuth dimension. Conversely, when the initial fluctuation decreases, the azimuth noise figure can be reduced, increasing the confidence in the azimuth observations and thus improving the positioning accuracy in the horizontal direction.

[0102] Step S320: Dynamically adjust the pitch noise figure of the Kalman filter according to the second fluctuation level;

[0103] In this embodiment, the second degree of fluctuation can be determined by calculating the statistical characteristics of the pitch angle measurement data within the sliding time window.

[0104] Since pitch angle reflects the vertical position change of a mobile device, its measurement is susceptible to multipath effects or device tilt. A high degree of second-order fluctuation indicates significant fluctuations in pitch angle measurements. In this case, the pitch noise figure can be dynamically increased to reduce the Kalman filter's dependence on pitch angle observations, smoothing transient errors in the vertical direction through predicted values. Conversely, as the second-order fluctuation decreases to a stable range, the pitch noise figure can be gradually reduced, fully utilizing high-precision observation data to optimize vertical positioning results, thereby improving vertical positioning accuracy.

[0105] Step S330: Dynamically adjust the distance noise figure of the Kalman filter according to the third fluctuation level.

[0106] In this embodiment, the third degree of fluctuation can be determined by calculating the statistical characteristics of the distance measurement data within the sliding time window.

[0107] Since distance noise typically originates from non-line-of-sight propagation of UWB signals or clock synchronization errors, an increase in the third fluctuation level indicates an abnormal jump or persistent deviation in distance measurements. In this case, the distance noise figure can be increased using a non-linear function (such as an exponential function) to significantly reduce the Kalman filter's trust in the distance observations and avoid interference from abnormal distance values ​​in the 3D coordinate solution. When the third fluctuation level returns to the normal range, the distance noise figure can be gradually restored to the baseline value to ensure the reasonable contribution of distance observation data in spatial calculation.

[0108] This embodiment analyzes the fluctuation characteristics of UWB measurement data independently along three dimensions: azimuth, elevation, and distance, and adjusts the corresponding noise figures accordingly, achieving refined adaptation of multi-dimensional noise parameters. Compared to the globally unified noise adjustment strategy in the first embodiment, this solution has the following advantages:

[0109] 1. Dimension-specific optimization: Differentiated fluctuation assessment and parameter adjustment strategies are adopted for physical quantities (angle, distance) and their noise sources in different dimensions (such as azimuth angle affected by horizontal multipath and pitch angle affected by equipment tilt) to avoid performance imbalance between dimensions caused by a single adjustment rule.

[0110] 2. Enhanced dynamic response capability: Since the degree of fluctuation in each dimension may change independently with the motion state (for example, the distance fluctuates significantly while the angle remains stable during high-speed linear motion), the independent adjustment mechanism can quickly respond to local noise changes in each dimension, ensuring the collaborative optimization of the Kalman filter in multidimensional state estimation.

[0111] 3. Enhanced Adaptability to Complex Scenes: In complex 3D motion (such as rotational ascent and curvilinear movement), the noise characteristics of measurement data in each dimension differ significantly. This solution effectively solves the problem of positioning error accumulation caused by coupling adjustment in traditional methods through dimensional noise suppression, and significantly improves positioning robustness, especially in asymmetric noise environments.

[0112] In summary, this embodiment achieves a comprehensive improvement in three-dimensional spatial positioning accuracy through multi-dimensional fluctuation analysis and independent adjustment of the noise figure, and is especially suitable for dynamic, complex indoor and outdoor positioning scenarios with prominent anisotropic noise.

[0113] Based on the second embodiment described above, a three-dimensional spatial positioning method according to the third embodiment of this application is proposed.

[0114] In the third embodiment of this application, the same or similar content as the above embodiments can be referred to the above description, and will not be repeated hereafter.

[0115] Please refer to Figure 3 , Figure 3 This is a flowchart illustrating the third embodiment of the three-dimensional spatial positioning method of this application.

[0116] In this embodiment, the UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, and the first UWB antenna, the second UWB antenna, and the third UWB antenna are arranged in an equilateral triangle. The UWB measurement data also includes first phase difference measurement data between the first UWB antenna and the second UWB antenna, and second phase difference measurement data between the first UWB antenna and the third UWB antenna.

[0117] Before step S310, step S301 may also be included:

[0118] Step S301: Obtain the first phase difference measurement value and the second phase difference measurement value at the current moment from the first phase difference measurement data and the second phase difference measurement data, respectively;

[0119] In this embodiment, the UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, which are arranged in an equilateral triangle with equal distances between them. This provides symmetrical spatial coverage in UWB positioning, ensuring that UWB signals received from any direction are processed consistently and reducing errors caused by asymmetrical antenna positions.

[0120] In addition, the equilateral triangle layout helps to better identify and eliminate multipath effects, thereby improving ranging accuracy and positioning accuracy, and enhancing positioning capabilities in three-dimensional space.

[0121] In this embodiment, the signal phase difference between the first UWB antenna and the second UWB antenna forms the first phase difference measurement data, and the signal phase difference between the first UWB antenna and the third UWB antenna forms the second phase difference measurement data. Through real-time sampling, the phase difference measurement values ​​between the first UWB antenna and the second UWB antenna at the current moment (i.e., the first phase difference measurement value) and the phase difference measurement values ​​between the first UWB antenna and the third UWB antenna at the current moment (the second phase difference measurement value) are extracted for subsequent error propagation analysis.

[0122] Step S310 may also include steps S311 to S312:

[0123] Step S311: Obtain the azimuth measurement value at the current moment from the azimuth measurement data, and calculate the azimuth error propagation coefficient based on the first partial derivative of the azimuth measurement value with respect to the first phase difference measurement value and the first partial derivative of the azimuth measurement value with respect to the second phase difference measurement value.

[0124] Step S312: Dynamically adjust the azimuth noise coefficient of the Kalman filter based on the first fluctuation degree and the azimuth error propagation coefficient.

[0125] It should be noted that the azimuth error propagation coefficient is a parameter used to quantify the influence of phase difference measurement error on the final azimuth calculation result.

[0126] This embodiment calculates the first-order partial derivative of the azimuth angle measurement with respect to the first and second phase difference measurements to quantify the amplification effect of azimuth angle error caused by small changes in phase difference, thus obtaining the azimuth angle error propagation coefficient. In the process of dynamically adjusting the azimuth angle noise coefficient according to the first fluctuation degree, the azimuth angle error propagation coefficient is combined to suppress the error propagation in the calculation process from phase difference to azimuth angle, reduce the cross-dimensional accumulation of measurement error, realize the fine dynamic adjustment of the azimuth angle noise coefficient, and further improve the positioning robustness and positioning accuracy of UWB positioning in complex motion and noise environments.

[0127] Furthermore, in a feasible implementation, step S320 may further include steps S321 to S322:

[0128] Step S321: Obtain the pitch angle measurement value at the current moment from the pitch angle measurement data, and calculate the pitch angle error propagation coefficient based on the first partial derivative of the pitch angle measurement value with respect to the first phase difference measurement value and the first partial derivative of the pitch angle measurement value with respect to the second phase difference measurement value.

[0129] Step S322: Dynamically adjust the pitch noise figure of the Kalman filter based on the second fluctuation degree and the pitch angle error propagation coefficient.

[0130] It should be noted that the pitch angle error propagation coefficient is a parameter used to quantify the influence of phase difference measurement error on the final pitch angle calculation result.

[0131] This implementation calculates the first-order partial derivative of the pitch angle measurement with respect to the first and second phase difference measurements to quantify the pitch angle error amplification effect caused by small changes in the phase difference, thus obtaining the pitch angle error propagation coefficient. In the process of dynamically adjusting the pitch angle noise coefficient according to the second fluctuation degree, the pitch angle error propagation coefficient is combined to suppress the error propagation in the calculation process from the phase difference to the pitch angle, reduce the cross-dimensional accumulation of measurement error, realize the fine dynamic adjustment of the pitch angle noise coefficient, and further improve the positioning robustness and positioning accuracy of UWB positioning in complex motion and noise environments.

[0132] Building upon the second embodiment, this example further introduces an equilateral triangular antenna layout and a phase difference error propagation analysis mechanism. Through a symmetrically arranged three-antenna structure, isotropic signal reception is ensured, effectively suppressing phase measurement skew caused by multipath interference. Simultaneously, by calculating the first-order partial derivatives of the azimuth / elevation angles with respect to the phase difference, the error amplification effect of the underlying phase difference noise on the final angle calculation is precisely quantified. By combining the error propagation coefficient with the fluctuation level of the measurement data, noise parameters are jointly adjusted to form a dual-layer robustness mechanism: suppressing instantaneous fluctuations in the measurement data while preventing the cascading spread of errors along the calculation link. Compared to the second embodiment, this scheme can further reduce the impact of random fluctuation noise in azimuth and elevation measurement data on UWB positioning results in complex electromagnetic environments, thereby improving the positioning accuracy of UWB positioning.

[0133] Based on the second embodiment described above, a three-dimensional spatial positioning method according to the fourth embodiment of this application is proposed.

[0134] In the fourth embodiment of this application, the same or similar content as the above embodiments can be referred to the above description, and will not be repeated hereafter.

[0135] Please refer to Figure 4 , Figure 4 This is a flowchart illustrating the fourth embodiment of the three-dimensional spatial positioning method of this application.

[0136] In this embodiment, the UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, which are arranged in an equilateral triangle. The UWB measurement data also includes first phase difference measurement data between the first and second UWB antennas, and second phase difference measurement data between the first and third UWB antennas. The fluctuation level also includes a fourth fluctuation level of the first phase difference measurement data and a fifth fluctuation level of the second phase difference measurement data. The three-dimensional spatial positioning method may further include steps S340 to S360:

[0137] Step S340: Dynamically adjust the azimuth noise figure of the Kalman filter based on the first fluctuation level, the fourth fluctuation level, and the fifth fluctuation level.

[0138] Step S350: Dynamically adjust the pitch noise figure of the Kalman filter based on the second, fourth, and fifth fluctuation levels.

[0139] Step S360: Dynamically adjust the distance noise figure of the Kalman filter based on the third, fourth, and fifth fluctuation levels.

[0140] In this embodiment, the fourth fluctuation level can be determined by calculating the statistical characteristics of the first phase difference measurement data within the sliding time window, and the fifth fluctuation level can be determined by calculating the statistical characteristics of the second phase difference measurement data within the sliding time window.

[0141] This implementation method, based on the second embodiment, further introduces the fluctuation degree of phase difference measurement data (including a fourth fluctuation degree and a fifth fluctuation degree). When adjusting the noise coefficients of each dimension, it considers both the inherent fluctuations of angle / distance and the stability characteristics of the corresponding phase channel, achieving multi-dimensional joint dynamic adjustment of the measurement noise parameters. For example, when the azimuth fluctuation (i.e., the first fluctuation degree) and the first phase difference fluctuation (i.e., the fourth fluctuation degree) intensify simultaneously, it is determined that there is systematic interference on the horizontal plane, and the azimuth noise coefficient can be multiplied using a nonlinear function. This multi-source fluctuation coupling analysis method enables the dynamic adjustment of the measurement noise parameters in this embodiment to have environmental cause tracing capabilities, thereby further improving the positioning accuracy of UWB positioning.

[0142] Based on the above embodiments, a three-dimensional spatial positioning method according to the fifth embodiment of this application is proposed.

[0143] In the fifth embodiment of this application, the same or similar content as in the above embodiments can be referred to the above description, and will not be repeated hereafter.

[0144] Please refer to Figure 5 , Figure 5This is a flowchart illustrating the fifth embodiment of the three-dimensional spatial positioning method of this application.

[0145] In this embodiment, the degree of fluctuation also includes a fourth degree of fluctuation of the first phase difference measurement data and a fifth degree of fluctuation of the second phase difference measurement data. The three-dimensional spatial positioning method may also include steps S370 to S390:

[0146] Step S370: Dynamically adjust the azimuth noise figure of the Kalman filter based on the first fluctuation degree, the fourth fluctuation degree, the fifth fluctuation degree and the azimuth error propagation coefficient;

[0147] Step S380: Dynamically adjust the pitch noise figure of the Kalman filter based on the second fluctuation degree, the fourth fluctuation degree, the fifth fluctuation degree and the pitch angle error propagation coefficient.

[0148] Step S390: Dynamically adjust the pitch noise figure of the Kalman filter based on the third, fourth, and fifth fluctuation levels.

[0149] In this embodiment, the fourth fluctuation level can be determined by calculating the statistical characteristics of the first phase difference measurement data within the sliding time window, and the fifth fluctuation level can be determined by calculating the statistical characteristics of the second phase difference measurement data within the sliding time window.

[0150] This implementation method, based on the second embodiment, integrates the technical solutions of the third and fourth embodiments, organically combining the error propagation coefficient and phase difference fluctuation. When dynamically adjusting the azimuth / elevation noise coefficient, it simultaneously considers the angle's own fluctuation (first or second fluctuation level), the phase difference data fluctuation (fourth and fifth fluctuation levels), and the error propagation coefficient from phase difference to angle (azimuth error propagation coefficient or elevation error propagation coefficient). This achieves a three-dimensional upgrade of the noise suppression mechanism, comprehensively suppressing the impact of composite noise on UWB positioning, avoiding the error residue problem caused by isolated noise source processing, and achieving a breakthrough in UWB positioning accuracy in complex interference environments.

[0151] Furthermore, in one feasible implementation, step S200, which involves statistically analyzing the UWB measurement data to determine the degree of fluctuation in the UWB measurement data, may include steps S210 to S250:

[0152] Step S210: Perform statistical analysis on the azimuth measurement data to obtain the first standard deviation, first range, and first extreme range of the azimuth measurement data, and characterize the first degree of fluctuation through the first standard deviation, first range, and first extreme range.

[0153] In this embodiment, the degree of fluctuation can be characterized based on the standard deviation, range, and subrange. Therefore, the standard deviation, range, and subrange of the azimuth measurement data can be obtained by statistical analysis of the azimuth measurement data, namely the first standard deviation, the first range, and the first range. Then, the first degree of fluctuation can be characterized by the first standard deviation, the first range, and the first range.

[0154] Step S220: Perform statistical analysis on the pitch angle measurement data to obtain the second standard deviation, second range, and second range of the pitch angle measurement data, and characterize the second fluctuation degree through the second standard deviation, second range, and second range;

[0155] Correspondingly, by performing statistical analysis on the pitch angle measurement data, the standard deviation, range, and subrange of the pitch angle measurement data can be obtained, namely the second standard deviation, second range, and second range. Then, the second standard deviation, second range, and second range can be used to characterize the second degree of fluctuation.

[0156] Step S230: Perform statistical analysis on the distance measurement data to obtain the third standard deviation, third range, and third range of the distance measurement data, and characterize the third degree of fluctuation through the third standard deviation, third range, and third range;

[0157] Correspondingly, by performing statistical analysis on the distance measurement data, the standard deviation, range, and second range of the distance measurement data can be obtained, namely the third standard deviation, third range, and third range. Then, the third standard deviation, third range, and third range can be used to characterize the third degree of fluctuation.

[0158] Step S240: Perform statistical analysis on the first phase difference measurement data to obtain the fourth standard deviation, fourth range and fourth order range of the first phase difference measurement data, and characterize the fourth fluctuation degree through the fourth standard deviation, fourth range and fourth order range;

[0159] Correspondingly, the standard deviation, range, and second range of the first phase difference measurement data can also be obtained by statistical analysis of the first phase difference measurement data, that is, the fourth standard deviation, the fourth range, and the fourth second range. Then, the fourth standard deviation, the fourth range, and the fourth second range can be used to characterize the fourth fluctuation degree.

[0160] Step S250: Perform statistical analysis on the second phase difference measurement data to obtain the fifth standard deviation, fifth range, and fifth degree of range of the second phase difference measurement data, and characterize the degree of fifth fluctuation through the fifth standard deviation, fifth range, and fifth degree of range.

[0161] Correspondingly, the standard deviation, range, and second range of the second phase difference measurement data can also be obtained by statistical analysis of the second phase difference measurement data, namely the fifth standard deviation, fifth range, and fifth second range. Then, the fifth standard deviation, fifth range, and fifth second range can be used to characterize the degree of fifth fluctuation.

[0162] This implementation method characterizes the degree of data fluctuation by using standard deviation, range, and subrange. It constructs a three-in-one composite fluctuation assessment system based on standard deviation, range, and subrange. For five types of measurement data, namely azimuth angle, pitch angle, distance, first phase difference, and second phase difference, the standard deviation (characterizing the overall dispersion), range (capturing the extreme fluctuation boundary), and subrange (depicting the core distribution stability) are calculated respectively to form a multi-dimensional noise feature vector. The degree of fluctuation is holographically quantified based on the linear weighted fusion of the three.

[0163] This implementation enhances the ability to identify sudden outliers through the collaborative analysis of the range and subrange. It utilizes a dynamic weighting strategy of standard deviation and subrange (emphasizing the range in high-speed scenarios and the subrange in low-speed scenarios), combined with a cross-validation mechanism of multi-dimensional data fluctuation characteristics, to achieve precise dynamic parameter tuning of each noise coefficient of the Kalman filter. This improves the ability to resist pulse interference while optimizing the dynamic response speed, ultimately achieving a breakthrough in the synergistic performance of 3D positioning accuracy and real-time performance in complex motion scenarios.

[0164] To facilitate understanding of the technical concept or principle of the above embodiments of this application, a specific embodiment is provided.

[0165] like Figure 6 As shown, in this specific implementation, the distance between the mobile device and the UWB base station can be measured using the time-of-flight method, with the specific formula as follows:

[0166]

[0167] r = c * Δt;

[0168] Where Δt is the time required for one-way transmission of the UWB signal between the mobile device and the UWB base station, t1 is the time from when the UWB base station first sends a UWB signal to the mobile device to when the UWB base station receives the signal returned by the mobile device based on the UWB signal, t2 is the time from when the UWB base station receives the signal returned by the mobile device based on the UWB signal to when the UWB base station sends a UWB signal to the mobile device for the second time, t3 is the time from when the mobile device receives the first UWB signal sent by the UWB base station to when the mobile device first returns a signal to the UWB base station based on the UWB signal, t4 is the time from when the mobile device first returns a signal to the UWB base station to when the mobile device receives the second UWB signal sent by the UWB base station, r is the distance between the mobile device and the UWB base station, and c is the speed of light.

[0169] like Figure 7 As shown in this specific embodiment, the UWB base station includes three UWB antennas D, E, and F, which are arranged in an equilateral triangle. That is, the UWB base station contains at least three UWB antennas, arranged in an equilateral triangle with a distance d between them. In this specific embodiment, a UWB coordinate system is established with the midpoint of the line connecting points D and E as the origin. The positive x-axis points from D to E, the positive y-axis points from the origin to F, and the z-axis is perpendicular to the plane defined by points D, E, and F.

[0170] like Figure 8 As shown, in this specific embodiment, the mobile device also includes three UWB antennas A (i.e., the first UWB antenna), B (i.e., the second UWB antenna), and C (i.e., the third UWB antenna), and A, B, and C are arranged in an equilateral triangle. That is, the UWB positioning device includes the first UWB antenna, the second UWB antenna, and the third UWB antenna, and the first UWB antenna, the second UWB antenna, and the third UWB antenna are arranged in an equilateral triangle.

[0171] In UWB positioning, mobile devices and UWB base stations establish a communication connection based on UWB signals via UWB antennas. UWB positioning is then performed using UWB measurement data acquired during this communication process. This UWB measurement data includes phase difference, azimuth, elevation, and range. Specifically, UWB measurement data includes azimuth measurement data, elevation measurement data, range measurement data, first phase difference measurement data, and second phase difference measurement data.

[0172] In this specific embodiment, the value φ can be observed through the phase difference between A and B. AB (i.e.) and the observed phase difference φ between A and C AC The distance difference d between A and B is then calculated accordingly. ABand the distance difference d between A and C AC The formula is as follows:

[0173]

[0174] Where λ = 2*d.

[0175] For example, in the UWB coordinate system, the three-dimensional position of the mobile device (i.e., the first spatial three-dimensional position) can be set as (x, y, z), the angle between the UWB antenna of the mobile device and the oxy plane in the UWB coordinate system is θ (i.e., azimuth angle), and the angle between the projection vector of the UWB antenna of the mobile device onto the oxy plane in the UWB coordinate system and the x-direction is... (i.e., pitch angle), then the three-dimensional position equation of the mobile device is as follows:

[0176]

[0177] From the above equation, the observation equation can be obtained as follows:

[0178]

[0179] Substituting x, y, and z into the expression, we can derive:

[0180]

[0181] The simplified equation is as follows:

[0182]

[0183] Simplifying the molecular formula yields:

[0184]

[0185] Solving the equation yields:

[0186]

[0187] As r approaches infinity, the equation further simplifies as follows:

[0188]

[0189] We can obtain:

[0190]

[0191] The solution was obtained through the above steps. θ, and then based on the three-dimensional position equation of the mobile device, the three-dimensional position of the mobile device in the UWB coordinate system can be solved.

[0192] In this specific embodiment, it is assumed that a world coordinate system OXYZ is established at a certain point O, which is the W system, and the attitude of the UWB base station in the W system is known. and location The three-dimensional position of the mobile device in the UWB coordinate system was also obtained. The three-dimensional position of the mobile device in the world coordinate system It can be determined by the following formula:

[0193]

[0194] This specific embodiment can use the three-dimensional position of the mobile device in the UWB coordinate system as the UWB positioning result, or the three-dimensional position of the mobile device in the world coordinate system as the UWB positioning result. Then, the UWB positioning results are fused and optimized using a Kalman filter to obtain a UWB positioning result with stronger robustness and anti-interference ability. The specific fusion technology is as follows:

[0195] This specific embodiment utilizes the Kalman filtering method for fusion.

[0196] Step 1: The state value of the 3D position can be assumed that the 3D position of the current frame is equal to the 3D position of the next frame. The state prediction calculation formula is as follows:

[0197] p(k+1)=p(k)

[0198] Step 2: The 3D position of the mobile device in the UWB positioning results is used as the measurement value to construct the measurement equation. A new technology is used to optimize the measurement noise parameters of the measurement equation in the real-time update fusion technology. The specific formula is as follows:

[0199] r θ =θcoef*rcoef*θerrorCoef;

[0200]

[0201] r R =Rcoef*rcoef;

[0202] θcoef=pdoa01Coef*pdoa20Coef*thetaCoef;

[0203]

[0204] Rcoef=pdoa01Coef*pdoa20Coef*disCoef;

[0205] Where, r θ This is the azimuth noise figure. R is the pitch noise figure. RHere, we have the range noise figure (i.e., the measurement noise parameters include azimuth noise figure, pitch noise figure, and range noise figure), pdoa01Coef is the first weighting parameter (i.e., the fourth degree of fluctuation), pdoa20Coef is the second weighting parameter (i.e., the fifth degree of fluctuation), thetaCoef is the third weighting parameter (i.e., the first degree of fluctuation), phiCoef is the fourth weighting parameter (i.e., the second degree of fluctuation), disCoef is the fifth weighting parameter (i.e., the third degree of fluctuation), rcoef is the sixth weighting parameter, and θerrorCoef is the seventh weighting parameter (i.e., the azimuth error propagation coefficient). This is the eighth weighting parameter (i.e., the pitch angle error propagation coefficient).

[0206] In other words, the azimuth noise figure of the Kalman filter is dynamically adjusted based on the first, fourth, and fifth fluctuation levels and the azimuth error propagation coefficient; the pitch noise figure of the Kalman filter is dynamically adjusted based on the second, fourth, and fifth fluctuation levels and the pitch error propagation coefficient; and the pitch noise figure of the Kalman filter is dynamically adjusted based on the third, fourth, and fifth fluctuation levels.

[0207] Among them, pdoa01Coef, pdoa20Coef, thetaCoef, phiCoef, and disCoef can be obtained by constructing a queue of length 10 to store UWB measurement data, and then solving for the degree of fluctuation of the UWB measurement data in the queue after the queue is full. The specific solution method is as follows:

[0208] pdoa01Coef=0.6*stdCoefPdoa01+0.3*maxminCoefPdoa01+0.1*

[0209] secmaxminCoefPdoa01;

[0210] pdoa20Coef=0.6*stdCoefPdoa20+0.3*maxminCoefPdoa20+0.1*

[0211] secmaxminCoefPdoa20;

[0212] thetaCoef=0.6*stdCoefTheta+0.3*maxminCoefTheta+0.1*

[0213] secmaxminCoefTheta;

[0214] phiCoef=

[0215] 0.6*stdCoefPhi+0.3*maxminCoefPhi+0.1*secmaxminCoefPhi;

[0216] disCoef =

[0217] 0.6*stdCoefDis+0.3*maxminCoefDis+0.1*secmaxminCoefDis;

[0218] Where stdCoefPdoa01 is the standard deviation (i.e., the fourth standard deviation) of the arrival phase difference between UWB antennas A and B in the UWB measurement data stored in the queue, maxminCoefPdoa01 is the range (i.e., the fourth range) of the arrival phase difference between UWB antennas A and B in the UWB measurement data stored in the queue, secmaxminCoefPdoa01 is the second range (i.e., the fourth range) of the arrival phase difference between UWB antennas A and B in the UWB measurement data stored in the queue, and stdCoefPdoa2 is the fourth range. 0 represents the standard deviation (i.e., the fifth standard deviation) of the arrival phase difference between UWB antennas AC in the UWB measurement data stored in the queue; maxminCoefPdoa20 represents the range (i.e., the fifth range) of the arrival phase difference between UWB antennas AC in the UWB measurement data stored in the queue; secmaxminCoefPdoa20 represents the second-largest range (i.e., the fifth-largest range) of the arrival phase difference between UWB antennas AC in the UWB measurement data stored in the queue; and stdCoefTheta represents the value of the arrival phase difference between UWB antennas AC in the UWB measurement data stored in the queue. The standard deviation of the azimuth angle (i.e., the first standard deviation), maxminCoefTheta is the range of the azimuth angles in the UWB measurement data stored in the queue (i.e., the first range), secmaxminCoefTheta is the second range of the azimuth angles in the UWB measurement data stored in the queue (i.e., the first range), stdCoefPhi is the standard deviation of the elevation angles in the UWB measurement data stored in the queue (i.e., the second standard deviation), and maxminCoefPhi is the standard deviation of the elevation angles in the UWB measurement data stored in the queue (i.e., the second standard deviation). The range (i.e., the second range), secmaxminCoefPhi is the second range (i.e., the second range) of the pitch angle in the UWB measurement data stored in the queue, stdCoefDis is the standard deviation (i.e., the third standard deviation) of the distance in the UWB measurement data stored in the queue, maxminCoefDis is the range (i.e., the third range) of the distance in the UWB measurement data stored in the queue, and secmaxminCoefDis is the second range (i.e., the third range) of the distance in the UWB measurement data stored in the queue.

[0219] In other words, statistical analysis of azimuth measurement data yields the first standard deviation, first range, and first extreme range of the azimuth measurement data, which characterize the first degree of fluctuation. Statistical analysis of pitch angle measurement data yields the second standard deviation, second range, and second extreme range of the pitch angle measurement data, which characterize the second degree of fluctuation. Statistical analysis of distance measurement data yields the third standard deviation, third range, and third extreme range of the distance measurement data, which characterize the third degree of fluctuation. Statistical analysis of first phase difference measurement data yields the fourth standard deviation, fourth range, and fourth extreme range of the first phase difference measurement data, which characterize the fourth degree of fluctuation. Statistical analysis of second phase difference measurement data yields the fifth standard deviation, fifth range, and fifth extreme range of the second phase difference measurement data, which characterize the fifth degree of fluctuation.

[0220] rcoef can be achieved by setting a fixed value during debugging, θerrorCoef and It can be obtained in the following way:

[0221] Based on the solution obtained in the aforementioned steps From the formula for θ, we can see that... and θ and d AB and d AC Since they are related, the partial derivatives with respect to both are given by the following formula:

[0222]

[0223] LAMDA = λ;

[0224]

[0225] Where pdoa20 is g AB pdoa01 is φ AC .

[0226] The partial derivative with respect to pdoa20 is The formula is as follows:

[0227]

[0228] about The partial derivative with respect to pdoa01 is The formula is as follows:

[0229]

[0230] The partial derivative of θ with respect to pdoa20 θ ′ pdoa20 The formula is as follows:

[0231]

[0232] in,

[0233] The partial derivative of θ with respect to pdoa01 ′ pdoa01 The formula is as follows:

[0234]

[0235] Taking the partial derivative of θ above, the following constraints apply:

[0236] d AB 2 -d AB *d AC +d AC 2 >0;

[0237] d AB 2 -d AB *d AC +d AC 2 ≤3*c 2 ;

[0238] Therefore, we can conclude that:

[0239]

[0240] In other words, the first phase difference measurement value and the second phase difference measurement value at the current moment are obtained from the first phase difference measurement data and the second phase difference measurement data, respectively; the azimuth angle measurement value at the current moment is obtained from the azimuth angle measurement data, and the azimuth angle error propagation coefficient is calculated based on the first partial derivative of the azimuth angle measurement value with respect to the first phase difference measurement value and the first partial derivative of the azimuth angle measurement value with respect to the second phase difference measurement value; the pitch angle measurement value at the current moment is obtained from the pitch angle measurement data, and the pitch angle error propagation coefficient is calculated based on the first partial derivative of the pitch angle measurement value with respect to the first phase difference measurement value and the first partial derivative of the pitch angle measurement value with respect to the second phase difference measurement value.

[0241] In summary, the measurement noise parameter r of the Kalman filter can be obtained. θ , and r R Then, the UWB positioning results are fused and optimized based on the measured noise parameters.

[0242] It should be noted that the above examples are only for the purpose of assisting in understanding this application and do not constitute a limitation on the three-dimensional spatial positioning method of this application. Any simple modifications based on this technical concept are within the protection scope of this application.

[0243] In addition, please refer to Figure 9 , Figure 9 This is a schematic diagram of the device structure of the hardware operating environment involved in the three-dimensional spatial positioning method in the embodiments of this application.

[0244] This application also provides a mobile device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, which are executed by the at least one processor to enable the at least one processor to perform the steps of the three-dimensional spatial positioning method in the above embodiments.

[0245] The following is for reference. Figure 9 This document illustrates a structural diagram of a mobile device suitable for implementing the embodiments of this application. The mobile devices in the embodiments of this application may include, but are not limited to, mobile terminals such as wearable devices, mobile phones (i.e., cell phones), laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Description), PMPs (Portable Media Players), in-vehicle terminals (e.g., in-vehicle navigation terminals), etc., or any mobile device capable of implementing the above functions. The wearable devices described include, but are not limited to, smart bracelets, Mixed Reality (MR) devices (e.g., MR glasses or MR helmets), Augmented Reality (AR) devices (e.g., AR glasses or AR helmets), Virtual Reality (VR) devices (e.g., VR glasses or VR helmets), Extended Reality (XR) devices, or some combination thereof. Figure 9 The mobile device shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0246] like Figure 9As shown, the mobile device may include a processing unit 1001 (e.g., a central processing unit, a graphics processing unit, etc.) that can perform various appropriate actions and processes according to a program stored in read-only memory 1002 or a program loaded from storage device 1003 into random access memory 1004. Random access memory 1004 also stores various programs and data required for the operation of the mobile device. The processing unit 1001, read-only memory 1002, and random access memory 1004 are interconnected via bus 1005. Input / output interface 1006 is also connected to the bus. Typically, the following systems can be connected to input / output interface 1006: input devices 1007 including, for example, touchscreens, touchpads, keyboards, mice, image sensors, microphones, accelerometers, gyroscopes, etc.; output devices 1008 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; storage devices 1003 including, for example, magnetic tapes, hard disks, etc.; and communication devices 1009. Communication device 1009 allows the mobile device to communicate wirelessly or wiredly with other devices to exchange data. Although the diagram shows mobile devices with various systems, it should be understood that it is not required to implement or have all of the systems shown. Alternatively, more or fewer systems may be implemented.

[0247] Specifically, according to the embodiments disclosed in this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device, or installed from storage device 1003, or installed from read-only memory 1002. When the computer program is executed by processing device 1001, it performs the functions defined in the methods of the embodiments disclosed in this application.

[0248] The mobile device provided in this application, employing the three-dimensional spatial positioning method described in the above embodiments, can solve the technical problem of insufficient UWB positioning accuracy caused by excessive random fluctuation noise in UWB measurement data. Compared with the prior art, the beneficial effects of the mobile device provided in this application are the same as those of the three-dimensional spatial positioning method provided in the above embodiments, and other technical features of this mobile device are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0249] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0250] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of protection of the above claims.

[0251] In addition, this application also provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, the computer-readable program instructions being used to perform the steps of the three-dimensional spatial positioning method in the above embodiments.

[0252] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory, read-only memory, erasable programmable read-only memory, optical fiber, portable compact disk read-only memory, optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0253] The aforementioned computer-readable storage medium may be included in a mobile device or may exist independently without being assembled into a mobile device.

[0254] The aforementioned computer-readable storage medium carries one or more programs that, when executed by a mobile device, cause the mobile device to: acquire the UWB positioning result of the mobile device and the UWB measurement data detected by the UWB positioning device within a sliding time window, wherein the window length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device, and the faster the movement speed, the shorter the window length; perform statistical analysis on the UWB measurement data to determine the degree of fluctuation of the UWB measurement data, wherein the degree of fluctuation is characterized based on at least one of variance, standard deviation, and range; dynamically adjust the measurement noise parameters of the Kalman filter according to the degree of fluctuation, wherein the measurement noise parameters include azimuth noise figure, pitch noise figure, and distance noise figure; and perform fusion optimization on the UWB positioning result through the Kalman filter with dynamically adjusted measurement noise parameters to obtain the target UWB positioning result.

[0255] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0256] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0257] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.

[0258] The computer-readable storage medium provided in this application stores computer-readable program instructions (i.e., a computer program) for performing the steps of the above-described three-dimensional spatial positioning method, which can solve the technical problem that the UWB measurement data has a lot of random fluctuation noise, resulting in insufficient UWB positioning accuracy. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as the beneficial effects of the three-dimensional spatial positioning method provided in the above embodiments, and will not be repeated here.

[0259] Furthermore, this application also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the three-dimensional spatial positioning method as described in the above embodiments.

[0260] The computer program product provided in this application can solve the technical problem of insufficient UWB positioning accuracy caused by excessive random fluctuation noise in UWB measurement data. Compared with the prior art, the beneficial effects of the computer program product provided in this application are the same as those of the three-dimensional spatial positioning method provided in the above embodiments, and will not be repeated here.

[0261] The above description is only a part of the embodiments of this application and does not limit the patent scope of this application. All equivalent structural transformations made under the technical concept of this application and using the contents of the specification and drawings of this application, or direct / indirect applications in other related technical fields, are included in the patent protection scope of this application.

Claims

1. A three-dimensional spatial positioning method, characterized in that, Applied to mobile devices that integrate a UWB positioning device and a Kalman filter, the method includes: The device acquires the UWB positioning result of the mobile device and the UWB measurement data detected by the UWB positioning device within a sliding time window, wherein the window length of the sliding time window is dynamically adjusted according to the movement speed of the mobile device, and the faster the movement speed, the shorter the window length. Statistical analysis is performed on the UWB measurement data to determine the degree of fluctuation of the UWB measurement data, wherein the degree of fluctuation is characterized based on at least one of variance, standard deviation and range; Based on the degree of fluctuation, the measurement noise parameters of the Kalman filter are dynamically adjusted, wherein the measurement noise parameters include azimuth noise figure, pitch noise figure and distance noise figure; The UWB positioning results are fused and optimized by using a Kalman filter whose noise parameters are dynamically adjusted to obtain the target UWB positioning result.

2. The three-dimensional spatial positioning method as described in claim 1, characterized in that, The mobile device also integrates an inertial sensor. The step of fusing and optimizing the UWB positioning results by measuring the Kalman filter with dynamically adjusted noise parameters to obtain the target UWB positioning result includes: The UWB positioning results are fused and optimized by using a Kalman filter whose noise parameters are dynamically adjusted to obtain fused and optimized UWB positioning results. The motion data of the mobile device is acquired through the inertial sensor. Based on the motion data, the optimized UWB positioning results are weighted and fused to obtain the target UWB positioning result.

3. The three-dimensional spatial positioning method as described in claim 1, characterized in that, The UWB measurement data includes azimuth measurement data, pitch measurement data, and distance measurement data. The degree of fluctuation includes a first degree of fluctuation of the azimuth measurement data, a second degree of fluctuation of the pitch measurement data, and a third degree of fluctuation of the distance measurement data. The step of dynamically adjusting the measurement noise parameters of the Kalman filter according to the degree of fluctuation includes: The azimuth noise figure of the Kalman filter is dynamically adjusted according to the first fluctuation level. The pitch noise figure of the Kalman filter is dynamically adjusted according to the second fluctuation level. The distance noise figure of the Kalman filter is dynamically adjusted based on the third fluctuation level.

4. The three-dimensional spatial positioning method as described in claim 3, characterized in that, The UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, and the first UWB antenna, the second UWB antenna, and the third UWB antenna are arranged in an equilateral triangle. The UWB measurement data also includes first phase difference measurement data between the first UWB antenna and the second UWB antenna, and second phase difference measurement data between the first UWB antenna and the third UWB antenna. Before the step of dynamically adjusting the azimuth noise coefficient of the Kalman filter according to the first fluctuation level, the method further includes: The first phase difference measurement value and the second phase difference measurement value at the current moment are obtained from the first phase difference measurement data and the second phase difference measurement data, respectively. The step of dynamically adjusting the azimuth noise figure of the Kalman filter based on the first fluctuation level further includes: From the azimuth measurement data, the azimuth measurement value at the current moment is obtained, and the azimuth error propagation coefficient is calculated based on the first partial derivative of the azimuth measurement value with respect to the first phase difference measurement value and the first partial derivative of the azimuth measurement value with respect to the second phase difference measurement value. The azimuth noise coefficient of the Kalman filter is dynamically adjusted based on the first fluctuation level and the azimuth error propagation coefficient.

5. The three-dimensional spatial positioning method as described in claim 4, characterized in that, The step of dynamically adjusting the pitch noise figure of the Kalman filter according to the second fluctuation level further includes: From the pitch angle measurement data, obtain the pitch angle measurement value at the current moment, and calculate the pitch angle error propagation coefficient based on the first partial derivative of the pitch angle measurement value with respect to the first phase difference measurement value and the first partial derivative of the pitch angle measurement value with respect to the second phase difference measurement value. The pitch noise coefficient of the Kalman filter is dynamically adjusted based on the second fluctuation level and the pitch angle error propagation coefficient.

6. The three-dimensional spatial positioning method as described in claim 5, characterized in that, The fluctuation level also includes a fourth fluctuation level of the first phase difference measurement data and a fifth fluctuation level of the second phase difference measurement data, and the method further includes: The azimuth noise coefficient of the Kalman filter is dynamically adjusted based on the first fluctuation degree, the fourth fluctuation degree, the fifth fluctuation degree, and the azimuth error propagation coefficient. The pitch noise figure of the Kalman filter is dynamically adjusted based on the second fluctuation level, the fourth fluctuation level, the fifth fluctuation level, and the pitch angle error propagation coefficient. The pitch noise figure of the Kalman filter is dynamically adjusted based on the third, fourth, and fifth fluctuation levels.

7. The three-dimensional spatial positioning method as described in claim 3, characterized in that, The UWB positioning device includes a first UWB antenna, a second UWB antenna, and a third UWB antenna, arranged in an equilateral triangle. The UWB measurement data also includes a first phase difference measurement data between the first and second UWB antennas, and a second phase difference measurement data between the first and third UWB antennas. The fluctuation level also includes a fourth fluctuation level of the first phase difference measurement data and a fifth fluctuation level of the second phase difference measurement data. The method further includes: The azimuth noise figure of the Kalman filter is dynamically adjusted based on the first fluctuation level, the fourth fluctuation level, and the fifth fluctuation level. The pitch noise figure of the Kalman filter is dynamically adjusted based on the second fluctuation level, the fourth fluctuation level, and the fifth fluctuation level. The distance noise figure of the Kalman filter is dynamically adjusted based on the third, fourth, and fifth fluctuation levels.

8. The three-dimensional spatial positioning method as described in claim 6, characterized in that, The step of statistically analyzing the UWB measurement data to determine the degree of fluctuation of the UWB measurement data includes: Statistical analysis is performed on the azimuth measurement data to obtain the first standard deviation, first range, and first extreme range of the azimuth measurement data, and the first standard deviation, first range, and first extreme range are used to characterize the first fluctuation degree; Statistical analysis is performed on the pitch angle measurement data to obtain the second standard deviation, second range, and second range of the pitch angle measurement data, and the second standard deviation, second range, and second range are used to characterize the second fluctuation degree; Statistical analysis is performed on the distance measurement data to obtain the third standard deviation, third range, and third range of the distance measurement data, and the third fluctuation degree is characterized by the third standard deviation, the third range, and the third range. Statistical analysis is performed on the first phase difference measurement data to obtain the fourth standard deviation, fourth range, and fourth order range of the first phase difference measurement data, and the fourth standard deviation, the fourth range, and the fourth order range are used to characterize the fourth fluctuation degree; Statistical analysis is performed on the second phase difference measurement data to obtain the fifth standard deviation, fifth range, and fifth order range of the second phase difference measurement data, and the fifth standard deviation, the fifth range, and the fifth order range are used to characterize the degree of the fifth fluctuation.

9. A mobile device, characterized in that, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the three-dimensional spatial positioning method as described in any one of claims 1 to 8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the three-dimensional spatial positioning method as described in any one of claims 1 to 8.

Images