Rejection relative positioning method based on adjustable confidence

Abstract

The application discloses a rejection relative positioning method based on adjustable confidence, and belongs to the field of unmanned system navigation and positioning. The method comprises the following steps: a cluster head node initializes a relative coordinate matrix, an iteration number variable and an iteration error variable, sets a maximum iteration number and a minimum iteration error; the observation distance between each two nodes is obtained, and the variance of the measurement error of each observation distance is set; an index matrix between each two nodes, the confidence of each observation distance and a transformation matrix are calculated; the relative coordinate matrix is iteratively updated to obtain a final relative coordinate matrix; the transformation relationship between the relative coordinates and the absolute coordinates of anchor nodes is estimated, and the absolute coordinates of all nodes are calculated according to the transformation relationship. The application gives different distance error confidence levels to different link differentiated ranging capabilities, realizes balanced acquisition of multiple ranging information, improves the precision and speed of the rejection relative positioning, and has important significance for constructing efficient and flexible cooperative positioning capability.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology for unmanned systems, and specifically refers to a denial relative positioning method based on adjustable confidence, that is, a precise and efficient relative positioning method for unmanned swarms in a satellite-denied environment. Background Technology

[0002] In recent years, unmanned aerial vehicle (UAV) swarm systems have emerged as a transformative technology with widespread applications. These systems consist of multiple autonomous or semi-autonomous agents, leveraging swarm intelligence to perform complex tasks with greater efficiency, scalability, and robustness. UAV swarms can be used in precision agriculture, disaster response, infrastructure inspection, and logistics management, where they demonstrate superior performance in large-scale distributed operations compared to single-agent solutions. The effectiveness of UAV swarms fundamentally depends on accurate and reliable positioning and navigation capabilities. Each member of the swarm must accurately determine its own absolute position and the relative positions of neighboring agents to ensure collision-free movement, formation maintenance, and collaborative task execution. Traditional positioning systems rely primarily on the Global Navigation Satellite System (GNSS) in outdoor environments, but these signals are susceptible to intentional interference, spoofing attacks, and environmental disturbances. Therefore, UAV swarms must employ alternative positioning strategies to maintain operational capability.

[0003] Relative positioning technology has emerged as a promising solution due to its independence from infrastructure. These technologies utilize wireless communication links to acquire distance information between nodes, achieving relative position estimation through multilateral measurements. When some nodes possess absolute position information, it can further facilitate global cluster positioning. However, current relative positioning research largely ignores the inherent heterogeneity of wireless communication links. Factors such as variations in physical channel conditions, electromagnetic interference, node movement patterns, clock synchronization errors, and hardware processing capabilities lead to significant differences in the reliability of distance measurements between different node pairs. The traditional assumption of equal measurement confidence introduces substantial positioning errors and reduces computational convergence speed.

[0004] Current research mainly focuses on three key aspects: absolute position awareness, network topology configuration, and relative-absolute conversion estimation. To improve absolute positioning capability, researchers integrate various sensors on unmanned platforms, such as cameras, barometers, odometers and inertial navigation systems, and further use Kalman filtering to combine data links with the inertial navigation system, achieving high-frequency, high-precision position updates. Current network architectures are mainly divided into two categories: centralized relative positioning and distributed relative positioning. Common methods for rotation matrix estimation in relative-absolute conversion include least square method, Newton method and genetic algorithm. There have been some studies on relative positioning technology, but almost all existing studies ignore the heterogeneity of wireless communication links and regard the ranging capability between nodes as consistent. In actual scenarios with significant differences in link quality, this assumption will inevitably lead to poor performance. To meet the positioning requirements of unmanned systems in denied environments, relevant high-precision and flexible positioning algorithms need to be designed for link asymmetry. Summary of the Invention

[0005] In view of this, the present invention proposes a denied relative positioning method based on adjustable confidence. This method endows different distance error confidence levels for the differentiated ranging capabilities of different links, realizes balanced adoption of multiple ranging information, and uses iterative calculation to complete rapid convergence, thereby improving the accuracy of denied relative positioning.

[0006] The technical solution adopted by the present invention is:

[0007] The adjustable confidence based denied relative positioning method is applied to an unmanned swarm, there are a total of N nodes in the unmanned swarm, one of which is a cluster head node and the others are ordinary nodes, among the N nodes, T are anchor nodes with known absolute coordinates, T < N; the method comprises the following steps:

[0008] Step 1: the cluster head node initializes a relative coordinate matrix, an iteration number variable t, and an iteration error variable ∈, and sets the maximum number of iterations to the minimum iteration error is obtain the observed distance between every two nodes, set the variance of the measurement error of each observed distance, and calculate the ranging confidence;

[0009] Step 2: calculate an index matrix between every two nodes, the confidence of each observed distance, and a transformation matrix;

[0010] Step 3: iteratively update the relative coordinate matrix according to the relative coordinate matrix, the observed distance, the index matrix, the confidence and the transformation matrix until ∈ is less than or t reaches obtain the final relative coordinate matrix;

[0011] Step 4: Normalize the relative and absolute coordinates of the T anchor nodes, estimate the transformation relationship between the relative and absolute coordinates of the anchor nodes, and calculate the absolute coordinates of all nodes based on the transformation relationship.

[0012] Furthermore, the specific method of step 1 is as follows:

[0013] Step 1.1: The cluster head node initializes the relative coordinate matrix M to M using a randomized method. (0) M is an N-row, 3-column matrix, where the i-th row of M represents the three-dimensional relative coordinates of the i-th node with respect to the cluster head node.

[0014] Step 1.2: The cluster head node initializes the iteration count variable t, with an initial value of 0, and sets the maximum iteration count to 0.

[0015] Step 1.3: Initialize the iteration error variable ∈ of the cluster head node, with an initial value of infinity, and set the minimum iteration error to .

[0016] Step 1.4: Each node in the unmanned cluster detects its own distance from all other nodes and sends this distance to the cluster head node; the cluster head node averages the distances between nodes i and j sent by nodes i and j respectively, obtaining the average value o. ij and with average value o ij As the observation distance between node i and node j, the cluster head node obtains a total of N(N-1) / 2 observation distances;

[0017] Step 1.5: Treat the measurement error of the observation distance as a random variable conforming to a Gaussian distribution. Based on the real-time dynamic changes in channel quality of the communication link between nodes i and j, differentiate the measurement error for each observation distance o. ij Measurement error setting variance θ ij .

[0018] Furthermore, the specific method for step 2 is as follows:

[0019] Step 2.1, calculate the index matrix between every two nodes, where the index matrix between node i and node j is A. ij :

[0020] A ij =(e i -e j (e) i -e j ) T

[0021] Among them, e i Let be an N-dimensional column vector, with the i-th element being 1 and all other elements being 0. The superscript T indicates the transpose of the matrix.

[0022] Step 2.2, calculate the scaling factor K:

[0023]

[0024] Step 2.3, calculate the confidence level for each observation distance, where the observation distance o ij The corresponding confidence level is c ij :

[0025] c ij =K·θ ij

[0026] Step 2.4, calculate the comprehensive index matrix V:

[0027]

[0028] Step 2.5, calculate the transformation matrix V + :

[0029] V + =(V+N) -1 1.1 T ) -1 -N -1 1.1 T

[0030] Where 1 is an N-dimensional column vector of all 1s, 1 T Let N be an N-dimensional row vector of all 1s, where the superscript of N is -1 to find the reciprocal, and the superscript of the matrix is ​​-1 to find the inverse.

[0031] Furthermore, step 3 is performed as follows:

[0032] Step 3.1, if the iterative error variable ∈ is less than Or the iteration number variable t is greater than or equal to If the iteration stops, the final relative coordinate matrix is ​​output; otherwise, the subsequent steps are executed.

[0033] Step 3.2, calculate the numerical value s ij :

[0034]

[0035] In the formula, the distance d ij (M (t) )for:

[0036]

[0037] Where the subscripts i and j represent node i and node j respectively, a (1) a (2) a (3) M respectively(t) The x-axis, y-axis, and z-axis components of the relative coordinates of the corresponding nodes;

[0038] Step 3.3, calculate the intermediate matrix B(M) (t) ):

