Efficient collaborative navigation method based on contour matching parameter optimization

Abstract

The invention discloses an efficient collaborative navigation method based on contour matching parameter optimization, which comprises the following steps of: S1, constraining positioning of a follower by using a time sequence of an inertial trajectory of a leader, and constructing a collaborative positioning optimization objective function; s2, correcting an elevation error by utilizing elevation information, determining a solving target as an optimal rotation matrix and an optimal translation matrix, and introducing a spatial relative position constraint between a leader and a follower; s3, determining core elements of an optimal rotation matrix and an optimal translation matrix, discretizing the position space of the unmanned aerial vehicle into a translation grid, and defining a rotation search range; s4, constructing an objective function used for solving the optimal rotation matrix and the optimal translation matrix, designing a fixed step length iterative search method, performing traversal search to obtain the optimal rotation matrix and the optimal translation matrix, and performing calculation to obtain a correction position of the follower; according to the method, the anti-interference capability can be improved, high-precision positioning of the follower is realized, and meanwhile, the dimensionality and complexity of a resolving process space are reduced.

Description

Technical Field

[0001] This invention relates to the field of cooperative navigation technology, and in particular to an efficient cooperative navigation method based on contour matching parameter optimization. Background Technology

[0002] Currently, the collaborative combat and information gathering capabilities based on UAV swarms are receiving increasing attention. However, in typical satellite signal denial environments, traditional positioning methods relying on the Global Positioning System (GPS) face serious challenges. To ensure that multiple UAV systems maintain high-precision navigation and collaborative capabilities without external positioning signal assistance, researching how to achieve highly robust collaborative positioning through network communication, inertial measurement, and relative observation information has become one of the key directions for the development of autonomous capabilities in unmanned systems.

[0003] In current cooperative navigation technologies, common implementation paths include methods based on extended Kalman filtering, factor graph optimization, and particle filtering. However, these methods generally rely on model linearization assumptions or are sensitive to sensor errors, and their high algorithm complexity makes them difficult to deploy on UAV platforms with limited computing resources. Especially when the localization solution has multiple extreme points and the objective function is complex, traditional extreme value solving methods relying on partial derivatives or gradient descent suffer from practical problems such as getting trapped in local optima, poor convergence, and difficulty in implementation. Therefore, developing a stable, efficient, and easily implementable solution method for complex objective functions is of significant theoretical and practical value for improving the autonomous localization capability of UAV swarms in denied environments. Summary of the Invention

[0004] The purpose of this invention is to provide an efficient collaborative navigation method based on contour matching parameter optimization to solve the above-mentioned technical problems.

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

[0006] An efficient collaborative navigation method based on contour matching parameter optimization, comprising the following steps:

[0007] S1. Take the unmanned vehicle equipped with high-precision navigation devices as the leader and the unmanned vehicle equipped with low-precision navigation devices as the follower, and use the time series of the leader's inertial trajectory to constrain the positioning of the follower; 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 follower is simplified into a two-dimensional optimization problem. The objective is to determine the optimal rotation matrix and the optimal translation matrix, and spatial relative position constraints between the leader and the follower are introduced.

[0009] S3. Determine the core elements of the optimal rotation matrix and the optimal translation matrix, and based on this, discretize the UAV position space into a translation grid, and define the rotation search range for solving the core elements;

[0010] S4. Construct an objective function to solve for the optimal rotation matrix and the optimal translation matrix, and design a fixed step-size iterative search method to obtain the optimal rotation matrix and the optimal translation matrix through traversal search, and calculate the corrected position of the follower.

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

[0012] S101, Continuously collect data from leaders and followers. a ~T b The inertial navigation output position information for a time period, and the ranging observation values ​​between the follower and the leader at each inertial navigation output moment obtained through the follower's ranging module;

[0013] S102, Based on Leader 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 leader position information is constructed, the expression of which is:

[0014]

[0015] In the formula, T represents a The location information output by the leader's navigation system at all times, [T a ,T b [] indicates a time period;

[0016] S103, Based on follower self-T a ~T b The inertial navigation output position information over a time period, along with the ranging observations between the follower and the leader, were used to construct the inertial trajectory sequence to be optimized. With the corresponding measurement sequence ρ:

[0017]

[0018] in, T represents i The position information of the follower at any given time, ρ represents T. i Distance measurements taken by the constant follower;

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

[0020]

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

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

[0023]

[0024] 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 leader's navigation system at time k. k Let ρ be the position information of the follower to be obtained by the cooperative localization algorithm at time k. k Let k be the distance measurement observation between the follower and the leader at time k;

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

