A multi-UAV classified collaborative navigation method and system based on resonance inertial navigation

Abstract

The present invention relates to the technical field of cooperative navigation of unmanned aerial vehicles, and provides a method and system for classified cooperative navigation of multiple unmanned aerial vehicles based on resonant inertial navigation. The method includes classifying a group of unmanned aerial vehicles into a leader and a wingman in real time, establishing state equations and measurement equations of the leader and the wingman respectively according to the resonant inertial navigation output and relative navigation information of the unmanned aerial vehicle itself, constructing a cooperative navigation system model, and estimating and correcting the resonant inertial navigation error of the unmanned aerial vehicle through Kalman filtering. A cooperative navigation performance enhancement algorithm is further proposed for the wingman, and a dynamic optimization criterion for the leader is established to determine the communication leader of the wingman. A higher precision estimation of the inertial navigation error of the wingman is achieved through a federal Kalman filtering algorithm based on dynamic matrix information allocation. The present invention establishes a cooperative navigation model for multiple unmanned aerial vehicles, performs optimal estimation and compensation of the resonant inertial navigation error, improves the cooperative navigation accuracy of the unmanned aerial vehicle group, and improves the robustness of the cooperative navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of cooperative navigation of unmanned aerial vehicles (UAVs), and in particular to a method and system for classified cooperative navigation of multiple UAVs based on resonance inertial navigation. Background Art

[0002] With its unique low cost and flexibility, drones have been widely used in many fields and have played an irreplaceable role. They are also developing in the direction of clustering and coordination. Excellent and stable navigation performance is a prerequisite for drones to perform tasks, but in actual use, satellite denial caused by satellite signal obstruction or interference will make the existing drone navigation method that relies on satellite navigation ineffective.

[0003] The resonant inertial navigation system mainly outputs angular velocity information from the resonant gyroscope. The resonant gyroscope based on the Coriolis vibration principle is a new type of high-precision solid-state vibration gyroscope. It uses the precession effect of vibration standing waves to sense external angular velocity. It has the advantages of simple structure, short start-up time, and long-term drift stability. It is known as the sensor with the best cost, size, quality, and power in the navigation field.

[0004] The cooperative navigation structure of UAVs is mainly divided into master-slave and parallel. The master-slave structure uses a high-precision lead aircraft to improve the navigation accuracy of a low-precision wingman, but this method has low reliability. When the lead aircraft fails, the entire cooperative navigation system will be paralyzed; the parallel structure can improve the robustness of the system in the face of failures by sharing multiple navigation information between UAVs, but this method has limited improvement in navigation accuracy.

[0005] The filtering fusion algorithm of multi-source information can be divided into centralized fusion and distributed fusion. Under the framework of the classified cooperative navigation system, it is necessary to select an information fusion method that matches it. For the centralized fusion algorithm, because the real-time classification algorithm of the swarm makes it impossible for a fixed leader to exist within the drone swarm, the amount of observation information will change with the number of communication leaders within the cooperative navigation algorithm, that is, the information fusion system is highly dynamic. It can be seen that the centralized fusion algorithm with low flexibility and robustness is not applicable. For the distributed fusion algorithm, on the one hand, the highly dynamic cooperative system will greatly affect the correlation of the observation information. If two drones have performed cooperative navigation in the previous flight, or have performed cooperative navigation with another aircraft, then the correlation between the two aircraft is not zero, and will increase with the cooperation time, which makes the assumption that the mutual covariance is zero invalid because it is out of touch with reality; on the other hand, since the distributed fusion algorithm cannot calculate the mutual covariance between the sub-filters, it is impossible to obtain the global optimal estimate of the data to be obtained. Summary of the invention

[0006] The present invention aims to solve at least one of the technical problems existing in the related art. To this end, the present invention provides a multi-UAV classified collaborative navigation method and system based on resonant inertial navigation, which realizes collaborative navigation between UAVs by utilizing resonant inertial navigation with high precision and low power consumption and rich relative navigation information between UAVs, and solves the problem of significantly reduced navigation accuracy of UAV navigation methods relying on satellite navigation in satellite-denied environments from the perspective of information conservation, effectively improves the navigation accuracy of UAVs, and at the same time improves the robustness of collaborative navigation.

[0007] The present invention provides a multi-UAV classified collaborative navigation method based on resonance inertial navigation, comprising:

[0008] S1: According to whether the drone can receive the relative navigation information sent by the ground base station at the current moment, a classified navigation structure of the drone group is established, and the drone group is classified into the leader and wingman in real time;

[0009] S2: Establish the state equation based on the inertial navigation error of the UAV;

[0010] S3: construct a long-distance machine measurement equation based on the calculated value of the relative distance between the long-distance machine and the ground base station and the measured value of the relative distance, and establish a long-distance machine sub-model based on the state equation and the long-distance machine measurement equation;

[0011] S4: When the number of the communication leader is 1, the wingman measurement equation is constructed according to the calculated value and measured value of the relative distance between the wingman and the communication leader, and the wingman sub-model is established according to the state equation and the wingman measurement equation;

[0012] S5: Estimate and correct the inertial navigation error of the leader sub-model through Kalman filtering, and estimate and correct the inertial navigation error of the wingman sub-model through Kalman filtering.

