Single-master-node collaborative navigation algorithm constrained by space-time kinematics information

Abstract

The invention discloses a single-master-node collaborative navigation algorithm constrained by space-time kinematics information, which comprises the following steps of: S1, constraining positioning of slave nodes by using a time sequence of an inertial trajectory of a master node, and constructing a collaborative positioning optimization objective function; s2, correcting an elevation error by utilizing elevation information, determining solving targets as an optimal rotation matrix and an optimal translation matrix, and introducing spatial relative position constraints between a master node and slave nodes; s3, solving an optimal rotation matrix and an optimal translation matrix by adopting an SVD (Singular Value Decomposition) method; according to the method, through the space-time continuity characteristic of motion of the unmanned aerial vehicle, fusion modeling is carried out by using the trajectory characteristics and the space structure, and high-precision positioning estimation of slave nodes is realized on the basis of keeping the lightweight design of the system.

Description

Technical Field

[0001] This invention relates to the field of cooperative navigation technology, and in particular to a single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematic information. Background Technology

[0002] Currently, UAV swarms, with their flexible deployment, high mobility, and collaborative capabilities, have become a core means of improving mission efficiency. To ensure the efficient execution of swarm missions, the relative positioning accuracy and global navigation capabilities between nodes are crucial. However, in environments with regional signal denial caused by factors such as third-party interference, satellite obstruction, and complex electromagnetic environments, traditional navigation schemes based on Global Navigation Satellite Systems often fail, leading to problems such as rapid degradation of navigation accuracy and failure of collaborative missions for UAV swarms. Therefore, developing highly robust and adaptable collaborative navigation algorithms has become one of the key technological challenges in the intelligent development of unmanned systems.

[0003] Under area denial conditions, due to environmental factors or mission requirements, only some nodes can obtain high-precision position information, while the remaining nodes must rely on low-cost sensors such as inertial navigation systems, ranging modules, and altimeters to acquire their own status. However, inertial navigation suffers from significant error accumulation, and ranging modules are highly susceptible to environmental fluctuations. Therefore, the overall navigation accuracy of the system is prone to deterioration over time, especially in situations with large formations and high relative speeds, where this deterioration can be rapidly amplified, severely impacting the stability and safety of mission completion. Adding to the complexity, in scenarios with a single lead node, only one master node can obtain high-precision global position information. How to improve the autonomous positioning capabilities of the remaining slave nodes without relying on external information or adding new hardware complexity becomes a pressing issue. Summary of the Invention

[0004] The purpose of this invention is to provide a single-master-node cooperative navigation algorithm that addresses the spatiotemporal kinematic information constraints of the aforementioned technical problems.

[0005] Therefore, the technical solution of the present invention is as follows:

[0006] A single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information, comprising the following steps:

[0007] S1. Take the unmanned vehicle equipped with high-precision navigation devices as the master node and the unmanned vehicle equipped with low-precision navigation devices as the slave node, so as to use the time series of the inertial trajectory of the master node to constrain the positioning of the slave node; construct a cooperative positioning optimization objective function with the goal of minimizing the error between the estimated position and the observed distance.

[0008] S2. Using elevation information to correct elevation errors, the three-dimensional optimization problem of the slave node is simplified into a two-dimensional optimization problem. The solution objectives are determined to be the optimal rotation matrix and the optimal translation matrix, and spatial relative position constraints between the master node and the slave node are introduced.

[0009] S3. Combine the cooperative positioning optimization objective function with the spatial relative position constraints between the master node and the slave node, and use the SVD method to solve for the optimal rotation matrix and the optimal translation matrix.

[0010] Furthermore, the specific implementation steps of step S1 are as follows:

[0011] S101, Continuously collect data from master and slave nodes. a ~T b The inertial navigation output position information for a time period, and the ranging observation values ​​between the slave node and the master node at each inertial navigation output time obtained by the ranging module of the slave node;

[0012] S102, Based on master node self-T a ~T b The inertial navigation system outputs position information over a time period, and a reference trajectory sequence consisting of N consecutive master node position information is constructed, the expression of which is:

[0013]

