Heterogeneous cluster cooperative navigation optimization method based on hybrid linearization message passing

Abstract

The application discloses a heterogeneous cluster cooperative navigation optimization method based on hybrid linearization message passing, which utilizes the relative measurement information of hybrid relative distance and angle between two members of the heterogeneous cluster to improve the overall positioning accuracy of the heterogeneous cluster when the aircraft formation with different positioning accuracies performs a task; according to the given cluster aircraft navigation mode and performance, the positioning errors of the respective cluster aircraft members are estimated, and the position and velocity variable nodes in the message passing model are established; the relative measurement information is linearized by using an algebraic method based on linear regression, and the relative measurement message in the message passing model is established; based on the linearized message passing model, the message passing algorithm is used to cooperatively enhance the overall navigation of the heterogeneous cluster. The application can effectively improve the overall navigation performance of the cluster aircraft and is suitable for practical application in the case that there are a large number of aircrafts, and a variety of relative measurement relationships of hybrid ranging and angle measurement in the cluster.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of positioning and navigation, and particularly relates to a heterogeneous cluster cooperative navigation optimization method based on hybrid linearization message passing. BACKGROUND

[0002] For most heterogeneous clusters, due to the limitation of manufacturing cost and flight load, only individual or a small part of the aircraft in the cluster is equipped with a high-precision inertial navigation system, and most members rely only on low-precision inertial navigation systems and sensors for navigation. In complex terrain or electromagnetic interference environment, the positioning signals of the reference aircraft and navigation satellites are often shielded or interfered, which will cause the inertial navigation error of part of the members in the heterogeneous cluster to diverge. Therefore, in the complex terrain or electromagnetic interference environment, the positioning performance of the heterogeneous cluster to its members and targets will be seriously reduced.

[0003] In the complex terrain or strong electromagnetic interference environment, the information interaction between different sensors of the cluster aircraft using cooperative navigation technology can realize the navigation information cooperation of the heterogeneous cluster, so as to improve the overall positioning accuracy of the cluster and suppress the error divergence of the inertial navigation system. Therefore, cooperation is of great significance for the navigation of the heterogeneous cluster.

[0004] The non-parametric message passing algorithm uses a large number of particles to approximate the message passing between cluster members, but this method will cause a large computational burden when a large number of particles are collected. The traditional Gaussian message passing algorithm estimates and corrects the positions of the cluster members based on Gaussian distribution, which is relatively easy to implement but difficult to suppress the nonlinear error in cluster cooperation, and the navigation performance is greatly affected by negative factors such as non-line-of-sight error. The traditional sigma point message passing performs one high-dimensional processing on the relative measurement messages of a member and all adjacent members, and lacks multiple iterative updates of the relative measurement messages between two members. In addition, most of the traditional message passing cooperative positioning algorithms are based on the relative distance between aircrafts, and the use of relative angle information is less, which cannot fully utilize the relative measurement information of the heterogeneous cluster.

[0005] In order to make full use of the relative distance and angle measurement information in the heterogeneous cluster, the application provides a heterogeneous cluster cooperative navigation optimization method based on mixed linearization message passing. Compared with the traditional method which can only use relative distance information, the method can use various mixed relative measurement information such as distance, pseudo-range rate and angle between cluster members. The method uses linear regression method to process the relative measurement information, which can effectively reduce the nonlinear error in the relative measurement information compared with the traditional method, and then establishes the relative measurement message between the members according to the processed relative measurement information, and iteratively updates the more accurate position and velocity estimation of the cluster members using the relative measurement message. At the same time, compared with the traditional non-parametric message passing algorithm, the number of sampling points is less, and the calculation burden is also lighter. SUMMARY

[0006] The technical problem to be solved by the application is to provide a heterogeneous cluster cooperative navigation optimization method based on mixed linearization message passing, which uses various relative measurement information between heterogeneous cluster members to improve the overall positioning accuracy of the heterogeneous cluster.

[0007] The application adopts the following technical solutions to solve the above technical problems:

[0008] The heterogeneous cluster cooperative navigation optimization method based on mixed linearization message passing comprises the following steps:

[0009] Step 1), obtaining the rectangular coordinates, velocities of each aircraft in the heterogeneous cluster in the earth coordinate system and the relative measurement information between the heterogeneous clusters, wherein the relative measurement information includes relative distance information and relative angle information;

[0010] Step 2), establishing the state variable nodes of each member in the message passing model of the cluster aircraft according to the covariance estimation of the position and velocity of the cluster aircraft;

[0011] Step 3), processing different relative measurement information in the cluster according to the types of the relative measurement information such as distance measurement, angle measurement and distance / angle measurement between each pair of aircrafts;

[0012] Step 4), linearizing the relative measurement information by linear regression algorithm according to the position, velocity of the cluster aircraft, the relative measurement information and the types of the relative measurement information between each pair of aircrafts;

[0013] Step 5), establishing the relative measurement message μ in the message passing model according to the linearization results of the relative measurement information of the aircraft;

[0014] Step 6), using the posterior probability message passing algorithm to realize the fusion of the position, velocity and relative measurement message of the cluster aircraft, and obtaining the fused position and velocity state of the cluster aircraft;

[0015] Step 7), optimizing the linearization parameters in step 4) according to the optimized cluster aircraft positioning based on message passing;

[0016] Step 8), repeating steps 5) to 7) to iteratively correct the cluster aircraft positioning until the iteration number is equal to a preset iteration number threshold.

[0017] As a further optimization scheme of the heterogeneous cluster collaborative navigation optimization method based on hybrid linearization message passing of the present application, the detailed steps of step 2) are as follows:

[0018] Step 2.1), estimating the three-dimensional position and velocity covariance P i of the i-th aircraft according to the coordinates (x i ,y i ,z i ) and velocity vector of the i-th aircraft in the earth coordinate system, using a multivariate normal distribution to approximate the state distribution of the aircraft, then the prior probability distribution function p(X i ) of the i-th aircraft position and velocity is p(X i ,P i ), wherein, N is the normal distribution in probability statistics;

[0019] Step 2.2), in the message passing model, using a variable node to represent the estimation q(X i ) of the i-th aircraft's own position and velocity ~ N(X i ,P i ).

[0020] As a further optimization scheme of the heterogeneous cluster collaborative navigation optimization method based on hybrid linearization message passing of the present application, the detailed steps of step 3) are as follows:

[0021] Step 3.1), if the relative measurement information z i,j between two aircrafts only contains pseudo-range and pseudo-range rate i.e. there is only a range cooperative relationship between the two relative measurement aircrafts , then the relative measurement information does not need to be processed, and step 4) is directly executed;

[0022] Step 3.2), if the relative measurement information z i,j between two aircrafts contains relative angle information i.e. there is an angle cooperative relationship or a range / angle cooperative relationship between the two relative measurement aircrafts i,j θ is the relative azimuth angle of the aircrafts, is the relative elevation angle of the aircrafts, and the relative angle information in the relative measurement information z i,j is processed according to the following formula The processing is performed:

[0023]

[0024]

[0025] wherein the unit of the vector θ, is radian.

[0026] As a further optimization scheme of the heterogeneous cluster cooperative navigation optimization method based on hybrid linearization message passing of the application, the detailed steps of step 4) are as follows:

[0027] Step 4.1), the relative measurement data z i,j contains a nonlinear error, and z i,j is decomposed by a linear regression method, and the position and velocity estimate X i , X j , P i , P j is selected m sampling points χ1,..., χ m and corresponding weights ω1,..., ω m , wherein the sampling points is the t-th sampling point of the position and velocity of the i-th aircraft, is the t-th sampling point of the position and velocity of the j-th aircraft.

[0028] Step 4.2), according to the type of the relative measurement data z i,j , the m sampling points are calculated:

[0029] Z t = h(χ t ), t = 1,..., m

[0030] Step 4.2.1), if the relative measurement information that is, z i,j only contains relative range information, ||·|| represents the vector norm, then wherein:

[0031]

[0032]

[0033] Step 4.2.2), if the relative measurement information that is, z i,j only contains relative angle information, then

[0034]

[0035]

[0036] where θ t , are azimuth and elevation angles obtained from sampling

[0037]

[0038]

[0039] Step 4.2.3), if the relative measurement information z i,j contains relative range / angle information, then

[0040]

