Relative positioning method based on SMACOF and progressive Helmert transformation

Abstract

The invention discloses a relative positioning method based on SMACOF and progressive Helmert transformation, and the method comprises the steps: calculating a weighted Euclidean distance between nodes, and obtaining a weighted Euclidean distance matrix; wherein the nodes comprise unknown nodes and anchor nodes; calculating relative coordinates of all nodes according to the weighted Euclidean distance matrix through an MDS positioning algorithm to obtain a relative coordinate matrix; solving an optimal solution of the relative coordinate matrix through an SMACOF algorithm to obtain an optimal relative coordinate matrix; mapping the optimal relative coordinate matrix to a global coordinate through Helmert transformation to obtain an absolute coordinate matrix; optimizing the absolute coordinate matrix through a progressive correction mechanism to obtain an optimal transformation parameter; and carrying out Helmert transformation on the optimal relative coordinate matrix again through an optimal transformation parameter to obtain an optimal absolute coordinate matrix, thereby obtaining accurate positions of all nodes.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a relative positioning method based on SMACOF and progressive Helmert transformation. Background Art

[0002] With the widespread application of the Global Positioning System (GPS) and other satellite navigation technologies, positioning technology has been widely used in many fields, such as unmanned driving, robot navigation, intelligent transportation, and the Internet of Things. However, the traditional GPS positioning system is limited by factors such as obstacles and signal attenuation in some environments, which affects the positioning accuracy. In this context, relative positioning based on distance measurement has become an important research direction. By measuring the relative distance between devices through wireless communication technologies (such as UWB, Wi-Fi, Bluetooth, etc.), the location of the device can be estimated, especially indoors or in environments with weak GPS signals.

[0003] Multidimensional scaling (MDS) is a method commonly used for data dimensionality reduction and visualization. It maps high-dimensional data into low-dimensional space by preserving the distance relationship between the original data points as much as possible. MDS technology is widely used in many fields such as image processing, data visualization, and recommendation systems. In multi-point positioning, MDS constructs a distance matrix representing the relative distance between devices and reduces the dimension according to the matrix, thereby providing accurate low-dimensional coordinates for the positioning system. In the multi-point positioning problem of cluster networks, the basic optimization method of the MDS algorithm is usually to map the distance relationship in the high-dimensional space to the low-dimensional space by minimizing the stress function. However, the basic optimization method has some shortcomings in practical applications. In particular, when the data contains noise or measurement errors, the optimization process of the stress function may be disturbed, resulting in reduced accuracy of the results. The optimization of the stress function usually relies on traditional methods such as gradient descent, but these methods are prone to fall into local optimal solutions, and for complex error structures or high-dimensional data, the convergence speed is slow and the accuracy is poor. In addition, the Helmert transformation is a linear transformation used to transform a local coordinate system into a global coordinate system, which is commonly used for coordinate transformation in geographic information systems (GIS) and surveying. Its core is to align two coordinate systems by scaling, rotating and translating. However, during the application of the Helmert transformation, errors may be amplified during the transformation process, especially when the node distribution is uneven or the ranging accuracy is poor, the errors will significantly affect the final positioning results.

[0004] The Chinese invention application with application number 2024108476147 discloses "A high-precision indoor positioning method based on ultra-wideband", which includes the following steps: randomly placing the tags to be located in the base station signal coverage area, and collecting the signal arrival time difference measurement values ​​of the tags; initializing the weight matrix, and substituting the matrix into the preset positioning algorithm for initial position estimation; substituting the initial coordinates of the tag to be located into the signal arrival time difference equation group to calculate the error vector, and then taking the average value; inputting the obtained average value into the particle swarm algorithm to obtain the optimal weight matrix calculation formula parameters; moving the tag to be located, collecting the signal arrival time difference measurement values ​​of the tag to be located in real time, and inputting them into the weight matrix calculation formula of the optimal coefficient to obtain the weight matrix of the current tag to be located; substituting the weight matrix of the current tag to be located into the positioning algorithm to obtain a more accurate estimated position of the tag to be located. Summary of the invention

[0005] In order to solve the technical problems of insufficient accuracy and high complexity of existing positioning and navigation technologies, the present invention provides a relative positioning method based on SMACOF and progressive Helmert transformation. The technical solution adopted by the present invention is:

[0006] A first aspect of the present invention provides a relative positioning method based on SMACOF and progressive Helmert transformation, the method comprising:

[0007] Calculating the weighted Euclidean distance between nodes to obtain a weighted Euclidean distance matrix; wherein the nodes include unknown nodes and anchor nodes;

[0008] The weighted Euclidean distance matrix is ​​used to calculate the relative coordinates of all nodes through the MDS positioning algorithm to obtain a relative coordinate matrix;

[0009] The relative coordinate matrix is ​​solved by the SMACOF algorithm to obtain the optimal relative coordinate matrix;

[0010] Mapping the optimal relative coordinate matrix to the global coordinates through Helmert transformation to obtain an absolute coordinate matrix;

[0011] The absolute coordinate matrix is ​​optimized by a progressive correction mechanism to obtain optimal transformation parameters;

[0012] The optimal relative coordinate matrix is ​​again subjected to Helmert transformation using optimal transformation parameters to obtain an optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes.

[0013] As a preferred solution, the method of calculating the weighted Euclidean distance between nodes and obtaining the weighted Euclidean distance matrix includes:

[0014] In the d-dimensional space, there are n nodes, including m anchor nodes and nm unknown nodes, and the coordinates are defined as follows:

[0015]

[0016] By weighting the distance between each pair of points, the weighted Euclidean distance between the i-th node and the j-th node is:

[0017]

[0018] Among them, w ij is the weight between the i-th node and the j-th node, and the weight is defined as follows:

[0019]

[0020] where σ k Represents the standard deviation of noise in each dimension k;

[0021] The weighted Euclidean distance matrix obtained is as follows:

[0022]

[0023] where δ ij =δ ji And δ ii =0, the weighted Euclidean distance matrix D is a symmetric matrix.

[0024] As a preferred solution, the weighted Euclidean distance matrix is ​​used to calculate the relative coordinates of all nodes through the MDS positioning algorithm to obtain a relative coordinate matrix, which includes:

[0025] Through the secondary centralization operation The row and column sums of become zero;

[0026] The definition of the secondary centering operation is:

[0027]

[0028] Among them, D ⊙m represents the mth Hadamard power of D, J is a central matrix and can be expressed as:

[0029]

[0030] Among them, I N represents the N×N identity matrix, 1 N represents a column vector with N elements all set to 1;

[0031] Define the EDM relative to coordinate Y as Δ = [d i,j ] N×N , di,j is the distance between the relative coordinates of the i-th node and the relative coordinates of the j-th node:

[0032]

[0033] right The eigenvalue decomposition is performed as follows:

[0034]

[0035] where ∧=diag(λ1,λ2…λ N ), λ1≥λ2≥…≥λ N for The diagonal matrix composed of the eigenvalues ​​of in descending order, Q = [q1, q2, ...q N ]for The eigenvector matrix composed of the eigenvectors corresponding to the eigenvalues ​​of ;

[0036] take out The three largest eigenvalues ​​λ1, λ2, λ3 form a diagonal matrix ∧3 = diag(λ1, λ2, λ3), and the corresponding eigenvectors form an eigenvector matrix Q3 = [q1, q2, q3]. The relative coordinate Y of the node can be expressed as:

[0037]

[0038] Calculate the relative coordinates of n nodes and get the relative coordinate matrix:

[0039]

[0040] As a preferred solution, the relative coordinate matrix is ​​subjected to the SMACOF algorithm to find the optimal solution, and the method for obtaining the optimal relative coordinate matrix includes:

[0041] The objective function to be optimized is defined as follows:

[0042]

[0043] Decompose the objective function into three parts:

[0044]

[0045] in, is a constant term, is a convex quadratic function with a minimum value; the third term is bounded according to the Cauchy-Schwarz inequality:

[0046]

[0047] The Cauchy-Schwarz inequality is:

[0048]

[0049] Among them, z ij is a constant; when y ij =z ij When , the inequality in formula (13) becomes an equality, and we get:

[0050]

[0051] Convert formula (15) into matrix form:

[0052]

[0053] For η in formula (16) 2 (Y), there are:

[0054]

[0055] Among them, A ij =(e i -e j )(e i -e j ) T , a ii =a jj =1, a ij =a ji =-1, and the rest are equal to 0, then:

[0056]

[0057] Where tr is the trace of the matrix, and the matrix V is the function η 2 The Hessian matrix of (Y) is as follows:

[0058]

[0059] For ρ(Y) in formula (16), we have:

[0060]

[0061] in:

[0062]

[0063] but:

[0064]

[0065] In summary:

[0066]

[0067] According to formula (15), we have:

[0068]

