Method and device for estimating inter-machine state of cluster unmanned aerial vehicles under communication limitation

Abstract

The invention provides an inter-machine state estimation method and device for cluster unmanned aerial vehicles under communication limitation. The method comprises the following steps: 1, establishing a first motion equation of the cluster unmanned aerial vehicles under the communication limitation; 2, obtaining epsilon k according to the change of the motion speed vk of the unmanned aerial vehicle, obtaining eta k according to the change of the track azimuth angle change rate omega k, obtaining state noise according to epsilon k and eta k, and introducing the state noise into the first motion equation to obtain a second motion equation; 3, obtaining a state equation of a cluster unmanned aerial vehicle inter-machine state estimation system according to the second motion equation; 4, establishing an observation equation of a state estimation system among the cluster unmanned aerial vehicles according to the unmanned aerial vehicle position information acquired among the cluster unmanned aerial vehicles; 5, estimating the motion state of the unmanned aerial vehicle according to extended Kalman filtering by using a state equation and an observation equation of a cluster unmanned aerial vehicle inter-machine state estimation system to obtain an estimated value # imgabs0 # of the motion state of the unmanned aerial vehicle at the (k + 1) moment; 6, estimating the motion state of the unmanned aerial vehicle to obtain an estimated value # imgabs1 # of the motion state of the unmanned aerial vehicle at the (k + 1) moment;

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle clusters, and in particular relates to a method and device for estimating the inter-machine state of clustered unmanned aerial vehicles under limited communication. Technical Background

[0002] With the development of drone swarm technology, drone swarm countermeasure technology is also constantly improving. Usually, electromagnetic interference is created to restrict the communication of drone swarms, making it impossible for drone swarms to fly stably. There are usually two situations for drone swarm communication restrictions: on the one hand, communication restrictions will affect the communication bandwidth of cluster drones, that is, all status information of cluster drones may not be fully transmitted. On the other hand, communication restrictions will lead to data loss between cluster drones, and intermittent communication interruptions will cause the information between drones to be unable to be updated at a fixed cycle. In order to ensure the stability of cluster drones under limited communication conditions, in the first case, it is necessary to reduce the communication content of cluster drones to reduce the communication bandwidth between drones; in the second case, when data is lost, it is necessary to rely on the original information of the drone to predict its motion status information in real time.

[0003] Generally speaking, position information is essential in the information between swarm drones, and information such as movement speed and direction can be obtained by differentiating the position information. However, the global navigation satellite system (GNSS) position information of the drone will generate large noise during its movement, and its differentiation will further cause large noise in terms of speed. In addition, when data is lost, it is difficult to predict the state of the drone. Therefore, a method with better performance is needed. Summary of the invention

[0004] The purpose of the present invention is to provide a method for estimating the inter-machine state of cluster UAVs under communication restriction, which solves the technical problems of bandwidth reduction and data loss of cluster UAVs under communication restriction conditions.

[0005] In a first aspect, the present application provides a method for estimating states between drones in a cluster under limited communication, the method comprising:

[0006] Step 1: Establish the first motion equation of the swarm UAV under communication constraints;

[0007] Step 2: According to the drone's movement speed v k The change of ε k , according to the track azimuth change rate ω k The change of η k , according to ε k and η k The state noise is obtained, and the state noise is introduced into the first motion equation to obtain the second motion equation;

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

[0009] Step 4: According to the UAV position information obtained between the cluster UAVs, the observation equation of the state estimation system between the cluster UAVs is established;

[0010] Step 5: According to the extended Kalman filter, the state equation and observation equation of the inter-drone state estimation system of the cluster drone are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1

[0011] Step 6: When the UAV position information obtained by the cluster UAVs in the observation equation at time k+1 is lost, the state equation and observation equation of the cluster UAV state estimation system are used to estimate the UAV motion state and obtain the estimated value of the UAV motion state at time k+1.

[0012] Furthermore, the first equation of motion is:

[0013]

