Multi-unmanned aerial vehicle cooperative pose estimation method based on federated Kalman filtering

Abstract

The invention provides a multi-unmanned aerial vehicle cooperative pose estimation method based on federated Kalman filtering, and the method comprises the steps: equipping UWB and IMU on the bodies of multiple unmanned aerial vehicles, carrying out the distance measurement and communication between UWBs between the unmanned aerial vehicles, carrying out the position initialization of an unknown UWB node in the space through each unmanned aerial vehicle, and obtaining the three-dimensional coordinates of the initialized UWB node; the unmanned aerial vehicle estimates the pose of the unmanned aerial vehicle and the UWB node position in real time based on unscented Kalman filtering, and sends the pose of the unmanned aerial vehicle and the UWB node position to the unmanned aerial vehicles communicating with each other; after the unmanned aerial vehicle receives the unmanned aerial vehicle pose and the UWB node position, a subfilter of a federated Kalman filter is constructed based on linear distance constraint; unifying UWB node positions based on federated Kalman filtering; and the multiple unmanned aerial vehicles re-execute the steps S2-S4 to start running until the flight is finished. The method further reduces the consumed manpower and material resources, achieves the estimation of the pose of the unmanned aerial vehicle and the position of the ultra-wideband anchor point in the environment, and does not need to measure the position of the ultra-wideband anchor point in advance.

Description

Technical Field

[0001] The present invention relates to the technical field of UAV pose estimation, and particularly relates to a multi-UAV collaborative pose estimation method based on federated Kalman filtering. Background Art

[0002] In inertial navigation theory, the UAV pose obtained by integrating the acceleration and angular velocity obtained by the IMU sensor on the UAV often has cumulative drift. In existing UAV pose estimation methods, data from multiple sensors are fused through filtering methods for more accurate pose estimation. The UWB technology can achieve centimeter-level high-precision positioning and is very useful in application scenarios that require precise positioning such as indoor navigation; the UWB system uses periodic pulses to send data, the pulse duration is very short, and the duty cycle is very low, so the power consumption can be very low. And the UWB sensor has a lower cost compared with sensors such as radar and cameras. Therefore, the prior art uses the fusion of IMU and UWB sensors to estimate the UAV pose.

[0003] Existing methods need to measure the positions of ultra-wideband anchors in the environment in advance, consuming a lot of manpower and material resources. And the existing UWB anchor initialization methods use the least squares positioning method or the optimization method to solve, which is easily affected by UWB ranging noise and requires the UAV to have a relatively rich trajectory. At the same time, in a multi-UAV system, the ultra-wideband anchor positions obtained after each filtering are not unified, resulting in a low estimation accuracy of the ultra-wideband anchor positions. The federated Kalman filtering can unify the ultra-wideband position estimations of multiple sub-filters, but the distance constraint between the UWB nodes and the UAVs is ignored during the unification process, resulting in a too slow convergence speed of the unknown UWB nodes and a large influence of the ultra-wideband ranging noise on the position estimation accuracy.

[0004] Therefore, the current multi-UAV collaborative pose estimation methods all have unsatisfactory situations. Summary of the Invention

[0005] Aiming at the problems existing in the prior art, the present invention provides a multi-UAV collaborative pose estimation method based on federated Kalman filtering to solve the technical problems that the existing multi-UAV collaborative pose estimation methods are all unsatisfactory.

[0006] The present invention provides a multi-UAV collaborative pose estimation method based on federated Kalman filtering, including:

[0007] S1. Equip UWB and IMU on the fuselage of multiple UAVs. The UWBs between each UAV measure distances and communicate with each other, and each UAV initializes the positions of unknown UWB nodes in space to obtain the three-dimensional coordinates of the initialized UWB nodes;

[0008] S2. The drone estimates the pose of the drone and the positions of UWB nodes in real time based on the unscented Kalman filter, and sends the pose of the drone and the positions of UWB nodes to the drones communicating with each other;

[0009] S3. After receiving the pose of the drone and the positions of UWB nodes, the drone constructs sub-filters of the federated Kalman filter based on the linear distance constraint;

[0010] S4. Unify the positions of UWB nodes based on the federated Kalman filter;

[0011] S5. The multi-drones re-execute steps S2 - S4 to start running until the flight ends.