[0039]

[0040] Step 3.4, Update the relative coordinate matrix:

[0041] M (t+1) =V + B(M (t) M (t)

[0042] Step 3.5, calculate the iteration error variable ∈:

[0043] ∈=‖M (t+1) -M (t) ||2

[0044] Where |||2 represents the 2-norm;

[0045] At the same time, the iteration count variable t is increased by 1, and we return to step 3.1.

[0046] Furthermore, step 4 is specifically implemented as follows:

[0047] Step 4.1, normalize the relative and absolute coordinates of the anchor nodes:

[0048]

[0049] Where X is a 3x3 matrix consisting of the relative coordinates of T anchor nodes, and Y is a 3x3 matrix consisting of the absolute coordinates of T anchor nodes. The matrix is ​​normalized, and 1 is a T-dimensional column vector of all 1s;

[0050] Step 4.2, estimate the rotation matrix between the relative and absolute coordinates of the anchor node.

[0051]

[0052] Step 4.3: Estimate the translation vector between the relative and absolute coordinates of the anchor node.

[0053]

[0054] Where 1 is a T-dimensional column vector of all 1s;

[0055] Step 4.4, calculate the matrix P consisting of the absolute coordinates of all nodes:

[0056]

[0057] Where M is the final relative coordinate matrix obtained in step 3.

[0058] The beneficial effects of this invention are as follows:

[0059] 1. This invention takes into account the differences in clock accuracy, flight dynamics, physical channel environment, and electromagnetic interference among the nodes of an unmanned swarm. It assigns different distance error confidence levels to the ranging capabilities of different links, thereby enabling flexible, high-precision, and high-efficiency rejection relative positioning for various scenarios.

[0060] 2. This invention dynamically adjusts measurement weights based on link quality assessment, enabling high-precision positioning and rapid computational convergence of UAV swarms.

[0061] 3. This invention is of great significance for building flexible, adaptable, high-precision, and high-efficiency collaborative positioning capabilities for unmanned systems in satellite-denied environments. Detailed Implementation

[0062] The present invention will be further described below with reference to the embodiments.

[0063] A denial-of-location method based on adjustable confidence is proposed. This method is applied to unmanned swarms, which may include rotary-wing UAVs, fixed-wing UAVs, etc. The unmanned platforms are equipped with communication devices to achieve high-precision ranging capabilities. This method leverages the heterogeneity of wireless communication links, dynamically allocating weighted confidence to the ranging measurements between each node, thereby enhancing the denial-of-location capability.

[0064] The specific steps of this method are as follows:

[0065] Step 1: For an unmanned cluster containing N nodes, initialize the relative coordinates of each node as a. i Let i = 1, 2, ..., N, and construct an initial relative coordinate matrix M. (0) Set the initial value t for the iteration count variable and the maximum number of iterations. Set the initial value of the iteration error variable ∈ and the minimum iteration error. Obtain N(N-1) / 2 observation distances o ij and the corresponding variance θ of the measurement error ij Specifically:

[0066] Step 1.1: Randomly initialize the relative coordinates a of each node. i for

[0067]

[0068] In this context, the superscripts (1), (2), and (3) represent the x, y, and z components, respectively.

[0069] N nodes form an N×3 dimensional relative coordinate matrix M:

[0070] M = [a1, a2, ..., a N ] T

[0071] Define the absolute coordinates p of each node. i for:

[0072]

[0073] N nodes form an N×3 dimensional relative coordinate matrix.

[0074] Y = [y1, y2, ..., y N ] T

[0075] Step 1.2: Set the initial value of the iteration count variable t to 1, and set the maximum iteration count to [value missing].

[0076] Step 1.3: Set the initial value of the iteration error variable ∈ to ∞, and set the minimum iteration error to .

[0077] Step 1.4: Each node in the unmanned cluster detects its own distance from all other nodes and sends this distance to the cluster head node; the cluster head node averages the distances between nodes i and j sent by nodes i and j respectively, obtaining the average value o. ij and with average value o ij As the observation distance between node i and node j, the cluster head node obtains a total of N(N-1) / 2 observation distances;

[0078] Step 1.5 treats the measurement error of the observation distance as a random variable conforming to a Gaussian distribution. Based on the dynamic real-time channel estimation capability of the communication link itself, the channel quality is divided into 5 levels. In this example, the channel quality is categorized into 5 levels for each observation distance. ij The measurement error is set with different variances θ ij , corresponding to (10m) 2 (5m) 2 (2m) 2 , (1m) 2 (0.5m) 2 .

[0079] Step 2, calculate the index matrix based on the variance θ of each distance measurement error. ij Calculate the scaling factor K, and then obtain the confidence level c of the distance.ij Finally, the transformation matrix V is calculated. + The specific method is as follows:

[0080] Step 2.1, for node i and node j, calculate the N×N dimensional index matrix A. ij for:

[0081] A ij =(e i -e j (e) i -e j ) T

[0082] Where e i It is an N-dimensional column vector, where the i-th element is 1 and all other elements are 0;

[0083] Step 2.2, based on the N(N-1) / 2 variances θ from step 1.4 ij The scaling factor K is calculated as follows:

[0084]

[0085] Step 2.3: Perform a linear transformation on the variance based on the scaling factor K from Step 2.2, and calculate the distances o for N(N-1) / 2 observations respectively. ij confidence level c ij :

[0086] c ij =K·θ ij

[0087] Step 2.4, based on the N(N-1) / 2 index matrices A from Step 2.1 ij The N×N dimensional comprehensive index matrix V is calculated as follows:

[0088]

[0089] Step 2.5: Based on the comprehensive index matrix V from Step 2.4, calculate the N×N dimensional transformation matrix V. + for:

[0090] V + =(V+N) -1 1.1 T ) -1 -N -1 1.1 T