[0014] The sampling time T is the time between time k+1 and time k, and the sampling time T is fixed; v k is the speed of the drone at time k, v k+1 is the speed of the drone at time k+1, ω k is the speed of the drone at time k, ω k+1 is the speed of the drone at time k+1, (x k ,y k ) is the spatial position of the UAV at time k, (x k+1 ,y k+1 ) is the spatial position of the UAV at time k+1, γ k is the azimuth of the UAV track at time k, γ k+1 is the azimuth of the UAV track at time k+1, and the UAV motion speed v k Constant, track azimuth change rate ω k Constant.

[0015] Furthermore, the second motion equation is:

[0016]

[0017] Furthermore, the state equation of the state estimation system between cluster drones is:

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

[0019] Among them, the state of the inter-machine state estimation system of the cluster UAV at time k is expressed as X k =[x k ,y k ,v k ,γ k ,ω k ] T At time k+1, the state of the inter-machine state estimation system of the cluster UAV is expressed as X k+1 =[x k+1 ,y k+1 ,v k+1 ,γ k+1 ,ω k+1 ] T , the state noise of the inter-machine state estimation system of the cluster UAV at time k is expressed as W k =[ε k ,η k ] T .

[0020] Furthermore, the observation equation of the state estimation system between cluster drones is:

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

[0022] Among them, Z k+1 =[x k+1 ,y k+1 ] T represents the drone location information obtained between cluster drones at time k+1, ν k+1 represents the observation noise at time k+1, and the observation matrix is ​​expressed as

[0023] Furthermore, step 5 includes:

[0024] Step 51: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state. in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0025] Step 52: 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) T, where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k;

[0026] Step 53: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix K(k+1)=P(k+1|k)H of the unmanned aerial vehicle inter-machine state estimation system T [HP(k+1|k)H T +R(k+1)] -1 , where K(k+1) is the Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of

[0027] Step 54: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1

[0028] More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1

[0029] Step 55: Calculate the covariance of the drone motion estimation error to obtain the covariance matrix of the drone motion estimation error at time k+1, P(k+1)=[IK(k+1)H]P(k+1|k), where I represents the unit matrix.

[0030] Furthermore, step 54 specifically includes:

[0031] The predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1

[0032] Furthermore, step 6 includes:

[0033] Step 61: Substitute the observation noise ν k+1Set to 0 and substitute the state estimate of the state equation into the observation equation Obtain the drone position information Z′(k+1) when the drone position information is lost at time k+1;

[0034] Step 62: Substitute the drone position information when the drone position information is lost into step 5, that is, the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between the cluster drones at time k+1;

[0035] Step 63: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0036] 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k;

[0037] Step 65: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix of the unmanned aerial vehicle inter-machine state estimation system 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 Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of

[0038] Step 66: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1

[0039] Step 67: 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)=[IK(k+1)H]P(k+1|k), where I represents the identity matrix;

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

[0041] Furthermore, step 66 specifically includes:

[0042] The predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 Since the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between cluster drones at time k+1, the residual is 0 at this time.

[0043] In a second aspect, the present application provides a device for estimating the inter-machine state of clustered unmanned aerial vehicles under limited communication, and the device is used to implement the above-mentioned method for estimating the inter-machine state of clustered unmanned aerial vehicles under limited communication.

[0044] In summary, the present application provides a method and device for inter-machine state estimation of clustered drones under restricted communication. By establishing the state equation of the inter-machine state estimation system of clustered drones and the observation equation of the inter-machine state estimation system of clustered drones, a state estimation strategy based on the extended Kalman filter (EKF) is further designed. It can estimate the motion state information by relying only on the position information, and has a good processing effect on the information loss situation. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a flow chart of a method for estimating the state between cluster drones under limited communication in this patent;

[0046] Figure 2 The flight track of the drone estimated in the implementation example of this patent;

[0047] Figure 3 This is the speed estimation effect in the implementation example of this patent;

[0048] Figure 4 This is the track azimuth estimation effect in the implementation example of this patent;

[0049] Figure 5 This is the speed estimation effect when data is lost in the implementation example of this patent;

