EKF cooperative positioning method and system based on hierarchical unmanned aerial vehicle group preferred by master unmanned aerial vehicle

Abstract

The application is based on the layered EKF cooperative positioning method and system of the preferred master unmanned aerial vehicle, relates to the unmanned aerial vehicle positioning technical field, and aims at solving the problems of information redundancy, low positioning efficiency and synchronization influence caused by too many master unmanned aerial vehicles in the master unmanned aerial vehicle layer in the prior art. The method comprises the following steps: S1, constructing the motion equation, observation equation and prior covariance expression of the target slave unmanned aerial vehicle i; S2, calculating the selection factor of each master unmanned aerial vehicle to determine the preferred master unmanned aerial vehicle; S3, calculating the measurement value of the slave unmanned aerial vehicle i and the preferred master unmanned aerial vehicle j, and calculating the measurement value u j and the covariance matrix U j after consistency processing; S4, calculating the consistency-processed Kalman gain M i ; S5, calculating the measurement gain matrix K t of the target slave unmanned aerial vehicle i i ; S6, updating the state estimation value and the covariance matrix. The method can improve the positioning accuracy of the unmanned aerial vehicle group and ensure the stability of the unmanned aerial vehicle system.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of unmanned aerial vehicle positioning, in particular to a layered unmanned aerial vehicle group EKF cooperative positioning method and system based on optimal master unmanned aerial vehicles. BACKGROUND

[0002] At present, the development of unmanned aerial vehicle technology is developing rapidly, and the application fields based on unmanned aerial vehicles are becoming more and more extensive. The master-slave type unmanned aerial vehicle group as a new intelligent manufacturing technology has attracted worldwide attention and research. In the process of layered unmanned aerial vehicle group cooperative positioning, when the number of master unmanned aerial vehicles in the master unmanned aerial vehicle layer is too large, information redundancy occurs, that is, when the number of master unmanned aerial vehicles participating in the target slave unmanned aerial vehicle is too large, the positioning accuracy of the system is not obviously improved, and the calculation time of the algorithm is increased, the system complexity is increased, the unmanned aerial vehicle group positioning efficiency is decreased, and the energy consumption is increased. If the redundant information occupies too much memory and the algorithm calculation time is too long, the synchronization is affected, and even the positioning accuracy of the unmanned aerial vehicle group positioning system is decreased and the positioning information is unstable. Aiming at the problem. SUMMARY

[0003] The technical problem solved by the application is:

[0004] In the process of layered unmanned aerial vehicle group cooperative positioning, when the number of master unmanned aerial vehicles in the master unmanned aerial vehicle layer is too large, the problem of information redundancy occurs, which leads to low positioning efficiency, affects synchronization, and even leads to low positioning accuracy and unstable information.

[0005] The technical scheme adopted by the application to solve the above technical problem is:

[0006] The application provides a layered unmanned aerial vehicle group EKF cooperative positioning method based on optimal master unmanned aerial vehicles, comprising the following steps:

[0007] S1, constructing a motion equation, an observation equation and a prior covariance expression of a target slave unmanned aerial vehicle i;

[0008] S2, calculating the distance measurement value between the target slave unmanned aerial vehicle i and each master unmanned aerial vehicle, and calculating the ranging standard deviation corresponding to each measurement value, multiplying the measurement value and the ranging standard deviation to obtain a selection factor of each master unmanned aerial vehicle, and selecting l optimal master unmanned aerial vehicles according to the selection factor from small to large;

[0009] S3, calculating the measurement value of the slave unmanned aerial vehicle i and the optimal master unmanned aerial vehicle j, and calculating the measurement value u j and the covariance matrix U j after consistency processing.

[0010] S4, calculate the Kalman gain M of the consistency processing i ;

[0011] S5, calculate the measurement gain matrix of the target slave unmanned aerial vehicle i

[0012] S6, update the state estimation value and the covariance matrix to obtain the state estimation value and the covariance matrix of the slave unmanned aerial vehicle i at the time t.

