Multi-unmanned aerial vehicle multi-source collaborative navigation method and equipment

Abstract

The invention provides a multi-unmanned aerial vehicle multi-source collaborative navigation method and equipment, and relates to the field of unmanned aerial vehicle control, and the method comprises the following steps: S1, carrying out dynamic adjustment on an unmanned aerial vehicle control model, and calculating to obtain a process noise covariance matrix and an observation noise covariance matrix of each slave unmanned aerial vehicle at the current moment; s2, inputting unmanned aerial vehicle positioning data into an unmanned aerial vehicle control model, and calculating to obtain an actual distance between each slave unmanned aerial vehicle and the master unmanned aerial vehicle; s3, a GDOP value is obtained through calculation of the actual distances and the satellite positioning data; and S4, training the BP neural network through the GDOP value, each process noise covariance matrix, each observation noise covariance matrix and each actual distance, and obtaining the actual positions of the master unmanned aerial vehicle and each slave unmanned aerial vehicle after the training is completed. According to the method, the covariance matrix and the mean vector are updated and controlled by adopting asynchronous message transmission, so that efficient information fusion and global rapid convergence of the system are realized, and the cooperative navigation performance of multiple unmanned aerial vehicles is optimized.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle control, and in particular to a multi-unmanned aerial vehicle multi-source collaborative navigation method and equipment. Background Art

[0002] An Unmanned Aerial Vehicle (UAV) is a low-cost, highly maneuverable and survivable aerial vehicle that can be controlled completely autonomously. It is particularly suitable for performing tasks over long distances, in high-risk, boring and harsh environments. It is therefore also called a 4D (Deep, Dangerous, Dull, Dirty) aircraft.

[0003] At present, there have been a series of targeted studies on multi-source information fusion. When UAVs use the combined navigation technology of the Global Navigation Satellite System / Inertial Navigation System (GNSS / INS) for positioning and attitude determination, in complex environments, satellite signals are interfered with, the received measurement information is limited, and the positioning accuracy is affected. Therefore, there are still problems to be solved in enabling formation flight and collaborative operation of multiple UAVs, such as: the collaborative positioning effect is highly dependent on the accuracy of the relative position estimation between UAVs. If the relative position estimation deviates, the performance of the entire collaborative positioning system will be affected. Due to the randomness of the weighting factor, the fusion algorithm has not been able to perform fusion well. For problems such as large errors in solving the position information between multiple collaborative navigation UAVs and poor positioning accuracy. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a multi-UAV multi-source collaborative navigation method and equipment to solve the technical problems of large error in solving position information and poor positioning accuracy between multiple collaborative navigation UAVs.

[0005] The present invention provides a multi-UAV multi-source collaborative navigation method, comprising the steps of:

[0006] S1: Construct the UAV control model, dynamically adjust the UAV control model, and calculate the process noise covariance matrix and observation noise covariance matrix of each slave UAV at the current moment;

[0007] S2: Obtain the drone positioning data, input the drone positioning data into the drone control model, and calculate the actual distance between each slave drone and the master drone;

[0008] S3: Obtain satellite positioning data, and calculate the GDOP value through the actual distance and satellite positioning data;

[0009] S4: Construct a BP neural network, train the BP neural network through the GDOP value, each process noise covariance matrix, each observation noise covariance matrix and each actual distance, and obtain the actual positions of the master UAV and each slave UAV after the training is completed.

[0010] Preferred:

[0011] The UAV control model includes: state space equations and observation equations;

[0012] The state space equation is expressed as:

[0013] x m,k+1 =F m,k x m,k +G m,k w m,k

[0014] x n,k+1 =F n,k x n,k +G n,k w n,k

[0015] Among them, k is the current time, x m,k 、F m,k , G m,k and w m,k are the state error vector, state transfer matrix, process noise matrix and process noise vector of the master drone m, respectively, n,k 、F n,k , G n,k and w n,k are the state error vector, state transfer matrix, process noise matrix and process noise vector from UAV n respectively;

[0016] The expression of the observation equation is:

[0017] Z k =H k X k +V k

[0018] Among them, Z k is the observed quantity, H k is the observation matrix, X k is the state vector, V k is the measurement noise matrix.

[0019] Preferably, step S1 specifically comprises:

[0020] S11: Set the parameter α and obtain the process noise covariance matrix Q from drone n at time k-1 through the drone control model. n,k-1 , and the process noise estimation matrix at time k Calculate the process noise covariance matrix Q at time k n,k , the expression is: k is the current time;

[0021] S12: Set the parameter β and obtain the observation noise covariance matrix R from drone n at time k-1 through the drone control model n,k-1 , and the observation noise estimation matrix at time k Calculate the observation noise covariance matrix R at time k n,k , the expression is:

[0022] S13: Repeat steps S11 to S12 to obtain the process noise covariance matrix and the observation noise covariance matrix of each slave UAV at the current moment.

[0023] Preferred:

[0024] The expression of the actual distance between the slave drone and the master drone is:

[0025]

[0026] Among them, d n is the actual distance between the slave UAV n and the master UAV m, (x m ,y m ,z m ) is the three-dimensional coordinate of the main UAV m in the inertial coordinate system, (x n ,y n ,z n ) is the three-dimensional coordinate of the slave UAV n in the inertial coordinate system.

[0027] Preferably, step S3 specifically comprises:

[0028] S31: Obtain the unit direction vector from each satellite to the receiver through calculation using satellite positioning data. The calculation formula is:

[0029]

[0030] Where i is the satellite number, u is the satellite i is the unit direction vector from the ith satellite to the receiver, is the position vector of satellite i, r = [r 1 ,r 2 ,r 3 ] T is the position vector of the receiver;

[0031] S32: Calculate the relative direction vector between each slave UAV and the master UAV based on the actual distance between each slave UAV and the master UAV. The calculation formula is:

[0032]

[0033] Among them, u m,n is the relative direction vector between the slave UAV n and the master UAV m;

[0034] S33: Construct a mixed direction matrix U through each unit direction vector and each relative direction vector K , the expression is: K =u 1 ,u 2 ,…,u I ,u m,1 ,u m,2 ,…,u m,N ]; where I is the total number of satellites and N is the total number of slave drones;

[0035] S34: By mixing the direction matrix U K The GDOP value is calculated using the following formula:

[0036]

[0037] Wherein, GDOP is the GDOP value, and tr represents the trace of the obtained matrix.

[0038] Preferably, step S4 is specifically:

[0039] S41: The process noise covariance matrix Q of each slave drone at time k n,k , calculate the process noise covariance at time k The observation noise covariance matrix R of each slave UAV at time k n,k-1 , calculate the observation noise variance at time k k is the current time;

[0040] S42: Input each actual distance into the BP neural network, calculate and obtain the predicted position of the master UAV and each slave UAV, and the predicted position error of each slave UAV;

[0041] S43: Obtain the true position error of each slave drone through GDOP value and process noise covariance Observation noise variance The actual position error and the predicted position error are used to calculate the loss function E;

[0042] S44: Obtain weight coefficients between slave drones through calculation using the loss function E;

[0043] S45: updating the control covariance matrix and mean vector between the slave drones through the weight coefficients;

[0044] S46: Repeat steps S41-S45 until the loss function E is less than a preset value, and use the output predicted positions of the master drone and each slave drone as the actual positions.

[0045] Preferred:

[0046] The expression of the loss function E is:

[0047]

[0048] Where n is the number of the slave drone, N is the total number of slave drones, and y n is the true position error from UAV n, is the predicted position error from UAV n.

[0049] Preferred:

[0050] The expression of weight coefficient is:

[0051]

[0052] Among them, n and j are the numbers of the slave drones, w n→j is the weight coefficient between slave UAV n and slave UAV j, η is the step size of the control weight update, is the control covariance matrix between slave UAV n and slave UAV j, and tr represents the trace of the obtained matrix.

[0053] Preferred:

[0054] The update expression of the control covariance matrix is:

[0055]

[0056] Among them, p is the serial number of the slave drone, is the inverse of the updated control covariance matrix, N(j) is the set of neighboring slave drones of slave drone j, is the inverse of the control covariance matrix estimated from the current state of UAV j, Λ n→j is the inverse of the control covariance matrix estimated between the current states of slave UAV n and slave UAV j;

[0057] The update expression of the mean vector is:

[0058]

