An Optimization Interaction Method for Navigation Information Source Groups in Cross-Domain Collaborative Navigation

Abstract

The present invention discloses a navigation information source group optimization interaction method for cross-domain collaborative navigation, which optimizes the selection of collaborative navigation information sources for the carrier to be assisted, so that the relative measurement and information interaction selection between the carrier to be assisted and the reference information source are more reasonable; the position error matrix is ​​obtained by dividing the covariance matrix of the carrier to be assisted, and the error ellipsoid of the actual navigation position is solved according to the error matrix to obtain the axis length and direction of the error ellipsoid. This method optimizes the selection of collaborative positioning signal sources, can reduce the amount of collaborative information calculation and processing, and speed up the collaborative positioning solution; considering the influence of signal source configuration and accuracy on positioning results, the positioning accuracy is improved by efficiently utilizing cluster positioning information optimization. Compared with the random signal source selection collaborative positioning algorithm that does not adopt optimization, the present invention can effectively improve the efficiency and accuracy of collaborative positioning in irregularly distributed clusters, and is suitable for practical applications.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation, and in particular to a method for optimizing the interaction of a navigation information source group for cross-domain collaborative navigation. Background Art

[0002] In recent years, the technology of aircraft swarm collaboration has attracted increasing attention at home and abroad. Aircraft swarms have the advantages of a large working range, high reliability, the ability to perform multiple tasks simultaneously, and high overall efficiency, and can be used in fields such as disaster survey and rescue. Aircraft swarm technology, that is, an organizational mode in which multiple aircraft perform a certain formation arrangement and task allocation to meet the requirements of tasks, includes both the generation, maintenance, and change of the formation of swarm flight and the planning and organization of swarm flight tasks, and is an important trend in the development of future aircraft flight technology.

[0003] The performance of traditional aircraft swarm collaborative positioning methods is greatly affected by navigation signal sources. Only when the aircraft in the swarm are relatively dense and uniform can better positioning accuracy be obtained, while the positioning accuracy is greatly affected in the case of an irregularly grouped swarm. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method for optimizing the interaction of a navigation information source group for cross-domain collaborative navigation to overcome the defects involved in the background art. The method optimizes the selection of collaborative positioning sources between the to-be-assisted vehicle and the reference information source, making the ranging and information interaction selection between the to-be-assisted vehicle and the reference information source more reasonable, so as to expand the applicability of the positioning algorithm of the swarm aircraft and improve the positioning accuracy, enabling it to be well applied in an irregularly grouped aircraft swarm.

[0005] The present invention adopts the following technical solutions to solve the above technical problems:

[0006] A method for optimizing the interaction of a navigation information source group for cross-domain collaborative navigation, comprising the following steps:

[0007] Step A.1), obtaining the system data required for the positioning and navigation of the aircraft swarm;

[0008] Step A.2), calculating the error ellipsoid E0 of the current position according to the filtering covariance matrix of the to-be-assisted aircraft;

[0009] Step A.3), obtaining the relative azimuth and relative distance between each reference information source aircraft and the to-be-assisted aircraft;

[0010] Step A.4), obtain the direction where the maximum axis length of the error ellipsoid E0 lies and the self-accuracy of the signal source, select a reference information source aircraft closest to the direction of the maximum axis length as the reference signal source aircraft, perform assisted positioning calculation and correct the original position to obtain a new error ellipsoid E1;

[0011] Step A.5), determine whether the axis length L1 of the longest axis of the error ellipsoid E1 satisfies a preset threshold δ. If the axis length L1 > δ, let E0 = E1, that is, take the error ellipsoid E1 as the error ellipsoid E0, and jump to execute Step 4); if the axis length L1 < δ, output the position information of the current assisted positioning.

[0012] As a further optimization scheme of a navigation information source group optimization interaction method for cross-domain cooperative navigation according to the present invention, the aircraft cluster described in Step A.1) includes an aircraft to be assisted in positioning and n reference information source aircraft, and the self-positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; when obtaining the system measurement data required for the positioning and navigation of the aircraft cluster, first obtain the navigation system information, longitude-latitude-altitude data and satellite navigation positioning accuracy of the n reference information source aircraft, and then obtain the GPS and inertial navigation combined navigation data, attitude angle data, Kalman filter covariance matrix, longitude-latitude-altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning.

[0013] As a further optimization scheme of a navigation information source group optimization interaction method for cross-domain cooperative navigation according to the present invention, Step A.2) includes the following specific steps:

[0014] Step A.2.1), obtain the covariance matrix P of the position using the covariance matrix of the Kalman filter pos , defined as follows:

[0015]

[0016]

[0017] In the formula, E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error;

[0018] Step A.2.2), convert the position estimation error of the integrated navigation system in the longitude error σ λ and the latitude error altitude error σ β into a linear error (x, y, z):

[0019]

[0020] In the formula, R is the radius of the earth, is the latitude where the aircraft is located;

[0021] Step A.2.3), according to the converted position error vector E p , further obtain the position error covariance matrix P p

[0022] E p = [x y z]

[0023]

[0024] Step A.2.4), find the eigenvalues λ p of the symmetric covariance matrix P 1 , λ 2 and λ 3 , and further take the square root to obtain the corresponding axis lengths of the ellipsoid:

[0025]

[0026] where axis x , axis y , axis z are the axis lengths in the X-axis, Y-axis, and Z-axis directions of the ellipsoid E0 respectively;

[0027]

[0028] As a further optimization scheme of a navigation information source group optimization interaction method for cross-domain cooperative navigation according to the present invention, the step A.3) includes the following specific steps:

[0029] Step A.3.1), obtain the relative distances between each reference information source aircraft and the aircraft to be assisted in positioning;

[0030] Step A.3.2), convert the longitude, latitude, and altitude coordinates of all aircraft into earth coordinate system coordinates, R N is the radius of curvature of the prime vertical of the earth, f is the flattening of the earth, λ i is the longitude of the i-th reference information source aircraft, L i is the latitude of the i-th reference information source aircraft, H i is the altitude of the reference information source aircraft, is the earth coordinate system coordinate of the i-th reference information source aircraft after conversion, and the conversion formula is as follows:

[0031]

[0032] A relative coordinate system is established with the aircraft to be assisted as the coordinate origin. The axes of the relative coordinate system are parallel to those of the ECEF coordinate system. The X-axis lies in the equatorial plane and intersects the zero meridian, the Z-axis is parallel to the Earth's axis of rotation, and the Y-axis, together with the X-axis and Z-axis, forms a right-handed rectangular coordinate system. The position coordinates of each reference information source in the relative coordinate system are transformed as follows:

[0033]

[0034] Where, is the position coordinate of the i-th reference signal source aircraft in the relative coordinate system after transformation, and (x e , y e , z e ) are the coordinates of the aircraft to be assisted in the Earth coordinate system;

[0035] The above relative coordinate system is transformed into a relative body coordinate system with the aircraft to be assisted as the coordinate origin through homogeneous coordinate transformation, is the relative body coordinate system with the aircraft to be assisted as the coordinate origin after transformation; the transformation formula from the relative coordinate system to the relative body coordinate system is as follows:

[0036]

[0037] θ and ψ are the roll angle, pitch angle, and heading angle of the aircraft to be assisted respectively; is the matrix for transforming the body coordinate system into the geographic coordinate system, specifically as follows:

[0038]

[0039] is the matrix for transforming the geographic coordinate system into the Earth coordinate system, specifically as follows:

[0040]

[0041] λ, L, and H are the longitude, latitude, and altitude of the aircraft to be assisted respectively, and R N is the radius of the prime vertical, and e is the eccentricity of the Earth;

[0042] The coordinates of each reference information source aircraft in the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin are obtained:

[0043]

[0044]

[0045]

[0046] α iis the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ i is the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, is the position coordinate of the reference information source aircraft in the relative body coordinate system.

[0047] As a further optimization scheme of a navigation information source group optimization interaction method for cross-domain cooperative navigation according to the present invention, step A.4) includes the following specific steps:

[0048] Step A.4.1), compare λ 1 、λ 2 and λ 3 to determine the longest axis of the error ellipsoid E0. According to the angles α i 、β i 、γ i between each reference signal source and each axis of the relative body coordinate system with the aircraft to be assisted as the coordinate origin, select the reference information source aircraft closest to the direction of the longest axis of the ellipsoid E0 as the signal source aircraft;

[0049] Step A.4.2), obtain the relative distance d i between the aircraft to be assisted and the selected signal source aircraft L i , and the relative azimuth information between the aircraft to be assisted and the signal source aircraft L i , where is the relative line-of-sight elevation angle, and θ i is the relative azimuth;

[0050] Decompose the relative distance:

[0051]

[0052] Among them, is the decomposition of the relative distance d i in the directions of the X-axis, Y-axis, and Z-axis of the aircraft to be assisted body coordinate system;

[0053] Subtract the position coordinate i of the reference signal source aircraft L in the relative body coordinate system from respectively to obtain the coordinates (x', y', z') of the aircraft to be assisted after auxiliary positioning calculation:

[0054]

[0055] In step A.4.3), calculate the difference between the position coordinates (x', y', z') of the aircraft to be assisted after assisted positioning solution and the position coordinates (x, y, z) of the on-board sensor, and use it as the observation quantity. Then, correct the navigation position error of the aircraft to be assisted through Kalman filtering. After correction, obtain a new Kalman filter covariance matrix, and calculate a new ellipsoid E1 based on the new covariance matrix.

[0056] The present invention also discloses another method for optimizing the interaction of navigation information sources for cross-domain cooperative navigation, including the following steps:

[0057] Step B.1), obtain the sensor measurement data required for the positioning and navigation of the aircraft cluster;

[0058] Step B.2), calculate the error ellipsoid E0 of the current position according to the Kalman filter covariance matrix of the aircraft to be assisted;

[0059] Step B.3), obtain the relative azimuth and relative distance between each reference information source aircraft and the aircraft to be assisted;

[0060] Step B.4), compare the axis lengths of the three directions of the error ellipsoid E0, and select two reference information source aircraft with the smallest included angle with the direction of the maximum axis length as the main signal source aircraft. Then, in the remaining two axis length directions, select one reference information source aircraft with the smallest included angle with the corresponding axis length direction as the secondary signal source; when there are several reference information source aircraft with similar included angles with the same axis length direction, preferentially select the reference information source aircraft with a high positioning accuracy factor of its own as the signal source aircraft;

