Unmanned aerial vehicle cluster collaborative navigation positioning method, system, device and medium

Abstract

The invention relates to the technical field of navigation, and discloses an unmanned aerial vehicle cluster collaborative navigation positioning method, system and device and a medium. The method comprises the following steps: acquiring theoretical covariance and actual covariance when the UKF performs collaborative navigation positioning, and adjusting theoretical noise variance in the theoretical covariance, so that the theoretical covariance after the theoretical noise variance is adjusted is equal to the actual covariance; extracting relative measurement data of a relative navigation sensor in the unmanned aerial vehicle cluster collaborative navigation system; capturing a dependency relationship between a relative position calculation value and a relative position measurement value in the relative measurement data, predicting the relative position measurement value through the dependency relationship and the relative position calculation value, and reconstructing the relative measurement data in combination with the predicted relative position measurement value and the relative position calculation value; according to the UKF after the theoretical noise variance is adjusted, collaborative navigation positioning of the unmanned aerial vehicle cluster is carried out, or the UKF is enabled to carry out collaborative navigation positioning of the unmanned aerial vehicle cluster according to the reconstructed measurement data, and the precision is relatively high.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a method, system, equipment and medium for collaborative navigation and positioning of an unmanned aerial vehicle cluster. Background Art

[0002] With the development of artificial intelligence and swarm technology, drone swarms have been widely used in civil and military fields. Swarm formations expand the execution capabilities and mission scope of single drones through mutual cooperation, and drone swarm collaborative navigation technology is the premise and core of achieving formation control and intelligent operation, so it has become one of the research hotspots.

[0003] There are two main modes for cooperative navigation of drone swarms, namely parallel mode and master-slave mode. The former requires all drones to have high-precision navigation capabilities, so it is only suitable for small-scale swarms. In contrast, the master-slave mode is relatively simple, requiring only a small number of drones with high-precision navigation capabilities as the "leader", while the "wingman" obtains navigation information relative to the "leader" through low-cost sensors to achieve self-positioning. The master-slave mode is more suitable for practical use because it has the advantages of simplicity, flexibility and low cost.

[0004] However, low-cost sensors in cooperative navigation, such as Miniature Inertial Measurement Unit (MIMU) and other relative navigation sensors (such as laser angle and distance sensors, Doppler velocity sensors, etc.), have their own limitations: MIMU has large inertial sensor drift, and navigation errors accumulate rapidly over time, while relative navigation sensors are easily disturbed by complex environments and maneuvering states, thus affecting the cooperative navigation performance of UAVs. In order to address these limitations, combined navigation provides an effective means to achieve the complementary advantages of these sensors.

[0005] Data fusion is a key technology for realizing combined navigation. It can process the measurement data from MIMU and relative navigation sensors to provide accurate collaborative navigation information. At present, Kalman Filtering (KF) is the core method for realizing multi-sensor collaborative navigation. However, due to the error characteristics of MIMU and the high maneuverability of UAVs, collaborative navigation models often exhibit nonlinear characteristics. Therefore, Kalman filtering, which is only applicable to linear systems, cannot be directly applied to nonlinear collaborative navigation. To this end, researchers are committed to studying various nonlinear filtering methods, such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF).

[0006] EKF uses Taylor series expansion to approximately linearize nonlinear systems and ignores high-order terms. However, for strongly nonlinear systems, the EKF linearization error is large and the calculation of the Jacobian matrix is ​​more complicated. UKF generates a set of Sigma points through unscented transform (UT) to approximate the mean and covariance of the system state, avoiding the calculation of the Jacobian matrix and having higher state estimation accuracy than EKF. Therefore, UKF becomes a better method to solve the state estimation problem of nonlinear systems.

[0007] Due to the inherent characteristics of Kalman filtering, data fusion based on UKF requires an accurate measurement model and its noise statistics. If there is a deviation in the measurement model and its noise statistics, the state estimation result will be affected and even cause filter divergence. However, as mentioned earlier, relative navigation sensors are easily disturbed by complex environments and maneuvering states, resulting in abnormal measurements (for example: measurement loss due to sensor failure or occlusion, measurement anomalies / drifts due to sensor failure or maneuvering states, and changes / deviations in measurement noise statistics due to environmental interference), making it impossible to achieve accurate collaborative navigation positioning. Summary of the invention

[0008] The purpose of the present invention is to provide a method, system, device and medium for collaborative navigation and positioning of a drone cluster, which can solve the technical problem that relative navigation sensors are easily disturbed by complex environments and maneuvering states, resulting in abnormal measurements and inability to achieve accurate collaborative navigation and positioning.

[0009] In order to solve the above technical problems, an embodiment of the present invention provides a drone cluster collaborative navigation and positioning method, comprising the following steps: Establish a measurement model of the UAV swarm collaborative navigation system to obtain the collaborative measurement data of the UAV swarm obtained by all navigation sensors in the UAV swarm collaborative navigation system; Obtain the theoretical innovation covariance and the actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance; Extracting relative measurement data of the relative navigation sensor in the UAV cluster cooperative navigation system from the cooperative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; Capturing the dependency between the relative position / velocity calculation value and the relative position / velocity measurement value in the relative measurement data, and predicting the relative position / velocity measurement value through the dependency and the relative position / velocity calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / velocity measurement value and the relative position / velocity calculation value; The collaborative navigation and positioning of the UAV cluster is performed based on the UKF after adjusting the theoretical measurement noise variance, or the UKF is made to perform the collaborative navigation and positioning of the UAV cluster based on the reconstructed measurement data.