[0041]

[0042]

[0043]

[0044] where θ t , are azimuth and elevation angles obtained from sampling

[0045]

[0046]

[0047] Step 4.3), obtain the mean and covariance of the relative measurement information z i,j

[0048]

[0049]

[0050] where,

[0051] Step 4.4), compute the linearization parameters of the relative measurement information:

[0052]

[0053]

[0054] Ω i,j = Φ - A i,j P joint (A​​​i,j ) T

[0055] Then the relative measurement information z i,j Decomposed into:

[0056]

[0057] in It follows a zero mean and has a covariance matrix of Ω. i,j Gaussian distributed variables,

[0058] Step 4.5), repeat steps 4.1) to 4.4) until the linearization of all relative measurement data of the cluster is completed.

[0059] As a further optimization scheme of the heterogeneous cluster cooperative navigation optimization method based on hybrid linearized message passing of the present invention, the detailed steps of step 5) are as follows:

[0060] Step 5.1), based on the relative measurement data z i,j Approximate decomposition is used to establish the relative measurement message μ from the i-th aircraft to the j-th aircraft in the message passing model. i→j If the i-th aircraft does not receive relative measurement messages μ from other aircraft p→i Execute step 5.2), if the i-th aircraft has received relative measurement messages μ from other aircraft. p→i Execute step 5.3);

[0061] Step 5.2), if the i-th aircraft does not receive relative measurement messages μ from any of the other aircraft besides j. p→i According to the relative measurement data z i,j The approximate decomposition directly establishes the relative measurement message μ between the i-th and j-th aircraft in the message passing model. i→j :

[0062] μ i→j (X j )∝N(α i→j ,Γ i→j )

[0063] in R is the relative measurement noise covariance;

[0064] Step 5.3), if the i-th aircraft has received relative measurement messages μ from all other aircraft except j. p→i common The first step is based on the relative measurement message μ. p→i Calculate the optimized position and velocity estimate of the i-th aircraft.

[0065]

[0066] wherein is calculated by times Kalman filter update, first let P i→j = P i , the filter update specific formula is:

[0067] Let

[0068] Filter gain

[0069] State estimation equation

[0070] Estimation mean square deviation equation P i→j = (I-KH p→i )P i→j

[0071] Get the i-th aircraft position and speed estimate after μ p→i optimization After that, according to the approximate decomposition of the relative measurement data z i,j , the relative measurement message μ i→j from the i-th aircraft to the j-th aircraft in the message passing model is established:

[0072] μ i→j (X j ) ∝ N (α i→j , Γ i→j )

[0073] wherein, R is the relative measurement noise covariance;

[0074] Step 5.4), repeat steps 5.1) to 5.3) for all aircraft in the cluster until all relative measurement messages μ in the cluster are established.

[0075] As a further optimization scheme of the heterogeneous cluster cooperative navigation optimization method based on hybrid linearization message passing of the present application, the detailed steps of step 6) are as follows:

[0076] Step 6.1), according to the n(i) relative measurement messages μ p→i received by the i-th, the i-th aircraft's corrected position and speed estimate and P i are obtained by:

[0077]

[0078] Step 6.2), let Formula 6.1) is derived from the formula through... The specific formula is obtained by performing n(i) Kalman filter updates:

[0079] make

[0080] Filter gain

[0081] State estimation equation

[0082] Estimating the mean squared error equation

[0083] Step 6.3): Based on the Kalman filter update in step 6.2), update the more accurate position and velocity estimate X of the i-th aircraft. i and P i ;

[0084] Step 6.4): For other aircraft in the cluster that are measuring each other, repeat steps 6.1) to 6.3) until the position and velocity estimates X and P of all aircraft in the cluster are updated.

[0085] As a further optimization scheme of the heterogeneous cluster cooperative navigation optimization method based on hybrid linearized message passing of the present invention, the detailed steps of step 7) are as follows:

[0086] Step 7.1), if relative measurement information is transmitted between the i-th and j-th aircraft, then the joint posterior probability distribution function of their positions and velocities is...