[0061] Step B.5), select any one of the main signal sources as the main reference information source, establish a relative coordinate system with the main reference information source as the coordinate origin, obtain the position coordinates and distance difference data of the remaining signal source aircraft in the relative coordinate system, and then according to the TDOA model, select the spherical interpolation method for solution to obtain the relative coordinate system solution result and solution error, and correct the position coordinates of the aircraft to be assisted through Kalman filtering.

[0062] As a further optimization scheme of the above-mentioned another method for optimizing the interaction of navigation information sources for cross-domain cooperative navigation, the aircraft cluster described in step B.1) includes an aircraft to be assisted in positioning and n reference information source aircraft, and the self-positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; when obtaining the system measurement data required for the positioning and navigation of the aircraft cluster, first obtain the navigation system information, longitude, latitude and altitude data, and satellite navigation positioning accuracy of the n reference information source aircraft, and then obtain the GPS and inertial navigation combined navigation data, attitude angle data, Kalman filter covariance matrix, longitude, latitude and altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning.

[0063] As a further optimization solution of the navigation information source group optimization interaction method for cross - domain collaborative navigation, step B.2) includes the following specific steps:

[0064] Step B.2.1), obtain the covariance matrix P of the position using the covariance matrix of the Kalman filter pos , defined as follows:

[0065]

[0066]

[0067] In the formula, E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error;

[0068] Step B.2.2), convert the position estimation error of the integrated navigation system in the longitude error σ λ and latitude error altitude error σ β into a straight - line error (x, y, z):

[0069]

[0070] In the formula, R is the radius of the earth, is the latitude where the aircraft is located;

[0071] Step B.2.3), according to the converted position error vector E p , further obtain the position error covariance matrix P p

[0072] E p = [x y z]

[0073]

[0074] Step B.2.4), find the eigenvalues λ p of the symmetric covariance matrix P 1 , λ 2 and λ 3 , and further take the square root to obtain the corresponding axis lengths of the ellipsoid:

[0075]

[0076] where, axis x , axis y , axis z are the axis lengths in the X - axis, Y - axis, and Z - axis directions of the ellipsoid E0 respectively;

[0077]

[0078] As a further optimization solution of the navigation information source group optimization interaction method for cross - domain collaborative navigation, step B.3) includes the following specific steps:

[0079] Step B.3.1), obtain the relative distance between each reference information source aircraft and the aircraft to be assisted in positioning;

[0080] Step B.3.2), convert the longitude, latitude and altitude coordinates of all aircraft into coordinates in the Earth coordinate system. R N is the radius of curvature of the Earth's prime vertical circle, f is the flattening of the Earth, and λ i is the longitude of the i - th reference information source aircraft, L i is the latitude of the i - th reference information source aircraft, H i is the altitude of the reference information source aircraft, and is the coordinate of the i - th reference information source aircraft in the Earth coordinate system after conversion. The conversion formula is as follows:

[0081]

[0082] Establish a relative coordinate system with the aircraft to be assisted as the coordinate origin. The coordinate axes of the relative coordinate system are parallel to the coordinate axes of the ECEF coordinate system. The X - axis intersects the zero - degree meridian in the equatorial plane, the Z - axis is parallel to the Earth's axis of rotation, and the Y - axis forms a right - hand rectangular coordinate system with the X - axis and Z - axis. The position coordinates of each reference information source in the relative coordinate system are converted as follows:

[0083]

[0084] In the formula, is the position coordinate of the i - th reference signal source aircraft in the relative coordinate system after conversion, and (x e , y e , z e ) is the coordinate of the aircraft to be assisted in the Earth coordinate system;

[0085] Convert the above - mentioned relative coordinate system into a relative body coordinate system with the aircraft to be assisted as the coordinate origin through homogeneous coordinate transformation, is the relative body coordinate system with the aircraft to be assisted as the coordinate origin after conversion. The conversion formula from the relative coordinate system to the relative body coordinate system is as follows:

[0086]

[0087] θ and ψ are the roll angle, pitch angle and heading angle of the aircraft to be assisted respectively; It is the matrix for converting the body coordinate system to the geographic coordinate system, which is specifically as follows:

[0088]

[0089] It is the matrix for converting the geographic coordinate system to the earth coordinate system, which is specifically as follows:

[0090]

[0091] λ, L, and H are respectively the longitude, latitude, and altitude of the aircraft to be assisted, and R N is the radius of the prime vertical, and e is the eccentricity of the earth;

[0092] The coordinates of each reference information source aircraft in the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin are obtained:

[0093]

[0094]

[0095]

[0096] α i is the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ i is the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, is the position coordinate of the reference information source aircraft in the relative body coordinate system.

[0097] Compared with the prior art by adopting the above technical solutions, the present invention has the following technical effects:

[0098] The present invention discloses a navigation information source group optimization interaction method for cross-domain cooperative navigation, which optimizes the selection of cooperative positioning sources between the carrier to be assisted and the reference information sources, making the ranging and information interaction selection between the carrier to be assisted and the reference information sources more reasonable; considering the influence of the configuration and positioning accuracy of the navigation signal sources on the positioning result, the comprehensive utilization of cluster positioning information optimizes and improves the positioning accuracy. Compared with the non-optimized cluster aircraft positioning algorithm, the present invention can effectively improve the cooperative positioning efficiency and accuracy in an irregularly distributed cluster and is suitable for practical applications. Brief Description of the Drawings

[0099] Figure 1 It is the schematic diagram of the principle flow of the method of the present invention;

[0100] Figure 2 It is the overall diagram of the position error ellipse E0;

[0101] Figure 3 It is the cross-sectional view of the position error ellipsoid E0;

[0102] Figure 4 It is the longitudinal sectional view of the position error ellipsoid E0;

[0103] Figure 5 Distribution map of the positions of the cluster aircraft;

[0104] Figure 6 It is the comparison diagram of the longitude, latitude, and altitude positioning errors of the aircraft to be assisted before and after optimization using the method of the present invention;

[0105] Figure 7 It is the change diagram of the axis lengths of the position error ellipsoid of the aircraft to be assisted;

[0106] Figure 8 It is the change diagram of the numbers of the selected signal source aircraft. Detailed implementation manners

[0107] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

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

[0109] The method of the present invention optimizes the selection of the cooperative positioning signal source of the aircraft to be assisted in the cluster aircraft, making the ranging and information interaction selection between the aircraft to be assisted and the reference information source more accurate and reasonable, thereby comprehensively using the cluster positioning information to optimize and improve the positioning accuracy, and at the same time enhancing the adaptability of the cluster aircraft positioning algorithm to various cluster formation situations.

[0110] As Figure 1 shown, the present invention discloses a navigation information source group optimization interaction method for cross-domain cooperative navigation, including the following steps:

[0111] Step A.1), obtaining the system data required for the positioning and navigation of the aircraft cluster;

[0112] Step A.2), as Figure 2 , Figure 3 , Figure 4 shown, calculating the error ellipsoid E0 of the current position according to the filtering covariance matrix of the aircraft to be assisted;

[0113] Step A.3), obtaining the relative azimuth and relative distance between each reference information source aircraft and the aircraft to be assisted;

[0114] Step A.4), obtain the direction where the maximum axis length of the error ellipsoid E0 is located and the self-accuracy of the signal source, select a reference information source aircraft closest to the direction of the maximum axis length as the reference signal source aircraft, perform assisted positioning calculation and correct the original position to obtain a new error ellipsoid E1;

[0115] Step A.5), determine whether the axis length L1 of the longest axis of the error ellipsoid E1 satisfies the preset threshold δ. If the axis length L1 > δ, let E0 = E1, that is, use the error ellipsoid E1 as the error ellipsoid E0, and jump to execute Step 4); if the axis length L1 < δ, output the position information of the current assisted positioning.

[0116] The aircraft cluster described in Step A.1) includes the aircraft to be assisted in positioning and n reference information source aircraft, and the self-positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; when obtaining the system measurement data required for the positioning and navigation of the aircraft cluster, as Figure 5 shown, first obtain the navigation system information, longitude, latitude and altitude data, and satellite navigation positioning accuracy of n reference information source aircraft, and then obtain the GPS and inertial navigation combined navigation data, attitude angle data, Kalman filter covariance matrix, longitude, latitude and altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning.

[0117] The said Step A.2) includes the following specific steps:

[0118] Step A.2.1), obtain the covariance matrix P of the position by using the covariance matrix of the Kalman filter pos , defined as follows:

[0119]

[0120]

[0121] In the formula, E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error;

[0122] Step A.2.2), convert the position estimation error of the integrated navigation system in the longitude error σ λ and latitude error altitude error σ β into a linear error (x, y, z):

[0123]

[0124] In the formula, R is the radius of the earth, is the latitude where the aircraft is located;

[0125] In step A.2.3), according to the converted position error vector E p , further obtain the position error covariance matrix P p

[0126] E p = [x y z]

[0127]

[0128] In step A.2.4), find the eigenvalues λ p of the symmetric covariance matrix P 1 、λ 2 and λ 3 , and further take the square root to obtain the corresponding axis lengths of the ellipsoid:

[0129]

[0130] where axis x 、axis y 、axis z are the axis lengths in the X-axis, Y-axis, and Z-axis directions of the ellipsoid E0 respectively;

[0131]

[0132] The said step A.3) includes the following specific steps:

[0133] In step A.3.1), obtain the relative distance between each reference information source aircraft and the aircraft to be assisted in positioning;

[0134] In step A.3.2), convert the longitude, latitude, and altitude coordinates of all aircraft into Earth coordinate system coordinates. R N is the radius of curvature of the prime vertical of the Earth, f is the flattening of the Earth, λ i is the longitude of the i-th reference information source aircraft, L i is the latitude of the i-th reference information source aircraft, H i is the altitude of the reference information source aircraft, is the Earth coordinate system coordinate of the i-th reference information source aircraft after conversion, and the conversion formula is as follows:

[0135]

