Unmanned aerial vehicle cluster positioning method and device based on kalman filtering in satellite denial environment

Abstract

The present application relates to the technical field of positioning, in particular to a satellite denial environment-based unmanned aerial vehicle cluster positioning method and device based on Kalman filtering, which can be used in the positioning of unmanned aerial vehicle clusters in a satellite denial environment, a plurality of ground stations are used to first position a plurality of aircraft, since the aircraft adopt cluster operation, the present application uses the observation data of the inertial sensors of adjacent aircraft in the cluster to verify the distance change between adjacent aircraft, and when an anomaly is verified, the abnormal unmanned aerial vehicle is quickly determined by using the calculated abnormal distance. Then call Kalman filtering to correct the positioning of the section of abnormal positioning data of the abnormal unmanned aerial vehicle. The present application performs Kalman filtering by screening abnormal points, so that the aircraft cluster can be accurately positioned by using a ground station with lower computing power, and has strong applicability.

Description

[0001] This application is a divisional application of Chinese application filed on October 24, 2024, with application number 202411491316.5 and entitled "Positioning method and apparatus based on multi-ground radio ranging assistance in satellite denial environment". Technical Field

[0002] This invention relates to the field of positioning technology, specifically to a method and apparatus for swarm positioning of unmanned aerial vehicles (UAVs) based on Kalman filtering in satellite-denied environments. Background Technology

[0003] Currently, aircraft positioning mostly uses the Global Positioning System (GPS), which is based on satellite positioning. However, in some scenarios, such as environments where satellite signals are interfered with, blocked, or completely denied, traditional navigation systems relying on GPS or GNSS will fail. Therefore, existing technologies may use other methods, such as ground base stations, to locate drones. However, positioning via ground base stations is susceptible to noise interference, so corresponding filtering algorithms are needed to eliminate the impact of noise.

[0004] In existing technologies, if filtering algorithms are used to eliminate noise for aircraft positioning, it will bring a huge computational burden, especially in the context of UAV swarms, where the computing power of ground base stations is insufficient to support continuous use of filtering algorithms for noise elimination in the positioning of aircraft swarms. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a method and apparatus for swarm localization of unmanned aerial vehicles (UAVs) based on Kalman filtering in satellite denial environments, so as to solve the problems in the background art.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] The present invention provides a method for UAV swarm localization based on Kalman filtering in satellite-denied environments, comprising the following steps:

[0008] Multiple aircraft are located using multiple ground base stations to obtain the location coordinates of each aircraft at multiple time points; and observation data from multiple sensors of each aircraft at multiple time points are acquired, including inertial sensors.

[0009] Calculate the relative distance between the positioning coordinates of the same group of aircraft at the same time point; and determine the relative position data of each group of aircraft during flight based on the relative distance at multiple time points. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft.

[0010] Based on the observation data of the inertial sensors of each group of aircraft at multiple time points, the positioning coordinates of each group of aircraft during the flight process are checked for anomalies, and the target aircraft with abnormal positioning coordinates are obtained.

[0011] Kalman filtering is performed on the target aircraft's positioning coordinates within a target time period and observation data from multiple sensors to obtain precise positioning. The target time period includes multiple time points within a preset duration preceding the abnormal time point where the positioning coordinates are abnormal. The precise positioning process involves: establishing a process model and an observation model of the aircraft during flight; selecting a target time point within the target time period and constructing the initial state of the target aircraft using its positioning coordinates and observation data from multiple sensors at that time point, and constructing an initial covariance matrix based on the initial state; and performing Kalman filtering on the initial state and initial covariance matrix to obtain precise positioning of the target aircraft at multiple time points after the target time point.

[0012] In one embodiment of this application, the aircraft is located based on multiple ground base stations to obtain the aircraft's location coordinates at multiple time points, including:

[0013] Obtain the coordinates of multiple base stations;

[0014] The distance to the aircraft is measured at multiple consecutive time points based on multiple ground base stations to obtain the distance at multiple consecutive time points;

[0015] Based on the coordinates of the multiple base stations, the positioning coordinates of the aircraft, and the distances at multiple consecutive time points, a set of distance equations for multiple consecutive time points is constructed.

[0016] By fitting the distance equations at each consecutive time point using the least squares method, the positioning coordinates of the aircraft at multiple time points are obtained.

[0017] In one embodiment of this application, the inertial sensor includes a three-axis gyroscope and an accelerometer. The anomaly verification of the positioning coordinates of each group of aircraft during flight, based on observation data from the inertial sensor at multiple time points, includes:

[0018] The instantaneous velocity and acceleration of the aircraft at multiple time points are obtained, wherein the instantaneous velocity includes a velocity value and a velocity direction, and the instantaneous velocity is obtained by integrating the observation data of the three-axis gyroscope and the accelerometer.

[0019] For each group of aircraft, the relative distance at the next time point is predicted based on the instantaneous velocity, acceleration, and relative distance at the previous time point, thus obtaining the predicted distance;

[0020] The relative distance at the next time point is compared with the predicted distance, and if the difference between the relative distance at the next time point and the predicted distance exceeds a preset threshold, it is determined that the relative distance at the next time point is abnormal.

[0021] The system filters out the abnormal relative distances and performs anomaly verification on the positioning coordinates of all aircraft based on these abnormal relative distances.

[0022] In one embodiment of this application, anomaly verification is performed on the positioning coordinates of all aircraft based on abnormal relative distances, including:

[0023] Mark the aircraft corresponding to all abnormal relative distances as candidate aircraft;

[0024] Candidate aircraft corresponding to the relative distances of multiple anomalies are marked as target aircraft;

[0025] Candidate aircraft that meet the first objective condition are identified as normal aircraft. The first objective condition includes: corresponding to only one abnormal relative distance, and the relative distance of the corresponding abnormality corresponds to the target aircraft.

[0026] Candidate aircraft that meet the second objective condition are identified as target aircraft. The second objective condition includes: corresponding to only one anomaly in relative distance, and the relative distance of the corresponding anomaly does not correspond to the target aircraft.

[0027] In one embodiment of this application, the relative distance at a subsequent time point is predicted based on the instantaneous velocity, acceleration, and relative distance at the previous time point, to obtain the predicted distance, including:

[0028] Connect the positioning coordinates of two aircraft in the aircraft group to obtain the reference horizontal axis, and construct a reference vertical axis and a reference longitudinal axis perpendicular to the reference horizontal axis with one of the aircraft as the origin;

[0029] The instantaneous velocities and accelerations of the two aircraft are decomposed into the reference horizontal axis, the reference vertical axis, and the reference longitudinal axis, and the differences between them are calculated to obtain the initial velocity v on the horizontal axis. h Horizontal accelerator a h Initial velocity v along the vertical axis v Vertical acceleration a v Initial velocity v along the vertical axis l and longitudinal acceleration a l ;

[0030] Based on the duration T between the previous and next time points, the initial velocity v of the horizontal axis h The horizontal axis acceleration a h The initial velocity v of the vertical axisv The vertical axis acceleration a v The initial velocity v along the longitudinal axis l and the longitudinal acceleration a l Calculate the relative displacement S of the horizontal axis at the next time point. h Vertical axis relative displacement S v The relative displacement S with respect to the vertical axis l ;

[0031] Based on the relative displacement S of the horizontal axis h The relative displacement S of the longitudinal axis l Calculate the predicted distance S k+1 ', wherein the predicted distance S k+1 The mathematical expression for ' is:

[0032]

[0033] In the formula, S k This represents the relative distance to the previous point in time.

[0034] In one embodiment of this application, selecting a target time point from the target time period includes:

[0035] Acquire basic stability data of the target aircraft at multiple time points within the target time period, wherein the basic stability data includes ranging signal strength and acceleration values ​​on each axis;

[0036] The stability baseline data at multiple time points are mapped to a two-dimensional coordinate system, wherein the vertical axis of the two-dimensional coordinate system is the data axis and the horizontal axis is the time axis.

[0037] Based on a pre-built sliding window, the distance is slid along the time axis, and at each sliding time, the mean of the distance measurement signal strength, the variance of the distance measurement signal strength, the mean of the acceleration, and the variance of the acceleration at multiple time points within the sliding window are calculated.

[0038] Candidate windows are selected where the average ranging signal strength is greater than a preset signal strength threshold and the average acceleration is less than or equal to a preset acceleration threshold.

[0039] The stability value of each candidate window is obtained by weighted summation of the variance of ranging signal intensity and the variance of acceleration.

[0040] The candidate window with the highest stability value is selected as the target window, and the midpoint of the target window is selected as the target time point.

[0041] In one embodiment of this application, the stability value W step The mathematical expression is:

[0042]

[0043] In the formula, step is the sequence number of the candidate window. Let be the variance of the ranging signal strength of the step-th candidate window. The acceleration variance of the step-th candidate window, where α is the first weight and β is the second weight.

[0044] In one embodiment of this application, it further includes:

[0045] After obtaining accurate positioning, the Kalman filter is exited, and the system returns to positioning multiple aircraft based on multiple ground base stations.

[0046] This application also provides a UAV swarm positioning device based on Kalman filtering in satellite-denied environments, characterized in that it includes:

[0047] The positioning module is used to locate multiple aircraft based on multiple ground base stations, obtain the positioning coordinates of each aircraft at multiple time points, and acquire observation data of each aircraft from multiple sensors at multiple time points, wherein the multiple sensors include inertial sensors.

[0048] The distance calculation module is used to calculate the relative distance between the positioning coordinates of the same group of aircraft at the same point in time; and to determine the relative position data of each group of aircraft during flight based on the relative distance at multiple points in time. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft.

[0049] The anomaly verification module is used to verify the positioning coordinates of each group of aircraft during flight based on the observation data of the inertial sensors of each group of aircraft at multiple time points, and to obtain the target aircraft with abnormal positioning coordinates.

[0050] The positioning correction module is used to perform Kalman filtering based on the positioning coordinates of the target aircraft during a target time period and observation data from multiple sensors to obtain accurate positioning. The target time period includes multiple time points within a preset duration preceding the abnormal time point where the positioning coordinates are abnormal. The process of performing Kalman filtering based on the positioning coordinates of the target aircraft during the target time period and observation data from multiple sensors to obtain accurate positioning includes: establishing a process model and an observation model of the aircraft during flight; selecting a target time point from the target time period, and constructing the initial state of the target aircraft using the positioning coordinates of the target aircraft at the target time point and observation data from multiple sensors, and constructing an initial covariance matrix based on the initial state of the aircraft; and performing Kalman filtering based on the initial state of the target aircraft and the initial covariance matrix to obtain accurate positioning of the target aircraft at multiple time points after the target time point. The beneficial effects of this invention are as follows: The UAV swarm positioning method and apparatus based on Kalman filtering in satellite-denied environments can be applied to UAV swarm positioning in satellite-denied environments. Multiple ground base stations are used to first locate multiple aircraft. Since the aircraft operate in swarms, this application uses observation data from the inertial sensors of adjacent aircraft to verify the distance changes between adjacent aircraft. When an anomaly is detected, the calculated anomaly distance is used to quickly identify the abnormal UAV. Then, Kalman filtering is applied to correct the positioning of the abnormal UAVs in the corresponding segments. This application performs Kalman filtering by filtering out anomaly points, thereby achieving accurate positioning of aircraft swarms using base stations with lower computing power, and has strong applicability. Attached Figure Description