[0087] Step 7.2), the mean of the joint posterior probability estimate for the i-th and j-th aircraft. covariance P joint By estimating the position and velocity N(X) of the i,j-th aircraft i ,P i ), N(X j ,P j The result is obtained by updating the Kalman filter:

[0088] First let P joint =[P i ,P j Then, Kalman filtering is performed using linearization parameters A, b, Ω, relative measurement information z, and relative measurement noise covariance R. The specific formula is as follows:

[0089] make

[0090] Filter gain

[0091] State estimation equation

[0092] Estimation mean square error equation P joint = (I - KA i,j )P joint ;

[0093] Step 7.3), using the optimized joint position and velocity estimation of the i,jth aerial vehicle, performing steps 4.1) to 4.4) to update the linearized parameters of the relative measurement information:

[0094] Step 7.4), repeating steps 7.1) to 7.3) until all the linearized parameters of the cluster aerial vehicles are updated.

[0095] Compared with the prior art, the present application has the following technical effects:

[0096] The present application can use inter-aerial vehicle ranging, angle measurement, and ranging / angle measurement three kinds of relative measurement information for collaborative navigation. Firstly, the relative measurement information between aerial vehicles is linearly decomposed to suppress the nonlinear error in the relative measurement. Then, through the message passing algorithm, the more accurate positioning of the cluster aerial vehicles is calculated on the message passing model. Although the positioning accuracy is lower than that of the traditional particle filter method due to the small number of sample points, the calculation amount is small and the nonlinear error in the heterogeneous cluster collaborative navigation can be suppressed, which is more suitable for the cluster aerial vehicle system working in complex terrain.

[0097] The present application converts the heterogeneous cluster collaborative positioning problem into an inference problem on the message passing model to obtain more accurate position and velocity estimation of the cluster aerial vehicles and improve the cluster positioning accuracy. The present application can be distributedly implemented without a central processing node, has strong adaptability and robustness, optimizes the positioning accuracy of the cluster aerial vehicles, and improves the positioning performance of the navigation system in complex terrain and electromagnetic interference environment. This has important economic and military significance for accurate positioning of the heterogeneous cluster itself and the target, and is very important for stable operation of other navigation and positioning applications in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0098] Figure 1 The present application is a principle flowchart;

[0099] Figure 2 The present application is a message passing model established;

[0100] Figure 3 The present application is a cluster collaborative navigation simulation schematic diagram;

[0101] Figure 4 The present application is a sampling process parameter diagram in the simulation process;

[0102] Figure 5 Positioning result comparison chart of different cooperative navigation enhancement methods for the same low-precision aircraft. DETAILED DESCRIPTION

[0103] The technical solutions of the present application will be further described in detail below with reference to the accompanying drawings:

[0104] The present application can be implemented in many different forms, and should not be considered limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the application to those skilled in the art. In the drawings, components are exaggerated for clarity.

[0105] It should be understood that although the terms first, second, third, etc. can be used herein to describe various elements, components and / or parts, these elements, components and / or parts are not limited by these terms. These terms are only used to distinguish one element, component and / or part from another. Therefore, the first element, component and / or part discussed below can become the second element, component or part without departing from the teachings of the present application.

[0106] As shown in Figure 1 The present application discloses a heterogeneous cluster cooperative navigation enhancement method based on hybrid linearization message passing. When different types and positioning accuracy aircrafts are in formation flight, the relative distance, angle and other relative measurement information between the members of the heterogeneous cluster is used to improve the overall positioning accuracy of the heterogeneous cluster. According to the given cluster aircraft navigation mode and performance, the positioning error of each cluster aircraft member is estimated. According to the different relative measurement information types of ranging, angle measurement, ranging / angle measurement between aircrafts, the cluster relative measurement information is processed. According to the aircraft position and speed estimation and the relative measurement information, the linear regression method is used to linearize the relative measurement information. According to the results of linearization processing of the aircraft relative measurement information, the relative measurement message in the message passing model is established. Using the message passing algorithm, the fusion of the cluster aircraft position and relative measurement information is realized, and the more accurate position and speed estimation of the cluster aircraft is obtained. According to the optimized cluster aircraft positioning after message passing, the linearization parameters are corrected, and the specific steps are as follows:

[0107] Step 1), obtaining the rectangular coordinates, velocities of each aircraft in the heterogeneous cluster in the earth coordinate system, and the relative measurement information between the heterogeneous clusters, the relative measurement information including relative distance information and relative angle information.