[0136] Establish a relative coordinate system with the aircraft to be assisted as the coordinate origin. The coordinate axes of the relative coordinate system are parallel to the coordinate axes of the ECEF coordinate system. The X-axis intersects the zero meridian in the equatorial plane, the Z-axis is parallel to the Earth's axis of rotation, and the Y-axis forms a right-handed rectangular coordinate system with the X-axis and Z-axis. The position coordinates of each reference information source in the relative coordinate system are converted as follows:

[0137]

[0138] Wherein, is the position coordinate of the i-th reference signal source aircraft after conversion in the relative coordinate system, and (x e , y e , z e ) is the coordinate of the aircraft to be assisted in the Earth coordinate system;

[0139] Convert the above relative coordinate system into a relative body coordinate system with the aircraft to be assisted as the coordinate origin through homogeneous coordinate transformation, is the relative body coordinate system with the aircraft to be assisted as the coordinate origin after conversion; the conversion formula from the relative coordinate system to the relative body coordinate system is as follows:

[0140]

[0141] θ and ψ are the roll angle, pitch angle, and heading angle of the aircraft to be assisted respectively; is the matrix for converting the body coordinate system to the geographic coordinate system, specifically as follows:

[0142]

[0143] Wherein, is the matrix for converting the geographic coordinate system to the Earth coordinate system, specifically as follows:

[0144]

[0145] λ, L, and H are the longitude, latitude, and altitude of the aircraft to be assisted respectively, and R N is the radius of the prime vertical, and e is the eccentricity of the Earth;

[0146] Obtain the coordinates of each reference information source aircraft in the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin:

[0147]

[0148]

[0149]

[0150] α i is the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ iis the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, is the position coordinate of the reference information source aircraft in the relative body coordinate system.

[0151] The step A.4) includes the following specific steps:

[0152] Step A.4.1), compare λ 1 、λ 2 and λ 3 to determine the longest axis of the error ellipsoid E0; according to the angles α i 、β i 、γ i between each reference signal source and each axis of the relative body coordinate system with the aircraft to be assisted as the coordinate origin, select the reference information source aircraft closest to the direction of the longest axis of the ellipsoid E0 as the signal source aircraft;

[0153] Step A.4.2), obtain the relative distance d i between the aircraft to be assisted and the selected signal source aircraft L i , and the relative azimuth information between the aircraft to be assisted and the signal source aircraft L i , where is the relative line-of-sight elevation angle, and θ i is the relative azimuth.

[0154] Decompose the relative distance,

[0155]

[0156] where, is the decomposition of the relative distance d i in the directions of the X-axis, Y-axis, and Z-axis of the aircraft to be assisted body coordinate system.

[0157] Subtract the position coordinate i of the reference signal source aircraft L in the relative body coordinate system from respectively to obtain the coordinates (x', y', z') of the aircraft to be assisted after auxiliary positioning calculation.

[0158]

[0159] Step A.4.3), calculate the difference between the position coordinates (x', y', z') of the aircraft to be assisted after the assisted positioning solution and the position coordinates (x, y, z) of the on-board sensor, and use it as the observation quantity. Then, use the Kalman filter to correct the navigation position error of the aircraft to be assisted. After correction, a new Kalman filter covariance matrix is obtained, and a new ellipsoid E1 is calculated according to the new covariance matrix.

[0160] The present invention also discloses another navigation information source group optimization interaction method for cross-domain cooperative navigation, including the following steps:

[0161] Step B.1), obtain the sensor measurement data required for the positioning and navigation of the aircraft cluster;

[0162] Step B.2), calculate the error ellipsoid E0 of the current position according to the Kalman filter covariance matrix of the aircraft to be assisted;

[0163] Step B.3), obtain the relative azimuth and relative distance between each reference information source aircraft and the aircraft to be assisted;

[0164] Step B.4), compare the axis lengths of the three directions of the error ellipsoid E0, and select the two reference information source aircraft with the smallest included angle with the direction of the maximum axis length as the main signal source aircraft. Then, in the remaining two axis length directions, select one reference information source aircraft with the smallest included angle with the corresponding axis length direction as the secondary signal source; when there are several reference information source aircraft with similar included angles with the same axis length direction, preferentially select the reference information source aircraft with a high positioning accuracy factor of itself as the signal source aircraft;

[0165] Step B.5), select any one of the main signal sources as the main reference information source, establish a relative coordinate system with the main reference information source as the coordinate origin, obtain the position coordinates and distance difference data of the remaining signal source aircraft in the relative coordinate system, and then according to the TDOA model, select the spherical interpolation method for calculation to obtain the relative coordinate system calculation result and calculation error, and correct the position coordinates of the aircraft to be assisted through the Kalman filter.

[0166] The aircraft cluster described in Step B.1) includes an aircraft to be assisted in positioning and n reference information source aircraft, and the self-positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; when obtaining the system measurement data required for the positioning and navigation of the aircraft cluster, first obtain the navigation system information, longitude, latitude and altitude data, and satellite navigation positioning accuracy of the n reference information source aircraft, and then obtain the GPS and inertial navigation combined navigation data, attitude angle data, Kalman filter covariance matrix, longitude, latitude and altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning.

[0167] The said Step B.2) includes the following specific steps:

[0168] In step B.2.1), obtain the covariance matrix P of the position using the covariance matrix of the Kalman filter pos , which is defined as follows:

[0169]

[0170]

[0171] In the formula, E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error;

[0172] In step B.2.2), convert the position estimation error of the integrated navigation system in the longitude error σ λ and latitude error altitude error σ β into the linear error (x, y, z):

[0173]

[0174] In the formula, R is the radius of the earth, is the latitude where the aircraft is located;

[0175] In step B.2.3), further obtain the position error covariance matrix P according to the converted position error vector E p p

[0176] E p = [x y z]

[0177]

[0178] In step B.2.4), find the eigenvalues λ p 、λ 1 、λ 2 and λ 3 of the symmetric covariance matrix P, and further take the square root to obtain the corresponding axis lengths of the ellipsoid:

[0179]

[0180] where axis x 、axis y 、axis z are the axis lengths in the X-axis, Y-axis, and Z-axis directions of the ellipsoid E0, respectively;

[0181]

[0182] ​Step B.3) includes the following specific steps:

[0183] Step B.3.1), obtaining the relative distance between each reference information source aircraft and the aircraft to be assisted in positioning;

[0184] Step B.3.2), converting the longitude, latitude and altitude coordinates of all aircraft into coordinates in the Earth coordinate system, where R N is the radius of curvature of the Earth's prime vertical, f is the flattening of the Earth, and λ i is the longitude of the i-th reference information source aircraft, and L i is the latitude of the i-th reference information source aircraft, and H i is the altitude of the reference information source aircraft, and is the coordinate of the i-th reference information source aircraft in the Earth coordinate system after conversion. The conversion formula is as follows:

[0185]

[0186] Establish a relative coordinate system with the aircraft to be assisted as the coordinate origin. The coordinate axes of the relative coordinate system are parallel to the coordinate axes of the ECEF coordinate system. The X-axis intersects the zero meridian in the equatorial plane, the Z-axis is parallel to the Earth's axis of rotation, and the Y-axis forms a right-handed rectangular coordinate system with the X-axis and Z-axis. The position coordinates of each reference information source in the relative coordinate system are converted as follows:

[0187]

[0188] In the formula, is the position coordinate of the i-th reference signal source aircraft in the relative coordinate system after conversion, and (x e , y e , z e ) is the coordinate of the aircraft to be assisted in the Earth coordinate system;

[0189] Convert the above relative coordinate system into a relative body coordinate system with the aircraft to be assisted as the coordinate origin through homogeneous coordinate transformation, which is the relative body coordinate system with the aircraft to be assisted as the coordinate origin after conversion; the conversion formula from the relative coordinate system to the relative body coordinate system is as follows:

[0190]

[0191] θ and ψ are the roll angle, pitch angle and heading angle of the aircraft to be assisted respectively; is the matrix for converting the body coordinate system to the geographic coordinate system, which is specifically as follows:

[0192]

[0193] The matrix for converting the geographic coordinate system to the earth coordinate system is as follows:

[0194]

[0195] λ, L, and H are the longitude, latitude, and altitude of the aircraft to be assisted, respectively. R N is the radius of the prime vertical, and e is the eccentricity of the earth;

[0196] Obtain the coordinates of each reference information source aircraft in the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin:

[0197]

[0198]

[0199]

[0200] α i is the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ i is the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, are the position coordinates of the reference information source aircraft in the relative body coordinate system.

[0201] In order to verify the effectiveness of a navigation information source group optimization interaction method for cross-domain cooperative navigation proposed by the present invention, simulation analysis is carried out. As Figure 6 shown, the simulation results show that after the signal source selection is optimized, the cooperative positioning accuracy of the cluster aircraft is basically better than that of randomly selecting signal sources for positioning. As Figure 7 shown, it is the change of the axis length of the position error ellipsoid of the aircraft to be assisted; as Figure 8 shown, it is the change of the selected signal source aircraft numbers.

[0202] Those skilled in the art of this technology can understand that, unless otherwise defined, all terms (including technical terms and scientific terms) used here have the same meaning as the general understanding of those of ordinary skill in the field to which the present invention belongs. It should also be understood that terms such as those defined in a general dictionary should be understood to have a meaning consistent with the meaning in the context of the prior art, and will not be interpreted with an idealized or overly formal meaning unless defined as here.

