Ultra-wideband-based multi-machine distributed cooperative positioning method

Abstract

The application discloses a kind of multi-machine distributed cooperative positioning method based on ultra-wideband, including the following contents: step S1, the GNSS data and IMU data of each anchor node unmanned aerial vehicle are fused, respectively obtain the anchor node coordinates of each anchor node unmanned aerial vehicle;Step S2, the IMU data of target node unmanned aerial vehicle is obtained, and the relative angle and relative distance of ultra-wideband between target node unmanned aerial vehicle and each anchor node unmanned aerial vehicle are obtained, and then the relative position of ultra-wideband is solved;Step S3, a plurality of anchor node unmanned aerial vehicles to be fused and the corresponding relative position of ultra-wideband to be fused are preferably fused;Based on improved distributed Kalman filtering algorithm, anchor node unmanned aerial vehicle to be fused and relative position of ultra-wideband to be fused are fused, and the result is the accurate position of target node unmanned aerial vehicle.It solves the problem that when existing unmanned aerial vehicle cluster executes task, because part of unmanned aerial vehicle loses satellite signal, thereby leading to low positioning accuracy and poor stability.

Description

A multi-machine distributed cooperative localization method based on ultra-wideband Technical Field

[0001] This invention belongs to the field of cooperative positioning technology for quadcopter multi-UAV systems, specifically relating to a multi-UAV distributed cooperative positioning method based on ultra-wideband. Background Technology

[0002] With the rapid development of 5G communication and autonomous driving technologies, drones have become a hot research topic in fields such as ecological environment monitoring and power line inspection. The advantages of drones lie in their independence, flexibility, and rapid deployment capabilities. When tasks require higher precision, wider coverage, or greater adaptability, a single drone often falls short. Therefore, drone swarm technology has emerged. A drone swarm is a collaborative system composed of multiple drones that work together autonomously, share information, and divide tasks to accomplish tasks that are difficult or inefficient for a single drone. As the scale of drone swarms continues to expand, achieving accurate positioning and navigation within the swarm, especially in environments with limited GNSS signals, remains a critical technical challenge.

[0003] To address this challenge, researchers have proposed various positioning methods. Among them, centralized multi-drone cooperative positioning algorithms are an important solution; however, they rely on the computing and communication capabilities of the central node, and the entire system may fail if the central node fails. To address these challenges, distributed positioning technology has gradually become a research hotspot in UAV swarms. In a distributed positioning system, each UAV can perform positioning calculations independently and achieve coordination and cooperation within the swarm by sharing location information with other UAVs. However, existing distributed positioning methods typically assume no communication latency between UAVs in the swarm, and most distributed positioning methods can only achieve effective fusion under specific constraints, failing to achieve true parallel processing, resulting in poor stability of the positioning system. Summary of the Invention

[0004] The purpose of this invention is to provide a multi-drone distributed cooperative positioning method based on ultra-wideband to solve the problem of low positioning accuracy and poor stability when some drones lose satellite signals during the execution of tasks in existing drone swarms.

[0005] The present invention adopts the following technical solution: a multi-machine distributed cooperative positioning method based on ultra-wideband, based on a UAV swarm, the UAV swarm includes a target node UAV that has lost GNSS positioning capability and multiple anchor node UAVs with GNSS positioning capability, and the target node UAV and multiple anchor node UAVs are equipped with ultra-wideband sensors.

[0006] The location method specifically includes the following:

[0007] Step S1: Based on the Kalman filter algorithm, fuse the GNSS data and IMU data of each anchor node UAV to obtain the anchor node coordinates of each anchor node UAV.

[0008] Step S2: Obtain the IMU data of the target node UAV, and obtain the ultrawideband relative angle and ultrawideband relative distance between the target node UAV and each anchor node UAV, and then calculate the ultrawideband relative position of the target node UAV relative to each anchor node UAV.

[0009] Step S3: Preprocess each anchor node UAV and each ultrawideband relative position to select multiple anchor node UAVs to be fused and their corresponding ultrawideband relative positions to be fused; Based on the improved distributed Kalman filter algorithm, fuse the anchor node coordinates of the anchor node UAVs to be fused and the ultrawideband relative positions to be fused, and the fusion result is the accurate position of the target node UAV.