[0026]

[0027] 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.

[0028] Furthermore, in step S2, the three-dimensional optimization problem of the follower is simplified to a two-dimensional optimization problem, resulting in the following expression for the cooperative localization optimization objective function:

[0029]

[0030] In the formula, This represents the observation distance of the follower relative to the leader in the xoy plane. and Let R represent the position vectors of the leader and follower 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;

[0031] in, The expression is:

[0032]

[0033] In the formula, ρ k Δh represents the distance observations of the follower and leader at time k; k Used to represent the elevation difference between the follower and the leader at time k.

[0034] Furthermore, in step S2, the spatial relative positional constraint between the leader and the follower is as follows:

[0035]

[0036] In the formula, and These are mutually orthogonal orientation bases in space; This represents the leader's position coordinates in the x-direction, output by the navigation system. This represents the position coordinates in the y-direction output by the leader's navigation system; This represents the position coordinates in the x-direction output by the navigation system of the follower. This represents the position coordinates in the y-direction output by the follower's navigation system; and for The elements in the table represent the position coordinates of the follower on the two horizontal axes, respectively.

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

[0038] S301. Determine the optimal rotation matrix R s,xoy The core element is the optimal rotation angle Optimal translation matrix t s,xoy The core element is the translation distance along the x-axis and y-axis, i.e., Δl. x and Δl y

[0039] S302, Setting T a Using the positioning point of the constant follower as the reference point, and centering on this reference point, the area near this point is divided into grids at equal intervals along the x-axis and y-axis, respectively, forming a two-dimensional grid search region of (nδx, mδy); then, using T... aThe heading output by the inertial navigation system is always followed as the reference axis. A fan-shaped region [-0.5Φ, 0.5Φ] with an angle of Φ is set as the rotation search range, and the angular resolution is defined as Δφ. Here, δx and δy represent the displacement of the inertial trajectory in the x-axis and y-axis directions during spatial translation, respectively, and Δφ is the rotation step size of the inertial trajectory. nδx represents the x-axis search range, and mδy represents the y-axis search range.

[0040] 6. The efficient collaborative navigation method based on contour matching parameter optimization according to claim 1, characterized in that the specific implementation steps of step S4 are as follows:

[0041] S401. Construct the optimal rotation matrix R for decoupling. s,xoy and the optimal translation matrix t s,xoy The objective function F is expressed as follows:

[0042]

[0043] S402, designed to implement R s,xoy and t s,xoy The fixed-step iterative search method for the optimal estimate is to discretize the continuous parameter space into a finite set and traverse it point by point with a fixed step size to achieve an approximate search for the optimal solution.

[0044] S403. Based on the fixed-step iterative search method designed in step S402, the objective function of step S401 is rewritten as follows:

[0045]

[0046] In the formula, F(α,β,γ), α represents the number of rotation operations, and β and γ represent the number of translations along the x-axis and y-axis, respectively. Therefore, F is a three-dimensional array to be processed, i.e., a matrix of μ3×μ2×μ1, and its expression is:

[0047]

[0048] R s,xoy The expression is: in,

[0049] The expression is:

[0050] S404. Using a fixed-step iterative search method, solve for the objective function in step S403 to obtain R. s,xoy and t s,xoy According to the formula: A = min(F), determine the R values ​​for each group in turn. s,xoy and t s,xoyIs it the optimal solution? When A reaches the global minimum, the corresponding solution R is obtained. s,xoy and R s,xoy That is, the optimal solution under the current conditions; at this time, α = α * ,β=β * and γ=γ * It is called the optimal factor;

[0051] S405. Substitute the optimal optimization factor obtained from step S404 into the spatial relative position constraints between the leader and followers constructed in step S2, and then calculate the corrected position of the followers:

[0052]

[0053] In the formula, This represents the corrected and optimized 3D spatial coordinate calibration value of the follower; The altitude information of the follower is obtained in real time by the altimeter.

[0054] Furthermore, this efficient collaborative navigation method based on contour matching parameter optimization also includes step S5: using a Savitzky-Golay filter to perform polynomial fitting on the historical positioning trajectory of the follower to obtain the noise-reduced corrected position of the follower.

[0055] Furthermore, the specific implementation steps of step S5 are as follows:

[0056] S501. Construct a noise reduction model based on the Savitzky-Golay filter. The specific steps are as follows:

[0057] 1) Establish a residual function that minimizes the cooperative localization history information of the followers, the expression of which is:

[0058]

