Clustered unmanned aerial vehicle inter-vehicle state estimation method and device under communication restriction

Abstract

This application provides a method and apparatus for estimating the inter-machine state of a cluster of unmanned aerial vehicles (UAVs) under communication constraints. The method includes: 1. Establishing a first equation of motion for the cluster of UAVs under communication constraints; 2. Estimating the state of the UAVs based on their velocity v. k The change yields ε k According to the rate of change of azimuth angle ω k The change yields η k According to ε k and η k 1. Obtain the state noise and introduce it into the first equation of motion to obtain the second equation of motion; 2. Obtain the state equation of the swarm UAV inter-machine state estimation system based on the second equation of motion; 3. Establish the observation equation of the swarm UAV inter-machine state estimation system based on the UAV position information obtained between the swarm UAVs; 4. Estimate the UAV motion state at time k+1 using the extended Kalman filter and the state equation and observation equation of the swarm UAV inter-machine state estimation system; 5. Estimate the UAV motion state at time k+1.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicle (UAV) swarm, and particularly relates to a method and device for estimating the state of UAVs in a swarm under communication restriction. TECHNICAL BACKGROUND

[0002] With the development of UAV swarm technology, UAV swarm countermeasure technology is also constantly improving. Generally, electromagnetic interference is generated to restrict the communication of the UAV swarm, so that the UAV swarm cannot fly stably. There are generally two situations of communication restriction of the UAV swarm. On the one hand, the communication restriction will affect the communication bandwidth of the UAV swarm, that is, the complete state information of the UAV swarm may not be completely transmitted. On the other hand, the communication restriction will cause data loss between the UAVs in the swarm. Intermittent communication interruption will cause the information between the UAVs to be unable to be updated at a fixed period. In order to ensure the stable maintenance of the UAV swarm under the limited communication condition, in the first situation, the communication content of the UAV swarm needs to be reduced to reduce the communication bandwidth between the UAVs; in the second situation, when the data is lost, the original information of the UAV is relied on to predict the motion state information in real time.

[0003] Generally, the position information between the UAVs in the swarm is essential, and the motion speed and direction information can be obtained by differentiating the position information. However, the global navigation satellite system (GNSS) position information of the UAV will generate a large noise in its motion process, and the differentiation thereof will further cause a large noise in the speed. In addition, when the data is lost, it is difficult to predict the state of the UAV. Therefore, a better method is needed. SUMMARY

[0004] The purpose of the application is to provide a method for estimating the state of UAVs in a swarm under communication restriction, which solves the technical problems of bandwidth reduction and data loss of the UAV swarm under communication restriction.

[0005] In a first aspect, the application provides a method for estimating the state of UAVs in a swarm under communication restriction, which comprises the following steps:

[0006] Step 1: establishing a first motion equation of the UAVs in the swarm under communication restriction;

[0007] Step 2: obtaining from the change of the motion speed v k of the UAVs, ε k , obtaining from the change of the track azimuth rate ω k , η k , obtaining the state noise from ε k and η k , and introducing the state noise into the first motion equation to obtain a second motion equation;

[0008] Step 3: Obtain the state equation of the inter-vehicle state estimation system of the swarm UAV according to the second motion equation;

[0009] Step 4: Establish the observation equation of the inter-vehicle state estimation system of the swarm UAV according to the inter-vehicle obtained UAV position information of the swarm UAV;

[0010] Step 5: According to the extended Kalman filter, the state equation and the observation equation of the inter-vehicle state estimation system of the swarm UAV are used to estimate the motion state of the UAV, and the motion state estimation value of the UAV at time k+1 is obtained

[0011] Step 6: When the inter-vehicle obtained UAV position information of the observation equation at time k+1 is lost, the state equation and the observation equation of the inter-vehicle state estimation system of the swarm UAV are used to estimate the motion state of the UAV, and the motion state estimation value of the UAV at time k+1 is obtained

[0012] Further, the first motion equation is:

[0013]

[0014] Wherein, the sampling time T is the time consumption between time k+1 and time k, and the sampling time T is fixed; v k is the UAV motion speed at time k, v k+1 is the UAV motion speed at time k+1, ω k is the UAV motion speed at time k, ω k+1 is the UAV motion speed at time k+1, (x k , y k ) is the UAV space position at time k, (x k+1 , y k+1 ) is the UAV space position at time k+1, γ k is the UAV track azimuth at time k, γ k+1 is the UAV track azimuth at time k+1, the UAV motion speed v k is constant, and the track azimuth change rate ω k is constant.

[0015] Further, the second motion equation is:

[0016]

[0017] Further, the state equation of the inter-vehicle state estimation system of the swarm UAV is:

[0018] X k+1 = f(X k , W k )

