Multi-machine combined positioning navigation method

Abstract

The invention belongs to the technical field of airborne positioning and navigation, and particularly relates to a multi-machine combined positioning and navigation method, which comprises the following steps of: 1, establishing a multi-machine combined positioning tight combination state equation and a measurement equation; 2, determining a multi-machine joint positioning measurement updating strategy based on a cross covariance state; and 3, evaluating the positioning calculation effect of the multi-machine combined positioning tight combination state equation and the measurement equation, and finding the optimal multi-machine combined positioning tight combination state equation and measurement equation for positioning navigation.

Description

Technical Field

[0001] The invention belongs to the technical field of airborne positioning and navigation, and in particular relates to a multi-aircraft joint positioning and navigation method. Background Art

[0002] The main UAV positioning and navigation systems include inertial navigation (INS), satellite navigation (GNSS), geomagnetic navigation, astronomical navigation, etc. With the rapid development of positioning and navigation technology, the UAV positioning and navigation methods have gradually developed from single positioning and navigation to multi-sensor distributed joint positioning and navigation. During the flight, if a single navigation method is used without using the multi-source navigation information provided by the integrated airborne navigation sensor, accurate and efficient positioning and navigation effects cannot be achieved.

[0003] When the satellite signal is good and there is no external interference, the target drone uses the high-precision inertial navigation information of the drone and the satellite navigation information of the satellite to achieve high-precision autonomous positioning of the drone. However, when the drone is in a complex airspace electromagnetic environment, the satellite navigation signal quality is poor and cannot continuously provide effective signals. Relying solely on inertial navigation information will cause the drone positioning information to be inaccurate or even divergent, and it will be impossible to complete positioning and navigation. Summary of the invention

[0004] The purpose of the present invention is to propose a multi-machine joint positioning and navigation method. Comprehensively utilize navigation sources such as inertial navigation and data link, and perform joint positioning through a multi-machine network formed by an anchor machine, a secondary anchor machine and a target UAV. After receiving the data link positioning information of other UAVs through the data link, it is tightly combined with the inertial navigation system of the local UAV to realize position correction. Through multi-source information fusion processing in a complex airspace electromagnetic environment, real-time and reliable positioning and navigation information is provided for the UAV.

[0005] The technical solution of the present invention:

[0006] A multi-machine joint positioning and navigation method comprises the following steps:

[0007] Step 1: Establish the tightly combined state equation and measurement equation for multi-machine joint positioning;

[0008] Step 2: Determine the multi-machine joint positioning measurement update strategy based on cross-covariance state decorrelation;

[0009] Step 3: Evaluate the positioning solution effect of the tightly combined state equation and measurement equation of multi-machine joint positioning, and find the optimal tightly combined state equation and measurement equation for multi-machine joint positioning for positioning and navigation.

[0010] Furthermore, in step one, the tightly combined state equation of multi-machine joint positioning adopts the northeast celestial coordinate system, and the position states of all drones with ranging relationships are augmented and modeled into the state equation, and the Kalman filter state equation is divided into two parts: a fixed state vector and an augmented state vector.

[0011] Further, the fixed state vector modeling process is as follows:

[0012] The 9 state quantities of the inertial instrument error and the 6 state quantities of the inertial navigation system positioning output parameter error are combined to obtain the 15-dimensional system state equation:

[0013]

[0014] in:

[0015]

[0016] W I =[w gx w gy w gz w rx w ry w rz w ax w ay w az ]

[0017] In the formula, G I is the error coefficient matrix, F I is the state coefficient matrix; X I is the state vector and W I is the white noise random error vector, and the subscripts E, N, and U represent the east, north, and sky directions of the geographic coordinate system; φ E ,φ N ,φ U They represent the platform error angles in the east, north and celestial directions of the inertial navigation system error state quantity; δv E ,δv N ,δv U They represent the velocity errors in the east, north and celestial directions respectively; δL, δλ, δh represent the errors in latitude, longitude and altitude respectively; ε rx ,ε ry ,ε rz Respectively represent the gyro first-order Markov drift error state quantity in the X-axis, Y-axis and Z-axis directions; Respectively represent the first-order Markov state quantity of the accelerometer in the X-axis, Y-axis and Z-axis directions; the superscript · represents the state vector of the next time state.

