Performance optimization method of collaborative navigation algorithm under communication delay interference

Abstract

The invention relates to a performance optimization method of a collaborative navigation algorithm under communication time delay interference, which belongs to the technical field of collaborative navigation, and comprises the following steps: before an aircraft cluster is launched, unified time service is carried out by using satellites, so that a leader aircraft and a follower aircraft have a unified time reference; the position information, timestamp information and the like of the leader aircraft are shared to the follower aircraft, so that the follower aircraft records own timestamp information while receiving the information shared by the leader aircraft; estimating a delay error between the follower aircraft and the leader aircraft according to a difference value of the two pieces of timestamp information; estimating the position information of the leader aircraft at the moment when the follower aircraft receives the shared information by using a dead reckoning principle according to the estimated delay error; the estimated position information is utilized to compensate for a position information lag error shared by the leader aircraft. According to the invention, the navigation precision of the collaborative navigation algorithm under communication delay interference is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of cooperative navigation, and particularly to a method for optimizing the performance of a cooperative navigation algorithm under communication delay interference. Background Art

[0002] Communication delay is a common phenomenon in the application of cooperative navigation algorithms. Whether it is a single-leader cooperative navigation algorithm or a multi-leader cooperative navigation algorithm, communication delay is one of the important factors affecting its navigation performance. Due to the existence of communication delay, when the follower aircraft receives the navigation information shared by the leader aircraft, the relative relationship between the two has already changed, that is, the position information of the leader aircraft obtained by the follower aircraft has a lag. The position information of the leader aircraft is the reference benchmark for the follower aircraft to correct its own navigation information. Therefore, the lag of cooperative navigation information will directly affect the performance of the cooperative navigation algorithm of the follower aircraft. If the communication delay is forcibly ignored, the performance of the cooperative navigation algorithm will be significantly reduced. Summary of the Invention

[0003] Aiming at the problem of the deterioration of the navigation accuracy of the cooperative navigation algorithm caused by communication delay, the purpose of the present invention is to provide a method for optimizing the performance of a cooperative navigation algorithm under communication delay interference to solve or improve the defects existing in the prior art.

[0004] To achieve the above purpose, the present invention adopts the following technical solutions: A method for optimizing the performance of a cooperative navigation algorithm under communication delay interference, comprising the following steps: S1. Before the aircraft cluster is launched, use satellite unified timekeeping to enable the leader aircraft and the follower aircraft to have a unified time reference benchmark; S2. Share the position information, speed information, acceleration information and timestamp information of the leader aircraft with the follower aircraft, so that the follower aircraft records its own timestamp information while receiving the information shared by the leader aircraft; S3. Estimate the delay error between the follower aircraft and the leader aircraft according to the difference between the timestamp information of the leader aircraft and the follower aircraft; S4. According to the estimated delay error and the position information, speed information and acceleration information shared by the leader aircraft, use the dead reckoning principle to estimate the position information of the leader aircraft at the moment when the follower aircraft receives the information shared by it; S5. Use the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, and the performance optimization of the cooperative navigation algorithm under communication delay interference can be achieved.

[0005] Preferably, in step S3, according to the difference between the timestamp information of the leader aircraft and the follower aircraft, the specific formula for estimating the delay error between the follower aircraft and the leader aircraft is: where is the delay error estimated by the follower aircraft according to the timestamp, t 2 is the moment when the follower aircraft receives the shared information, and t 1 is the moment when the leader aircraft sends the shared information.

[0006] Preferably, in step S4, according to the estimated delay error and the position information, speed information, and acceleration information shared by the leader aircraft, using the dead reckoning principle, the specific formula for estimating the position information of the leader aircraft at the moment when the follower aircraft receives its shared information is: where is the position information of the leader aircraft estimated by dead reckoning at time t 1 at time t 2 , is the 2D vector formed by the position information of the leader aircraft in the xoy plane at time t 1 , and Λ t1 are the speed and acceleration of the leader aircraft at time t 1 respectively, is the delay error estimated by the follower aircraft according to the timestamp, represents the position information lag error caused by the communication delay estimated by the estimated time delay and the speed information of the leader aircraft.

[0007] Preferably, in step S5, using the estimated position information to compensate for the position information lag error shared by the leader aircraft, the specific method for optimizing the performance of the cooperative navigation algorithm under communication delay interference includes the following steps: S511. Reconstruct the optimization objective function using the estimated position information to obtain the objective function regarding the rotation matrix and the translation matrix; S512. Use the method of fixed-step search to optimally estimate the rotation matrix and the translation matrix to compensate for the cooperative information lag error, thereby realizing the performance optimization of the single-leader cooperative navigation algorithm under communication delay interference.