[0010] Furthermore, the specific method for step S2 is as follows:

[0011] Step S2.1: Arbitrarily select an anchor node UAV, denoted as the first anchor node UAV. Based on the ultra-wideband signal sent by the first anchor node UAV to the target node UAV, and using the PDOA angle of arrival measurement algorithm, calculate the ultra-wideband relative angle between the target node UAV and the first anchor node UAV by utilizing the carrier phase difference of the dual antenna received signals of the target node UAV.

[0012] Step S2.2: Using the ultra-wideband signal sent from the first anchor node UAV to the target node UAV, and using the time-of-arrival algorithm, the ultra-wideband relative distance between the target node UAV and the first anchor node UAV is calculated using the propagation time of the ultra-wideband signal between the first anchor node UAV and the target node UAV.

[0013] Step S2.3: Combine the ultra-wideband relative angle obtained in step S2.1 and the ultra-wideband relative distance obtained in step S2.2 to calculate the ultra-wideband relative position of the target node UAV relative to the first anchor node UAV.

[0014] Step S2.4: Repeat steps S2.1 to S2.3 to obtain the ultrawideband relative position of the target node UAV with respect to each of the other anchor node UAVs.

[0015] Furthermore, the specific method for preprocessing in step S3 is as follows:

[0016] Based on experience, any value within the range of 0~90° can be selected as the relative elevation angle threshold. Each anchor node UAV whose relative elevation angle with the target node UAV is less than the relative elevation angle threshold is recorded as the preferred anchor node UAV.

[0017] Then, an outlier detection method based on quartiles is used to mark the ultrawideband relative distance between each preferred anchor node UAV and the target node UAV. Using 1.5 times the interquartile range as the judgment threshold, the ultrawideband relative distance is divided into two categories: normal value and outlier value. The preferred anchor node UAV corresponding to each ultrawideband relative distance marked as normal value is recorded as the anchor node UAV to be fused.

[0018] Based on each UWB relative distance marked as a normal value, the corresponding UWB relative position is calculated, and then delay compensation is applied to each calculated UWB relative position. Each UWB relative position after delay compensation is recorded as the UWB relative position to be merged.

[0019] Furthermore, the delay compensation method is as follows: analyze the ultra-wideband measurement process between the target node UAV and the anchor node UAV, calculate the ranging estimation error caused by the relative motion based on the duration of the ranging process and the relative motion speed between the target node UAV and the anchor node UAV, and then compensate the ranging estimation error into the calculation process of the ultra-wideband relative position, thereby making correction.

[0020] Furthermore, in step S3, when the number of anchor node drones to be fused within the communication range of the target node drone is ≥3, the IMU data of the target node drone, the relative position of each ultrawideband drone to be fused, and the anchor node coordinates of each anchor node drone to be fused are fused based on the improved distributed Kalman filter algorithm. The fusion result is the accurate position of the target node drone.

[0021] Alternatively, when the number of anchor node drones to be fused within the communication range of the target node drone is less than 3, multiple target node drone positions are calculated based on all ultra-wideband relative positions obtained in step S2 and the anchor node coordinates of all anchor node drones obtained in step S1. Then, the covariance weighting of all the obtained target node drone positions is performed to obtain the position of a target node drone to be fused. Based on the improved distributed Kalman filter algorithm, the IMU data of the target node drone and the position of the target node drone to be fused are fused, and the fusion result is the accurate position of the target node drone.

[0022] Furthermore, the specific method for improving the distributed Kalman filter algorithm is as follows:

[0023] Set the state vector of any drone in the drone swarm to include two state variables: position and velocity. Obtain the state equation of the drone based on the IMU positioning model.

[0024] The relative position of the ultra-wideband to be fused is used as the state observation value of the UAV at the target node. Based on this, a measurement equation is formed, and the measurement covariance matrix is ​​calculated.

[0025] The measurement equation is updated using the position predictions of the target node UAV by the UAV to be fused anchor node; at the same time, the position estimation covariance of the target node UAV is calculated and used as input to update the measurement covariance matrix.

[0026] Based on the updated measurement equations and the updated measurement covariance matrix, the state update of the target node UAV is completed.