[0059] In the formula, κ=[κ0,κ1,…,κ n ] T , The weight coefficients of the j-th polynomial in the polynomial fitting function of the Savitzky-Golay filter are represented by a 3×1 matrix; the length of the sliding window is 2m+1, and n represents the order of the polynomial.

[0060] 2) Perform a plot of the function established in step 1) with respect to κ. j The partial differential of , and setting the derivative to 0, satisfies:

[0061]

[0062] In the formula, The value range is k∈{0,1,2,…,n}, and i is the index variable for summation;

[0063] 3) The partial differential expression obtained in step 2) is represented in matrix form, specifically as follows:

[0064]

[0065] In the formula, the expression for matrix C is: C T =[t 0 ;t 1 ;…;t n ], t=[-m,…,-1,0,1,…,m];

[0066] 4) Based on step 3), the estimated value expression for the weight coefficient matrix κ is obtained as follows:

[0067]

[0068] In the formula, κ is constrained by both the smoothing window 2m+1 and the order n of the polynomial;

[0069] S502. The sequence of relative distance observations within the time window [-m, m] obtained from step S4. The input is fed into the filter constructed in step S501 to obtain the output result, the expression of which is:

[0070]

[0071] In the formula, the time-series index Its value range is [-m, m].

[0072] Compared with existing technologies, this efficient cooperative navigation method based on contour matching parameter optimization is an efficient solution method for cooperative positioning of unmanned vehicles in formations under conditions of satellite signal interference or denial. On the one hand, it fully utilizes the spatiotemporal continuity of unmanned vehicle motion to extract the time-series information contained in the inertial navigation trajectory, and combines this with the relative spatial position constraints between followers and the leader in the formation to construct a cooperative positioning optimization model with physical consistency and solution feasibility. This effectively eliminates erroneous solutions caused by inertial navigation drift or measurement errors, thus improving system performance. The system enhances its anti-interference capabilities to achieve high-precision positioning estimation of followers while maintaining a lightweight system design. Furthermore, it innovatively introduces an ergonomic search strategy to replace the partial differential method, eliminating reliance on derivative information and ensuring global reachability to the objective function's extrema, thus improving the algorithm's stability and accuracy. Simultaneously, it combines flight space discretization modeling with a noise reduction mechanism targeting high-frequency noise and system drift in inertial navigation systems, effectively replacing the continuous search space with controllable accuracy. This reduces the dimensionality and complexity of the solution space and effectively mitigates non-physical data jumps, significantly enhancing the usability and robustness of the results. Attached Figure Description

[0073] Figure 1 The flowchart shows the efficient collaborative navigation method based on contour matching parameter optimization according to the present invention.

[0074] Figure 2 This is a schematic diagram illustrating the error problem between the leader and follower in cooperative navigation localization in an efficient cooperative navigation method based on contour matching parameter optimization according to an embodiment of the present invention.

[0075] Figure 3 This is a schematic diagram of single-leader cooperative localization with inertial trajectory timing assistance in step S2 of the efficient cooperative navigation method based on contour matching parameter optimization in an embodiment of the present invention.

[0076] Figure 4 This is a schematic diagram of the core elements of the optimal rotation matrix and optimal translation vector in step S3 of the efficient cooperative navigation method based on contour matching parameter optimization in this embodiment of the invention.

[0077] Figure 5 This is a schematic diagram of the translational mesh rotation sector region in step S3 of the efficient collaborative navigation method based on contour matching parameter optimization in an embodiment of the present invention.

[0078] Figure 6 This is a schematic diagram of the cooperative localization calculation process in step S4 of the efficient cooperative navigation method based on contour matching parameter optimization in an embodiment of the present invention.

[0079] Figure 7This is a comparison chart of the positioning errors of the method of this application, the inertial navigation method, and the cooperative navigation method without data denoising in the simulation experiment of the embodiment of the present invention. Detailed Implementation

[0080] 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.

[0081] 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 leader. 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 followers. Each follower is assigned a leader and navigation constraints are applied to achieve accurate acquisition of cooperative navigation position information for the follower UAVs.

[0082] Based on this, see Figure 1 The specific implementation steps of the efficient collaborative navigation method based on contour matching parameter optimization of the present invention are described below.

[0083] S1. Use the time series of the leader's inertial trajectory to constrain the positioning of followers, so as to eliminate incorrect solutions and reduce positioning risk.

[0084] S101, Continuously collect data from leaders and followers. a ~T b The inertial navigation output position information for a time period, and the distance measurement observation values ​​between the follower and the leader at each inertial navigation output moment obtained by the follower's ranging module; in this embodiment, through this step S101, the inertial navigation output position information of the leader and the follower 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.

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