[0008] Preferably, in step S511, the specific method for reconstructing the optimization objective function using the estimated position information to obtain the objective function regarding the rotation matrix and the translation matrix is: Considering the lag error introduced by the communication delay to the cooperative information, the optimization objective function is described as: wherein, is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is from t 1 The position information of the leader aircraft estimated by dead reckoning at time t 2 At time, C s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is at time t 2 The 2D vector formed by the position information of the follower aircraft in the xoy plane; Using the estimated position information, the optimization objective function is rewritten as the objective function F about the rotation matrix and the translation matrix as follows: wherein, is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is at time t 1 The 2D vector formed by the position information of the leader aircraft in the xoy plane, and are respectively the velocity and acceleration of the leader aircraft at time t 1 , is the delay error estimated by the follower aircraft according to the timestamp, C s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is at time t 2 The 2D vector formed by the position information of the follower aircraft in the xoy plane.

[0009] Preferably, in step S512, a fixed-step search method is adopted to optimally estimate the rotation matrix and the translation matrix to compensate for the collaborative information lag error, and the performance optimization of the single-pilot cooperative navigation algorithm under communication time-delay interference can be realized. The specific method is as follows: The objective function F about the rotation matrix and the translation matrix is rewritten as the objective optimization function F(κ, μ, υ) for estimating the rotation matrix and the translation matrix based on the fixed-step search method as follows: wherein, F(κ, μ, υ) is a three-dimensional array, κ is the number of rotations, and μ and υ are respectively the number of translations on the x-axis and the y-axis; is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is at time t 1The 2D vector formed by the position information of the leader aircraft in the xoy plane and Λ t1 are the velocity and acceleration of the leader aircraft at time t 1 respectively, is the delay error estimated by the follower aircraft based on the timestamp, is the rotation matrix rotated κ times, is the translation matrix translated μ times on the x-axis and υ times on the y-axis, is the 2D vector formed by the position information of the follower aircraft in the xoy plane at time t 2 ; κ that minimizes F(κ, μ, υ) within the search range * , μ * and υ * are the optimal search factors that make the optimization objective function of the single-leader cooperative navigation algorithm hold. κ * is the optimal number of rotation times for search, μ * and υ * are the optimal translation times for search on the x-axis and y-axis respectively. The corresponding and are the optimal rotation matrix and optimal translation matrix to be obtained respectively, and the performance optimization of the single-leader cooperative navigation algorithm under communication delay interference can be achieved.

[0010] Preferably, in step S4, according to the estimated delay error and the position information, velocity information, and acceleration information shared by the leader aircraft, using the dead reckoning principle, the specific formula for estimating the position information of the leader aircraft at the moment when the follower aircraft receives its shared information is: where and are the three-axis position information of the leader aircraft estimated by dead reckoning at time t 1 at time t 2 , and are the three-axis position information of the leader aircraft output by the inertial navigation system on the leader aircraft in the ECEF coordinate system at time t 1 , and are the three-axis velocities of the leader aircraft in the ECEF coordinate system at time t 1 , and are the three-axis accelerations of the leader aircraft in the ECEF coordinate system at time t 1 , is the delay error estimated by the follower aircraft based on the timestamp.

[0011] Preferably, in step S5, by using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the specific method for optimizing the performance of the cooperative navigation algorithm under communication delay interference includes the following steps: S521. Construct a pseudo-observation equation by using the position information shared by the leader aircraft at the moment when the follower aircraft receives data, the position information of the follower aircraft, and the relative ranging information between the follower aircraft and the leader aircraft; S522. Modify the pseudo-observation equation by using the estimated position information to obtain a modified observation equation; S523. Take the partial derivative of the modified observation equation with respect to the three-dimensional position error to obtain a measurement matrix; S524. Substitute the measurement matrix into the multi-leader cooperative navigation algorithm filter to compensate for the offset of the position information of the leader aircraft during the communication delay stage, thereby realizing the performance optimization of the multi-leader cooperative navigation algorithm under communication delay interference.

[0012] Preferably, in step S521, the specific method for constructing a pseudo-observation equation by using the position information shared by the leader aircraft at the moment when the follower aircraft receives data, the position information of the follower aircraft, and the relative ranging information between the follower aircraft and the leader aircraft is as follows: Theoretically, at time t 2 The true observation equation Z constructed by the relative ranging information is: Where is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time t 2 by using its own position information and the position information shared by the leader aircraft, is the distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t 2 , and are the three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t 2 , and are the three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time t 2 ; Without considering communication delay, the pseudo-observation equation constructed by the follower aircraft according to the received shared information is: Among them, is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time 1 estimated time 2 using its own position information and the position information shared by the leader aircraft, is the distance measured by the ranging sensor on the follower aircraft relative to the leader aircraft at time 2 t, and is the three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time 1 t, and is the three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time 2 t.

