A Multi-Mode Wireless Measurement Dynamic Fusion Relative Positioning Method Based on Spatiotemporal Cooperation

Abstract

This invention relates to a dynamic fusion relative positioning method for multi-mode wireless measurements based on spatiotemporal cooperation, belonging to the field of relative positioning. The method includes: initially estimating the relative positions of moving nodes using distance and time difference measurements to estimate the initial relative poses between nodes; using the initial relative poses as priors, fusing time-series measurement data and distance and time difference measurement data within a sliding window, solving a joint optimization function that minimizes Mahalanobis distance, and outputting the continuous state estimate of nodes within the window and its covariance matrix; determining the phase ambiguity period value based on the continuous state estimate of nodes by constraining the solution period and its adjacent periods through probability distribution constraints; verifying the spatiotemporal measurement consistency based on the chi-square test result of the Mahalanobis distance after sliding window optimization; dynamically identifying phase ambiguity errors or non-line-of-sight errors, adaptively switching measurement modes and eliminating outliers, and outputting the accurate relative pose. This invention improves the efficiency and stability of multi-mode measurement fusion.

Description

Technical Field

[0001] This invention relates to the field of relative positioning technology, and in particular to a dynamic fusion relative positioning method based on spatiotemporal cooperation and multi-mode wireless measurement. Background Technology

[0002] Relative positioning technology, as a crucial means of modern high-precision positioning, can calculate the relative spatial position and attitude between various moving platforms or devices such as drones, unmanned vehicles, mobile phones, VR headsets, and containers in real time. In increasingly intelligent application scenarios, relative positioning not only breaks through the dependence of traditional absolute navigation on external base stations or satellite signals but also significantly improves the efficiency and security of multi-device collaborative operations. Currently, this technology is widely used in areas such as digital key unlocking, smart home automation, gesture and pointing remote control, automated sorting in smart warehousing, collaborative perception in intelligent driving, unmanned system platooning, and collaborative transportation, providing solid technical support for the intelligent upgrade of the Internet of Things and automation systems.

[0003] Estimating the relative pose between nodes is quite complex, and currently it is mainly divided into vision-based detection methods, wireless measurement methods, and multi-source fusion methods. Vision-based methods are limited in their application scenarios due to their susceptibility to environmental conditions, difficulty in correlating detection results, and difficulty in forming effective closed loops. Wireless methods, on the other hand, are not only easy to install and mount, but also have advantages such as omnidirectional sensing measurement, independence from the environment, and friendly data correlation, making them widely used in positioning systems. Among various wireless measurement methods, ultra-wideband timestamp (UWB) offers high resolution and combines sensing and communication capabilities, making it a widely used high-precision wireless measurement method between nodes, and it also supports measurement modes such as TWR bilateral distance, TDoA time difference, and PDoA phase difference.

[0004] Distance measurement is the simplest mode, but due to its scalar nature, it cannot satisfy the observability requirements of relative pose estimation. Some methods alleviate the unobservability of the state by equipping each node with multiple measurement antennas and obtaining the relative angles between nodes through time difference of arrival or multiple ranging measurements. However, due to the size limitations of the moving nodes, the baseline length between antennas is limited, resulting in a significant dilution effect on geometric accuracy, and it is often only suitable for short-range scenarios. The phase difference measurement mode has higher theoretical accuracy than the time difference mode at the same baseline length because it directly estimates the phase correlation of the signals between antennas. However, when the baseline length exceeds half a wavelength, there is a phase periodic ambiguity problem. Some studies use methods such as particle swarm optimization to combine time difference and measurement difference multi-mode measurements in an attempt to alleviate the contradiction between measurement accuracy and uniqueness, but this requires a large number of iterative calculations and still cannot avoid getting trapped in local optima caused by phase ambiguity. Summary of the Invention

[0005] Based on the above analysis, this invention aims to disclose a multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation. This method breaks the limitation of differential antenna baseline length in traditional single methods, avoids the problem that traditional fusion methods are prone to getting trapped in local optima, and improves the efficiency and stability of multi-mode measurement fusion. It provides accurate relative position services for collaborative systems in scenarios where base station deployment is limited, such as mobile platforms or devices.

[0006] This invention discloses a multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation, comprising:

[0007] S1. Initial estimation of the relative position of moving nodes is performed using distance and time difference measurements; an optimization model is constructed based on the distance measurement converted from the time difference and the multi-antenna geometric constraints to estimate the initial relative pose between nodes.

[0008] S2. Construct a sliding window with the initial relative pose as a priori, and perform joint optimization by fusing the time-series measurement data, distance and time difference measurement data within the window, and output the continuous state estimate of the nodes within the window and its covariance matrix.