[0013] S6: When the number of lead aircraft is 2 or more, the lead aircraft of the wingman is selected by the principle of dynamic optimization of lead aircraft, and a multi-UAV classified collaborative navigation model is established;

[0014] S7: The multi-UAV classification collaborative navigation model is estimated and corrected through the federated Kalman filter algorithm based on dynamic matrix information allocation.

[0015] Furthermore, the lead aircraft obtains the relative navigation information between the lead aircraft and the ground base station through the relative navigation equipment; the wingman obtains the collaborative navigation information of the communicating lead aircraft using the airborne data link and the relative navigation equipment.

[0016] Furthermore, the collaborative navigation model includes the leader sub-model and the wingman sub-model. In the navigation coordinate system, the state equation of the leader sub-model and the state equation of the wingman sub-model are established according to the inertial navigation error of the UAV.

[0017] Furthermore, the resonant inertial navigation error equation of the UAV is used as the state equation of the lead aircraft and the wingman, and the geographic coordinate system is selected as the navigation coordinate system of the UAV, then the state equation is:

[0018]

[0019] in, for The moment state error, for The state transfer matrix of the inertial guidance error at time, for The system noise drives the matrix at every moment. for The system noise sequence at each moment, for Moment state variables;

[0020] The state equation is transformed into the Earth-centered Earth-fixed coordinate system using the transformation matrix between the geographic coordinate system and the Earth-centered Earth-fixed coordinate system.

[0021] Furthermore, the measurement equation of the lead aircraft is established based on the relative navigation information between the lead aircraft and the ground base station. The calculated value of the relative distance is obtained using the inertial navigation output of the lead aircraft and the known anchor coordinates of the ground base station. The measured value comes from the laser radar relative navigation device. The observation value of the lead aircraft is the difference between the calculated value and the measured value of the relative distance between the UAV and the ground base station. In the Earth-centered Earth-fixed coordinate system, the measurement equation of the lead aircraft is established as:

[0022]

[0023] in, is the long-machine observation quantity, is the observation matrix of the lead aircraft, is the state estimate of the long machine, Observe the noise for the lead aircraft.

[0024] Furthermore, the measurement equation of the wingman is established through the collaborative navigation information of the wingman and the communication leader. The wingman observation is the difference between the calculated value and the measured value of the relative distance between the wingman and the communication leader. In the Earth-centered Earth-fixed coordinate system, the measurement equation of the wingman is established as:

[0025]

[0026] in, is the wingman observation quantity, is the wingman observation matrix, is the wingman state estimate, Observe noise for the wingman.

[0027] Furthermore, the wingman selects the communication leader from multiple leader aircraft within the communication range according to the dynamic optimization principle of the communication leader aircraft. If multiple communication leader aircraft participate in the collaborative navigation of the wingman, the federal Kalman filter algorithm based on dynamic matrix information allocation is used to estimate the inertial navigation error of the wingman.

[0028] Furthermore, the dynamic optimization principle of the communication leader includes quality optimization based on the relative distance between the leaders and quantity optimization by setting the upper limit of the number of communication leaders;

[0029] The qualities preferably include:

[0030] According to the range of the laser radar used Set the relative distance threshold between machines;

[0031] Calculate the The wingman's spatial position is close to the leader's gathering ,gather The number of long machines included is ;

[0032] Calculate the The wingman and the The relative distance between the rack length machines;

[0033] According to The wingman and the The relative distance between the rack length machines and the relative distance threshold between the machines are obtained. The wingman's spatial position is close to the leader's gathering ;

[0034] The quantity preferably includes:

[0035] Through the information fusion algorithm, Adding a communications leader to the wingman's 2nd communications leader brings about a navigation accuracy improvement of , the computational and communication load increases to , then the quantity measurement index It can be expressed as:

[0036]

[0037] like , then set the upper limit of the number of long-distance communication devices to ;

[0038] like ,but Otherwise, the communication leader is selected according to the principle of the closest distance between machines and the collection is obtained. , and order ,

[0039]

[0040] in, For the Wingman and Collection The first The relative distance between the communication terminals. No. Wingman and Collection The first The relative distance between the communication terminals. For the Wingman and Collection The first The relative distance between the communication terminals.

[0041] Furthermore, the federated Kalman filter algorithm based on dynamic matrix information allocation includes the design of dynamic matrix allocation coefficients, uses the estimated covariance matrix to set the information allocation coefficients, performs real-time information allocation coefficient calculations on each state variable of each filter, and dynamically allocates information matrix coefficients in the federated filter, giving each filter a different fusion weight to achieve optimal fusion of multi-source information.

[0042] Furthermore, the present invention also provides a multi-UAV classified collaborative navigation system based on resonant inertial navigation, which is used to execute any of the above-mentioned multi-UAV classified collaborative navigation methods based on resonant inertial navigation, including:

[0043] A classification module, wherein the classification module establishes a classified navigation structure for the drone group according to whether the drone can currently receive relative navigation information sent by the ground base station, and classifies the drone group into a leader and a wingman in real time;

[0044] A first building block, wherein the first building block establishes a state equation according to an inertial navigation error of the UAV;

[0045] A second construction module, wherein the second construction module constructs a long-machine measurement equation according to a calculated value of a relative distance between the long-machine and the ground base station and a measured value of the relative distance, and establishes a long-machine sub-model according to the state equation and the long-machine measurement equation;