[0013] Preferably, in step S522, the pseudo-observation equation is corrected using the estimated position information to obtain a corrected observation equation. The specific method is as follows: The corrected observation equation is: Among them, is a non-linear coupling function, is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time 1 estimated time 2 t using its own position information and the position information shared by the leader aircraft, is the velocity of the leader aircraft at time 1 t, Λ t1 is the acceleration of the leader aircraft at time 1 t, is the delay error estimated by the follower aircraft according to the timestamp; is the distance measured by the ranging sensor on the follower aircraft relative to the leader aircraft at time 2 t; and is the three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time 1 t, and is the three-axis velocity of the leader aircraft in the ECEF coordinate system at time 1 t, and is the three-axis acceleration of the leader aircraft in the ECEF coordinate system at time 1 t, and Follow the three-axis position information of the inertial navigation system on the aircraft at time t in the ECEF coordinate system; 2 Suppose the inertial navigation system on the leader aircraft outputs velocity information in the northeast-down coordinate system. Given that the coordinate transformation matrix S from the northeast-down coordinate system to the ECEF coordinate system is: Among them, is the longitude information with error output by the inertial navigation system on the leader aircraft at time t 1 ; is the latitude information with error output by the inertial navigation system on the leader aircraft at time t 1 ; Therefore, the three-axis velocity expression of the leader aircraft in the ECEF coordinate system is: Among them, and are the three-axis velocities of the leader aircraft at time t 1 in the ECEF coordinate system, and are the three-axis velocities of the inertial navigation system on the leader aircraft at time t 1 in the northeast-down coordinate system; The three-axis acceleration expression of the leader aircraft in the ECEF coordinate system is: Among them, and are the three-axis accelerations of the leader aircraft at time t 1 in the ECEF coordinate system, and are the three-axis accelerations of the inertial navigation system on the leader aircraft at time t 1 in the northeast-down coordinate system.

[0014] Preferably, in step S523, take the partial derivative of the corrected observation equation with respect to the three-dimensional position error to obtain the measurement matrix. The specific method is: Taking the partial derivative of the corrected observation equation with respect to the three-dimensional position error, the expression after transposing the obtained measurement matrix is as follows: Among them, is the measurement matrix corresponding to the three-dimensional position error state quantity at time t, T is the transpose symbol, 2 ; is at time t 2 ​The distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at any moment using its own position information and the position information shared by the leader aircraft is the attitude transformation matrix are respectively the longitude, latitude and altitude information with errors output by the inertial navigation system on the follower aircraft at time t 2 is the radius of the prime vertical curvature of the meridian circle and the radius of the prime vertical curvature of the prime vertical circle calculated by the inertial navigation system on the follower aircraft at altitude 2 at time t are respectively the radius of the prime vertical curvature of the meridian circle and the radius of the prime vertical curvature of the prime vertical circle calculated by the inertial navigation system on the follower aircraft at time t 2 The measurement matrix is written as: where γ is the number of leader aircraft within the effective ranging range of the follower aircraft at time t 2 is the measurement matrix formed by the ρ-th leader aircraft to the Υ-th leader aircraft within the effective ranging range of the follower aircraft at time t 2 1 is the measurement matrix of the ρ-th leader aircraft within the effective ranging range of the follower aircraft at time t 2 1 is the measurement matrix of the ρ-th leader aircraft within the effective ranging range of the follower aircraft at time t 2 2 is the measurement matrix of the Υ-th leader aircraft within the effective ranging range of the follower aircraft at time t 2

[0015] Compared with the prior art, the present invention has the following beneficial effects: The present invention adopts a method combining time delay estimation and dead reckoning to compensate for the lag error of the position information shared by the leader aircraft, improves the cooperative navigation accuracy, realizes the performance optimization of the cooperative navigation algorithm under communication time delay interference, is applicable to optimizing the cooperative navigation algorithm of the follower aircraft in both single-leader mode and multi-leader mode, and is applicable to optimizing the performance of the cooperative navigation algorithm not only under fixed time delay conditions but also under non-fixed time delay conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] ​​​​​​​​​​​To more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0017] Figure 1 This is a flowchart of a method for optimizing the performance of a cooperative navigation algorithm under communication delay interference of the present invention.

[0018] Figure 2 This is a technical roadmap of a method for optimizing the performance of a cooperative navigation algorithm under communication delay interference of the present invention.

[0019] Figure 3 This is a flowchart of step S5 in Embodiment 1 of the present invention.

[0020] Figure 4 This is a flowchart of step S5 in Embodiment 2 of the present invention. Detailed implementation manners

[0021] To make the objectives, technical solutions, and advantages of the present invention clearer, the following will clearly and completely describe the technical solutions in the present invention with reference to the drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention. To make the above features and advantages of the present invention more obvious and understandable, specific embodiments are hereby given and detailed descriptions are made in conjunction with the drawings as follows.

[0022] As Figure 1 shown, the present invention provides a method for optimizing the performance of a cooperative navigation algorithm under communication delay interference, including the following steps: S1. Before the launch of the aircraft cluster, use satellite unified timekeeping to enable the leader aircraft and the follower aircraft to have a unified time reference benchmark; S2. Share the position information, speed information, acceleration information, and timestamp information of the leader aircraft with the follower aircraft, so that the follower aircraft records its own timestamp information while receiving the information shared by the leader aircraft; S3. Estimate the delay error between the follower aircraft and the leader aircraft according to the difference in the timestamp information of the leader aircraft and the follower aircraft; S4. According to the estimated delay error and the position information, speed information, and acceleration information shared by the leader aircraft, use the dead reckoning principle to estimate the position information of the leader aircraft at the moment when the follower aircraft receives the information shared by it; S5. By using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the performance optimization of the cooperative navigation algorithm under communication delay interference can be achieved.