[0009] S3. Based on the continuous state estimation of the node, calculate the fuzzy period value of the phase difference measurement; when the antenna baseline length is greater than half the carrier wavelength, perform fusion distribution calculation on the continuous state estimation, and determine the unique phase fuzzy period value by solving the period and its adjacent periods through probability distribution constraints.

[0010] S4. Perform spatiotemporal measurement consistency verification based on the chi-square test results of the Mahalanobis distance after sliding window optimization; dynamically identify phase ambiguity errors or non-line-of-sight errors based on residual statistical characteristics, adaptively switch between phase difference measurement and time difference measurement and eliminate outliers, and output relative pose.

[0011] The beneficial effects that this invention can achieve are:

[0012] The present invention discloses a dynamic fusion relative positioning method for multi-mode wireless measurement based on spatiotemporal cooperation. By using a spatiotemporal information cooperation mechanism and information evolution analysis theory, it dynamically fuses multi-mode wireless measurement information such as distance, time difference, and phase difference to obtain stable and accurate relative positions and directions between nodes. This method breaks through the limitation of differential antenna baseline length in traditional single-method approaches and avoids the problem of traditional fusion methods easily getting trapped in local optima. At the same time, it improves the efficiency and stability of multi-mode measurement fusion, providing accurate relative position services for collaborative systems in scenarios where base station deployment is limited, such as mobile platforms or devices. Attached Figure Description

[0013] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals refer to the same parts.

[0014] Figure 1 This is a flowchart of the multi-mode wireless measurement dynamic fusion relative positioning method in an embodiment of the present invention;

[0015] Figure 2 This is a graph showing the variation of the lower limit of single-point positioning error with node distance under different measurement modes in this embodiment of the invention;

[0016] Figure 3 These are multiple hyperbolic graphs corresponding to the phase difference measurement period ambiguity in this embodiment of the invention;

[0017] Figure 4 This is an approximate angle diagram of each hyperbola in the far field in the embodiments of the present invention;

[0018] Figure 5 This is a schematic diagram of the reference node position and the trajectory of the positioned node in an embodiment of the present invention;

[0019] Figure 6 This is a diagram showing the evolution of relative position error under different antenna configurations in this embodiment of the invention;

[0020] Figure 7 This is a diagram showing the evolution of relative direction error in an embodiment of the present invention. Detailed Implementation

[0021] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and, together with the embodiments of the present invention, serve to illustrate the principles of the present invention.

[0022] Example 1

[0023] This embodiment discloses a multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation, such as... Figure 1 As shown, it includes:

[0024] S1. Initial estimation of the relative position of moving nodes is performed using distance and time difference measurements; an optimization model is constructed based on the distance measurement converted from the time difference and the multi-antenna geometric constraints to estimate the initial relative pose between nodes.

[0025] S2. Construct a sliding window with the initial relative pose as a priori, and perform joint optimization by fusing the time-series measurement data, distance and time difference measurement data within the window, and output the continuous state estimate of the nodes within the window and its covariance matrix.

[0026] S3. Based on the continuous state estimation of the node, calculate the fuzzy period value of the phase difference measurement; when the antenna baseline length is greater than half the carrier wavelength, perform fusion distribution calculation on the continuous state estimation, and determine the unique phase fuzzy period value by solving the period and its adjacent periods through probability distribution constraints.

[0027] S4. Perform spatiotemporal measurement consistency verification based on the chi-square test results of the Mahalanobis distance after sliding window optimization; dynamically identify phase ambiguity errors or non-line-of-sight errors based on residual statistical characteristics, adaptively switch between phase difference measurement and time difference measurement and eliminate outliers, and output relative pose.

[0028] Specifically, S1 includes:

[0029] S1-1. Configure a multi-antenna array for the reference node and the node to be located, and collect distance or distance difference measurement data between the multi-antenna pairs between the nodes; wherein, the reference node is configured with at least two antennas, and the node to be located is configured with at least one antenna. When the node to be located is configured with two or more antennas, the relative position and direction are calculated simultaneously. When a single antenna is configured, only the relative position is calculated.

[0030] S1-2. Collect distance measurement data and time difference measurement data; where, the distance measurement data is the Euclidean distance observation value between each antenna pair, which includes measurement noise; the time difference measurement data is the arrival time difference observation value from different antennas at the same node to the target antenna, which is converted into a distance difference including measurement noise;

[0031] S1-3. Construct a nonlinear least squares objective function that includes the Huber loss function; wherein, the objective function is composed of a weighted sum of distance residuals and time difference residuals, the distance residuals being the Huber weighted sum of the differences between the measured distance and the predicted distance, and the time difference residuals being the Huber weighted sum of the differences between the measured time difference and the predicted time difference;

[0032] The objective function supports three optimization modes: using only distance measurement, using only time difference measurement, or a combination of both.

[0033] S1-4. Iteratively solve the optimization objective function to obtain the initial relative pose estimate of the node to be located relative to the reference node.