[0010] Optionally, the adjusting the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance includes: Inputting the theoretical innovation covariance and the actual innovation covariance into the first adaptive neuro-fuzzy inference network ANFIS; The matching degree between the theoretical innovation covariance and the actual innovation covariance is obtained through the ANFIS network, and an adjustment coefficient for adjusting the theoretical measurement noise variance is output according to the matching degree, so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance.

[0011] Optionally, capturing the dependency between the relative position / velocity calculation value and the relative position / velocity measurement value in the relative measurement data, and predicting the relative position / velocity measurement value through the dependency and the relative position / velocity calculation value, comprises: The second ANFIS network is trained using the relative position / speed calculation values ​​and relative position / speed measurement values ​​of the leader and wingman of several UAV clusters as sample data to obtain an ANFIS model, so as to capture the dependency between the relative position / speed calculation values ​​and the relative position / speed measurement values ​​through the ANFIS model; The relative position / velocity calculations are input into the ANFIS model to obtain the predicted relative position / velocity measurements.

[0012] Optionally, before obtaining the theoretical innovation covariance and the actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning according to the collaborative measurement data, the method further includes: Construct anomaly detection functions for collaborative measurement data; The anomaly detection function is: ; In the formula, is a unit vector, and The element is 1, is the dimension of the measurement vector, is the innovation vector, is the corresponding covariance matrix; According to the anomaly detection function, a first event trigger condition is set, and according to the first event trigger condition, whether the collaborative measurement data is abnormal; The first event trigger condition is: ; In the formula, is the false alarm rate, is the detection threshold.

[0013] Optionally, before obtaining the theoretical innovation covariance and the actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning according to the collaborative measurement data, the method further includes: If the collaborative measurement data is determined to be abnormal, a relative position change index of the collaborative measurement data is constructed; The relative position change index is: ; In the formula, Indicates the measurement dimension involving anomalies, is the total dimension of the measurement, In isolation After dimensional measurement, the state estimation error covariance matrix diagonal elements, is the first value of the state estimation error covariance matrix during UKF filtering. diagonal elements; According to the relative position change index, a second event trigger condition is set, and according to the second event trigger condition, whether to adjust the theoretical measurement noise variance or whether to predict the relative position / speed measurement value; The second event trigger condition is: ; In the formula, is the threshold for the second event trigger.

[0014] An embodiment of the present invention further provides a drone cluster collaborative navigation and positioning system, comprising: A model building module is used to build a measurement model of the UAV cluster collaborative navigation system to obtain collaborative measurement data of the UAV cluster obtained by all navigation sensors in the UAV cluster collaborative navigation system; A noise adjustment module is used to obtain the theoretical innovation covariance and the actual innovation covariance when the unscented Kalman filter UKF performs collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance; A data acquisition module is used to extract relative measurement data of the relative navigation sensor in the UAV cluster cooperative navigation system from the cooperative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; A position / speed prediction module is used to capture the dependency between the relative position / speed calculation value and the relative position / speed measurement value in the relative measurement data, and predict the relative position / speed measurement value through the dependency and the relative position / speed calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / speed measurement value and the relative position / speed calculation value; The navigation and positioning module is used to perform collaborative navigation and positioning of the UAV cluster based on the UKF after adjusting the theoretical measurement noise variance, or to enable the UKF to perform collaborative navigation and positioning of the UAV cluster based on the reconstructed measurement data.

[0015] An embodiment of the present invention also provides a computer device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned drone cluster collaborative navigation and positioning method.

[0016] An embodiment of the present invention further provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the above-mentioned drone cluster collaborative navigation and positioning method.

[0017] The drone cluster collaborative navigation and positioning method provided by the present invention has at least the following beneficial effects: The present invention reduces the negative impact of abnormal measurement values ​​of the UAV cluster collaborative navigation system through the following two methods, thereby improving the accuracy of subsequent UKF for UAV cluster collaborative navigation positioning: One is to obtain the theoretical innovation covariance and actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance. Since the UKF theoretical innovation covariance includes the theoretical measurement noise variance in the collaborative navigation system, if the value of the theoretical measurement noise variance is closer to the actual measurement noise variance, the theoretical innovation covariance is closer to the actual innovation covariance, then this method can be used to reduce the deviation of the measurement noise and reduce the impact of abnormal measurements.

[0018] The other is to extract the relative measurement data of the relative navigation sensor in the cooperative navigation system of the drone cluster from the cooperative measurement data, capture the dependency between the relative position / velocity calculation value and the relative position / velocity measurement value in the relative measurement data, and predict the relative position / velocity measurement value through the dependency and the relative position / velocity calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / velocity measurement value and the relative position / velocity calculation value. Since the relative measurement data is composed of the relative position / velocity calculation value between the leader and the wingman in the drone cluster and the relative position / velocity measurement value between the leader and the wingman measured by the relative navigation sensor, where the relative position / velocity calculation value can be calculated based on the relative position / velocity between the leader and the wingman, and its deviation is relatively fixed, it can be considered that most of the reasons for the measurement anomaly are the relative position / velocity measurement value. By introducing measurement prediction to reconstruct the measurement data, the impact of abnormal measurement can also be reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] One or more embodiments are exemplarily described by the pictures in the corresponding drawings, and these exemplary descriptions do not constitute limitations on the embodiments.