[0051] The present invention will be further described below with reference to the accompanying drawings and embodiments:

[0052] Figure 1 This is a diagram illustrating an application scenario in one embodiment of this application;

[0053] Figure 2 This is a flowchart illustrating a UAV swarm localization method based on Kalman filtering in a satellite denial environment, as shown in one embodiment of this application.

[0054] Figure 3 This is a schematic diagram illustrating the discrimination principle of an abnormal aircraft in one embodiment of this application;

[0055] Figure 4 This is a structural diagram of a UAV swarm positioning device based on Kalman filtering in a satellite denial environment, as shown in one embodiment of this application. Detailed Implementation

[0056] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, unless otherwise specified, the following embodiments and features described therein can be combined with each other.

[0057] It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Therefore, the drawings only show the layers related to the present invention and are not drawn according to the actual number, shape and size of the layers in the actual implementation. In the actual implementation, the form, number and proportion of each layer can be arbitrarily changed, and the layer layout may also be more complex.

[0058] Numerous details are explored in the following description to provide a more thorough explanation of embodiments of the invention; however, it will be apparent to those skilled in the art that embodiments of the invention may be practiced without these specific details.

[0059] Figure 1 This is an application scenario diagram in one embodiment of this application, such as... Figure 1 As shown, this application uses multiple ground base station radios, such as base station radio 1 (x1, y1, z1), base station radio 2 (x2, y2, z2), base station radio 3 (x3, y3, z3), and base station radio 4 (x4, y4, z4), to measure the distance of the aircraft 1 to be located, for example, corresponding to d1, d2, d3, and d4. Using the above data, this application can perform positioning, and then use an inertial measurement unit (IMU) to predict the distance between adjacent aircraft at the next time point, and use the predicted distance to verify and correct the distance at the next time point. The specific process is as follows:

[0060] Figure 2 This is a flowchart illustrating a UAV swarm localization method based on Kalman filtering in a satellite-denied environment, as shown in one embodiment of this application. Figure 2 As shown: The UAV swarm localization method based on Kalman filtering in a satellite denial environment of this embodiment may include steps S210 to S240:

[0061] S210, based on multiple ground base stations, locate multiple aircraft to obtain the positioning coordinates of each aircraft at multiple time points; and acquire observation data of each aircraft from multiple sensors at multiple time points, wherein the multiple sensors include inertial sensors;

[0062] In this application, the aircraft obtains the relative distance, or ranging value, by calculating the signal transmission time between the ground base station and the airborne radio. Three-dimensional spatial positioning requires at least four sets of ground base stations for assistance. After obtaining the ranging values ​​between the airborne radio and multiple ground base stations, the approximate position of the aircraft can be determined.

[0063] The approximate location of an aircraft is determined as follows:

[0064] S211, obtain the coordinates of multiple base stations, for example: base station radio 1 (x1, y1, z1), base station radio 2 (x2, y2, z2), base station radio 3 (x3, y3, z3), base station radio 4 (x4, y4, z4);

[0065] S212, ranging of the aircraft is performed on multiple ground base stations at multiple consecutive time points to obtain the distance at multiple consecutive time points; for example, the ranging values ​​of the four base station radios at each time point are d1, d2, d3, and d4 respectively.

[0066] S213, Based on the coordinates of the multiple base stations, the positioning coordinates of the aircraft, and the distances at multiple consecutive time points, construct a set of distance equations for multiple consecutive time points;

[0067] The distance equations are: i is 1, 2, 3, or 4;

[0068] Where (x, y, z) represents the position of the spacecraft.

[0069] S214, the distance equations for each consecutive time point are fitted using the least squares method to obtain the positioning coordinates of the aircraft at multiple time points.

[0070] The specific solution is as follows:

[0071] make r = x 2 +y 2 +z 2

[0072] but

[0073] make

[0074] According to the least squares method, we can obtain...

[0075] P dt =(A T A) -1 A T Y

[0076] The approximate position coordinates P of the aircraft can then be obtained. dt .

[0077] S220, calculate the relative distance between the positioning coordinates of the same group of aircraft at the same time point; and determine the relative position data of each group of aircraft during the flight process based on the relative distance at multiple time points. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft.