[0018] Further, the augmented state vector modeling process is as follows:

[0019] On the basis of fixed state vector modeling, the position information received from other drones in the network is augmented into the fixed state vector. Assuming that the data link receives information from drone i and drone j, the augmented state vector is:

[0020]

[0021] The augmented filter state vector is The augmented part of the system state coefficient matrix is ​​composed of unit matrix, and the augmented state coefficient matrix is ​​F * :

[0022]

[0023] Among them, F I is the fixed state coefficient matrix, F N is the system array corresponding to 9 basic positioning parameters, F s and F M They are:

[0024]

[0025] in, is the attitude transfer matrix, and T is the correlation time between the gyro and the accelerometer.

[0026] Through the above formula, the dimension of the augmented state equation is expanded from 15 dimensions for fixed states to 33 dimensions, 45 dimensions or even 15+3n dimensions, giving full play to the advantages of multi-data link position information and improving the measurement accuracy of the system.

[0027] Furthermore, in step 1, the measurement equation establishment process is as follows:

[0028] The navigation coordinate system is established with the position of the drone itself as the origin. The real positions of drone i and drone j in this coordinate system are

[0029] [x i y i z i ] and [x j y j z j ];

[0030] The positions calculated by inertial navigation are and The actual distance between the two drones is d ij and estimated distance They are

[0031]

[0032] Expanding according to Taylor's formula and ignoring high-order terms and small quantities, we get

[0033]

[0034] In the formula

[0035]

[0036] In order to be consistent with the variables in the state vector, the position error in the navigation system is converted to the earth coordinate system.

[0037]

[0038] Where: [A i A j ]=[-e 1 -e 0 -e 2 e 1 e 0 e 2 ]A

[0039] A=Diag[R Mj +h j (R Ni +h i )cosL i 1R Mi +h i (R Nj +h j )cosL j 1]

[0040] From the above analysis, the data link ranging measurement equation is obtained:

[0041]

[0042] In the formula is the data link measurement vector, A i and A j The measurement matrix is ​​composed of is the data link ranging noise.

[0043] Further, in step 2, the update strategy is as follows:

[0044] Taking GNSS measurement and data link ranging measurement as an example, assuming that drone i obtains GNSS information, i and j measure distance through data link, drone a does not receive measurement information, but it is irrelevant to the status of i and j. At this time, the quantities that need to be updated are as follows:

[0045] Time update:

[0046] Drone i: One-step state prediction

[0047] UAVJ: One-step prediction of state

[0048] Drone A: One-step state prediction

[0049] GNSS measurement update, drone i receives GNSS information:

[0050] Dronei:

[0051] Drone j: No

[0052] Drone a: No

[0053] Data link ranging measurement update, drones i and j receive ranging information:

[0054] Dronei:

[0055] Drone j:

[0056] Drone a: No

[0057] By replacing the covariance matrix with the minimum bound, the states between UAVs are decorrelated, and the data link reduces the amount of data communication during transmission.

[0058] Furthermore, in step 3, the positioning solution effect evaluation method is as follows:

[0059] In the satellite navigation system, assuming that the source of the positioning error is the satellite pseudorange error, and that the satellite pseudorange errors are independent of each other, the following formula is used to represent the relationship between the user position error and the pseudorange error:

[0060] σ 3d =GDOP×σ μ

[0061] Define the three-dimensional position error of UAV k as The relationship between the drone position error and the distance measurement error is expressed by the formula:

[0062]

[0063] The left term of the formula is the total characteristic of the three-dimensional position error during the collaborative positioning of the UAV formation, and the right term is the product of the collaborative geometric precision factor GDOP and the standard deviation of the distance measurement error;

[0064] In the process of multi-machine joint positioning and navigation, the measurement errors are independent and identically distributed, and the standard deviation σ d Unchanged, ignoring other error terms, the smaller the GDOP, the smaller the 3D position error The smaller it is, the better the system navigation and positioning performance is. Therefore, GDOP is used to analyze the impact of the geometric configuration of the unmanned cluster on the positioning performance. The smaller the GDOP is, the better the positioning performance of the geometric configuration is.