[0014] In the formula, T represents a The position information output by the navigation system of the master node at any given time, [T a ,T b [] indicates a time period;

[0015] S103, Based on slave node self-T a ~T b The inertial navigation output position information for the time period, as well as the ranging observations between the slave and master nodes, are used to construct the inertial trajectory sequence to be optimized. With the corresponding measurement sequence ρ:

[0016]

[0017] in, T represents i Time-based node location information, ρ represents T i The distance measurement observation value from the node at any time;

[0018] S104. The expression for the constructed cooperative localization optimization objective function is as follows:

[0019]

[0020] In the formula, ρ represents the distance measurement observation between the slave node and the master node. The spatial coordinate vector of the master node. r represents the location information of the slave node to be obtained through the cooperative localization algorithm. Where a1, a2, and a3 represent the coordinates of the master node in three-dimensional space, and b1, b2, and b3 represent the coordinates to be estimated for the slave nodes. and These are mutually orthogonal orientation bases in space;

[0021] S105. Incorporate the constraint information from steps S102 and S103 into the cooperative localization optimization objective function constructed based on step S104, to obtain:

[0022]

[0023] In the formula, k is T a ~T b At any point within the time period, Let r be the position information output by the navigation system of the master node at time k. k Let ρ be the location information of the slave node to be obtained by the cooperative localization algorithm at time k. k Let be the distance measurement observation between the slave node and the master node at time k;

[0024] S106, r can be regarded as The approximate result after rotation and translation leads to the following cooperative localization optimization objective function:

[0025]

[0026] In the formula, R s Let t represent a 3×3 rotation matrix. s It is a 3×1 translation matrix; R i,j and t i,j R respectively s and t s The element in the i-th row and j-th column.

[0027] Furthermore, in step S2, the expression for the cooperative localization optimization objective function, obtained by simplifying the three-dimensional optimization problem of the nodes into a two-dimensional optimization problem, is as follows:

[0028]

[0029] In the formula, This represents the observation distance of the slave node relative to the master node in the xoy plane. and Let R represent the position vectors of the master node and the slave node in the two-dimensional plane at time k; s,xoyIt is a 2×2 optimal rotation matrix, t s,xoy This represents the corresponding optimal translation matrix of order 2×1;

[0030] in, The expression is:

[0031]

[0032] In the formula, ρ k Δh represents the distance measurement between the slave node and the master node at time k; k Used to represent the elevation difference between the slave node and the master node at time k.

[0033] Furthermore, in step S2, the spatial relative position constraint between the master node and the slave node is as follows:

[0034]

[0035] In the formula, and These are mutually orthogonal orientation bases in space; This represents the position coordinates in the x-direction output by the navigation system of the master node. This represents the position coordinates in the y-direction output by the navigation system of the master node; This represents the position coordinates in the x-direction output from the node's navigation system. This represents the position coordinates in the y-direction output from the node's navigation system; and for The elements in the table represent the position coordinates of the two horizontal axes from which the node is corrected.

[0036] Furthermore, the specific implementation steps of step S3 are as follows:

[0037] S301. Construct a data matrix to represent the coordinate set of the master node as follows: The coordinate set of the node will be represented as: In the formula, N is the number of valid sampling points within the time window, N = T b -T a +1;

[0038] S302. Using the SVD method, calculate the initial solutions for the optimal rotation matrix and the optimal translation matrix, including:

[0039] (1) Represent the geometric center of the master node and slave node coordinates as: In the formula, The mean vector representing the coordinates of the master node. This represents the mean vector of the node coordinates;

[0040] (2) Construct the covariance matrix H: In the formula, H is the covariance matrix of the centered coordinates. To center the vector from the node coordinates, The transpose of the centralized vector of the master node coordinates;

[0041] (3) Using the SVD decomposition formula: [U,Σ,V]=SVD(H), the initial solution of the optimal rotation matrix is ​​calculated. and the initial solution of the optimal translation matrix in,

[0042] S303. Based on the initial solution obtained in step S302, the optimal rotation matrix R... s,xoy and the optimal translation matrix t s,xoy Performing constrained nonlinear optimization includes:

[0043] (1) The rotation matrix is ​​parameterized and rewritten as: In the formula, θ represents the rotation angle of the slave node coordinate system relative to the master node coordinate system; the optimized vector parameterization is rewritten as: x = [θ, t x ,t y ] T In the formula, t x Let t be the translation in the x-direction. y This represents the translation in the y-direction;

[0044] (2) Based on the distance residual and the sign constraint, construct the augmented residual function, the expression of which is: In the formula, r dist (x) represents the set of distance residual terms, containing the distance error at all times; r sign (x) represents the set of symbolic constraint terms, which includes all spatial relation constraints;

[0045] The expression for the distance residual term at time K is: In the formula, This represents the projection of the three-dimensional distance measurement value onto the xoy plane; R(θ) represents the true coordinates of the master node in the plane at time k; R(θ) is the rotation operator, i.e., the aforementioned rotation matrix. The original position of the node is the coordinate value to be corrected.

[0046] The expression for the symbolic constraint term is: In the formula, sign is the symbol extraction function. These are the original spatial relation values, referring to the constraint matrix. The element in the i-th row and j-th column; p ij It is the transformed spatial relation value, referring to the corresponding element in the constraint matrix p;

[0047] S304. Construct the Jacobian matrix, its expression is:

[0048] The expression for the derivative of the distance residual is: In the formula, r dist,k Represents the k-th distance residual. This represents the corrected coordinate difference vector between the master node and the slave node. These are the coordinates of the master node in the xoy plane at time k. It is the coordinate of the node in the xoy plane at time k; It is the partial derivative of the rotation matrix with respect to the rotation angle θ;

[0049] The expression for the sign-constrained derivative is: α = 10 3 In the formula, r sign,ij For the sign constraint residual, it corresponds to the element in the i-th row and j-th column of the constraint matrix; These are the corrected coordinate elements, which are elements in the constraint matrix p, and α is the smoothing factor;

[0050] S305. Using the LM algorithm, the optimal rotation matrix and the optimal translation matrix are obtained by iterative solution.

[0051] Furthermore, the specific implementation steps of step S305 are as follows:

[0052] 1) Set the iterative update equation, whose expression is:

[0053] (J T J+λI)δ=-J T r,

[0054] In the formula, λ is the damping factor, I is the identity matrix, and δ is the parameter update amount;

[0055] 2) Set the parameter update formula:

[0056] x (n+1) =x (n) +δ,

[0057] In the formula, x (n) x represents the parameter of the nth iteration of the optimization vector. (n) =[θ (n) ,t x (n) ,t y (n) ] T ;

[0058] 3) Set the adaptive adjustment formula for the damping factor:

[0059]

[0060] In the formula, r (n) The residual of the nth iteration is expressed as: And it is used to evaluate the objective function value under the current parameters;

[0061] 4) Set the iteration termination condition, which is as follows:

[0062] The iteration terminates when any of the following conditions are met:

[0063] In the formula, τ1=10 -6 The threshold for residual variation is τ2 = 10. -5 N represents the parameter update amount. max =100 is the threshold for the maximum number of iterations;

[0064] Therefore, the optimal transformation parameters are:

[0065] In the formula, θ * , This is a reference value for convergence.

[0066] Compared with existing technologies, this spatiotemporal kinematics-constrained single-master-node cooperative navigation algorithm fully exploits the time-series information contained in the inertial navigation trajectory and combines it with the relative spatial position constraints between nodes in the cooperative navigation formation to construct a cooperative positioning optimization model with physical consistency and solution feasibility. This not only effectively eliminates erroneous solutions caused by inertial navigation drift or measurement errors and improves the system's anti-interference capability, but also introduces uniqueness constraints on the geometric structure, making the cooperative positioning problem have uniqueness and analytical properties. While maintaining the lightweight design of the system, it achieves high-precision positioning estimation of slave nodes. Attached Figure Description

[0067] Figure 1 This is a flowchart of the single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information according to the present invention.

[0068] Figure 2 This is a schematic diagram illustrating the error problem between the master node and slave node in cooperative navigation positioning in a single master node cooperative navigation algorithm constrained by spatiotemporal kinematic information according to an embodiment of the present invention.

[0069] Figure 3 This is a schematic diagram of the inertial trajectory timing-assisted single-master-node cooperative positioning in step S2 of the single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematic information in an embodiment of the present invention. Detailed Implementation

[0070] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but the following embodiments are by no means intended to limit the present invention.

[0071] This embodiment takes a UAV swarm cooperative positioning system in a satellite signal denial environment as an example. In this system, there is only one UAV with a reliable and stable navigation information source, which is identified as the master node. The other UAVs, without the introduction of additional hardware, are equipped with only low-cost inertial navigation systems, ranging modules and altimeters, and are identified as slave nodes. Each slave node is equipped with a master node and navigation constraints are applied to achieve accurate acquisition of cooperative navigation position information of the slave node UAVs.

[0072] Based on this, see Figure 1 The specific implementation steps of the single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information of the present invention are described below.

[0073] S1. Use the time series constraints of the master node's inertial trajectory to locate the slave node, in order to eliminate erroneous solutions and reduce the risk of location errors.

[0074] S101, Continuously collect data from master and slave nodes. a ~T b The inertial navigation output position information for a time period, and the ranging observation values ​​between the slave node and the master node at each inertial navigation output time obtained by the ranging module of the slave node; in this embodiment, through this step S101, the inertial navigation output position information of the master node and the slave node is obtained in a total of 700s, and the number of position information N is 1000, which is composed of the data information collected in the total of 700s.

[0075] like Figure 2 As shown, Represents the slave node coordinates in a three-dimensional coordinate system. Let r be the spatial coordinates of the master node, and r be the coordinates of the slave node to be obtained through the cooperative localization algorithm. Since a slave node cannot obtain accurate information about its own location when it knows the location of a master node and its distance relative to the master node, additional constraints are needed to obtain the optimal estimate of its own position without the assistance of other sensor information.

[0076] S102, Based on master node self-T a ~T b The inertial navigation system outputs position information over a time period, and a reference trajectory sequence consisting of N consecutive master node position information is constructed, the expression of which is:

[0077]