[0034] More specifically,

[0035] In S1-1, the system typically divides participating nodes into reference nodes and designated nodes. Reference nodes usually integrate multiple wireless transceiver antennas. The node being located can be configured with one or more antennas as needed. It is used to calculate its position and orientation relative to the reference node;

[0036] It needs to be further explained that,

[0037] When the node being located is equipped with only a single antenna, the system can only resolve its relative position information, but cannot obtain its relative direction information. When the node being located is equipped with two or more antennas, the relative position and direction of the node can be calculated simultaneously by combining the geometric relationship between the antennas. In addition, the roles of the reference node and the node being located can be flexibly interchanged, and this process does not affect the essential function of the relative positioning system or the final positioning effect.

[0038] In S1-2, regarding distance measurement, considering the need for multiple antenna nodes, the world coordinate system in the mathematical model is used. Next node upper antenna The position is expressed as:

[0039] ;

[0040] in, The extrinsic parameters of each antenna and node in the coordinate system are the node parameters. Upper The three-dimensional position coordinates of the root antenna in its volume coordinate system; to facilitate the representation of different coordinate systems and poses and their transformations, a volume coordinate system is established for each node. ,node In reference frame The pose can be determined by the transformation matrix. This indicates that the transformation matrix includes a position vector. and a rotation matrix Sensor measurement data are represented by superscript. Indicate that the estimated value is indicated by a superscript. Representation; Node upper antenna With nodes upper antenna The distance between them is denoted as .

[0041] The noise in distance measurements follows a normal distribution. Under the given conditions, in S1-2, the distance measurement model is:

[0042] ;

[0043] in, For at any time ,node upper antenna With nodes upper antenna The distance measurement between them; For nodes At any moment The pose transformation matrix that transforms the point from the volume coordinate system to the world coordinate system, transforming the point from the node... Transform from volume coordinate system to world coordinate system. For nodes Upper The three-dimensional position coordinates of the antenna in its volume coordinate system. For nodes At any moment The pose transformation matrix from volume coordinates to world coordinates. For nodes Upper The three-dimensional position coordinates of the antenna in its volume coordinate system. The distance measurement noise term is modeled as Gaussian white noise. , This represents the noise variance.

[0044] In a multi-antenna structure, multiple ranging lines are generated between nodes at the same time, and the distances at the same time are recorded. The set of distance measurements between nodes is denoted as: .

[0045] In S1-2, regarding time difference measurement, the noise follows a normal distribution. Under these conditions, the time difference measurement data consists of the observed time difference of arrival from different antennas at the same node to the target antenna. Taking the first antenna on the reference node as the target antenna, the observed time difference of arrival is converted into the corresponding distance difference. The time difference measurement model is as follows:

[0046] ;

[0047] in, For at any time The signal comes from the node Upper Root antenna reaches node The first antenna and the second The time difference measurement of the root antenna, At the speed of light, The distance difference is converted into For nodes The first antenna at time World coordinate system position, For nodes The Root antenna at time World coordinate system position, For nodes The Root antenna at time Position in the world coordinate system; The distance difference measurement noise term is typically modeled as Gaussian white noise. .

[0048] In a multi-antenna structure, multiple distance differences are generated between nodes at the same time, resulting in multiple distance differences at the same time. The set of distance difference measurements between nodes is denoted as: .

[0049] Based on the Gaussian noise model assumption of distance and distance difference measurements, relative pose estimation is equivalent to a nonlinear least squares problem; in S1-3, the nonlinear least squares objective function including the Huber loss function is:

[0050] ;

[0051] in, The node to be solved Relative to the reference node The relative position; The node to be solved Relative to the reference node A set of distance measurements from multiple distance measurement methods; The residual of the distance measurement. The node to be solved Relative to the reference node A set of distance difference measurements converted from multiple time-difference observations;

[0052] ;

[0053] ;

[0054] In the formula, Here is the Huber loss function. , Reference nodes Location Node The number of antennas; For at any time Reference Node Upper Root antenna and the node being located Upper The measured distance between the antennas; For at any time The signal comes from the node Upper Root antenna reaches node The first antenna and the second Time difference measurement of the antenna; , These are the reference nodes during the optimization process. Location Node At any moment The estimated value of the pose transformation matrix; , , Reference nodes The first antenna, the first Root antenna, positioning node Upper The installation position of the antenna in its volume coordinate system; , These are the inverse covariance matrices of the distance measurement noise, respectively. Inverse covariance matrix of distance difference measurement noise is the weighted squared Mahalanobis norm of the weights.

[0055] When using phase difference measurement, in the nonlinear least squares objective function that includes the Huber loss function, The node to be solved Relative to the reference node A set of distance difference measurements converted from multiple phase difference observations.