[0108] Step 2), according to the covariance estimation of the cluster aircraft position and speed, establishing the state variable nodes of each member in the message passing model of the cluster aircraft;

[0109] Step 2.1), based on the coordinates (x, y) of the i-th spacecraft in the Earth coordinate system... i ,y i ,z i ) and velocity vector Estimate the covariance P of the three-dimensional position and velocity of the i-th aircraft. i Using a multivariate normal distribution to approximate the state distribution of an aircraft, the prior probability distribution function p(X) of the position and velocity of the i-th aircraft is... i )=N(X i ,P i ),in, N represents a normal distribution in probability and statistics;

[0110] Step 2.2): In the message passing model, variable nodes represent the estimated position and velocity q(X) of the i-th aircraft. i )~N(X i ,P i ).

[0111] Step 3) Process the different relative measurement information in the cluster according to the type of relative measurement information between pairs of aircraft, such as distance measurement, angle measurement, and distance / angle measurement;

[0112] Step 3.1), if the relative measurement information z between the two aircraft i,j Includes only pseudorange and pseudorange rate That is, the two relative measuring aircraft only have a ranging cooperative relationship. Then there is no need to process the relative measurement information; proceed directly to step 4).

[0113] Step 3.2), if the relative measurement information z of the two aircraft i,j Includes relative angle information That is, there is an angle measurement cooperation relationship between the two relative measurement aircraft. Or distance measurement / angle measurement cooperative relationship θ is the relative azimuth angle of the aircraft. Let z be the relative altitude angle of the aircraft. The relative measurement information z is calculated using the following formula. i,j Relative angle information Processing:

[0114]

[0115]

[0116] Among them, vector θ, The units are all in radians.

[0117] Step 4) Based on the position, speed, relative measurement information of each pair of aircraft and their types, the relative measurement information is linearized using a linear regression algorithm.

[0118] Step 4.1), the relative measurement data z of the i-th and j-th aircraft. i,j Including nonlinear error, for z i,j X is decomposed using linear regression and estimated based on the relative position and velocity of the measurement aircraft. i X j P i P j Select m sampling points χ1,...,χ m and the corresponding weights ω1,...,ω m Sampling points Let t be the sampling point for the position and velocity of the i-th aircraft. This is the t-th sampling point for the position and velocity of the j-th aircraft.

[0119] Step 4.2), based on the relative measurement data z i,j The type is used to calculate for m sampling points:

[0120] Z t =h(χ t ), t=1,...,m

[0121] Step 4.2.1), if relative measurement information That is, z i,j If only relative ranging information is included, and ||·|| represents the vector norm, then... in:

[0122]

[0123]

[0124] Step 4.2.2), if relative measurement information That is, z i,j If only relative angle measurement information is included, then

[0125]

[0126]

[0127]

[0128] Where θ t , To be based on sampling The obtained azimuth and elevation angles;

[0129]

[0130]

[0131] Step 4.2.3), if the relative measurement information i.e. z = (x, y, h) i,j contains relative range / angle information, then

[0132]

[0133]

[0134]

[0135]

[0136]

[0137] where θ = arctan(y / x) and h = arctan(z / h) t , are the azimuth and elevation angles, respectively, obtained from the sampling

[0138]

[0139]

[0140] Step 4.3), obtain the mean and covariance of the relative measurement information z i,j

[0141]

[0142]

[0143] where

[0144] Step 4.4), compute the linearization parameters of the relative measurement information:

[0145]

[0146]

[0147] Ω i,j = Φ - A i,j P joint (A i,j ) T

[0148] Then the relative measurement information z i,j is decomposed as:

[0149] ​​

[0150] in It follows a zero mean and has a covariance matrix of Ω. i,j Gaussian distributed variables,

[0151]

[0152] Step 4.5), repeat steps 4.1) to 4.4) until the linearization of all relative measurement data of the cluster is completed.

[0153] Step 5): Based on the linearized result of the relative measurement information of the aircraft, establish the relative measurement message μ in the message passing model;