[0046] A third construction module, wherein when the number of the communication leader is one, the third construction module is used to construct a wingman measurement equation according to the calculated value and the measured value of the relative distance between the wingman and the communication leader, and to establish a wingman sub-model according to the state equation and the wingman measurement equation;

[0047] a first estimation and correction module, wherein the first estimation and correction module estimates and corrects the inertial navigation error of the leader sub-model through Kalman filtering, and estimates and corrects the inertial navigation error of the wingman sub-model through Kalman filtering;

[0048] A fourth building module, wherein when the number of the communication leaders is more than 2, the fourth building module is used to select the communication leader of the wingman according to the dynamic optimization principle of the communication leader, and establish a multi-UAV classification collaborative navigation model;

[0049] The second estimation and correction module estimates and corrects the multi-UAV classification collaborative navigation model through a federal Kalman filter algorithm based on dynamic matrix information allocation.

[0050] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0051] The present invention provides a multi-UAV classified collaborative navigation method and system based on resonant inertial navigation, which utilizes the resonant inertial navigation with high precision and low power consumption and the rich relative navigation information between UAVs to perform collaborative navigation between UAVs. From the perspective of information conservation, it solves the problem that the navigation accuracy of UAVs relying on satellite navigation is significantly reduced in a satellite denied environment, and effectively improves the navigation accuracy of UAVs.

[0052] The classified collaborative navigation structure of the present invention classifies UAVs into leader aircraft and wingman aircraft in real time according to whether they can communicate with the ground base station at the current moment, which is more in line with reality and can improve the robustness of collaborative navigation while improving the navigation accuracy of UAVs.

[0053] The present invention proposes a collaborative navigation performance enhancement algorithm based on dynamic optimization of communication leader and dynamic matrix information allocation of federated filtering, which effectively improves the utilization rate of redundant relative navigation information and further improves the collaborative navigation accuracy of UAV swarms through optimal estimation and compensation of resonant inertial navigation errors.

[0054] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0056] Figure 1 It is a flow chart of a multi-UAV classified collaborative navigation method based on resonant inertial navigation provided by the present invention.

[0057] Figure 2 The present invention provides a classified collaborative navigation structure of a multi-UAV classified collaborative navigation method based on resonant inertial navigation.

[0058] Figure 3 It is a schematic diagram of the relative relationship between the lead aircraft and the ground base station in a multi-UAV classified collaborative navigation method based on resonant inertial navigation provided by the present invention.

[0059] Figure 4 This is a navigation error comparison diagram of individual UAVs under collaborative navigation in a multi-UAV classified collaborative navigation method based on resonant inertial navigation provided by the present invention.

[0060] Figure 5 The present invention provides a multi-UAV classified cooperative navigation method based on resonant inertial navigation, in which a wingman has a navigation error of cooperative navigation when two communication lead aircraft are present.

[0061] Figure 6 It is a structural schematic diagram of a multi-UAV classified collaborative navigation system based on resonant inertial navigation provided by the present invention.

[0062] Reference numerals:

[0063] 101. Classification module; 102. First construction module; 103. Second construction module; 104. Third construction module; 105. First estimation correction module; 106. Fourth construction module; 107. Second estimation correction module. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical scheme and advantages of the present invention clearer, the technical scheme in the present invention will be clearly and completely described below. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but cannot be used to limit the scope of the present invention.

[0065] In the description of the embodiments of the present invention, the terms "first", "second", and "third" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance. In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiments of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples without contradiction.

[0066] Combine the following Figures 1 to 6 The present invention describes a multi-UAV classified collaborative navigation method and system based on resonant inertial navigation.

[0067] like Figure 1 As shown, a multi-UAV classified collaborative navigation method based on resonant inertial navigation includes:

[0068] S1: According to whether the drone can receive the relative navigation information sent by the ground base station at the current moment, a classified navigation structure of the drone group is established, and the drone group is classified into the leader and wingman in real time;

[0069] like Figure 2 As shown in the figure, in the swarm UAV, each UAV is equipped with relative navigation equipment such as resonant inertial navigation system and laser rangefinder and goniometer of the same accuracy. The ground base station can also use radar to complete relative navigation with the UAV. The UAVs in the swarm can share and coordinate navigation information with each other through the airborne data link. Some UAVs can receive relative navigation information sent by the ground base station through radio equipment. Since each UAV in the swarm cannot always maintain navigation information communication with the ground base station, the UAV swarm is classified into leader and wingman in real time according to whether the UAV can receive the relative navigation information sent by the ground base station at the current moment; the leader obtains the relative navigation information between the leader and the ground base station through the relative navigation equipment; the wingman selects the communication leader from the multiple leader aircraft that may exist within the communication range according to the dynamic optimization principle of the communication leader, and obtains the collaborative navigation information of the communication leader by using the airborne data link and relative navigation equipment.

[0070] The collaborative navigation system model is established in the Earth-centered Earth-fixed coordinate system, including the leader sub-model and the wingman sub-model. The state equations of the leader sub-model and the wingman sub-model are established in their respective navigation coordinate systems according to the inertial navigation errors of the UAVs. The leader's measurement equation is established based on the relative navigation information between the leader and the ground base station, and the wingman's measurement equation is established through the collaborative navigation information of the wingman and the communicating leader. According to the established system model, the Kalman filter is used to correct the inertial navigation errors of the leader and the wingman respectively, thereby improving the navigation accuracy of the entire UAV group.