[0027] The beneficial effects of this invention are as follows: This invention designs a delay compensation algorithm for the ultra-wideband ranging process, analyzes and compensates for measurement errors caused by communication delays during ranging, thereby improving the measurement accuracy of ultra-wideband relative positions. This invention also designs an improved distributed Kalman filter algorithm and an optimization method for situations where the number of anchor node UAVs is insufficient. It fully utilizes the measurement information of anchor node UAVs, supports decentralized distributed positioning, and ensures that UAV swarms can still achieve accurate positioning and stable operation under GNSS partial rejection conditions. This invention uses ultra-wideband sensor measurement information to improve the positioning accuracy when multiple UAVs are working collaboratively. Compared to traditional centralized positioning methods, the distributed collaborative positioning method achieves parallel data processing, improves positioning robustness, and introduces functions such as UAV-based inter-UAV measurement, making it widely applicable to various civilian scenarios, such as UAV swarm collaborative ecological detection and power system inspection. Attached Figure Description

[0028] Figure 1 is a schematic diagram of the calculation process of the ultra-wideband relative position in step S2.3 of the present invention;

[0029] Figure 2 is a schematic diagram of the ranging process between a group of anchor node UAVs and target node UAVs in this invention.

[0030] Among them, 1. the first anchor node UAV, and 2. the target node UAV. Detailed Implementation

[0031] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0032] This invention provides a multi-machine distributed cooperative positioning method based on ultra-wideband. The positioning method is based on a UAV swarm, which includes a target node UAV that loses GNSS positioning capability in a partially denied satellite environment and multiple anchor node UAVs with GNSS positioning capability. The target node UAV and the multiple anchor node UAVs are all equipped with ultra-wideband sensors.

[0033] The location method specifically includes the following:

[0034] Step S1: Based on the Kalman filter algorithm, the GNSS data and IMU data of each anchor node UAV are fused to obtain the anchor node coordinates of each UAV. IMU stands for Inertial Measurement Unit. Both IMU data and GNSS data are positioning data.

[0035] Step S2: Obtain the IMU data of the target node UAV itself, and obtain the ultra-wideband relative angle and ultra-wideband relative distance between the target node UAV and each anchor node UAV, and then calculate the ultra-wideband relative position of the target node UAV relative to each anchor node UAV.

[0036] Step S3: Preprocess each anchor node UAV and each ultrawideband relative position to select multiple anchor node UAVs to be fused and their corresponding ultrawideband relative positions to be fused; Based on the improved distributed Kalman filter algorithm, fuse the anchor node coordinates of the anchor node UAVs to be fused and the ultrawideband relative positions to be fused, and the fusion result is the accurate position of the target node UAV.

[0037] In some embodiments, the specific method of step S2 is as follows:

[0038] Step S2.1: Arbitrarily select an anchor node UAV, denoted as the first anchor node UAV. On this first anchor node UAV, arbitrarily select three points A, S, and C that are not on the same straight line, and install antennas at each of these three points. Based on the ultra-wideband signal transmitted from the first anchor node UAV to the target node UAV, and using the PDOA-based angle of arrival measurement algorithm, calculate the ultra-wideband relative angle between the target node UAV and the first anchor node UAV using the carrier phase difference of the signals received by the target node UAV's dual antennas. PDOA is the phase difference of arrival, and the ultra-wideband relative angle is the horizontal angle and pitch angle between the target node UAV and the first anchor node UAV.

[0039] Step S2.2: Using the ultra-wideband signal sent from the first anchor node UAV to the target node UAV, and using the time-of-arrival algorithm, the ultra-wideband relative distance between the target node UAV and the first anchor node UAV is calculated based on the propagation time of the ultra-wideband signal between the first anchor node UAV and the target node UAV.

[0040] Step S2.3: Combine the ultra-wideband relative angle obtained in step S2.1 and the ultra-wideband relative distance obtained in step S2.2 to calculate the ultra-wideband relative position of the target node UAV relative to the first anchor node UAV.

[0041] As shown in Figure 1, the midpoint of the line connecting point S and point C can be taken as the origin of the coordinate system, denoted as O; and let OC be... In the positive direction of the axis, OA is Positive axis direction The axis is perpendicular to the OAC plane and points upwards, thus establishing a base station rectangular coordinate system. A point D is randomly selected on the target node UAV 2 and an antenna is installed. The coordinates of point D in the base station rectangular coordinate system are calculated, yielding the ultra-wideband relative position between the target node UAV and the first anchor node UAV.