[0019] wherein the state of the inter-vehicle state estimation system of the swarm UAV at time k is represented as X k = [x k , y k , v k , γ k , ω k ] T the state of the inter-vehicle state estimation system of the swarm UAV at time k+1 is represented as X k+1 = [x k+1 , y k+1 , v k+1 , γ k+1 , ω k+1 ] T the state noise of the inter-vehicle state estimation system of the swarm UAV at time k is represented as W k = [ε k , η k ] T .

[0020] Further, the observation equation of the inter-vehicle state estimation system of the swarm UAV is:

[0021] Z k+1 = HX k+1 + v k+1

[0022] wherein Z k+1 = [x k+1 , y k+1 ] T represents the position information of the UAV obtained by the inter-vehicle state estimation system of the swarm UAV at time k+1, v k+1 represents the observation noise at time k+1, and the observation matrix is represented as

[0023] Further, step 5 comprises:

[0024] Step 51: predicting the state equation of the inter-vehicle state estimation system of the swarm UAV to obtain the predicted value of the UAV motion state wherein X represents the estimated value of the UAV motion state at time k, and X represents the predicted value of the UAV motion state at time k+1.

[0025] Step 52: calculating the covariance of the UAV motion state prediction error to obtain the covariance matrix of the UAV motion state prediction error P(k+1|k) = F(k)P(k|k)F(k) T + G(k)Q(k)G(k) Twhere P(k+1|k) represents the covariance matrix of the UAV motion state prediction error at time k+1, P(k|k) represents the covariance matrix of the UAV motion state estimation error at time k, Q(k) is the covariance matrix of the state noise W k at time k, F(k) represents the linearized approximation state transition matrix at time k, and G(k) represents the linearized approximation noise matrix at time k;

[0026] Step 53: calculating the UAV inter-vehicle state estimation system Kalman gain, obtaining the UAV inter-vehicle state estimation system Kalman gain matrix K(k+1) = P(k+1|k)H T [HP(k+1|k)H T +R(k+1)] -1 where K(k+1) is the UAV inter-vehicle state estimation system Kalman gain matrix at time k+1, and R(k+1) is the covariance matrix of the observation noise v k+1 at time k+1.

[0027] Step 54: calculating the UAV motion state estimation value, obtaining the UAV motion state estimation value at time k+1

[0028] More specifically, the UAV motion state prediction value is brought into the observation equation of the swarm UAV inter-vehicle state estimation system, and the UAV position information Z(k+1) obtained by the swarm UAV inter-vehicle at time k+1 is subtracted therefrom to obtain the estimation residual at time k+1 Further, the UAV motion state estimation value at time k+1 is obtained by further utilizing the residual

[0029] Step 55: calculating the covariance of the UAV motion estimation error, obtaining the covariance matrix P(k+1) = [I-K(k+1)H]P(k+1|k) of the UAV motion estimation error at time k+1, where I represents the unit matrix.

[0030] Further, step 54 specifically includes:

[0031] The UAV motion state prediction value is brought into the observation equation of the swarm UAV inter-vehicle state estimation system, and the UAV position information Z(k+1) obtained by the swarm UAV inter-vehicle at time k+1 is subtracted therefrom to obtain the estimation residual at time k+1 Further, the UAV motion state estimation value at time k+1 is obtained by further utilizing the residual

[0032] Further, step 6 includes:

[0033] Step 61: calculating the observation noise v k+1Set to 0, the state equation of state estimates into the observation equation Obtain the unmanned aerial vehicle position information Z'(k+1) when the unmanned aerial vehicle position information at k+1 time is lost;

[0034] Step 62: Substitute the unmanned aerial vehicle position information when the unmanned aerial vehicle position information is lost into step 5, that is, the unmanned aerial vehicle position information Z'(k+1) when the unmanned aerial vehicle position information at k+1 time is lost instead of the unmanned aerial vehicle position information Z(k+1) obtained by the inter-cluster unmanned aerial vehicle at k+1 time;

[0035] Step 63: Predict the state equation of the inter-cluster unmanned aerial vehicle state estimation system to obtain the unmanned aerial vehicle motion state prediction value Wherein represents the unmanned aerial vehicle motion state estimation value at k time, represents the unmanned aerial vehicle motion state prediction value at k+1 time;

[0036] Step 64: Calculate the covariance of the unmanned aerial vehicle motion state prediction error to obtain the covariance matrix of the unmanned aerial vehicle motion state prediction error P(k+1|k)=F(k)P(k|k)F(k) T +G(k)Q(k)G(k) T , wherein P(k+1|k) represents the covariance matrix of the unmanned aerial vehicle motion state prediction error at k+1 time, P(k|k) represents the covariance matrix of the unmanned aerial vehicle motion state estimation error at k time, Q(k) is the covariance matrix of the state noise W k at k time, F(k) represents the linearized approximation state transition matrix at k time, and G(k) represents the linearized approximation noise matrix at k time;

