Distributed UAV swarm EKF collaborative positioning method and system based on scaling factor
By adopting distributed EKF collaborative positioning method based on scaling factor in the drone group, the state and measurement equations are constructed and the system error covariance matrix is updated, and the positioning accuracy problem of Kalman filtering algorithm in noise environment is solved, which significantly improves the The positioning accuracy and noise resistance of the drone cluster.
Patent Information
- Application Number
- CN202310794112.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-06-30
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2043-06-30
AI Technical Summary
The existing Kalman filtering algorithms are difficult to directly construct the linear relationship between process noise and the error of Kalman filtering estimation, resulting in low accuracy of coordinated positioning of drones and even failure of positioning system.
The distributed UAV cluster EKF collaborative positioning method based on scaling factor is adopted, and the Jacobian matrix and system gain matrix are calculated by constructing state equations, absolute measurement equations and relative measurement equations, and the system error covariance matrix is updated using scaling factor. Achieve collaborative positioning of the drone swarm.
By adjusting the observation noise of the distributed extended Kalman filter in real time, the positioning accuracy of the drone cluster is improved and the impact of sudden observation noise on positioning accuracy is reduced.
Smart Images

Figure CN116907499B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of unmanned aerial vehicle positioning technology, and in particular to a distributed unmanned aerial vehicle group EKF collaborative positioning method and system based on scaling factors. Background Art
[0002] At present, the development of drone technology is advancing by leaps and bounds, and the application fields based on drones are becoming more and more extensive. It has been widely used in fire detection, environment and traffic monitoring. In order to successfully complete the mission in order to cope with the complex and changeable surrounding environment during the mission, the collaborative positioning of drone swarms is crucial. The Kalman filter algorithm is one of the most commonly used algorithms for collaborative positioning of drone swarms, but it has strict requirements on the prior knowledge of the process and measurement noise of the positioning system. However, it is difficult to directly construct the linear relationship between the process noise and measurement noise and the error estimated by the Kalman filter. The uncertainty of the covariance parameters of the process noise and measurement error will significantly reduce the performance of the Kalman filter, resulting in a decrease in the positioning accuracy of the drone and the failure of the drone swarm positioning system.
[0003] At present, the navigation and positioning systems of UAVs are being studied. By adjusting the observation noise in real time, the accuracy of sensor tracking and positioning can be improved. Therefore, there is an urgent need for a method to reduce the decrease in positioning accuracy caused by the change in observation noise to improve the positioning effect of UAV swarms. Summary of the invention
[0004] The technical problems to be solved by the present invention are:
[0005] The existing Kalman filter algorithm is difficult to directly construct a linear relationship between process noise and the error estimated by the Kalman filter for the collaborative positioning of drone swarms, which reduces the performance of the filter, resulting in low positioning accuracy and even failure of the positioning system.
[0006] The present invention adopts the following technical solutions to solve the above technical problems:
[0007] The present invention provides a distributed UAV swarm EKF collaborative positioning method based on a scaling factor, comprising the following steps:
[0008] S1. Select the state variables of the UAV, construct the state equation, and initialize the state variables and covariance matrix;
[0009] S2. Construct the absolute measurement equation between the master drone and the slave drone i and the relative measurement value equation between the slave drone i and the slave drone j, and calculate the Jacobian matrix H of the absolute measurement equation and the relative measurement equation i and D i ;
[0010] S3, constructing a scaling factor α;
[0011] S4. Update the system error covariance matrix using the scaling factor
[0012] S5. Calculate the system gain matrix of the absolute measurement value and the relative measured system gain matrix
[0013] S6. Calculate the relative measurement value, update the state estimation value and the system error covariance matrix, and realize the collaborative positioning of the drone group.
[0014] Furthermore, S1 is specifically: the position, velocity, and acceleration of the drone are selected as state variables, that is, in, Respectively represent the position information of drone i along the x-axis, y-axis and z-axis, Respectively represent the speed information of drone i along the x-axis, y-axis and z-axis, Respectively represent the acceleration information of i along the x-axis, y-axis and z-axis;
[0015] The state equation is constructed as:
[0016]
[0017] in, is the estimated state value of UAV i at time t relative to time t-1, is the state vector of UAV i at time t-1, and F is the state transfer matrix;
[0018] The calculation of the state transfer matrix F is:
[0019]
[0020] Where f is the motion equation of the UAV.
[0021] Furthermore, the absolute measurement equation between the master UAV and the slave UAV i is constructed in S2 as follows:
[0022]
[0023] in, is the absolute measurement value between the master drone and the slave drone i, is the absolute measurement matrix from UAV i,
[0024] The relative measurement value equation between slave UAV i and slave UAV j is constructed as:
[0025]
[0026] in, is the relative measurement value between slave UAV i and slave UAV j, is the relative measurement matrix between slave UAV i and slave UAV j, and All are Gaussian white noise.
[0027] Furthermore, the Jacobian matrix of the absolute measurement equation and the relative measurement equation is calculated in S2, that is, the absolute measurement matrix H i and the relative measurement matrix D i for:
[0028]
[0029]
[0030] Furthermore, the scaling factor constructed in S3 is specifically:
[0031] In the process update and measurement update phases of the EKF filter, the system’s innovation error sequence is defined
[0032]
[0033] Substituting formula (2) into formula (6), we can obtain:
[0034]
[0035] Taking the variance of both sides of formula (7), and since the measurement error and process error are orthogonal, we can get:
[0036]
[0037] in, is the prediction error covariance matrix at time t relative to time t-1, For Variance, that is The cosquare matrix of the forecast error at time t-1 The relationship is:
[0038]
[0039] in, is the process noise of the UAV caused by external interference;
[0040] According to formula (8), the covariance of the observation error is estimated using a finite number of new information value samples. Calculate, that is:
[0041]
[0042] In the formula, m is the size of the estimation window. In order to make formula (10) effective, the sequence It must be traversed smoothly over m steps;
[0043] For the filter, assume that:
[0044]
[0045] in, is the estimate of the forecast error covariance matrix;
[0046] Define the scaling factor as:
[0047]
[0048] Where trace represents the trace of the matrix, and the scaling factor α means the rough ratio between the calculated innovation error sequence and the predicted innovation error sequence;
[0049] Substituting formula (9) into formula (12), we get:
[0050]
[0051] in, For The predicted estimated value of .
[0052] Furthermore, the system error covariance matrix is updated using the scaling factor in S4 Right now:
[0053]
[0054] Furthermore, S5 is specifically:
[0055] If the absolute measurement error is smaller than the relative measurement error, the system gain matrix of the absolute measurement value is calculated first: Then calculate the relative measurement system gain matrix Right now:
[0056]
[0057]
[0058] Among them, N i represents the set of drones’ communicable neighbor drones, is the prediction covariance matrix, for The variance of
[0059] If the absolute measurement error is greater than the relative measurement error, first calculate the relative measurement system gain matrix Then calculate the system gain matrix of the absolute measurement value Right now:
[0060]
[0061]
[0062] Further, the relative measurement value is calculated in S6 as:
[0063]
[0064] Update the state estimate and system error covariance matrix, namely:
[0065]
[0066]
[0067] A distributed UAV swarm EKF collaborative positioning system based on scaling factors, the system has a program module corresponding to the steps of any one of the above technical solutions, and executes the steps in the above distributed UAV swarm EKF collaborative positioning method based on scaling factors during operation.
[0068] A computer-readable storage medium stores a computer program, wherein the computer program is configured to implement the steps of the distributed UAV swarm EKF collaborative positioning method based on scaling factor described in any one of the above technical solutions when called by a processor.
[0069] Compared with the prior art, the present invention has the following beneficial effects:
[0070] The distributed UAV swarm EKF collaborative positioning method and system based on the scaling factor of the present invention combines the scaling factor with the distributed extended Kalman filter, integrates the absolute measurement information between the master UAV and the slave UAV, and the relative measurement information between the slave UAVs, and uses the scaling factor to adjust the observation noise of the two measurement information of the distributed extended Kalman filter in real time, thereby further improving the positioning accuracy of the UAV swarm.
[0071] Compared with a simple distributed extended Kalman filter, the distributed extended Kalman filter with a scaling factor introduced in the present invention can effectively reduce the impact of sudden changes in observation noise on positioning accuracy through real-time adjustment of the scaling factor in the case of sudden changes in observation noise. BRIEF DESCRIPTION OF THE DRAWINGS
[0072] Figure 1 It is a flow chart of the EKF collaborative positioning method of a distributed UAV swarm based on scaling factors in an embodiment of the present invention;
[0073] Figure 2This is a positioning error analysis diagram of method 1 before and after adding a scaling factor in an embodiment of the present invention;
[0074] Figure 3 This is a positioning error analysis diagram of method 2 before and after adding a scaling factor in an embodiment of the present invention;
[0075] Figure 4 This is a comparison diagram of positioning errors of a distributed EKF collaborative positioning method 1 based on scaling factors in a noise interference environment in an embodiment of the present invention;
[0076] Figure 5 This is a comparison chart of positioning errors of distributed EKF collaborative positioning method 2 based on scaling factors in a noise interference environment in an embodiment of the present invention. DETAILED DESCRIPTION
[0077] In the description of the present invention, it should be noted that the terms "first", "second", and "third" mentioned in the embodiments of the present invention are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second", and "third" may explicitly or implicitly include one or more of the features.
[0078] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.
[0079] like Figure 1 As shown, the distributed UAV swarm EKF collaborative positioning method based on scaling factor of the present invention includes the following steps:
[0080] S1. Select the state variables of the UAV, construct the state equation, and initialize the state variables and covariance matrix;
[0081] S2. Construct the absolute measurement equation between the master drone and the slave drone i and the relative measurement value equation between the slave drone i and the slave drone j, and calculate the Jacobian matrix H of the absolute measurement equation and the relative measurement equation i and D i ;
[0082] S3, constructing a scaling factor α;
[0083] S4. Update the system error covariance matrix using the scaling factor
[0084] S5. Calculate the system gain matrix of the absolute measurement value and the relative measured system gain matrix
[0085] S6. Calculate the relative measurement value, update the state estimation value and the system error covariance matrix, and realize the collaborative positioning of the drone group.
[0086] S1 is as follows: select the position, velocity and acceleration of the drone as state variables, that is, in, Respectively represent the position information of drone i along the x-axis, y-axis and z-axis, Respectively represent the speed information of drone i along the x-axis, y-axis and z-axis, Respectively represent the acceleration information of i along the x-axis, y-axis and z-axis;
[0087] The state equation is constructed as:
[0088]
[0089] in, is the estimated state value of UAV i at time t relative to time t-1, is the state vector of UAV i at time t-1, and F is the state transfer matrix;
[0090] The calculation of the state transfer matrix F is:
[0091]
[0092] Where f is the motion equation of the UAV.
[0093] The absolute measurement equation between the master UAV and the slave UAV i constructed in S2 is:
[0094]
[0095] in, is the absolute measurement value between the master drone and the slave drone i, is the absolute measurement matrix from UAV i,
[0096] The relative measurement value equation between slave UAV i and slave UAV j is constructed as:
[0097]
[0098] in, is the relative measurement value between slave UAV i and slave UAV j, is the relative measurement matrix between slave UAV i and slave UAV j, and All are Gaussian white noise.
[0099] In S2, the Jacobian matrix of the absolute measurement equation and the relative measurement equation is calculated, that is, the absolute measurement matrix Hi and the relative measurement matrix D i for:
[0100]
[0101]
[0102] Assume that there is 1 master drone and n slave drones in the drone system. The master drone can communicate with all slave drones, but the slave drones are not fully connected. Assume that each slave drone can only communicate with p slave drones adjacent to it.
[0103] The scaling factor constructed in S3 is specifically:
[0104] In the process update and measurement update phases of the EKF filter, the system’s innovation error sequence is defined
[0105]
[0106] Substituting formula (2) into formula (6), we can obtain:
[0107]
[0108] Taking the variance of both sides of formula (7), and since the measurement error and process error are orthogonal, we can get:
[0109]
[0110] in, is the prediction error covariance matrix at time t relative to time t-1, For Variance, that is The cosquare matrix of the forecast error at time t-1 The relationship is:
[0111]
[0112] in, is the process noise of the UAV caused by external interference;
[0113] According to formula (8), the covariance of the observation error is estimated using a finite number of new information value samples. Calculate, that is:
[0114]
[0115] In the formula, m is the size of the estimation window. If the observations of all moments before this moment are used, the number of samples saved will be greatly increased. In order to reduce the complexity of the algorithm, the size of the window is limited. In order to make formula (10) effective, the sequence It must be traversed smoothly over m steps;
[0116] In order to improve the robustness of the adaptive filtering algorithm, for the filter, it is assumed that:
[0117]
[0118] in, is the estimate of the forecast error covariance matrix;
[0119] Define the scaling factor as:
[0120]
[0121] Where trace represents the trace of the matrix, and the scaling factor α means the rough ratio between the calculated innovation error sequence and the predicted innovation error sequence;
[0122] Substituting formula (9) into formula (12), we get:
[0123]
[0124] in, For The predicted estimated value of .
[0125] Scaling factors are used in S4 to update the system error covariance matrix That is, using the smoothing effect of the square root, modify the process noise of the algorithm:
[0126]
[0127] Calculate the system's prior error covariance matrix The process noise of the covariance matrix is processed by the scaling factor, making the process noise closer to the real noise and improving the stability of the system.
[0128] S5 provides two different calculation methods for the absolute measurement error and the relative measurement error, specifically:
[0129] (Method 1) If the absolute measurement error is smaller than the relative measurement error, first calculate the system gain matrix of the absolute measurement value Then calculate the relative measurement system gain matrix Right now:
[0130]
[0131]
[0132] Among them, N i represents the set of drones’ communicable neighbor drones, is the prediction covariance matrix, for The variance of
[0133] (Method 2) If the absolute measurement error is greater than the relative measurement error, first calculate the relative measurement system gain matrix Then calculate the system gain matrix of the absolute measurement value Right now:
[0134]
[0135]
[0136] The relative measurement value calculated in S6 is:
[0137]
[0138] Update the state estimate and system error covariance matrix, namely:
[0139]
[0140]
[0141] Example 1
[0142] When there is no mutation noise during the operation of the system, the observation noise of the system is constant. Set the absolute observation noise R between the master UAV and the slave UAV in the system d is 10cm, the absolute angle measurement error R angle is 0.5°; the absolute observation noise V between the slave drone and the slave drone in the system d The absolute angle measurement error V is 10 cm. angle Under this system setting, the two original distributed EKF collaborative positioning methods in S5 and the two distributed EKF collaborative positioning methods based on scaling factors of the present invention are simulated and analyzed respectively, and the following results are obtained: Figure 2 and Figure 3 Positioning error comparison chart shown.
[0143] from Figure 2 and Figure 3 It can be seen that the positioning effect of the distributed EKF collaborative positioning method based on scaling factor of the present invention is improved compared with the original distributed EKF collaborative positioning method. The positioning accuracy in the initial stage is not much different. As time goes by, the positioning accuracy of the method of the present invention gradually improves. Table 1 shows the root mean square results of the positioning errors of each method.
[0144] Table 1
[0145]
[0146] It can be seen from Table 1 that during the normal operation of the system, compared with the original distributed EKF collaborative positioning method 1, the positioning accuracy is improved by 24.99% after the scaling factor is used, and compared with the original distributed EKF collaborative positioning method 2, the positioning accuracy is improved by 24.22% after the scaling factor is used. It can be seen that after the scaling factor is used, the influence of measurement noise is reduced, and the positioning accuracy is significantly better than the original distributed EKF collaborative positioning method. In the initial stage, due to the small amount of data, the scaling factor does not play a big role, so the positioning effects of the two methods are almost the same. As the positioning process proceeds, the number of samples gradually increases, and the influence of the scaling factor on the observation noise gradually increases, thereby obtaining better positioning results.
[0147] Example 2
[0148] When there is sudden noise during the operation of the system, the distance measurement errors between the master and slave drones and between the slave and slave drones in the system are set to 10 cm, and the angle measurement errors are set to 0.5°. Between 30s and 60s, the slave drone i is affected by the environment, causing the distance measurement error of the system to increase from 10 cm to 20 cm, and the angle measurement error to increase from 0.5° to 1°. Under this system setting, the two original distributed EKF collaborative positioning methods 1 and 2 in S5 and the distributed EKF collaborative positioning methods 1 and 2 based on scaling factors of the present invention are simulated and analyzed, and the following results are obtained: Figure 4 and Figure 5 The positioning error comparison diagram is shown in Figure 2. Figure 4 and Figure 5 It can be seen that the positioning effect of the distributed EKF collaborative positioning method based on the scaling factor of the present invention is better than that of the original distributed EKF collaborative positioning method, and in the case of a sudden noise change, the distributed EKF collaborative positioning method based on the scaling factor of the present invention is more stable and has stronger anti-noise performance. Table 2 shows the root mean square results of the positioning error of each method.
[0149] Table 2
[0150]
[0151]
[0152] According to Table 2, after the noise mutation, the positioning error of distributed EKF collaborative positioning method 1 increased by 15.811% compared with the whole process, and the positioning error of method 2 increased by 17.859% compared with the whole process. The positioning error of distributed EKF collaborative positioning method 1 based on the scaling factor increased by 4.018% compared with the whole process, and the positioning error of distributed EKF collaborative positioning method 2 based on the scaling factor increased by 14.749% compared with the whole process. The increase in positioning error is significantly lower than that of the original distributed EKF collaborative positioning method. This shows that the scaling factor can effectively reduce the impact of the measurement noise mutation.
[0153] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.
Claims
1. A distributed UAV swarm EKF collaborative positioning method based on scaling factor, characterized in that: The steps include: S1. Select the state variables of the UAV, construct the state equation, and initialize the state variables and covariance matrix; S2. Construct the absolute measurement equation between the master drone and the slave drone i and the relative measurement value equation between the slave drone i and the slave drone j, and calculate the Jacobian matrix H of the absolute measurement equation and the relative measurement equation i and D i ; S3, constructing a scaling factor α; S4. Update the system error covariance matrix using the scaling factor S5. Calculate the system gain matrix of the absolute measurement value and the relative measured system gain matrix S6. Calculate the relative measurement value, update the state estimation value and the system error covariance matrix, and realize the coordinated positioning of the drone group; The absolute measurement equation between the master UAV and the slave UAV i constructed in S2 is: in, is the absolute measurement value between the master drone and the slave drone i, is the absolute measurement matrix from UAV i, The relative measurement value equation between slave UAV i and slave UAV j is constructed as: in, is the relative measurement value between slave UAV i and slave UAV j, is the relative measurement matrix between slave UAV i and slave UAV j, and All are Gaussian white noise; The scaling factor constructed in S3 is specifically: In the process update and measurement update phases of the EKF filter, the system’s innovation error sequence is defined Substituting formula (2) into formula (6), we can obtain: Taking the variance of both sides of formula (7), and since the measurement error and process error are orthogonal, we can get: in, is the prediction error covariance matrix at time t relative to time t-1, W t i For Variance, that is The cosquare matrix of the forecast error at time t-1 The relationship is: in, is the process noise of the UAV caused by external interference; According to formula (8), the covariance of the observation error is estimated using a finite number of new information value samples. Calculate, that is: In the formula, m is the size of the estimation window. In order to make formula (10) effective, the sequence It must be traversed smoothly over m steps; For the filter, assume that: in, is the estimate of the forecast error covariance matrix; Define the scaling factor as: Where trace represents the trace of the matrix, and the scaling factor α means the rough ratio between the calculated innovation error sequence and the predicted innovation error sequence; Substituting formula (9) into formula (12), we get: in, For The predicted estimated value of .
2. The distributed UAV swarm EKF collaborative positioning method based on scaling factor according to claim 1 is characterized in that: S1 is as follows: select the position, velocity and acceleration of the drone as state variables, that is, in, Respectively represent the position information of drone i along the x-axis, y-axis and z-axis, Respectively represent the speed information of drone i along the x-axis, y-axis and z-axis, Respectively represent the acceleration information of drone i along the x-axis, y-axis and z-axis; The state equation is constructed as: in, is the estimated state value of UAV i at time t relative to time t-1, is the state vector of UAV i at time t-1, and F is the state transfer matrix; The calculation of the state transfer matrix F is: Where f is the motion equation of the UAV.
3. The distributed UAV swarm EKF collaborative positioning method based on scaling factor according to claim 2 is characterized in that: In S2, the Jacobian matrix of the absolute measurement equation and the relative measurement equation is calculated, that is, the absolute measurement matrix H i and the relative measurement matrix D i for:
4. The distributed UAV swarm EKF collaborative positioning method based on scaling factor according to claim 1 is characterized in that: Scaling factors are used in S4 to update the system error covariance matrix Right now:
5. The distributed UAV swarm EKF collaborative positioning method based on scaling factor according to claim 4 is characterized in that: S5 is as follows: If the absolute measurement error is smaller than the relative measurement error, the system gain matrix of the absolute measurement value is calculated first: Then calculate the relative measurement system gain matrix Right now: Among them, N i represents the set of drones’ communicable neighbor drones, is the prediction covariance matrix, for The variance of If the absolute measurement error is greater than the relative measurement error, first calculate the relative measurement system gain matrix Then calculate the system gain matrix of the absolute measurement value Right now:
6. The distributed UAV swarm EKF collaborative positioning method based on scaling factor according to claim 5 is characterized in that: The relative measurement value calculated in S6 is: Update the state estimate and system error covariance matrix, namely:
7. The distributed UAV swarm EKF collaborative positioning system based on scaling factor is characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 6 above, and executes the steps in the above-mentioned distributed drone swarm EKF collaborative positioning method based on scaling factor when running.
8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the distributed UAV swarm EKF collaborative positioning method based on scaling factors according to any one of claims 1 to 6 when called by a processor.