[0020] Figure 1 is a flowchart of a drone cluster collaborative navigation and positioning method provided according to an embodiment of the present invention; Figure 2 is a schematic diagram of relative navigation measurement of a wingman provided according to an embodiment of the present invention; Figure 3 is a schematic diagram of a dual event triggered ANFIS mode structure provided according to an embodiment of the present invention; Figure 4 is a schematic diagram of an ANFIS structure and optimization process provided according to an embodiment of the present invention; Figure 5 is a schematic diagram of a flight trajectory of a drone cluster provided according to an embodiment of the present invention; Figure 6 is a schematic diagram of position root mean square error curves of three algorithms under a measurement mutation situation provided according to an embodiment of the present invention; Figure 7 is a schematic diagram of the position cumulative root mean square error of three algorithms under the situation of measurement mutation provided by one embodiment of the present invention; Figure 8 is a schematic diagram of position root mean square error curves of three algorithms under non-Gaussian measurement noise conditions provided according to an embodiment of the present invention; Fig. 9 is a schematic diagram of the position cumulative root mean square error of three algorithms under non-Gaussian measurement noise provided according to an embodiment of the present invention; Fig.10 is a schematic diagram of position root mean square error curves of three algorithms in a measurement missing situation provided according to an embodiment of the present invention; Fig.11 is a schematic diagram of the cumulative root mean square error of positions of three algorithms in a measurement missing situation provided according to an embodiment of the present invention; Fig.12 It is a schematic diagram comparing the computational efficiency of UKF, UKF-ANFIS and UKF-DEANFIS provided according to an embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical scheme and advantages of the embodiments of the present invention clearer, the embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings. However, it will be appreciated by those skilled in the art that in the embodiments of the present invention, many technical details are proposed in order to enable the reader to better understand the present invention. However, even without these technical details and various changes and modifications based on the following embodiments, the technical scheme claimed in the present invention can be implemented. The division of the following embodiments is for the convenience of description and should not constitute any limitation on the specific implementation of the present invention. The various embodiments can be combined and referenced with each other without contradiction.

[0022] One embodiment of the present invention relates to a drone cluster collaborative navigation and positioning method. The specific process of the drone cluster collaborative navigation and positioning method of this embodiment can be as follows: Figure 1 As shown, including: Step 101: Establish a measurement model of the UAV cluster collaborative navigation system to obtain collaborative measurement data of the UAV cluster obtained by all navigation sensors in the UAV cluster collaborative navigation system.

[0023] Step 102, obtain the theoretical innovation covariance and actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance.

[0024] Step 103, extracting the relative measurement data of the relative navigation sensor in the UAV cluster collaborative navigation system from the collaborative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor.

[0025] Step 104, capturing the dependency between the relative position / velocity calculation value and the relative position / velocity measurement value in the relative measurement data, and predicting the relative position / velocity measurement value through the dependency and the relative position / velocity calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / velocity measurement value and the relative position / velocity calculation value.

[0026] The implementation details of the drone cluster collaborative navigation and positioning method of this embodiment are described in detail below. The following content is only provided for the convenience of understanding the implementation details and is not necessary for the implementation of this solution.

[0027] Step 1: Establish a measurement model of the UAV swarm collaborative navigation system to obtain the collaborative measurement data of the UAV swarm obtained by all navigation sensors in the UAV swarm collaborative navigation system.

[0028] In the specific implementation, the state model of the collaborative navigation system is established according to the error equation of the wingman MIMU in the UAV cluster collaborative navigation system; the relative navigation vector measurement value and the relative navigation vector calculation value are obtained according to the relative distance, speed and angle information obtained by the lead and wingman MIMUs and the relative navigation sensor, and the measurement model of the collaborative navigation system is established by difference.