[0050] Figure 6 This is the effect of track azimuth estimation when data is lost in the implementation example of this patent. DETAILED DESCRIPTION

[0051] Embodiment 1

[0052] like Figure 1 As shown, the present application provides a method for estimating the state between cluster drones under limited communication, comprising:

[0053] Step 1: Establish the first motion equation of the cluster UAV under communication constraints:

[0054]

[0055] The sampling time T is the time between time k+1 and time k, and the sampling time T is fixed; v k is the speed of the drone at time k, v k+1 is the speed of the drone at time k+1, ω k is the speed of the drone at time k, ω k+1 is the speed of the drone at time k+1, (x k ,y k ) is the spatial position of the UAV at time k, (x k+1 ,y k+1 ) is the spatial position of the UAV at time k+1, γ k is the azimuth of the UAV track at time k, γ k+1 is the azimuth of the UAV track at time k+1, and the UAV motion speed v k Constant, track azimuth change rate ω k Constant.

[0056] Step 2: According to the drone's movement speed v k The change of ε k , according to the track azimuth change rate ω k The change of η k , according to ε k and η k The state noise is obtained, and the state noise is introduced into the first motion equation to obtain the second motion equation:

[0057]

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

[0059] Step 3: Obtain the state equation of the inter-drone state estimation system of the cluster according to the second motion equation:

[0060]

[0061] Among them, the state of the inter-machine state estimation system of the cluster UAV at time k is expressed as X k =[xk ,y k ,v k ,γ k ,ω k ] T At time k+1, the state of the inter-machine state estimation system of the cluster UAV is expressed as X k+1 =[x k+1 ,y k+1 ,v k+1 ,γ k+1 ,ω k+1 ] T , the state noise of the inter-machine state estimation system of the cluster UAV at time k is expressed as W k =[ε k ,η k ] T .

[0062] Step 4: According to the drone position information obtained between the cluster drones, the observation equation of the cluster drone state estimation system is established:

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

[0064] Among them, Z k+1 =[x k+1 ,y k+1 ] T represents the drone location information obtained between cluster drones at time k+1, ν k+1 represents the observation noise at time k+1, and the observation matrix is ​​expressed as

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

[0066] Step 5: According to the extended Kalman filter, the state equation and observation equation of the inter-drone state estimation system of the cluster drone are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1

[0067] Specifically, step 5 includes:

[0068] Step 51: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state. in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0069] More specifically, assume that at time k, the state noise W of the state estimation system between cluster drones isk = 0, the estimated motion state of the UAV at time k Substitute the second motion equation to obtain the state equation of the inter-drone state estimation system of the swarm drones, that is, obtain the predicted value of the drone motion state;

[0070] Step 52: 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k;

[0071] More specifically, according to the EKF filtering process, the linearized approximate state transfer matrix and noise matrix can be obtained by obtaining the partial derivative of the state equation of the inter-machine state estimation system of the cluster UAV by the second motion equation.

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

[0073] Step 54: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1

[0074] More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1

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

[0076] Step 6: When the drone position information obtained between the cluster drones of the observation equation at time k+1 is lost, that is, the drone position information Z(k+1) data obtained between the cluster drones at time k+1 is lost, the state equation and observation equation of the state estimation system between the cluster drones are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1

[0077] Step 6 includes:

[0078] Step 61: Substitute the observation noise ν k+1 Set to 0 and substitute the state estimate of the state equation into the observation equation Obtain the drone position information Z′(k+1) when the drone position information is lost at time k+1;

[0079] Step 62: Substitute the drone position information when the drone position information is lost into step 5, that is, the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between the cluster drones at time k+1;

[0080] Step 63: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0081] More specifically, assume that at time k, the state noise W of the state estimation system between cluster drones is k = 0, the estimated motion state of the UAV at time k Substitute the second motion equation into the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state.

[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) T, where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer 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 transfer matrix and noise matrix can be obtained by obtaining the partial derivative of the state equation of the inter-machine state estimation system of the cluster UAV by the second motion equation.

[0084] Step 65: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix of the unmanned aerial vehicle inter-machine state estimation system 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 Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of

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