[0013] Further, in S2, at each sampling time, the distance is sampled multiple times, the distance estimation measurement value between the target slave unmanned aerial vehicle i and each master unmanned aerial vehicle is calculated, and the average value is taken as the measurement value at the sampling time t to obtain a sequence The ranging standard deviation corresponding to each measurement value is calculated to obtain a sequence The measurement value and the ranging standard deviation are multiplied to obtain the selection factor of the master unmanned aerial vehicle j relative to the slave unmanned aerial vehicle i:

[0014]

[0015] Further, in S1, the motion equation and the observation equation of the slave unmanned aerial vehicle i are constructed as:

[0016]

[0017] The prior covariance expression is constructed as:

[0018]

[0019] Wherein, F is the state transition matrix of the slave unmanned aerial vehicle i, is the true value of the state variable of the slave unmanned aerial vehicle i at the time t-1, is the prior state estimation value of the slave unmanned aerial vehicle i at the time t, is the posterior state estimation value of the slave unmanned aerial vehicle i at the time t-1, is the measurement value between the slave unmanned aerial vehicle i and the master unmanned aerial vehicle j at the time t, H ij is the measurement matrix between the slave unmanned aerial vehicle i and the master unmanned aerial vehicle j, is the prior covariance matrix of the slave unmanned aerial vehicle i, is the covariance matrix of the slave unmanned aerial vehicle i at the time t-1, and is the Gaussian white noise.

[0020] Further, in S3, the measurement value of the slave unmanned aerial vehicle i and the preferred master unmanned aerial vehicle j is calculated j

[0021] ​

[0022] wherein, represents the fusion measurement value;

[0023] The covariance matrix U is calculated j is:

[0024]

[0025] wherein, is the fusion covariance matrix.

[0026] Further, the Kalman gain M of the consistency processing in S4 is calculated i is:

[0027]

[0028] wherein, is the fusion covariance matrix.

[0029] Further, the system gain matrix of the target from the unmanned aerial vehicle i in S5 is calculated is:

[0030]

[0031] wherein, γ j is the measurement information weighting factor of the master unmanned aerial vehicle j, is the variance of the measurement noise γ j is:

[0032]

[0033] wherein, μ j is the weighting factor of the master unmanned aerial vehicle j, and is:

[0034]

[0035] Further, the state estimation value is updated in S6, and the formula is:

[0036]

[0037] wherein, is the optimal state estimation value of the unmanned aerial vehicle i relative to the unmanned aerial vehicle j at time t-1, is the optimal state estimation value of the unmanned aerial vehicle i at time t, is the measurement prediction value between the unmanned aerial vehicle i and the master unmanned aerial vehicle j, and

[0038] The covariance matrix is updated, and the formula is:

[0039]

[0040] The layered UAV group EKF cooperative positioning system based on the preferred master UAV has program modules corresponding to the steps of any of the above technical solutions, and executes the steps in the layered UAV group EKF cooperative positioning method based on the preferred master UAV when running.

[0041] A computer readable storage medium stores a computer program, and the computer program is configured to realize the steps of the layered UAV group EKF cooperative positioning method based on the preferred master UAV when called by a processor.

[0042] Compared with the prior art, the beneficial effects of the present application are:

[0043] The layered UAV group EKF cooperative positioning method and system based on the preferred master UAV can accurately filter out the master UAV with high-quality information of the slave UAV as a reference, fuse the high-quality information according to the weight, improve the positioning accuracy of the UAV group, and exclude some redundant information through the selection of the selection factor, thereby reducing the adverse effects on the positioning of the UAV group system.

[0044] The layered filtering algorithm based on the consistency principle in the method makes the multiple sensors in the UAV group gradually tend to a globally consistent state in the process of information interaction and filtering iteration, and can effectively ensure the stability of the UAV system.

[0045] The present application can effectively improve the structure of the layered UAV group, increase the number of master UAVs, and more fully utilize the cooperative navigation information to share the workload of the master UAV. BRIEF DESCRIPTION OF DRAWINGS

[0046] Figure 1 The flowchart of the layered UAV group EKF cooperative positioning method based on the preferred master UAV in the embodiments of the present application is shown.

[0047] Figure 2 The flowchart of the preferred method of the master UAV in the embodiments of the present application is shown.