[0078] Here, assuming the location coordinates of the same group of aircraft at the same point in time are (x, y, z) and (x', y', z'), then the corresponding relative distance is...

[0079] The reason this application uses relative distance as a benchmark for verification is that each drone in a drone swarm may have positioning errors. By using relative distance as a benchmark for verification, these errors can be detected and corrected in a timely manner, preventing errors from accumulating in the swarm and thus improving overall positioning accuracy.

[0080] Furthermore, drone swarms place greater emphasis on collaborative operations between drones. Therefore, using relative distance as a benchmark can provide a more effective data foundation for subsequent collaborative operations between drones.

[0081] S230 verifies the anomalies of the positioning coordinates of each group of aircraft during flight based on observation data from the inertial sensors of each group of aircraft at multiple time points;

[0082] In this application, given that radio ranging is susceptible to measurement errors and signal multipath propagation, which directly affect the accuracy of the positioning results and the robustness of the system, this application verifies the relative position of the aircraft at a later time point using observation data from inertial sensors at multiple time points from the previous time point, starting from takeoff. This allows for the timely detection of positioning points with significant deviations, facilitating subsequent correction using Kalman filtering. This improves positioning accuracy while reducing the computational load associated with Kalman filtering, specifically including:

[0083] S231, acquire the instantaneous velocity and acceleration of the aircraft at multiple time points, wherein the instantaneous velocity includes a velocity value and a velocity direction, and the instantaneous velocity is obtained by integrating the observation data of the three-axis gyroscope and the accelerometer.

[0084] The inertial detection unit (IMU) of the aircraft in this application includes an accelerometer and a three-axis gyroscope. By integrating the accelerations along the three axes, the instantaneous velocity of the aircraft during flight can be obtained. The accelerations at multiple time points can be directly read from the accelerometer.

[0085] S232, for each group of aircraft, the relative distance at the next time point is predicted based on the instantaneous velocity, acceleration and relative distance at the previous time point, and the predicted distance is obtained;

[0086] In this application, the parameter of interest is the distance between adjacent aircraft. Therefore, it is necessary to analyze the distance changes between adjacent aircraft using instantaneous velocity and acceleration to obtain the predicted distance, specifically including:

[0087] S2321, connect the positioning coordinates of two aircraft in the aircraft group to obtain the reference horizontal axis (h axis), and construct the reference vertical axis (v axis) and reference longitudinal axis (l axis) perpendicular to the reference horizontal axis with one of the aircraft as the origin.

[0088] S2322, the instantaneous velocities and accelerations of the two aircraft are decomposed into the reference horizontal axis, the reference vertical axis, and the reference longitudinal axis, and the differences are calculated for each axis to obtain the initial velocity v on the horizontal axis. h Horizontal accelerator a h Initial velocity v along the vertical axis v Vertical acceleration a v Initial velocity v along the vertical axis l and longitudinal acceleration a l ;

[0089] To facilitate displacement analysis within the aforementioned coordinate system, and considering that the flight direction and acceleration direction of the aircraft do not coincide with the aforementioned reference axes in most cases, an orthogonal decomposition method is adopted. The velocity and acceleration vectors are decomposed into reference horizontal axis (h-axis), reference vertical axis (v-axis), and reference vertical axis (l-axis), thereby obtaining the initial velocity v along the horizontal axis. h Horizontal accelerator a h Initial velocity v along the vertical axis v Vertical acceleration a v Initial velocity v along the vertical axis l and longitudinal acceleration a l .

[0090] S2323, based on the duration T between the previous and next time points, and the initial velocity v of the horizontal axis. h The transverse axis accelerator a h The initial velocity v of the vertical axis v The vertical axis acceleration a v The initial velocity v along the longitudinal axis l and the longitudinal acceleration a l Calculate the relative displacement S of the horizontal axis at the next time point. h Vertical axis relative displacement S v The relative displacement S with respect to the vertical axis l ;

[0091] The interval between the two time points in this application is relatively short, approximately between 0.1 and 0.5 seconds. Therefore, this application treats each displacement as uniformly accelerated motion, and utilizes the initial horizontal velocity v obtained from orthogonal decomposition. h Horizontal accelerator a h Initial velocity v along the vertical axis v Vertical acceleration a v Initial velocity v along the vertical axis l and longitudinal acceleration a l Calculate the relative displacement S along the horizontal axis respectively. h Vertical axis relative displacement S v The relative displacement S with respect to the vertical axis l Wherein, the relative displacement S of the horizontal axis h The relative displacement S with respect to the vertical axis l The mathematical expression is:

[0092]

[0093] S2324, based on the relative displacement S of the horizontal axis h The relative displacement S of the longitudinal axis l Calculate the predicted distance S k+1 ', wherein the predicted distance S k+1 The mathematical expression for ' is:

[0094]

[0095] In the formula, S k This represents the relative distance to the previous point in time.

[0096] Finally, the predicted distance S at the next time point is calculated. k+1 This allows us to obtain the predicted distance between adjacent aircraft from the perspective of the inertial measurement unit (IMU). Since the IMU is not easily affected by noise, performing inertial prediction at each time point can accurately predict the relative displacement state at the next time point and avoid the error accumulation caused by continuously using inertial navigation.

[0097] S233, the relative distance S at the next time point k+1 Distance S from prediction k+1 The comparison is performed, and the difference between the relative distance and the predicted distance at the next time point exceeds a preset threshold S. th When the relative distance at the next time point is determined to be abnormal;

[0098] In |S k+1 -S k+1 '|>S thThis indicates a significant discrepancy between the relative displacement predicted by the inertial observation unit and the relative displacement measured by the ground base station. Given the short-term accuracy of the inertial system, the predicted distance S is used as the benchmark. k+1 'Using this as a benchmark to verify the relative distance S k+1 This allows for the detection of abnormal relative distances.

[0099] S234, filter out the relative distances that are abnormal, and perform anomaly verification on the positioning coordinates of all aircraft based on the abnormal relative distances to obtain the target aircraft with abnormal positioning coordinates.

[0100] Since each relative distance corresponds to two aircraft, it is not possible to directly determine the target aircraft with positioning anomalies. Therefore, this application employs the following process to filter out aircraft with positioning anomalies, including:

[0101] Figure 3 This is a schematic diagram illustrating the discrimination principle of an abnormal aircraft in one embodiment of this application, as shown below. Figure 3 As shown, all aircraft corresponding to abnormal relative distances are marked as candidate aircraft; and the target aircraft and ordinary aircraft are identified by the following discrimination criteria:

[0102] (1) Mark the candidate aircraft corresponding to the relative distances of multiple anomalies as the target aircraft; if an aircraft corresponds to the relative distances of multiple anomalies, then it is highly likely that the aircraft's positioning is abnormal, causing multiple relative distance anomalies with adjacent aircraft. For example, Figure 3 If the t9 positioning is abnormal, it will most likely cause all the relative distances S9-S16 to be abnormal.

[0103] (2) The candidate aircraft that meets the first target condition is identified as a normal aircraft. The first target condition includes: it corresponds to only one abnormal relative distance, and the relative distance of the corresponding abnormality corresponds to the target aircraft.

[0104] For example, Figure 3 If t9 is the target aircraft, then if only S9 is abnormal at t1, but S1 and S8 are not abnormal, then the relative distance abnormality is caused by t9, and t1 is the normal aircraft.

[0105] (3) The candidate aircraft that meets the second target condition is identified as the target aircraft. The second target condition includes: it corresponds to the relative distance of only one anomaly, and the relative distance of the corresponding anomaly does not correspond to the target aircraft.

[0106] For example, Figure 3S1 is abnormal, but there are no other relative distance anomalies in t1 and t2. At this time, it is impossible to determine whether the positioning is abnormal in t1 or t2. Therefore, both t1 and t2 are marked as target aircraft for positioning correction.

[0107] If a candidate aircraft has only one abnormal relative distance with the target aircraft, then the cause of the abnormal relative distance is attributed to the target aircraft. However, if neither of the aircraft causing the abnormal relative distance is the target aircraft, then both are marked as the target aircraft when it is impossible to determine which aircraft caused the abnormal relative distance.

[0108] S240, Kalman filtering is performed based on the positioning coordinates of the target aircraft during the target time period and the observation data of multiple sensors to obtain accurate positioning. The target time period includes multiple time points within a preset time period before the abnormal time point where the positioning coordinates are abnormal.

[0109] After identifying the target aircraft, Kalman filtering is applied to its location coordinates based on the time points where anomalies occurred, resulting in precise positioning, as detailed below:

[0110] S241, Establish process and observation models for the aircraft during flight;

[0111] The process model is as follows:

[0112] x k =f(x) k-1 ,u k-1 ,w k-1 ),w k ~N(0,Q) k )