[0029] (1) According to the error equation of MIMU, the state model of the cooperative navigation system is established: To establish the cluster cooperative combined navigation model, the following coordinate systems are used: inertial coordinate system ( system), carrier coordinate system ( system), Earth coordinate system ( system) and navigation coordinate system ( Tie), The system is usually chosen to be the East-North-Sky geographic coordinate system ( In addition, due to the existence of rotation error, the calculated navigation coordinate system is different from the real There are differences in the system, which is defined as The wingman MIMU error equation is selected as the state model of cluster cooperative navigation, where the state vector is:

[0030] ; In the formula, , and are the attitude, velocity and position errors of the MIMU; , Represent the constant drift of the gyroscope and accelerometer respectively.

[0031] Therefore, the MIMU attitude error equation can be written as: ; In the formula, and is from Department and Tie to The transformation matrix of the system; Represents the conversion matrix from the angular velocity of the computing platform coordinate system to the error angle of the Euler platform; yes Relative to The angular velocity of the system, is the corresponding error.

[0032] Furthermore, the MIMU velocity and position errors can be written as: ; ; In the formula, is the specific force measured by the accelerometer, is the angular velocity of the Earth's rotation, is the angular velocity relative to the Earth, and yes and The error, A and B is the coefficient matrix, which is specifically expressed as: In the formula, and are the speeds of the wingman in the east and north directions respectively; and are the principal curvature radii along the earth's meridian and meridian respectively; and is the latitude and altitude of the wingman.

[0033] The MIMU error also includes the output errors of the gyroscope and accelerometer, which can be described as: ; In the formula, and is the corresponding white noise.

[0034] The state model of the wingman's cooperative navigation system can be obtained and further discretized as: ; In the formula, yes The state vector at the moment; is a discrete nonlinear function describing the state model, is zero-mean Gaussian white noise.

[0035] (2) Establishment of the measurement model of the collaborative navigation system: according to Figure 2 The relative distance between the lead and wingman shown can be The system is decomposed into: ; In the formula, and is the relative distance between the lead aircraft and the wingman exist The weight in the system, and are measurements of relative azimuth and elevation.

[0036] Since the measured value includes errors, we have , and Considering , and is smaller, the above formula can be further re-expressed as:

[0037] ; In the formula, the symbol "~" indicates the measured value, and the symbol without "~" indicates the true value; Represents measurement error.

[0038] Further, The relative position vector measurement in the system can be expressed as: ; Furthermore, the relative position vector calculation value can be constructed based on the MIMU. The position in the system can be It is expressed in the system as:

[0039] ; In the formula, and Respectively indicate the positions of the leader and wingman, is the eccentricity of the Earth.

[0040] The relative position error between the lead aircraft and the wingman can be expressed as: ; The relative position between the leader and the wingman can be calculated as: ; In the formula, is from Tie to The transformation matrix of the system can be calculated using the position information output by the wingman MIMU.

[0041] Finally, the relative position error vector can be obtained ; Similar to the above process, the relative velocity error vector can also be established as: ; The measurement model of wingman collaborative navigation can be obtained as ; In the formula, is the overall measurement noise, is the measurement vector.

[0042] Step 2: Design a dual event trigger mechanism to reduce unnecessary computational effort in normal measurements and improve the computational performance of ANFIS in cluster collaborative navigation. First, construct an anomaly detection function for collaborative measurement data, set the first event trigger condition based on the anomaly detection function, and determine whether the collaborative measurement data is abnormal based on the first event trigger condition; second, if the collaborative measurement data is determined to be abnormal, construct a relative position change index for the collaborative measurement data, set the second event trigger condition based on the relative position change index, and determine whether to adjust the theoretical measurement noise variance or predict the relative position / velocity measurement value based on the second event trigger condition.

[0043] In the specific implementation, the dual event trigger mechanism structure is as follows Figure 3 As shown, it contains two event trigger conditions: Event trigger condition 1 (i.e., the first event trigger condition): This condition is intended to detect measurement failures or anomalies to avoid unnecessary predictions and optimizations when there are no failures or anomalies. This condition constructs a chi-square test based on the filter's innovation vector, where the innovation vector is defined as:

[0044] ; In the absence of measurement anomalies, the above formula should obey the zero-mean Gaussian distribution, and the calculation formula of its error covariance matrix is: ; When the measurement is abnormal, the mean of the innovation vector will no longer be zero. Therefore, the anomaly detection function can be constructed as:

[0045] ; In the formula, is a unit vector, whose The element is 1, is the dimension of the measurement vector, is the innovation vector, is the corresponding covariance matrix.

[0046] Therefore, the corresponding event trigger condition is defined as: ; In the formula, is the false alarm rate, is the corresponding detection threshold.

[0047] Event trigger condition 2 (i.e., the second event trigger condition): If the new information vector does not meet trigger condition 1, that is, there is an abnormality, then according to the degree of influence of the measurement on the filtering accuracy, it is decided whether to isolate the corresponding measurement. Therefore, in order to evaluate the influence of abnormal measurements on filtering accuracy, the present invention constructs a relative position change index (RPCI) to determine whether ANFIS should be used for network prediction.

[0048] RPCI is defined as: ; In the formula, Indicates the measurement dimension involving anomalies, is the total dimension of the measurement, In isolation After dimensional measurement, the state estimation error covariance matrix diagonal elements, is the first value of the state estimation error covariance matrix during UKF filtering. diagonal elements.

[0049] The above formula reflects the degree of decrease in filtering accuracy after isolating a certain measurement. The larger the RPCI value, the greater the impact of the measurement on the filtering accuracy. Therefore, the second event trigger condition can be constructed as:

[0050] ; In the formula, is the threshold for the second event to trigger.

[0051] Therefore, the event triggering mechanism is established as follows: ; From the above formula, we can see that if , it means that there are no abnormal measurements in collaborative navigation and conventional UKF can be used. , indicating that the sensor measurement has little effect on the filter accuracy and the sensor can be directly isolated. In this case, the time update result of the UKF is used as the final system state estimate. If , it means that there is an abnormal measurement, and this measurement is very important to the filtering result. Therefore, this measurement should be retained and ANFIS should be used to further compensate for the error caused by its influence.

[0052] Step 3: Design the prediction and optimization adjustment strategy based on the adaptive neural network fuzzy inference system (ANFIS). The structure and optimization process of ANFIS are as follows: Figure 4 Based on ANFIS, the present invention handles the negative impact of measurement anomalies in the following two ways: one is to adjust the measurement noise covariance to reduce the weight of abnormal measurements, and the other is to introduce measurement prediction values.

[0053] There are two ways to use adaptive neuro-fuzzy inference systems to deal with the negative impact of abnormal measurement values: one is to adjust the measurement noise variance matrix to reduce the weight of abnormal measurements; the other is to introduce prediction to assist the filtering process.

[0054] (1) Adaptive adjustment of measurement noise variance: The theoretical innovation covariance and actual innovation covariance of the unscented Kalman filter (UKF) when performing collaborative navigation positioning based on collaborative measurement data are obtained, and the theoretical measurement noise variance in the theoretical innovation covariance is adjusted so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance.

[0055] In the specific implementation, the theoretical innovation covariance of the system is defined as and the actual innovation series variance for: ; ; At each time step If the theoretical measurement noise variance is The closer the value of is to the actual measurement noise variance, the greater the theoretical innovation covariance The closer to the actual innovation covariance However, in practice, sensor anomalies can cause and To improve the performance of UKF, it is necessary to adjust To reduce and The difference between them is the covariance matching.

[0056] Therefore, the simplest approach is to make the two covariances equal and update It can be calculated by the following formula: ; However, in the calculation process of the actual data fusion algorithm, direct subtraction may produce a negative definite situation, which will cause the filtering to fail to proceed normally. In order to solve this problem, the theoretical innovation covariance and the actual innovation covariance are input into the first adaptive neural fuzzy inference network ANFIS; the matching degree between the theoretical innovation covariance and the actual innovation covariance is obtained through the ANFIS network, and an adjustment coefficient for adjusting the theoretical measurement noise variance is output according to the matching degree, so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance.

[0057] In the specific implementation, an adaptive estimation algorithm based on fuzzy logic is designed to avoid possible system errors and enhance the robustness of the system. and is the input, expressed as Degree of Matching (DOM):

[0058] ; Based on DOM, the fuzzy logic-based algorithm outputs an adjustment coefficient , adjust the measurement noise variance .Will and The matching degree between the two is used as the input of the fuzzy logic system. Then, the UKF is adjusted to measure the noise variance. To make and matches, that is:

[0059] ; In general, And DOM has the following three cases: a) If , Compare Big, then ,Increase To reduce the difference between the two covariances; b) If , and If similar, ,Keep constant; c) If , Compare Small, then , reduce to reduce the difference between the two covariances.