[0203] The specific embodiments described above further elaborate on the object, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only the specific embodiments of the present invention and is not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A method for optimizing the interactive operation of a navigation information source group for cross - domain collaborative navigation, characterized in that, it includes the following steps: Step A.1), obtaining the system data required for the positioning and navigation of the aircraft cluster; The aircraft cluster includes an aircraft to be assisted in positioning and n reference information source aircraft. The self - positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; When obtaining the system measurement data required for the positioning and navigation of the aircraft cluster, first obtain the navigation system information, longitude - latitude - altitude data, and satellite navigation positioning accuracy of the n reference information source aircraft, and then obtain the GPS and inertial navigation combined navigation data, attitude angle data, Kalman filter covariance matrix, longitude - latitude - altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning; Step A.2), calculating the error ellipsoid E0 of the current position according to the filter covariance matrix of the aircraft to be assisted; Step A.2.1), obtaining the covariance matrix P of the position using the covariance matrix of the Kalman filter pos , which is defined as follows: where E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error; Step A.2.2), convert the position estimation error of the integrated navigation system into the longitude error σ λ and the latitude error altitude error σ β into the linear error (x, y, z): where R is the radius of the earth, is the latitude of the aircraft; Step A.2.3), according to the converted position error vector E p , further obtain the position error covariance matrix P p E p = [x y z] Step A.2.4), find the symmetric covariance matrix P p for the eigenvalues λ 1 , λ 2 and λ 3 , and further take the square root to obtain the corresponding axis lengths of the ellipsoid: Among them, axis x 、axis y 、axis z are the axis lengths in the X-axis, Y-axis, and Z-axis directions of the ellipsoid E0, respectively; Step A.3), obtaining the relative azimuth and relative distance between each reference information source aircraft and the aircraft to be assisted; Step A.3.1), obtaining the relative distance between each reference information source aircraft and the aircraft to be assisted in positioning; Step A.3.2), convert the longitude, latitude and altitude coordinates of all aircraft into coordinates in the Earth coordinate system, where R N is the radius of curvature of the prime vertical of the Earth, f is the flattening of the Earth, and λ i is the longitude of the i-th reference information source aircraft, L i is the latitude of the i-th reference information source aircraft, H i is the altitude of the reference information source aircraft, and is the coordinate of the i-th reference information source aircraft in the Earth coordinate system after conversion. The conversion formula is as follows: Establish a relative coordinate system with the aircraft to be assisted as the coordinate origin. The coordinate axes of the relative coordinate system are parallel to the coordinate axes of the ECEF coordinate system. The X - axis intersects the zero - degree meridian in the equatorial plane, the Z - axis is parallel to the earth's rotation axis, and the Y - axis forms a right - hand rectangular coordinate system with the X - axis and the Z - axis. The position coordinates of each reference information source in the relative coordinate system are transformed as follows: In the formula, is the position coordinate of the i-th reference signal source aircraft in the relative coordinate system after conversion, and (x e , y e , z e ) is the coordinate of the aircraft to be assisted in the Earth coordinate system; Convert the above relative coordinate system into a relative body coordinate system with the auxiliary aircraft as the coordinate origin through homogeneous coordinate transformation. It is the relative body coordinate system with the auxiliary aircraft as the coordinate origin after conversion; the conversion formula from the relative coordinate system to the relative body coordinate system is as follows: θ and ψ are the roll angle, pitch angle, and heading angle of the aircraft to be assisted, respectively; is the matrix for converting the body coordinate system to the geographic coordinate system, specifically as follows: It is the matrix for converting the geographic coordinate system to the earth coordinate system, specifically as follows: λ, L, and H are the longitude, latitude, and altitude of the aircraft to be assisted, respectively, and R N is the radius of the prime vertical, and e is the eccentricity of the Earth; Obtaining the coordinates of each reference information source aircraft in the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin: α i is the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ i is the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, is the position coordinate of the reference information source aircraft in the relative body coordinate system; Step A.4), obtaining the direction of the maximum axis length of the error ellipsoid E0 and the self - accuracy of the signal source, selecting a reference signal source aircraft closest to the direction of the maximum axis length for assisted positioning calculation and correcting the original position to obtain a new error ellipsoid E1; Step A.4.1), compare λ 1 , λ 2 and λ 3 to determine the longest axis of the error ellipsoid E0. According to the angles α i , β i , γ i between each reference signal source and each axis of the relative body coordinate system with the aircraft to be assisted as the coordinate origin, select the reference information source aircraft closest to the direction of the longest axis of the ellipsoid E0 as the signal source aircraft; Step A.4.2), obtain the relative distance d between the aircraft to be assisted and the selected signal source aircraft L i and the relative azimuth information between the aircraft to be assisted and the signal source aircraft L i , where i is the relative line-of-sight elevation angle, and θ i is the relative azimuth angle; Decomposing the relative distance: Among them, is the relative distance d i resolved in the directions of the X-axis, Y-axis, and Z-axis of the body coordinate system of the aircraft to be assisted; will be the reference signal source aircraft L i Position coordinates in the relative body coordinate system Subtract respectively Obtain the coordinates (x′, y′, z′) of the aircraft to be assisted after assisted positioning calculation: Step A.4.3), calculating the difference between the position coordinates (x′, y′, z′) of the aircraft to be assisted after the assisted positioning calculation and the position coordinates (x, y, z) of the on - board sensor, and using it as the observation quantity to correct the navigation position error of the aircraft to be assisted through Kalman filtering. After correction, a new Kalman filter covariance matrix is obtained, and a new ellipsoid E1 is calculated according to the new covariance matrix; Step A.5), judging whether the axis length L1 of the longest axis of the error ellipsoid E1 satisfies the preset threshold δ. If the axis length L1 > δ, let E0 = E1, that is, take the error ellipsoid E1 as the error ellipsoid E0, and jump to execute step 4); if the axis length L1 < δ, output the position information of the current assisted positioning.