[0086] More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 Since the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between cluster drones at time k+1, the residual is 0 at this time.

[0087] Step 67: 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)=[IK(k+1)H]P(k+1|k), where I represents the identity matrix;

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

[0089] Embodiment 2

[0090] This embodiment of the application takes a typical fixed-wing UAV formation flight as an example to provide a method for estimating the inter-machine state of a cluster UAV under limited communication, including:

[0091] In the simulation test of a drone in the swarm of drones, the flight speed is set to 20 meters per second. The movement includes uniform linear motion and fixed angular velocity turning. Gaussian white noise (mean is 0, variance is 1) is added to the real-time position of the drone. The flight effect is as follows Figure 2 shown.

[0092] The following steps are used to achieve inter-drone state estimation in clustered UAVs under limited communication, including:

[0093] Step 1: Establish the first motion equation of the cluster UAV under communication constraints:

[0094]

[0095] The sampling time T is the time between time k+1 and time k, and the sampling time T is fixed; v k is the speed of the drone at time k, v k+1 is the speed of the drone at time k+1, ω k is the speed of the drone at time k, ω k+1 is the speed of the drone at time k+1, (x k ,y k ) is the spatial position of the UAV at time k, (x k+1 ,y k+1 ) is the spatial position of the UAV at time k+1, γ k is the azimuth of the UAV track at time k, γ k+1 is the azimuth of the UAV track at time k+1, and the UAV motion speed v k Constant, track azimuth change rate ω k Constant.

[0096] Step 2: According to the drone's movement speed v k The change of ε k , according to the track azimuth change rate ω k The change of η k , according to ε k and η k The state noise is obtained, and the state noise is introduced into the first motion equation to obtain the second motion equation:

[0097]

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

[0099] Step 3: Obtain the state equation of the inter-drone state estimation system of the cluster according to the second motion equation:

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

[0101] Among them, the state of the inter-machine state estimation system of the cluster UAV at time k is expressed as X k =[x k ,y k ,v k ,γ k ,ω k ] T At time k+1, the state of the inter-machine state estimation system of the cluster UAV is expressed as X k+1 =[x k+1 ,y k+1 ,v k+1 ,γ k+1 ,ω k+1 ] T , the state noise of the inter-machine state estimation system of the cluster UAV at time k is expressed as W k =[ε k ,η k ] T .

[0102] Step 4: According to the drone position information obtained between the cluster drones, the observation equation of the cluster drone state estimation system is established:

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

[0104] Among them, Z k+1 =[x k+1 ,y k+1 ] T represents the drone location information obtained between cluster drones at time k+1, ν k+1 represents the observation noise at time k+1, and the observation matrix is ​​expressed as

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

[0106] Step 5: According to the extended Kalman filter, the state equation and observation equation of the inter-drone state estimation system of the cluster drone are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1

[0107] Specifically, step 5 includes:

[0108] Step 51: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state. in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0109] More specifically, assume that at time k, the state noise W of the state estimation system between cluster drones is k = 0, the estimated motion state of the UAV at time k Substitute the second motion equation to obtain the state equation of the inter-drone state estimation system of the swarm drones, that is, obtain the predicted value of the drone motion state;

[0110] Step 52: 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k;

[0111] More specifically, according to the EKF filtering process, the linearized approximate state transfer matrix and noise matrix can be obtained by obtaining the partial derivative of the state equation of the inter-machine state estimation system of the cluster UAV by the second motion equation.

[0112] Step 53: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix K(k+1)=P(k+1|k)H of the unmanned aerial vehicle inter-machine state estimation system T [HP(k+1|k)H T +R(k+1)] -1 , where K(k+1) is the Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of

[0113] Step 54: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1

[0114] More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1

[0115] Step 55: Calculate the covariance of the drone motion estimation error to obtain the covariance matrix of the drone motion estimation error at time k+1: P(k+1)=[IK(k+1)H]P(k+1|k), where I represents the unit matrix;