[0042] Step S2.4: Repeat steps S2.1 to S2.3 to obtain the ultrawideband relative position of the target node UAV with respect to each of the other anchor node UAVs.

[0043] In some embodiments, the specific method for preprocessing in step S3 is as follows:

[0044] Step 3.1: Based on experience, arbitrarily select a value within the range of 0~90° as the relative elevation angle threshold, and record each anchor node UAV whose relative elevation angle with the target node UAV is less than the relative elevation angle threshold as the preferred anchor node UAV.

[0045] During the execution of tasks by drone swarms, several factors can significantly impact ranging accuracy, primarily including the following three aspects:

[0046] a. In actual experiments, when there is a relatively large elevation angle between two UAV nodes in the cluster, the signal transmitted by the ultra-wideband module may be blocked by the fuselage, resulting in a large outlier in the ranging data.

[0047] b. When obstacles appear between drones in the cluster, it will have a significant impact on ranging accuracy.

[0048] c. Due to the high maneuverability of drones, the relative speed between drones in a cluster is generally large. During distance measurement, the relative motion of each drone will lead to dynamic errors.

[0049] In summary, when there is a situation where the relative elevation angles between drones in a cluster are large, the present invention needs to identify the stray anchor node drones and remove them. The remaining anchor node drones are denoted as preferred anchor node drones.

[0050] The method for elimination is to define an elevation angle criterion to eliminate potential outliers.

[0051] The formula for determining the elevation angle is:

[0052] , (1)

[0053] in, For drones anchoring nodes within the cluster and target node drones The information obtained between them via the ultra-wideband module The distance measured at a given time is the ultra-wideband relative distance. and Anchor node drones and target node drones exist The height estimate at time t. This is the elevation angle threshold.

[0054] In actual testing, an elevation angle threshold can be used. The angle is 32°. If the calculated relative elevation angle is greater than 32°, then the distance measurement value is considered to be... The distance measurement value may be affected by the drone's body obstructing the view. The corresponding anchor node drones are marked as outliers and removed.

[0055] Step 3.2: Then, the outlier detection method based on quartiles is used to mark the ultrawideband relative distance between each preferred anchor node UAV and the target node UAV. The ultrawideband relative distance is divided into two categories: normal value and outlier value, with 1.5 times the interquartile range as the judgment threshold. The preferred anchor node UAVs corresponding to all ultrawideband relative distances marked as normal values ​​are all recorded as anchor node UAVs to be fused.

[0056] Because ultra-wideband ranging may produce outliers due to obstacles or other factors, this invention utilizes quartiles, a statistical method, to remove outliers. Since each drone is constantly moving and the ranging information obtained is constantly changing, this invention sets a time window and uses quartiles to label outliers within the window.

[0057] The present invention uses (Interquartile Range) method. yes and The difference between them, that is:

[0058] , (2)

[0059] in, The third quartile of all distance measurements within the time window. It represents the first quartile of all distance measurements within the time window.

[0060] Therefore, according to 1.5 times to identify the ranging value The identification conditions are as follows:

[0061] , (3)

[0062] That is, if the distance measurement value satisfies the formula (3), then the distance measurement value is marked as a normal value; otherwise, the distance measurement value is marked as an outlier and the outlier value is removed.

[0063] Step 3.3: Calculate the corresponding UWB relative position based on each UWB relative distance marked as a normal value, and then perform delay compensation on each calculated UWB relative position. Each UWB relative position after delay compensation is recorded as the UWB relative position to be fused.

[0064] In some embodiments, the method for delay compensation of the ultra-wideband relative position marked as normal value is as follows: analyze the ultra-wideband measurement process between the target node UAV and the anchor node UAV, calculate the ranging estimation error caused by the relative motion based on the duration of the ranging process and the relative motion speed between the target node UAV and the anchor node UAV, and then compensate the ranging estimation error into the calculation process of the ultra-wideband relative position to make correction.

[0065] The specific method for delay compensation is as follows:

[0066] Step 3.3.1: Select any UWB relative position requiring delay compensation, corresponding to an anchor node UAV and a target node UAV. Figure 2 shows a schematic diagram of the ranging process between the anchor node UAV and the target node UAV. T1 is the starting time of ranging, T2 is the time when the target node UAV receives the ranging request message, T3 is the time when the target node UAV sends the ranging response information, T4 is the time when the anchor node UAV receives the ranging response information, T5 is the time when the anchor node UAV sends the ranging termination message, and T6 is the time when the target node UAV receives the ranging abort message.

[0067] Step 3.3.2: Perform delay compensation on the ultra-wideband relative position selected in Step 3.1.1. Specifically, assume that at time T1, the anchor node UAV sends a ranging request message to the target node UAV. At this time, the distance between the initial position I of the anchor node UAV and the initial position IV of the target node UAV is... At time T4, the anchor node UAV received the response information sent by the target node UAV. At this time, due to relative motion, the distance between the anchor node UAV and the target node UAV changed. At time T5, the anchor node UAV sends a ranging termination message to the target node UAV. At this point, the actual distance between the anchor node UAV and the target node UAV becomes... .

[0068] Therefore, we can conclude that:

[0069] , (4)

[0070] , (5)

[0071] in, This represents the relative displacement between the anchor node UAV and the target node UAV during the ranging request response process. This represents the relative displacement of the anchor node UAV and the target node UAV during the ranging termination response, assuming the relative velocity between the anchor node UAV and the target node UAV during the ranging process. The duration of the response process remains unchanged. Then we have:

[0072] ; (6)

[0073] The distance between the anchor node UAV and the target node UAV measured by the ultra-wideband sensor is based on the assumption that the two UAVs remain relatively stationary during the ranging process. Therefore, the ultra-wideband relative distance information between the two UAVs... for:

[0074] , (7)

[0075] Therefore, the distance estimation error caused by relative motion can be obtained. for:

[0076] , (8)

[0077] Substituting formulas (4) to (7) into formula (8) and simplifying, we get:

[0078] ; (9)

[0079] Therefore, the ranging estimation error caused by the relative motion between UAVs is related to the relative speed between UAVs and the time for the ultra-wideband module to process the signal, and the ranging estimation error can be obtained according to formula (9).

[0080] Step 3.3.3: Compensate the ranging estimation error to the UWB relative position selected in Step 3.3.1 to obtain the UWB relative position to be fused. Repeat steps 3.3.1 to 3.3.3 to complete the delay compensation for other UWB relative positions.

[0081] In some embodiments, in step S3, when the number of anchor node drones to be fused within the communication range of the target node drone is ≥3, the IMU data of the target node drone, the relative position of each ultrawideband drone to be fused, and the anchor node coordinates of each anchor node drone to be fused are fused based on the improved distributed Kalman filter algorithm, and the fusion result is the accurate position of the target node drone.

[0082] Alternatively, when the number of anchor node drones to be fused within the communication range of the target node drone is less than 3, multiple target node drone positions are calculated based on all ultra-wideband relative positions obtained in step S2 and the anchor node coordinates of all anchor node drones obtained in step S1. Then, the covariance weighting of all the obtained target node drone positions is performed to obtain the position of a target node drone to be fused. Based on the improved distributed Kalman filter algorithm, the IMU data of the target node drone and the position of the target node drone to be fused are fused, and the fusion result is the accurate position of the target node drone.

[0083] In real-world scenarios, during missions in complex environments, the number of anchor nodes within the communication range of a target drone node may be insufficient. For a target drone node, since the number of surrounding anchor nodes may not meet the positioning convergence condition, it is necessary to utilize the positioning information of other drones within its communication range to improve its positioning accuracy. This invention does not limit the communication structure of drones within the cluster but instead uses a dynamically adjusted covariance weighted algorithm to address the measurement noise uncertainty caused by dynamic changes in the communication topology and the unknown states of adjacent drone nodes. Different anchor nodes adjacent to a target drone node have unknown correlations in their estimations of the target drone node's position. Therefore, the covariance weighted algorithm can be used to fuse different information sources to obtain the target drone node position with the minimum comprehensive covariance, i.e., the position of the target drone node to be fused.