[0056] In S1-4, iterative optimization is performed using methods such as the Levenberg-Marquardt algorithm. The Huber loss function is used in the optimizer. This can suppress the influence of outliers that may appear in distance and distance difference measurements in real-world environments, improving the robustness of the optimization. It should be noted that...

[0057] For general purposes, measurements may only include distance measurements. or time difference measurement This can be either the original expression containing both, or the expression containing both. After optimization, the initial relative positions can be obtained.

[0058] By utilizing time-series measurement information and combining it with state filtering or sliding window optimization methods, the accuracy of relative positioning can be gradually improved through a spatiotemporal information cooperation mechanism. Time-series measurement information can be obtained through inertial navigation systems, wheel speedometers, odometers, etc., carried by nodes. Here, odometer measurement is used as an example, and its mathematical model is as follows:

[0059] ;

[0060] The noise from the relative change in odometer readings is assumed to be Gaussian noise. .

[0061] Here, a sliding window optimization approach is used for state estimation, which offers higher estimation accuracy and robustness, and reduces divergence compared to iterative filtering. The maximum a posteriori estimate of the node states within the window is obtained by minimizing the Mahalanobis distance of all measurement residuals within the window.

[0062] Specifically, S2 includes:

[0063] S2-1. Obtain the initial relative pose. The data includes node time-series measurement data, distance and time difference measurement data within the sliding window, and prior information on the state carried over from the historical window, as well as its covariance matrix.

[0064] The node timing measurement data includes: the set of all odometer measurements within the sliding window. Nodes in the set At any moment arrive Odometry relative pose change ; Odometer measurement information matrix ;

[0065] Multi-mode wireless measurement data at all times within the sliding window includes: distance measurement set The set includes reference nodes. With the node being located At any moment Distance measurement value Distance difference measurement set The set includes reference nodes. With the node being located At any moment Distance difference measurement ;

[0066] The node obtained by carrying over the previous window from the history window At any moment pose prior information and the state covariance carried over from historical measurements The carry-over method can be based on Fisher's information and equivalent information theory.

[0067] S2-2. Construct the sliding window state variables and establish a multi-residual joint optimization function. Solve the problem by minimizing the Mahalanobis distance using robust iterative methods to obtain the refined pose and the covariance carry-over value calculated by the Fisher information matrix.

[0068] The overall objective function of sliding window optimization :

[0069] ;

[0070] Among them, the variables to be optimized It is the sequence of relative pose states of all nodes within the sliding window relative to the reference node;

[0071] The prior residuals constrain the deviation between the current estimate and the historical carryover state;

[0072] The distance residual constrains the deviation between the measured distance and the distance predicted based on the current state.

[0073] The distance difference residual constrains the deviation between the measured distance difference and the predicted distance difference.

[0074] For odometry residuals, constrain the temporal continuity (motion consistency) between adjacent time states.

[0075] ;

[0076] ;

[0077] To optimize the nodes obtained from the solution Relative to the reference node At any moment The estimated pose transformation matrix includes position and orientation; For distance difference measurement set Middle Reference Node With the node being located At any moment The measured distance difference; For all odometer measurement sets Middle node At any moment arrive The relative pose change of the odometer; For distance measurement set Middle Reference Node With the node being located At any moment Distance measurement value;

[0078] , respectively with , Let be the weighted squared Markov norm of the information matrix.

[0079] S2-3. Output the precise relative pose of each node within the sliding window relative to the reference node, as well as the updated state covariance matrix, for optimization in the next window.

[0080] Specifically, S3 includes:

[0081] S3-1. Obtain the antenna baseline length, carrier wavelength and phase difference measurements, as well as the continuous relative position estimate and its covariance matrix output by S2. When the antenna baseline is greater than half the wavelength, establish a phase-range difference mapping model that converts the periodic phase difference into a range difference containing fuzzy integers.

[0082] Since phase difference can only be measured within one waveform period, when the antenna baseline is greater than half the wavelength, phase difference measurement suffers from periodic phase ambiguity. The mathematical form of phase difference ambiguity and its conversion into range difference is as follows:

[0083] ;in, The signal carrier wavelength, This represents the phase difference.

[0084] The phase-distance difference mapping model in this step converts the periodic phase difference into fuzzy integers. The distance difference

[0085] The expression is: ; For phase ambiguity integers,

[0086] When the first antenna on the reference node is taken as the target antenna, for phase difference measurement, under the condition that the noise follows a normal distribution, the phase difference measurement data are the phase difference observations of signals received by different antennas at the same node. When converted to range difference, there are integer ambiguities. The phase difference measurement model is:

[0087] ;

[0088] For at any time The signal comes from the node Upper Root antenna reaches node The first antenna and the second Phase difference measurement of the antenna; To measure the noise, it follows a normal distribution; let the relative position estimate corresponding to the fuzzy period n be... Its cumulative probability within an isodense ellipsoid is It can be seen that each estimated fuzzy period value is a probability, which is subject to some fluctuation. It is difficult to effectively determine the fuzzy period value based on only one calculation.