[0012] Optionally, each drone initializes the positions of unknown UWB nodes in space to obtain the three-dimensional coordinates of the initialized UWB nodes, including:

[0013] S101. Taking the initial take-off point of the drone as the center of the sphere, initialize a sphere with the first distance measurement as the radius of the sphere, and perform spherical sampling on the sphere. Specifically:

[0014] Define the golden ratio φ as:

[0015]

[0016] For n spherical sampling points, calculate the angle θ and the z coordinate of the spherical sampling points:

[0017] θ i = 2πφi, i = 0, 1, …, n - 1

[0018]

[0019] where i is the serial number of the spherical sampling point, and calculate the x i；UWB and y i；UWB coordinates:

[0020]

[0021] x i；UWB = r i cos(θ i )

[0022] y i；UWB = r i sin(θ i )

[0023] Represent the spherical sampling points as (x i；UWB , y i；UWB , z i；UWB );

[0024] S102, Initialize a sphere with d1 as the center and d2 as the radius, and use uniform sampling to obtain n uniform spherical sampling points. For each point:

[0025]

[0026] Find S u =min(S1,…,S n ), S u The corresponding [x u ,y u ,z u ] is the initialized three-dimensional coordinate of the UWB node.

[0027] Optionally, the drone estimates the drone posture and UWB node position in real time based on unscented Kalman filtering, including:

[0028] S201, the state vector of the drone is expressed as:

[0029]

[0030] in, is the three-dimensional position coordinate of the UAV in the world coordinate system; is the three-dimensional coordinate of the unknown UWB node; is the attitude of the drone in the world coordinate system, expressed as quaternion represents the three-dimensional velocity in the world coordinate system, b a and b g denote the bias terms of the accelerometer and gyroscope respectively;

[0031] S202, using the acceleration information of the accelerometer to update the position and speed of the drone, which are respectively expressed as:

[0032]

[0033] Where k represents the sampling time, Δt represents the sampling interval, is the acceleration information in the world coordinate system, and the acceleration information Coordinate transformation is performed, expressed as:

[0034]

[0035] Among them, n a represents the random noise of the accelerometer, g represents the acceleration due to gravity, It is the rotation matrix from B system to W system, through the quaternion q = [q0, q1, q2, q3] TConvert the attitude of the drone from the body coordinate system to the global coordinate system:

[0036]

[0037] S203. Calculate the angular velocity measured by the IMU to calculate the attitude of the drone at time k+1

[0038]

[0039] where n g represents the random noise of the gyroscope, thus describing the state update process of the drone as:

[0040] X k+1 = f(X k ) + w k

[0041] where the expression of f(X k ) is as follows:

[0042]

[0043] where w k represents the process noise, which follows a Gaussian distribution:

[0044]

[0045] Optionally, each drone estimates the pose of the drone and the position of the UWB node in real time based on the unscented Kalman filter, and further includes:

[0046] S204. Represent the distance between the UWB of the drone and the unknown UWB node as:

[0047]

[0048] and represent the observation equation of the drone as:

[0049] Z k+1 = h(X k+1 ) + v k+1

[0050] where h(X k+1 ) = d k+1 , v k+1 is the measurement noise, R k+1 represents the variance of the measurement noise, r k+1 represents the expectation of the measurement noise, and its statistical characteristics are described as:

[0051]

[0052] Optionally, each of the drones estimates the pose of the drone and the position of the UWB node in real time based on the unscented Kalman filter, and further includes:

[0053] S205. Initialize the state estimation vector and the state covariance matrix P0. Based on the state estimation vector and the state covariance matrix P0 at the previous time step, select the Sigma points, which are expressed as:

[0054]

[0055] And calculate the sampling point weights:

[0056]

[0057] where n is the dimension of the state vector, λ = α 2 (n + κ) - n is the fitness parameter, and S p,k represents the Cholesky decomposition of P k ;

[0058] S206. Substitute the Sigma points into the state transition equation of the nonlinear propagation to calculate the next prediction of the state vector and the covariance P k+1|k :

[0059]

[0060] Select new Sigma points according to the state prediction vector , and the distribution of the Sigma points depends on the one-step prediction value of P k+1|k :

[0061]