[0059] in, is the updated mean vector, is the updated control covariance matrix, μ j is the mean vector estimated from the current state of UAV j, μ n→jis the mean vector of the current state estimate between slave UAV n and slave UAV j.

[0060] A multi-UAV multi-source collaborative navigation device comprises: a processor and a storage medium; the processor loads and executes instructions and data in the storage medium to implement the multi-UAV multi-source collaborative navigation method.

[0061] The present invention has the following beneficial effects:

[0062] 1. By calculating the distance between the satellite and the receiver and the relative distance between the master and slave drones, the GDOP value is calculated to determine the optimal geometric configuration of the drone group to ensure higher positioning accuracy.

[0063] 2. By setting parameters α and β to dynamically adjust the process noise covariance matrix and the observation noise covariance matrix, the robustness of the EKF algorithm in nonlinear dynamic environments is improved, which is used to optimize the collaborative navigation of multiple UAVs.

[0064] 3. Dynamically adjust the weight and threshold of the BP neural network loss function through the weight coefficient, reduce the interference of low-credibility information, improve the estimation accuracy and system robustness. At the same time, adopt asynchronous message passing to update the control covariance matrix and mean vector, realize efficient information fusion and global rapid convergence of the system, and optimize the collaborative navigation performance of multiple UAVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a flow chart of a method according to an embodiment of the present invention;

[0066] Figure 2 3D trajectory map of the main UAV;

[0067] Figure 3 is the graph of GDOP value changing with time;

[0068] Figure 4 is the longitude error after host filtering;

[0069] Figure 5 is the latitude error after filtering by the host;

[0070] Figure 6 RMSE position error map of the main UAV;

[0071] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0072] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0073] Reference Figure 1The present invention provides a multi-UAV multi-source collaborative navigation method, comprising the steps of:

[0074] S1: Construct the UAV control model, dynamically adjust the UAV control model, and calculate the process noise covariance matrix and observation noise covariance matrix of each slave UAV at the current moment;

[0075] As an embodiment, for the UAV control model, the master UAV corrects the inertial navigation error through the tight coupling of GNSS / INS / UWB, uses the position information received by GNSS and the position information and angular rate output by INS to estimate the error, and fuses the data through the extended Kalman filter. The slave UAV relies on UWB information and inertial navigation information as well as the status information of the master UAV, and corrects its error through a loose combination to achieve collaborative navigation and information interaction within the cluster. UWB is used for relative positioning to achieve high-precision relative position measurement between multiple UAVs.

[0076] The UAV control model includes: state space equations and observation equations;

[0077] The state error of the master drone is composed of errors such as position, speed, attitude, gyroscope drift and accelerometer bias. The slave drone does not directly receive GNSS signals, but obtains position information through relative navigation.

[0078] The state space equation is expressed as:

[0079] x m,k+1 =F m,k x m,k +G m,k w m,k

[0080] x n,k+1 =F n,k x n,k +G n,k w n,k

[0081] Among them, k is the current time, x m,k 、F m,k , G m,k and w m,k are the state error vector, state transfer matrix, process noise matrix and process noise vector of the master drone m, respectively, n,k 、F n,k , G n,k and w n,k are the state error vector, state transfer matrix, process noise matrix and process noise vector from UAV n respectively;

[0082] The expression of the observation equation is:

[0083] Z k =H k X k +V k

[0084] Among them, Z k is the observed quantity, H k is the observation matrix, X k is the state vector, V k is the measurement noise matrix.

[0085] As an example, in the traditional EKF algorithm, it is assumed that the process noise covariance matrix Q n,k and the observation noise covariance matrix R n,k The value is fixed. However, in actual multi-UAV cooperative navigation, the noise characteristics may change dynamically with the change of the environment. The present invention proposes a method for dynamic noise covariance adjustment. Specifically, at each filter update, the process noise covariance matrix Q is dynamically adjusted according to the latest state estimation and observation residual of the UAV. n,k and the observation noise covariance matrix R n,k , the specific process is as follows:

[0086] Step S1 is specifically as follows:

[0087] S11: Set the parameter α and obtain the process noise covariance matrix Q from drone n at time k-1 through the drone control model. n,k-1 , and the process noise estimation matrix at time k Calculate the process noise covariance matrix Q at time k n,k , the expression is: k is the current time;