[0037] Step 65: Calculate the Kalman gain of the inter-unmanned aerial vehicle state estimation system to obtain the Kalman gain matrix K(k+1)=P(k+1|k)H T [HP(k+1|k)H T +R(k+1)] -1 , wherein K(k+1) is the Kalman gain matrix of the inter-unmanned aerial vehicle state estimation system at k+1 time, and R(k+1) is the covariance matrix of the observation noise v k+1 at k+1 time;

[0038] Step 66: Calculate the unmanned aerial vehicle motion state estimation value to obtain the unmanned aerial vehicle motion state estimation value at k+1 time

[0039] Step 67: Calculate the covariance of the unmanned aerial vehicle motion estimation error to obtain the covariance matrix of the unmanned aerial vehicle motion estimation error at k+1 time P(k+1)=[I-K(k+1)H]P(k+1|k), wherein I represents the unit matrix;

[0040] Step 68: complete the target state prediction under data loss.

[0041] Further, step 66 specifically includes:

[0042] The UAV motion state prediction value is brought into the observation equation of the inter-cluster UAV inter-vehicle state estimation system, and the estimation residual at time k+1 is obtained by subtracting the UAV position information Z(k+1) obtained by the inter-cluster UAV inter-vehicle at time k+1 from it Further, the UAV motion state estimation value at time k+1 is obtained by using the residual Since the UAV position information Z'(k+1) at time k+1 is used instead of the UAV position information Z(k+1) obtained by the inter-cluster UAV inter-vehicle at time k+1 when the UAV position information at time k+1 is lost, the residual is 0 at this time.

[0043] In a second aspect, the application provides an inter-cluster UAV inter-vehicle state estimation device under communication restriction, which is used to realize the inter-cluster UAV inter-vehicle state estimation method under communication restriction.

[0044] In summary, the application provides an inter-cluster UAV inter-vehicle state estimation method and device under communication restriction, which establishes the state equation of the inter-cluster UAV inter-vehicle state estimation system and the observation equation of the inter-cluster UAV inter-vehicle state estimation system, further designs a state estimation strategy based on extended Kalman filter (EKF), and can estimate the motion state information only by relying on position information, and has good processing effect for information loss. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 A flow chart of the inter-cluster UAV inter-vehicle state estimation method under communication restriction of the application;

[0046] Figure 2 The flight path of the estimated UAV in the implementation example of the application;

[0047] Figure 3 The speed estimation effect in the implementation example of the application;

[0048] Figure 4 The flight path azimuth estimation effect in the implementation example of the application;

[0049] Figure 5 The speed estimation effect when data is lost in the implementation example of the application;

[0050] Figure 6 The flight path azimuth estimation effect when data is lost in the implementation example of the application. DETAILED DESCRIPTION

[0051] Embodiment one

[0052] As Figure 1 shown, the application provides a method for inter-vehicle state estimation of a swarm of unmanned aerial vehicles under communication constraints, comprising:

[0053] Step 1: Establish a first motion equation for the swarm of unmanned aerial vehicles under communication constraints:

[0054]

[0055] wherein the sampling time T is the time consumption between the k+1 time and the k time, and the sampling time T is fixed; v k is the unmanned aerial vehicle motion speed at the k time, v k+1 is the unmanned aerial vehicle motion speed at the k+1 time, ω k is the unmanned aerial vehicle motion speed at the k time, ω k+1 is the unmanned aerial vehicle motion speed at the k+1 time, (x k , y k ) is the unmanned aerial vehicle space position at the k time, (x k+1 , y k+1 ) is the unmanned aerial vehicle space position at the k+1 time, γ k is the unmanned aerial vehicle track azimuth at the k time, γ k+1 is the unmanned aerial vehicle track azimuth at the k+1 time, the unmanned aerial vehicle motion speed v k is constant, the track azimuth change rate ω k is constant.

[0056] Step 2: Obtain ε k according to the change of the unmanned aerial vehicle motion speed v k , obtain η k according to the change of the track azimuth change rate ω k , obtain the state noise according to ε k and η k , and introduce the state noise into the first motion equation to obtain a second motion equation:

[0057]

[0058] It should be noted that ε k and η k are Gaussian white noises with a mean of 0.

[0059] Step 3: Obtain the state equation of the inter-vehicle state estimation system of the swarm of unmanned aerial vehicles according to the second motion equation:

[0060]

[0061] wherein the state of the inter-vehicle state estimation system of the swarm of unmanned aerial vehicles at the k time is represented as X k = [xk ,y k ,v k ,γ k ,ω k ] T The state of the inter-vehicle state estimation system of the swarm UAV at the k+1 time is represented as X k+1 =[x k+1 ,y k+1 ,v k+1 ,γ k+1 ,ω k+1 ] T The state noise of the inter-vehicle state estimation system of the swarm UAV at the k time is represented as W k =[ε k ,η k ] T .