[0062] where S p,k+1|k represents the Cholesky decomposition of the one-step prediction of P k+1|k , and propagate the Sigma points through the nonlinear measurement equation Then the cross-covariance is expressed as:

[0063]

[0064] Calculate the Kalman gain Update the state prediction

[0065]

[0066] Update P k+1 as follows:

[0067]

[0068] Optionally, the sub-filter for constructing a federated Kalman filter based on linear distance constraints includes:

[0069] S301. Construct linear constraints on the positions of UWB nodes and the positions in the poses of unmanned aerial vehicles:

[0070] Ax = b,

[0071]

[0072] where represents the positions of UWB nodes, r1, r2, r3, r4 are the measured distances of any four unmanned aerial vehicles among multiple unmanned aerial vehicles, and the positions of the four unmanned aerial vehicles are respectively

[0073] S302. Construct an optimization problem to unify the positions of i UWB nodes

[0074]

[0075] where and represent the linear constraints corresponding to the i-th UWB node at time k, represents the three-dimensional position of the i-th UWB node estimated by the j-th unmanned aerial vehicle at time k, represents the corresponding posterior covariance, is the preliminary estimation result obtained by the unscented Kalman filter at time k;

[0076] S303. Add the linear constraints to the optimization problem for solution:

[0077]

[0078] ρ is the weight of the linear constraint, and the closed-form solution of the optimization problem is expressed as:

[0079]

[0080] The inverse of the Hessian matrix of the optimization problem is added to the federated Kalman filter as the covariance of the closed-form solution, expressed as:

[0081]

[0082] Optionally, the unification of UWB node positions based on the federated Kalman filter includes:

[0083] S401. The UWB node positions obtained in step S2 and the corresponding covariance Pi,k Input to the federated Kalman filter;

[0084] S402. Perform observation update on each sub-filter in the federated Kalman filter based on step S2;

[0085] S403. Perform information fusion on the main filter in the federated Kalman filter, expressed as:

[0086]

[0087] S404. Allocate the information of the main filter and each sub-filter in the federated Kalman filter, expressed as:

[0088]

[0089] where the information factor satisfies the following constraints:

[0090]

[0091] where V is the number of sub-filters in the federated Kalman filter.

[0092] Compared with the prior art, the present invention:

[0093] After unifying the UWB node positions with the federated Kalman filter, the present invention increases the estimation accuracy of the UWB node positions and the UAV poses compared with the unscented Kalman filter. After adding the sub-filter composed of the linear distance constraint, the solution accuracy is further increased, and the human and material resources consumed are further reduced. At the same time, the estimation of the UAV pose and the positions of the ultra-wideband anchors in the environment is realized, and the positions of the ultra-wideband anchors do not need to be measured in advance. BRIEF DESCRIPTION OF THE DRAWINGS

[0094] The accompanying drawings herein are incorporated into the specification and form a part of the specification, showing embodiments consistent with the present invention and used together with the specification to explain the principles of the present invention.

[0095] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required for the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0096] Figure 1 is a schematic flowchart of the method of the present invention;

[0097] Figure 2 is a schematic structural flowchart of the present invention;

[0098] Figure 3Schematic diagram of initializing an unknown UWB anchor in another embodiment of the present invention;

[0099] Figure 4 Schematic diagram of the overall implementation of the algorithm in another embodiment of the present invention (including four drones and four unknown UWB nodes);

[0100] Figure 5 Schematic diagram of the position error of an unknown UWB anchor in another embodiment of the present invention;

[0101] Figure 6 Schematic diagram of the comparison between the unscented Kalman filtering method derived from Drone 1 and the angle error without filtering in another embodiment of the present invention;

[0102] Figure 7 Schematic diagram of the comparison between the unscented Kalman filtering method derived from Drone 2 and the angle error without filtering in another embodiment of the present invention;

[0103] Figure 8 Schematic diagram of the comparison between the unscented Kalman filtering method derived from Drone 3 and the angle error without filtering in another embodiment of the present invention;

[0104] Figure 9 Schematic diagram of the comparison between the unscented Kalman filtering method derived from Drone 4 and the angle error without filtering in another embodiment of the present invention;

[0105] Figure 10 Schematic diagram of the comparison of the position error of Drone 1 in another embodiment of the present invention;