[0088] S12: Set the parameter β and obtain the observation noise covariance matrix R from drone n at time k-1 through the drone control model n,k-1 , and the observation noise estimation matrix at time k Calculate the observation noise covariance matrix R at time k n,k , the expression is:

[0089] S13: Repeat steps S11 to S12 to obtain the process noise covariance matrix and the observation noise covariance matrix of each slave UAV at the current moment.

[0090] Specifically, the smoothing factor parameters α and β are used to adjust the historical covariance and the current noise estimate. α and β are real numbers between 0 and 1, which are used to update Q n,k and R n,kThrough trial and error, it was found that the collaborative positioning error was smaller after setting both α and β to 0.5, so their values ​​were determined to be 0.5, which means that when updating the noise covariance matrix, historical information and current estimates have the same weight, taking into account the stability and adaptability of the system, which can reduce the number of parameters that need to be adjusted and reduce the difficulty and cost of algorithm implementation.

[0091] S2: Obtain the drone positioning data, input the drone positioning data into the drone control model, and calculate the actual distance between each slave drone and the master drone;

[0092] As an example:

[0093] The expression of the actual distance between the slave drone and the master drone is:

[0094]

[0095] Among them, d n is the actual distance between the slave UAV n and the master UAV m, (x m ,y m ,z m ) is the three-dimensional coordinate of the main UAV m in the inertial coordinate system, (x n ,y n ,z n ) is the three-dimensional coordinate of the slave UAV n in the inertial coordinate system.

[0096] S3: Obtain satellite positioning data, and calculate the GDOP value through the actual distance and satellite positioning data;

[0097] As an embodiment, in a multi-UAV collaborative navigation system, the present invention uses the value of the geometric dilution of precision (GDOP) as an indicator to evaluate the quality of the geometric configuration of the sensor arrangement, reflecting the influence of the relative geometric position between the sensor and the target on the measurement accuracy. A data fusion model is established between multiple UAVs by combining multiple sensors such as inertial navigation and inertial measurement units. The minimum GDOP value is solved by the distance between the satellite and the receiver and the relative distance between the master and slave UAVs, and the optimal geometric configuration that the beacon UAV and the target UAV can achieve is determined. In the present invention, when the master UAV moves along the set trajectory, the slave UAV maintains an appropriate distance and collects information through monitoring sensors in different directions.

[0098] Step S3 is specifically as follows:

[0099] S31: Obtain the unit direction vector from each satellite to the receiver through calculation using satellite positioning data. The calculation formula is:

[0100]

[0101] Where i is the satellite number, u is the satellite i is the unit direction vector from the ith satellite to the receiver, is the position vector of satellite i, r = [r 1 ,r 2 ,r 3 ] T is the position vector of the receiver;

[0102] S32: Calculate the relative direction vector between each slave UAV and the master UAV based on the actual distance between each slave UAV and the master UAV. The calculation formula is:

[0103]

[0104] Among them, u m,n is the relative direction vector between the slave UAV n and the master UAV m;

[0105] S33: Construct a mixed direction matrix U through each unit direction vector and each relative direction vector K , the expression is: K =u 1 ,u 2 ,…,u I ,u m,1 ,u m,2 ,…,u m,N ]; where I is the total number of satellites and N is the total number of slave drones;

[0106] S34: By mixing the direction matrix U K The GDOP value is calculated using the following formula:

[0107]

[0108] Wherein, GDOP is the GDOP value, and tr represents the trace of the obtained matrix.

[0109] Specifically, it is found through calculation that when the main drone is 45 degrees to the ground and the flight angle of the drone is designed to be 135 degrees, -45 degrees and -135 degrees, the unit direction vector u i The angle between them becomes larger, u i As dispersed as possible in space, the receiver receives the signal from multiple angles, u i The dot product between them will be small. All eigenvalues ​​of are as close to the identity matrix as possible, which will have good properties, and the inverse matrix The trace is small, so the GDOP value is small.

[0110] This geometric configuration effectively disperses the positions of UAVs in space, ensuring the relative distance and height difference between UAVs, forming a certain depth, increasing the diversity of received signals, and enabling the positioning system to obtain spatial information more comprehensively, which helps to maximize observation capabilities when performing positioning tasks, thereby optimizing the GDOP value and ensuring higher positioning accuracy.