[0091] Where 1 is an N-dimensional column vector of all 1s.

[0092] Step 3: Update the relative coordinate matrix using iterative calculation. Specifically:

[0093] Step 3.1, if the iterative error variable ∈ is less than Or the iteration number variable t is greater than or equal to If the iteration stops, the final relative coordinate matrix is ​​output; otherwise, the subsequent steps are executed.

[0094] Step 3.2, based on the observation distance o in step 1.4 ij and the current relative coordinate matrix M (t) Calculate N(N-1) / 2 values ​​of s. For node i and node j, the value of s is s. ij :

[0095]

[0096] Where, the distance d between node i and node j ij (M (t) () represents the Euclidean distance:

[0097]

[0098] Step 3.3, according to A in step 2.1 ij c in step 2.3 ij and s in step 3.2 ij and the current relative coordinate matrix M (t) Calculate the N×N dimensional intermediate matrix B(M) (t) ):

[0099]

[0100] Step 3.4, based on the current relative coordinate matrix M (t) and the intermediate matrix B(M) in step 3.3 (t) Update the relative coordinate matrix:

[0101] M (t+1) =V + B(M (t) M (t)

[0102] Step 3.5, use the 2-norm to calculate the iteration error variable ∈:

[0103] ∈=‖M (t+1) -M (t) ||2

[0104] At the same time, the iteration count variable t is increased by 1, and we return to step 3.1.

[0105] Step 4: Select T points with known absolute coordinates as anchor nodes. Normalize the relative and absolute coordinates of the anchor nodes, and estimate the rotation matrix using the relative and absolute coordinates of the anchor nodes. Translation vector The absolute coordinates are calculated based on the relative coordinates of all nodes. The specific method is as follows:

[0106] Step 4.1: For the T anchor nodes with known absolute coordinates, normalize their relative and absolute coordinates:

[0107]

[0108] Where X is a matrix composed of the relative coordinates of T anchor nodes, and Y is a matrix composed of the absolute coordinates of T anchor nodes. is the normalized matrix, and 1 is a T-dimensional column vector of all 1s.

[0109] Step 4.2, according to step 4.1 and Estimate the 3×3 rotation matrix for relative-absolute coordinates. for:

[0110]

[0111] Step 4.3, according to step 4.1 and and in step 4.2 Estimate the translation vector of relative-absolute coordinates for:

[0112]

[0113] Among them, column vectors The dimension is 3.

[0114] Step 4.4, based on step 4.2 and step 4.3 Calculate the matrix P consisting of the absolute coordinates of all nodes:

[0115]

[0116] This allows us to obtain the absolute coordinates of each node.

[0117] This method addresses the positioning needs of unmanned swarms under satellite denial conditions. It considers the differences in clock accuracy, flight dynamics, physical channel environment, and electromagnetic interference among the nodes of the unmanned swarm. Different distance error confidence levels are assigned to the different ranging capabilities of different links, achieving balanced acceptance of multiple ranging information. It also uses an iterative calculation method to achieve rapid convergence, thereby improving the accuracy and speed of relative positioning under denial. This method is of great significance for building efficient and flexible cooperative positioning capabilities.

[0118] The effectiveness of this method can be demonstrated through the following simulation experiments:

[0119] (1) Simulation Implementation Conditions

[0120] Thirty-two nodes are randomly distributed in three-dimensional space, and their coordinates are used as the true absolute positions. The distance between nodes is limited to a practical working range of 1000-5000 meters. The true Euclidean distance between all node pairs is calculated, and then random noise terms are added to introduce simulation measurement errors. The variances of these error terms are set to (10m) according to the channel estimation quality. 2 (5m) 2 (2m) 2 , (1m) 2 (0.5m) 2 This produces an observation distance that reflects the imperfections of the actual measurement. To establish a reference frame, four nodes are designated as anchor points with known absolute positions. The positioning accuracy is quantitatively evaluated by calculating the root mean square error between the estimated absolute coordinates of all nodes and their true positions.

[0121] The simulation platform utilizes an AMD Ryzen 76800H processor (8 cores, base frequency 3.20GHz), equipped with 32GB of memory, and supported by a 1TB M.2 solid-state drive for efficient data processing. The software environment runs on Windows 10, uses Python 3.11, and leverages the NumPy library for numerical acceleration to handle computationally intensive operations.

[0122] (2) Simulation Implementation Content and Results

[0123] Experimental results show that, compared with the method without confidence adjustment (see patent CN 2024104395475), the method of the present invention has significant improvements in both positioning accuracy and speed, as shown in Tables 1 and 2:

[0124] Table 1 Comparison of Positioning Accuracy

[0125] This patent 99.28％ 95.33％ 92.10％ 91.05％ 90.43％ Comparison Methods 91.24％ 86.15％ 83.05％ 76.57％ 67.50％

[0126] In terms of positioning speed, the method of this invention has a 94.10% probability of completing the calculation within 7 steps, while the probability of the comparative method drops to 78.83%, with a probability difference of 15.27%.

[0127] Table 2 Speed ​​Comparison

[0128] This patent 90.55％ 92.77％ 94.10％ 96.73％ 97.99％ Comparison Methods 64.26％ 70.17％ 73.83％ 84.69％ 89.98％

[0129] It can be seen that the method of this invention has a 91.05% probability of achieving a positioning accuracy within 1 meter, while the comparative method has only a 76.57% probability, representing an improvement of 14.48%. The method of this invention has a 90.43% probability of achieving a positioning accuracy within 0.5 meters, while the comparative method has only a 67.50% probability, representing an improvement of 22.93%. Considering all positioning accuracy levels of 10 meters, 5 meters, 2 meters, 1 meter, and 0.5 meters, the average probability improvement is 12.74%.

[0130] In summary, to achieve relative positioning of unmanned swarms with differing links in a satellite-denied environment, this invention employs a denial-based relative positioning method with adjustable confidence levels. Different distance error confidence levels are assigned to the varying ranging capabilities of different links, achieving balanced acceptance of multiple ranging information sources. Iterative calculations are used to achieve rapid convergence, thereby improving the accuracy of denial-based relative positioning. This invention is of great significance for building high-precision and high-efficiency spatial awareness capabilities for unmanned swarms under various satellite-denied conditions.

[0131] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined in this invention may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0132] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A rejection relative localization method based on adjustable confidence, applied to an unmanned cluster, wherein there are N nodes in the unmanned cluster, one of which is a cluster head node and the others are ordinary nodes, among the N nodes, T are anchor nodes with known absolute coordinates, and T<N; characterized by Includes the following steps: Step 1: Initialize the relative coordinate matrix and iteration count variable of the cluster head node. Iteration error variables Set the maximum number of iterations to The minimum iteration error is ; Obtain the observation distance between every two nodes, and set the variance of the measurement error for each observation distance; The specific method is as follows: Step 1.1, the cluster head node randomly selects values ​​for the relative coordinate matrix. Initialize to , It is an N x 3 matrix. The i-th row is used to represent the three-dimensional relative coordinates of the i-th node with respect to the cluster head node; Step 1.2: Initialize the iteration count variable in the cluster head node. The initial value is 0, and the maximum number of iterations is set to 0. ; Step 1.3: Initialize the iteration error variable in the cluster head node. The initial value is infinity, and the minimum iteration error is set to... ; Step 1.4: Each node in the unmanned cluster detects its own distance from all other nodes and sends this distance to the cluster head node; the cluster head node averages the distances between node i and node j sent by node i and node j respectively, to obtain the average value. and with average As the observation distance between node i and node j, the cluster head node obtains a total of [data missing]. Each observation distance; Step 1.5: Treat the measurement error of the observation distance as a random variable conforming to a Gaussian distribution. Based on the real-time dynamic changes in channel quality of the communication link between node i and node j, differentiate the error for each observation distance. Measurement error setting variance ; Step 2: Calculate the index matrix between every two nodes, the confidence level of each observation distance, and the transformation matrix; specifically: Step 2.1, calculate the index matrix between every two nodes, where the index matrix between node i and node j is... : in, for dimensional column vector, the first One element is 1, and all other elements are 0. The superscript T indicates the transpose of the matrix. Step 2.2, calculate the scaling factor K: Step 2.3, calculate the confidence level for each observation distance, where the observation distance... The confidence level is : Step 2.4, calculate the comprehensive index matrix V: Step 2.5, calculate the transformation matrix. : in, for A 1-dimensional column vector of all ones. for A dimensional all-1 row vector, where the superscript of N is -1 to find the reciprocal, and the superscript of a matrix is ​​-1 to find the inverse; Step 3: Iteratively update the relative coordinate matrix based on the relative coordinate matrix, observation distance, index matrix, confidence level, and transformation matrix until... Less than ,or achieve This yields the final relative coordinate matrix; Step 4, Normalization The relative and absolute coordinates of each anchor node are determined, the transformation relationship between the relative and absolute coordinates of the anchor nodes is estimated, and the absolute coordinates of all nodes are calculated based on the transformation relationship.