[0065] Assume that the distance between the i-th target UAV and its adjacent UAVs is where d i is the actual distance between the target UAV and the anchor winch, δd tu is the equivalent clock error, n range is the ranging noise, and its variance is δ 2 ;

[0066] The distance d between the target drones given by the inertial system Ii satisfy:

[0067]

[0068] Then the corresponding ranging error equation is:

[0069]

[0070] It can be considered that the position error of the anchor winch is unrelated to the data link ranging error, thus obtaining the distance difference measurement equation obtained by subtracting the estimated and measured distances:

[0071]

[0072] When the target UAV selects the information of n anchor machines to assist itself, the distance measurement equation is:

[0073]

[0074] The covariance of the ranging error in the ECEF coordinate system and the position error of the anchor winch itself have the following relationship:

[0075] cov(δd)=E[(δd)(δd) T ]

[0076] =E[(Hδp)(Hδp) T ]

[0077] =H·E[(δp)(δp) T ]·H T

[0078] =Hcov(δp)H T

[0079] Where: δp=[δxδyδzδd tu ] T , E represents expectation;

[0080] From this, the covariance of the position error of the UAV with data link is obtained as:

[0081] cov(δp)=H -1 cov(δd)(H T )-1

[0082] =(H T (cov(δd)) -1 H) -1

[0083] After upconverting to the navigation coordinate system (ENU), the covariance of the collaborative navigation error is expressed as:

[0084]

[0085] Therefore, for the target UAV, its coordinated position precision factor DOP is defined as:

[0086]

[0087] Based on the above formula, a DOP-based collaborative navigation accuracy cost function is designed, and the optimal collaborative observation matrix H is selected with the goal of minimizing DOP, thereby realizing the optimization of nodes in the collaborative navigation information and ensuring the real-time and accuracy of the collaborative navigation system.

[0088] Furthermore, the route cost is set as the sum of DOP costs, and the cost function is defined as follows:

[0089]

[0090] After the cost function is established, the point with the minimum cost in the attachment space is searched in combination with the flight characteristics of the anchor winch. Based on the point with the minimum cost, the parameters of the optimal state equation and measurement equation are deduced.

[0091] Beneficial effects of the present invention:

[0092] 1. Based on the inertial / data link combination, the multi-machine joint positioning tight combination algorithm and modeling integrate the ranging information between the local drone and other drones in the network, and the ranging information between other drones in the network, to complete the multi-machine joint positioning and navigation. By modeling the navigation system status, the conventional state quantity and the dynamic augmentation quantity of the position are modeled to provide a basis for subsequent measurement updates.

[0093] 2. A multi-machine joint positioning measurement update method based on cross-covariance state decorrelation replaces the original correlated covariance matrix with the upper bound of the covariance matrix according to the semi-positive definite property of the matrix, so that the UAV navigation state is regarded as unrelated. At this time, the relative distance information update between any two UAVs will not cause the navigation state of the UAVs in the entire network to gain any gain, thereby indirectly improving the positioning accuracy of the UAV itself.

[0094] 3. The multi-machine joint positioning system is a navigation method that observes the master and slave UAVs and then fuses the navigation data. In addition to the filtering algorithm, the formation configuration between the anchor winch and the UAVs also has a great impact on the navigation accuracy of the system. Referring to the concept of geometric precision factor GDOP in the GNSS system, the navigation accuracy of the formation configuration is evaluated using DOP value analysis. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 Architecture diagram of multi-machine joint positioning system based on inertial / data link ranging;

[0096] Figure 2 A flow chart of a multi-machine joint positioning and navigation method;

[0097] Figure 3 Schematic diagram of positioning solution of UAV and multiple anchor machines;

[0098] Figure 4 Block diagram of the anchor winch optimization algorithm based on geometric precision factor. DETAILED DESCRIPTION