[0111] In addition to the theoretical advantages, this geometric configuration helps to avoid visual interference between drones, reduce energy consumption during flight, or improve flight efficiency. Therefore, in this simulation, the master drone moves at a speed of 10 meters per second in a direction 45 degrees to the ground, starting at an altitude of 120 meters. The three slave drones also fly at the same speed, but in different directions (135 degrees, -45 degrees, and -135 degrees), maintaining relative positions.

[0112] S4: Construct a BP neural network, train the BP neural network through the GDOP value, each process noise covariance matrix, each observation noise covariance matrix and each actual distance, and obtain the actual positions of the master UAV and each slave UAV after the training is completed.

[0113] As an example, Gaussian Belief Propagation (GABP) is a distributed algorithm for inferring multivariate Gaussian distribution, which is widely used in signal processing, machine learning, network systems and other fields. The GABP algorithm consists of a genetic algorithm (GA) and a BP neural network (BP). The GA optimizes and modifies the weights and biases of the BP neural network according to the situation of the BP neural network to improve the prediction performance of the network, and the BP neural network adjusts the weights and biases through the back propagation algorithm to minimize the prediction error.

[0114] In the distributed filtering and information fusion system, each slave drone updates its own state estimation after receiving information from other slave drones. The traditional GABP algorithm gives the same weight to all information sources in the message update, and fails to fully consider the uncertainty differences of each information source. This method may cause the fusion result to be disturbed by low-credibility information, thereby affecting the accuracy and robustness of the state estimation. In order to improve the accuracy and robustness of the fusion result, the present invention proposes the W-GABP algorithm, which introduces a dynamic weight coefficient to quantify the message uncertainty and enhance the influence of high-credibility information in the asynchronous message transmission process.

[0115] Step S4 is specifically as follows:

[0116] S41: The process noise covariance matrix Q of each slave drone at time k n,k , calculate the process noise covariance at time k The observation noise covariance matrix R of each slave UAV at time kn,k-1 , calculate the observation noise variance at time k k is the current time;

[0117] S42: Input each actual distance into the BP neural network, calculate and obtain the predicted position of the master UAV and each slave UAV, and the predicted position error of each slave UAV;

[0118] S43: Obtain the true position error of each slave drone through GDOP value and process noise covariance Observation noise variance The actual position error and the predicted position error are used to calculate the loss function E;

[0119] Specifically, a weight is assigned to each received information to reflect its uncertainty, and the loss function E is used to reflect the accuracy of the model prediction and its robustness to noise;

[0120] The expression of the loss function E is:

[0121]

[0122] Where n is the number of the slave drone, N is the total number of slave drones, and y n is the true position error from UAV n, is the predicted position error from UAV n.

[0123] S44: Obtain weight coefficients between slave drones through calculation using the loss function E;

[0124] Specifically, the weight coefficient measures the uncertainty of messages from one drone to another;

[0125] The expression of weight coefficient is:

[0126]

[0127] Among them, n and j are the numbers of the slave drones, w n→j is the weight coefficient between slave UAV n and slave UAV j, η is the step size of the control weight update, is the control covariance matrix between slave UAV n and slave UAV j, and tr represents the trace of the obtained matrix.

[0128] Specifically, when updating the state estimate of the slave drone j, the weight coefficient w is used n→jThe information received from the slave drone n is weighted. The smaller the GDOP value is, the smaller E is made by acting on the loss function, which affects the calculation of the weights and the higher the weight of the slave drone is. The slave drone updates the current state estimate after receiving some messages without waiting for messages from all neighboring slave drones, and immediately sends new messages to other neighboring slave drones. The message transmission of each slave drone is asynchronous, allowing the slave drone to process part of the information independently. The modified message update rule is implemented in the filtering process. By calculating the weight coefficient w n→j , the trace of the matrix is ​​converted into a quantity independent of the matrix to achieve normalization, the message uncertainty is quantified, and dynamic weights are applied in the asynchronous message passing process to enhance the influence of high-confidence information, thereby optimizing the state estimation of the slave UAV.

[0129] S45: updating the control covariance matrix and mean vector between the slave drones through the weight coefficients;

[0130] Specific:

[0131] The update expression of the control covariance matrix is:

[0132]