[0113] The observation model is:

[0114] z k =h(x k ,v k ),v k ~N(0,R k )

[0115] Where x k and x k-1 Let z be the system state variables at time k and time k-1, respectively. k Let f() be the system observations at time k, h() be the nonlinear state transition function, and u be the nonlinear observation function. k-1 Let w be the control vector at time k-1. k For process noise, v k For noise measurement, all noise follows zero-mean Gaussian white noise, N(0,Q) k ) and N(0,R k) represents zero-mean Gaussian white noise.

[0116] S242, Select a target time point from the target time period, and construct the initial state of the target aircraft using the positioning coordinates of the target aircraft at the target time point and the observation data of multiple sensors, and construct an initial covariance matrix based on the initial state of the aircraft;

[0117] Since this application does not perform Kalman filtering on the entire flight process of the target aircraft, but rather starts Kalman filtering from a point in time before the anomaly occurs after the target aircraft's positioning coordinates become abnormal, this application needs to select the most accurate positioning coordinates possible as the initial values ​​for Kalman filtering in order to achieve a quick fit and then exit, thus avoiding prolonged high computational resource consumption.

[0118] In the positioning configuration, the accuracy of positioning is related to the flight attitude and signals of the aircraft during flight. Therefore, this application selects a target time point within a target time period based on the flight attitude and signals, and uses the positioning coordinates of the target time point as the initial value to perform Kalman filtering, thereby enabling the filtering result to fit as quickly as possible and obtain accurate results. Specifically, this includes:

[0119] S2421, acquire the basic stability data of the target aircraft at multiple time points within the target time period, wherein the basic stability data includes the ranging signal strength and the acceleration value on each axis;

[0120] This application collects inertial data, including acceleration values, from the aircraft's inertial observation unit (IMU) at each time point. During positioning, the ranging signal strength is also automatically collected. Therefore, based on these two parameters, the flight attitude and ranging signal of the UAV at each time point are analyzed to determine whether there is any risk to the positioning coordinates.

[0121] S2422, Map the stability baseline data at multiple time points to a two-dimensional coordinate system, wherein the vertical axis of the two-dimensional coordinate system is the data axis and the horizontal axis of the two-dimensional coordinate system is the time axis;

[0122] S2423, based on a pre-constructed sliding window, slide along the time axis, and at each slide, calculate the mean of the ranging signal strength, the variance of the ranging signal strength, the mean of the acceleration, and the variance of the acceleration at multiple time points within the sliding window;