[0089] S3-2. The relative position estimates at consecutive times are weighted and fused to obtain the fusion distribution. Based on the mapping model in S3-1, the candidate values ​​of the solution period and its adjacent periods are obtained by reverse inference. The fusion distribution is used to establish the probabilistic constraints of the solution period and its adjacent periods, restricting the position corresponding to the solution period to the central confidence region and excluding the position corresponding to the adjacent period from the outer confidence region.

[0090] include:

[0091] 1) Based on the mapping model of S3-1, the solution period and its adjacent candidate values ​​are obtained by reverse calculation;

[0092] include:

[0093] Calculate the geometric distance difference using the rough relative position output by S2. ;

[0094] The solution period and its adjacent candidate values ​​are estimated using the phase-distance difference mapping model;

[0095] The phase-distance difference mapping model formula is transformed as follows: ;

[0096] This is a rounding operation; it incorporates a rough geometric distance difference. Obtain the solution cycle and adjacent periods are Candidate values; thereby reducing the number of candidate values ​​in the solution cycle;

[0097] 2) By analyzing the Gaussian distribution over consecutive time intervals Information weighting and fusion are performed to obtain a fused distribution, where... , They are nodes At any moment The mean of the relative position estimate with respect to the reference node, the relative position estimate covariance matrix; the mean of the fused distribution. covariance Among them, the sum of the information matrices The sum of information vectors , Covariance matrix The inverse matrix;

[0098] The expression for the fusion distribution is: .

[0099] 3) Establish probabilistic constraints using the squared Mahalanobis distance of each candidate value's position relative to the fusion distribution; in the probabilistic constraints, the solution period will be... The corresponding position is limited to 1. Within the interval, adjacent periods The corresponding position is excluded in 3. Outside the interval; The standard deviation of the fusion distribution;

[0100] That is, the solution cycle The corresponding position is limited to 1. Within the interval, i.e. ; adjacent periods The corresponding position is limited to 3. Outside the interval, i.e. ;

[0101] Indicates based on candidate period The relative position obtained by reverse calculation; Indicates based on adjacent periods The relative position is obtained by reverse calculation.

[0102] S3-3. During the evolution of spatiotemporal information, when the solution period satisfies the probability constraint condition, a unique and reliable phase ambiguity period value is determined and output.

[0103] Once a reliable fuzzy period is determined through spatiotemporal information collaboration, the measurement can be switched from lower-precision time difference measurement to higher-precision phase difference measurement, and a unique exact solution can be obtained based on state prior constraints.

[0104] However, in practical applications, if the measurement information is not accurately estimated, it will lead to inaccurate estimation of the phase ambiguity period. Switching to phase difference measurement may cause phase ambiguity error. At the same time, wireless measurement is also affected by environmental occlusion and other factors that cause NLOS error. When phase ambiguity exists, phase difference measurement will introduce measurement bias. Under NLOS conditions, the measurement error will deviate from the normal distribution. All these errors will reduce the positioning accuracy.

[0105] To address this issue, in S4 of this embodiment, a mode verification and NLOS processing method based on spatiotemporal measurement consistency is adopted to achieve dynamic and reliable multi-mode measurement fusion.

[0106] Specifically, S4 includes:

[0107] S4-1. Based on the covariance matrix output by S2, the sliding window state, and the phase ambiguity period value determined by S3, construct a phase difference measurement model with determined integer ambiguity. Calculate the Mahalanobis distance of the measurement residuals within the window using the chi-square test to verify the consistency of spatiotemporal measurements. If the consistency requirements are not met, trigger the error identification process and proceed to S4-2.

[0108] The Mahalanobis distance is the weighted sum of all phase difference residuals and odometer measurement residuals within the window. After each optimization of the sliding window estimator, the weighted sum is compared with the chi-square distribution threshold. When the weighted sum is not less than the chi-square distribution threshold, it is determined that the measurement consistency does not meet the requirements.

[0109] The phase difference residual term is calculated based on the fuzzy period value determined in S3:

[0110] ;

[0111] here, The distance difference derived from the phase difference. Represents the phase difference residual function; for The 0.9 probability quantile of the chi-square distribution with degrees of freedom can be adjusted according to actual needs.

[0112] S4-2. Identify phase ambiguity error or non-line-of-sight error based on the statistical characteristics of the mean and variance of the wireless measurement residuals within the window, adaptively switch the measurement mode or remove outliers, and obtain the verified measurement data; among them, when a phase ambiguity error is detected, the measurement mode is reversed from phase difference measurement to time difference measurement, and when a non-line-of-sight error is detected, outliers are removed with the help of the odometer and the window is re-optimized.