[0062] Step 4: According to the UAV position information obtained by the inter-vehicle of the swarm UAV, the observation equation of the inter-vehicle state estimation system of the swarm UAV is established:

[0063] Z k+1 =HX k+1 +ν k+1 (4)

[0064] Wherein, Z k+1 =[x k+1 ,y k+1 ] T represents the UAV position information obtained by the inter-vehicle of the swarm UAV at the k+1 time, v k+1 represents the observation noise at the k+1 time, and the observation matrix is represented as

[0065] It should be noted that the observation noise is a Gaussian white noise with a mean of zero.

[0066] Step 5: According to the extended Kalman filter, the state equation and the observation equation of the inter-vehicle state estimation system of the swarm UAV are used to estimate the motion state of the UAV, and the motion state estimation value of the UAV at the k+1 time is obtained

[0067] Specifically, step 5 includes:

[0068] Step 51: The state equation of the inter-vehicle state estimation system of the swarm UAV is predicted to obtain the motion state prediction value of the UAV Wherein represents the motion state estimation value of the UAV at the k time, represents the motion state prediction value of the UAV at the k+1 time;

[0069] More specifically, it is assumed that the state noise Wk = 0, the k time unmanned aerial vehicle motion state estimation value The state equation of the inter-vehicle state estimation system of the swarm unmanned aerial vehicle is obtained by bringing the second motion equation, that is, the unmanned aerial vehicle motion state prediction value is obtained.

[0070] Step 52: Calculate the covariance of the unmanned aerial vehicle motion state prediction error, and obtain the covariance matrix P(k+1|k) = F(k)P(k|k)F(k) of the unmanned aerial vehicle motion state prediction error. T + G(k)Q(k)G(k) T Wherein P(k+1|k) represents the covariance matrix of the unmanned aerial vehicle motion state prediction error at k+1, P(k|k) represents the covariance matrix of the unmanned aerial vehicle motion state estimation error at k, Q(k) is the covariance matrix of the state noise W k at k, F(k) represents the linear approximation state transition matrix at k, and G(k) represents the linear approximation noise matrix at k.

[0071] More specifically, according to the EKF filtering process, the linear approximation state transition matrix and the noise matrix can be obtained by taking the partial derivative of the state equation of the inter-vehicle state estimation system of the swarm unmanned aerial vehicle obtained by the second motion equation.

[0072] Step 53: Calculate the Kalman gain of the unmanned aerial vehicle inter-vehicle state estimation system, and obtain the Kalman gain matrix K(k+1) = P(k+1|k)H T [HP(k+1|k)H T + R(k+1)] -1 Wherein K(k+1) is the Kalman gain matrix of the unmanned aerial vehicle inter-vehicle state estimation system at k+1, and R(k+1) is the covariance matrix of the observation noise v k+1 at k+1.

[0073] Step 54: Calculate the unmanned aerial vehicle motion state estimation value, and obtain the unmanned aerial vehicle motion state estimation value at k+1

[0074] More specifically, the unmanned aerial vehicle motion state prediction value is brought into the observation equation of the inter-vehicle state estimation system of the swarm unmanned aerial vehicle, and the estimation residual at k+1 is obtained by subtracting the unmanned aerial vehicle position information Z(k+1) obtained by the swarm unmanned aerial vehicle at k+1 from it. Further, the unmanned aerial vehicle motion state estimation value at k+1 is obtained by using the residual.

[0075] Step 55: Calculate the covariance of the UAV motion estimation error to obtain the covariance matrix of the UAV motion estimation error at time k+1, P(k+1) = [I-K(k+1)H]P(k+1|k), wherein I represents a unit matrix;

[0076] Step 6: When the inter-cluster UAV-acquired UAV position information of the observation equation at time k+1 is lost, that is, the inter-cluster UAV-acquired UAV position information Z(k+1) data at time k+1 is lost, the UAV motion state is estimated by using the state equation and the observation equation of the inter-cluster UAV state estimation system, and the UAV motion state estimation value at time k+1 is obtained

[0077] Step 6 includes:

[0078] Step 61: Substitute the observation noise v k+1 into the observation equation, and set the state estimation value of the state equation to 0 to obtain the UAV position information Z'(k+1) when the UAV position information at time k+1 is lost.

[0079] Step 62: Substitute the UAV position information when the UAV position information is lost into step 5, that is, substitute the UAV position information Z'(k+1) when the UAV position information at time k+1 is lost into the inter-cluster UAV-acquired UAV position information Z(k+1) at time k+1.