[0099] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0100] The present invention provides a multi-machine joint positioning and combined navigation method based on inertial / data link ranging. When the target UAV is in a complex airspace electromagnetic environment, high-precision positioning and navigation requirements cannot be met by relying solely on inertial navigation information. The method comprehensively utilizes navigation sources such as inertial navigation and data link, and performs joint positioning through a multi-machine network formed by an anchor machine, a secondary anchor machine and a target UAV. After receiving the data link positioning information of other UAVs through a data link, it is tightly combined with the local inertial navigation system to achieve position correction. Through multi-source information fusion processing in a complex airspace electromagnetic environment, real-time and reliable positioning and navigation information is provided for the UAV.

[0101] The specific process is as follows:

[0102] (1) Tightly integrated algorithm and modeling of multi-machine joint positioning based on inertial / data link combination

[0103] In rugged terrain environments such as mountains, forests, and hills, drones lack absolute positioning information and cannot achieve their own positioning and navigation. In this case, a multi-machine joint positioning algorithm based on inertial / data link combination is studied. The anchor machine that can be used as a time and space reference and the data link network of drones in the formation are fully utilized. By obtaining the position reference and ranging information provided by the rear anchor machine or other sub-precision drones in the formation, a joint positioning algorithm framework is constructed, and the position solution is completed by fusing its own navigation information and data link-assisted navigation information.

[0104] The UAV navigation system should not only directly integrate the information of the UAVs with which it has a ranging relationship, but also use the ranging information between other UAVs in the network to indirectly improve the navigation performance. In order to make full use of the ranging information between other UAVs in the network, when constructing the tightly combined state equation, it is necessary to augment the position states of all UAVs with ranging relationships received in the network into the state vector. Using the "Northeast Sky" coordinate system, the state vector of the filter is divided into two parts: the fixed state and the augmented state, and the two parts of the state vector are modeled.

[0105] 1. Fixed state vector modeling:

[0106] When the UAV navigation system uses a linear Kalman filter, the 9 state quantities of the inertial instrument error and the 6 state quantities of the inertial navigation system positioning output parameter error are combined to obtain a 15-dimensional system state equation:

[0107]

[0108] in:

[0109]

[0110] W I =[w gx w gy w gz w rx w ry w rz w ax w ay w az ]

[0111] In the formula, G I is the error coefficient matrix, F I is the state coefficient matrix; the state vector X I and the white noise random error vector W I The subscripts E, N, and U represent the east, north, and sky directions of the geographic coordinate system. E ,φ N ,φ UThey represent the platform error angles in the east, north and celestial directions of the inertial navigation system error state quantity; δv E ,δv N ,δv U They represent the velocity errors in the east, north and celestial directions respectively; δL, δλ, δh represent the errors in latitude, longitude and altitude respectively; ε rx ,ε ry ,ε rz Respectively represent the gyro first-order Markov drift error state quantity in the X-axis, Y-axis and Z-axis directions; Represent the first-order Markov state of the accelerometer in the X-axis, Y-axis and Z-axis directions respectively.

[0112] 2. Augmented state vector modeling:

[0113] On the basis of the fixed state vector modeling of the navigation system, the position information received from other drones in the network should be augmented into the state vector. Assuming that the data link receives the information of drone i and drone j, the augmented vector is

[0114]

[0115] The augmented filter state vector is The augmented part of the system state coefficient matrix is ​​composed of unit matrix, and the augmented state coefficient matrix is ​​F * :

[0116]

[0117] Among them, F I is the fixed state coefficient matrix, F N is the system array corresponding to 9 basic positioning parameters, F s and F M They are:

[0118]

[0119] in, is the attitude transfer matrix, and T is the correlation time between the gyro and the accelerometer.

[0120] Through the above formula, it is found that the dimension of the augmented state equation is expanded from 15 dimensions for fixed states to 33 dimensions, 45 dimensions and even 15+3n dimensions, which fully utilizes the advantages of multi-data link position information and improves the measurement accuracy of the system.

[0121] 3. Construct the measurement equation of the data link of the tight combination algorithm

[0122] The measurement equation of the combined filtering system is one of the two sets of mathematical equations that describe the performance of the combined navigation system, which links the output of the combined navigation system with the error state equation of the system. The source of the positioning error of the UAV in the network is the data link ranging error, and the ranging errors of each data link are independent of each other. By using the information such as the position, relative distance and covariance of the anchor and secondary anchor, the ranging value and the inertial navigation data are tightly combined with Kalman filtering to achieve multi-aircraft joint positioning and navigation.