[0154] Step 5.1), based on the relative measurement data z i,j Approximate decomposition is used to establish the relative measurement message μ from the i-th aircraft to the j-th aircraft in the message passing model. i→j If the i-th aircraft does not receive relative measurement messages μ from other aircraft p→i Execute step 5.2), if the i-th aircraft has received relative measurement messages μ from other aircraft. p→i Execute step 5.3);

[0155] Step 5.2), if the i-th aircraft does not receive relative measurement messages μ from any of the other aircraft besides j. p→i According to the relative measurement data z i,j The approximate decomposition directly establishes the relative measurement message μ between the i-th and j-th aircraft in the message passing model. i→j :

[0156] μ i→j (X j )∝N(α i→j ,Γ i→j )

[0157] in R is the relative measurement noise covariance;

[0158] Step 5.3), if the i-th aircraft has received relative measurement messages μ from all other aircraft except j. p→i common The first step is based on the relative measurement message μ. p→i Calculate the optimized position and velocity estimate of the i-th aircraft.

[0159]

[0160] in The calculation is done through The secondary Kalman filter update is obtained by first letting P i→j = P i The filter update is given by the following formula:

[0161] Let

[0162] The filter gain

[0163] The state estimation equation

[0164] The estimation variance equation P i→j = (I - KH p→i )P i→j

[0165] The i-th aircraft position and velocity estimate after μ p→i optimization is obtained After that, according to the approximate decomposition of the relative measurement data z i,j , the relative measurement message μ i→j from the i-th aircraft to the j-th aircraft in the message passing model is established:

[0166] μ i→j (X j ) ∝ N (α i→j , Γ i→j )

[0167] wherein, R is the relative measurement noise covariance;

[0168] Step 5.4), repeat steps 5.1) to 5.3) for all aircraft in the cluster until all relative measurement messages μ in the cluster are established.

[0169] Step 6), use the posterior probability message passing algorithm to realize the fusion of the cluster aircraft position, velocity and relative measurement messages, and obtain the fused position and velocity state of the cluster aircraft;

[0170] Step 6.1), according to the n(i) relative measurement messages μ p→i received by the i-th aircraft, the corrected position and velocity estimate of the i-th aircraft and P i , the following is obtained:

[0171]

[0172] Step 6.2), let The formula in formula 6.1) is obtained by performing n(i) times of Kalman filter update on , and the specific formula is:

[0173] Let

[0174] Filtering gain

[0175] State estimation equation

[0176] Estimation mean square error equation

[0177] Step 6.3), according to the Kalman filter update of step 6.2), update the more accurate position and velocity estimation X of the i-th aircraft i and P i ;

[0178] Step 6.4), repeat steps 6.1) to 6.3) for other mutually measured aircraft in the cluster until all aircraft position and velocity estimations X and P in the cluster are updated.

[0179] Step 7), optimize the linearization parameters in step 4) according to the optimized cluster aircraft positioning by message passing;

[0180] Step 7.1), if the relative measurement information is passed between the i-th aircraft and the j-th aircraft, then the joint posterior probability distribution function of their position and velocity

[0181] Step 7.2), the mean of the joint posterior probability estimation of the i-th and j-th aircraft Covariance P joint By Kalman filter updating the estimation N(X i , P i ) of the position and velocity of the i-th and j-th aircraft, N(X j , P j ), we get:

[0182] First let P joint = [P i , P j ], then use the linearization parameters A, b, Ω, the relative measurement information z, and the relative measurement noise covariance R to perform Kalman filter updating, the specific formula is:

[0183] Let

[0184] Filtering gain

[0185] State estimation equation

[0186] Estimation mean square error equation P joint = (I-KAi,j )P joint ;

[0187] Step 7.3), using the optimized joint position and velocity estimation of the ith,j aircraft, perform steps 4.1) to 4.4) to update the linearized parameters of the relative measurement information:

[0188] Step 7.4), repeat steps 7.1) to 7.3) until all the linearized parameters of the cluster aircraft are updated.

[0189] Step 8), repeat steps 5) to 7) to iteratively correct the cluster aircraft positioning until the number of iterations is equal to the preset iteration threshold.