[0060] According to the above relationship, the following fuzzy rules can be defined: Rule 1. if , then Increase; Rule 2. if , then remain unchanged; Rule 3. if , then Reduce. in, and are two small thresholds used to divide different fuzzy rules.

[0061] The membership function of the input can be further defined according to the scope of the DOM as: Large: ; Equal: ; Small: ; In the formula, the parameters , and Used to determine the shape of the input membership function.

[0062] Based on the fuzzy rules established above, The range of determines the output membership function as: Large: ; Equal: ; Small: ; In the formula, the parameters , , and It is used to determine the shape of the output membership function. It is worth noting that the parameters of the input and output membership functions ( and ) is obtained based on ANFIS network training.

[0063] Finally, the centroid method was used to To defuzzify: ; (2) Measurement prediction: Extract the relative measurement data of the relative navigation sensor in the cooperative navigation system of the drone cluster from the collaborative measurement data; the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the drone cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; capture the dependency between the relative position / speed calculation value and the relative position / speed measurement value in the relative measurement data, and predict the relative position measurement value through the dependency and the relative position / speed calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / speed measurement value and the relative position / speed calculation value. The flight trajectories of the leader and the wingman in the drone cluster are as follows: Figure 5 shown.

[0064] In the specific implementation, since the measurement value in the measurement model is / and / If an abnormal measurement occurs, this indicates / In this case, an adaptive neuro-fuzzy inference system can be used to capture / and / The process consists of two stages: training and prediction. In the training stage, ANFIS uses / and / As raw input data, the network is trained and the ANFIS model is constructed. It is worth noting that the ANFIS model and its related fuzzy membership functions and fuzzy rules are constructed by learning a large amount of data collected under normal measurement conditions. / It is input into the prediction model as input data, and the network model trained in the training phase is used to obtain / The predicted value of / Based on the predicted value, the measured value can be predicted and reconstructed to obtain ,Right now:

[0065] Simulation verification: In order to better evaluate the robustness and effectiveness of the proposed UKF-DEANFIS in the presence of abnormal measurements, the proposed UKF-DEANFIS was simulated and evaluated, and compared with the traditional UKF and the UKF algorithm based on adaptive neural network fuzzy inference (UKF-ANFIS). Three typical scenarios were considered in the simulation: i) abnormal measurement mutation; ii) non-Gaussian measurement noise; iii) missing measurement. In addition, 20 Monte Carlo simulations were performed, and the root mean square error (RMSE) was used to evaluate the performance of the above three algorithms:

[0066] ; In the formula, Indicates the total number of Monte Carlo simulations; Indicated in The navigation parameter error vector of the wingman in the Monte Carlo simulation; express The Euclidean norm of .

[0067] In addition, in order to further evaluate the performance of the above three algorithms, the cumulative root mean square error (ARMSE) of the wingman position is calculated respectively, which is defined as follows: ; In the formula, Indicates the amount of data used for comparison.

[0068] Simulation parameter settings: The sensor parameters required for wingman integrated navigation are shown in Table 1: Table 1 Scenario 1: Measuring abnormal mutations: In this case, the measurement data of collaborative navigation (i.e., the relative measurement values ​​between the leader and the wingman) will be disturbed by external interference and produce anomalies. Assume that there is a 10m ranging mutation during the ranging process in the following three time periods (630s, 650s), (930s, 950s), and (1230s, 1250s).