[0080] Step 63: Predict the state equation of the inter-cluster UAV state estimation system to obtain the UAV motion state prediction value wherein represents the UAV motion state estimation value at time k, and represents the UAV motion state prediction value at time k+1.

[0081] More specifically, it is assumed that the state noise W k of the inter-cluster UAV state estimation system at time k is 0, and the UAV motion state estimation value at time k is substituted into the second motion equation to obtain the state equation of the inter-cluster UAV state estimation system, that is, to obtain the UAV motion state prediction value.

[0082] Step 64: Calculate the covariance of the UAV motion state prediction error to obtain the covariance matrix of the UAV motion state prediction error, P(k+1|k) = F(k)P(k|k)F(k) T + G(k)Q(k)G(k) Twhere P(k+1|k) represents the covariance matrix of the UAV motion state prediction error at time k+1, P(k|k) represents the covariance matrix of the UAV motion state estimation error at time k, Q(k) is the covariance matrix of the state noise W k at time k, F(k) represents the linearized approximate state transition matrix at time k, and G(k) represents the linearized approximate noise matrix at time k.

[0083] More specifically, according to the EKF filtering process, the linearized approximate state transition matrix and the noise matrix can be obtained by taking the partial derivative of the state equation of the inter-UAV state estimation system of the swarm UAV obtained from the second motion equation

[0084] Step 65: Calculate the Kalman gain of the inter-UAV state estimation system of the UAV, and obtain the Kalman gain matrix K(k+1) of the inter-UAV state estimation system of the UAV at time k+1 T [HP(k+1|k)H T +R(k+1)] -1 where K(k+1) is the Kalman gain matrix of the inter-UAV state estimation system of the UAV at time k+1, and R(k+1) is the covariance matrix of the observation noise v k+1 at time k+1.

[0085] Step 66: Calculate the UAV motion state estimation value, and obtain the UAV motion state estimation value at time k+1

[0086] More specifically, the UAV motion state prediction value is brought into the observation equation of the inter-UAV state estimation system of the swarm UAV, and the estimation residual at time k+1 is obtained by subtracting the UAV position information Z(k+1) obtained by the inter-UAV of the swarm UAV at time k+1 from it. Further, the UAV motion state estimation value at time k+1 is obtained by using the residual Since the UAV position information Z'(k+1) at time k+1 is used instead of the UAV position information Z(k+1) obtained by the inter-UAV of the swarm UAV at time k+1 when the UAV position information at time k+1 is lost, the residual is 0 at this time.

[0087] Step 67: Calculate the covariance of the UAV motion estimation error, and obtain the covariance matrix P(k+1) of the UAV motion estimation error at time k+1

[0088] Step 68: Complete the target state prediction under data loss.

[0089] Embodiment Two

[0090] The embodiment of the application takes typical fixed-wing unmanned aerial vehicle formation flight as an example, and provides an inter-aircraft state estimation method for cluster unmanned aerial vehicles under communication restriction, comprising:

[0091] The flight speed of the simulation test of the unmanned aerial vehicle in the cluster unmanned aerial vehicle is set to 20 m / s, the motion includes uniform linear motion and fixed angular velocity turning, and the real-time position of the unmanned aerial vehicle is added with Gaussian white noise (mean value is 0 and variance is 1), and the flight effect is as shown in Figure 2 .

[0092] The inter-aircraft state estimation of the cluster unmanned aerial vehicle under communication restriction is realized through the following steps, comprising:

[0093] Step 1: establishing a first motion equation of the cluster unmanned aerial vehicle under communication restriction:

[0094]

[0095] Wherein, the sampling time T is the time consumption between the k+1 moment and the k moment, and the sampling time T is fixed; v k is the motion speed of the unmanned aerial vehicle at the k moment, v k+1 is the motion speed of the unmanned aerial vehicle at the k+1 moment, ω k is the motion speed of the unmanned aerial vehicle at the k moment, ω k+1 is the motion speed of the unmanned aerial vehicle at the k+1 moment, (x k , y k ) is the space position of the unmanned aerial vehicle at the k moment, (x k+1 , y k+1 ) is the space position of the unmanned aerial vehicle at the k+1 moment, γ k is the track azimuth angle of the unmanned aerial vehicle at the k moment, γ k+1 is the track azimuth angle of the unmanned aerial vehicle at the k+1 moment, the motion speed of the unmanned aerial vehicle v k is constant, the track azimuth angle change rate ω k is constant.

[0096] Step 2: obtaining ε k according to the change of the motion speed v k of the unmanned aerial vehicle, obtaining η k according to the change of the track azimuth angle change rate ω k , obtaining state noise according to ε k and η k , and introducing the state noise into the first motion equation to obtain a second motion equation:

[0097]