[0086] S102, Based on Leader Self-T a ~T bThe inertial navigation system outputs position information over a time period, and a reference trajectory sequence consisting of N consecutive leader position information is constructed, the expression of which is:

[0087]

[0088] In the formula, T represents a The location information output by the leader's navigation system at all times, [T a ,T b [] 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.

[0089] S103, Based on follower self-T a ~T b The inertial navigation output position information over a time period, along with the ranging observations between the follower and the leader, were 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:

[0090]

[0091] in, T represents i The position information of the follower at any given time, ρ represents T. i The distance measurement observation of the follower at any given time, i∈[a,b],

[0092] S104. As described above, in the context of this embodiment, the problem to be solved is the minimization of follower-leader cooperative localization, with the goal of minimizing the error between the estimated position and the observed distance.

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

[0094]

[0095] In the formula, ρ represents the distance observation between the follower and the leader, which is scalar observation data; Let r be the spatial coordinate vector of the leader, and r be the position information of the followers to be obtained through the cooperative localization algorithm.

[0096] 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 follower.

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

[0098]

[0099] In the formula, a1, a2, and a3 represent the coordinates of the leader in three-dimensional space, and b1, b2, and b3 represent the coordinates of the followers to be estimated. and They are mutually orthogonal directional bases in space.

[0100] 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:

[0101]

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

[0103]

[0104] 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 leader's navigation system at time k. k Let ρ be the position information of the follower to be obtained by the cooperative localization algorithm at time k. k Let be the distance measurement observation between the follower and the leader at time k.

[0105] Based on the above construction results, after establishing the optimization objective function for collaborative positioning based on a single leader, how can we effectively solve for r in the equation? k This becomes the core issue of this section.

[0106] 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.

[0107] 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:

[0108]

[0109] In the formula, R sLet 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.

[0110] like Figure 3 The diagram shows a schematic of single-leader cooperative localization with inertial trajectory timing assistance in step S2. Represents T a Distance information at any 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 error of INSs accumulates over time, its 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 the follower being mispositioned.

[0111] S2. Use elevation information to correct elevation errors, thereby simplifying the three-dimensional optimization problem of the follower 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.

[0112] As shown in the expression obtained from step S1, to achieve optimal estimation of the follower's position in three-dimensional space, it is usually necessary to simultaneously solve an attitude transformation problem involving three degrees of freedom rotation and three degrees 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 follower can use 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 cooperative localization estimation process to handle only a combination of one-dimensional rotation and two-dimensional translation, thereby significantly reducing the problem dimensionality and algorithm complexity and improving the solution efficiency.

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

[0114] 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;

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

[0116]

[0117] In the formula, This represents the observation distance of the follower relative to the leader in the xoy plane. and Let R represent the position vectors of the leader and follower 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.

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

[0119] in, The expression is:

[0120]

[0121] In the formula, ρ k Δh represents the distance observations of the follower and leader at time k; k This is used to represent the elevation difference between the follower and the leader at time k, specifically the difference between the altimeter measurements of the follower and the leader.

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

[0123] 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.

[0124] In this regard, the spatial relative positional constraint between the leader and followers can be formally described as:

[0125]

[0126] in,

[0127] In the formula, This represents the leader's position coordinates in the x-direction, output by the navigation system. This represents the position coordinates in the y-direction output by the leader's navigation system; This represents the position coordinates in the x-direction output by the navigation system of the follower. This represents the position coordinates in the y-direction output by the follower's navigation system; and for The elements in the table represent the position coordinates of the follower on the two horizontal axes, respectively.

[0128] After step S2, the cooperative localization optimization objective function is transformed into a two-dimensional solution function. Simultaneously, the spatial relative positional constraint expression between the leader and followers is determined. These two expressions are then used to determine the optimal rotation matrix R. s,xoy and the optimal translation matrix t s,xoy In the process of solving.

[0129] S3. Discretize the UAV's position space into a translation grid and a rotating sector region.

[0130] In this step, to reduce the dimensionality and complexity of the solution space, the translation parameter space and rotation parameter space are first discretized into a two-dimensional translation grid and a one-dimensional angle sector, respectively, to effectively replace the continuous search space with controllable accuracy. Then, by employing a gridded + sector discretization strategy, the possible spatial poses of the UAV are discretized into a set of controllable parameters. The search space is defined using translation grids and angle segments, respectively, thereby achieving accurate estimation of the optimal translation parameters and translation angles through enumeration. In summary, this discretization method not only reduces the spatial dimensionality and solution complexity, ensuring a balance between controllable computational accuracy and efficiency, but also guarantees global search capability, avoiding local optima traps, while maintaining a clear structure and ease of engineering implementation.

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