[0069] By optimizing τ Z (Y) to iteratively solve σ(Y):

[0070]

[0071] Solving (25) yields:

[0072] Y * =V -1 B(Z)Z(26)

[0073] Among them, V -1 is the generalized inverse of V, and the obtained Y * That is, the optimal solution of the relative coordinate matrix Y.

[0074] As a preferred solution, the method of mapping the optimal relative coordinate matrix to the global coordinate by Helmert transformation to obtain the absolute coordinate matrix includes:

[0075] The following Helmert transformation formula is defined:

[0076] C′=T+Ro·S·X(27)

[0077] Where X is the local coordinate of all nodes, C′ is the global coordinate of the device, and R O is the rotation parameter, S is the scaling parameter, and T is the translation parameter;

[0078] The Helmert transformation consists of the following steps:

[0079] First, calculate the centroid node of the local and global coordinates of the anchor node, set and is the relative coordinate of the anchor node and the absolute coordinate of the centroid node; therefore, and The calculation formula is as follows:

[0080]

[0081] The formula for finding the anchor node in the new coordinate system using the centroid node is as follows:

[0082]

[0083] Assume Ro m is the rotation matrix, defined as follows:

[0084]

[0085] Let v1, v2, v3 and e1, e2, e3 be The eigendecomposition is calculated, so the rotation matrix is ​​calculated by formula (33):

[0086]

[0087] The calculation formula of the proportional factor is shown in formula (34):

[0088]

[0089] The translation matrix T is calculated by formula (35):

[0090] T=X g ′-SRoX l ′(35)

[0091] Finally, the optimal relative coordinate matrix Y for all nodes * , and its corresponding absolute matrix coordinate C′ is calculated using formula (36):

[0092] C′=T+Ro·S·Y * (36).

[0093] As a preferred solution, the method of optimizing the absolute coordinate matrix through a progressive correction mechanism to obtain the optimal transformation parameters includes:

[0094] After the initial Helmert transformation is completed, the transformed positioning error is calculated;

[0095] Define the residual as:

[0096]

[0097] in is the actual observed distance value, is the theoretical calculated distance between the transformed coordinates;

[0098] By calculating the residual ε between all nodes ij Perform statistical analysis to obtain the distribution of the current error:

[0099] Calculate the root mean square error of the overall error as the error evaluation indicator:

[0100]

[0101] Where N is the number of node pairs. According to the error evaluation index, the positioning error of the initial Helmert transformation is evaluated. If the root mean square error does not meet the preset convergence condition, the transformation parameters are adjusted; the transformation parameters include the scale factor S, the rotation matrix R O, and the translation variable T, the transformation parameter adjustment formula is as follows:

[0102]

[0103]

[0104] Among them, S o ,R o ,T o represents the current transformation parameters, S n ,R n ,T n represents the transformation parameters after iteration; η, α, γ are S o ,R o ,T o The learning rate controls the adjustment step size; RMSE is the gradient of the translation vector to the root mean square error;

[0105] After each iterative adjustment, the residual and root mean square error are recalculated to evaluate the current positioning error level. When the root mean square error meets the preset convergence condition, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iterative process is repeated to continue adjusting the transformation parameters.

[0106] As a preferred solution, the preset convergence condition is expressed as:

[0107] |ΔRMSE|<ε

[0108] Among them, ΔRMSE is the change of RMSE, and ε is a given threshold; when |ΔRMSE|<ε, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iteration process is repeated to continue adjusting the transformation parameters.

[0109] As a preferred solution, the method of performing Helmert transformation on the optimal relative coordinate matrix again by using optimal transformation parameters to obtain the optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes includes:

[0110] The obtained optimal transformation parameters and the optimal relative coordinate matrix are input into formula (36) to obtain the optimal absolute coordinate matrix.

[0111] A second aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the aforementioned relative positioning method based on SMACOF and progressive Helmert transformation are implemented.

[0112] The third aspect of the present invention provides a computer device, including a storage medium, a processor, and a computer program stored in the storage medium and executable by the processor. When the computer program is executed by the processor, the steps of the aforementioned relative positioning method based on SMACOF and progressive Helmert transformation are implemented.

[0113] Compared with the prior art, the present invention has the following beneficial effects:

[0114] The research object of the method of the present invention is several nodes of the cluster network in multidimensional space. Through the known position coordinates of the anchor nodes and the ranging information between all nodes, the weighted Euclidean distance matrix between all nodes is constructed, and the matrix reflects the relative position relationship between the nodes. The SMACOF algorithm is used to accurately capture and correct the deviation caused by measurement error or noise during the optimization process by adjusting the Euclidean distance between the nodes, so that the relative coordinates calculated by MDS are closer to the actual position. After obtaining the optimized relative coordinate matrix, the progressive Helmert transformation is carried out through multiple iterative steps. In each iterative process, the residual between the actual ranging value of the node and the distance calculation value between the transformed coordinates is gradually adjusted to adjust the scale factor, rotation matrix and translation vector, and the spatial position of the node is optimized until the positioning error converges to an acceptable range. Finally, the optimal scale factor, rotation matrix and translation vector are obtained, and the position of the unknown node in the cluster network is obtained using the Helmert transformation formula. It can be seen that the method of the present invention can accurately estimate the positions of all nodes in the cluster network by constructing a weighted Euclidean distance matrix and using the joint optimization method of the SMACOF algorithm and the progressive Helmert transform to solve the node position, especially in an environment with low signal-to-noise ratio or large errors. It can still maintain a high positioning accuracy, and provides a high-precision, low-complexity positioning method for cluster network nodes in multi-dimensional space. BRIEF DESCRIPTION OF THE DRAWINGS

[0115] Figure 1 A flow chart of a relative positioning method based on SMACOF and progressive Helmert transformation is provided for this embodiment. DETAILED DESCRIPTION

[0116] The accompanying drawings are only used for illustrative purposes and are not to be construed as limiting the present invention;

[0117] It should be clear that the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the embodiments of the present application.

[0118] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the embodiments of the present application. The singular forms of "a", "said" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0119] When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. On the contrary, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the attached claims. In the description of the present application, it should be understood that the terms "first", "second", "third", etc. are only used to distinguish similar objects, and do not have to be used to describe a specific order or sequence, nor can they be understood as indicating or implying relative importance. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances.

[0120] In addition, in the description of this application, unless otherwise specified, "multiple" means two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the previous and next associated objects are in an "or" relationship. The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0121] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0122] Example 1

[0123] Please refer to Figure 1 This embodiment provides a relative positioning method based on SMACOF and progressive Helmert transformation, the method comprising:

[0124] S1: Calculate the weighted Euclidean distance between nodes to obtain a weighted Euclidean distance matrix; wherein the nodes include unknown nodes and anchor nodes;

[0125] In a specific embodiment, the method of calculating the weighted Euclidean distance between nodes to obtain a weighted Euclidean distance matrix includes:

[0126] In the d-dimensional space, there are n nodes, including m anchor nodes and nm unknown nodes, and the coordinates are defined as follows:

[0127]

[0128] By weighting the distance between each pair of points, the weighted Euclidean distance between the i-th node and the j-th node is:

[0129]

[0130] Among them, w ij is the weight between the i-th node and the j-th node, and the weight is defined as follows:

[0131]

[0132] where σ k Represents the standard deviation of noise in each dimension k;

[0133] The weighted Euclidean distance matrix obtained is as follows:

[0134]

[0135] where δ ij =δ ji And δ ii =0, the weighted Euclidean distance matrix D is a symmetric matrix.

[0136] S2: Calculate the relative coordinates of all nodes using the weighted Euclidean distance matrix through the MDS positioning algorithm to obtain a relative coordinate matrix;

[0137] In a specific embodiment, the weighted Euclidean distance matrix is ​​used to calculate the relative coordinates of all nodes through the MDS positioning algorithm to obtain a relative coordinate matrix, which includes:

[0138] Through the secondary centralization operation The row and column sums of become zero;

[0139] The definition of the secondary centering operation is:

[0140]

[0141] Among them, D ⊙m represents the mth Hadamard power of D, J is a central matrix and can be expressed as:

[0142]

[0143] Among them, I N represents the N×N identity matrix, 1 N represents a column vector with N elements all set to 1;

[0144] Define the EDM relative to coordinate Y as Δ = [d i,j ] N×N , di,j is the distance between the relative coordinates of the i-th node and the relative coordinates of the j-th node:

[0145]

[0146] right The eigenvalue decomposition is performed as follows:

[0147]

[0148] where ∧=diag(λ1,λ2…λ N ), λ1≥λ2≥…≥λ N for The diagonal matrix composed of the eigenvalues ​​of in descending order, Q = [q1, q2, ...q N ]for The eigenvector matrix composed of the eigenvectors corresponding to the eigenvalues ​​of ;

