Collaborative positioning method and system based on confidence factor from preferred distributed EKF of unmanned aerial vehicle

Abstract

The application discloses a master-slave positioning method and system based on a confidence factor from a UAV preferably distributed EKF, and relates to the technical field of UAV positioning. i , if the master UAV is invalid, calculating the confidence of each slave UAV i , selecting the slave UAV with the highest confidence, upgrading it to a temporary master UAV; constructing a state equation and a measurement equation of the slave UAV i; calculating an absolute measurement gain matrix and a relative measurement gain matrix respectively; calculating a predicted value of relative measurement information and updating a state variable and an error covariance matrix; and completing positioning. The application realizes normal operation of the system in the case of invalidation of the master UAV, and effectively controls the increase of positioning error caused by invalidation of the master UAV.

Description

Technical Field

[0001] This invention relates to the field of UAV positioning technology, and more specifically, to a method and system for optimized distributed EKF cooperative positioning of UAVs based on confidence factors. Background Technology

[0002] Master-slave drone swarms have great application potential and broad prospects in traversing complex environments, and are expected to bring more innovation and changes to fields such as forest monitoring, disaster relief, and resource exploration in the future. During flight, if drones enter complex environments such as forests, building complexes, or caves, they are easily susceptible to signal blockage, leading to communication failures. In a master-slave drone swarm cooperative positioning system, the master drone plays a crucial role, needing to communicate with each slave drone to provide high-precision positioning information. However, because drones are highly susceptible to interference, if the master drone fails, the slave drones will fail in positioning, rendering the entire system unusable. When the master drone fails, all slave drones are in an equal position. When using an upgraded temporary master drone to locate the other slave drones, if the temporary master drone has a large positioning error, the positioning error will accumulate and propagate to the other slave drones, causing even larger positioning errors for the other slave drones being assisted in positioning. Therefore, there is an urgent need for a collaborative positioning method that, when the master drone fails, selects a slave drone with higher positioning accuracy to upgrade into a temporary master drone. This slave drone temporarily takes the position of the master drone until the master drone recovers, receives information from other slave drones, and maintains its own high positioning accuracy. Summary of the Invention

[0003] The technical problem to be solved by this invention is:

[0004] If the master drone in an existing master-slave drone swarm fails, and the temporary master drone has a large positioning error, the positioning error will accumulate and propagate to other slave drones, causing the other slave drones that are being assisted in positioning to have even larger positioning errors.

[0005] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0006] This invention provides a method for selecting distributed EKF cooperative localization from unmanned aerial vehicles based on confidence factors, comprising the following steps:

[0007] S1. Determine if the main drone is faulty. If it is faulty, proceed to S2; otherwise, proceed to S4.

[0008] S2. Calculate the confidence level η of each drone. i ;

[0009] S3. Sort all the confidence values ​​from the drones and select the confidence level η. i The highest-level drone was upgraded to a temporary master drone;

[0010] S4. Construct the state equations and measurement equations for UAV i;

[0011] S5. Calculate the absolute measurement gain matrix respectively. and relative measurement gain matrix

[0012] S6. Calculate the predicted value of relative measurement information. And update the state variables And error covariance matrix

[0013] S7. Determine if the positioning is complete. If it is complete, end the program. If it is not complete, return to S1.

[0014] Furthermore, in S2, the confidence level η of each drone is calculated. i for:

[0015]

[0016] Among them, (x i ,y i ,z i (x) represents the confidence factor to be calculated for the coordinates of UAV i. j ,y j ,z j ) represents the coordinates of other drones j, d ij This represents the distance measurement between drone i and drone j.

[0017] Furthermore, in S4, the position, velocity, and acceleration of the UAV are selected as state variables, i.e. in, These represent the position information of drone i along the x-axis, y-axis, and z-axis, respectively. These represent the velocity information of UAV i along the x-axis, y-axis, and z-axis, respectively. These represent the acceleration information of i along the x-axis, y-axis, and z-axis, respectively.

[0018] The state equation for drone i is constructed as follows:

[0019]

[0020] Among them, F i The state transition matrix is ​​calculated using the following formula: Here, f is the equation of motion for the UAV;

[0021] The measurement equation is constructed as follows:

[0022]