[0113] Calculate all wireless measurement residuals within the calculation window mean and variance ,in, The number of residuals measured within the window; For the current measurement mode (phase difference or time difference) the first Measure residuals; set residual jitter threshold. ,like Residual bias threshold ;like ; The preset standard deviation of wireless measurement noise

[0114] when If this is determined to be a phase ambiguity error, the system will revert to the time difference measurement mode; when It was determined to be a non-line-of-sight error. At this point, outliers are identified by the deviation between the odometer prediction and the measured value. The weight of the measurement corresponding to the outlier is reset to zero or reduced, and the sliding window optimization is re-executed; otherwise, the phase difference measurement mode is maintained.

[0115] The error judgment method can be expressed by a formula as follows:

[0116] ;

[0117] After reverting to the time difference measurement mode, re-execute the consistency check of S4-1. If the check passes, maintain the time difference mode; otherwise, continue executing S4-2.

[0118] S4-3. Based on the verified measurement data and state prior constraints, a unique and accurate relative pose is output through nonlinear optimization.

[0119] The nonlinear optimization solution incorporates the phase difference measurement residual term into the sliding window optimization function of S2, which includes a joint optimization objective function of distance residual, phase difference residual, odometer residual and prior residual. By minimizing the Mahalanobis distance of the objective function, the maximum a posteriori estimate of the node state within the window is obtained, and a unique relative pose solution is output.

[0120] The phase difference measurement residual term:

[0121] ;

[0122] in, ; It is the only reliable fuzzy period value.

[0123] In summary, the spatiotemporal cooperative multi-mode wireless measurement dynamic fusion relative positioning method disclosed in this embodiment dynamically fuses multi-mode wireless measurement information such as distance, time difference, and phase difference through a spatiotemporal information cooperation mechanism and information evolution analysis theory to obtain stable and accurate relative positions and directions between nodes. This breaks the limitation of differential antenna baseline length in traditional single methods, avoids the problem of traditional fusion methods easily getting trapped in local optima, and improves the efficiency and stability of multi-mode measurement fusion. It provides accurate relative position services for collaborative systems in scenarios where base station deployment is limited, such as mobile platforms or devices.

[0124] Example 2

[0125] This embodiment uses numerical simulation to illustrate and demonstrate the method proposed in Embodiment 1.

[0126] First, simulations of single-point positioning errors were performed under different measurement modes; the antenna baseline was set to be the same for all measurement modes. The distance measurement error was... =0.1m, the time difference measurement error is 100ps (corresponding to) =0.03m), the phase difference measurement error is 20ps (corresponding to =0.006m), the spacing between the anchor antennas is 1m, and they are located at positive and negative symmetrical positions on the y-axis, with a carrier frequency of 3.1GHz.

[0127] like Figure 2The lower limit of single-point positioning error varies with node distance under different measurement modes. It can be seen that the single-point positioning error increases with the increase of node distance under different measurement modes. However, due to the different measurement accuracies such as distance, time difference, and phase difference, there are large differences in accuracy at the same node distance. Among them, the phase difference measurement has the highest theoretical accuracy, but in practice, there will be periodic ambiguity problems.

[0128] Furthermore, the characteristics of the periodic ambiguity hyperbola based on phase difference measurement are explained. Taking the coordinate position of the located node as (10, 2) as an example, multiple hyperbolas corresponding to the phase ambiguity can be obtained as follows: Figure 3 As shown.

[0129] Figure 3 In this context, the more distant the baseline or the shorter the wavelength, the more blurring periods occur. Furthermore, the curve approximates a straight line with a fixed slope and a fixed angle between adjacent blurring lines when far from the reference node. The angle between the slope line corresponding to the far field of each hyperbola and the x-axis is as follows: Figure 4 As shown.

[0130] Figure 4 In the process, the angle gradually increases as the blur curve expands outward, and the angle increases even faster for the same wavelength interval.

[0131] Next, we analyze the state evolution under spatiotemporal information collaboration. Assume the located node moves in a circle around the reference node at a speed of 1 m / s, and the data acquisition frequency is 20 Hz. The trajectory drift error is:

[0132] ;

[0133] The distance between nodes is 30m, and the baseline distance between antennas in the multi-antenna structure is 1.0m. Here, the information is converted into positioning error to visually demonstrate the effect of information coupling. The trajectory diagram and state evolution results are as follows. Figure 5 and Figure 6 As shown, this also illustrates the situation under different antenna configurations of the located node.