[0048] Figure 3 The comparison chart of the positioning error of the present application method and the layered consistent EKF cooperative positioning method without the selection factor in the embodiments of the present application is shown. DETAILED DESCRIPTION

[0049] In the description of the present application, it should be noted that the terms "first", "second", "third" mentioned in the embodiments of the present application are only for the purpose of description, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined with "first", "second", "third" can be explicitly or implicitly included one or more of the features.

[0050] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0051] As shown in Figure 1 The present application provides a hierarchical unmanned aerial vehicle group EKF cooperative positioning method based on preferred master unmanned aerial vehicles, which comprises the following steps:

[0052] S1, constructing the motion equation, observation equation and prior covariance expression of the target slave unmanned aerial vehicle i;

[0053] S2, calculating the distance measurement value between the target slave unmanned aerial vehicle i and each master unmanned aerial vehicle, and calculating the ranging standard deviation corresponding to each measurement value, multiplying the measurement value and the ranging standard deviation to obtain the selection factor of each master unmanned aerial vehicle, and selecting the l preferred master unmanned aerial vehicles according to the selection factor from small to large;

[0054] S3, calculating the measurement value of the slave unmanned aerial vehicle i and the preferred master unmanned aerial vehicle j, and calculating the measurement value and the covariance matrix after consistency processing;

[0055] S4, calculating the consistency processing Kalman gain;

[0056] S5, calculating the measurement gain matrix of the target slave unmanned aerial vehicle i

[0057] S6, updating the state estimation value and the covariance matrix to obtain the state estimation value and the covariance matrix of the slave unmanned aerial vehicle i at time t.

[0058] The present embodiment assumes that the unmanned aerial vehicle group positioning system contains n master unmanned aerial vehicles and m slave unmanned aerial vehicles, and the positioning of a certain slave unmanned aerial vehicle i is carried out. The slave unmanned aerial vehicle can communicate with each master unmanned aerial vehicle, that is, the slave unmanned aerial vehicle i can obtain the measurement information (including distance value d, pitch angle β and heading angle a) of the n master unmanned aerial vehicles. Since the distance and ranging variance of each master unmanned aerial vehicle to the target slave unmanned aerial vehicle are different, and the smaller the ranging variance is, the more reliable the observation value of the master unmanned aerial vehicle is, therefore the distance and ranging variance are the basis for measuring the reliability of the observation value of the master unmanned aerial vehicle.

[0059] The present invention combines a hierarchical consistent extended Kalman filter and a master UAV selection strategy. The master UAV selection strategy is used to calculate the selection factors between each UAV and the slave UAVs, and the optimal l master UAVs are selected. The hierarchical consistent extended Kalman filter is used to locate the slave UAV i, and the measurement information of each master UAV is weighted and fused therein to achieve efficient and accurate cooperative positioning of the UAVs.

[0060] As Figure 2 shown, in S2, at each sampling moment, the distance is sampled multiple times, the estimated measurement values of the distances between the target slave UAV i and each master UAV are calculated, and their average value is taken as the measurement value at the sampling moment t, obtaining the sequence and calculating the ranging standard deviation corresponding to each measurement value, obtaining the sequence The product of the measurement value and the ranging standard deviation is used to obtain the selection factor of the master UAV j relative to the slave UAV i as:

[0061]

[0062] All the selection factors of the obtained master UAVs are sorted in ascending order, obtaining the sequence and the master UAVs corresponding to the first l (l < n) selection factors are selected as L = {A1, A2,..., A j ,..., A l} as the optimized selected master UAVs, and the optimized selected master UAVs are used to perform cooperative positioning on the target slave UAV i.

[0063] In S1, the motion equation and observation equation of the slave UAV i are constructed as:

[0064]

[0065] The prior covariance expression is constructed as:

[0066]

[0067] where F is the state transition matrix of the slave UAV i, is the true value of the state variable of the slave UAV i at the moment t - 1, is the prior state estimate value of the slave UAV i at the moment t, is the posterior state estimate value of the slave UAV i at the moment t - 1, is the measurement value between the slave UAV i and the master UAV j at the moment, H ij is the measurement matrix between the slave UAV i and the master UAV j, is the prior covariance matrix of the slave UAV i, is the covariance matrix of the slave UAV i at the moment t - 1, and is a Gaussian white noise.