[0071] S2: Establish the state equation based on the inertial navigation error of the UAV;

[0072] The resonant inertial navigation error equation of the UAV is used as the state equation of the lead aircraft and the wingman, and the geographic coordinate system is selected as the navigation coordinate system of the UAV, then the state equation is:

[0073]

[0074] in, for The moment state error, for The state transfer matrix of the inertial guidance error at time, for The system noise drives the matrix at every moment. for The system noise sequence at each moment, for Moment state variables;

[0075] The state equation is transformed into the Earth-centered Earth-fixed coordinate system using the transformation matrix between the geographic coordinate system and the Earth-centered Earth-fixed coordinate system.

[0076] The expression of the state variable is:

[0077]

[0078] in, is the eastward platform error angle, is the north platform error angle, is the error angle of the celestial platform, East velocity error, North velocity error, is the celestial velocity error, is the latitude position error, is the latitude position error, is the height position error, is the random constant drift of the x-axis gyroscope, is the random constant drift of the y-axis gyroscope, is the random constant drift of the z-axis gyroscope, is the error of the x-axis accelerometer, is the error of the y-axis accelerometer, is the error of the z-axis accelerometer, is transposed.

[0079] S3: construct a long-distance machine measurement equation based on the calculated value of the relative distance between the long-distance machine and the ground base station and the measured value of the relative distance, and establish a long-distance machine sub-model based on the state equation and the long-distance machine measurement equation;

[0080] The measurement equation of the lead aircraft is established based on the relative distance and angle measurement information between the lead aircraft and the ground base station. The observed value is the difference between the calculated value and the measured value of the relative distance between the UAV and the ground base station. The calculated value of the relative distance is obtained by using the inertial navigation output of the lead aircraft and the known anchor coordinates of the ground base station, and the measured value comes from the laser radar relative navigation device.

[0081] Distance measurement and angle measurement sensors can measure the relative distance and angle between the drone and the ground base station:

[0082]

[0083] in, is the relative distance measurement, is the true value of the relative distance, is the ranging error, is the elevation angle measurement of the long-distance aircraft relative to the ground base station, is the measured value of the azimuth of the long-distance aircraft relative to the ground base station, is the true value of the elevation angle, is the true value of the azimuth, is the angle measurement error of the elevation angle, is the angular measurement error of the azimuth;

[0084] For measurement error, assume , , It obeys a Gaussian white noise process with zero expectation and variances of , and .

[0085] According to the relative relationship between the lead aircraft and the ground base station established by the relative navigation equipment, Figure 3 As shown in the figure, since the relative information is measured in the geographic coordinate system of the ground base station, the relative distance is decomposed into three axes of the geographic coordinate system of the ground base station according to the geometric principle:

[0086]

[0087] in, is the east component of the relative distance measurement, is the north component of the relative distance measurement, is the celestial component of the relative distance measurement;

[0088] Since the errors in distance and angle measurement are small, the first-order infinitesimals in the equation are first treated equivalently, and then the higher-order small quantities are ignored, and the equation can be obtained:

[0089]

[0090] in, is the eastward component of the true distance, is the north component of the true distance, is the celestial component of the true distance, is the east component of the relative distance measurement error, is the north component of the relative distance measurement error, is the celestial component of the relative distance measurement error, and:

[0091]

[0092] The collaborative navigation model is established in the Earth-centered Earth-fixed coordinate system, so the conversion matrix between the long-range aircraft geographic coordinate system and the Earth-centered Earth-fixed coordinate system is used. Processing, conversion matrix The calculation expression is:

[0093]

[0094] in, is the latitude of the ground base station, is the longitude of the ground base station;

[0095] In the Earth-centered Earth-fixed coordinate system, the measured value of the relative distance of the long aircraft is:

[0096]

[0097] in, is the x-axis component of the measured value of the relative distance between the long-distance aircraft and the ground base station, is the y-axis component of the measured value of the relative distance between the long-distance aircraft and the ground base station, is the z-axis component of the measured value of the relative distance between the long-distance aircraft and the ground base station, is the x-axis component of the true relative distance value, is the true relative distance value y-axis component, is the z-axis component of the true relative distance value, is the x-axis component of the relative distance measurement error, is the x-axis component of the relative distance measurement error, is the x-axis component of the relative distance measurement error.

[0098] In the Earth-centered Earth-fixed coordinate system, the relative distance between the lead aircraft and the ground base station is calculated based on the inertial navigation output of the lead aircraft and the base station position:

[0099]

[0100] in, is the calculated value of the relative distance between the long-distance aircraft and the ground base station in the Earth-centered Earth-fixed coordinate system Axis component, is the calculated value of the relative distance between the long-distance aircraft and the ground base station in the Earth-centered Earth-fixed coordinate system Axis component, is the calculated value of the relative distance between the long-distance aircraft and the ground base station in the Earth-centered Earth-fixed coordinate system Axis component, The inertial navigation output position of the lead aircraft in the Earth-centered Earth-fixed coordinate system Axis component, The inertial navigation output position of the lead aircraft in the Earth-centered Earth-fixed coordinate system Axis component, The inertial navigation output position of the lead aircraft in the Earth-centered Earth-fixed coordinate system Axis component, The base station position of the long-distance aircraft in the Earth-centered Earth-fixed coordinate system Axis component, The base station position of the long-distance aircraft in the Earth-centered Earth-fixed coordinate system Axis component, The base station position of the long-distance aircraft in the Earth-centered Earth-fixed coordinate system Axis component, is the inertial navigation error Axis component, is the inertial navigation error Axis component, is the inertial navigation error Axis component; The true value of the inertial output position of the lead aircraft Axis component, The true value of the inertial output position of the lead aircraft Axis component, The true value of the inertial output position of the lead aircraft Axis component;