[0190] The present application can significantly improve the positioning performance of low-precision aircrafts in the presence of fewer high-precision aircrafts, and is suitable for practical applications. The principle flowchart of the present application is shown in Figure 1 The message passing model established by the present application is shown in Figure 2 The heterogeneous cluster cooperative navigation simulation diagram is shown in Figure 3 ; Figure 4 The sampling process parameter diagram in the simulation process is shown in Figure 5 The positioning results of the same aircraft using different cooperative navigation enhancement methods are shown in

[0191] Those skilled in the art can understand that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with those in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless otherwise defined.

[0192] The above specific embodiments further illustrate the purposes, technical solutions and beneficial effects of the present application. It should be understood that the above description is only a specific embodiment of the present application and is not intended to limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application should be included in the protection scope of the present application.

Claims

1. A method for cooperative navigation optimization based on hybrid linearization message passing for heterogeneous clusters, characterized in that, The method comprises the following steps: Step 1), obtaining the rectangular coordinates, velocities of each aircraft in the heterogeneous cluster in the earth coordinate system, and the relative measurement information between the heterogeneous clusters, wherein the relative measurement information comprises relative distance information and relative angle information; Step 2), establishing the state variable nodes of each member in the message passing model of the cluster aircraft according to the covariance estimation of the position and velocity of the cluster aircraft; Step 2.1), estimate the covariance P i of the 3D position and velocity of the ith aircraft using the coordinates (x i ,y i ,z i ) and velocity vector of the ith aircraft in the earth coordinate system, and the state distribution of the aircraft is approximated by a multivariate normal distribution, then the prior probability distribution function p(X i ) of the position and velocity of the ith aircraft is p(X i ,P i ), where, N is the normal distribution in probability statistics; Step 2.2), in the message passing model, the variable node represents the estimation of the i-th aerial vehicle's own position, velocity q(X i ) ~ N(X i , P i ); Step 3), processing different relative measurement information in the cluster according to the types of the ranging, angle measurement, ranging / angle measurement between each two aircrafts; Step 3.1), if the relative measurement information z i,j Only containing pseudo-range and pseudo-range rate That is, the two relative measurement aircrafts only exist the range coordination relationship Then, the relative measurement information does not need to be processed, and step 4) is directly executed; Step 3.2), if the relative measurement information z of the two aircraft i,j Includes relative angle information That is, there is an angle measurement cooperation relationship between the two relative measurement aircraft. Or distance measurement / angle measurement cooperative relationship θ is the relative azimuth angle of the aircraft. Let z be the relative altitude angle of the aircraft. The relative measurement information z is calculated using the following formula. i,j Relative angle information Processing: wherein the vector θ, in radians; Step 4), linearizing the relative measurement information based on the linear regression algorithm according to the position, velocity of the cluster aircraft, the relative measurement information between each two aircrafts and the types thereof; Step 4.1), relative measurement data z of the i-th aircraft and the j-th aircraft i,j Including non-linear errors, z i,j Decomposed by linear regression method, according to the position and speed estimate X of the relative measurement aircraft i , X j , P i , P j Select m sampling points χ1,...,χ m And the corresponding weight ω1,...,ω m Where the sampling points The t-th sampling point of the i-th aircraft position and speed, The t-th sampling point of the j-th aircraft position and speed; Step 4.2), based on the relative measurement data z i,j of the type, the m sampling points are calculated: Z t = h(x t ), t = 1,..., m Step 4.2.1), if the relative measurement information i.e. z i,j contains only relative ranging information, ||·|| denotes the vector norm, then where: Step 4.2.2), if the relative measurement information i.e. z i,j contains only relative angular information, then where θ t , are azimuth and elevation angles obtained from sampling ; Step 4.2.3), if relative measurement information i.e. z i,j contains relative range / angle information, then where θ t , are azimuth and elevation angles, respectively, obtained from sampling . Step 4.3), obtaining relative measurement information z i,j the mean and covariance of wherein, Step 4.4), calculating the linearization parameters of the relative measurement information: Ω i,j = Φ - A i,j P joint (A i,j ) T The relative measurement information z i,j is decomposed into: wherein is a Gaussian distributed variable with zero mean and covariance matrix Ω i,j is a Gaussian distributed variable with zero mean and covariance matrix Ω Step 4.5), repeating steps 4.1) to 4.4) until the linearization processing of all the relative measurement data of the cluster is completed; Step 5), establishing the relative measurement message μ in the message passing model according to the results of the linearization processing of the relative measurement information of the aircrafts; Step 6), realizing the fusion of the position, velocity and relative measurement message of the cluster aircrafts by using the posterior probability message passing algorithm, and obtaining the fused position and velocity state of the cluster aircrafts; Step 7), optimizing the linearization parameters in step 4) according to the optimized positioning of the cluster aircrafts in the message passing; Step 8), repeating steps 5) to 7) to iteratively correct the positioning of the cluster aircrafts until the iteration number is equal to the preset iteration number threshold.