[0132] S301. Determine the core elements to be solved;

[0133] To obtain the optimal rotation matrix R of the key parameter s,xoy and the optimal translation matrix t s,xoy First, define the core elements of both; such as Figure 4 As shown, the optimal rotation matrix R s,xoy Its core function is to rotate the follower's trajectory to a position that better matches the leader's trajectory; therefore, its key element is the optimal rotation angle. It determines the direction and magnitude of the entire rotation transformation; while the optimal translation matrix t s,xoy Its core function is to translate and align the rotated trajectory; therefore, its core element is the translation distance along the x-axis and y-axis, i.e., Δl. x and Δl y Together, they determine the final alignment of the follower's trajectory relative to the leader's trajectory.

[0134] S302. Discretize the UAV's position space into a translational grid and define a rotating sector region as the rotational search range;

[0135] like Figure 5 As shown, in the UAV swarm cooperative positioning system, T is set aUsing the positioning point of the constant follower as the reference point, and centering on this reference point, the area near this point is divided into grids at equal intervals along the x-axis and y-axis, respectively, forming a two-dimensional grid search region of (nδx, mδy); then, using T... a The heading output by the inertial navigation system is always followed as the reference axis. A fan-shaped region [-0.5Φ, 0.5Φ] with an angle of Φ is set as the rotation search range, and the angular resolution is defined as Δφ. Here, δx and δy represent the displacement of the inertial trajectory in the x-axis and y-axis directions during spatial translation, respectively, and Δφ is the rotation step size of the inertial trajectory. nδx represents the x-axis search range, and mδy represents the y-axis search range.

[0136] In step S3, the optimal rotation matrix R is solved efficiently by meshing the motion space using parametric subdivisions. s,xoy and the optimal translation matrix t s,xoy The rotation matrix, applied to the trajectory data, aims to accurately calibrate the heading deviation and eliminate its systematic offset on cooperative positioning accuracy. Meanwhile, the translation vector is applied to spatially compensate for the follower's pose deviation, thereby achieving global optimization control of the three-dimensional positioning field.

[0137] S4. Construct an objective function to solve for the optimal rotation matrix and the optimal translation matrix, and design a fixed step-size iterative search method to obtain the optimal rotation matrix and the optimal translation matrix through traversal search, calculate the corrected position of the follower, and realize the position correction.

[0138] S401. To achieve optimal joint correction of spatial pose, construct an optimal rotation matrix R for decoupling. s,xoy and the optimal translation matrix t s,xoy The objective function F is expressed as follows:

[0139]

[0140] S402. Design a fixed-step iterative search method to implement R. s,xoy and t s,xoy The optimal estimate;

[0141] Specifically, the fixed-step iterative search method involves discretizing the continuous parameter space into a finite set and then traversing it point by point with a fixed step size to approximate the search for the optimal solution. This method does not depend on the differentiability or convexity of the objective function, thus maintaining robustness under complex constraints and non-smooth functions.

[0142] In practical applications, after constructing the objective function based on the problem-solving objective and determining the domain of its variables, an appropriate step size is set according to the accuracy requirements. Then, the objective function value is enumerated and calculated point by point in the parameter space, and the optimal solution is obtained through iterative updates. For multidimensional problems, a multi-stage or hierarchical strategy can be adopted: that is, a coarse search is performed first with a larger step size, and then a fine search is performed in the region to be solved with a smaller step size to further balance efficiency and accuracy. Furthermore, since the calculation of each point in the region is independent, this method is suitable for parallel computing, thus possessing good scalability in engineering implementation.

[0143] Traditional cooperative localization optimization processes typically employ differential methods for finding extrema, such as gradient descent or Newton's method. These methods have significant limitations when dealing with complex nonlinear objective functions. In contrast, the approach of this invention does not rely on derivative information, ensuring global reachability to the objective function's extrema and improving the algorithm's stability and accuracy. Furthermore, by constructing an optimization objective function that jointly considers inertial trajectory contour information and ranging constraints, a mathematical model for follower position correction is established.

[0144] S403. Based on the fixed-step iterative search method designed in step S402, the objective function of step S401 is further rewritten as follows:

[0145]

[0146] In the formula, F(α,β,γ), α represents the number of rotation operations, and β and γ represent the number of translations along the x-axis and y-axis, respectively. Therefore, F is a three-dimensional array to be processed, i.e., a matrix of μ3×μ2×μ1, and its expression is:

[0147]

[0148] R s,xoy The expression is: in,