[0101] Since the inertial navigation outputs positioning information in the form of latitude, longitude and altitude, the position of the lead aircraft needs to be converted into coordinates:

[0102]

[0103] in, is the latitude output by the inertial navigation of the lead aircraft, is the longitude output by the inertial navigation of the lead aircraft, is the altitude output by the inertial navigation of the lead aircraft, is the radius of curvature of the Earth's circumplex, is the Earth's flattening;

[0104] Through error analysis, we can get:

[0105]

[0106] in, is the latitude of the aircraft in the geodetic coordinate system, is the longitude of the aircraft in the geodetic coordinate system, is the height of the long aircraft in the geodetic coordinate system,

[0107] We can get:

[0108]

[0109] in, The transformation matrix for converting navigation errors from the UAV's navigation coordinate system to the Earth-centered Earth-fixed coordinate system; is the latitude error, is the longitude error, is the height error;

[0110] The difference between the calculated value and the measured value of the relative distance between the long-distance aircraft and the ground base station is used as the observation value to establish the measurement equation of the long-distance aircraft:

[0111]

[0112] Using state variables Substituting the independent variable, we get:

[0113]

[0114] in, is the long-machine observation quantity, is the observation matrix of the lead aircraft, is the state estimate of the long machine, Observe the noise for the long aircraft. ,

[0115]

[0116] The noise characteristics of the distance and angle measurement errors are consistent, and the measurement noise covariance matrix of the Kalman filter used by the long-distance aircraft is obtained. :

[0117]

[0118] in, To find the variance function, To find the covariance function;

[0119] The inertial navigation error of the lead aircraft is estimated and corrected through Kalman filtering, thereby improving the navigation accuracy of the lead aircraft, and the corrected high-precision navigation results are shared with the wingman through the airborne data link.

[0120] S4: When the number of the communication leader is 1, the wingman measurement equation is constructed according to the calculated value and measured value of the relative distance between the wingman and the communication leader, and the wingman sub-model is established according to the state equation and the wingman measurement equation;

[0121] The measurement equation of the wingman is established based on the cooperative navigation information of the wingman and its communication leader, and the observed quantity is the difference between the calculated value and the measured value of the relative distance between the wingman and the communication leader;

[0122] Relative distance measurements:

[0123]

[0124] in, is the relative distance error between aircraft in the navigation coordinate system of the lead aircraft Axis component, is the relative distance error between aircraft in the navigation coordinate system of the lead aircraft Axis component, is the relative distance error between aircraft in the navigation coordinate system of the lead aircraft Axis component, is the transformation matrix between the wingman's geographic coordinate system and the Earth-centered Earth-fixed coordinate system, and the calculation expression is:

[0125]

[0126] in, is the longitude of the lead aircraft after correction of navigation error, is the latitude of the lead aircraft after correction of navigation error;

[0127] and Same effect;

[0128] In the Earth-centered Earth-fixed coordinate system, the relative distance between the two aircraft is calculated based on the high-precision navigation results shared by the wingman's own inertial navigation output and the communication leader:

[0129]

[0130] in, Inertial navigation output for the wingman Axis component, Inertial navigation output for the wingman Axis component, Inertial navigation output for the wingman Axis component, The high-precision position of the communication leader after navigation error correction in the Earth-centered Earth-fixed coordinate system Axis component, The high-precision position of the communication leader after navigation error correction in the Earth-centered Earth-fixed coordinate system Axis component, The high-precision position of the communication leader after navigation error correction in the Earth-centered Earth-fixed coordinate system Axis component;

[0131] The transformation matrix for converting the wingman's inertial navigation error to the Earth-centered Earth-fixed coordinate system;

[0132] The measurement equation of the wingman is established as follows:

[0133]

[0134] Using state variables Substituting the independent variable, we get:

[0135]

[0136] in, is the wingman observation quantity, is the wingman observation matrix, is the wingman state estimate, Observe noise for the wingman.

[0137] The measurement noise covariance matrix of the Kalman filter used by the wingman is obtained as follows: :

[0138]

[0139] The inertial navigation error of the wingman is estimated and corrected through Kalman filtering, thereby improving the navigation accuracy of the wingman.

[0140] S5: Estimate and correct the inertial navigation error of the leader sub-model through Kalman filtering, and estimate and correct the inertial navigation error of the wingman sub-model through Kalman filtering.

[0141] S6: When the number of lead aircraft is 2 or more, the lead aircraft of the wingman is selected by the principle of dynamic optimization of lead aircraft, and a multi-UAV classified collaborative navigation model is established;

[0142] If multiple communication leader aircraft participate in the collaborative navigation process of the wingman, they can provide the wingman with richer and more accurate navigation references, and can further improve the navigation accuracy of the wingman through multi-source information fusion algorithms, thereby enhancing the collaborative navigation performance of the drone swarm to a greater extent.