[0133] Among them, p is the serial number of the slave drone, is the inverse of the updated control covariance matrix, N(j) is the set of neighboring slave drones of slave drone j, is the inverse of the control covariance matrix estimated from the current state of UAV j, Λ n→j is the inverse of the control covariance matrix estimated between the current states of slave UAV n and slave UAV j;

[0134] The update expression of the mean vector is:

[0135]

[0136] in, is the updated mean vector, is the updated control covariance matrix, μ j is the mean vector estimated from the current state of UAV j, μ n→j is the mean vector of the current state estimate between slave UAV n and slave UAV j.

[0137] S46: Repeat steps S41-S45 until the loss function E is less than a preset value, and use the output predicted positions of the master drone and each slave drone as the actual positions.

[0138] Experimental simulation settings:

[0139] To verify the effectiveness of the designed multi-source fusion navigation system with distributed collaborative positioning and to meet the relevant verification requirements for the multi-UAV cooperative navigation algorithm based on GNSS / INS / UWB combination proposed in the present invention, the following assumptions are proposed in the present invention:

[0140] One master UAV and three slave UAVs are adopted, and the flight requirements between each UAV satisfy the following relationships:

[0141] Table 1 Sensor Configuration and Simulation Error Parameters

[0142]

[0143] Using the complex mountainous and dense forest environment and in the case of insufficient cluster positioning accuracy and fusion strategy, a simulation experimental study on cooperative navigation was carried out. The integrated navigation system in the simulation consists of an inertial navigation / satellite navigation / ultra-wideband system. The total duration of the simulation experiment is 450 seconds, and a simulation platform is built using MATLAB for simulation verification.

[0144] A flight path is designed for the master UAV, as shown in Table 2 specifically. The trajectories of the slave UAVs are calculated based on the trajectory of the master UAV and the relative position relationship between the master and slave UAVs. All UAVs can receive their own position information through satellite navigation equipment, and information interaction can be carried out between adjacent UAVs.

[0145] Table 2 UAV Flight Path

[0146]

[0147] Based on the multi-UAV cooperative positioning model proposed in the present invention, the flight path is as Figure 2 shown;

[0148] Simulation Results and Analysis

[0149] Based on the above simulation conditions, the accuracy of the GABP algorithm of the present invention is analyzed. To make the verification representative, the W_GABP algorithm of the present invention is compared with the cooperative navigation algorithm based on EKF

[46] , the single-UAV positioning algorithm based on EKF (C_EKF), and the positioning results of the cooperative navigation algorithm based on the original GABP (GABP_EKF)

[47] . To quantitatively analyze the positioning accuracy of the four different navigation algorithms, the positioning error of the UAV cluster is calculated.

[0150] Under the original conditions, the variation of the GDOP value with time in the case of single-UAV N_EKF and cooperative positioning of one master and three slave UAVs is as Figure 3 shown.

[0151] At Figure 3In the figure, the blue line shows the GDOP value change of a single UAV system, and the red line represents the C_GDOP value change when four UAVs, one master and three slaves, work together. Through comparative analysis, it can be clearly seen that under the coordination of multiple UAVs, the GDOP value is significantly smaller than the GDOP of a single UAV. The addition of slave UAVs to assist the master UAV in positioning and measurement has significantly improved the spatial geometric configuration of the UAV system. This optimization of geometric configuration reduces the GDOP value, thereby improving the overall positioning accuracy and reliability of the UAV system. As time goes by, the geometric configuration between UAVs will deteriorate, so the GDOP value will increase.

[0152] Figure 4 The curves showing the change of the longitude positioning error of the master and slave drones over time are shown. Figure 5 The curves of the latitude positioning errors of the master UAV and the slave UAV changing over time are shown.

[0153] from Figure 4 , Figure 5 It can be seen from the positioning error statistics that, compared with the final divergence of the latitude and longitude positioning results of a single UAV without coordination, the three cooperative navigation algorithms have achieved the fusion of navigation information within the UAV cluster, and have significantly improved the positioning effect of longitude and latitude. From the GDOP value change diagram, it can be seen that with the passage of time, the GDOP value will gradually increase, and the effect on the UAV will increase the longitude and latitude errors between UAVs. After the UAV runs around the track for 3 circles, the N_EKF algorithm has a large increase in the GDOP value, resulting in a dispersed increase in the error. The error result of the EKF positioning algorithm is relatively flat compared to the N_EKF positioning, but the error also shows an upward trend as the GDOP value increases. The original GABP algorithm has a greatly reduced error of 0.46m, but it also shows an upward trend with the passage of time. The algorithm of the present invention improves the navigation positioning accuracy by 10 times compared with the single EKF positioning, and the positioning error is reduced to within 0.2 meters, which fully demonstrates the effectiveness of the algorithm of the present invention.