[0123] The sliding window method can collect signal strength characteristics (mean and variance) over a short period of time, as well as flight attitude characteristics (mean and variance of acceleration) over a short period of time, in order to avoid errors in the analysis results caused by data fluctuations at a single point in time.

[0124] S2424, filter out candidate windows where the average ranging signal strength is greater than a preset signal strength threshold and the average acceleration is less than or equal to a preset acceleration threshold;

[0125] S2425, the weighted sum of the variance of the ranging signal strength and the variance of the acceleration is performed to obtain the stability value of each candidate window; the stability value W step The mathematical expression is:

[0126]

[0127] In the formula, step is the sequence number of the candidate window. Let be the variance of the ranging signal strength of the step-th candidate window. The acceleration variance of the step-th candidate window, where α is the first weight and β is the second weight.

[0128] In this embodiment, the strength and stability of the ranging signal, as well as the magnitude and stability of the acceleration, are reflected by the mean, variance, and variance of the ranging signal strength, over a short period of time corresponding to each sliding window movement. First, windows with relatively strong ranging signals and relatively small accelerations are selected as candidate windows. Then, the window with the strongest stability is chosen from these candidate windows, and the overall stability is obtained by weighted summation of the variances of the ranging signal strength and acceleration.

[0129] S2426, the candidate window with the largest stability value is taken as the target window, and the median time point of the target window is taken as the target time point.

[0130] The candidate window with the highest stability corresponds to the lowest ranging bias risk value. Therefore, the time point in this window is selected as the target time point to perform subsequent Kalman filtering.

[0131] S243, Kalman filtering is performed based on the initial state and initial covariance matrix of the target aircraft to obtain the precise positioning of the target aircraft at multiple time points after the target time point.

[0132] This application employs existing Kalman filtering, the steps of which can be summarized as follows:

[0133] The target spacecraft's positioning coordinates, inertial sensor observation parameters, magnetometer and other sensor parameters corresponding to the target time point are used as the initial state for Kalman filtering;

[0134] Set up the system covariance matrix and the observation noise covariance matrix. These matrices are used to describe the uncertainty of the system state and the observation data.

[0135] Based on the estimated UAV motion state from the previous moment, the current UAV state is predicted using the state change matrix. Simultaneously, the system covariance matrix is ​​predicted to reflect the uncertainty of the predicted state.

[0136] Acquire observation data at the current moment, such as GPS positioning information or other sensor data.

[0137] Calculate the Kalman filter gain, which is used to weigh the confidence of the predicted and observed values.

[0138] The estimated state of the UAV is updated by combining the Kalman filter gain and observation data.

[0139] Update the system covariance matrix to reflect the uncertainty of the updated state.

[0140] Assuming the target drone moves at a constant speed over a short period of time, the current motion state matrix of the drone is corrected based on the optimal estimate from the current drone positioning system.

[0141] Repeat the above prediction and update steps, and continuously execute the Kalman filter algorithm to obtain continuous UAV positioning correction results.

[0142] When the deviation between multiple consecutive observations and predicted values ​​does not exceed a preset threshold, the result is considered a good fit, and precise positioning is obtained. The precise positioning time point is uncertain and can be performed in real time during subsequent flights.

[0143] After obtaining precise positioning, the Kalman filtering is exited, and the process returns to step S210. During this process, Kalman filtering is continuously performed on some aircraft for certain time periods. Compared to performing Kalman filtering on all aircraft throughout the entire process, this application can significantly reduce the computational power requirement and has greater adaptability.

[0144] This invention discloses a UAV swarm localization method based on Kalman filtering in satellite-denied environments. This application can be used for UAV swarm localization in satellite-denied environments. Multiple ground base stations are used to first locate multiple aircraft. Since the aircraft operate in swarms, this application uses observation data from the inertial sensors of adjacent aircraft to verify the distance changes between adjacent aircraft. When an anomaly is detected, the calculated anomaly distance is used to quickly identify the abnormal UAV. Then, Kalman filtering is applied to correct the localization of the abnormal UAVs in the affected areas. This application performs Kalman filtering by filtering out anomaly points, thereby achieving accurate localization of aircraft swarms using base stations with lower computing power, and has strong applicability.

[0145] like Figure 4 As shown, this application also provides a UAV swarm positioning device based on Kalman filtering in satellite-denied environments, including:

[0146] The positioning module is used to locate multiple aircraft based on multiple ground base stations, obtain the positioning coordinates of each aircraft at multiple time points, and acquire observation data of each aircraft from multiple sensors at multiple time points, wherein the multiple sensors include inertial sensors.

[0147] The distance calculation module is used to calculate the relative distance between the positioning coordinates of the same group of aircraft at the same point in time; and to determine the relative position data of each group of aircraft during flight based on the relative distance at multiple points in time. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft.

[0148] The anomaly verification module is used to verify the positioning coordinates of each group of aircraft during flight based on the observation data of the inertial sensors of each group of aircraft at multiple time points, and to obtain the target aircraft with abnormal positioning coordinates.

[0149] The positioning correction module is used to perform Kalman filtering based on the positioning coordinates of the target aircraft during the target time period and observation data from multiple sensors to obtain accurate positioning. The target time period includes multiple time points within a preset duration before the abnormal time point where the positioning coordinates are abnormal.