[0123] Assume that the navigation coordinate system is established with the position of the drone itself as the origin. The real positions of drone i and drone j in this coordinate system are [x i y i z i ] and [x j y j z j ], the positions calculated by inertial navigation are and The actual distance between the two drones is d ij and estimated distance They are

[0124]

[0125] Expanding according to Taylor's formula and ignoring high-order terms and small quantities, we get

[0126]

[0127] In the formula

[0128]

[0129] In order to be consistent with the variables in the state vector, the position error in the navigation system is converted to the earth coordinate system, that is,

[0130]

[0131] Where: [A i A j ]=[-e 1 -e 0 -e 2 e 1 e 0 e 2 ]AA=Diag[R Mj +h j (R Ni +h i )cosL i 1R Mi +h i (R Nj +h j)cosL j 1]

[0132] From the above analysis, the data link ranging measurement equation is obtained:

[0133]

[0134] In the formula is the data link measurement vector, A i and A j The measurement matrix is ​​composed of is the data link ranging noise.

[0135] (2) Multi-machine joint positioning measurement update method based on cross-covariance state decorrelation

[0136] In multi-machine joint positioning, due to the existence of ranging measurement updates, the navigation states between drones are highly correlated. This state correlation will cause the navigation states of other drones to change when the state of one drone is updated. The state correlation of the compact combination filter is reflected in the cross terms of the state covariance matrix between drones after the ranging value is updated. According to the semi-positive definite property of the matrix, the upper bound of the covariance matrix is ​​used to replace the original related covariance matrix, so that the navigation state of the drone is regarded as unrelated, improving the accuracy of positioning navigation.

[0137] 1. Kalman filter equation state correlation analysis

[0138] Assuming that the navigation information of all drones, including inertial navigation, satellite and data link information, is processed uniformly, the centralized state equation is:

[0139] X k =Φ k / k-1 X k-1 +Γ k / k-1 W k-1

[0140] Where X k is the navigation state vector of all UAVs, Φ k / k-1 is the state transfer matrix, Γ k / k-1 is the system noise array. The state of any UAV is represented as The one-step prediction covariance equation of the state is

[0141] P k =Φ k / k-1 P k-1 (Φ k / k-1 ) T +Q k-1

[0142] Where P k Elements in When i≠j,

[0143] From the above formula, we know that since the system noise is uncorrelated, the time update of the navigation system does not change the correlation between the states. Assuming that at time k, the navigation system receives the satellite navigation information of the anchor winch i and the data link ranging information between UAV i and UAV j, the measurement equation is described as

[0144]

[0145] The residual covariance of the GNSS measurement update is expressed as

[0146]

[0147] The Kalman filter gain is

[0148]

[0149] The updated state estimate covariance is

[0150]

[0151] Through the analysis of the updated covariance matrix and Kalman filter gain, the GNSS measurement update is only related to its own navigation state and will not affect the related items of the drone state. If it is not zero, the navigation state estimation also experiences a gain.

[0152] The data link measurement update process is analyzed, and the covariance of the measurement residual is

[0153]

[0154] The Kalman filter gain is

[0155]

[0156] The updated state estimation covariance matrix is

[0157]

[0158] By analyzing the state covariance and Kalman filter gain after data link ranging measurement update, the measurement information between any two UAVs is relevant to the state estimation of both.

[0159] 2. State covariance matrix decorrelation measurement update method

[0160] Taking GNSS measurement and data link ranging measurement as an example, assuming that drone i obtains GNSS information, i and j measure distance through data link, drone a does not receive measurement information, but is related to the status of i and j. The variables that need to be updated for each drone are shown in the following table:

[0161]