[0078] In the formula, T represents a The position information output by the navigation system of the master node at any given time, [T a ,Tb [] represents a time period, where two time points satisfy T a <T b , And a < b, Denotes the set of rational numbers. Represents the set of positive integers.

[0079] S103, Based on slave node self-T a ~T b The inertial navigation output position information for the time period, as well as the ranging observations between the slave and master nodes, are used to construct the inertial trajectory sequence to be optimized. Together with the corresponding measurement sequence ρ for subsequent collaborative estimation, their expressions are as follows:

[0080]

[0081] in, T represents i Time-based node location information, ρ represents T i The distance observation value from the node at time i ∈ [a, b]

[0082] S104. As described above, in the context of this embodiment, the problem to be solved is the problem of minimizing the cooperative localization between slave nodes and master nodes, with the goal of minimizing the error between the estimated position and the observed distance.

[0083] Therefore, the expression for the cooperative localization optimization objective function constructed based on the above optimization problem is as follows:

[0084]

[0085] In the formula, ρ is the distance observation value between the slave node and the master node, which belongs to scalar observation data; is the spatial coordinate vector of the master node, and r is the position information of the slave node to be obtained through the cooperative localization algorithm.

[0086] In this application, the key to cooperative localization lies in finding the optimal solution r that makes the above cooperative localization optimization objective function hold, that is, determining the three-dimensional spatial position of the slave node.

[0087] because Both r and r are 3-dimensional vectors, and since they are two vectors, they can be further expressed as:

[0088]

[0089] In the formula, a1, a2, and a3 represent the coordinates of the master node in three-dimensional space, and b1, b2, and b3 represent the coordinates to be estimated for the slave node. and They are mutually orthogonal directional bases in space.

[0090] S105. Based on the cooperative localization optimization objective function constructed in step S104, and incorporating the constraint information introduced in steps S102 and S103, the cooperative localization optimization objective function is further rewritten as follows:

[0091]

[0092] For ease of calculation, the above formula is rewritten as:

[0093]

[0094] In the formula, k is T a ~T b At any point within the time period, Let r be the position information output by the navigation system of the master node at time k. k Let ρ be the location information of the slave node to be obtained by the cooperative localization algorithm at time k. k Let k be the distance measurement observation between the slave node and the master node at time k.

[0095] Based on the above construction results, after establishing the cooperative localization optimization objective function based on a single master node, how to effectively solve for r in the formula? k This becomes the core issue of this section.

[0096] S106. Given the inherent characteristics of inertial navigation systems, the trajectory sequences they generate often have systematic deviations from the actual flight paths. These deviations are not only reflected in the offset in position estimation but also in the angular drift in attitude estimation. Therefore, in the actual solution process, the coupled effects of position error and orientation error on the estimation results must be considered simultaneously to improve the accuracy and stability of the overall solution.

[0097] Furthermore, r can be viewed as The approximate results after rotation and translation lead to the following rewrite of the cooperative localization optimization objective function:

[0098]

[0099] In the formula, R s Let t represent a 3×3 rotation matrix. s It is a 3×1 translation matrix; R i,j and t i,j R respectively s and t s The element in the i-th row and j-th column.

[0100] like Figure 3The diagram shows a single-master-node cooperative localization method with inertial trajectory timing assistance in step S2. Represents T a Distance information at any given time, Represents T b The figure shows the ranging information at each moment. As can be seen from the figure, although the navigation and positioning errors of INSs accumulate over time, their ability to maintain attitude and position accuracy in the short term is strong. Therefore, the time-series information of the inertial trajectory can be used as a constraint to eliminate most erroneous solutions and reduce the risk of incorrect positioning of slave nodes.

[0101] S2. Use elevation information to correct elevation errors, so as to simplify the three-dimensional optimization problem of the nodes into a two-dimensional optimization problem; at the same time, introduce relative spatial relationship constraints to ensure that the optimization problem has a unique correct solution.

[0102] As shown in the expression obtained from step S1, to achieve optimal estimation of the slave node's position in three-dimensional space, it is usually necessary to simultaneously solve an attitude transformation problem involving three-degree-of-freedom rotation and three-degree-of-freedom translation. This inevitably leads to excessive computational load, which is not conducive to the real-time deployment of the algorithm on resource-constrained platforms. However, since the slave node can use the measurement of its altimeter to compensate for its own elevation error, the original three-dimensional optimization problem can be effectively simplified into a two-dimensional solution task. This allows the estimation process of cooperative localization to only handle a combination of one-dimensional rotation and two-dimensional translation, thereby significantly reducing the problem dimensionality and algorithm complexity and improving the solution efficiency.

[0103] The specific implementation steps of step S2 are described below.

[0104] S201. Based on the above theory, with the assistance of high-level information, the three-dimensional problem is transformed into two-dimensional optimization, so as to achieve... Figure 3 Solve for the xoy horizontal plane shown;

[0105] Based on this, the expression for the cooperative localization optimization objective function can be rewritten as:

[0106]

[0107] In the formula, This represents the observation distance of the slave node relative to the master node in the xoy plane. and Let R represent the position vectors of the master node and the slave node in the two-dimensional plane at time k, respectively; both are 2-dimensional vectors. s,xoy It is a 2×2 optimal rotation matrix, t s,xoy This represents the corresponding 2×1 order optimal translation matrix.

[0108] The aforementioned variables together form the basis for solving the cooperative localization optimization problem in two-dimensional space with the assistance of height information.

[0109] in, The expression is:

[0110]

[0111] In the formula, ρ k Δh represents the distance measurement between the slave node and the master node at time k; k Used to represent the elevation difference between the slave node and the master node at time k, specifically the difference between the altimeter measurements of the slave node and the master node.

[0112] S202. In the process of cooperative positioning, in addition to inertial trajectory and ranging information, the relative spatial relationship between slave nodes and master nodes is also an important constraint information that can be fully utilized. Especially in formation flight, the aircraft usually maintain a stable geometric formation, and their relative positions such as left and right, front and back have clear structural characteristics.

[0113] Therefore, introducing relative spatial constraints on the basis of the original optimization model helps to enhance the uniqueness and stability of the solution, thereby improving the robustness and accuracy of the overall positioning.

[0114] The spatial relative position constraint between the master node and the slave node can be formally described as follows:

[0115]

[0116] in,

[0117] In the formula, This represents the position coordinates in the x-direction output by the navigation system of the master node. This represents the position coordinates in the y-direction output by the navigation system of the master node; This represents the position coordinates in the x-direction output from the node's navigation system. This represents the position coordinates in the y-direction output from the node's navigation system; and for The elements in the table represent the position coordinates of the two horizontal axes from which the node is corrected.

[0118] After step S2, the cooperative localization optimization objective function is transformed into a two-dimensional solution function. Simultaneously, the spatial relative position constraint expressions between the master node and the slave node are determined. These two expressions are then combined and used to optimize the rotation matrix R. s,xoy and the optimal translation matrix t s,xoy In the process of solving.

[0119] S3. Combine the cooperative positioning optimization objective function with the spatial relative position constraints between the master node and the slave node, and use the SVD method to solve for the optimal rotation matrix and the optimal translation matrix.

[0120] The specific implementation steps of step S3 are described below.

[0121] S301. Construct the data matrix.

[0122] The coordinate set of the master node is represented as:

[0123]

[0124] The coordinate set of the node will be represented as:

[0125]

[0126] In the formula, N is the number of valid sampling points within the time window, N = T b -T a +1.

[0127] S302. Using the SVD method, calculate the initial solution of the optimal rotation matrix. and the initial solution of the optimal translation matrix The specific steps are as follows:

[0128] (1) The expressions for the geometric centers of the master node and slave node coordinates are:

[0129]

[0130] In the formula, The mean vector representing the coordinates of the master node. This represents the mean vector of the node coordinates.

[0131] (2) Construct the covariance matrix H, whose expression is:

[0132]

[0133] In the formula, H is the covariance matrix of the centered coordinates. To center the vector from the node coordinates, The transpose of the centralized vector of the master node coordinates;

[0134] (3) Using the SVD decomposition formula: [U,Σ,V]=SVD(H), the initial solution of the optimal rotation matrix is ​​calculated. and the initial solution of the optimal translation matrix in, It is the initial rotation matrix, which is the optimal orthogonal transformation matrix obtained by SVD. It is the initial translation vector, representing the centroid shift after compensation rotation;

[0135] In the SVD decomposition formula, U is the left singular vector matrix, containing the principal component directions of the covariance matrix H; Σ is the singular value matrix; V represents the right singular vector matrix, containing H. T The principal component directions; U and V are orthogonal matrices, and the physical meaning of the Σ diagonal matrix is ​​the scaling factor of the transformation.

[0136] In this step, since we only care about the rotation part in the actual node transformation, the singular values ​​U, Σ and V themselves are not used directly.

[0137] S303. Based on the initial solution obtained in step S302, the optimal rotation matrix R... s,xoy and the optimal translation matrix t s,xoy The specific steps for performing constrained nonlinear optimization are as follows:

[0138] S3031. Parameterize and rewrite the variables, including:

[0139] (1) The rotation matrix is ​​expressed as follows:

[0140] In the formula, θ represents the rotation angle of the slave node coordinate system relative to the master node coordinate system, with counterclockwise rotation being the positive direction; cosθ represents the cosine component of the rotation angle, which determines the scaling ratio in the x-axis direction and controls the projection of the coordinates in the x-direction; sinθ represents the sine component of the rotation angle, which determines the scaling ratio in the y-axis direction and controls the projection of the coordinates in the y-direction.

[0141] (2) Optimize the vector, its expression is: x=[θ,t x ,t y ] T ,

[0142] In the formula, t x t represents the translation in the x-direction, used to compensate for the difference in horizontal displacement between the two coordinate systems; y The translation in the y-direction is used to compensate for the difference in displacement between the two coordinate systems in the vertical direction; the translations in both directions together eliminate the drift from the origin at the node position.

[0143] S3032. Based on the distance residual and sign constraint, construct the augmented residual function, the expression of which is:

[0144]

[0145] In the formula, r dist (x) represents the set of distance residual terms, containing the distance error at all times; r sign (x) represents the set of symbolic constraint terms, which includes all spatial relation constraints;

[0146] The expression for the distance residual term at time K is:

[0147]

[0148] In the formula, This represents the projection of the three-dimensional distance measurement value onto the xoy plane; R(θ) represents the true coordinates of the master node in the plane at time k; R(θ) is the rotation operator, i.e., the aforementioned rotation matrix. The original position of the node is the coordinate value to be corrected.

[0149] The expression for the symbolic constraint term is:

[0150]

[0151] In the formula, sign is the symbol extraction function. These are the original spatial relation values, referring to the constraint matrix. The element in the i-th row and j-th column; p ij It is the transformed spatial relation value, referring to the corresponding element in the constraint matrix p.

[0152] S304. Construct the Jacobian matrix, its expression is:

[0153]

[0154] In the formula, J(x) is the Jacobian matrix, which consists of the partial derivatives of the distance residual and the sign constraint residual;

[0155] The expression for the derivative of the distance residual is:

[0156]

[0157] In the formula, r dist,k Represents the k-th distance residual. This represents the corrected coordinate difference vector between the master node and the slave node. is the coordinate of the principal node in the xoy plane at time k, and is a two-dimensional vector; It is the coordinate of the node in the xoy plane at time k, which is also a two-dimensional vector; It is the partial derivative of the rotation matrix with respect to the rotation angle θ;

[0158] The expression for the sign-constrained derivative (using a smooth approximation) is as follows:

[0159]

[0160] In the formula, r sign,ij For the sign constraint residual, it corresponds to the element in the i-th row and j-th column of the constraint matrix; These are the corrected coordinate elements, which are elements in the constraint matrix p. α is a smoothing factor used to control the steepness of the hyperbolic tangent approximation of the sign function sign(·) to avoid discontinuities.

[0161] S305. Using the LM algorithm, the optimal rotation matrix and optimal translation matrix are obtained iteratively. The specific steps are as follows:

[0162] 1) Set the iterative update equation, whose expression is:

[0163] (J T J+λI)δ=-J T r,

[0164] In the formula, λ is the damping factor, which decreases when the residual decreases (accelerating convergence) and increases when the residual decreases (enhancing robustness); I is the identity matrix, and δ is the parameter update amount, which updates the parameter vector according to the optimization step size of each iteration;

[0165] 2) Set the parameter update formula:

[0166] x (n+1) =x (n) +δ,

[0167] In the formula, x (n) The parameter representing the nth iteration of the optimization vector is used to record the rotation angle and translation during the iteration process, and can be specifically written as: x (n) =[θ (n) ,t x (n) ,t y (n) ] T ;

[0168] 3) Set the adaptive adjustment formula for the damping factor:

[0169]

[0170] In the formula, r (n) The residual of the nth iteration is expressed as: And it is used to evaluate the objective function value under the current parameters;

[0171] 4) Set the iteration termination condition, which is as follows:

[0172] The iteration terminates when any of the following conditions are met:

[0173] In the formula, τ1=10 -6 The threshold for residual variation is τ2 = 10. -5 N represents the parameter update amount. max =100 is the threshold for the maximum number of iterations;

[0174] Therefore, the optimal transformation parameters are:

[0175] In the formula, θ * , This is a reference value for convergence.

[0176] To verify the method proposed in this application for cooperative navigation algorithms, a flight formation process of aircraft was simulated, and five sets of simulation data from INSs and ranging sensors were generated. The proposed method was used to process the data. As a comparison, the simulation experiment also introduced slave node position information output by pure inertial navigation and corrected slave node position information obtained by the cooperative navigation method with standard SVD registration.