[0023] in, The absolute measurement gain matrix to be determined. Let N be the relative measurement matrix to be determined. i For the set of neighboring drones of drone i; The predicted value is the absolute measurement value, representing the predicted value of the measurement information of the master UAV and slave UAV i. The observed values ​​are absolute measurements, representing the measured information of the master UAV and slave UAV i. The predicted value is the relative measurement value, representing the predicted value of the measurement information from UAV i and UAV j. The observations are relative measurements, representing the measurement information from UAV i and UAV j.

[0024] Furthermore, the predicted value of the absolute measurement in S4 The calculation formula is:

[0025]

[0026] Observations of absolute measurement The calculation formula is:

[0027]

[0028] The formula for calculating the predicted value of a relative measurement is:

[0029]

[0030] The formula for calculating the observed value of a relative measurement is:

[0031]

[0032] in, For absolute measurement matrix, D i For relative measurement matrices, and All noise is Gaussian white noise; calculate the Jacobian matrix H of the absolute measurement equation and the relative measurement equation. i and D i for:

[0033]

[0034]

[0035] Furthermore, the absolute measurement gain matrix is ​​calculated in S5. for:

[0036]

[0037] in, for The variance, i.e. Calculate the relative measurement gain matrix for:

[0038]

[0039] in, for The variance, i.e. Furthermore, the predicted values ​​of the relative measurement information are calculated. for:

[0040]

[0041] Update state variables And error covariance matrix for:

[0042]

[0043]

[0044] The system is a distributed EKF cooperative localization system for selecting drones based on confidence factors. This system has a program module corresponding to the steps of any of the above technical solutions, and executes the steps in the above-mentioned distributed EKF cooperative localization method for selecting drones based on confidence factors when it runs.

[0045] A computer-readable storage medium storing a computer program configured to, when invoked by a processor, implement the steps of the distributed EKF cooperative localization method based on confidence factor from unmanned aerial vehicles as described in any of the above technical solutions.

[0046] Compared with the prior art, the beneficial effects of the present invention are:

[0047] This invention presents a distributed EKF cooperative localization method and system based on confidence factors for slave drones. When the master drone fails, confidence factors are calculated for all located slave drones based on the relative positioning estimation error. The slave drone with the highest confidence is selected and upgraded to a temporary master drone to assist other slave drones in localization. This ensures normal system operation even when the master drone fails, effectively controlling the increase in positioning error caused by master drone failure. Furthermore, the confidence factor calculation in this invention is relatively small. It effectively selects drones with higher positioning accuracy as master drones, ensuring the stability and efficiency of the entire localization system.

[0048] This invention is based on distributed EKF, which not only utilizes the absolute measurement information provided by the master UAV, but also integrates the relative measurement information of the slave UAV and its neighboring slave UAVs, thereby reducing the impact of master UAV failure to a certain extent. Attached Figure Description

[0049] Figure 1 This is a flowchart of the distributed EKF cooperative localization method based on confidence factor from UAVs in an embodiment of the present invention;

[0050] Figure 2 This is a comparison chart of positioning errors using different methods in the case of main UAV failure in an embodiment of the present invention. Detailed Implementation

[0051] In the description of this invention, it should be noted that the terms "first," "second," and "third" mentioned in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first," "second," and "third" may explicitly or implicitly include one or more of that feature.

[0052] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0053] like Figure 1 As shown, this invention provides a distributed EKF cooperative localization method for UAVs based on confidence factors, comprising the following steps:

[0054] S1. Determine if the main drone is faulty. If it is faulty, proceed to S2; otherwise, proceed to S4.

[0055] S2. Calculate the confidence level η of each drone. i ;

[0056] S3. Sort all the confidence values ​​from the drones and select the confidence level η. i The highest-level drone was upgraded to a temporary master drone;

[0057] S4. Construct the state equations and measurement equations for UAV i;

[0058] S5. Calculate the absolute measurement gain matrix respectively. and relative measurement gain matrix

[0059] S6. Calculate the predicted value of relative measurement information. And update the state variables And error covariance matrix

[0060] S7. Determine if the positioning is complete. If it is complete, end the program. If it is not complete, return to S1.