2. A method for optimizing the interactive operation of a navigation information source group for cross - domain collaborative navigation, characterized in that, it includes the following steps: Step B.1), obtaining the sensor measurement data required for the positioning and navigation of the aircraft cluster; The aircraft cluster includes an aircraft to be assisted in positioning and n reference information source aircraft. The self - positioning accuracy of the reference information source aircraft is higher than that of the aircraft to be assisted in positioning; When obtaining the system measurement data required for the positioning and navigation of an aircraft cluster, first obtain the navigation system information, longitude-latitude-altitude data, and satellite navigation positioning accuracy of n reference information source aircraft, and then obtain the GPS and inertial navigation integrated navigation data, attitude angle data, Kalman filter covariance matrix, longitude-latitude-altitude data, latitude error, longitude error, and altitude error of the aircraft to be assisted in positioning; In step B.2), calculate the error ellipsoid E0 of the current position according to the Kalman filter covariance matrix of the aircraft to be assisted; Step B.2.1), obtain the covariance matrix P of the position using the covariance matrix of the Kalman filter pos , which is defined as follows: where, E pos is the position error vector matrix, is the latitude error, σ λ is the longitude error, σ β is the altitude error; Step B.2.2), convert the position estimation error of the integrated navigation system, including the longitude error σ λ and the latitude error and the altitude error σ β into a linear error (x, y, z): where R is the radius of the Earth, is the latitude where the aircraft is located; Step B.2.3), according to the converted position error vector E p , further obtain the position error covariance matrix P p E p = [x y z] Step B.2.4), find the eigenvalues λ p of the symmetric covariance matrix P 1 , λ 2 and λ 3 , and further take the square root to obtain the corresponding axis lengths of the ellipsoid: where axis x , axis y , and axis z are the lengths of the axes of the ellipsoid E0 in the X-axis, Y-axis, and Z-axis directions, respectively; In step B.3), obtain the relative azimuth angle and relative distance between each reference information source aircraft and the aircraft to be assisted; In step B.3.1), obtain the relative distance between each reference information source aircraft and the aircraft to be assisted in positioning; Step B.3.2), convert the longitude, latitude, and altitude coordinates of all aircraft into Earth coordinate system coordinates, where R N is the radius of curvature of the Earth's prime vertical, f is the flattening of the Earth, and λ i is the longitude of the i-th reference information source aircraft, L i is the latitude of the i-th reference information source aircraft, H i is the altitude of the reference information source aircraft, is the Earth coordinate system coordinate of the i-th reference information source aircraft after conversion, and the conversion formula is as follows: Establish a relative coordinate system with the aircraft to be assisted as the coordinate origin. The coordinate axes of the relative coordinate system are parallel to the coordinate axes of the ECEF coordinate system. The X-axis intersects the zero meridian in the equatorial plane, the Z-axis is parallel to the earth's axis of rotation, and the Y-axis forms a right-handed rectangular coordinate system with the X-axis and Z-axis. The position coordinates of each reference information source in the relative coordinate system are converted as follows: In the formula, is the position coordinate of the i-th reference signal source aircraft in the relative coordinate system after conversion, (x e , y e , z e 0 is the coordinate of the aircraft to be assisted in the Earth coordinate system; Convert the above relative coordinate system into a relative body coordinate system with the auxiliary aircraft as the coordinate origin through homogeneous coordinate transformation. It is the relative body coordinate system with the auxiliary aircraft as the coordinate origin after conversion; the conversion formula from the relative coordinate system to the relative body coordinate system is as follows: θ and ψ are respectively the roll angle, pitch angle, and heading angle of the aircraft to be assisted; is the matrix for converting the body coordinate system to the geographic coordinate system, specifically as follows: The matrix for converting the geographic coordinate system to the Earth coordinate system is as follows: λ, L, and H are the longitude, latitude, and altitude of the aircraft to be assisted, respectively, and R N is the radius of the prime vertical, and e is the eccentricity of the earth; Obtain the coordinates of each reference information source aircraft of the cluster system on the relative body coordinate system with the aircraft to be assisted as the coordinate origin; α i is the angle between each reference information source aircraft and the Y-axis of the relative body coordinate system; β i is the angle between each reference information source aircraft and the Z-axis of the relative body coordinate system; γ i is the angle between each reference information source aircraft and the X-axis of the relative body coordinate system; (x, y, z) are the coordinates of the aircraft to be assisted in the relative body coordinate system, are the position coordinates of the reference information source aircraft in the relative body coordinate system; In step B.4), compare the axis lengths of the three directions of the error ellipsoid E0, select the two reference information source aircraft with the smallest included angle with the direction of the maximum axis length as the main signal source aircraft, and then select one reference information source aircraft with the smallest included angle with the corresponding axis length direction in the remaining two axis length directions as the secondary signal source; when there are several reference information source aircraft with similar included angles with the same axis length direction, preferentially select the reference information source aircraft with a high positioning accuracy dilution of precision as the signal source aircraft; In step B.5), select any one of the main signal sources as the main reference information source, establish a relative coordinate system with the main reference information source as the coordinate origin, obtain the position coordinates and distance difference data of the remaining signal source aircraft in the relative coordinate system, and then select the spherical interpolation method for solution according to the TDOA model to obtain the solution result and solution error of the relative coordinate system, and correct the position coordinates of the aircraft to be assisted through Kalman filtering.

Images