[0177] The simulation parameters used in this simulation test are shown in Table 1 below.

[0178] Table 1:

[0179] Parameter categories Parameter value illustrate Scene settings <![CDATA[100×100m 2 Two-dimensional plane Simulated Cooperative Positioning Area Node configuration 1 master node + 5 slave nodes Formation flight mode movement trajectory Spiral + Broken Line Composite Trajectory Includes sharp turns, acceleration and deceleration maneuvers Sampling parameters T = 60s, f = 10Hz, N = 600 Total duration: 60 seconds, 10Hz sampling Noise settings <![CDATA[σ p =0.5m,s ρ =0.3m,s h =0.2m]]> Standard deviation of noise at location / distance / height

[0180] The specific test results after the above simulation test are shown in Table 2 below.

[0181] Table 2:

[0182] Evaluation indicators Inertial navigation output results Standard SVD registration method This application method Improvement range Rotation error (°) 1.82±0.43 0.71±0.24 0.19±0.08 73.2％↓ Translation error (m) 1.58±0.39 0.68±0.19 0.13±0.06 80.9％↓ Trajectory Consistency 0.892 0.937 0.982 +4.8％↑ Maximum positional deviation (m) 2.41 1.05 0.28 73.3％↓ Single-frame calculation latency (ms) 0.05±0.01 1.2±0.3 5.8±1.2 - Cumulative error / m 4.32 1.87 0.39 79.1％↓

[0183] As shown in Table 2, the test results indicate that the slave node position correction results obtained using the standard SVD registration method are improved compared to the direct output results of inertial navigation. However, the slave node position correction results obtained by the cooperative navigation method in this application further reduce the rotation error and translation error by 73.2% and 80.9% respectively compared to the standard SVD method, improving the consistency between the slave node position correction results and the actual position trajectory by 4.8%. In the five sets of simulation tests, although the single-frame calculation delay increased compared to the solution results of the standard SVD method, the maximum position deviation of the five slave node position correction results was effectively reduced by 73.3%, and the cumulative error was reduced by 79.1%, showing a significant improvement effect.

[0184] In summary, under GNSS denial conditions—that is, when only one master node in an unmanned formation has a reliable and stable navigation information source—this invention utilizes communication between the master and slave nodes to achieve high-precision positioning estimation of the slave nodes by combining the trajectory characteristics of the master node with the relative spatial position constraints between the master and slave nodes. This method significantly improves the robustness and persistence of slave node positioning in GNSS denied areas. Especially in scenarios facing complex terrain, multipath effects, and dynamic topology changes, the method of this invention is expected to exhibit good scalability and stability.

Claims

1. A single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information, characterized by the following steps: include: S1. Take the unmanned vehicle equipped with high-precision navigation devices as the master node and the unmanned vehicle equipped with low-precision navigation devices as the slave node, so as to use the time series of the inertial trajectory of the master node to constrain the positioning of the slave node; construct a cooperative positioning optimization objective function with the goal of minimizing the error between the estimated position and the observed distance. S2. Using elevation information to correct elevation errors, the three-dimensional optimization problem of the slave node is simplified into a two-dimensional optimization problem. The solution objectives are determined to be the optimal rotation matrix and the optimal translation matrix, and spatial relative position constraints between the master node and the slave node are introduced. S3. Combine the cooperative positioning optimization objective function with the spatial relative position constraints between the master node and the slave node, and use the SVD method to solve for the optimal rotation matrix and the optimal translation matrix.

2. The single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information according to claim 1, characterized in that, The specific implementation steps of step S1 are as follows: S101, Continuously collect data from the master node and slave node. a ~T b The inertial navigation output position information for a time period, and the ranging observation values ​​between the slave node and the master node at each inertial navigation output time obtained by the ranging module of the slave node; S102, Based on master node self-T a ~T b The inertial navigation system outputs position information over a time period, and a reference trajectory sequence consisting of N consecutive master node position information is constructed, the expression of which is: In the formula, T represents a The position information output by the navigation system of the master node at any given time, [T a ,T b [] indicates a time period; S103, Based on slave node self-T a ~T b The inertial navigation output position information for the time period, as well as the ranging observations between the slave and master nodes, are used to construct the inertial trajectory sequence to be optimized. With the corresponding measurement sequence ρ: in, T represents i Time-based node location information, ρ represents T i Distance observations from nodes at all times; S104. The expression for the constructed cooperative localization optimization objective function is as follows: In the formula, ρ represents the distance measurement observation between the slave node and the master node. The spatial coordinate vector of the master node. r represents the location information of the slave node to be obtained through the cooperative localization algorithm. Where a1, a2, and a3 represent the coordinates of the master node in three-dimensional space, and b1, b2, and b3 represent the coordinates to be estimated for the slave nodes. and These are mutually orthogonal orientation bases in space; S105. Incorporate the constraint information from steps S102 and S103 into the cooperative localization optimization objective function constructed based on step S104, to obtain: In the formula, k is T a ~T b At any point within the time period, Let r be the position information output by the navigation system of the master node at time k. k Let ρ be the location information of the slave node to be obtained by the cooperative localization algorithm at time k. k Let be the distance measurement observation between the slave node and the master node at time k; S106, r can be regarded as The approximate result after rotation and translation leads to the following cooperative localization optimization objective function: In the formula, R s Let t represent a 3×3 rotation matrix. s It is a 3×1 translation matrix; R i,j and t i,j R respectively s and t s The element in the i-th row and j-th column.

3. The single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information according to claim 1, characterized in that, In step S2, the objective function for cooperative localization optimization, obtained by simplifying the three-dimensional optimization problem of the nodes into a two-dimensional optimization problem, is expressed as follows: In the formula, This represents the observation distance of the slave node relative to the master node in the xoy plane. and Let R represent the position vectors of the master node and the slave node in the two-dimensional plane at time k; s,xoy It is a 2×2 optimal rotation matrix, t s,xoy This represents the corresponding optimal translation matrix of order 2×1; in, The expression is: In the formula, ρ k Δh represents the distance measurement between the slave node and the master node at time k; k Used to represent the elevation difference between the slave node and the master node at time k.

4. The single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematic information according to claim 1, characterized in that, In step S2, the spatial relative position constraints between the master node and the slave node are as follows: In the formula, and These are mutually orthogonal orientation bases in space; This represents the position coordinates in the x-direction output by the navigation system of the master node. This represents the position coordinates in the y-direction output by the navigation system of the master node; This represents the position coordinates in the x-direction output from the node's navigation system. This represents the position coordinates in the y-direction output from the node's navigation system; and for The elements in the table represent the position coordinates of the two horizontal axes from which the node is corrected.

5. The single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information according to claim 1, characterized in that, The specific implementation steps of step S3 are as follows: S301. Construct a data matrix to represent the coordinate set of the master node as follows: The coordinate set of the node will be represented as: In the formula, N is the number of valid sampling points within the time window, N = T b -T a +1; S302. Using the SVD method, calculate the initial solutions for the optimal rotation matrix and the optimal translation matrix, including: (1) Represent the geometric center of the master node and slave node coordinates as: In the formula, The mean vector representing the coordinates of the master node. This represents the mean vector of the node coordinates; (2) Construct the covariance matrix H: In the formula, H is the covariance matrix of the centered coordinates. To center the vector from the node coordinates, The transpose of the centralized vector of the master node coordinates; (3) Using the SVD decomposition formula: [U,Σ,V]=SVD(H), the initial solution of the optimal rotation matrix is ​​calculated. and the initial solution of the optimal translation matrix in, S303. Based on the initial solution obtained in step S302, the optimal rotation matrix R... s,xoy and the optimal translation matrix t s,xoy Performing constrained nonlinear optimization includes: (1) The rotation matrix is ​​parameterized and rewritten as: In the formula, θ represents the rotation angle of the slave node coordinate system relative to the master node coordinate system; the optimized vector parameterization is rewritten as: x = [θ, t x ,t y ] T In the formula, t x Let t be the translation in the x-direction. y This represents the translation in the y-direction; (2) Based on the distance residual and the sign constraint, construct the augmented residual function, the expression of which is: In the formula, r dist (x) represents the set of distance residual terms, containing the distance error at all times; r sign (x) represents the set of symbolic constraint terms, which includes all spatial relation constraints; The expression for the distance residual term at time K is: In the formula, This represents the projection of the three-dimensional distance measurement value onto the xoy plane; Let R(θ) represent the true coordinates of the master node in the plane at time k; R(θ) is the rotation operator, specifically substituted into the rotation matrix. The original position of the node is the coordinate value to be corrected. The expression for the symbolic constraint term is: In the formula, sign is the symbol extraction function. It refers to the original spatial relation value, specifically the constraint matrix. The element in the i-th row and j-th column; p ij It is the transformed spatial relation value, referring to the corresponding element in the constraint matrix p; S304. Construct the Jacobian matrix, its expression is: The expression for the derivative of the distance residual is: In the formula, r dist,k Represents the k-th distance residual. This represents the corrected coordinate difference vector between the master node and the slave node. These are the coordinates of the master node in the xoy plane at time k. It is the coordinate of the node in the xoy plane at time k; It is the partial derivative of the rotation matrix with respect to the rotation angle θ; The expression for the sign-constrained derivative is: α = 10 3 In the formula, r sign,ij For the sign constraint residual, it corresponds to the element in the i-th row and j-th column of the constraint matrix; These are the corrected coordinate elements, which are elements in the constraint matrix p, and α is the smoothing factor; S305. Using the LM algorithm, the optimal rotation matrix and the optimal translation matrix are obtained by iterative solution.

6. The single-master-node cooperative navigation algorithm constrained by spatiotemporal kinematics information according to claim 5, characterized in that, The specific implementation steps of step S305 are as follows: 1) Set the iterative update equation, whose expression is: (J T J+λI)δ=-J T r, In the formula, λ is the damping factor, I is the identity matrix, and δ is the parameter update amount; 2) Set the parameter update formula: x (n+1) =x (n) +δ, In the formula, x (n) x represents the parameter of the nth iteration of the optimization vector. (n) =[θ (n) ,t x (n) ,t y (n) ] T ; 3) Set the adaptive adjustment formula for the damping factor: In the formula, r (n) The residual of the nth iteration is expressed as: And it is used to evaluate the objective function value under the current parameters; 4) Set the iteration termination condition, which is as follows: The iteration terminates when any of the following conditions are met: In the formula, τ1=10 -6 The threshold for residual variation is τ2 = 10. -5 N represents the parameter update amount. max =100 is the threshold for the maximum number of iterations; Therefore, the optimal transformation parameters are: In the formula, θ * , This is a reference value for convergence.

Images