[0116] Step 6: When the drone position information obtained between the cluster drones of the observation equation at time k+1 is lost, that is, the drone position information Z(k+1) data obtained between the cluster drones at time k+1 is lost, the state equation and observation equation of the state estimation system between the cluster drones are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1

[0117] Step 6 includes:

[0118] Step 61: Substitute the observation noise ν k+1 Set to 0 and substitute the state estimate of the state equation into the observation equation Obtain the drone position information Z′(k+1) when the drone position information is lost at time k+1;

[0119] Step 62: Substitute the drone position information when the drone position information is lost into step 5, that is, the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between the cluster drones at time k+1;

[0120] Step 63: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1;

[0121] More specifically, assume that at time k, the state noise W of the state estimation system between cluster drones is k = 0, the estimated motion state of the UAV at time k Substitute the second motion equation to obtain the state equation of the inter-drone state estimation system of the swarm drones, that is, obtain the predicted value of the drone motion state;

[0122] 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k;

[0123] More specifically, according to the EKF filtering process, the linearized approximate state transfer matrix and noise matrix can be obtained by obtaining the partial derivative of the state equation of the inter-machine state estimation system of the cluster UAV by the second motion equation.

[0124] Step 65: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix of the unmanned aerial vehicle inter-machine state estimation system 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 Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of

[0125] Step 66: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1

[0126] More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 Since the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between cluster drones at time k+1, the residual is 0 at this time;

[0127] Step 67: 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)=[IK(k+1)H]P(k+1|k), where I represents the identity matrix;

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

[0129] By using the inter-machine state estimation method of clustered drones under limited communication, the estimated error of drone motion speed and track azimuth when there is no data loss is as follows: Figure 3 and Figure 4 As shown in the figure, the proposed method estimates the UAV motion speed within an error of 0.15 m / s and the UAV track azimuth within an error of 4.5 degrees, which meets the requirements of cluster UAV formation flight.

[0130] By using the inter-machine state estimation method of clustered drones under communication constraints, when data loss occurs (the data loss is set to 10 seconds to 60 seconds: 3 seconds of data are randomly lost every 10 seconds), the obtained drone motion speed and track azimuth errors are as follows: Figure 5 and Figure 6 As shown, data loss occurred at 15 seconds to 18 seconds, 25 seconds to 28 seconds, 35 seconds to 38 seconds, 45 seconds to 48 seconds, and 55 seconds to 58 seconds. The error of the proposed method in estimating the UAV motion speed is within 0.7 m / s, and the error in estimating the UAV track azimuth is within 6.5 degrees, which meets the requirements for cluster UAV formation flight.

Claims

1. A method for estimating the state of a cluster of unmanned aerial vehicles under limited communication, characterized in that: Methods include: Step 1: Establish the first motion equation of the swarm UAV under communication constraints; Step 2: According to the drone's movement speed v k The change of ε k , according to the track azimuth change rate ω k The change of η k , according to ε k and η k The state noise is obtained, and the state noise is introduced into the first motion equation to obtain the second motion equation; Step 3: Obtain the state equation of the inter-UAV state estimation system of the cluster according to the second motion equation; Step 4: According to the UAV position information obtained between the cluster UAVs, the observation equation of the state estimation system between the cluster UAVs is established; Step 5: According to the extended Kalman filter, the state equation and observation equation of the inter-drone state estimation system of the cluster drone are used to estimate the drone motion state and obtain the estimated value of the drone motion state at time k+1 Step 6: When the UAV position information obtained by the cluster UAVs in the observation equation at time k+1 is lost, the state equation and observation equation of the cluster UAV state estimation system are used to estimate the UAV motion state and obtain the estimated value of the UAV motion state at time k+1.

2. The method according to claim 1, characterized in that The first equation of motion is: The sampling time T is the time between time k+1 and time k, and the sampling time T is fixed; v k is the speed of the drone at time k, v k+1 is the speed of the drone at time k+1, ω k is the speed of the drone at time k, ω k+1 is the speed of the drone at time k+1, (x k ,y k ) is the spatial position of the UAV at time k, (x k+1 ,y k+1 ) is the spatial position of the UAV at time k+1, γ k is the azimuth of the UAV track at time k, γ k+1 is the azimuth of the UAV track at time k+1, and the UAV motion speed v k Constant, track azimuth change rate ω k Constant.