[0149] take out The three largest eigenvalues ​​λ1, λ2, λ3 form a diagonal matrix ∧3 = diag(λ1, λ2, λ3), and the corresponding eigenvectors form an eigenvector matrix Q3 = [q1, q2, q3]. The relative coordinate Y of the node can be expressed as:

[0150]

[0151] Calculate the relative coordinates of n nodes and get the relative coordinate matrix:

[0152]

[0153] S3: The relative coordinate matrix is ​​subjected to the SMACOF algorithm to find the optimal solution, thereby obtaining the optimal relative coordinate matrix;

[0154] In a specific embodiment, the relative coordinate matrix is ​​subjected to the SMACOF algorithm to find the optimal solution, and the method for obtaining the optimal relative coordinate matrix includes:

[0155] The objective function to be optimized is defined as follows:

[0156]

[0157] Decompose the objective function into three parts:

[0158]

[0159] in, is a constant term, is a convex quadratic function with a minimum value; the third term is bounded according to the Cauchy-Schwarz inequality:

[0160]

[0161] The Cauchy-Schwarz inequality is:

[0162]

[0163] Among them, z ij is a constant; when y ij =z ij When , the inequality in formula (13) becomes an equality, and we get:

[0164]

[0165] Convert formula (15) into matrix form:

[0166]

[0167] For η in formula (16) 2 (Y), there are:

[0168]

[0169] Among them, A ij =(e i -e j )(e i -e j ) T , a ii =a jj =1,a ij =a ji =-1, and the rest are equal to 0, then:

[0170]

[0171] Where tr is the trace of the matrix, and the matrix V is the function η 2 The Hessian matrix of (Y) is as follows:

[0172]

[0173] For ρ(Y) in formula (16), we have:

[0174]

[0175] in:

[0176]

[0177] but:

[0178]

[0179] In summary:

[0180]

[0181] According to formula (15), we have:

[0182]

[0183] By optimizing τ Z (Y) to iteratively solve σ(Y):

[0184]

[0185] Solving (25) yields:

[0186] Y * =V -1 B(Z)Z(26)

[0187] Among them, V -1 is the generalized inverse of V, and the obtained Y * That is, the optimal solution of the relative coordinate matrix Y.

[0188] S4: Mapping the optimal relative coordinate matrix to the global coordinate through Helmert transformation to obtain an absolute coordinate matrix;

[0189] In a specific embodiment, the method of mapping the optimal relative coordinate matrix to the global coordinate by Helmert transformation to obtain the absolute coordinate matrix includes:

[0190] The following Helmert transformation formula is defined:

[0191] C′=T+Ro·S·X(27)

[0192] Where X is the local coordinate of all nodes, C′ is the global coordinate of the device, and R O is the rotation parameter, S is the scaling parameter, and T is the translation parameter;

[0193] The Helmert transformation consists of the following steps:

[0194] First, calculate the centroid node of the local and global coordinates of the anchor node, set and is the relative coordinate of the anchor node and the absolute coordinate of the centroid node; therefore, and The calculation formula is as follows:

[0195]

[0196] The formula for finding the anchor node in the new coordinate system using the centroid node is as follows:

[0197]

[0198] Assume Ro m is the rotation matrix, defined as follows:

[0199]

[0200] Let v1, v2, v3 and e1, e2, e3 be The eigendecomposition is calculated, so the rotation matrix is ​​calculated by formula (33):

[0201]

[0202] The calculation formula of the proportional factor is shown in formula (34):

[0203]

[0204] The translation matrix T is calculated by formula (35):

[0205] T=X g ′-SRoX l ′(35)

[0206] Finally, the optimal relative coordinate matrix Y for all nodes * , and its corresponding absolute matrix coordinate C′ is calculated using formula (36):

[0207] C′=T+Ro·S·Y * (36).

[0208] S5: Optimizing the absolute coordinate matrix through a progressive correction mechanism to obtain optimal transformation parameters;

[0209] In a specific embodiment, the method of optimizing the absolute coordinate matrix through a progressive correction mechanism to obtain the optimal transformation parameters includes:

[0210] After the initial Helmert transformation is completed, the transformed positioning error is calculated;

[0211] Define the residual as:

[0212]

[0213] in is the actual observed distance value, is the theoretical calculated distance between the transformed coordinates;

[0214] By calculating the residual ε between all nodes ij Perform statistical analysis to obtain the distribution of the current error:

[0215] Calculate the root mean square error of the overall error as the error evaluation indicator:

[0216]

[0217] Where N is the number of node pairs. According to the error evaluation index, the positioning error of the initial Helmert transformation is evaluated. If the root mean square error does not meet the preset convergence condition, the transformation parameters are adjusted; the transformation parameters include the scale factor S, the rotation matrix R O , and the translation variable T, the transformation parameter adjustment formula is as follows:

[0218]

[0219]

[0220] Among them, S o ,R o ,T o represents the current transformation parameters, S n ,R n ,T n represents the transformation parameters after iteration; η, α, γ are S o ,R o ,T o The learning rate controls the adjustment step size; RMSE is the gradient of the translation vector to the root mean square error;

[0221] After each iterative adjustment, the residual and root mean square error are recalculated to evaluate the current positioning error level. When the root mean square error meets the preset convergence condition, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iterative process is repeated to continue adjusting the transformation parameters.

[0222] In a specific embodiment, the preset convergence condition is expressed as:

[0223] |ΔRMSE|<ε

[0224] Among them, ΔRMSE is the change of RMSE, and ε is a given threshold; when |ΔRMSE|<ε, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iteration process is repeated to continue adjusting the transformation parameters.

[0225] Specifically, ε is set to 10 -5 .

[0226] S6: performing Helmert transformation on the optimal relative coordinate matrix again using the optimal transformation parameters to obtain an optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes;

[0227] In a specific embodiment, the method of performing Helmert transformation on the optimal relative coordinate matrix again by using optimal transformation parameters to obtain the optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes includes:

[0228] The obtained optimal transformation parameters and the optimal relative coordinate matrix are input into formula (36) to obtain the optimal absolute coordinate matrix.

[0229] Example 2

[0230] Please refer to Figure 1 This embodiment provides a relative positioning method based on SMACOF and progressive Helmert transformation, the method comprising:

[0231] Calculating the weighted Euclidean distance between nodes to obtain a weighted Euclidean distance matrix; wherein the nodes include unknown nodes and anchor nodes;

[0232] The weighted Euclidean distance matrix is ​​used to calculate the relative coordinates of all nodes through the MDS positioning algorithm to obtain a relative coordinate matrix;

[0233] The relative coordinate matrix is ​​solved by the SMACOF algorithm to obtain the optimal relative coordinate matrix;

[0234] Mapping the optimal relative coordinate matrix to the global coordinates through Helmert transformation to obtain an absolute coordinate matrix;

[0235] The absolute coordinate matrix is ​​optimized by a progressive correction mechanism to obtain optimal transformation parameters;

[0236] The optimal relative coordinate matrix is ​​again subjected to Helmert transformation using optimal transformation parameters to obtain an optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes.

[0237] Specifically, suppose there is a drone swarm in three-dimensional space. Some drones cannot receive satellite signals due to signal obstruction or other reasons, and thus cannot locate themselves through the GPS on board. Therefore, collaborative positioning is required through other drones. Assume that there are N drones in the drone swarm, and the real coordinate matrix is ​​X = [x1, x2, …, x N ] T ∈R N×3 , where x i =[a i ,b i ,c i ] T is the 3D coordinate of the i-th UAV. The Euclidean distance d between the i-th UAV and the j-th UAV ij It can be expressed as:

[0238]

[0239] The Euclidean distance matrix D between all N drones can be written as:

[0240]

[0241] where d i,j =d j,i And d i,i = 0, so D is a symmetric matrix. The UAVs measure the distance between each other, and the i-th UAV measures the distance between itself and the j-th UAV. It can be expressed as:

[0242]

[0243] in is the ranging error that follows Gaussian distribution After all drones have completed the distance measurement, they send the measured distance to the central drone. The central drone forms an estimated distance matrix with the received distance measurements In getting the distance matrix Finally, the relative coordinate matrix Y = [y1, y2, ..., y N ] T ∈R N×3 , and then perform SMACOF optimization to obtain the optimal solution of the relative coordinate matrix