[0084] In some embodiments, the specific method for improving the distributed Kalman filter algorithm in step S3 is as follows:

[0085] Set the state vector of any drone in the drone swarm, including position and velocity, and obtain the state equation of the drone based on the IMU positioning model.

[0086] The relative position of the ultra-wideband to be fused is used as the state observation of the target node UAV to form a measurement equation and obtain the measurement covariance matrix;

[0087] Considering the time delay, the position prediction of the target node UAV by the UAV to be fused anchor node is used to update the measurement equation; at the same time, the position estimation covariance of the target node UAV is calculated and the measurement covariance matrix is ​​updated.

[0088] Based on the updated measurement equations and the updated measurement covariance matrix, the state update of the target node UAV is completed.

[0089] Specifically, suppose any drone within the cluster exist The state vector at time t is:

[0090] , (10)

[0091] in, and drones exist The position and velocity vectors at each moment. Based on the IMU positioning model, the drone's position and velocity vectors can be obtained. exist The state equation at time:

[0092] , (11)

[0093] in, Let be the state transition matrix, describing the state transition from... Transfer to time; To control the input matrix; The control input vector is the acceleration information obtained from the IMU positioning model. For process noise, satisfy ,in Let be the process noise covariance.

[0094] For anchor node UAVs, since they can directly receive GNSS signals to obtain their own positioning, GNSS positioning data is used as a measurement, and positioning prediction and estimation are performed using Kalman filtering iterations. For target node UAVs, they cannot directly obtain GNSS signals and need to rely on the cooperation of surrounding anchor node UAVs for their own positioning. Let the target node UAV be... Anchor node drones are Within the communication range of the anchor node UAV, the target node UAV Ultra-wideband modules can be used to obtain information about drones connected to anchor nodes. relative position measurement Simultaneously, the target node drone's onboard IMU can obtain information about the target node drone. pose matrix The takeoff points of all drones within the cluster are known, so the target node's drones can be identified. With anchor node drones Relative position in the world coordinate system:

[0095] , (12)

[0096] Therefore, the measurement equation for the target node UAV can be obtained:

[0097] , (13)

[0098] Among them, the measurement vector For target node drones With anchor node drones Relative position in the world coordinate system ; For measurement matrix; For anchor node drones exist Position coordinates at the given time; To measure noise, let it satisfy the following under ideal conditions. ,in To measure the noise covariance.

[0099] Analyze formula (13). The measurement equation for the target node UAV at any given time includes not only its own state variables but also the position coordinates of its neighboring anchor node UAVs at the same time. Considering that network latency and bandwidth limitations may still affect the efficiency of information transmission and thus the real-time performance of the system, in practice... Currently, the target node UAV cannot immediately obtain the position information of the anchor node UAV. To solve this problem, the measurement equations for the target node UAV are rewritten:

[0100] , (14)

[0101] in, For anchor node drones use Location information at time of the prediction The position of the time. This improvement can enhance the system's real-time performance, but it also introduces new errors. :

[0102] , (15)

[0103] The updated measurement equations for the target node UAV can be obtained:

[0104] , (16)

[0105] in, To introduce new errors The total measurement error of the system afterwards satisfies:

[0106] ; (17)

[0107] Below, we will discuss the measurement error. Analysis:

[0108] , (18)

[0109] , (19)

[0110] Substituting formulas (17) and (18) into formula (19), we get:

[0111] , (20)

[0112] Assuming the relative position noise measured by the ultra-wideband module originates from hardware noise, environmental interference, etc., and is independent of the position change of the target node UAV, then we have and independent.

[0113] Therefore, we can conclude that:

[0114] , (twenty one)

[0115] Note Representative anchor node drone exist The position estimation error at time t is then:

[0116] , (twenty two)

[0117] in, For anchor node drones exist The position estimation covariance matrix at time.

[0118] Combining formulas (21) and (22), a new measurement error can be obtained. The corresponding covariance matrix The updated measurement covariance matrix is:

[0119] , (twenty three)

[0120] Finally, based on the updated measurement equations and the updated measurement covariance matrix, the state update of the target node UAV is completed.

[0121] Next, we will use experimental simulations to verify the effectiveness of this invention in terms of positioning accuracy.