[0162] When each drone updates its status, in order to calculate the corresponding variables in the above table, drones need to exchange a lot of information. For example, when the time is updated, drone i needs to calculate It must interact with the drone m (m∈{1,2,…,n}) to obtain When the data link measurement is updated, UAV A calculates Must interact with drone i to get and This places a great burden on the data link communication system. By decorrelating the navigation status between drones, the calculation process is simplified and the communication burden is reduced. Assume that the covariance matrix of drone i and drone j at time k is expressed as:

[0163]

[0164] According to the properties of semi-positive definite matrices, we have:

[0165]

[0166] Therefore, there is

[0167]

[0168] In order to With the minimum upper bound, there is the following optimization problem:

[0169]

[0170] At this time, get The minimum upper bound of Instead of this As the covariance matrix of drone i and drone j, the states of the two drones are unrelated. After the states are decorrelated, the amount of calculation required by the drone is as follows:

[0171]

[0172]

[0173] By replacing the covariance matrix with the minimum bound, the states between drones are decorrelated, achieving the purpose of reducing the amount of data communication during data link transmission.

[0174] (3) Positioning method and performance evaluation based on DOP value analysis and configuration optimization

[0175] The multi-machine joint positioning system is a navigation method that observes each other between the master and slave UAVs and then fuses the navigation data. In addition to the filtering algorithm, the formation configuration between the anchor winch and the UAVs also has a great impact on the navigation accuracy of the system. Referring to the concept of geometric precision factor GDOP in the GNSS system, the navigation accuracy of the formation configuration is evaluated using DOP value analysis.

[0176] In the satellite navigation system, assuming that the source of positioning error is mainly satellite pseudorange error, and each satellite pseudorange error is independent of each other, the relationship between user position error and pseudorange error can be represented by a simple formula:

[0177] σ 3d =GDOP×σ μ

[0178] Define the three-dimensional position error of UAV k as The relationship between the drone position error and the distance measurement error can be expressed by the formula:

[0179]

[0180] The left term of the formula is the total characteristic of the three-dimensional position error in the process of cooperative positioning of the UAV formation, and the right term is the product of the cooperative geometric precision factor and the standard deviation of the distance measurement error. In the process of joint positioning and navigation of multiple aircraft, the measurement errors are independent and identically distributed, and the standard deviation σ d Under the premise of ignoring other error terms, the smaller the GDOP, the smaller the 3D position error. The smaller the GDOP, the better the system navigation and positioning performance. Therefore, the GDOP can be used to analyze the impact of the geometric configuration of the unmanned cluster on the positioning performance. The smaller the GDOP, the better the positioning performance of the geometric configuration.

[0181] As shown in the figure, the anchor winch sends its own position and distance value from the target UAV to the target UAV navigation system through the data link. The target UAV needs to establish an evaluation system for the GDOP value of the unmanned anchor winch.

[0182] (x,y,z)

[0183] Assume that the distance between the i-th target UAV and its adjacent UAVs is where d i is the actual distance between the target UAV and the anchor winch, δd tu is the equivalent clock error, n range is the ranging noise, and its variance is δ 2 The distance d between the target drones given by the inertial systemIi satisfy:

[0184]

[0185] Then the corresponding ranging error equation is:

[0186]

[0187] It can be considered that the position error of the anchor winch is unrelated to the data link ranging error, thus obtaining the distance difference measurement equation obtained by subtracting the estimated and measured distances:

[0188]

[0189] When the target UAV selects the information of n anchor machines to assist itself, the distance measurement equation is:

[0190]

[0191] The covariance of the ranging error in the ECEF coordinate system and the position error of the anchor winch itself have the following relationship:

[0192] cov(δd)=E[(δd)(δd) T ]

[0193] =E[(Hδp)(Hδp) T ]

[0194] =H·E[(δp)(δp) T ]·H T

[0195] =Hcov(δp)H T

[0196] Where: δp=[δxδyδzδd tu ] T , E stands for expectation.

[0197] From this, the covariance of the position error of the UAV with data link is obtained as:

[0198] cov(δp)=H -1 cov(δd)(H T ) -1

[0199] =(H T (cov(δd)) -1 H) -1

[0200] After upconverting to the navigation coordinate system (ENU), the covariance of the collaborative navigation error can be expressed as:

[0201]

[0202] Therefore, for the target UAV, its coordinated position precision factor DOP can be defined as:

[0203]

[0204] Based on the above formula, the DOP-based collaborative navigation accuracy cost function can be designed, and the optimal collaborative observation matrix H is selected with the goal of minimizing DOP, thereby realizing the optimization of nodes in the collaborative navigation information and ensuring the real-time and accuracy of the collaborative navigation system.

[0205] According to the optimization idea of ​​cluster collaborative geometric precision factor, combined with configuration factors and the positioning error of the anchor winch, the configuration of the multi-machine joint system is optimized by minimizing the cost function.

[0206] If the route cost is set as the sum of DOP costs, the cost function can be defined as follows:

[0207]

[0208] After the cost function is established, the point with the lowest cost in the attachment space can be searched in combination with the flight characteristics of the anchor winch, so that the optimal configuration can be found for the target UAV to provide better positioning services.

[0209] The above is only a specific embodiment of the present invention, and the present invention is described in detail. The unexplained part is a conventional technology. However, the protection scope of the present invention is not limited to this. Any changes or substitutions that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. The protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A multi-machine joint positioning and navigation method, characterized in that: The method comprises the following steps: Step 1: Establish the tightly combined state equation and measurement equation for multi-machine joint positioning; Step 2: Determine the multi-machine joint positioning measurement update strategy based on the cross-covariance state; Step 3: Evaluate the positioning solution effect of the tightly combined state equation and measurement equation of multi-machine joint positioning, and find the optimal tightly combined state equation and measurement equation for multi-machine joint positioning for positioning and navigation.

2. The method according to claim 1, characterized in that: In step 1, the tightly combined state equation of multi-machine joint positioning adopts the northeast celestial coordinate system, and the position states of all drones with ranging relationships are augmented and modeled into the state equation, and the Kalman filter state equation is divided into two parts: a fixed state vector and an augmented state vector.

3. The method according to claim 2, characterized in that: The fixed state vector modeling process is as follows: The 9 state quantities of the inertial instrument error and the 6 state quantities of the inertial navigation system positioning output parameter error are combined to obtain the 15-dimensional system state equation: in: IN I =[in gx In gy In gz In rx In ry In rz In ax In ay In az ] In the formula, G I is the error coefficient matrix, F I is the state coefficient matrix; X I is the state vector and W I is the white noise random error vector, and the subscripts E, N, and U represent the east, north, and sky directions of the geographic coordinate system; φ E ,φ N ,φ U They represent the platform error angles in the east, north and celestial directions of the inertial navigation system error state quantity; δv E ,δv N ,δv U They represent the velocity errors in the east, north and celestial directions respectively; δL, δλ, δh represent the errors in latitude, longitude and altitude respectively; ε rx ,ε ry ,ε rz Respectively represent the gyro first-order Markov drift error state quantity in the X-axis, Y-axis and Z-axis directions; Respectively represent the first-order Markov state quantity of the accelerometer in the X-axis, Y-axis and Z-axis directions; the superscript · represents the state vector of the next time state.

4. The method according to claim 3, characterized in that: The augmented state vector modeling process is as follows: On the basis of fixed state vector modeling, the position information received from other drones in the network is augmented into the fixed state vector. Assuming that the data link receives information from drone i and drone j, the augmented state vector is: The augmented filter state vector is The augmented part of the system state coefficient matrix is ​​composed of unit matrix, and the augmented state coefficient matrix is ​​F * : where F I is the fixed state coefficient matrix, F N is the system array corresponding to 9 basic positioning parameters, F s and F M They are: in, is the attitude transfer matrix, and T is the correlation time between the gyroscope and the accelerometer.

5. The method according to claim 4, characterized in that: In step 1, the measurement equation is established as follows: The navigation coordinate system is established with the position of the drone itself as the origin. The real positions of drone i and drone j in this coordinate system are [x i y i z i ] and ]x j y j z j ]; The positions calculated by inertial navigation are and The actual distance between the two drones is d ij and estimated distance They are Expanding according to Taylor's formula and ignoring high-order terms and small quantities, we get In the formula In order to be consistent with the variables in the state vector, the position error in the navigation system is converted to the earth coordinate system. In the formula: [A i A j ]=[-e1-e0-e2 e1 e0 e2]A A=Diag[R Mj +h j (R Ni +h i )cosL i 1R Mi +h i (R Nj +h j )cosL j 1] From the above analysis, the data link ranging measurement equation is obtained: In the formula is the data link measurement vector, A i and A j The measurement matrix is ​​composed of is the data link ranging noise.