[0069] Figure 6The root mean square error of the wingman position obtained by UKF, UKF-ANFIS and UKF-DEANFIS in the case of measurement mutation is given. It can be seen that in the time period without abnormal measurement mutation, the above three algorithms can estimate the position information of the wingman more accurately. However, in the three time periods of (630s, 650s), (930s, 950s) and (1230s, 1250s), due to the influence of abnormal measurement mutation, the performance of traditional UKF is seriously affected, and its position accuracy is significantly reduced. In contrast, UKF-ANFIS and UKF-DEANFIS can significantly reduce the influence of abnormal measurement mutation, and the obtained position error is smaller. Figure 7 The cumulative root mean square errors of the wingman positions obtained by UKF, UKF-ANFIS and UKF-DEANFIS during the period of time when the measurement has abnormal mutations are intuitively compared. It can be seen that UKF has the worst positioning accuracy, with cumulative root mean square errors of 4.35m, 2.18m and 0.29m, respectively. Compared with UKF, UKF-ANFIS and UKF-DEANFIS show strong robustness in suppressing abnormal mutations in measurement. In addition, the estimation accuracy of UKF-DEANFIS is very close to that of UKF-ANFIS. The cumulative root mean square errors of UKF-ANFIS are reduced by 85.98%, 88.53% and 51.72% respectively compared with UKF; the cumulative root mean square errors of UKF-DEANFIS are reduced by 85.52%, 87.16% and 44.83% respectively compared with UKF, and both have stronger robustness.

[0070] Case 2: Non-Gaussian measurement noise: In this case, Gaussian mixture distribution is used to describe non-Gaussian noise, that is, the original Gaussian distribution of the measurement noise is interfered by another Gaussian distribution, generating non-Gaussian measurement noise, that is, ; In the formula, The perturbation parameter representing the noise contamination is set to 0.3 in this case. represents the original Gaussian distribution, and represents a perturbed Gaussian distribution whose standard deviation is 10 times that of the original distribution. In this case, it is assumed that the measurement noise is contaminated by the perturbed Gaussian distribution in the time interval (600s, 1200s).

[0071] The position root mean square error obtained by UKF, UKF-ANFIS and UKF-DEANFIS is as follows: Figure 8As shown. Within (600s, 1200s), due to the influence of the disturbed Gaussian distribution, the above three filters all deviate from their original error amplitudes. Among them, UKF has a poor ability to suppress non-Gaussian noise interference, so the navigation positioning error increases significantly. In contrast, UKF-ANFIS and UKF-DEANFIS use the designed prediction and optimization adjustment strategies to improve the estimation accuracy, effectively overcoming the influence of non-Gaussian measurement noise on navigation solution. In addition, Fig. 9 The cumulative root mean square error of the position when using the above three algorithms is intuitively compared. Compared with UKF, the latitude errors of UKF-ANFIS and UKF-DEANFIS are reduced by 25.58% and 23.26%, respectively, the longitude errors are reduced by 21.05%, and the altitude errors are reduced by 28.57% and 21.43%, respectively. UKF-DEANFIS and UKF-ANFIS show similar navigation estimation accuracy performance, which shows that UKF-DEANFIS retains the strong adaptability and robustness of UKF-ANFIS and can effectively suppress the negative impact of non-Gaussian measurement noise.

[0072] Scenario 3: Missing Measurement: During the flight of a drone swarm, the measurement data of the wingman may be missing due to obstacles or maneuvers of the lead aircraft. To simulate this situation, it is assumed that no measurement data is available in the time intervals (600s, 650s) and (1200s, 1300s).

[0073] Fig.10 The root mean square errors of the positions solved by UKF, UKF-ANFIS and UKF-DEANFIS are given in this case. Fig.11 The author gives an intuitive comparison of the cumulative root mean square error of the position when the wingman position is estimated using the above three filtering algorithms. It can be observed that in the time interval without measurement data, due to the lack of available measurement data, the wingman can only rely on the low-precision MIMU for self-positioning, resulting in a large navigation positioning error. Fig.10The positioning results of UKF in the proposed model diverge. In contrast, UKF-ANFIS and UKF-DEANFIS use MIMU information and the navigation data of the lead aircraft as the input of the ANFIS prediction model to predict the measurement data, thereby effectively alleviating the problem of navigation error divergence caused by the lack of measurement data. Compared with UKF, the latitude errors of UKF-ANFIS and UKF-DEANFIS are reduced by 82.15% and 81.96%, respectively, the longitude errors are reduced by 93.93% and 93.82%, respectively, and the altitude errors are reduced by 81.01% and 78.48%, respectively. The above results show that the proposed UKF-DEANFIS has a strong prediction ability, can suppress the adverse effects of missing measurement data, and provide reliable measurement predictions for MIMU to avoid the divergence of navigation errors.

[0074] (2) Real-time performance evaluation: Based on the above three typical abnormal measurement situations, the proposed UKF-DEANFIS is evaluated by Monte Carlo simulation in terms of computational performance and real-time performance compared with UKF and UKF-ANFIS. TM The Monte Carlo simulation was performed 20 times using Matlab program on a computer with an i9-12900H 2.5GHz processor and 16GB RAM memory.