[0122] A cluster of five UAVs was established, with four UAVs equipped with GPS modules serving as anchor nodes and one UAV without a GPS module serving as the target node. Each UAV was equipped with an ultra-wideband sensor. It was assumed that any UAV within the cluster could communicate with all other UAVs in the cluster. To ensure the diversity and complexity of the experimental scenario, the target trajectory of each UAV was planned based on randomly generated initial positions, velocities, and attitudes. The experimental group used the improved distributed Kalman filter (DKF) algorithm of this invention for fusion, while the control group used the centralized Kalman filter (CKF) algorithm. Based on the true trajectory values ​​of each UAV, the single-step time, convergence time, and steady-state error of the experimental and control groups were calculated under different noise standard deviations. Specific data are shown in Table 1.

[0123] Table 1. Experimental results of the experimental group and the control group.

[0124]

[0125] As shown in Table 1, the DKF algorithm exhibits significant advantages over the CKF algorithm in terms of both computation time and convergence time, especially under high noise conditions. Regarding computation time, the single-step time of the CKF algorithm increases significantly with increasing noise levels. This is mainly because the CKF algorithm requires centralized processing of global information; increased noise leads to increased computational complexity and covariance matrix update time, while the DKF algorithm, with its distributed architecture, effectively reduces the computational burden. Regarding convergence time, the convergence time of the DKF algorithm gradually becomes faster than that of the CKF algorithm, especially under high noise conditions, where the DKF algorithm reaches the system's stable state more quickly, demonstrating higher adaptability and robustness. Regarding stability error, at the same noise level, the steady-state error of the DKF algorithm is smaller than that of the CKF algorithm. With increasing noise, the DKF algorithm exhibits stronger robustness and can effectively control errors in complex environments. Therefore, overall, the DKF algorithm in this invention significantly reduces computation time and improves positioning accuracy and robustness in high-noise environments.

[0126] Next, we will verify the effectiveness of this invention in terms of positioning accuracy through real-world experiments.

[0127] A swarm of five quadcopter drones was established, each equipped with an ultra-wideband sensor. Four of the drones, equipped with GPS modules, served as anchor nodes, while one drone, without a GPS module, served as the target node. Each drone could acquire the azimuth and distance information of the other drones via its ultra-wideband module.

[0128] The flight process of the five drones is as follows: at t = 0s, the five drones take off in a trapezoidal shape and fly randomly on the same altitude plane; at t = 50s, the five drones increase their longitudinal motion; at t = 100s, the five drones land.

[0129] Experimental data shows that the absolute trajectory error (ATE) of the five quadcopter drone swarm is 0.46m, the relative pose error (RPE) is 0.05m, the CPU utilization is 54.3%, the communication bandwidth is 289kbps, and the single positioning processing time is 26.1ms. Notably, the ATE remains within 1m and the RPE within 0.1m, indicating that the method of this invention can provide high-precision and robust positioning data for the five quadcopter drone swarm, meeting the positioning requirements of drone swarm missions under partial satellite denial conditions.

Claims

1. A multi-machine distributed cooperative positioning method based on ultra-wideband, characterized in that, Based on a drone swarm, the drone swarm includes a target node drone that has lost GNSS positioning capability and multiple anchor node drones that have GNSS positioning capability. Both the target node drone and the multiple anchor node drones are equipped with ultra-wideband (UWB) sensors. The multi-drone distributed cooperative positioning method specifically includes the following steps: Step S1: Based on the Kalman filter algorithm, the GNSS data and IMU data of each anchor node drone are fused to obtain the anchor node coordinates of each anchor node drone. Step S2: The IMU data of the target node drone is obtained, and the UWB relative angle and UWB relative distance between the target node drone and each anchor node drone are obtained, thereby calculating the UWB relative position of the target node drone with respect to each anchor node drone. The specific method of step S2 is as follows: Step S2.1: Arbitrarily select one of the anchor node drones, denoted as the first anchor node drone. Based on the UWB signal emitted by the first anchor node drone to the target node drone, using the PDOA angle of arrival measurement algorithm and the carrier phase difference of the dual-antenna received signals of the target node drone, the relative position of the target node drone with each anchor node drone is calculated. Step S2.1: Describe the ultra-wideband relative angle between the first anchor node UAVs; Step S2.2: Using the ultra-wideband signal emitted by the first anchor node UAV to the target node UAV, and using the time-of-arrival algorithm, calculate the ultra-wideband relative distance between the target node UAV and the first anchor node UAV based on the propagation time of the ultra-wideband signal between the first anchor node UAV and the target node UAV; Step S2.3: Combine the ultra-wideband relative angle obtained in Step S2.1 and the ultra-wideband relative distance obtained in Step S2.2 to calculate the ultra-wideband relative position between the target node UAV and the first anchor node UAV; Step S2.4: Repeat the contents of Steps S2.1 to S2.3 to obtain the ultra-wideband relative position between the target node UAV and each other anchor node UAV; Step S3: Preprocess each anchor node UAV and each ultra-wideband relative position to select multiple anchor node UAVs to be fused and their corresponding ultra-wideband relative positions to be fused; Based on the improved distributed Kalman filter algorithm, fuse the anchor node coordinates of the anchor node UAVs to be fused and the ultra-wideband relative positions to be fused, and the fusion result is the accurate position of the target node UAV.