[0098] It should be noted that ε k and η k are Gaussian white noise with a mean value of 0.

[0099] Step 3: Obtain the state equation of the inter-vehicle state estimation system of the swarm UAV according to the second motion equation:

[0100] X k+1 = f(X k , W k ) (3)

[0101] wherein the state of the inter-vehicle state estimation system of the swarm UAV at time k is represented as X k = [x k , y k , v k , γ k , ω k ] T , the state of the inter-vehicle state estimation system of the swarm UAV at time k+1 is represented as X k+1 = [x k+1 , y k+1 , v k+1 , γ k+1 , ω k+1 ] T , and the state noise of the inter-vehicle state estimation system of the swarm UAV at time k is represented as W k = [ε k , η k ] T .

[0102] Step 4: Establish the observation equation of the inter-vehicle state estimation system of the swarm UAV according to the UAV position information obtained by the inter-vehicle of the swarm UAV:

[0103] Z k+1 = HX k+1 + v k+1 (4)

[0104] wherein Z k+1 = [x k+1 , y k+1 ] T represents the UAV position information obtained by the inter-vehicle of the swarm UAV at time k+1, v k+1 represents the observation noise at time k+1, and the observation matrix is represented as

[0105] It should be noted that the observation noise is Gaussian white noise with a mean of zero.

[0106] Step 5: Estimate the motion state of the UAV according to the extended Kalman filter, using the state equation and the observation equation of the inter-vehicle state estimation system of the swarm UAV, to obtain the UAV motion state estimation value at time k+1

[0107] Specifically, step 5 includes:

[0108] Step 51: predicting the state equation of the inter-vehicle state estimation system of the swarm UAV to obtain a UAV motion state prediction value wherein represents the UAV motion state estimation value at time k, represents the UAV motion state prediction value at time k+1;

[0109] More specifically, assuming that the state noise W k = 0 of the inter-vehicle state estimation system of the swarm UAV at time k, is brought into the second motion equation to obtain the state equation of the inter-vehicle state estimation system of the swarm UAV, i.e., to obtain the UAV motion state prediction value;

[0110] Step 52: calculating the covariance of the UAV motion state prediction error to obtain the covariance matrix P(k+1|k) = F(k)P(k|k)F(k) + G(k)Q(k)G(k) of the UAV motion state prediction error T + G(k)Q(k)G(k) T , wherein P(k+1|k) represents the covariance matrix of the UAV motion state prediction error at time k+1, P(k|k) represents the covariance matrix of the UAV motion state estimation error at time k, Q(k) is the covariance matrix of the state noise W k at time k, F(k) represents the linearized approximation state transition matrix at time k, and G(k) represents the linearized approximation noise matrix at time k;

[0111] More specifically, according to the EKF filtering process, the linearized approximation state transition matrix and the noise matrix

[0112] Step 53: calculating the Kalman gain of the inter-vehicle state estimation system of the UAV to obtain the Kalman gain matrix K(k+1) = P(k+1|k)H T [HP(k+1|k)H T + R(k+1)] -1 , wherein K(k+1) is the Kalman gain matrix of the inter-vehicle state estimation system of the UAV at time k+1, and R(k+1) is the covariance matrix of the observation noise v k+1 at time k+1;

[0113] Step 54: calculating the UAV motion state estimation value to obtain the UAV motion state estimation value at time k+1

[0114] More specifically, the UAV motion state prediction value The observation equation of the inter-vehicle state estimation system of the swarm UAV is brought in, and the estimation residual at the k+1 moment is obtained by subtracting the UAV position information Z(k+1) obtained by the inter-vehicle of the swarm UAV at the k+1 moment from the UAV position information The estimation residual is further used to obtain the UAV motion state estimation value at the k+1 moment

[0115] Step 55: Calculate the covariance of the UAV motion estimation error, and obtain the covariance matrix P(k+1) of the UAV motion estimation error at the k+1 moment, wherein I represents the unit matrix;

[0116] Step 6: When the inter-vehicle of the swarm UAV obtains the UAV position information of the observation equation at the k+1 moment is lost, that is, the data of the UAV position information Z(k+1) obtained by the inter-vehicle of the swarm UAV at the k+1 moment is lost, the state equation and the observation equation of the inter-vehicle state estimation system of the swarm UAV are used to estimate the UAV motion state, and the UAV motion state estimation value at the k+1 moment is obtained

[0117] Step 6 includes:

[0118] Step 61: Set the observation noise v k+1 to 0, and substitute the state estimation value of the state equation into the observation equation Obtain the UAV position information Z'(k+1) when the UAV position information at the k+1 moment is lost;

[0119] Step 62: Substitute the UAV position information when the UAV position information is lost into step 5, that is, substitute the UAV position information Z'(k+1) when the UAV position information at the k+1 moment is lost instead of the UAV position information Z(k+1) obtained by the inter-vehicle of the swarm UAV at the k+1 moment;