[0143] A collaborative navigation performance enhancement algorithm is further proposed for the wingman. The communication leader of the wingman is determined by the dynamic optimization principle of the communication leader. The federated Kalman filter algorithm based on dynamic matrix information allocation is used to achieve a higher accuracy estimation of the wingman's inertial navigation error.

[0144] The communication leader aircraft participating in the wingman collaborative navigation process is determined through dynamic selection of the communication leader aircraft. According to the real-time classification algorithm of the fleet, there are Long-haul machine and Set up a wingman and set up all the long machines to form a group , then The wingman and the Relative distance between the rack length machines for:

[0145]

[0146] in, For the The wingman and the The relative distance between the rack length machines, For the Wingman Axis component, For the Wingman Axis component, For the Wingman Axis component, For the Long machine Axis component, For the Long machine Axis component, For the Long machine Axis component;

[0147] Since the collaborative navigation process between each wingman and the lead aircraft is independent and distributed at the moment, the lead aircraft selection process for all wingmen is similar. Taking the wingman as an example, the optimization algorithm for the communication leader includes:

[0148] The dynamic optimization principle for communication long machines includes quality optimization based on the relative distance between machines and quantity optimization based on setting an upper limit on the number of communication long machines.

[0149] Quality optimization includes:

[0150] According to the range of the laser radar used Set the relative distance threshold between machines;

[0151] Calculate the The wingman's spatial position is close to the leader's gathering ,gather The number of long machines included is ;

[0152] Calculate the The wingman and the The relative distance between the rack length machines; and the assembly The leader element contained in it is relative to the wingman The relative distances are arranged from small to large;

[0153] According to The wingman and the The relative distance between the rack length machines and the relative distance threshold between the machines are obtained. The wingman's spatial position is close to the leader's gathering ;

[0154] The preferred quantity includes:

[0155] Through the information fusion algorithm, Adding a communications leader to the wingman's 2nd communications leader brings about a navigation accuracy improvement of , the computational and communication load increases to , then the quantity measurement index It can be expressed as:

[0156]

[0157] like , then set the upper limit of the number of long-distance communication devices to ;

[0158] like ,but Otherwise, the communication leader is selected according to the principle of the closest distance between machines and the collection is obtained. , and order ,

[0159]

[0160] in, For the Wingman and Collection The first The relative distance between the communication terminals. No. Wingman and Collection The first The relative distance between the communication terminals. For the Wingman and Collection The first The relative distance between the communication terminals.

[0161] The dynamic optimization algorithm for the communication leader can make full use of the redundant relative information within the fleet by reasonably selecting the wingman corresponding to the communication leader in different environments and different information fusion methods, thereby providing the wingman with more accurate and richer collaborative navigation information, laying a data foundation for the technical route of using data fusion algorithms to enhance collaborative navigation performance.

[0162] S7: Estimation and correction of multi-UAV classification cooperative navigation model through federated Kalman filter algorithm based on dynamic matrix information allocation;

[0163] Since the classified cooperative structure between UAVs is determined by the relative navigation state between the UAV and the ground base station, that is, the correlation between the UAVs is unknown, the federated Kalman filter algorithm is a distributed two-stage filtering method. It distributes the global process noise to each sub-filter by using the information conservation theorem and eliminates the correlation of multi-source information by using the variance upper bound technology. After the wingman has undergone the optimization process, When the communication long machine is installed, the filter fusion system contains sub-filters.

[0164] The federated filtering algorithm based on dynamic matrix information allocation includes the design of dynamic matrix allocation coefficients, adopts the estimated covariance matrix as the basis for setting the information allocation coefficients, and performs real-time information allocation coefficient calculations on each state variable of each filter. By dynamically allocating information matrix coefficients in the federated filtering, each sub-filter is given a different fusion weight to achieve the optimal fusion of multi-source information.

[0165] The estimated covariance matrix of each sub-filter Perform eigenvalue decomposition, expressed as:

[0166]

[0167] in, for The characteristic matrix of , for The first eigenvalue of for The second eigenvalue of for No. eigenvalues, for The dimension of for The eigenvector matrix of

[0168] For The sub-filter state component , the information distribution coefficient calculation expression is:

[0169]

[0170] in, For the The sub-filter The information distribution coefficient of each state component;

[0171] No. Information allocation coefficient of sub-filter The calculation expression is:

[0172]

[0173] The sum of the coefficients of multiple sub-filters changes from the number 1 to the unit matrix, satisfying the principle of information conservation. The calculation expression is:

[0174]

[0175] in, is the identity matrix;

[0176] The optimal estimation of the wingman inertial navigation error is achieved by using a federated filtering algorithm based on dynamic matrix information allocation.

[0177] The beneficial effects of the present invention are as follows: cooperative navigation between drones is carried out by utilizing the resonant inertial navigation that takes into account both high precision and low power consumption and the rich relative navigation information between drones. From the perspective of information conservation, the problem that the navigation accuracy of drones that rely on satellite navigation is significantly reduced in a satellite-denied environment is solved, thereby effectively improving the navigation accuracy of drones.

[0178] The classified collaborative navigation structure of the present invention classifies UAVs into leader aircraft and wingman aircraft in real time according to whether they can communicate with the ground base station at the current moment, which is more in line with reality and can improve the robustness of collaborative navigation while improving the navigation accuracy of UAVs.