[0149] The expression is:

[0150] S404. Using a fixed-step iterative search method, solve for the objective function in step S403 to obtain R. s,xoy and t s,xoy And according to the formula: A = min(F), sequentially determine the R values ​​obtained from each solution. s,xoy and t s,xoy Is it the optimal solution? In the formula, A is the minimum result of minimizing the objective function F, indicating that when A reaches the global minimum, the corresponding solution R is optimal. s,xoy and R s,xoyThat is, the optimal solution under the current conditions.

[0151] When A reaches the global minimum, α = α * ,β=β * and γ=γ * This is called the optimal optimization factor, and its corresponding R s,xoy and t s,xoy That is, to find the optimal rotation and optimal translation matrices.

[0152] S405. Substitute the optimal optimization factor obtained from step S404 into the spatial relative position constraint expression between the leader and followers constructed in step S202 above, and obtain:

[0153]

[0154] The corrected position of the follower can then be calculated:

[0155]

[0156] In the formula, This represents the corrected and optimized 3D spatial coordinate calibration value of the follower; The altitude information of the follower is obtained in real time by the altimeter.

[0157] like Figure 6 The diagram shows the cooperative positioning solution process in step S4.

[0158] However, due to the unavoidable ranging noise and attitude perturbations in the original observations, the corrected results obtained after step S4 still suffer from jitter and discontinuities. Therefore, directly using them for cooperative positioning will still result in insufficient trajectory accuracy. To address this, further noise reduction processing is needed on the corrected results to suppress random errors and improve the stability and smoothness of the trajectory solution.

[0159] S5. A Savitzky-Golay filter is used to perform polynomial fitting on the historical positioning trajectory of the follower. This is done to suppress non-stationary disturbances caused by inertial noise and obtain the noise-reduced corrected position of the follower, thereby further improving the smoothness and physical consistency of the positioning trajectory.

[0160] This step addresses the impact of high-frequency noise and system drift in inertial navigation systems. It employs a Savitzky-Golay filter and constructs a polynomial fitting model to process the positioning correction results obtained in step S4. The output results are then processed to smooth the trajectory using a sliding window polynomial fitting method, effectively reducing non-physical jumps and significantly enhancing the usability and robustness of the results.

[0161] The specific implementation steps of step S5 are described below.

[0162] S501. Construct a denoising model based on the Savitzky-Golay filter to denoise the corrected position of the follower. The specific steps are as follows:

[0163] 1) Establish a residual function that minimizes the cooperative localization history information of the followers, the expression of which is:

[0164]

[0165] In the formula, κ=[κ0,κ1,…,κ n ] T , The weight coefficients of the j-th polynomial in the polynomial fitting function of the Savitzky-Golay filter are represented by a 3×1 matrix; the length of the sliding window is 2m+1, and n represents the order of the polynomial.

[0166] 2) Perform a plot of the function established in step 1) with respect to κ. j The partial differential of , and setting the derivative to 0, satisfies:

[0167]

[0168] In the formula, The value range is k∈{0,1,2,…,n}, and i is the index variable for summation, which is a series of integers from -m to m, and represents a specific, discrete sampling point in the historical time window.

[0169] 3) The partial differential expression obtained in step 2) is represented in matrix form, specifically as follows:

[0170]

[0171] In the formula, the expression for matrix C is: C T =[t 0 ;t 1 ;…;t n ], t=[-m,…,-1,0,1,…,m];

[0172] 4) Based on step 3), the estimated value expression for the weight coefficient matrix κ is obtained as follows:

[0173]

[0174] Among them, κ is constrained by the smoothing window 2m+1 and the order n of the polynomial.

[0175] S502. The sequence of relative distance observations within the time window [-m, m] obtained from step S4. The input is fed into the filter constructed in step S501 to obtain the output result, the expression of which is:

[0176]

[0177] In the formula, the time-series index Its value range is [-m, m].

[0178] To verify the feasibility and rationality of the proposed method in a single-leader cooperative positioning algorithm, a flight formation of aircraft was simulated, and six 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 follower position information from simple inertial navigation output, as well as follower position information obtained from traditional cooperative navigation methods without parameter optimization and noise reduction (see Sun W, Liu J. Design of robust cubature fission particle filter algorithm in multi-source cooperative navigation[J]. Scientific reports, 2022, 12(1):4210.).

[0179] The simulation parameters and results of the cooperative localization algorithm in the single-leader mode are compared in Table 1 below.

[0180] Table 1:

[0181]