[0075] In order to exclude the influence of different computer and processor performance, the relative computational efficiency of UKF-ANFIS and UKF-DEANFIS relative to UKF was calculated and Fig.12 The comparison is intuitively made in Figure 1. It can be seen that UKF, as a standard filter, has the shortest computational time for navigation solution. However, it is not robust to abnormal measurement values. The computational time of UKF-ANFIS is almost twice that of UKF (163.34%, 181.49% and 145.02% in the above three cases, respectively), which is much longer than UKF and difficult to achieve real-time performance. At the same time, the proposed UKF-DEANFIS reduces the computational time by 54.59%, 54.96% and 24.97% respectively compared with UKF-ANFIS, which is very close to UKF. This is because UKF-ANFIS executes the designed ANFIS-based measurement prediction and measurement noise covariance optimization strategy at each time step, which introduces a huge computational burden. However, UKF-DEANFIS uses a dual event trigger mechanism to avoid unnecessary computational burden in the time period without abnormal measurement. It only executes the designed ANFIS-based prediction and optimization strategy when the event condition triggers. Therefore, the proposed UKF-DEANFIS can achieve better real-time performance while ensuring the robustness of ANFIS.

[0076] The above simulation results on robustness and real-time performance evaluation show that the proposed UKF-DEANFIS not only retains the superior robustness of ANFIS and enhances the adaptive ability of traditional UKF to deal with the influence of abnormal measurement values, but also achieves better real-time performance than UKF-ANFIS through the constructed dual-event trigger mechanism, thereby improving the application performance of UAV cluster collaborative navigation in complex special environments.

[0077] The step division of the various methods above is only for clear description. When implemented, they can be combined into one step or some steps can be split and decomposed into multiple steps. As long as they include the same logical relationship, they are all within the protection scope of the present invention. Adding insignificant modifications or introducing insignificant designs to the algorithm or process without changing the core design of the algorithm and process are all within the protection scope of the invention.

[0078] Another embodiment of the present invention relates to a drone cluster collaborative navigation and positioning system. The implementation details of the drone cluster collaborative navigation and positioning system of this embodiment are specifically described below. The following content is only for the convenience of understanding the implementation details provided, and is not necessary for the implementation of this solution. The drone cluster collaborative navigation and positioning system of this embodiment includes: A model building module is used to build a measurement model of the UAV cluster collaborative navigation system to obtain collaborative measurement data of the UAV cluster obtained by all navigation sensors in the UAV cluster collaborative navigation system; A noise adjustment module is used to obtain the theoretical innovation covariance and the actual innovation covariance when the unscented Kalman filter UKF performs collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance; A data acquisition module is used to extract relative measurement data of the relative navigation sensor in the UAV cluster cooperative navigation system from the cooperative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; A position / speed prediction module is used to capture the dependency between the relative position / speed calculation value and the relative position / speed measurement value in the relative measurement data, and predict the relative position / speed measurement value through the dependency and the relative position / speed calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / speed measurement value and the relative position / speed calculation value; The navigation and positioning module is used to perform collaborative navigation and positioning of the UAV cluster based on the UKF after adjusting the theoretical measurement noise variance, or to enable the UKF to perform collaborative navigation and positioning of the UAV cluster based on the reconstructed measurement data.

[0079] It is not difficult to find that this embodiment is a system embodiment corresponding to the above method embodiment, and this embodiment can be implemented in conjunction with the above method embodiment. The relevant technical details and technical effects mentioned in the above embodiment are still valid in this embodiment, and in order to reduce repetition, they are not repeated here. Accordingly, the relevant technical details mentioned in this embodiment can also be applied in the above embodiment.

[0080] It is worth mentioning that all modules involved in this embodiment are logic modules. In practical applications, a logic unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. In addition, in order to highlight the innovative part of the present invention, this embodiment does not introduce units that are not closely related to solving the technical problem proposed by the present invention, but this does not mean that there are no other units in this embodiment.

[0081] Another embodiment of the present invention relates to a computer device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the drone cluster collaborative navigation and positioning method in the above-mentioned embodiments.

[0082] Among them, the memory and the processor are connected in a bus manner, and the bus may include any number of interconnected buses and bridges, and the bus connects various circuits of one or more processors and memories together. The bus can also connect various other circuits such as peripherals, voltage regulators, and power management circuits, which are well known in the art and are therefore not further described herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be one element or multiple elements, such as multiple receivers and transmitters, providing a unit for communicating with various other devices on a transmission medium. The data processed by the processor is transmitted on a wireless medium via an antenna, and further, the antenna also receives data and transmits the data to the processor.

[0083] The processor is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interfaces, voltage regulation, power management, and other control functions. Memory can be used to store data used by the processor when performing operations.

[0084] Another embodiment of the present invention relates to a computer-readable storage medium storing a computer program, which implements the above method embodiment when executed by a processor.

[0085] That is, those skilled in the art can understand that all or part of the steps in the above-mentioned embodiment method can be completed by instructing the relevant hardware through a program, and the program is stored in a storage medium, including a number of instructions to enable a device (which can be a single-chip microcomputer, chip, etc.) or a processor to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, referred to as: ROM), random access memory (Random Access Memory, referred to as: RAM), disk or optical disk and other media that can store program codes.

[0086] Those skilled in the art will appreciate that the above embodiments are specific embodiments for implementing the present invention, and in actual applications, various changes may be made thereto in form and detail without departing from the spirit and scope of the present invention.

Claims