[0179] The present invention proposes a collaborative navigation performance enhancement algorithm based on dynamic optimization of communication leader and dynamic matrix information allocation of federated filtering, which effectively improves the utilization rate of redundant relative navigation information and further improves the collaborative navigation accuracy of UAV swarms through optimal estimation and compensation of resonant inertial navigation errors.

[0180] like Figure 6 As shown, a multi-UAV classified collaborative navigation system based on resonant inertial navigation includes:

[0181] The classification module 101 establishes a classified navigation structure for the drone group according to whether the drone can currently receive the relative navigation information sent by the ground base station, and classifies the drone group into the leader and the wingman in real time;

[0182] The first building module 102 is used to establish a state equation according to the inertial navigation error of the UAV;

[0183] The second construction module 103 constructs a long-machine measurement equation according to the calculated value of the relative distance between the long-machine and the ground base station and the measured value of the relative distance, and establishes a long-machine sub-model according to the state equation and the long-machine measurement equation;

[0184] The third construction module 104 is used to construct a wingman measurement equation according to the calculated value and the measured value of the relative distance between the wingman and the communication leader when the number of the wingman is one, and to establish a wingman sub-model according to the state equation and the wingman measurement equation;

[0185] The first estimation and correction module 105 estimates and corrects the inertial navigation error of the leader sub-model through Kalman filtering, and estimates and corrects the inertial navigation error of the wingman sub-model through Kalman filtering;

[0186] The fourth building module 106 is used to select the communication leader of the wingman according to the dynamic optimization principle of the communication leader when the number of the communication leader is more than 2, and establish a multi-UAV classification collaborative navigation model;

[0187] The second estimation and correction module 107 estimates and corrects the multi-UAV classification cooperative navigation model through a federated Kalman filter algorithm based on dynamic matrix information allocation.

[0188] Through the collaborative work of the above modules, the optimal estimation and compensation of the resonant inertial navigation error are achieved, which improves the collaborative navigation accuracy of the UAV swarm and the robustness of the collaborative navigation.

[0189] Example:

[0190] 1. Power on multiple drones, bind the initial position coordinates, put the resonant inertial navigation system into full-angle working state, perform initial alignment for 5 minutes, and obtain initial attitude information;

[0191] 2. The drone takes off and relies entirely on its own resonant inertial navigation system for pure inertial navigation before taking off to a safe altitude of more than 30m;

[0192] 3. When the altitude of the drone exceeds the safe altitude, classified collaborative navigation is carried out based on relative navigation information and data link communication technology. The drone group is classified into leader and wingman in real time according to whether the drone can receive relative navigation information sent by the ground base station at the current moment. The classification cycle is 5s.

[0193] 4. The leader aircraft uses the relative navigation information between itself and the ground base station as a reference to estimate and compensate its own resonant inertial navigation error every 1 second; the wingman uses the relative navigation information between itself and the communicating leader aircraft as a reference to estimate and compensate its own resonant inertial navigation error every 1 second. When the wingman has two communicating leaders, the collaborative navigation performance enhancement algorithm is used to further improve the navigation accuracy.

[0194] like Figure 4 The figure shows the navigation error of individual drones under collaborative navigation. The navigation error comparison chart shows the navigation error of individual drones when they are always the wingman, always the leader, first the leader and then the wingman, and first the wingman and then the leader. Figure 5 The figure shows the navigation error of the coordinated navigation when the wingman has two communication leaders. The navigation error is reflected by the longitude error, latitude error and altitude error. Figure 4 and Figure 5It can be seen that when the wingman has two communication leaders, the collaborative navigation performance enhancement algorithm can be used to further improve the navigation accuracy.

[0195] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-UAV classification collaborative navigation method based on resonant inertial navigation, characterized in that: include: S1: According to whether the drone can receive the relative navigation information sent by the ground base station at the current moment, a classified navigation structure of the drone group is established, and the drone group is classified into the leader and wingman in real time; S2: Establish the state equation based on the inertial navigation error of the UAV; S3: construct a long-distance machine measurement equation based on the calculated value of the relative distance between the long-distance machine and the ground base station and the measured value of the relative distance, and establish a long-distance machine sub-model based on the state equation and the long-distance machine measurement equation; The measurement equation of the lead aircraft is established based on the relative navigation information between the lead aircraft and the ground base station. The calculated value of the relative distance is obtained using the inertial navigation output of the lead aircraft and the known anchor coordinates of the ground base station. The measured value comes from the laser radar relative navigation equipment. The observation value of the lead aircraft is the difference between the calculated value and the measured value of the relative distance between the UAV and the ground base station. The measurement equation of the lead aircraft is established in the Earth-centered Earth-fixed coordinate system. S4: When the number of the communication leader is 1, the wingman measurement equation is constructed according to the calculated value and measured value of the relative distance between the wingman and the communication leader, and the wingman sub-model is established according to the state equation and the wingman measurement equation; S5: Estimate and correct the inertial navigation error of the leader sub-model through Kalman filtering, and estimate and correct the inertial navigation error of the wingman sub-model through Kalman filtering; S6: When the number of lead aircraft is 2 or more, the lead aircraft of the wingman is selected by the principle of dynamic optimization of lead aircraft, and a multi-UAV classified collaborative navigation model is established; The dynamic optimization principle of the communication leader includes quality optimization based on the relative distance between the leaders and quantity optimization based on the upper limit of the number of communication leaders. The qualities preferably include: According to the range of the laser radar used Set the relative distance threshold between machines; Calculate the The wingman's spatial position is close to the leader's gathering ,gather The number of long machines included is ; Calculate the The wingman and the The relative distance between the rack length machines; According to The wingman and the The relative distance between the rack length machines and the relative distance threshold between the machines are obtained. The wingman's spatial position is close to the leader's gathering ; The quantity preferably includes: Through the information fusion algorithm, Adding a communications leader to the wingman's 2nd communications leader brings about a navigation accuracy improvement of , the computational and communication load increases to , then the quantity measurement index It can be expressed as: like , then set the upper limit of the number of long-distance communication devices to ; like ,but Otherwise, the communication leader is selected according to the principle of the closest distance between machines and the collection is obtained. , and order , in, For the Wingman and Collection The first The relative distance between the communication terminals. No. Wingman and Collection The first The relative distance between the communication terminals. For the Wingman and Collection The first The relative distance between the communication towers; S7: The multi-UAV classification collaborative navigation model is estimated and corrected through the federated Kalman filter algorithm based on dynamic matrix information allocation.