2. The method for rejecting relative positioning based on adjustable confidence as described in claim 1, characterized in that, The specific method for step 3 is as follows: Step 3.1, if the iteration error variable Less than Or the iteration count variable Greater than or equal to If the condition is met, the iteration stops and the final relative coordinate matrix is ​​output; otherwise, the subsequent steps are executed. Step 3.2, Calculate the numerical values : In the formula, distance for: Where the subscripts i and j represent node i and node j, respectively. , , They represent The x-axis, y-axis, and z-axis components of the relative coordinates of the corresponding nodes; Step 3.3, Calculate the intermediate matrix : Step 3.4, Update the relative coordinate matrix: Step 3.5, calculate the iteration error variable : in, Represents the 2-norm; Meanwhile, the iteration number variable Increase by 1, then return to step 3.

1.

3. The method for rejecting relative positioning based on adjustable confidence as described in claim 2, characterized in that, The specific method for step 4 is as follows: Step 4.1, normalize the relative and absolute coordinates of the anchor nodes: in, for A 3x3 matrix consisting of the relative coordinates of the anchor nodes. for A 3xT matrix consisting of the absolute coordinates of the anchor nodes. , The normalized matrix, for A dimensional column vector of all 1s; Step 4.2, estimate the rotation matrix between the relative and absolute coordinates of the anchor node. : Step 4.3: Estimate the translation vector between the relative and absolute coordinates of the anchor node. : in, for A dimensional column vector of all 1s; Step 4.4: Calculate the matrix consisting of the absolute coordinates of all nodes. : in, This is the final relative coordinate matrix obtained in step 3.

Images