[0068] In S3, the measurement value from the unmanned aerial vehicle i and the preferred master unmanned aerial vehicle j is calculated The measurement value after consistency processing is calculated u j is:

[0069]

[0070] wherein, indicates the fusion measurement value;

[0071] The covariance matrix U is calculated j is:

[0072]

[0073] wherein, is the fusion covariance matrix.

[0074] In S4, the consistency processing Kalman gain M is calculated i is:

[0075]

[0076] wherein, is the fusion covariance matrix.

[0077] In S5, the system gain matrix of the target from the unmanned aerial vehicle i is calculated is:

[0078]

[0079] wherein, γ j is the measurement information weighting factor of the master unmanned aerial vehicle j, is the variance of the measurement noise γ j is:

[0080]

[0081] wherein, μ j is the weighting factor of the master unmanned aerial vehicle j, and is:

[0082]

[0083] Since the smaller the selection factor, the lower the accuracy of the ranging, therefore, in the process of weighting, the smaller the selection factor of the master unmanned aerial vehicle, the greater the proportion of the measurement information of the master unmanned aerial vehicle in the fusion process. In order to fuse the measurement information of the master unmanned aerial vehicle after selection, the weighting factor of the master unmanned aerial vehicle j is constructed as: The measurement information weighting factor of the master unmanned aerial vehicle j is:

[0084] The state estimation value is updated in S6, and the formula is:

[0085]

[0086] wherein, is the optimal state estimation value of the unmanned aerial vehicle i at t relative to t-1, is the optimal state estimation value of the unmanned aerial vehicle i at t, is the measurement prediction value between the unmanned aerial vehicle i and the master unmanned aerial vehicle j, and

[0087] The covariance matrix is updated, and the formula is:

[0088]

[0089] Embodiment 1

[0090] The hierarchical consistent EKF cooperative positioning method without the selection factor and the EKF cooperative positioning method of the application using the selection factor to select the master unmanned aerial vehicle are used to position the target unmanned aerial vehicle i, wherein the distributed consistent EKF cooperative positioning algorithm is respectively used to simulate and analyze 5 master unmanned aerial vehicles and 10 master unmanned aerial vehicles, and the EKF cooperative positioning algorithm of the application selects 5 master unmanned aerial vehicles from 10 master unmanned aerial vehicles to participate in the cooperative positioning of the unmanned aerial vehicle, and the error result is as shown in Figure 3 The positioning error of the distributed consistent EKF cooperative positioning method is obviously reduced compared with the distributed consistent EKF cooperative positioning method in the case of using the information of 5 master unmanned aerial vehicles to position the unmanned aerial vehicle, and the positioning accuracy of the distributed consistent EKF cooperative positioning algorithm is indeed improved under 10 master unmanned aerial vehicles, but compared with the EKF cooperative positioning method of the application using the selection factor to select the master unmanned aerial vehicle, the positioning accuracy has no obvious advantage. This is because the method of the application selects the high-quality master unmanned aerial vehicle for the target unmanned aerial vehicle i as a reference through the selection factor, and fuses the information according to the weight, so that the cooperative positioning with higher accuracy can be realized. As shown in Table 1, the positioning error comparison results are shown.

[0091] Table 1

[0092]

[0093] According to the results in Table 1, it can be seen that the EKF cooperative positioning method of the application using the selection factor for the master unmanned plane is improved by 11.985% in positioning accuracy compared with the hierarchical consistent EKF cooperative positioning method of information fusion of 5 unmanned planes, and the positioning accuracy is almost the same as that of the hierarchical consistent EKF cooperative positioning method of information fusion of 10 master unmanned planes. However, if the number of master unmanned planes in the master unmanned plane layer is too large, the system complexity increases, which increases the calculation time of the algorithm, increases the energy consumption, and even affects the synchronization, resulting in the decrease of the positioning accuracy of the unmanned plane group positioning system and the instability of the positioning information. The method of the application can solve the above problems while realizing the higher precision positioning of the slave unmanned plane. It can be seen that the method of the application has significant advantages.