2. The hybrid linearization message passing based heterogeneous cluster collaborative navigation optimization method according to claim 1, wherein, The detailed steps of step 5) are as follows: Step 5.1), based on the relative measurement data z i,j Approximate decomposition is used to establish the relative measurement message μ from the i-th aircraft to the j-th aircraft in the message passing model. i→j If the i-th aircraft does not receive relative measurement messages μ from other aircraft p→i Execute step 5.2), if the i-th aircraft has received relative measurement messages μ from other aircraft. p→i Execute step 5.3); Step 5.2), if the i-th aircraft does not receive the relative measurement message μ p→i from the j-th aircraft, the i-th aircraft sends a relative measurement message μ i,j to the j-th aircraft i→j : μ i→j (X j )∝N(α i→j ,Γ i→j ) wherein R is the relative measurement noise covariance; Step 5.3), if the i-th aircraft has received relative measurement messages μ p→i Common Clause, first according to the relative measurement messages μ p→i Calculate the optimized i-th aircraft position and velocity estimate wherein The calculation of is obtained by a Kalman filter update of order P i→j = P i The filter update is specified by the formula: Let Filtering gain State estimation equation Estimate the mean square error equation P i→j = (I - KH p→i )P i→j get the relative measurement message μ p→i Optimized position and velocity estimate of the ith aircraft After that, according to the relative measurement data z i,j The approximation decomposition of the ith aircraft to the jth aircraft in the message passing model is established i→j : μ i→j (X j )∝N(α i→j ,Γ i→j ) wherein, R is the relative measurement noise covariance; Step 5.4), repeating steps 5.1) to 5.3) for all the aircrafts in the cluster until all the relative measurement messages μ of the cluster are established.

3. The hybrid linearization message passing based heterogeneous cluster collaborative navigation optimization method according to claim 2, wherein, The detailed steps of step 6) are as follows: Step 6.1), the n(i) relative measurement messages μ received by the i-th station are processed p→i , the corrected position and velocity estimate of the i-th vehicle and P i are obtained by Step 6.2), let Formula 6.1) is derived from the formula through... The specific formula is obtained by performing n(i) Kalman filter updates: Let Filtering gain State estimation equation Estimate mean square error equation Step 6.3), update the more accurate position and velocity estimate of the i-th aircraft X based on the Kalman filter update of step 6.2) i and P i ; Step 6.4), repeating steps 6.1) to 6.3) for other mutually measured aircrafts in the cluster until the position and velocity estimation X and P of all the aircrafts in the cluster are updated.

4. The hybrid linearization message passing based heterogeneous cluster collaborative navigation optimization method according to claim 3, characterized in that, The detailed steps of step 7) are as follows: Step 7.1), if relative measurement information is exchanged between the ith aircraft and the jth aircraft, the joint posterior probability distribution function of their positions and velocities Step 7.2), Mean of the joint posterior probability estimate of the i,jth aircraft Covariance P joint By Kalman filtering update of the estimate N(X i ,P i ), N(X j ,P j ) First let P joint = [P i , P j ], then use the linearization parameters A, b, Ω, relative measurement information z, relative measurement noise covariance R to perform Kalman filter update, and the specific formula is: Let Filtering gain State estimation equation Estimate the mean square error equation P joint = (I - KA i,j )P joint ; Step 7.3), updating the linearization parameters of the relative measurement information by steps 4.1) to 4.4) using the optimized joint position and velocity estimation of the i,jth aircraft: Step 7.4), repeating steps 7.1) to 7.3) until all the linearization parameters of the cluster aircrafts are updated.

Images