[0154] To ensure flight safety, the altitude of drones is limited to between 50 and 300 meters. At the same time, to avoid mutual interference, the height difference between any two drones must not be less than 1.5 meters, and the horizontal distance must not be less than 5 meters. If it is found that the height difference or horizontal distance between drones does not meet the requirements, their altitude will be automatically increased by 10 meters to maintain a safe distance.

[0155] To analyze the performance of the algorithm of the present invention, the number of iterations is 200 and the weight update factor is 0.01 during the algorithm simulation process. The Euclidean distance between the global optimal position and the preset target position of the main UAV is calculated by iteratively using the improved GABP algorithm to obtain the main UAV position error iteration graph, as shown in Figure 6As shown in the figure, as the iteration progresses, the difference between the main UAV position and the expected position gradually decreases. The algorithm effectively adjusts the position of the UAV during the iteration process, making it gradually approach the preset target position. It can be concluded that the GABP algorithm shows good convergence and accuracy in the UAV cluster optimization task.

[0156] Table 3 Comparison of algorithm positioning error statistics RMSE

[0157]

[0158] The present invention provides a multi-UAV multi-source collaborative navigation device, a processor and a storage medium.

[0159] A multi-UAV multi-source collaborative navigation device: The multi-UAV multi-source collaborative navigation device implements the multi-UAV multi-source collaborative navigation method.

[0160] Processor: The processor loads and executes the instructions and data in the storage medium to implement the multi-UAV multi-source collaborative navigation method.

[0161] Storage medium: The storage medium stores instructions and data; the storage medium is used to implement the multi-UAV multi-source collaborative navigation method.

[0162] It should be noted that, in the present invention, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0163] The serial numbers of the embodiments of the present invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments. In a unit claim that lists several means, several of these means may be embodied by the same hardware item. The use of the words first, second, and third, etc. does not indicate any order and these words may be interpreted as identifiers.

[0164] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A multi-UAV multi-source collaborative navigation method, characterized in that: Includes steps: S1: Construct the UAV control model, dynamically adjust the UAV control model, and calculate the process noise covariance matrix and observation noise covariance matrix of each slave UAV at the current moment; S2: Obtain the drone positioning data, input the drone positioning data into the drone control model, and calculate the actual distance between each slave drone and the master drone; S3: Obtain satellite positioning data, and calculate the GDOP value through the actual distance and satellite positioning data; S4: Construct a BP neural network, train the BP neural network through the GDOP value, each process noise covariance matrix, each observation noise covariance matrix and each actual distance, and obtain the actual positions of the master UAV and each slave UAV after the training is completed.

2. The multi-UAV multi-source collaborative navigation method according to claim 1, characterized in that: The UAV control model includes: state space equations and observation equations; The state space equation is expressed as: x m,k+1 =F m,k x m,k +G m,k w m,k x n,k+1 =F n,k x n,k +G n,k w n,k Among them, k is the current time, x m,k 、F m,k , G m,k and w m,k are the state error vector, state transfer matrix, process noise matrix and process noise vector of the master drone m, respectively, n,k 、F n,k , G n,k and w n,k are the state error vector, state transfer matrix, process noise matrix and process noise vector from UAV n respectively; The expression of the observation equation is: Z k =H k X k +V k Among them, Z k is the observed quantity, H k is the observation matrix, X k is the state vector, V k is the measurement noise matrix.

3. The multi-UAV multi-source collaborative navigation method according to claim 1, characterized in that: Step S1 is specifically as follows: S11: Set the parameter α and obtain the process noise covariance matrix Q from drone n at time k-1 through the drone control model. n,k-1 , and the process noise estimation matrix at time k Calculate the process noise covariance matrix Q at time k n,k , the expression is: k is the current time; S12: Set the parameter β and obtain the observation noise covariance matrix R from drone n at time k-1 through the drone control model n,k-1 , and the observation noise estimation matrix at time k Calculate the observation noise covariance matrix R at time k n,k , the expression is: S13: Repeat steps S11 to S12 to obtain the process noise covariance matrix and the observation noise covariance matrix of each slave UAV at the current moment.