6. The method according to claim 5, characterized in that: In step 2, the update strategy is as follows: Taking GNSS measurement and data link ranging measurement as an example, assuming that drone i obtains GNSS information, i and j measure distance through data link, drone a does not receive measurement information, but it is irrelevant to the status of i and j. At this time, the quantities that need to be updated are as follows: Time update: Drone i: One-step state prediction UAVJ: One-step prediction of state Drone A: One-step state prediction GNSS measurement update, drone i receives GNSS information: Dronei: S k,GNSS , Drone j: No Drone a: No Data link ranging measurement update, drones i and j receive ranging information: Dronei: ω Drone j: ω Drone a: No By replacing the covariance matrix with the minimum bound, the states between UAVs are decorrelated, and the data link reduces the amount of data communication during transmission.

7. The method according to claim 6, characterized in that: In step 3, the positioning solution effect is evaluated as follows: In the satellite navigation system, assuming that the source of the positioning error is the satellite pseudorange error, and that the satellite pseudorange errors are independent of each other, the following formula is used to represent the relationship between the user position error and the pseudorange error: s 3d =GDOP×σ μ Define the three-dimensional position error of UAV k as The relationship between the drone position error and the distance measurement error is expressed by the formula: The left term of the formula is the total characteristic of the three-dimensional position error during the collaborative positioning of the UAV formation, and the right term is the product of the collaborative geometric precision factor GDOP and the standard deviation of the distance measurement error; In the process of multi-machine joint positioning and navigation, the measurement errors are independent and identically distributed, and the standard deviation σ d Unchanged, ignoring other error terms, the smaller the GDOP, the smaller the 3D position error The smaller it is, the better the system navigation and positioning performance is. Therefore, GDOP is used to analyze the impact of the geometric configuration of the unmanned cluster on the positioning performance. The smaller the GDOP is, the better the positioning performance of the geometric configuration is. Assume that the distance between the i-th target UAV and its adjacent UAVs is where d i is the actual distance between the target UAV and the anchor winch, δd tu is the equivalent clock error, n range is the ranging noise, and its variance is δ 2 ; The distance d between the target drones given by the inertial system Ii satisfy: Then the corresponding ranging error equation is: It can be considered that the position error of the anchor windlass is unrelated to the data link ranging error, thus obtaining the distance difference measurement equation obtained by subtracting the estimated and measured distances: When the target UAV selects the information of n anchor machines to assist itself, the distance measurement equation is: The covariance of the ranging error in the ECEF coordinate system and the position error of the anchor winch itself have the following relationship: cov(δd)=E[(δd)(δd) T ] =E[(Hδp)(Hδp) T ] =H·E[(δp)(δp) T ]·H T =Hcov(δp)H T Where: δp=[δxδyδzδd tu ] T , E represents expectation; From this, the covariance of the position error of the UAV with data link is obtained as: cov(δp)=H -1 cov(δd)(H T ) -1 =(H T (cov(δd)) -1 H) -1 After upconverting to the navigation coordinate system (ENU), the covariance of the collaborative navigation error is expressed as: Therefore, for the target UAV, its coordinated position precision factor DOP is defined as: Based on the above formula, a DOP-based collaborative navigation accuracy cost function is designed, and the optimal collaborative observation matrix H is selected with the goal of minimizing DOP, thereby realizing the optimization of nodes in the collaborative navigation information and ensuring the real-time and accuracy of the collaborative navigation system.

8. The method according to claim 7, characterized in that: Set the route cost as the sum of DOP costs, and define the cost function as follows: After the cost function is established, the point with the minimum cost in the attachment space is searched in combination with the flight characteristics of the anchor winch. Based on the point with the minimum cost, the parameters of the optimal state equation and measurement equation are deduced.

Images