[0182] As shown in Table 1, this application uses controlled variable simulation to analyze the cooperative positioning effect of the proposed method on INSs with different accuracy levels. Specifically, the attitude errors of the simulation data in groups 1 to 5 in Table 1 are roughly set with reference to the initial self-alignment limit accuracy of INSs with different accuracy levels. Among them, in the simulation conditions of relative ranging accuracy of 3m, flight speed of 100m / s and flight time of 1160s, the cooperative positioning error increases as the accuracy level of INSs decreases.

[0183] like Figure 7The figure shows a comparison of positioning errors using the method of this application, an inertial navigation method, and a cooperative navigation method without data denoising. The error comparisons in six parts (a) to (f) of the figure are consistent with the comparison results in Table 1. It can be seen from the figure that when the gyroscope zero bias of the INSs is ≤0.1° / h and the accelerometer zero bias is ≤100μg, under the condition of a relative distance of 500m, that is, the method of this application can ensure that the navigation accuracy of the follower aircraft is better than 100m, and the positioning error does not accumulate over time. Furthermore, it can be seen from figures (a) to (c) that the error of pure inertial navigation accumulates continuously over time, and the lower the accuracy level of the INSs, the faster the error accumulates. This phenomenon causes the trajectory of the aircraft relying on pure inertial flight to continuously deviate from the actual flight path, while the proposed method can ensure that the navigation trajectory of the follower aircraft basically coincides with the actual trajectory.

[0184] In summary, the collaborative localization method based on contour matching parameter optimization proposed in this paper not only has a clear theoretical structure and flexible parameter design, but also possesses good engineering adaptability, making it suitable for UAV swarm deployment in various complex environments. It is of great significance for promoting high-precision collaborative localization of multiple unmanned systems in satellite signal denial scenarios, and also provides new solutions and technical paths for the design of navigation systems in extreme environments.

Claims

1. A highly efficient cooperative navigation method based on contour matching parameter optimization, characterized in that, The steps are as follows: S1. Take the unmanned vehicle equipped with high-precision navigation devices as the leader and the unmanned vehicle equipped with low-precision navigation devices as the follower, and use the time series of the leader's inertial trajectory to constrain the positioning of the follower; 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 follower is simplified into a two-dimensional optimization problem. The objective is to determine the optimal rotation matrix and the optimal translation matrix, and spatial relative position constraints between the leader and the follower are introduced. S3. Determine the core elements of the optimal rotation matrix and the optimal translation matrix, and based on this, discretize the UAV position space into a translation grid, and define the rotation search range for solving the core elements; S4. Construct an objective function to solve for the optimal rotation matrix and the optimal translation matrix, and design a fixed step-size iterative search method to obtain the optimal rotation matrix and the optimal translation matrix through traversal search, and calculate the corrected position of the follower.

2. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, The specific implementation steps of step S1 are as follows: S101, Continuously collect data from leaders and followers. a ~T b The inertial navigation output position information for a time period, and the ranging observation values ​​between the follower and the leader at each inertial navigation output moment obtained through the follower's ranging module; S102, Based on Leader 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 leader position information is constructed, the expression of which is: In the formula, T represents a The location information output by the leader's navigation system at all times, [T a ,T b [] indicates a time period; S103, Based on follower self-T a ~T b The inertial navigation output position information over a time period, along with the ranging observations between the follower and the leader, were used to construct the inertial trajectory sequence to be optimized. With the corresponding measurement sequence ρ: in, T represents i The position information of the follower at any given time, ρ represents T. i Distance measurements taken by the constant follower; S104. The expression for the constructed cooperative localization optimization objective function is as follows: In the formula, ρ represents the distance observation between the follower and the leader. The spatial coordinate vector of the leader. r represents the location information of the follower to be obtained through the cooperative localization algorithm. Where a1, a2, and a3 represent the coordinates of the leader in three-dimensional space, and b1, b2, and b3 represent the coordinates of the followers to be estimated. 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 leader's navigation system at time k. k Let ρ be the position information of the follower to be obtained by the cooperative localization algorithm at time k. k Let k be the distance measurement observation between the follower and the leader 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 efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, In step S2, the three-dimensional optimization problem of the follower is simplified to a two-dimensional optimization problem, and the expression of the cooperative localization optimization objective function is as follows: In the formula, This represents the observation distance of the follower relative to the leader in the xoy plane. and Let R represent the position vectors of the leader and follower 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 observations of the follower and leader at time k; k Used to represent the elevation difference between the follower and the leader at time k.

4. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, In step S2, the spatial relative positional constraints between the leader and the follower are as follows: In the formula, and These are mutually orthogonal orientation bases in space; This represents the leader's position coordinates in the x-direction, output by the navigation system. This represents the position coordinates in the y-direction output by the leader's navigation system; This represents the position coordinates in the x-direction output by the navigation system of the follower. This represents the position coordinates in the y-direction output by the follower's navigation system; and for The elements in the table represent the position coordinates of the follower on the two horizontal axes, respectively.

5. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, The specific implementation steps of step S3 are as follows: S301. Determine the optimal rotation matrix R s,xoy The core element is the optimal rotation angle Optimal translation matrix t s,xoy The core element is the translation distance along the x-axis and y-axis, i.e., Δl. x and Δl y S302, Setting T a Using the positioning point of the constant follower as the reference point, and centering on this reference point, the area near this point is divided into grids at equal intervals along the x-axis and y-axis, respectively, forming a two-dimensional grid search region of (nδx, mδy); then, using T... a The heading output by the inertial navigation system is always followed as the reference axis. A fan-shaped region [-0.5Φ, 0.5Φ] with an angle of Φ is set as the rotation search range, and the angular resolution is defined as Δφ. Here, δx and δy represent the displacement of the inertial trajectory in the x-axis and y-axis directions during spatial translation, respectively, and Δφ is the rotation step size of the inertial trajectory. nδx represents the x-axis search range, and mδy represents the y-axis search range.

6. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, The specific implementation steps of step S4 are as follows: S401. Construct the optimal rotation matrix R for decoupling. s,xoy and the optimal translation matrix t s,xoy The objective function F is expressed as follows: S402, designed to implement R s,xoy and t s,xoy The fixed-step iterative search method for the optimal estimate is to discretize the continuous parameter space into a finite set and traverse it point by point with a fixed step size to achieve an approximate search for the optimal solution. S403. Based on the fixed-step iterative search method designed in step S402, the objective function of step S401 is rewritten as follows: In the formula, F(α,β,γ), α represents the number of rotation operations, and β and γ represent the number of translations along the x-axis and y-axis, respectively. Therefore, F is a three-dimensional array to be processed, i.e., a matrix of μ3×μ2×μ1, and its expression is: R s,xoy The expression is: in, The expression is: S404. Using a fixed-step iterative search method, solve for the objective function in step S403 to obtain R. s,xoy and t s,xoy According to the formula: A = min(F), determine the R values ​​for each group in turn. s,xoy and t s,xoy Is it the optimal solution? When A reaches the global minimum, the corresponding solution R is obtained. s,xoy and R s,xoy That is, the optimal solution under the current conditions; at this time, α = α * ,β=β * and γ=γ * It is called the optimal factor; S405. Substitute the optimal optimization factor obtained from step S404 into the spatial relative position constraints between the leader and followers constructed in step S2, and then calculate the corrected position of the followers: In the formula, This represents the corrected and optimized 3D spatial coordinate calibration value of the follower; The altitude information of the follower is obtained in real time by the altimeter.

7. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 1, characterized in that, It also includes step S5: using a Savitzky-Golay filter to perform polynomial fitting on the historical positioning trajectory of the follower to obtain the noise-reduced corrected position of the follower.

8. The efficient cooperative navigation method based on contour matching parameter optimization according to claim 7, characterized in that, The specific implementation steps of step S5 are as follows: S501. Construct a noise reduction model based on the Savitzky-Golay filter. The specific steps are as follows: 1) Establish a residual function that minimizes the cooperative localization history information of the followers, the expression of which is: In the formula, κ=[κ0,κ1,…,κ n ] T , The weight coefficients of the j-th polynomial in the polynomial fitting function of the Savitzky-Golay filter are represented by a 3×1 matrix; the length of the sliding window is 2m+1, and n represents the order of the polynomial. 2) Perform a plot of the function established in step 1) with respect to κ. j The partial differential of , and setting the derivative to 0, satisfies: In the formula, The range of values ​​is k∈{0,1,2,…,n}, and i is the index variable for summation; 3) The partial differential expression obtained in step 2) is represented in matrix form, specifically as follows: In the formula, the expression for matrix C is: C T =[t 0 ; t 1 ;…;t n ],t=[-m,…,-1,0,1,…,m]; 4) Based on step 3), the estimated value expression for the weight coefficient matrix κ is obtained as follows: In the formula, κ is constrained by both the smoothing window 2m+1 and the order n of the polynomial; S502. The sequence of relative distance observations within the time window [-m, m] obtained from step S4. The input is fed into the filter constructed in step S501 to obtain the output result, the expression of which is: In the formula, the time-series index Its value range is [-m, m].

Images