[0244] After obtaining the relative coordinates of all drones, that is, the global coordinates, it is necessary to transform the global coordinates into absolute coordinates. Assume that there are a total of M drones with available GPS coordinates in the drone group, where M≤N. When M<3, only relative positioning of all drones can be achieved. Only when M≥3 can the absolute coordinates of all drones be obtained. Let the GPS coordinate matrix of these M drones be G=[g α1 ,g α2 ,…g αM ] T ∈R M×3 , where g αi For the αth i The GPS coordinates of the M drones, let the global coordinate matrix of these M drones be Z M =[z β1 ,z β2 ,…z βM ] T ∈R M×3 , where z βi For the β iThe global coordinates of the M drones. The initial values ​​of the Helmert transformation parameters are obtained using the GPS coordinates of the M drones and the global coordinates. The scale factor S and the rotation matrix R are iteratively obtained using the progressive correction mechanism by analyzing the residual between the actual observed distance measurement value and the distance calculated by the relative coordinates. O , and the optimal solution of the translation variable T, and finally the absolute coordinates of the drone swarm can be obtained by performing Helmert coordinate transformation on the global coordinate Z

[0245]

[0246] Ultimately, the drone swarm network achieves the positioning of all drones.

[0247] Example 3

[0248] A computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of a relative positioning method based on SMACOF and progressive Helmert transformation described in Example 1 are implemented.

[0249] Example 4

[0250] A computer device includes a storage medium, a processor, and a computer program stored in the storage medium and executable by the processor. When the computer program is executed by the processor, the steps of a relative positioning method based on SMACOF and progressive Helmert transformation described in Example 1 are implemented.

[0251] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A relative positioning method based on SMACOF and progressive Helmert transformation, characterized in that: The method comprises: Calculating the weighted Euclidean distance between nodes to obtain a weighted Euclidean distance matrix; wherein the nodes include unknown nodes and anchor nodes; The weighted Euclidean distance matrix is ​​used to calculate the relative coordinates of all nodes through the MDS positioning algorithm to obtain a relative coordinate matrix; The relative coordinate matrix is ​​solved by the SMACOF algorithm to obtain the optimal relative coordinate matrix; Mapping the optimal relative coordinate matrix to the global coordinates through Helmert transformation to obtain an absolute coordinate matrix; The absolute coordinate matrix is ​​optimized by a progressive correction mechanism to obtain optimal transformation parameters; The optimal relative coordinate matrix is ​​again subjected to Helmert transformation using optimal transformation parameters to obtain an optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes.

2. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 1, characterized in that: Methods for calculating the weighted Euclidean distance between nodes and obtaining a weighted Euclidean distance matrix include: In the d-dimensional space, there are n nodes, including m anchor nodes and nm unknown nodes, and the coordinates are defined as follows: By weighting the distance between each pair of points, the weighted Euclidean distance between the i-th node and the j-th node is: Among them, w ij is the weight between the i-th node and the j-th node, and the weight is defined as follows: where σ k Represents the standard deviation of noise in each dimension k; The weighted Euclidean distance matrix obtained is as follows: where δ ij =δ ji And δ ii =0, the weighted Euclidean distance matrix D is a symmetric matrix.

3. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 2, characterized in that: The method of calculating the relative coordinates of all nodes by using the weighted Euclidean distance matrix through the MDS positioning algorithm to obtain the relative coordinate matrix includes: Through the secondary centralization operation The row and column sums of become zero; The definition of the secondary centering operation is: Among them, D ⊙m represents the mth Hadamard power of D, J is a central matrix and can be expressed as: Among them, I N represents the N×N identity matrix, 1 N represents a column vector with N elements all set to 1; Define the EDM relative to coordinate Y as Δ = [d i,j ] N×N , d i,j is the distance between the relative coordinates of the i-th node and the relative coordinates of the j-th node: right The eigenvalue decomposition is performed as follows: where ∧=diag(λ1,λ2…λ N ), λ1≥λ2≥…≥λ N for The diagonal matrix composed of the eigenvalues ​​of in descending order, Q = [q1, q2, ...q N ]for The eigenvector matrix composed of the eigenvectors corresponding to the eigenvalues ​​of ; take out The three largest eigenvalues ​​λ1, λ2, λ3 form a diagonal matrix ∧3 = diag(λ1, λ2, λ3), and the corresponding eigenvectors form an eigenvector matrix Q3 = [q1, q2, q3]. The relative coordinate Y of the node can be expressed as: Calculate the relative coordinates of n nodes and get the relative coordinate matrix:

4. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 3, characterized in that: The relative coordinate matrix is ​​solved by the SMACOF algorithm to obtain the optimal solution. The method of obtaining the optimal relative coordinate matrix includes: The objective function to be optimized is defined as follows: Decompose the objective function into three parts: in, is a constant term, is a convex quadratic function with a minimum value; the third term is bounded according to the Cauchy-Schwarz inequality: The Cauchy-Schwarz inequality is: Among them, z ij is a constant; when y ij =z ij When , the inequality in formula (13) becomes an equality, and we get: Convert formula (15) into matrix form: For η in formula (16) 2 (Y), there are: Among them, A ij =(e i -e j )(e i -e j ) T , a ii =a jj =1,a ij =a ji =-1, and the rest are equal to 0, then: Where tr is the trace of the matrix, and the matrix V is the function η 2 The Hessian matrix of (Y) is as follows: For ρ(Y) in formula (16), we have: in: but: In summary: According to formula (15), we have: By optimizing τ Z (Y) to iteratively solve σ(Y): Solving (25) yields: Y * =V -1 B(Z)Z(26) Among them, V -1 is the generalized inverse of V, and the obtained Y * That is, the optimal solution of the relative coordinate matrix Y.

5. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 4, characterized in that: The method of mapping the optimal relative coordinate matrix to the global coordinate by Helmert transformation to obtain the absolute coordinate matrix includes: The following Helmert transformation formula is defined: C′=T+Ro·S·X(27) Where X is the local coordinate of all nodes, C′ is the global coordinate of the device, and R O is the rotation parameter, S is the scaling parameter, and T is the translation parameter; The Helmert transformation consists of the following steps: First, calculate the centroid node of the local and global coordinates of the anchor node, set and is the relative coordinate of the anchor node and the absolute coordinate of the centroid node; therefore, and The calculation formula is as follows: The formula for finding the anchor node in the new coordinate system using the centroid node is as follows: Assume Ro m is the rotation matrix, defined as follows: Let v1, v2, v3 and e1, e2, e3 be The eigendecomposition is calculated, so the rotation matrix is ​​calculated by formula (33): The calculation formula of the proportional factor is shown in formula (34): The translation matrix T is calculated by formula (35): T=X g ′-SRoX l ′(35) Finally, the optimal relative coordinate matrix Y for all nodes * , and its corresponding absolute matrix coordinate C′ is calculated using formula (36): C′=T+Ro·S·Y * (36)。 6. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 5, characterized in that: The method of optimizing the absolute coordinate matrix by a progressive correction mechanism to obtain the optimal transformation parameters includes: After the initial Helmert transformation is completed, the transformed positioning error is calculated; The residual is defined as: in is the actual observed distance value, is the theoretical calculated distance between the transformed coordinates; By calculating the residual ε between all nodes ij Perform statistical analysis to obtain the distribution of the current error: Calculate the root mean square error of the overall error as the error evaluation indicator: Where N is the number of node pairs. According to the error evaluation index, the positioning error of the initial Helmert transformation is evaluated. If the root mean square error does not meet the preset convergence condition, the transformation parameters are adjusted; the transformation parameters include the scale factor S, the rotation matrix R O , and the translation variable T, the transformation parameter adjustment formula is as follows: Among them, S o ,R o ,T o represents the current transformation parameters, S n ,R n ,T n represents the transformation parameters after iteration; η, α, γ are S o ,R o ,T o The learning rate controls the adjustment step size; is the gradient of the translation vector to the root mean square error; After each iterative adjustment, the residual and root mean square error are recalculated to evaluate the current positioning error level. When the root mean square error meets the preset convergence condition, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iterative process is repeated to continue adjusting the transformation parameters.

7. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 6, characterized in that: The preset convergence condition is expressed as: |ΔRMSE|<ε Among them, ΔRMSE is the change of RMSE, and ε is a given threshold; when |ΔRMSE|<ε, the iteration is stopped to obtain the optimal transformation parameters; otherwise, the iteration process is repeated to continue adjusting the transformation parameters.

8. A relative positioning method based on SMACOF and progressive Helmert transformation according to claim 6, characterized in that: The method of performing Helmert transformation on the optimal relative coordinate matrix again by using the optimal transformation parameters to obtain the optimal absolute coordinate matrix, thereby obtaining the precise positions of all nodes includes: The obtained optimal transformation parameters and the optimal relative coordinate matrix are input into formula (36) to obtain the optimal absolute coordinate matrix.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of a relative positioning method based on SMACOF and progressive Helmert transformation as described in any one of claims 1 to 8 are implemented.

10. A computer device, characterized in that: It includes a storage medium, a processor, and a computer program stored in the storage medium and executable by the processor. When the computer program is executed by the processor, the steps of a relative positioning method based on SMACOF and progressive Helmert transformation as described in any one of claims 1 to 8 are implemented.

Images