4. The multi-UAV multi-source collaborative navigation method according to claim 1, characterized in that: The expression of the actual distance between the slave drone and the master drone is: Among them, d n is the actual distance between the slave UAV n and the master UAV m, (x m ,y m ,z m ) is the three-dimensional coordinate of the main UAV m in the inertial coordinate system, (x n ,y n ,z n ) is the three-dimensional coordinate of the slave UAV n in the inertial coordinate system.

5. The multi-UAV multi-source collaborative navigation method according to claim 4 is characterized in that: Step S3 is specifically as follows: S31: Obtain the unit direction vector from each satellite to the receiver through calculation using satellite positioning data. The calculation formula is: Where i is the satellite number, u is the satellite i is the unit direction vector from the ith satellite to the receiver, is the position vector of satellite i, r=[r1,r2,r3] T is the position vector of the receiver; S32: Calculate the relative direction vector between each slave UAV and the master UAV based on the actual distance between each slave UAV and the master UAV. The calculation formula is: Among them, u m,n is the relative direction vector between the slave UAV n and the master UAV m; S33: Construct a mixed direction matrix U through each unit direction vector and each relative direction vector K , the expression is: K =[u1,u2,…,u I ,u m,1 ,u m,2 ,…,u m,N ]; where I is the total number of satellites and N is the total number of slave drones; S34: By mixing the direction matrix U K The GDOP value is calculated using the following formula: Wherein, GDOP is the GDOP value, and tr represents the trace of the obtained matrix.

6. The multi-UAV multi-source collaborative navigation method according to claim 1, characterized in that: Step S4 is specifically as follows: S41: The process noise covariance matrix Q of each slave drone at time k n,k , calculate the process noise covariance at time k The observation noise covariance matrix R of each slave UAV at time k n,k-1 , calculate the observation noise variance at time k k is the current time; S42: Input each actual distance into the BP neural network, calculate and obtain the predicted position of the master UAV and each slave UAV, and the predicted position error of each slave UAV; S43: Obtain the true position error of each slave drone through GDOP value and process noise covariance Observation noise variance The actual position error and the predicted position error are used to calculate the loss function E; S44: Obtain weight coefficients between slave drones through calculation using the loss function E; S45: updating the control covariance matrix and mean vector between the slave UAVs through the weight coefficients; S46: Repeat steps S41-S45 until the loss function E is less than a preset value, and use the output predicted positions of the master drone and each slave drone as the actual positions.

7. The multi-UAV multi-source collaborative navigation method according to claim 6, characterized in that: The expression of the loss function E is: Where n is the number of the slave drone, N is the total number of slave drones, and y n is the true position error from UAV n, is the predicted position error from UAV n.

8. The multi-UAV multi-source collaborative navigation method according to claim 6, characterized in that: The expression of weight coefficient is: Among them, n and j are the numbers of the slave drones, w n→j is the weight coefficient between slave UAV n and slave UAV j, η is the step size of the control weight update, is the control covariance matrix between slave UAV n and slave UAV j, and tr represents the trace of the obtained matrix.

9. The multi-UAV multi-source collaborative navigation method according to claim 8, characterized in that: The update expression of the control covariance matrix is: Among them, p is the serial number of the slave drone, is the inverse of the updated control covariance matrix, N(j) is the set of neighboring slave drones of slave drone j, is the inverse of the control covariance matrix estimated from the current state of UAV j, Λ n→j is the inverse of the control covariance matrix estimated between the current states of slave UAV n and slave UAV j; The update expression of the mean vector is: in, is the updated mean vector, is the updated control covariance matrix, μ j is the mean vector estimated from the current state of UAV j, μ n→j is the mean vector of the current state estimate between slave UAV n and slave UAV j.

10. A multi-UAV multi-source collaborative navigation device, characterized in that: include: Processor and storage medium; the processor loads and executes instructions and data in the storage medium to implement the multi-UAV multi-source collaborative navigation method described in any one of claims 1 to 9.

Images