[0061] In the initial phase, the drone swarm system operates normally, with the master drone participating in the localization of other slave drones using the distributed EKF cooperative localization algorithm. When the master drone fails, the confidence factor of the currently located slave drones is calculated, and the slave drone with the highest confidence factor is selected as the temporary master drone to assist the other slave drones in localization. Since the localization of the slave drones themselves contains errors, error propagation will occur. Therefore, the drone with the smallest positional error among the slave drones is selected using the confidence factor to minimize error propagation. When the master drone recovers, the system resumes normal operation.

[0062] In S2, the confidence level η of each drone is calculated. i for:

[0063]

[0064] Among them, (x i ,y i ,z i (x) represents the confidence factor to be calculated for the coordinates of UAV i. j ,y j ,z j ) represents the coordinates of other drones j, d ij This represents the distance measurement between drone i and drone j.

[0065] As can be seen from the above formula, the confidence factor of the UAV is defined as 1 minus the relative positioning estimation error of the UAV. The larger the relative positioning estimation error of the UAV, the higher its confidence factor η. i The smaller the value, the lower the drone's positioning accuracy. Conversely, the higher the drone's positioning accuracy, the greater the relative positioning estimation error, and thus the higher its confidence factor η. i The larger.

[0066] This invention uses relative information between the master and slave UAVs when calculating the confidence factor, and performs normalization processing. The confidence factor defined by this method is easy to calculate and has low complexity, which is beneficial for the UAV system to react quickly and select a new master UAV.

[0067] This invention is based on a hierarchical EKF cooperative localization algorithm for UAV swarm cooperative localization. The system has one master UAV and n slave UAVs. The master UAV can communicate with all slave UAVs, while the slave UAVs only communicate with the p nearest slave UAVs.

[0068] In S4, the position, velocity, and acceleration of the UAV are selected as state variables, i.e. in, These represent the position information of drone i along the x-axis, y-axis, and z-axis, respectively. These represent the velocity information of UAV i along the x-axis, y-axis, and z-axis, respectively. These represent the acceleration information of i along the x-axis, y-axis, and z-axis, respectively.

[0069] The state equation for drone i is constructed as follows:

[0070]

[0071] Among them, F i The state transition matrix is ​​calculated using the following formula: Here, f is the equation of motion for the UAV;

[0072] The measurement equation is constructed as follows:

[0073]

[0074] in, The absolute measurement gain matrix to be determined. Let N be the relative measurement matrix to be determined. i For the set of neighboring drones of drone i; The predicted value is the absolute measurement value, representing the predicted value of the measurement information of the master UAV and slave UAV i. The observed values ​​are absolute measurements, representing the measured information of the master UAV and slave UAV i. The predicted value is the relative measurement value, representing the predicted value of the measurement information from UAV i and UAV j. The observations are relative measurements, representing the measurement information from UAV i and UAV j.

[0075] Predicted values ​​of absolute measurements in S4 The calculation formula is:

[0076]

[0077] Observations of absolute measurement The calculation formula is:

[0078]

[0079] The formula for calculating the predicted value of a relative measurement is:

[0080]

[0081] The formula for calculating the observed value of a relative measurement is:

[0082]

[0083] in, For absolute measurement matrix, D i For relative measurement matrices, and All are Gaussian white noise;

[0084] Initialize state variables Covariance Matrix Calculate the predicted values ​​of state variables Error covariance matrix

[0085] Calculate the Jacobian matrix H for the absolute measurement equation and the relative measurement equation. i and D i for:

[0086]

[0087]

[0088] Calculate the absolute measurement gain matrix in S5 for:

[0089]

[0090] in, for The variance, i.e.

[0091] Calculate the relative measurement gain matrix for:

[0092]

[0093] in, for The variance, i.e.

[0094] S6 calculates the predicted value of relative measurement information. for:

[0095]

[0096] Update state variables And error covariance matrix for:

[0097]

[0098]

[0099] Update state variables This method combines absolute measurement information between master and slave drones with relative measurement information between slave drones, making fuller use of the positioning information in the cooperative positioning system and achieving more accurate positioning results.

[0100] Example 1

[0101] When measurement between the master and slave UAVs fails, the proposed distributed EKF filter-based UAV swarm cooperative localization method (DEKF) is simulated and analyzed, and compared with other traditional master-slave localization methods such as Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Cubature Kalman Filter (CKF).

[0102] Assuming the system operates normally for the first 30 seconds, at 30 seconds, the main UAV suddenly malfunctions and loses its measurement capability, causing the UAV's absolute measurement equations to fail and become:

[0103]

[0104] In this simulation environment, the distributed EKF cooperative localization method based on confidence factors, the CKF cooperative localization algorithm, the UKF cooperative localization algorithm, and the EKF cooperative localization algorithm of this invention are simulated, analyzed, and compared, and the results are as follows: Figure 2 The positioning error distribution diagram is shown below. Figure 2 As can be seen, when the master UAV fails, the positioning accuracy of the traditional master-slave algorithm gradually diverges after the master UAV fails, and the divergence is rapid. However, the distributed EKF cooperative positioning method based on confidence factor of the present invention has a lower positioning accuracy than under normal circumstances, but still controls the positioning error within a small range. It can be seen that the present invention has high stability and positioning accuracy in dealing with the sudden failure of the master UAV.

[0105] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A distributed EKF cooperative localization method for UAVs based on confidence factors, characterized in that, Includes the following steps: S1. Determine if the main drone is faulty. If it is faulty, proceed to S2; otherwise, proceed to S4. S2. Calculate the confidence level of each drone. ; S3. Sort all confidence values ​​from the drones and select the confidence level. The highest-level drone was upgraded to a temporary master drone; S4, Building from drones The state equations and measurement equations; S5. Calculate the absolute measurement gain matrix respectively. and relative measurement gain matrix ; S6. Calculate the predicted value of relative measurement information. and update the state variables. And error covariance matrix ; S7. Determine whether the positioning is complete. If it is complete, end the program. If it is not complete, return to S1. Calculate the absolute measurement gain matrix in S5 for: in, for The variance, i.e. , This is an absolute measurement matrix; Calculate the relative measurement gain matrix for: in, for The variance, i.e. , For relative measurement matrix, and All are Gaussian white noise. For drones A collection of neighboring drones.

2. The method for optimal distributed EKF cooperative localization from unmanned aerial vehicles based on confidence factors according to claim 1, characterized in that, S2 calculates the confidence level of each drone. for: in, The confidence factor to be calculated from the drone coordinates For the coordinates of other drones j, For drones and from drones The distance measurement between them.

3. The method for selecting distributed EKF cooperative localization from unmanned aerial vehicles based on confidence factors according to claim 1, characterized in that, In S4, the position, velocity, and acceleration of the UAV are selected as state variables, i.e. ,in, These represent the position information of drone i along the x-axis, y-axis, and z-axis, respectively. These represent the velocity information of UAV i along the x-axis, y-axis, and z-axis, respectively. These represent the acceleration information of i along the x-axis, y-axis, and z-axis, respectively. Building from drones The state equation is: in, The state transition matrix is ​​calculated using the following formula: ,here Here is the equation of motion for the UAV; The measurement equation is constructed as follows: in, The absolute measurement gain matrix to be determined. The relative measurement matrix to be determined; The predicted value is an absolute measurement, representing the difference between the master drone and the slave drone. The predicted value of the measurement information, The observed values ​​are absolute measurements, representing the difference between the master UAV and the slave UAV. The observed values ​​of the measurement information, The predicted value is a relative measurement, representing the value from the drone. With drones The predicted value of the measurement information, The observed values ​​are relative measurements, representing the values ​​from the UAV. With drones The observed values ​​of the measurement information.

4. The distributed EKF cooperative localization method for UAVs based on confidence factors according to claim 3, characterized in that, Predicted values ​​of absolute measurements in S4 The calculation formula is: Observations of absolute measurement The calculation formula is: The formula for calculating the predicted value of a relative measurement is: The formula for calculating the observed value of a relative measurement is: Calculate the Jacobian matrix for the absolute and relative measurement equations. and for: 。 5. The distributed EKF cooperative localization method for UAVs based on confidence factors according to claim 4, characterized in that, Calculate the predicted value of relative measurement information for: Update state variables And error covariance matrix for: 。 6. A distributed EKF cooperative positioning system for UAVs based on confidence factors, characterized in that, The system has a program module corresponding to the steps of any one of claims 1 to 5 above, and executes the steps in the above-described method for selecting distributed EKF cooperative localization from UAVs based on confidence factors when it is run.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to, when invoked by a processor, implement the steps of the distributed EKF cooperative localization method based on confidence factor from unmanned aerial vehicles as described in any one of claims 1 to 5.

Images