[0120] Step 63: Predict the state equation of the inter-vehicle state estimation system of the swarm UAV, and obtain the UAV motion state prediction value Wherein represents the UAV motion state estimation value at the k moment, represents the UAV motion state prediction value at the k+1 moment;

[0121] More specifically, it is assumed that the state noise W k of the inter-vehicle state estimation system of the swarm UAV at the k moment is 0, and the UAV motion state estimation value at the k moment is brought into the second motion equation to obtain the state equation of the inter-vehicle state estimation system of the swarm UAV, that is, to obtain the UAV motion state prediction value;

[0122] Step 64: calculating the covariance of the UAV motion state prediction error, obtaining the covariance matrix of the UAV motion state prediction error P(k+1|k) = F(k)P(k|k)F(k) + G(k)Q(k)G(k) T T wherein P(k+1|k) represents the covariance matrix of the UAV motion state prediction error at k+1 time, P(k|k) represents the covariance matrix of the UAV motion state estimation error at k time, Q(k) is the covariance matrix of the state noise W k at k time, F(k) represents the linearized approximation state transition matrix at k time, and G(k) represents the linearized approximation noise matrix at k time;

[0123] More specifically, according to the EKF filtering process, the linearized approximation state transition matrix and the noise matrix can be obtained by taking the partial derivative of the state equation of the inter-vehicle state estimation system of the swarm UAV obtained from the second motion equation

[0124] Step 65: calculating the Kalman gain of the UAV inter-vehicle state estimation system, obtaining the Kalman gain matrix K(k+1) = P(k+1|k)H T [HP(k+1|k)H T + R(k+1)] -1 wherein K(k+1) is the Kalman gain matrix of the UAV inter-vehicle state estimation system at k+1 time, and R(k+1) is the covariance matrix of the observation noise v k+1 at k+1 time;

[0125] Step 66: calculating the UAV motion state estimation value, obtaining the UAV motion state estimation value at k+1 time

[0126] More specifically, the UAV motion state prediction value is brought into the observation equation of the inter-vehicle state estimation system of the swarm UAV, and the estimation residual at k+1 time is obtained by using the UAV position information Z(k+1) obtained by the swarm UAV at k+1 time and the UAV position information Z′(k+1) obtained by the swarm UAV at k+1 time. Further, the UAV motion state estimation value at k+1 time is obtained by using the estimation residual Since the UAV position information Z′(k+1) obtained at k+1 time is used to replace the UAV position information Z(k+1) obtained by the swarm UAV at k+1 time when the UAV position information at k+1 time is lost, the residual is 0 at this time.

[0127] Step 67: calculating the covariance of the UAV motion estimation error, obtaining the covariance matrix of the UAV motion estimation error at k+1 time P(k+1) = [I-K(k+1)H]P(k+1|k), wherein I represents the unit matrix; ​

[0128] Step 68: complete the target state prediction under data loss.

[0129] By using the inter-vehicle state estimation method of cluster UAVs under communication constraints, the estimation error of the UAV movement speed and the track azimuth angle estimated without data loss is as shown in Figure 3 and Figure 4 The proposed method estimates the UAV movement speed estimation error to be within 0.15 m / s, and the UAV track azimuth angle estimation error to be within 4.5 degrees, meeting the requirements of cluster UAV formation flight;

[0130] By using the inter-vehicle state estimation method of cluster UAVs under communication constraints, when data loss occurs (data loss conditions are set to 10 seconds to 60 seconds: 3 seconds of data are randomly lost every 10 seconds), the obtained UAV movement speed and track azimuth angle error is as shown in Figure 5 and Figure 6 wherein 15 seconds-18 seconds, 25 seconds-28 seconds, 35 seconds-38 seconds, 45 seconds-48 seconds, 55 seconds-58 seconds, data loss occurs, the proposed method estimates the UAV movement speed estimation error to be within 0.7 m / s, and the UAV track azimuth angle estimation error to be within 6.5 degrees, meeting the requirements of cluster UAV formation flight.

Claims