3. The method according to claim 2, characterized in that The second equation of motion is:

4. The method according to claim 3, characterized in that The state equation of the inter-machine state estimation system of the swarm drone is: X k+1 =f(X k ,W k ) Among them, the state of the inter-machine state estimation system of the cluster UAV at time k is expressed as X k =[x k ,y k ,v k ,γ k ,ω k ] T At time k+1, the state of the inter-machine state estimation system of the cluster UAV is expressed as X k+1 =[x k+1 ,y k+1 ,v k+1 ,γ k+1 ,ω k+1 ] T , the state noise of the inter-machine state estimation system of the cluster UAV at time k is expressed as W k =[ε k ,η k ] T .

5. The method according to claim 4, characterized in that Observation equation of the state estimation system between cluster drones: Z k+1 =HX k+1 +ν k+1 Among them, Z k+1 =[x k+1 ,y k+1 ] T represents the drone location information obtained between cluster drones at time k+1, ν k+1 represents the observation noise at time k+1, and the observation matrix is ​​expressed as 6. The method according to claim 5, characterized in that Step 5 includes: Step 51: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state. in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1; Step 52: 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k; Step 53: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix K(k+1)=P(k+1|k)H of the unmanned aerial vehicle inter-machine state estimation system T [HP(k+1|k)H T +R(k+1)] -1 , where K(k+1) is the Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of Step 54: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1 More specifically, the predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 Step 55: Calculate the covariance of the drone motion estimation error to obtain the covariance matrix of the drone motion estimation error at time k+1, P(k+1)=[IK(k+1)H]P(k+1|k), where I represents the unit matrix.

7. The method according to claim 6, characterized in that Step 54 specifically includes: The predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 8. The method according to claim 1, characterized in that Step 6 includes: Step 61: Substitute the observation noise ν k+1 Set to 0 and substitute the state estimate of the state equation into the observation equation Obtain the drone position information Z′(k+1) when the drone position information is lost at time k+1; Step 62: Substitute the drone position information when the drone position information is lost into step 5, that is, the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between the cluster drones at time k+1; Step 63: Predict the state equation of the inter-drone state estimation system of the swarm drones to obtain the predicted value of the drone motion state in represents the estimated value of the UAV motion state at time k, represents the predicted value of the UAV motion state at time k+1; 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) T , where 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, and Q(k) is the state noise W at time k. k The covariance matrix of , F(k) represents the linearized approximate state transfer matrix at time k, and G(k) represents the linearized approximate noise matrix at time k; Step 65: Calculate the Kalman gain of the unmanned aerial vehicle inter-machine state estimation system to obtain the Kalman gain matrix of the unmanned aerial vehicle inter-machine state estimation system 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 Kalman gain matrix of the UAV inter-machine state estimation system at time k+1, and R(k+1) is the observation noise ν at time k+1 k+1 The covariance matrix of Step 66: Calculate the estimated value of the UAV motion state and obtain the estimated value of the UAV motion state at time k+1 Step 67: 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)=[IK(k+1)H]P(k+1|k), where I represents the identity matrix; Step 68: Complete the target state prediction under data loss.

9. The method according to claim 8, characterized in that Step 66 specifically includes: The predicted value of the UAV motion state Substitute the observation equation of the inter-drone state estimation system into the observation equation, and use the drone position information Z(k+1) obtained between the drones at time k+1 to obtain the estimated residual at time k+1. The residual is further used to obtain the estimated value of the UAV motion state at time k+1 Since the drone position information Z′(k+1) when the drone position information is lost at time k+1 replaces the drone position information Z(k+1) obtained between cluster drones at time k+1, the residual is 0 at this time.

10. A device for estimating the state of drones in a cluster under limited communication, characterized in that: The device is used to implement the method for estimating the state between clustered drones under communication constraints as described in claim 1.

Images