[0150] This invention discloses a UAV swarm positioning device based on Kalman filtering in satellite-denied environments. This application can be used for UAV swarm positioning in satellite-denied environments. Multiple ground base stations are used to first locate multiple aircraft. Since the aircraft operate in swarms, this application uses observation data from the inertial sensors of adjacent aircraft to verify the distance changes between adjacent aircraft. When an anomaly is detected, the calculated anomaly distance is used to quickly identify the abnormal UAV. Then, Kalman filtering is applied to correct the positioning of the abnormal UAVs in the affected areas. This application performs Kalman filtering by filtering out anomaly points, thereby achieving accurate positioning of aircraft swarms using base stations with lower computing power, and has strong applicability.

[0151] This embodiment also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements any one of the methods in this embodiment, wherein the method is the execution logic of this system.

[0152] This embodiment also provides an electronic terminal, including: a processor and a memory;

[0153] The memory is used to store computer programs, and the processor is used to execute the computer programs stored in the memory so that the terminal performs any of the methods in this embodiment.

[0154] As will be understood by those skilled in the art, the computer-readable storage medium described in this embodiment allows for the implementation of all or part of the steps in the above method embodiments by computer program-related hardware. The aforementioned computer program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0155] The electronic terminal provided in this embodiment includes a processor, a memory, a transceiver, and a communication interface. The memory and the communication interface are connected to the processor and the transceiver and complete communication between them. The memory is used to store computer programs, the communication interface is used to perform communication, and the processor and the transceiver are used to run the computer programs, so that the electronic terminal performs the steps of the above method.

[0156] In this embodiment, the memory may include random access memory (RAM) and may also include non-volatile memory, such as at least one disk storage device.

[0157] The processors mentioned above can be general-purpose processors, including central processing units (CPUs), network processors (NPs), etc.; they can also be digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0158] In the above embodiments, although the invention has been described in conjunction with specific embodiments thereof, many substitutions, modifications, and variations of these embodiments will be apparent to those skilled in the art from the foregoing description. The embodiments of the invention are intended to cover all such substitutions, modifications, and variations falling within the broad scope of the appended claims.

[0159] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the invention. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in the present invention should still be covered by the claims of the present invention.

Claims

1. A method for UAV swarm localization based on Kalman filtering in satellite-denied environments, characterized in that, include: Multiple aircraft are located using multiple ground base stations to obtain the location coordinates of each aircraft at multiple time points; and observation data from multiple sensors of each aircraft at multiple time points are acquired, including inertial sensors. Calculate the relative distance between the positioning coordinates of the same group of aircraft at the same time point; and determine the relative position data of each group of aircraft during flight based on the relative distance at multiple time points. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft. Anomaly verification of the positioning coordinates of each group of aircraft during flight is performed based on observation data from the inertial sensors at multiple time points, identifying target aircraft with abnormal positioning coordinates. The inertial sensors include a three-axis gyroscope and an accelerometer. The anomaly verification process for the positioning coordinates of each group of aircraft during flight includes: acquiring the instantaneous velocity and acceleration of the aircraft at multiple time points, where the instantaneous velocity includes a velocity value and velocity direction, obtained by integrating the observation data from the three-axis gyroscope and accelerometer; and directly reading the acceleration at multiple time points using an accelerometer. For each group of aircraft, the relative distance at the next time point is predicted based on the instantaneous velocity, acceleration, and relative distance at the previous time point. The predicted distance is then compared with the predicted distance, and if the difference between the relative distance and the predicted distance exceeds a preset threshold, the relative distance at the next time point is determined to be abnormal. Abnormal relative distances are then selected, and the positioning coordinates of all aircraft are verified based on these abnormal relative distances. Kalman filtering is performed on the target aircraft's positioning coordinates within a target time period and observation data from multiple sensors to obtain precise positioning. The target time period includes multiple time points within a preset duration preceding the abnormal time point where the positioning coordinates are abnormal. The precise positioning process involves: establishing a process model and an observation model of the aircraft during flight; selecting a target time point within the target time period and constructing the initial state of the target aircraft using its positioning coordinates and observation data from multiple sensors at that time point, and constructing an initial covariance matrix based on the initial state; and performing Kalman filtering on the initial state and initial covariance matrix to obtain precise positioning of the target aircraft at multiple time points after the target time point.

2. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 1, characterized in that, The aircraft is located using multiple ground base stations, and its location coordinates at multiple points in time are obtained, including: Obtain the coordinates of multiple base stations; The distance to the aircraft is measured at multiple consecutive time points based on multiple ground base stations to obtain the distance at multiple consecutive time points; Based on the coordinates of the multiple base stations, the positioning coordinates of the aircraft, and the distances at multiple consecutive time points, a set of distance equations for multiple consecutive time points is constructed. The distance equations for each consecutive time point are fitted using the least squares method to obtain the positioning coordinates of the aircraft at multiple time points.

3. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 1, characterized in that, Anomaly verification was performed on the positioning coordinates of all aircraft based on the presence of abnormal relative distances, including: Mark the aircraft corresponding to all abnormal relative distances as candidate aircraft; Candidate aircraft corresponding to the relative distances of multiple anomalies are marked as target aircraft; Candidate aircraft that meet the first objective condition are identified as normal aircraft. The first objective condition includes: corresponding to only one abnormal relative distance, and the relative distance of the corresponding abnormality corresponds to the target aircraft. Candidate aircraft that meet the second objective condition are identified as target aircraft. The second objective condition includes: corresponding to only one anomaly in relative distance, and the relative distance of the corresponding anomaly does not correspond to the target aircraft.

4. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 1, characterized in that, The relative distance at the next time point is predicted based on the instantaneous velocity, acceleration, and relative distance at the previous time point, resulting in the predicted distance, including: Connect the positioning coordinates of two aircraft in the aircraft group to obtain the reference horizontal axis, and construct a reference vertical axis and a reference longitudinal axis perpendicular to the reference horizontal axis with one of the aircraft as the origin; The instantaneous velocities and accelerations of the two aircraft are decomposed into the reference horizontal axis, the reference vertical axis, and the reference longitudinal axis, and the differences between them are calculated to obtain the initial velocity along the horizontal axis. Horizontal axis accelerator Initial velocity of the vertical axis Vertical axis acceleration Initial velocity along the vertical axis and longitudinal acceleration ; Based on the duration of the previous time point and the next time point The initial velocity of the horizontal axis The horizontal axis acceleration The initial velocity of the vertical axis The vertical axis acceleration The initial velocity along the longitudinal axis and the longitudinal axis acceleration Calculate the relative displacement of the horizontal axis at the next time point. Vertical axis relative displacement Displacement relative to the vertical axis ; Based on the relative displacement of the horizontal axis The relative displacement with respect to the longitudinal axis Calculate the predicted distance The predicted distance The mathematical expression is: In the formula, This represents the relative distance to the previous time point.

5. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 1, characterized in that, Selecting a target time point from the target time period includes: Acquire basic stability data of the target aircraft at multiple time points within the target time period, wherein the basic stability data includes ranging signal strength and acceleration values ​​on each axis; The stability baseline data at multiple time points are mapped to a two-dimensional coordinate system, wherein the vertical axis of the two-dimensional coordinate system is the data axis and the horizontal axis is the time axis. Based on a pre-built sliding window, the distance is slid along the time axis, and at each sliding time, the mean of the distance measurement signal strength, the variance of the distance measurement signal strength, the mean of the acceleration, and the variance of the acceleration at multiple time points within the sliding window are calculated. Candidate windows are selected where the average ranging signal strength is greater than a preset signal strength threshold and the average acceleration is less than or equal to a preset acceleration threshold. The stability value of each candidate window is obtained by weighted summation of the variance of ranging signal intensity and the variance of acceleration. The candidate window with the highest stability value is selected as the target window, and the midpoint of the target window is selected as the target time point.

6. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 5, characterized in that, The stability value The mathematical expression is: In the formula, The candidate window number. For the first The variance of the ranging signal strength of each candidate window No. The acceleration variance of each candidate window As the first weight, It is the second weight.

7. The UAV swarm localization method based on Kalman filtering in a satellite-denied environment according to claim 1, characterized in that, Also includes: After obtaining accurate positioning, the Kalman filter is exited, and the system returns to positioning multiple aircraft based on multiple ground base stations.

8. A UAV swarm positioning device based on Kalman filtering in a satellite-denied environment, applied to the UAV swarm positioning method based on Kalman filtering in a satellite-denied environment as described in claim 1, characterized in that, include: The positioning module is used to locate multiple aircraft based on multiple ground base stations, obtain the positioning coordinates of each aircraft at multiple time points, and acquire observation data of each aircraft from multiple sensors at multiple time points, wherein the multiple sensors include inertial sensors. The distance calculation module is used to calculate the relative distance between the positioning coordinates of the same group of aircraft at the same point in time; and to determine the relative position data of each group of aircraft during flight based on the relative distance at multiple points in time. In this process, multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft. An anomaly verification module is used to verify the positioning coordinates of each group of aircraft during flight based on observation data from the inertial sensors at multiple time points, identifying target aircraft with abnormal positioning coordinates. The inertial sensors include a three-axis gyroscope and an accelerometer. The anomaly verification process for each group of aircraft during flight, based on observation data from the inertial sensors at multiple time points, includes: acquiring the instantaneous velocity and acceleration of the aircraft at multiple time points, where the instantaneous velocity includes a velocity value and velocity direction, and is obtained by integrating the observation data from the three-axis gyroscope and accelerometer; for each group of aircraft, predicting the relative distance at the next time point based on the instantaneous velocity, acceleration, and relative distance at the previous time point; comparing the relative distance at the next time point with the predicted distance, and determining that the relative distance at the next time point is abnormal if the difference exceeds a preset threshold; filtering out the abnormal relative distances and performing anomaly verification on the positioning coordinates of all aircraft based on these abnormal relative distances. The positioning correction module is used to perform Kalman filtering based on the positioning coordinates of the target aircraft during a target time period and observation data from multiple sensors to obtain accurate positioning. The target time period includes multiple time points within a preset duration preceding the abnormal time point where the positioning coordinates are abnormal. The process of performing Kalman filtering based on the positioning coordinates of the target aircraft during the target time period and observation data from multiple sensors to obtain accurate positioning includes: establishing a process model and an observation model of the aircraft during flight; selecting a target time point from the target time period, and constructing the initial state of the target aircraft using the positioning coordinates of the target aircraft at the target time point and observation data from multiple sensors, and constructing an initial covariance matrix based on the initial state of the aircraft; and performing Kalman filtering based on the initial state of the target aircraft and the initial covariance matrix to obtain accurate positioning of the target aircraft at multiple time points after the target time point.

Images