[0023] As Figure 2 shown, the present invention is applicable to optimizing the cooperative navigation algorithm in single-leader mode and multi-leader mode. Both the single-leader mode and the multi-leader mode can compensate for the cooperative information lag error through the combination of delay estimation and dead reckoning, achieve the performance optimization of the cooperative navigation algorithm, and feedback the information of the cooperative navigation result to the follower aircraft.

[0024] Example 1: As Figure 1 and Figure 3 shown, a method for optimizing the performance of a single-leader cooperative navigation algorithm under communication delay interference includes the following steps: S1. Before the aircraft cluster launches, use satellite unified timekeeping to enable the leader aircraft and the follower aircraft to have a unified time reference benchmark; S2. Share the position information, speed information, acceleration information, and timestamp information of the leader aircraft with the follower aircraft, so that the follower aircraft records its own timestamp information while receiving the information shared by the leader aircraft; S3. According to the difference between the timestamp information of the leader aircraft and the follower aircraft, estimate the delay error between the follower aircraft and the leader aircraft; S4. According to the estimated delay error and the position information, speed information, and acceleration information shared by the leader aircraft, use the dead reckoning principle to estimate the position information of the leader aircraft at the moment when the follower aircraft receives the shared information; S5. By using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the performance optimization of the single-leader cooperative navigation algorithm under communication delay interference can be achieved.

[0025] In this embodiment, in step S3, the specific formula for estimating the delay error between the follower aircraft and the leader aircraft according to the difference between the timestamp information of the leader aircraft and the follower aircraft is: where is the delay error estimated by the follower aircraft according to the timestamp, t 2 is the moment when the follower aircraft receives the shared information, and t 1 is the moment when the leader aircraft sends the shared information.

[0026] In this embodiment, under the condition of communication delay, for the single-leader cooperative navigation algorithm, accurately estimating the offset of the leader aircraft's position during the communication delay stage by using the position information, velocity information, and acceleration information shared by the leader aircraft is the key to improving its performance. Therefore, in step S4, according to the estimated delay error and the position information, velocity information, and acceleration information shared by the leader aircraft, using the dead reckoning principle, the specific formula for estimating the position information of the leader aircraft at the moment when the follower aircraft receives its shared information is as follows: Wherein, is the position information of the leader aircraft estimated by dead reckoning at time t 1 at time t 2 , is the 2D vector formed by the position information of the leader aircraft in the xoy plane at time t 1 , and are the velocity and acceleration of the leader aircraft at time t 1 respectively, is the delay error estimated by the follower aircraft according to the timestamp, represents the position information lag error caused by the communication delay estimated by the estimated delay and the velocity information of the leader aircraft.

[0027] In this embodiment, in step S5, using the estimated position information to compensate for the position information lag error of the leader aircraft's shared information, the specific method for optimizing the performance of the single-leader cooperative navigation algorithm under communication delay interference includes the following steps: S511. Reconstruct the optimization objective function using the estimated position information to obtain the objective function regarding the rotation matrix and the translation matrix; S512. Use the method of fixed-step search to optimally estimate the rotation matrix and the translation matrix to compensate for the cooperative information lag error, and thus the performance optimization of the single-leader cooperative navigation algorithm under communication delay interference can be achieved.

[0028] In this embodiment, in step S511, the specific method for reconstructing the optimization objective function using the estimated position information to obtain the objective function regarding the rotation matrix and the translation matrix is as follows: Considering the lag error introduced by communication delay to the cooperative information, considering the lag error introduced by communication delay to the cooperative information, the optimization objective function is described as: Wherein, argmin represents the minimum value function, C i,j represents the rotation matrix of the i-th row and j-th column, Γ i,jThe translation matrix representing the i-th row and j-th column, R represents real numbers, and T a represents t 2 the lower limit of the time range, T b represents t 2 the upper limit of the time range; is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is from t 1 the position information of the leader aircraft estimated by dead reckoning at time t 2 at time t s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is for t 2 the 2D vector formed by the position information of the follower aircraft in the xoy plane at time t; Using the estimated position information, the optimization objective function is rewritten as the objective function F about the rotation matrix and the translation matrix as follows: where, is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is for t 1 the 2D vector formed by the position information of the leader aircraft in the xoy plane at time t, and Λ t1 are respectively the velocity and acceleration of the leader aircraft at time t 1 at time t, is the delay error estimated by the follower aircraft according to the timestamp, C s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is for t 2 the 2D vector formed by the position information of the follower aircraft in the xoy plane at time t.