[0106] Figure 11 Schematic diagram of the comparison of the position error of Drone 2 in another embodiment of the present invention;

[0107] Figure 12 Schematic diagram of the comparison of the position error of Drone 3 in another embodiment of the present invention;

[0108] Figure 13 Schematic diagram of the comparison of the position error of Drone 4 in another embodiment of the present invention. Detailed implementation manners

[0109] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions in the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are some but not all of the embodiments of this application. All other implementation cases obtained by those of ordinary skill in the art based on the embodiments in this application without creative efforts fall within the scope of protection of this application. In the examples of the present invention, functional units with the same reference numerals have the same and similar structures and functions.

[0110] SeeFigure 1 and Figure 2 , the present invention provides a multi-UAV collaborative pose estimation method based on federated Kalman filtering, including:

[0111] S1. Equip UWB and IMU on the fuselage of multiple UAVs. The UWB between each UAV measures distance and communicates with each other. Each UAV initializes the position of unknown UWB nodes in space to obtain the three-dimensional coordinates of the initialized UWB nodes;

[0112] S2. The UAVs use the unscented Kalman filter to estimate the pose of the UAVs and the positions of the UWB nodes in real time, and send the UAV poses and UWB node positions to the UAVs that communicate with each other;

[0113] S3. After receiving the UAV poses and UWB node positions, the UAVs construct sub-filters of the federated Kalman filter based on linear distance constraints;

[0114] S4. Unify the positions of the UWB nodes based on federated Kalman filtering;

[0115] S5. The multiple UAVs re-execute steps S2 - S4 and start running until the flight ends.

[0116] In this embodiment, S1. Equip UWB and IMU on the fuselage of multiple UAVs. The UWB between each UAV measures distance and communicates with each other. Each UAV initializes the position of unknown UWB nodes in space to obtain the three-dimensional coordinates of the initialized UWB nodes.

[0117] S101. Take the initial take-off point of the UAV as the center of the sphere, and initialize a sphere with the first distance measurement as the radius of the sphere. Use "Fibonacci spherical sampling" to sample the sphere. Specifically:

[0118] Theoretically, the UWB anchors in the environment should be on the sphere. Therefore, using the point distribution technology of the golden ratio (Phi), define the golden ratio φ as:

[0119]

[0120] For n spherical sampling points, calculate the angle θ and the z coordinate of the spherical sampling points:

[0121] θ i = 2πφi, i = 0, 1, …, n - 1

[0122]

[0123] where i is the serial number of the spherical sampling point, and calculate the x i；UWB and y i；UWB coordinates:

[0124]

[0125] x i；UWB =r i cos(θ i )

[0126] y i；UWB =r i sin(θ i )

[0127] The spherical collection point is represented as (x i；UWB ,y i；UWB ,z i；UWB ), in actual use, if you need to scale these points to different radii, you can multiply them by the corresponding radius value R:

[0128] (x i；UWB ',y i；UWB ',z i；UWB ')=R·(x i；UWB ,y i；UWB ,z i；UWB )

[0129] S102, find the initialization point, is the three-dimensional position coordinate of the UAV in the world coordinate system; d k represents the kth UWB distance measurement value;

[0130] by Initialize a sphere with d1 as the center and d2 as the radius, and use uniform sampling to obtain n uniform spherical sampling points. For each point:

[0131]

[0132] Find S u =min(S1,…,S n ), S u The corresponding [x u ,y u ,z u ] is the initialized three-dimensional coordinate of the UWB node.

[0133] S2. The UAV estimates the UAV posture and UWB node position in real time based on the unscented Kalman filter, and sends the UAV posture and UWB node position to the UAVs communicating with each other.

[0134] S201. First, the state vector of the UAV system is expressed as:

[0135]

[0136] in, is the three-dimensional position coordinates of the UAV in the world coordinate system; are the three-dimensional coordinates of the unknown UWB node; is the attitude of the UAV in the world coordinate system, represented by quaternion to avoid gimbal lock as represents the three-dimensional velocity in the world coordinate system, b a and b g respectively represent the bias terms of the accelerometer and the gyroscope;

[0137] For the unknown UWB node, the position should not change with time, that is:

[0138]

[0139] S202. Update the pose and velocity of the UAV using the acceleration information of the accelerometer, which are respectively expressed as:

[0140]

[0141] where k represents the sampling time, and Δt represents the sampling interval, is the acceleration information in the world coordinate system,

[0142] and since the acceleration information directly obtained by the IMU of the UAV is in the UAV coordinate system therefore, convert the acceleration information for coordinate transformation, which is expressed as:

[0143]

[0144] where n a represents the random noise of the accelerometer, and g represents the gravitational acceleration, is the rotation matrix from the B system to the W system, and the attitude of the UAV is converted from the body coordinate system to the global coordinate system through the quaternion q = [q0, q1, q2, q3] T as follows:

[0145]

[0146] S203. Use the angular velocity measured by the IMU to calculate the attitude of the UAV at the k + 1 moment according to time integration

[0147]

[0148] where n g represents the random noise of the gyroscope, and thus the state update process of the UAV is described as:

[0149] X k+1 = f(X k ) + w k

[0150] where f(X k ) is expressed as follows:

[0151]

[0152] where, w k represents process noise, which follows a Gaussian distribution:

[0153]

[0154] S204. Then, the observation equation of the UAV system is represented. The distance between the UWB of the UAV and the unknown UWB node is expressed as:

[0155]

[0156] And the observation equation of the UAV is represented as:

[0157] Z k+1 = h(X k+1 ) + v k+1

[0158] where, h(X k+1 ) = d k+1 , v k+1 is measurement noise, R k+1 represents the variance of the measurement noise, r k+1 represents the expectation of the measurement noise, and its statistical characteristics can be described as:

[0159]

[0160] S205. The process of the unscented Kalman filter algorithm. The above nonlinear time-discrete system is as follows:

[0161] X k+1 = f(X k ) + w k

[0162] Z k+1 = h(X k+1 ) + v k+1

[0163] Initialize the state estimation vector and the state covariance matrix P0. After initialization, start the loop phase: Based on the state estimation vector and the state covariance matrix P0 of the previous time step, select the Sigma points, which are expressed as:

[0164]

[0165] And calculate the weights of the sampling points:

[0166]

[0167] where n is the dimension of the state vector, λ = α 2 (n + κ) - n is the fitness parameter, and S p,k represents the Cholesky decomposition of P k , which is used to reduce the positive definiteness of the sampling points. α controls the distribution of the sampling points. The distribution of the previous state quantity is integrated using β, and the value of κ should ensure the positive definiteness of P k , and generally takes the value of n - 3;

[0168] S206. Substitute the Sigma points into the state transition equation of the nonlinear propagation to calculate the next prediction of the state vector and the covariance P k+1|k :

[0169]

[0170] Select new Sigma points according to the state prediction vector , and the distribution of the Sigma points depends on the one-step prediction value of P k+1|k :

[0171]

[0172] where S p,k+1|k represents the Cholesky decomposition of the one-step prediction of P, and the Sigma points are propagated through the nonlinear measurement equation k+1|k Then the cross-covariance is expressed as: as:

[0173]

[0174] Calculate the Kalman gain Update the state prediction

[0175]

[0176] Update P k+1 as follows:

[0177]

[0178] S3. After the UAV receives the UAV pose and the UWB node positions, construct a sub-filter of the federated Kalman filter based on the linear distance constraint.

[0179] Since distance measurements are made between the UWB nodes and multiple drones, the Euclidean distance between the positions of the UWB nodes in the environment and the UWB nodes on the drones should be equal to the distance measurement value between the UWBs.

[0180] S301. Subtracting the above equations gives linear constraints. Assume that the positions of four drones at a certain moment are The measured distances are r1, r2, r3, r4. The positions of the UWB nodes and the drones should satisfy the following linear constraints:

[0181] Ax = b,

[0182]

[0183] where represents the position of the UWB node. After each filtering is completed, each drone estimates the positions of all unknown anchors. In fact, the positions of the same anchor estimated by each drone should be the same.

[0184] S302. Therefore, in addition to satisfying the linear constraints, the position of the UWB node also needs to be the same as the position estimates of the four drones. At time k, an optimization problem is constructed to unify the positions of i UWB nodes

[0185]

[0186] where and represent the linear constraints corresponding to the i-th UWB node at time k, represents the three-dimensional position of the i-th UWB node estimated by the j-th drone at time k, represents the corresponding posterior covariance, is the preliminary estimation result obtained by the unscented Kalman filter at time k

[0187] S303. Due to the ranging noise of the UWB, the position of the UWB node does not strictly satisfy the above linear constraints. Add the linear constraints to the optimization problem for solution:

[0188]

[0189] ρ is the weight of the linear constraint, and the closed-form solution of the optimization problem is expressed as:

[0190]

[0191] Take the second derivative of the above formula, that is, the Hessian matrix of the optimization problem. The inverse of the Hessian matrix of the optimization problem is added to the federated Kalman filter as the covariance of the closed-form solution, expressed as:

[0192]

[0193] S4. Unify the UWB node positions based on the federated Kalman filter.

[0194] S401. Time update. The time update steps of each sub-filter and the main filter are independent of each other, that is, each sub-filter performs an independent measurement update, and the UWB node positions and the corresponding covariance P i,k are input into the federated Kalman filter. In addition, the solution obtained after adding the linear constraint to the optimization problem is also added as a sub-filter to the federated Kalman filtering process;

[0195] S402. Observation update. Only the fusion of other sub-filters is completed in the main filter. Therefore, there is no observation input in the main filter and no observation update is performed. For each sub-filter, perform observation update on each sub-filter in the federated Kalman filter based on Step S2;

[0196] S403. Perform information fusion on the main filter in the federated Kalman filter, expressed as:

[0197]

[0198] S404. Allocate the information of the main filter and each sub-filter in the federated Kalman filter, expressed as:

[0199]

[0200] where the information factor satisfies the following constraints:

[0201]

[0202] where V is the number of sub-filters in the federated Kalman filter.

[0203] In this embodiment, in actual use, multiple drones fly in space, and the fuselage is equipped with UWB and IMU. The UWB between each drone can measure distances and communicate with each other. There are several UWB nodes with unknown positions in space, and each UWB node can measure distances with the UWB on the drones. Each drone flies a certain distance and runs the initialization algorithm in part S1. At this time, the pose of the drone is only obtained by integrating the IMU information.

[0204] For each drone, use the UWB ranging information and the IMU acceleration and angular velocity information to perform the UKF algorithm in part S2 to simultaneously estimate the UWB node positions in the environment and the pose of the drone.

[0205] Subsequently, the drone uses UWB to transmit the drone pose and UWB position estimated by the algorithm to all drones with UWB connections.

[0206] After each drone receives the transmitted drone pose and UWB position, it runs the algorithm in Part S3 to calculate the information of the sub-filter, and finally runs the algorithm in Part S4 to obtain the positions of new unknown UWB nodes.

[0207] Finally, it starts running from the algorithm in Part S2 again until the end of the entire flight process.

[0208] The present invention designs an unscented Kalman filtering method that fuses IMU and UWB information to realize the initialization and real-time update of the drone pose and the positions of UWB anchor nodes. Compared with the existing methods, it eliminates the need to measure the positions of UWB nodes in the unknown environment in advance, saving labor costs. Especially in the case of a short drone flight trajectory and a large noise environment, it has a high solution accuracy. It also uses the federated Kalman filter to unify the positions of UWB nodes, accelerating the convergence speed of UWB nodes. And the accurate UWB positions can increase the solution accuracy of the drone positions, thereby increasing the accuracy of the estimated positions of UWB nodes. Adding a sub-filter composed of linear distance constraints further increases the solution accuracy of the positions of UWB nodes and the drone pose.

[0209] In another embodiment, the projection of the local trajectory of a single drone on the XOY plane is an approximately elliptical trajectory, and it first ascends and then descends in the z-axis direction, as Figure 4 shown.

[0210] The IMU frequency is 100 hz. Assuming that there is UWB ranging at each position, the entire flight is completed within 30 s. The noises of the IMU acceleration and angular velocity are 0.05 m·s -2 and 0.05 rad·s -1 . The true positions of the UWB are [-6; -12; -2], [-6; 5; -2], [9; -12; 12], [9; 5; 12]. The initial positions of the four drones are: [0; 0; 0], [8; 0; 0], [0; -8; 0], [8; -8; 0]; the unit of the above positions is m. The weight ρ in the linear sub-filter is 0.04.

[0211] First, initialize the positions of the unknown UWB nodes in the environment. Each drone obtains 600 position points through IMU integration, and takes one point out of every four position points as the input for initialization. The trajectory of each drone is as Figure 3 shown. When initializing each UWB anchor node, 4000 points are evenly sampled on the sphere. Conduct 500 Monte Carlo experiments, and the initialization errors of the four UWB nodes are shown in the following table:

[0212] UWB Node Initialization Error (m) Node 1 1.2486 Node 2 0.3351 Node 3 1.0371 Node 4 0.5055

[0213] Run the algorithm after initialization. Since the results estimated by each UAV are the same after unification by the UWB nodes, there is only one such figure, as shown in Figure 5 to obtain the estimation error of the UWB nodes; the angular error results of the four UAVs are as shown in Figure 6 and Figure 7 and Figure 8 and Figure 9 ; the position error of the UAVs is as shown in Figure 10 and Figure 11 and Figure 12 and Figure 13 ;

[0214] After completing the initialization, let Algorithm 1 be the unscented Kalman filter to simultaneously estimate the pose of the UAV and the position of the UWB anchor points;

[0215] Algorithm 2 is the unscented Kalman filter + federated Kalman filter to unify the positions of the UWB nodes to simultaneously estimate the pose of the UAV and the positions of the UWB nodes;

[0216] Compare the estimation errors of the UWB nodes, the position estimation errors of the UAVs, and the angular estimation errors of the UAVs in the table as follows:

[0217] Table 1: Comparison of Method Results

[0218]

[0219]

[0220] It can be seen from Table 1 above that after unifying the positions of the UWB nodes using the federated Kalman filter, the estimation accuracy of the positions of the UWB nodes and the poses of the UAVs is increased compared to the unscented Kalman filter, and the solution accuracy is further increased after adding the sub-filter composed of linear distance constraints.

[0221] It should be noted that in this article, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device comprising a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the existence of additional identical elements in the process, method, article or device comprising the element.

[0222] The above are only specific embodiments of the present invention, enabling those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but rather to the broadest scope consistent with the principles and novel features claimed herein.

Claims

1. A method for multi-UAV collaborative pose estimation based on federated Kalman filtering, characterized in that: include: S1. Equip UWB and IMU on the fuselages of multiple drones. The UWB between each drone measures distance and communicates with each other. Each drone initializes the position of unknown UWB nodes in space and obtains the three-dimensional coordinates of the initialized UWB nodes. S2, the UAV estimates the UAV posture and UWB node position in real time based on the unscented Kalman filter, and sends the UAV posture and UWB node position to the UAVs communicating with each other; S3, after receiving the drone posture and the UWB node position, the drone constructs a sub-filter of the federated Kalman filter based on a linear distance constraint; S4, unifying UWB node locations based on federated Kalman filtering; S5. The multiple drones re-execute steps S2-S4 and start running until the flight ends.

2. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 1, characterized in that: Each UAV initializes the position of an unknown UWB node in space to obtain the initialized three-dimensional coordinates of the UWB node, including: S101, taking the initial take-off point of the drone as the center of the sphere, initializing the first distance measurement as the radius of the sphere as a sphere, and performing spherical sampling on the sphere, specifically: Define the golden ratio φ as: For n spherical collection points, calculate the angle θ and the z coordinate of the spherical collection point: i i =2πφi i=0,1,…,n-1 Among them, i is the serial number of the spherical sampling point, and the x of each spherical sampling point is calculated. i；UWB and i；UWB coordinate: x i；UWB =r i cos(θ i ) y i；UWB =r i sin(θ i ) The spherical collection point is represented as (x i；UWB ,y i；UWB ,z i；UWB ); S102, Initialize a sphere with d1 as the center and d2 as the radius, and use uniform sampling to obtain n uniform spherical sampling points. For each point: Find S u =min(S1,…,S n ), S u The corresponding [x u ,y u ,z u ] is the initialized three-dimensional coordinate of the UWB node.

3. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 2, characterized in that: The UAV estimates the UAV posture and UWB node position in real time based on unscented Kalman filtering, including: S201, the state vector of the drone is expressed as: in, is the three-dimensional position coordinate of the drone in the world coordinate system; is the three-dimensional coordinate of the unknown UWB node; is the attitude of the drone in the world coordinate system, expressed as quaternion represents the three-dimensional velocity in the world coordinate system, b a and b g denote the bias terms of the accelerometer and gyroscope respectively; S202, using the acceleration information of the accelerometer to update the position and speed of the drone, which are respectively expressed as: Where k represents the sampling time, Δt represents the sampling interval, is the acceleration information in the world coordinate system, and the acceleration information Coordinate transformation is performed, expressed as: Among them, n a represents the random noise of the accelerometer, g represents the acceleration due to gravity, It is the rotation matrix from B system to W system, through the quaternion q = [q0, q1, q2, q3] T Convert the drone's attitude from the body coordinate system to the global coordinate system: S203, angular velocity measured by IMU Calculate the attitude of the drone at time k+1 Among them, n g Represents the random noise of the gyroscope, thus describing the state update process of the drone as: X k+1 =f(X k )+w k Where f(X k ) is expressed as follows: Among them, w k represents the process noise, which follows a Gaussian distribution:

4. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 3, characterized in that: Each UAV estimates the UAV posture and UWB node position in real time based on the unscented Kalman filter, and also includes: S204, the distance between the UWB of the drone and the unknown UWB node is expressed as: And the observation equation of the UAV is expressed as: Z k+1 =h(X k+1 )+v k+1 Among them, h(X k+1 ) = d k+1 , v k+1 is the measurement noise, R k+1 represents the variance of the measurement noise, r k+1 represents the expectation of the measurement noise, and its statistical characteristics are described as:

5. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 4, characterized in that: Each UAV estimates the UAV posture and UWB node position in real time based on the unscented Kalman filter, and also includes: S205, initialization state estimation vector and the state covariance matrix P0, based on the state estimate vector at the previous time step Select the Sigma point with the state covariance matrix P0, expressed as: And calculate the sampling point weights: Where n is the dimension of the state vector, λ = α 2 (n+κ)-n is the fitness parameter, S p,k Indicates P k Cholesky decomposition of ; S206, bringing the Sigma point into the state transfer equation of nonlinear propagation, and calculating the next prediction of the state vector and covariance P k+1|k : According to the state prediction vector Select new Sigma points, the distribution of which depends on P k+1|k One-step forecast value of : Among them, S p,k+1|k Indicates P k+1|k Cholesky decomposition of the one-step prediction and propagation of Sigma points through the nonlinear measurement equation The cross-covariance It is expressed as: Calculate Kalman gain Update status prediction Update P k+1 as follows:

6. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 5, characterized in that: The sub-filter of the federated Kalman filter constructed based on the linear distance constraint includes: S301, construct linear constraints between the UWB node position and the position in the UAV posture: Ax=b, in, represents the position of the UWB node, r1, r2, r3, and r4 are the measured distances of any four drones in the multi-drone system, and the positions of the four drones are S302: Construct an optimization problem to unify the positions of i UWB nodes in, and represents the linear constraint corresponding to the i-th UWB node at time k, represents the 3D position of the i-th UWB node estimated by the j-th UAV at time k, represents the corresponding posterior covariance, is the preliminary estimate obtained by the unscented Kalman filter at time k; S303, adding the linear constraint to the optimization problem to solve: ρ is the weight of the linear constraint, and the closed-ended solution of the optimization problem is expressed as: The inverse of the Hessian matrix of the optimization problem is added to the federated Kalman filter as the covariance of the closed solution, which is expressed as:

7. The method for multi-UAV collaborative pose estimation based on federated Kalman filtering as claimed in claim 6, characterized in that: The method of unifying UWB node positions based on federated Kalman filtering includes: S401: The UWB node position obtained in step S2 and the corresponding covariance P i,k Input to the Federated Kalman Filter; S402, performing observation update on each sub-filter in the federated Kalman filter based on step S2; S403, information fusion is performed on the main filter in the federal Kalman filter, which is expressed as: S404, allocating information of the main filter and each sub-filter in the federated Kalman filter, expressed as: The information factor satisfies the following constraints: Where V is the number of sub-filters in the federated Kalman filter.

Images