[0094] Although the application is disclosed as above, the protection scope of the application is not limited to this. The person skilled in the art can make various changes and modifications without departing from the spirit and scope of the application, and these changes and modifications will fall within the protection scope of the application.

Claims

1. A hierarchical UAV group EKF cooperative localization method based on master UAV preference, characterized in that, The method comprises the following steps: S1, constructing a target from a motion equation of an unmanned aerial vehicle equation of motion, an observation equation, and a prior covariance expression S2, calculating the distance measurement value between the target and each master UAV from the UAV i, and calculating the ranging standard deviation corresponding to each measurement value, multiplying the measurement value and the ranging standard deviation to obtain the selection factor of each master UAV, and selecting the preferred master UAV from small to large according to the selection factor one preferred master UAV; S3, the target calculates the measurement value of the unmanned aerial vehicle i and the preferred master unmanned aerial vehicle j, and calculates the measurement value after consistency processing and the covariance matrix ; S4, computing Kalman gain for consistency processing ; S5, calculating target from unmanned aerial vehicle of measurement gain matrix ; S6, update the state estimation value and the covariance matrix to obtain a target unmanned aerial vehicle At the state estimation value and the covariance matrix at the time moment The system gain matrix in S5 is calculated from the UAV as: ​ in, Main drone The weighting factor of the measurement information, For measuring noise variance For the target from drone Prior covariance matrix, For the target from drone exist The covariance matrix at time t, For the target from drone With the main drone Measurement matrix between is: wherein, is a selection factor of the master drone j relative to the target drone i, is a weighting factor of the master drone . 。 2. The hierarchical UAV group EKF cooperative positioning method based on master UAV preference according to claim 1, characterized in that, In S2, at each sampling time, the distance is sampled multiple times, the distance estimation measurement value between the target slave unmanned aerial vehicle and each master unmanned aerial vehicle is calculated, the average value is taken as the measurement value at the sampling time t, and a sequence is obtained The ranging standard deviation corresponding to each measurement value is calculated, and a sequence is obtained The measurement value and the ranging standard deviation are multiplied to obtain the selection factor of the master unmanned aerial vehicle j relative to the target slave unmanned aerial vehicle i. 。 3.The hierarchical UAV group EKF cooperative positioning method based on master UAV preference according to claim 2, wherein, The construction target in S1 is from the unmanned aerial vehicle The motion equation and observation equation of the unmanned aerial vehicle are as follows: The prior covariance expression is constructed as: in, For the target from drone The state transition matrix, For the target from drone exist The true value of the state variable at time t. for Target from drone The prior state estimate, for Target from drone The posterior state estimate, for Target from drone Measurements between the main UAV j and the host UAV j , and It is Gaussian white noise.

4. The hierarchical UAV group EKF cooperative positioning method based on master UAV preference according to claim 3, characterized in that, In S3, the measurement values ​​of the target UAV i and the preferred master UAV j are calculated. Calculate the measurement values ​​after consistency processing. for: wherein represents the fusion measurement value; Computing the covariance matrix is: wherein, is the fused covariance matrix.

5. The hierarchical UAV group EKF cooperative positioning method based on master UAV preference according to claim 4, characterized in that, Kalman gain for consistency processing in S4 is: wherein, is the fused covariance matrix.

6. The hierarchical UAV group EKF cooperative positioning method based on master UAV preference according to claim 5, characterized in that, The state estimation value is updated in S6, and the formula is: in, for Time relative to Target from drone The optimal state estimate, for Target from drone The optimal state estimate, For the target from drone With the main drone The predicted values ​​of the measurements between, and ; The covariance matrix is updated, and the formula is: 。 7. The hierarchical UAV group EKF cooperative positioning system based on the master UAV's preference, characterized in that, The system has program modules corresponding to the steps of any one of claims 1-6, and when running, the steps of the above-mentioned layered UAV group EKF cooperative positioning method based on the preferred master UAV are executed.

8. A computer-readable storage medium, characterized in that: The computer readable storage medium stores a computer program configured to implement the steps of the layered UAV group EKF cooperative positioning method based on the preferred master UAV in any one of claims 1-6 when called by the processor.

Images