[0029] In this embodiment, in step S512, a fixed-step search method is adopted to optimally estimate the rotation matrix and the translation matrix to compensate for the collaborative information lag error, and thus the performance optimization of the single-leader cooperative navigation algorithm under communication time-delay interference can be achieved. The specific method is as follows: The objective function F about the rotation matrix and the translation matrix is rewritten as the objective optimization function F(κ, μ, υ) for estimating the rotation matrix and the translation matrix based on the fixed-step search method as follows: where, F(κ, μ, υ) is a three-dimensional array, κ is the number of rotations, and μ and υ are respectively the number of translations on the x-axis and the y-axis; is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is t 1 The 2D vector formed by the position information of the leader aircraft in the xoy plane at time t and are the velocity and acceleration of the leader aircraft at time t 1 respectively, is the delay error estimated by the follower aircraft based on the timestamp, is the rotation matrix rotated κ times, is the translation matrix translated μ times on the x-axis and υ times on the y-axis, is t 2 The 2D vector formed by the position information of the follower aircraft in the xoy plane at time t; The above formula is a model for measuring whether the cooperative navigation result is the optimal estimate based on the minimum Euclidean distance. Therefore, κ that minimizes F(κ, μ, υ) within the search range * , μ * and υ * are the optimal search factors that make the optimization objective function of the single-leader cooperative navigation algorithm hold. κ * is the optimal number of rotations for optimal search, μ * and υ * are the optimal number of translations for optimal search on the x-axis and y-axis respectively. The corresponding and are the optimal rotation matrix and the optimal translation matrix to be obtained respectively, which can realize the performance optimization of the single-leader cooperative navigation algorithm under communication delay interference.

[0030] Embodiment 2: As Figure 1 and Figure 4 shown, a method for optimizing the performance of a multi-leader cooperative navigation algorithm under communication delay interference includes the following steps: S1. Before the aircraft cluster is launched, use satellite unified timekeeping to enable the leader aircraft and the follower aircraft to have a unified time reference benchmark; S2. Share the position information, velocity information, acceleration information, and timestamp information of the leader aircraft with the follower aircraft, so that the follower aircraft records its own timestamp information while receiving the information shared by the leader aircraft; S3. Estimate the delay error between the follower aircraft and the leader aircraft according to the difference between the timestamp information of the leader aircraft and the follower aircraft; S4. According to the estimated delay error and the position information, velocity information, and acceleration information shared by the leader aircraft, use the dead reckoning principle to estimate the position information of the leader aircraft at the moment when the follower aircraft receives the information it shared; S5. By using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the performance optimization of the multi-leader cooperative navigation algorithm under communication delay interference can be achieved.

[0031] In this embodiment, in step S3, according to the difference between the timestamp information of the leader aircraft and the follower aircraft, the specific formula for estimating the delay error between the follower aircraft and the leader aircraft is: Where, is the delay error estimated by the follower aircraft according to the timestamp, t 2 is the moment when the follower aircraft receives the shared information, t 1 is the moment when the leader aircraft sends the shared information.

[0032] In this embodiment, under the condition of communication delay, for the multi-leader cooperative navigation algorithm, how to accurately estimate the offset of the leader aircraft position during the communication delay stage by using the position information, speed information, and acceleration information shared by the leader aircraft is the key to improving its performance. Therefore, in step S4, according to the estimated delay error and the position information, speed information, and acceleration information shared by the leader aircraft, using the dead reckoning principle, the specific formula for estimating the position information of the leader aircraft at the moment when the follower aircraft receives its shared information is: Where, and are the three-axis position information of the leader aircraft estimated by dead reckoning at time t 1 at time t 2 respectively, and are the three-axis position information of the leader aircraft output by the inertial navigation system on the leader aircraft in the ECEF (Earth-Centered Earth-Fixed) coordinate system at time t 1 respectively, and are the three-axis velocities of the leader aircraft in the ECEF coordinate system at time t 1 respectively, and are the three-axis accelerations of the leader aircraft in the ECEF coordinate system at time t 1 respectively, is the delay error estimated by the follower aircraft according to the timestamp.

[0033] Preferably, in step S5, by using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the specific method for optimizing the performance of the cooperative navigation algorithm under communication delay interference includes the following steps: S521. Construct a pseudo-observation equation by using the position information shared by the leader aircraft at the moment when the follower aircraft receives data, the position information of the follower aircraft, and the relative ranging information between the follower aircraft and the leader aircraft; S522. Correct the pseudo-observation equation by using the estimated position information to obtain a corrected observation equation; S523. Take the partial derivative of the corrected observation equation with respect to the three-dimensional position error to obtain a measurement matrix; S524. Substitute the measurement matrix into the multi-leader cooperative navigation algorithm filter, thereby compensating for the offset of the position information of the leader aircraft during the communication delay stage, and the performance of the multi-leader cooperative navigation algorithm under communication delay interference can be optimized.

[0034] Preferably, in step S521, the specific method for constructing a pseudo-observation equation by using the position information shared by the leader aircraft at the moment when the follower aircraft receives data, the position information of the follower aircraft, and the relative ranging information between the follower aircraft and the leader aircraft is as follows: Theoretically, at time t 2 the true observation equation Z constructed by the relative ranging information is: where, is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time t 2 using its own position information and the position information shared by the leader aircraft, is the distance measured by the ranging sensor on the follower aircraft relative to the leader aircraft at time t 2 and, and are the three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t 2 and, and are the three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time t 2 ; Without considering communication delay, the pseudo-observation equation constructed by the follower aircraft according to the received shared information is: where, is obtained from t1 Time estimate t 2 The distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time t using its own position information and the position information shared by the leader aircraft At t 2 The distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t And At t 1 The three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t And At t 2 The three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time t From the true observation equation Z and the pseudo-observation equation It can be seen that the position information shared by the leader aircraft has a lag, resulting in the follower aircraft being unable to obtain the accurate position information of the leader aircraft at time t 2 As a result, colored noise is introduced into the observation model, and the existence of colored noise will lead to a decline in the data fusion performance of the cooperative navigation filter. Therefore, a method combining estimated time delay and dead reckoning can be used to correct the pseudo-observation equation