1. A method for estimating the inter-machine state of a cluster of unmanned aerial vehicles (UAVs) under communication constraints, characterized in that, The method comprises: Step 1: establishing a first motion equation of the cluster unmanned aerial vehicle under communication restriction; Step 2: Based on the drone's speed The changes were obtained According to the rate of change of azimuth angle of the flight path The changes were obtained ,according to and The state noise is obtained, and the state noise is introduced into the first equation of motion to obtain the second equation of motion. Step 3: obtaining a state equation of the inter-cluster unmanned aerial vehicle state estimation system according to the second motion equation; Step 4: establishing an observation equation of the inter-cluster unmanned aerial vehicle state estimation system according to the unmanned aerial vehicle position information obtained by the inter-cluster unmanned aerial vehicle; Step 5: According to the extended Kalman filter, the state equation and the observation equation of the inter-vehicle state estimation system of the cluster UAV are used to estimate the motion state of the UAV, and the motion state estimation value of the UAV at time k+1 is obtained ; Step 6: When the cluster UAV inter-machine obtained UAV position information of the cluster UAV inter-machine observation equation is lost at k+1 moment, the state equation and the observation equation of the cluster UAV inter-machine state estimation system are used to estimate the UAV motion state, and the UAV motion state estimation value at k+1 moment is obtained ; specifically includes: The observation noise is set to 0, and the state estimation value of the state equation is substituted into the observation equation to obtain the unmanned aerial vehicle position information at the k+1 moment when the unmanned aerial vehicle position information is lost ; the unmanned aerial vehicle motion state estimation value is calculated to obtain the unmanned aerial vehicle motion state estimation value at the k+1 moment ; and the unmanned aerial vehicle position information at the k+1 moment when the unmanned aerial vehicle position information is lost is substituted for the unmanned aerial vehicle position information obtained by the cluster unmanned aerial vehicles at the k+1 moment , and at this time, the residual error is 0.

2. The method of claim 1, wherein, The first motion equation is: ； Wherein, the sampling time T is the time consumption between k+1 time and k time, the sampling time T is fixed; is the unmanned aerial vehicle movement speed at k time, is the unmanned aerial vehicle movement speed at k+1 time, is the unmanned aerial vehicle movement speed at k time, is the unmanned aerial vehicle movement speed at k+1 time, is the unmanned aerial vehicle space position at k time, is the unmanned aerial vehicle space position at k+1 time, is the unmanned aerial vehicle track azimuth at k time, is the unmanned aerial vehicle track azimuth at k+1 time, the unmanned aerial vehicle movement speed is constant, the track azimuth change rate is constant.

3. The method of claim 2, wherein, The second motion equation is: 。 4. The method of claim 3, wherein, The state equation of the inter-cluster unmanned aerial vehicle state estimation system is: Wherein, the state of the inter-vehicle state estimation system of the swarm UAV at the k moment is represented as The state of the inter-vehicle state estimation system of the swarm UAV at the k+1 moment is represented as The state noise of the inter-vehicle state estimation system of the swarm UAV at the k moment is represented as .

5. The method of claim 4, wherein, The observation equation of the inter-cluster unmanned aerial vehicle state estimation system is: wherein, represents the unmanned aerial vehicle position information obtained by the cluster unmanned aerial vehicle between unmanned aerial vehicles at k+1 moment, represents the observation noise at k+1 moment, and the observation matrix is represented as .

6. The method of claim 5, wherein, Step 5 comprises: Step 51: predicting the state equation of the inter-vehicle state estimation system of the cluster UAV to obtain a UAV motion state prediction value wherein represents the UAV motion state estimation value at time k, represents the UAV motion state prediction value at time k+1; Step 52: calculating the covariance of the unmanned aerial vehicle motion state prediction error to obtain the covariance matrix of the unmanned aerial vehicle motion state prediction error wherein denotes a covariance matrix of the prediction error of the UAV motion state at time k+1, denotes a covariance matrix of the estimation error of the UAV motion state at time k, is a covariance matrix of the state noise at time k, denotes a linearized approximation state transition matrix at time k, denotes a linearized approximation noise matrix at time k ; Step 53: calculating the Kalman gain of the inter-cluster unmanned aerial vehicle state estimation system to obtain the Kalman gain matrix of the inter-cluster unmanned aerial vehicle state estimation system wherein is a Kalman gain matrix of the inter-vehicle state estimation system at time k+1, is a covariance matrix of the observation noise at time k+1. Step 54: calculate the UAV motion state estimation value, and obtain the UAV motion state estimation value at the k+1 moment ; More specifically, the unmanned aerial vehicle motion state prediction value The observation equation of the inter-cluster unmanned aerial vehicle inter-aircraft state estimation system is introduced, and the unmanned aerial vehicle position information obtained by the cluster unmanned aerial vehicle at time k+1 is used The estimated residual error at time k+1 is obtained by subtracting it , further utilize the residual to obtain the UAV motion state estimation value at time k+1 ; Step 55: Calculate the covariance of the UAV motion estimation error, and obtain the covariance matrix of the UAV motion estimation error at time k+1 where I represents a unit matrix.

7. A device for estimating the inter-machine state of a cluster of unmanned aerial vehicles (UAVs) under communication constraints, characterized in that, The device is used to realize the inter-cluster unmanned aerial vehicle state estimation method under communication restriction according to claim 1.

Images