1. A drone cluster collaborative navigation and positioning method, characterized in that: The method comprises: Establish a measurement model of the UAV swarm collaborative navigation system to obtain the collaborative measurement data of the UAV swarm obtained by all navigation sensors in the UAV swarm collaborative navigation system; Obtain the theoretical innovation covariance and the actual innovation covariance of the unscented Kalman filter UKF when performing collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance; Extracting relative measurement data of the relative navigation sensor in the UAV cluster cooperative navigation system from the cooperative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; Capturing the dependency between the relative position / velocity calculation value and the relative position / velocity measurement value in the relative measurement data, and predicting the relative position / velocity measurement value through the dependency and the relative position / velocity calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / velocity measurement value and the relative position / velocity calculation value; The collaborative navigation and positioning of the UAV cluster is performed based on the UKF after adjusting the theoretical measurement noise variance, or the UKF is made to perform the collaborative navigation and positioning of the UAV cluster based on the reconstructed measurement data.

2. The UAV cluster collaborative navigation and positioning method according to claim 1 is characterized in that: The adjusting of the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance includes: Inputting the theoretical innovation covariance and the actual innovation covariance into the first adaptive neuro-fuzzy inference network ANFIS; The matching degree between the theoretical innovation covariance and the actual innovation covariance is obtained through the ANFIS network, and an adjustment coefficient for adjusting the theoretical measurement noise variance is output according to the matching degree, so that the theoretical innovation covariance after adjusting the theoretical noise variance is equal to the actual innovation covariance.

3. The UAV cluster collaborative navigation and positioning method according to claim 1 is characterized in that: The method captures the dependency between the relative position / speed calculation value and the relative position / speed measurement value in the relative measurement data, and predicts the relative position / speed measurement value through the dependency and the relative position / speed calculation value, including: The second ANFIS network is trained using the relative position / speed calculation values ​​and relative position / speed measurement values ​​of the leader and wingman of several UAV clusters as sample data to obtain an ANFIS model, so as to capture the dependency between the relative position / speed calculation values ​​and the relative position / speed measurement values ​​through the ANFIS model; The relative position / velocity calculations are input into the ANFIS model to obtain the predicted relative position / velocity measurements.

4. The UAV cluster collaborative navigation and positioning method according to claim 1, characterized in that: Before obtaining the theoretical innovation covariance and the actual innovation covariance when the unscented Kalman filter UKF performs collaborative navigation positioning according to the collaborative measurement data, the method further includes: Construct anomaly detection functions for collaborative measurement data; The anomaly detection function is: ； In the formula, is a unit vector, and The element is 1, is the dimension of the measurement vector, is the innovation vector, is the corresponding covariance matrix; According to the anomaly detection function, a first event trigger condition is set, and according to the first event trigger condition, whether the collaborative measurement data is abnormal; The first event trigger condition is: ； In the formula, is the false alarm rate, is the detection threshold.

5. The UAV cluster collaborative navigation and positioning method according to claim 4 is characterized in that: Before obtaining the theoretical innovation covariance and the actual innovation covariance when the unscented Kalman filter UKF performs collaborative navigation positioning according to the collaborative measurement data, the method further includes: If the collaborative measurement data is determined to be abnormal, a relative position change index of the collaborative measurement data is constructed; The relative position change index is: ； In the formula, Indicates the measurement dimension involving anomalies, is the total dimension of the measurement, In isolation After dimensional measurement, the state estimation error covariance matrix diagonal elements, is the first value of the state estimation error covariance matrix during UKF filtering. diagonal elements; According to the relative position change index, a second event trigger condition is set, and according to the second event trigger condition, whether to adjust the theoretical measurement noise variance or whether to predict the relative position / speed measurement value; The second event trigger condition is: ； In the formula, is the threshold for the second event trigger.

6. A drone cluster collaborative navigation and positioning system, characterized in that: The system comprises: A model building module is used to build a measurement model of the UAV cluster collaborative navigation system to obtain collaborative measurement data of the UAV cluster obtained by all navigation sensors in the UAV cluster collaborative navigation system; A noise adjustment module is used to obtain the theoretical innovation covariance and the actual innovation covariance when the unscented Kalman filter UKF performs collaborative navigation positioning based on collaborative measurement data, and adjust the theoretical measurement noise variance in the theoretical innovation covariance so that the theoretical innovation covariance after adjusting the theoretical measurement noise variance is equal to the actual innovation covariance; A data acquisition module is used to extract relative measurement data of the relative navigation sensor in the UAV cluster cooperative navigation system from the cooperative measurement data; wherein the relative measurement data is composed of the relative position / speed calculation value between the leader and the wingman in the UAV cluster and the relative position / speed measurement value between the leader and the wingman measured by the relative navigation sensor; A position / speed prediction module is used to capture the dependency between the relative position / speed calculation value and the relative position / speed measurement value in the relative measurement data, and predict the relative position / speed measurement value through the dependency and the relative position / speed calculation value, so as to reconstruct the relative measurement data by combining the predicted relative position / speed measurement value and the relative position / speed calculation value; The navigation and positioning module is used to perform collaborative navigation and positioning of the UAV cluster based on the UKF after adjusting the theoretical measurement noise variance, or to enable the UKF to perform collaborative navigation and positioning of the UAV cluster based on the reconstructed measurement data.

7. A computer device, characterized in that: include: at least one processor; And, a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the drone cluster collaborative navigation and positioning method as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the drone cluster collaborative navigation and positioning method according to any one of claims 1 to 5 is implemented.

Images