2. The multi-machine distributed cooperative positioning method based on ultra-wideband as described in claim 1, characterized in that, The specific method of preprocessing in step S3 is as follows: Based on experience, any value in the range of 0~90° is selected as the relative elevation angle threshold, and each anchor node UAV whose relative elevation angle with the target node UAV is less than the relative elevation angle threshold is recorded as the preferred anchor node UAV. Then, an outlier detection method based on quartiles is used to mark the ultrawideband relative distance between each of the preferred anchor node UAVs and the target node UAVs. Using 1.5 times the interquartile range as the judgment threshold, the ultrawideband relative distance data is divided into two categories: normal values ​​and outliers. The preferred anchor node UAV corresponding to each UWB relative distance marked as a normal value is denoted as the anchor node UAV to be fused; the corresponding UWB relative position is calculated based on each UWB relative distance marked as a normal value, and then delay compensation is performed on each calculated UWB relative position. Each UWB relative position after delay compensation is denoted as the UWB relative position to be fused.

3. The multi-machine distributed cooperative positioning method based on ultra-wideband as described in claim 2, characterized in that, The delay compensation method is as follows: analyze the ultra-wideband measurement process between the target node UAV and the anchor node UAV, calculate the ranging estimation error caused by the relative motion based on the duration of the ranging process and the relative motion speed between the target node UAV and the anchor node UAV, and then compensate the error into the calculation process of the ultra-wideband relative position to make correction.

4. The multi-machine distributed cooperative positioning method based on ultra-wideband as described in claim 3, characterized in that, In step S3, when the number of anchor node drones to be fused within the communication range of the target node drone is ≥3, the IMU data of the target node drone, the relative position of each ultra-wideband drone to be fused, and the anchor node coordinates of each anchor node drone to be fused are fused based on the improved distributed Kalman filter algorithm. The fusion result is the accurate position of the target node drone. When the number of anchor node drones to be fused within the communication range of the target node drone is <3, the positions of multiple target node drones are calculated based on all ultra-wideband relative positions obtained in step S2 and all anchor node drone coordinates obtained in step S1. Then, the covariance weighting of all the obtained target node drone positions is performed to obtain the position of a target node drone to be fused. Based on the improved distributed Kalman filter algorithm, the IMU data of the target node UAV and the location of the target node UAV to be fused are fused, and the fusion result is the accurate location of the target node UAV.

5. The multi-machine distributed cooperative positioning method based on ultra-wideband as described in claim 4, characterized in that, The specific method of the improved distributed Kalman filter algorithm is as follows: set the state vector of any UAV in the UAV cluster to include two state variables, position and velocity, and obtain the state equation of the UAV according to the IMU positioning model; take the UAV's relative position to be fused as the state observation value of the target node UAV, form a measurement equation accordingly, and calculate the measurement covariance matrix. The measurement equation is updated using the predicted position of the target node UAV by the anchor node UAV to be fused; simultaneously, the position estimation covariance of the target node UAV is calculated and used as input to update the measurement covariance matrix; based on the updated measurement equation and the updated measurement covariance matrix, the state update of the target node UAV is completed.

Images