2. According to claim 1, a multi-UAV classified collaborative navigation method based on resonance inertial navigation is characterized in that: The lead aircraft obtains the relative navigation information between it and the ground base station through relative navigation equipment; the wingman uses the airborne data link and relative navigation equipment to obtain the collaborative navigation information of the communicating lead aircraft.

3. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The collaborative navigation model includes the leader sub-model and the wingman sub-model. In the navigation coordinate system, the state equation of the leader sub-model and the state equation of the wingman sub-model are established according to the inertial navigation error of the UAV.

4. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The resonant inertial navigation error equation of the UAV is used as the state equation of the lead aircraft and the wingman, and the geographic coordinate system is selected as the navigation coordinate system of the UAV, then the state equation is: in, for The moment state error, for The state transfer matrix of the inertial guidance error at time, for The system noise drives the matrix at every moment. for The system noise sequence at each moment, for Moment state variables; The state equation is transformed into the Earth-centered Earth-fixed coordinate system using the transformation matrix between the geographic coordinate system and the Earth-centered Earth-fixed coordinate system.

5. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The measurement equation of the long machine is: in, is the long-machine observation quantity, is the observation matrix of the lead aircraft, is the state estimate of the long machine, Observe the noise for the lead aircraft.

6. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The measurement equation of the wingman is established through the collaborative navigation information of the wingman and the communication leader. The wingman observation is the difference between the calculated value and the measured value of the relative distance between the wingman and the communication leader. In the Earth-centered Earth-fixed coordinate system, the measurement equation of the wingman is established as: in, is the wingman observation quantity, is the wingman observation matrix, is the wingman state estimate, Observe noise for the wingman.

7. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The wingman selects the communication leader from among multiple leader aircraft within the communication range according to the dynamic optimization principle of the communication leader. If there are multiple communication leaders participating in the collaborative navigation of the wingman, the federal Kalman filter algorithm based on dynamic matrix information allocation is used to estimate the inertial navigation error of the wingman.

8. The multi-UAV classified collaborative navigation method based on resonance inertial navigation according to claim 1 is characterized in that: The federated Kalman filter algorithm based on dynamic matrix information allocation includes the design of dynamic matrix allocation coefficients, the use of estimated covariance matrix to set the information allocation coefficients, real-time information allocation coefficient calculation for each state variable of each filter, and dynamic allocation of information matrix coefficients in the federated filter. Each filter is given a different fusion weight to achieve optimal fusion of multi-source information.

9. A multi-UAV classified collaborative navigation system based on resonant inertial navigation, characterized in that: The method for executing a multi-UAV classified collaborative navigation method based on resonance inertial navigation as claimed in any one of claims 1 to 8 comprises: A classification module, wherein the classification module establishes a classified navigation structure for the drone group according to whether the drone can currently receive relative navigation information sent by the ground base station, and classifies the drone group into a leader and a wingman in real time; A first building block, wherein the first building block establishes a state equation according to an inertial navigation error of the UAV; A second construction module, wherein the second construction module constructs a long-machine measurement equation according to a calculated value of a relative distance between the long-machine and the ground base station and a measured value of the relative distance, and establishes a long-machine sub-model according to the state equation and the long-machine measurement equation; A third construction module, wherein when the number of the communication leader is one, the third construction module is used to construct a wingman measurement equation according to the calculated value and the measured value of the relative distance between the wingman and the communication leader, and to establish a wingman sub-model according to the state equation and the wingman measurement equation; a first estimation and correction module, wherein the first estimation and correction module estimates and corrects the inertial navigation error of the leader sub-model through Kalman filtering, and estimates and corrects the inertial navigation error of the wingman sub-model through Kalman filtering; A fourth building module, wherein when the number of the communication leaders is more than 2, the fourth building module is used to select the communication leader of the wingman according to the dynamic optimization principle of the communication leader, and establish a multi-UAV classification collaborative navigation model; The second estimation and correction module estimates and corrects the multi-UAV classification collaborative navigation model through a federal Kalman filter algorithm based on dynamic matrix information allocation.

Images