[0035] In this embodiment, in step S522, the pseudo-observation equation is corrected using the estimated position information to obtain a corrected observation equation. The specific method is as follows Corrected observation equation Is Where Is a non-linear coupling function Is from t 1 Time estimate t 2 The distance between the two aircraft calculated by the inertial navigation system on the follower aircraft at time t using its own position information and the position information shared by the leader aircraft Is the speed of the leader aircraft at time t 1 Time Is the acceleration of the leader aircraft at time t 1 Time Is the delay error estimated by the follower aircraft according to the timestamp At t 2 The distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t And At t 1The three-axis position information of the leader aircraft in the ECEF coordinate system output by the inertial navigation system at a certain moment, and are the three-axis velocities of the leader aircraft at time t 1 in the ECEF coordinate system, and are the three-axis accelerations of the leader aircraft at time t 1 in the ECEF coordinate system; and are the three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time t 2 ; Assume that the inertial navigation system on the leader aircraft outputs velocity information in the northeast-down coordinate system. Given that the coordinate transformation matrix S from the northeast-down coordinate system to the ECEF coordinate system is: where, is the longitude information with error output by the inertial navigation system on the leader aircraft at time t 1 , is the latitude information with error output by the inertial navigation system on the leader aircraft at time t 1 ; Therefore, the expression for the three-axis velocity of the leader aircraft in the ECEF coordinate system is: where, and are the three-axis velocities of the leader aircraft at time t 1 in the ECEF coordinate system, and are the three-axis velocities in the northeast-down coordinate system output by the inertial navigation system on the leader aircraft at time t 1 ; The expression for the three-axis acceleration of the leader aircraft in the ECEF coordinate system is: where, and are the three-axis accelerations of the leader aircraft at time t 1 in the ECEF coordinate system, and are the three-axis accelerations in the northeast-down coordinate system output by the inertial navigation system on the leader aircraft at time t 1 ;

[0036] In this embodiment, in step S523, for the corrected observation equation Perform partial differentiation of the three-dimensional position error to obtain the measurement matrix. The specific method is: The partial differentiation of the modified observation equation with respect to the three-dimensional position error gives the following expression of the transposed measurement matrix: in, t 2 The measurement matrix corresponding to the three-dimensional position error state at the moment, T is the transpose symbol, For 2 The inertial navigation system on the follower aircraft uses its own position information and the position information shared by the leader aircraft to calculate the distance between the two aircraft. is the attitude transformation matrix, are the inertial navigation systems on the follower aircraft, respectively. 2 The longitude, latitude and altitude information with errors are output at all times. For height The inertial navigation system on the follower vehicle at 2 The main curvature radius of the meridian circle and the main curvature radius of the meridian circle calculated at the moment, are the inertial navigation systems on the follower aircraft, respectively. 2 The main curvature radius of the meridian circle and the main curvature radius of the meridian circle calculated at all times; The measurement matrix is ​​written as: Where Υ is t 2 The number of leader aircraft that are within the effective range of the follower aircraft at any given time; t 2 The ρth time in the effective range of the follower aircraft 1 The measurement matrix from the leader aircraft to the γth leader aircraft, t 2 The ρth time in the effective range of the follower aircraft 1 The measurement matrix of the leader spacecraft, t 2 The ρth time in the effective range of the follower aircraft 2 The measurement matrix of the leader spacecraft, t 2 The measurement matrix of the γth leader aircraft that is within the effective range of the follower aircraft at any given moment.

[0037] The performance optimization method of a cooperative navigation algorithm under communication delay interference of the present invention is not only applicable to the performance optimization of the cooperative navigation algorithm under fixed delay conditions, but also applicable to the performance optimization of the cooperative navigation algorithm under non-fixed delay conditions.

[0038] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A performance optimization method for a cooperative navigation algorithm under communication delay interference, characterized in that: The following steps are involved: S1. Before the launch of the aircraft cluster, the leader and follower aircraft have a unified time reference benchmark by using satellite timing. S2, sharing the position information, speed information, acceleration information and timestamp information of the leader aircraft with the follower aircraft, so that the follower aircraft records its own timestamp information while receiving the information shared by the leader aircraft; S3, estimating the delay error between the follower aircraft and the leader aircraft according to the difference in timestamp information between the leader aircraft and the follower aircraft; S4, based on the estimated delay error and the position information, velocity information and acceleration information shared by the leader aircraft, using the dead reckoning principle, estimate the position information of the leader aircraft at the moment when the follower aircraft receives the shared information; S5. By using the estimated position information to compensate for the lag error of the position information shared by the leader aircraft, the performance optimization of the collaborative navigation algorithm under the interference of communication delay can be achieved.

2. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 1 is characterized in that: In step S3, the specific formula for estimating the delay error between the follower aircraft and the leader aircraft is as follows: in, is the delay error estimated by the follower aircraft based on the timestamp, t2 is the time when the follower aircraft receives the shared information, and t1 is the time when the leader aircraft sends the shared information.

3. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 1 is characterized in that: In step S4, based on the estimated delay error and the position information, velocity information and acceleration information shared by the leader aircraft, the dead reckoning principle is used to estimate the position information of the leader aircraft at the moment when the follower aircraft receives its shared information. The specific formula is: in, is the position information of the leader aircraft at time t2 estimated by dead reckoning at time t1, is the 2D vector consisting of the position information of the leader aircraft in the xoy plane at time t1, and Λ t1 are the speed and acceleration of the leader aircraft at time t1, is the delay error estimated by the follower aircraft based on the timestamp, It represents the lag error of the position information caused by the estimated delay and the communication delay estimated by the leader aircraft's velocity information.

4. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 1 is characterized in that: In step S5, the estimated position information is used to compensate for the lag error of the position information shared by the leader aircraft, and the specific method for optimizing the performance of the cooperative navigation algorithm under communication delay interference includes the following steps: S511, reconstructing the optimization objective function using the estimated position information to obtain an objective function related to the rotation matrix and the translation matrix; S512. A fixed-step search method is used to optimally estimate the rotation matrix and the translation matrix and compensate for the cooperative information lag error, thereby achieving performance optimization of the single-pilot cooperative navigation algorithm under communication delay interference.

5. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 4 is characterized in that: In step S511, the estimated position information is used to reconstruct the optimization objective function to obtain the objective function of the rotation matrix and the translation matrix. The specific method is: Taking into account the lag error introduced by communication delay to cooperative information, the optimization objective function is described as: in, is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is the position information of the leader aircraft at time t2 estimated by dead reckoning at time t1, C s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is the 2D vector consisting of the position information of the follower aircraft in the xoy plane at time t2; Using the estimated position information, the optimization objective function is rewritten as the objective function F about the rotation matrix and the translation matrix as follows: in, is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is the 2D vector consisting of the position information of the leader aircraft in the xoy plane at time t1, and Λ t1 are the speed and acceleration of the leader aircraft at time t1, is the delay error estimated by the follower aircraft based on the timestamp, C s,xoy is a 2×2 rotation matrix, Γ s,xoy is a 2×1 translation matrix, is the 2D vector consisting of the position information of the follower aircraft in the xoy plane at time t2; In step S512, a fixed step search method is used to optimally estimate the rotation matrix and the translation matrix, and to compensate for the cooperative information lag error, so as to achieve the performance optimization of the single pilot cooperative navigation algorithm under the interference of communication delay. The specific method is as follows: The objective function F about the rotation matrix and the translation matrix is ​​rewritten as the objective optimization function F(κ,μ,υ) for estimating the rotation matrix and the translation matrix based on the fixed step search method as follows: Among them, F(κ,μ,υ) is a three-dimensional array, κ is the number of rotations, μ and υ are the number of translations on the x-axis and y-axis respectively; is the distance of the follower aircraft relative to the leader aircraft in the xoy plane, is the 2D vector consisting of the position information of the leader aircraft in the xoy plane at time t1, and are the speed and acceleration of the leader aircraft at time t1, is the delay error estimated by the follower aircraft based on the timestamp, is the rotation matrix of κ rotations, is the translation matrix for μ translations on the x-axis and υ translations on the y-axis, is the 2D vector consisting of the position information of the follower aircraft in the xoy plane at time t2; The κ that minimizes F(κ,μ,υ) within the search range * , μ * and * is the best optimization factor that makes the optimization objective function of the single pilot cooperative navigation algorithm valid, κ * is the optimal number of rotations, μ * and * The optimal translation times on the x-axis and y-axis are respectively, and the optimal optimization factor corresponds to and The optimal rotation matrix and optimal translation matrix to be obtained are respectively, and the performance optimization of the single pilot cooperative navigation algorithm under communication delay interference can be achieved.

6. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 1 is characterized in that: In step S4, based on the estimated delay error and the position information, velocity information and acceleration information shared by the leader aircraft, the dead reckoning principle is used to estimate the position information of the leader aircraft at the moment when the follower aircraft receives its shared information. The specific formula is: in, and is the three-axis position information of the leader aircraft at time t2 estimated by dead reckoning at time t1, and is the three-axis position information in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t1, and is the three-axis velocity of the leader vehicle in the ECEF coordinate system at time t1, and is the three-axis acceleration of the leader vehicle in the ECEF coordinate system at time t1, is the delay error estimated by the follower aircraft based on the timestamp.

7. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 6 is characterized in that: In step S5, the estimated position information is used to compensate for the lag error of the position information shared by the leader aircraft, and the specific method for optimizing the performance of the cooperative navigation algorithm under communication delay interference includes the following steps: S521, constructing a pseudo observation equation using the position information shared by the leader aircraft at the time when the follower aircraft receives data, the position information of the follower aircraft, and the relative distance information between the follower aircraft and the leader aircraft; S522, using the estimated position information to correct the pseudo observation equation to obtain a corrected observation equation; S523, performing partial differentiation of the modified observation equation with respect to the three-dimensional position error to obtain a measurement matrix; S524, substituting the measurement matrix into the multi-pilot cooperative navigation algorithm filter to compensate for the offset of the leader aircraft position information during the communication delay stage, thereby achieving performance optimization of the multi-pilot cooperative navigation algorithm under communication delay interference.

8. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 7 is characterized in that: Step S521, constructing a pseudo observation equation using the position information shared by the leader aircraft at the time when the follower aircraft receives data, the position information of the follower aircraft, and the relative distance information between the follower aircraft and the leader aircraft. The specific method is: Theoretically, the real observation equation Z constructed relative to the ranging information at time t2 is: in, is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft using its own position information and the position information shared by the leader aircraft at time t2, is the distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t2, and is the three-axis position information in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t2, and The three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system at time t2; Without considering the communication delay, the pseudo observation equation constructed by the follower aircraft based on the received shared information is for: in, is the distance between the two aircrafts estimated at time t2 by the inertial navigation system on the follower aircraft using its own position information and the position information shared by the leader aircraft, is the distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t2, and is the three-axis position information in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t1, and It is the three-axis position information in the ECEF coordinate system output by the inertial navigation system on the follower aircraft at time t2.

9. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 8 is characterized in that: In step S522, the pseudo observation equation is corrected using the estimated position information to obtain a corrected observation equation. The specific method is: Modified observation equation for: in, is the nonlinear coupling function, is the distance between the two aircrafts estimated at time t2 by the inertial navigation system on the follower aircraft using its own position information and the position information shared by the leader aircraft, is the speed of the leader aircraft at time t1, Λ t1 is the acceleration of the leader vehicle at time t1, is the delay error estimated by the follower aircraft based on the timestamp; is the distance of the follower aircraft relative to the leader aircraft measured by the ranging sensor on the follower aircraft at time t2; and is the three-axis position information in the ECEF coordinate system output by the inertial navigation system on the leader aircraft at time t1, and is the three-axis velocity of the leader vehicle in the ECEF coordinate system at time t1, and is the three-axis acceleration of the leader vehicle in the ECEF coordinate system at time t1, and The three-axis position information of the follower aircraft in the ECEF coordinate system output by the inertial navigation system at time t2; Assuming that the inertial navigation system on the leader aircraft outputs the velocity information in the northeast sky coordinate system, it is known that the coordinate transformation matrix S from the northeast sky coordinate system to the ECEF coordinate system is: in, is the longitude information with error output by the inertial navigation system on the leader aircraft at time t1, The latitude information with error output by the inertial navigation system on the leader aircraft at time t1; Therefore, the three-axis velocity expression of the leader aircraft in the ECEF coordinate system is: in, and is the three-axis velocity of the leader vehicle in the ECEF coordinate system at time t1, and It is the three-axis velocity in the northeast celestial coordinate system output by the inertial navigation system on the leader aircraft at time t1; The three-axis acceleration expression of the leader aircraft in the ECEF coordinate system is: in, and is the three-axis acceleration of the leader vehicle in the ECEF coordinate system at time t1, and It is the three-axis acceleration in the northeast celestial coordinate system output by the inertial navigation system on the leader spacecraft at time t1.

10. The performance optimization method of the cooperative navigation algorithm under communication delay interference according to claim 9 is characterized in that: In step S523, the observation equation is modified Perform partial differentiation with respect to the three-dimensional position error to obtain the measurement matrix. The specific method is: perform partial differentiation with respect to the three-dimensional position error on the modified observation equation to obtain the measurement matrix. The expression after transposition is as follows: in, is the measurement matrix corresponding to the three-dimensional position error state at time t2, T is the transposition symbol, is the distance between the two aircraft calculated by the inertial navigation system on the follower aircraft using its own position information and the position information shared by the leader aircraft at time t2, is the attitude transformation matrix, are the longitude, latitude and altitude information with errors output by the inertial navigation system on the follower aircraft at time t2, For height The main curvature radius of the meridian circle and the main curvature radius of the meridian circle calculated by the inertial navigation system on the follower aircraft at time t2, are respectively the principal curvature radius of the meridian circle and the principal curvature radius of the meridian circle calculated by the inertial navigation system on the follower aircraft at time t2; The measurement matrix is ​​written as: Where γ is the number of leader aircraft within the effective range of the follower aircraft at time t2; is the measurement matrix formed by the ρ1th leader aircraft to the γth leader aircraft within the effective range of the follower aircraft at time t2, is the measurement matrix of the ρ1th leader aircraft within the effective range of the follower aircraft at time t2, is the measurement matrix of the ρ2th leader aircraft within the effective range of the follower aircraft at time t2, is the measurement matrix of the γth leader aircraft within the effective ranging range of the follower aircraft at time t2.

Images