[0134] Figure 7In the error evolution results, the initial time is the single-point initialization result, which has low accuracy. It can be seen that as spatiotemporal information cooperation progresses, the node state evolves, and the error gradually decreases and converges. Notably, when the antennas of the located node and the reference node are collinear, i.e., at the S1 and S2 markers, the error is relatively large. This is because the measurement constraints are poor near this location, and geometric dilution is more pronounced, which can be addressed by increasing the number of antennas on the reference node. Simultaneously, the accuracy is higher when multiple transmit and receive antennas are present on the located node, and the relative directions between nodes can be obtained. When the movement reaches half a cycle, the measurement switches from time difference to phase difference, further reducing the positioning error, demonstrating the effectiveness of multi-mode fusion. Therefore, this method can significantly improve positioning accuracy through spatiotemporal information cooperation, and the baseline length is unrestricted, making full use of the node space.

[0135] As described above, these are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A multi-mode wireless measurement dynamic fusion relative positioning method based on space-time cooperation, characterized in that, include: S1. Initial estimation of the relative position of the moving nodes is performed using distance and time difference measurements; An optimization model is constructed based on the distance measurement with multi-antenna geometric constraints and the distance difference converted from the time difference to estimate the initial relative pose between nodes. S2. Construct a sliding window with the initial relative pose as a priori, and perform joint optimization by fusing the time-series measurement data, distance and time difference measurement data within the window, and output the continuous state estimate of the nodes within the window and its covariance matrix. S3. Based on the continuous state estimation of the node, calculate the fuzzy period value of the phase difference measurement; when the antenna baseline length is greater than half the carrier wavelength, perform fusion distribution calculation on the continuous state estimation, and determine the unique phase fuzzy period value by solving the period and its adjacent periods through probability distribution constraints. S4. Perform spatiotemporal measurement consistency verification based on the chi-square test results of the Mahalanobis distance after sliding window optimization; dynamically identify phase ambiguity error or non-line-of-sight error based on residual statistical characteristics, adaptively switch between phase difference measurement and time difference measurement and remove outliers, and output relative pose; S3 includes: S3-1. Obtain the antenna baseline length, carrier wavelength and phase difference measurements, as well as the continuous relative position estimate and its covariance matrix output by S2. When the antenna baseline is greater than half the wavelength, establish a phase-range difference mapping model that converts the periodic phase difference into a range difference containing fuzzy integers. S3-2. The relative position estimates at consecutive times are weighted and fused to obtain the fusion distribution. Based on the mapping model in S3-1, the candidate values ​​of the solution period and its adjacent periods are obtained by reverse inference. The fusion distribution is used to establish the probabilistic constraints of the solution period and its adjacent periods, restricting the position corresponding to the solution period to the central confidence region and excluding the position corresponding to the adjacent period from the outer confidence region. S3-3. During the evolution of spatiotemporal information, when the solution period satisfies the probability constraint condition, a unique and reliable phase ambiguity period value is determined and output.

2. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 1, characterized in that, S3-2 includes: 1) Based on the mapping model of S3-1, the solution period and its adjacent candidate values ​​are obtained by reverse calculation; 2) by Gaussian distribution of continuous time perform information weighted fusion to obtain a fusion distribution, wherein, , are the node At time relative position estimation mean, relative position estimation covariance matrix; the mean of the fusion distribution , covariance ; wherein, the sum of information matrices , the sum of information vectors , is the inverse matrix of the covariance matrix . 3) Establishing the probability constraint condition by the Mahalanobis distance square of each candidate value position relative to the fusion distribution; in the probability constraint condition, the solution period The corresponding position is limited to 1 interval, the adjacent period , the corresponding position is excluded from 3 interval; is the standard deviation of the fusion distribution.

3. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 1, characterized in that, S4 includes: S4-1. Based on the covariance matrix output by S2, the sliding window state, and the phase ambiguity period value determined by S3, construct a phase difference measurement model with determined integer ambiguity. Calculate the Mahalanobis distance of the measurement residuals within the window using the chi-square test to verify the consistency of spatiotemporal measurements. If the consistency requirements are not met, trigger the error identification process and proceed to S4-2. S4-2. Identify phase ambiguity error or non-line-of-sight error based on the statistical characteristics of the mean and variance of the wireless measurement residuals within the window, adaptively switch the measurement mode or remove outliers, and obtain the verified measurement data; among them, when a phase ambiguity error is detected, the measurement mode is reversed from phase difference measurement to time difference measurement, and when a non-line-of-sight error is detected, outliers are removed with the help of the odometer and the window is re-optimized. S4-3. Based on the verified measurement data and state prior constraints, a unique and accurate relative pose is output through nonlinear optimization.

4. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 3, characterized in that, In S4-1, the Mahalanobis distance is calculated as the weighted sum of the squares of all distance difference measurement residuals and odometer measurement residuals within the window. After each optimization of the sliding window estimator, the weighted sum of the Mahalanobis distances is compared with the chi-square distribution threshold. When the weighted sum is not less than the chi-square distribution threshold, it is determined that the measurement consistency does not meet the requirements.

5. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 3, characterized in that, In S4-2, calculate the residuals of all wireless measurements within the calculation window. mean and variance ,in, The number of residuals measured within the window; For the first measurement under the current measurement mode Measure residuals; set residual jitter threshold. Residual bias threshold ; when If the error is determined to be phase ambiguity, then the system should revert to time difference measurement mode. when If the error is determined to be non-line-of-sight error, the outlier is identified by the deviation between the odometer prediction and the measured value. The weight of the measurement corresponding to the outlier is reset to zero or reduced, and the sliding window optimization is re-executed. Otherwise, maintain the phase difference measurement mode.

6. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to any one of claims 1-5, characterized in that, S1 includes: S1-1. Configure a multi-antenna array for the reference node and the node to be located, and collect distance or distance difference measurement data between the multi-antenna pairs between the nodes; wherein, the reference node is configured with at least two antennas, and the node to be located is configured with at least one antenna. When the node to be located is configured with two or more antennas, the relative position and direction are calculated simultaneously. When a single antenna is configured, only the relative position is calculated. S1-2. Collect distance measurement data and time difference measurement data; where, the distance measurement data is the Euclidean distance observation value between each antenna pair, which includes measurement noise; the time difference measurement data is the arrival time difference observation value from different antennas at the same node to the target antenna, which is converted into a distance difference including measurement noise; S1-3. Construct a nonlinear least squares objective function that includes the Huber loss function; wherein, the objective function is composed of a weighted sum of distance residuals and time difference residuals, the distance residuals being the Huber weighted sum of the differences between the measured distance and the predicted distance, and the time difference residuals being the Huber weighted sum of the differences between the measured time difference and the predicted time difference; The objective function supports three optimization modes: using only distance measurement, using only time difference measurement, or a combination of both. S1-4. Iteratively solve the optimization objective function to obtain the initial relative pose estimate of the node to be located relative to the reference node.

7. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 6, characterized in that, In S1-3, the nonlinear least squares objective function that includes the Huber loss function is: ； in, The node to be solved Relative to the reference node The relative position; The node to be solved Relative to the reference node A set of distance measurements from multiple distance measurement methods; The residual of the distance measurement. The node to be solved Relative to the reference node A set of distance difference measurements converted from multiple time-difference observations; ； ； In the formula, Here is the Huber loss function. , Reference nodes Location Node The number of antennas; For at any time Reference Node Upper Root antenna and the node being located Upper The measured distance between the antennas; For at any time The signal comes from the node Upper Root antenna reaches node The first antenna and the second Time difference measurement of the antenna; , These are the reference nodes during the optimization process. Location Node At any moment The estimated value of the pose transformation matrix; , , Reference nodes The first antenna, the first Root antenna, positioning node Upper The installation position of the antenna in its volume coordinate system; , These are the inverse covariance matrices of the distance measurement noise. Inverse covariance matrix of distance difference measurement noise is the weighted squared Mahalanobis norm of the weights.

8. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 6, characterized in that, S2 includes: S2-1. Obtain the initial relative pose, node timing measurement data, distance and time difference measurement data set within the sliding window, and the prior state information and covariance matrix carried over from the historical window. S2-2. Construct the sliding window state variables and establish a multi-residual joint optimization function. Solve the problem by minimizing the Mahalanobis distance using robust iterative methods to obtain the refined pose and the covariance carry-over value calculated by the Fisher information matrix. S2-3. Output the precise relative pose of each node within the sliding window relative to the reference node, as well as the updated state covariance matrix, for optimization in the next window.

9. The multi-mode wireless measurement dynamic fusion relative positioning method based on spatiotemporal cooperation according to claim 8, characterized in that, In S2-2, the sliding window state variables are constructed and a multi-residual joint optimization function is established. Robust iterative solution is performed by minimizing the Mahalanobis distance to obtain the refined pose and the covariance carry-over value calculated by the Fisher information matrix. The overall objective function of sliding window optimization : ； Among them, the variables to be optimized It is the sequence of relative pose states of all nodes within the sliding window relative to the reference node; The prior residuals constrain the deviation between the current estimate and the historical carryover state; The distance residual constrains the deviation between the measured distance and the distance predicted based on the current state. The distance difference residual constrains the deviation between the measured distance difference and the predicted distance difference. For odometer residuals, constrain the temporal continuity between states at adjacent time points; ； ； To optimize the nodes obtained from the solution Relative to the reference node At any moment The estimated pose transformation matrix includes position and orientation; For distance difference measurement set Middle Reference Node With the node being located At any moment The measured distance difference; For all odometer measurement sets Middle node At any moment arrive The relative pose change of the odometer; For distance measurement set Middle Reference Node With the node being located At any moment Distance measurement value; To use the pose state covariance carried over from historical measurements The weighted squared Mahalanobis norm; Odometer measurement information matrix Let be the weighted squared Markov norm of the information matrix.

Images