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

Abstract

The invention relates to the technical field of positioning, in particular to an unmanned aerial vehicle cluster positioning method and device based on Kalman filtering in a satellite denial environment, which can be applied to unmanned aerial vehicle cluster positioning in the satellite denial environment, and can utilize a plurality of ground base stations to position a plurality of aircrafts first. Therefore, the distance change between the adjacent aircrafts is verified by using the observation data of the inertial sensors of the adjacent aircrafts during clustering, and when the verification is abnormal, the abnormal unmanned aerial vehicle is rapidly determined by using the calculated abnormal distance. And then calling Kalman filtering to carry out positioning correction on the section of the abnormal positioning data of the abnormal unmanned aerial vehicle. According to the method, the Kalman filtering is executed by screening the abnormal points, so that the aircraft cluster can be accurately positioned by using the base station with relatively low computing power, and the method has relatively high applicability.

Description

[0001] This application is a divisional application of a Chinese application with an application date of October 24, 2024, an application number of 202411491316.5, and an invention title of "Positioning Method and Device Based on Multi-Ground Station Ranging Assistance in Satellite Denial Environment". Technical Field

[0002] The present invention relates to the technical field of positioning, and specifically to a method and device for positioning an unmanned aerial vehicle (UAV) cluster based on Kalman filtering in a satellite denial environment. Background Art

[0003] Currently, most aircraft positioning uses the Global Positioning System (GPS) based on satellite positioning. However, in some scenarios, such as environments where satellite signals are interfered with, blocked, or completely denied, traditional navigation systems that rely on GPS or GNSS will fail. Therefore, in the prior art, other methods, such as ground base stations, may be used to position UAVs. However, positioning through ground base stations is easily affected by noise interference, so corresponding filtering algorithms need to be used to eliminate the influence of noise.

[0004] In the prior art, if filtering algorithms are continuously used to eliminate noise in aircraft positioning, it will bring a huge computational burden. Especially in a UAV cluster environment, the computing power of ground base stations is difficult to meet the requirement of continuously using filtering algorithms to eliminate noise in the positioning of UAV clusters. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a method and device for positioning a UAV cluster based on Kalman filtering in a satellite denial environment to solve the problems in the background art.

[0006] To achieve the above purpose, the present invention adopts the following technical solutions:

[0007] The method for positioning a UAV cluster based on Kalman filtering in a satellite denial environment of the present invention includes the steps of:

[0008] Positioning multiple aircraft based on multiple ground base stations to obtain the positioning coordinates of each aircraft at multiple time points; and acquiring the observation data of multiple sensors of each aircraft at multiple time points, where the multiple sensors include inertial sensors;

[0009] Calculating the relative distances of the positioning coordinates of the same group of aircraft at the same time point; and determining the relative position data of each group of aircraft during flight based on the relative distances at multiple time points, where multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft respectively;

[0010] Anomaly verification is performed on 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 the target aircraft with abnormal positioning coordinates is obtained;

[0011] Kalman filtering is performed based on the positioning coordinates of the target aircraft in the target time period and the observation data of multiple sensors to obtain precise positioning. Among them, the target time period includes multiple time points within a preset duration before the abnormal time point where the positioning coordinates are abnormal; Performing Kalman filtering based on the positioning coordinates of the target aircraft in the target time period and the observation data of multiple sensors to obtain precise positioning includes: establishing a process model and an observation model for the aircraft during flight; Selecting a target time point from the target time period, and constructing the initial state of the target aircraft with the positioning coordinates of the target aircraft and the observation data of multiple sensors at the target time point, and constructing an initial covariance matrix based on the initial state of the aircraft; Performing Kalman filtering based on the initial state and the 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.

[0012] In an embodiment of the present application, positioning the aircraft based on multiple ground base stations to obtain the positioning coordinates of the aircraft at multiple time points includes:

[0013] Obtain the coordinates of multiple base stations;

[0014] Ranging the aircraft based on multiple ground base stations at multiple consecutive time points to obtain the distances at multiple consecutive time points;

[0015] Construct distance equations for 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;

[0016] Fit the distance equations for each consecutive time point based on the least squares method to obtain the positioning coordinates of the aircraft at multiple time points.

[0017] In an embodiment of the present application, the inertial sensor includes a three-axis gyroscope and an acceleration sensor. Among them, performing anomaly verification on 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 includes:

[0018] Obtain the instantaneous velocity and acceleration of the aircraft at multiple time points. Among them, 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 acceleration sensor;

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

[0020] Compare the relative distance at the latter time point with the predicted distance, and when the difference between the relative distance at the latter time point and the predicted distance exceeds a preset threshold, determine that the relative distance at the latter time point is abnormal;

[0021] Filter out the relative distances with abnormalities, and perform abnormality verification on the positioning coordinates of all aircraft based on the relative distances with abnormalities.

[0022] In an embodiment of the present application, performing abnormality verification on the positioning coordinates of all aircraft based on the relative distances with abnormalities includes:

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

[0024] Mark the candidate aircraft corresponding to multiple abnormal relative distances as target aircraft;

[0025] Confirm the candidate aircraft that meet the first target condition as normal aircraft, and the first target condition includes: corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance corresponds to the target aircraft;

[0026] Confirm the candidate aircraft that meet the second target condition as target aircraft, and the second target condition includes: corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance does not correspond to the target aircraft either.

[0027] In an embodiment of the present application, predicting the relative distance at the latter time point based on the instantaneous velocity, acceleration, and relative distance at the previous time point to obtain a predicted distance includes:

[0028] Connect the positioning coordinates of two aircraft in the aircraft group to obtain a 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] Decompose the instantaneous velocities and accelerations of the two aircraft into the reference horizontal axis, the reference vertical axis, and the reference longitudinal axis and perform difference calculations respectively to obtain the initial horizontal velocity v h , the horizontal accelerator a h , the initial vertical velocity v v , the vertical acceleration a v , the initial longitudinal velocity v l and the longitudinal acceleration a l ;

[0030] Based on the time duration T between the previous time point and the latter time point, the initial horizontal velocity v h , the horizontal acceleration a h , the initial vertical velocity vv The vertical-axis acceleration a v The initial velocity v of the longitudinal axis l and the longitudinal-axis acceleration a l Calculate the relative displacement S of the horizontal axis at the next time point h The relative displacement S of the vertical axis v and the relative displacement S of the longitudinal axis l ;

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

[0032]

[0033] In the formula, S k is the relative distance at the previous time point.

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

[0035] Obtain the stability basic data of the target aircraft at multiple time points within the target time period, where the stability basic data includes the ranging signal strength and the acceleration value on each axis;

[0036] Map the stability basic data of multiple time points into a two-dimensional coordinate system, where 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;

[0037] Based on a pre-constructed sliding window, slide along the time axis, and calculate the mean value of the ranging signal strength, the variance of the ranging signal strength, the mean value of the acceleration, and the variance of the acceleration within the sliding window each time the window slides;

[0038] Screen out candidate windows where the mean value of the ranging signal strength is greater than a preset signal strength threshold and the mean value of the acceleration is less than or equal to a preset acceleration threshold;

[0039] Perform a weighted sum of the variance of the ranging signal strength and the variance of the acceleration to obtain the stability value of each candidate window;

[0040] Take the candidate window with the maximum stability value as the target window, and take the middle time point of the target window as the target time point.

[0041] In an embodiment of the present application, the mathematical expression of the stability value W step is:

[0042]

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

[0044] In an embodiment of the present application, it further includes:

[0045] After obtaining the precise positioning, the Kalman filter is exited, and the positioning of multiple aircraft based on multiple ground base stations is returned.

[0046] The present application also provides a UAV cluster positioning device based on Kalman filtering in a satellite denial environment, which is characterized by including:

[0047] A positioning module, configured to position multiple aircraft based on multiple ground base stations to obtain the positioning coordinates of each aircraft at multiple time points; and acquire the observation data of multiple sensors of each aircraft at multiple time points, where the multiple sensors include inertial sensors;

[0048] A distance calculation module, configured to 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 distances at multiple time points, where multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft and its adjacent aircraft form an aircraft group respectively;

[0049] An anomaly verification module, configured to perform anomaly verification on 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 to obtain the target aircraft with abnormal positioning coordinates;

[0050] A positioning correction module is used to perform Kalman filtering based on the positioning coordinates of a target aircraft during a target time period and the observation data of multiple sensors to obtain accurate positioning. Among them, the target time period includes multiple time points within a preset duration before an abnormal time point where the positioning coordinates are abnormal; performing Kalman filtering based on the positioning coordinates of a target aircraft during a target time period and the observation data of multiple sensors to obtain accurate positioning includes: establishing a process model and an observation model for the aircraft during flight; selecting a target time point from the target time period, and constructing an initial state of the target aircraft with the positioning coordinates of the target aircraft and the observation data of multiple sensors at the target time point, and constructing an initial covariance matrix based on the initial state of the aircraft; performing Kalman filtering based on the initial state and the initial covariance matrix of the target aircraft to obtain the accurate positioning of the target aircraft at multiple time points after the target time point. The beneficial effects of the present invention are: The method and device for positioning an unmanned aerial vehicle cluster based on Kalman filtering in a satellite denial environment of the present invention can be applied to the positioning of an unmanned aerial vehicle cluster in a satellite denial environment. Multiple ground base stations are used to first position multiple aircraft. Since the aircraft operate in a cluster, the present application uses the observation data of the inertial sensors of adjacent aircraft during clustering to verify the distance change between adjacent aircraft, and when the verification is abnormal, the calculated abnormal distance is used to quickly determine the abnormal unmanned aerial vehicle. Then, Kalman filtering is called to perform positioning correction on the section where the abnormal unmanned aerial vehicle has abnormal positioning data. The present application performs Kalman filtering by screening abnormal points, so as to realize the accurate positioning of the aircraft cluster by a base station with relatively low computing power, and has strong applicability. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0052] Figure 1 It is a usage scenario diagram in an embodiment of the present application;

[0053] Figure 2 It is a flowchart of a method for positioning an unmanned aerial vehicle cluster based on Kalman filtering in a satellite denial environment shown in an embodiment of the present application;

[0054] Figure 3 It is a schematic diagram of the discrimination principle of an abnormal aircraft in an embodiment of the present application;

[0055] Figure 4 It is a structural diagram of a device for positioning an unmanned aerial vehicle cluster based on Kalman filtering in a satellite denial environment shown in an embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0056] The following describes the embodiments of the present invention through specific examples. Those skilled in the art can easily understand the 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. 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, without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0057] It should be noted that the diagrams provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner. Therefore, only the layers related to the present invention are shown in the diagrams, rather than being drawn according to the number, shape, and size of the layers in actual implementation. The type, quantity, and ratio of each layer in actual implementation can be arbitrarily changed, and the layer layout type may also be more complex.

[0058] In the following description, a large number of details are explored to provide a more thorough explanation of the embodiments of the present invention. However, it is obvious to those skilled in the art that the embodiments of the present invention can be implemented without these specific details.

[0059] Figure 1 This is a usage scenario diagram in an embodiment of the present application. As Figure 1 shown, the present application measures the distance to the positioning aircraft 1 through 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), for example, the corresponding d1, d2, d3, and d4. Using the above data, the present application can perform positioning, and then use the 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 of a method for positioning an unmanned aerial vehicle cluster based on Kalman filtering in a satellite denial environment shown in an embodiment of the present application. As Figure 2 shown: The method for positioning an unmanned aerial vehicle cluster based on Kalman filtering in the satellite denial environment of this embodiment may include steps S210 to S240:

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

[0062] In this application, the aircraft obtains the relative distance, that is, the ranging value, by calculating the signal transmission time between the ground base station and the on-board radio station. At least four ground base stations are required for three-dimensional space positioning. After obtaining the ranging values between the on-board radio station and multiple ground base stations, the approximate position of the aircraft can be obtained.

[0063] The method for obtaining the approximate position of the aircraft is 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, Range the aircraft based on multiple ground base stations at multiple consecutive time points to obtain the distances at multiple consecutive time points. For example, within each time point, the ranging values of the 4 base station radios are d1, d2, d3, and d4 respectively;

[0066] S213, Construct a distance equation system 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;

[0067] The distance equation system is: i is 1, 2, 3, 4;

[0068] Among them, (x, y, z) is the position of the aircraft.

[0069] S214, Fit the distance equation system at each consecutive time point based on the least squares method to obtain the positioning coordinates of the aircraft at multiple time points.

[0070] The specific solution is as follows:

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

[0072] Then

[0073] Let

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

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

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

[0077] S220, calculate the relative distance of 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 distances at multiple time points, where multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft respectively;

[0078] Among them, assuming that the positioning coordinates of the same group of aircraft at the same time point are (x, y, z) and (x', y', z') respectively, then the corresponding relative distance is

[0079] The reason why this application uses the relative distance as the basis for verification is that in a drone swarm, each drone may have positioning errors. By verifying with the relative distance as the basis, these errors can be detected and corrected in a timely manner, avoiding the accumulation of errors in the swarm, thereby improving the overall positioning accuracy.

[0080] In addition, in a drone swarm, more emphasis is placed on the collaborative operation between drones. Therefore, using the relative distance as the basis can provide a more effective data foundation for subsequent control of the collaborative operation between drones.

[0081] S230, perform anomaly verification on 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;

[0082] In this application, in view of the fact that the radio ranging process is vulnerable to measurement errors and signal multipath propagation, which directly affects the accuracy of the positioning result and the robustness of the system. Therefore, since takeoff, the observation data of the inertial sensor at the previous time point at multiple time points is used to verify the relative position of the aircraft at the subsequent time point. Thus, positioning points with large deviations can be discovered in a timely manner, so as to be corrected in a timely manner using the Kalman filter subsequently. Thus, while improving the positioning accuracy, the computational complexity brought by the Kalman filter can be reduced, specifically including:

[0083] S231, obtain the instantaneous velocity and acceleration of the aircraft at multiple time points, where the instantaneous velocity includes a velocity value and a velocity direction, and the instantaneous velocity is obtained by integrating the observation data of a three-axis gyroscope and an acceleration sensor;

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

[0085] S232. For each group of aircraft, predict the relative distance at the next time point based on the instantaneous velocity, acceleration, and relative distance at the previous time point to obtain the predicted distance.

[0086] In this application, the parameter of concern is the spacing between adjacent aircraft. Therefore, it is necessary to analyze the change in the distance between adjacent aircraft using the instantaneous velocity and acceleration to obtain the predicted distance, which specifically includes:

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

[0088] S2322. Decompose the instantaneous velocities and accelerations of the two aircraft into the reference horizontal axis, the reference vertical axis, and the reference longitudinal axis and perform subtraction respectively to obtain the initial horizontal velocity v h , the horizontal accelerator a h , the initial vertical velocity v v , the vertical acceleration a v , the initial longitudinal velocity v l and the longitudinal acceleration a l .

[0089] To facilitate displacement analysis in the above coordinate system, since the flight direction and acceleration direction of the aircraft do not coincide with the above reference axes in most cases, the velocity vector and acceleration vector are decomposed into the reference horizontal axis (h-axis), the reference vertical axis (v-axis), and the reference longitudinal axis (l-axis) by means of orthogonal decomposition, so as to obtain the initial horizontal velocity v h , the horizontal accelerator a h , the initial vertical velocity v v , the vertical acceleration a v , the initial longitudinal velocity v l and the longitudinal acceleration a l .

[0090] S2323. Calculate the horizontal relative displacement S h , the vertical relative displacement S h , and the longitudinal relative displacement S v at the next time point based on the time duration T between the previous time point and the next time point, the initial horizontal velocity v v , the horizontal accelerator a l , the initial vertical velocity v l , the vertical acceleration a h , and the longitudinal acceleration a v ; l

[0091] The time interval between two time points in this application is relatively short, approximately between 0.1 and 0.5 seconds. Therefore, in this application, the displacement at each time is regarded as a uniformly accelerated motion, and the initial velocity v of the horizontal axis obtained by orthogonal decomposition is used. h , the horizontal axis accelerator a h , the initial velocity v of the vertical axis v , the vertical axis acceleration a v , the initial velocity v of the longitudinal axis l and the longitudinal axis acceleration a l , and the relative displacement S of the horizontal axis h , the relative displacement S of the vertical axis v and the relative displacement S of the longitudinal axis l are calculated respectively. Among them, the mathematical expressions of the relative displacement S of the horizontal axis h and the relative displacement S of the longitudinal axis l are:

[0092]

[0093] S2324, based on the relative displacement S of the horizontal axis h and the relative displacement S of the longitudinal axis l to calculate the predicted distance S k+1 ', where the mathematical expression of the predicted distance S k+1 ' is:

[0094]

[0095] In the formula, S k is the relative distance at the previous time point.

[0096] Finally, calculate the predicted distance S k+1 ' at the next time point, and the predicted distance between adjacent aircraft from the perspective of the inertial measurement unit can be obtained. Since the inertial measurement unit is not easily affected by noise interference, inertial prediction at each time point can accurately predict the relative displacement state at the next time point and avoid error accumulation caused by continuous use of inertial navigation.

[0097] S233, compare the relative distance S at the next time point k+1 with the predicted distance S k+1 ', and when the difference between the relative distance and the predicted distance at the next time point exceeds the preset threshold S th , it is determined that the relative distance at the next time point is abnormal;

[0098] When |S k+1 - S k+1 '| > S thWhen there is a large deviation between the relative displacement predicted by the inertial observation unit and the relative displacement measured by the ground base station, considering the accuracy of the inertial system in a short period of time, the predicted distance S k+1 ' is used as a reference to verify the relative distance S k+1 . Thus, the relative distance with anomalies can be detected.

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

[0100] Since each relative distance corresponds to two aircraft, it is impossible to directly determine the target aircraft with positioning anomalies at this time. Therefore, the following process is adopted in this application to filter out the aircraft with abnormal positioning, including:

[0101] Figure 3 This is a schematic diagram of the discrimination principle of abnormal aircraft in an embodiment of this application. As Figure 3 shown, mark the aircraft corresponding to all abnormal relative distances as candidate aircraft; and identify the target aircraft and normal aircraft through the following discrimination conditions:

[0102] (1) Mark the candidate aircraft corresponding to multiple abnormal relative distances as target aircraft; if an aircraft corresponds to multiple abnormal relative distances, then it is very likely that the positioning of this aircraft is abnormal, causing multiple relative distance anomalies between it and adjacent aircraft. For example, Figure 3 if the positioning of t9 in

[0103] is abnormal, then it is very likely that all the relative distances S9 - S16 will be abnormal.

[0104] For example, Figure 3 if t9 in

[0105] is the target aircraft, then when only S9 of t1 is abnormal, but S1 and S8 are not abnormal, it is determined that the relative distance anomaly is caused by t9, and t1 is a normal aircraft.

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

[0107] If there is only one abnormal relative distance between a candidate aircraft and the target aircraft, then the cause of the abnormal relative distance is attributed to the target aircraft. And if neither of the aircraft causing an abnormal relative distance is the target aircraft, then when it is impossible to determine which aircraft causes the abnormal relative distance, both of them are marked as target aircraft.

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

[0109] After determining the target aircraft, combine the time point of the abnormal positioning coordinates and perform Kalman filtering on the positioning coordinates of the target aircraft to obtain accurate positioning, specifically as follows:

[0110] S241. Establish a process model and an observation model for the aircraft during flight;

[0111] Among them, the process model is:

[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] Among them, x k and x k-1 are the system state variables at the kth moment and the (k - 1)th moment respectively, and z k is the system observation value at the kth moment. f() is a non-linear state transition function, h() is a non-linear observation function, u k-1 is the control vector at the (k - 1)th moment, w k is the process noise, v k is the measurement noise, all of which follow 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 based on 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 starts Kalman filtering from a certain time point before the abnormal time point after the positioning coordinates of the target aircraft are abnormal, in order to enable the Kalman filtering to fit as soon as possible and then exit, so as to avoid occupying high computing power for a long time. Therefore, this application needs to select as accurate positioning coordinates as possible as the initial value to perform Kalman filtering.

[0118] In the positioning composition, the accuracy of positioning is related to the flight attitude and signals of the aircraft during flight. Therefore, this application selects the target time point in the target time period based on the flight attitude and signals, and uses the positioning coordinates at the target time point as the initial value to perform Kalman filtering, so as to make the filtering result fit as soon as possible and obtain accurate results. Specifically, it includes:

[0119] S2421. Obtain the stability basic data of the target aircraft at multiple time points in the target time period, where the stability basic data includes the ranging signal strength and the acceleration value on each axis.

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

[0121] S2422. Map the stability basic data of multiple time points to a two-dimensional coordinate system, where 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. Slide along the time axis based on a pre-constructed sliding window, and calculate the mean value of the ranging signal strength, the variance of the ranging signal strength, the mean value of the acceleration, and the variance of the acceleration of multiple time points within the sliding window each time it slides.

[0123] Among them, using a sliding window can collect the signal strength characteristics (mean value and variance) within a short period of time, as well as the flight attitude characteristics (mean value and variance of acceleration) within a short period of time, so as to avoid the error of the analysis result caused by the data fluctuation at a single time point.

[0124] S2424, select candidate windows where the mean value of the ranging signal strength is greater than a preset signal strength threshold and the mean value of the acceleration is less than or equal to a preset acceleration threshold;

[0125] S2425, perform weighted summation on the variance of the ranging signal strength and the variance of the acceleration to obtain the stability value of each candidate window; the stability value W step has the following mathematical expression:

[0126]

[0127] where step is the serial number of the candidate window, is the variance of the ranging signal strength of the step-th candidate window, is the variance of the acceleration of the step-th candidate window, α is the first weight, and β is the second weight.

[0128] In this embodiment, the mean value of the ranging signal strength, the variance of the ranging signal strength, the mean value of the acceleration, and the variance of the acceleration corresponding to a small period of time during each sliding of the sliding window are used to reflect the strength and stability of the ranging signal, the magnitude and stability of the acceleration. First, windows with relatively strong ranging signals and relatively small accelerations are screened out to obtain candidate windows. And the window with the strongest stability is selected from the candidate windows, and weighted summation is performed on the variance of the ranging signal strength and the variance of the acceleration to obtain the comprehensive stability.

[0129] S2426, take the candidate window with the maximum stability value as the target window, and take the middle time point of the target window as the target time point.

[0130] For the candidate window with the maximum stability, the corresponding ranging deviation risk value is the smallest. Therefore, the time point in this window is selected as the target time point to perform subsequent Kalman filtering.

[0131] S243, perform Kalman filtering based on the initial state and the 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 adopts the existing Kalman filtering, and its steps can be summarized as:

[0133] Take the positioning coordinates of the target aircraft corresponding to the target time point, the observation parameters of the inertial sensor, and the sensor parameters such as the magnetometer as the initial state of the Kalman filtering;

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

[0135] Based on the estimated motion state of the drone at the previous moment, use the state change matrix to predict the state of the drone at the current moment. At the same time, predict the system covariance matrix to reflect the uncertainty of the predicted state.

[0136] Obtain the 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 balance the credibility of the predicted value and the observed value.

[0138] Combine the Kalman filter gain and the observation data to update the estimated value of the drone state.

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

[0140] Assume that the motion of the target drone is uniform in a short period of time, and correct the current motion state matrix of the drone according to the optimal estimated value of the current drone positioning system.

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

[0142] When the deviation between consecutive multiple observed values and predicted values does not exceed the preset threshold, determine that the result is fitted and obtain accurate positioning. The time point of accurate positioning is uncertain and can be carried out in real time during subsequent flights.

[0143] After obtaining accurate positioning, exit the Kalman filter and return to step S210. During this process, continuously perform the Kalman filter on some time periods of some aircraft. Compared with performing the Kalman filter on all aircraft throughout the process, the present application can reduce a large amount of computing power requirements and has stronger adaptability.

[0144] The method for positioning a drone cluster based on Kalman filter in a satellite denial environment according to the present invention can be applied to the positioning of a drone cluster in a satellite denial environment. First, use multiple ground base stations to position multiple aircraft. Since the aircraft operate in a cluster, 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 the verification is abnormal, uses the calculated abnormal distance to quickly determine the abnormal drone. Then call the Kalman filter to correct the positioning of the section with abnormal positioning data of the abnormal drone. The present application performs the Kalman filter by screening abnormal points, so as to realize the accurate positioning of the aircraft cluster by using a base station with low computing power and has strong applicability.

[0145] As Figure 4 shown, the present application also provides a device for positioning a drone cluster based on Kalman filter in a satellite denial environment, including:

[0146] A positioning module, configured to position multiple aircraft based on multiple ground base stations, obtain the positioning coordinates of each aircraft at multiple time points; and acquire the observation data of multiple types of sensors of each aircraft at multiple time points, wherein the multiple types of sensors include inertial sensors;

[0147] A distance calculation module, configured to calculate the relative distance of 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 distances at multiple time points, wherein multiple aircraft are pre-grouped to obtain multiple aircraft groups, and each aircraft forms an aircraft group with its adjacent aircraft respectively;

[0148] An anomaly verification module, configured to perform anomaly verification on 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 obtain target aircraft with abnormal positioning coordinates;

[0149] A positioning correction module, configured to perform Kalman filtering based on the positioning coordinates of the target aircraft in the target time period and the observation data of multiple types of sensors to obtain accurate positioning, wherein the target time period includes multiple time points within a preset duration before the abnormal time point where the positioning coordinates are abnormal.

[0150] The UAV cluster positioning device based on Kalman filtering in a satellite denial environment of the present invention. This application can be applied to the positioning of UAV clusters in a satellite denial environment. Multiple ground base stations are used to first position multiple aircraft. Since the aircraft operate in a cluster, this application uses the observation data of the inertial sensors of adjacent aircraft during clustering to verify the distance changes between adjacent aircraft, and when the verification is abnormal, the calculated abnormal distance is used to quickly determine the abnormal UAVs. Then, Kalman filtering is called to perform positioning correction on the section where the abnormal UAVs have abnormal positioning data. This application performs Kalman filtering by screening abnormal points, thereby enabling accurate positioning of the aircraft cluster using a base station with relatively low computing power, and having strong applicability.

[0151] This embodiment also provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it 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 a computer program, and the processor is used to execute the computer program stored in the memory so that the terminal executes any one of the methods in this embodiment.

[0154] For the computer-readable storage medium in this embodiment, those of ordinary skill in the art can understand that all or part of the steps for implementing the above method embodiments can be completed by hardware related to a computer program. The aforementioned computer program can be stored in a computer-readable storage medium. When this program is executed, it performs the steps including those of the above method embodiments; and the aforementioned storage medium includes: various media such as ROM, RAM, magnetic disks, or optical discs that can store program codes.

[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 with each other. The memory is used to store a computer program, the communication interface is used for communication, and the processor and the transceiver are used to run the computer program so that the electronic terminal executes each step of the above method.

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

[0157] The above-mentioned processor may be a general-purpose processor, including a central processing unit (Central Processing Unit, abbreviated as CPU), a network processor (Network Processor, abbreviated as NP), etc.; it may also be a digital signal processor (Digital Signal Processing, abbreviated as DSP), an application specific integrated circuit (Application Specific Integrated Circuit, abbreviated as ASIC), a field-programmable gate array (Field-Programmable Gate Array, abbreviated as FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0158] In the above embodiments, although the present invention has been described in combination with specific embodiments of the present invention, according to the previous description, many substitutions, modifications, and variations of these embodiments will be obvious to those of ordinary skill in the art. The embodiments of the present invention are intended to cover all such substitutions, modifications, and variations that fall within the broad scope of the appended claims.

[0159] The above embodiments are only illustrative of the principles and effects of the present invention and are not intended to limit the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those with ordinary knowledge in the technical field without departing from the spirit and technical idea disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. A UAV cluster positioning method based on Kalman filtering in a satellite denial environment, characterized in that: include: Positioning multiple aircraft based on multiple ground base stations to obtain positioning coordinates of each aircraft at multiple time points; and obtaining observation data of multiple sensors of each aircraft at multiple time points, wherein the multiple sensors include inertial sensors; Calculating the relative distances of the positioning coordinates of the same group of aircraft at the same time point; and determining the relative position data of each group of aircraft during the flight process based on the relative distances at multiple time points, wherein the multiple aircraft are grouped in advance to obtain multiple aircraft groups, and each aircraft and its adjacent aircraft respectively form an aircraft group; 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 flight are checked for abnormalities, and the target aircraft with abnormal positioning coordinates are obtained; Kalman filtering is performed based on the positioning coordinates of the target aircraft in a target time period and the observation data of multiple sensors to obtain precise positioning, wherein the target time period includes multiple time points within a preset time period before an abnormal time point where the positioning coordinates are abnormal; Kalman filtering is performed based on the positioning coordinates of the target aircraft in the target time period and the observation data of multiple sensors to obtain precise positioning, including: 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 an initial state of the target aircraft with the positioning coordinates of the target aircraft at the target time point and the observation data of multiple sensors, and constructing an initial covariance matrix based on the initial state of the aircraft; Kalman filtering is performed based on the initial state and the initial covariance matrix of the target aircraft to obtain precise positioning of the target aircraft at multiple time points after the target time point.

2. The positioning method based on multi-ground radio station ranging assistance in a satellite-denied environment according to claim 1 is characterized in that: Positioning an aircraft based on multiple ground base stations to obtain positioning coordinates of the aircraft at multiple time points includes: Get the coordinates of multiple base stations; At multiple consecutive time points, the aircraft is measured based on multiple ground base stations to obtain the distances at the multiple consecutive time points; Constructing a distance equation group for multiple consecutive time points based on the coordinates of the multiple base stations, the positioning coordinates of the aircraft, and the distances of multiple consecutive time points; The distance equation group at each continuous time point is fitted based on the least square method to obtain the positioning coordinates of the aircraft at multiple time points.

3. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 1 is characterized in that: The inertial sensor includes a three-axis gyroscope and an acceleration sensor. The positioning coordinates of each group of aircraft during flight are checked for abnormalities based on the observation data of the inertial sensor of each group of aircraft at multiple time points, including: Obtaining instantaneous speed and acceleration of the aircraft at multiple time points, wherein the instantaneous speed includes a speed value and a speed direction, and the instantaneous speed is obtained based on integrating observation data of a three-axis gyroscope and an acceleration sensor; For each group of aircraft, the relative distance at the next time point is predicted based on the instantaneous speed, acceleration and relative distance at the previous time point to obtain the predicted distance; The relative distance at the next time point is compared with the predicted distance, and when 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; The relative distances with abnormalities are screened out, and the positioning coordinates of all aircraft are checked for abnormalities based on the relative distances with abnormalities.

4. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 3 is characterized in that: Perform anomaly checks on the positioning coordinates of all aircraft based on the relative distances where anomalies exist, including: Mark all aircraft corresponding to abnormal relative distances as candidate aircraft; marking candidate aircraft corresponding to relative distances of the plurality of anomalies as target aircraft; Confirming a candidate aircraft that meets a first target condition as a normal aircraft, wherein the first target condition includes: corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance corresponds to the target aircraft; A candidate aircraft that meets a second target condition is confirmed as a target aircraft, wherein the second target condition includes: corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance does not correspond to the target aircraft either.

5. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 3 is characterized in that: The relative distance at the next time point is predicted based on the instantaneous speed, acceleration and relative distance at the previous time point to obtain the predicted distance, including: Connect the positioning coordinates of two aircraft in the aircraft group to obtain a 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; Decompose the instantaneous speed and acceleration of the two aircraft into the reference horizontal axis, the reference vertical axis and the reference longitudinal axis and calculate the difference respectively to obtain the initial velocity v of the horizontal axis h , horizontal axis acceleratora h , initial velocity of vertical axis v v , vertical axis acceleration a v , initial velocity of the longitudinal axis v l and the longitudinal acceleration a l ; Based on the time length T between the previous time point and the next time point, the horizontal axis initial velocity v h , the horizontal axis acceleration a h , the vertical axis initial velocity v v , the vertical axis acceleration a v , the initial velocity of the longitudinal axis v 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 and the relative displacement S of the longitudinal axis l ; Based on the horizontal axis relative displacement S h and the longitudinal axis relative displacement S l Calculate the prediction distance S k+1 ', where the predicted distance S k+1 The mathematical expression of ' is: In the formula, S k is the relative distance to the previous time point.

6. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 1 is characterized in that: Selecting a target time point from within 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; Mapping the stability basic data of multiple time points into 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; Sliding along the time axis based on a pre-constructed sliding window, and calculating the ranging signal strength mean, ranging signal strength variance, acceleration mean and acceleration variance at multiple time points in the sliding window during each sliding; Filter out candidate windows whose mean ranging signal strength is greater than a preset signal strength threshold and whose mean acceleration is less than or equal to a preset acceleration threshold; The weighted sum of the ranging signal strength variance and the acceleration variance is performed to obtain the stability value of each candidate window; The candidate window with the largest stability value is used as the target window, and the middle time point of the target window is used as the target time point.

7. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 8 is characterized in that: The stability value W step The mathematical expression is: In the formula, step is the sequence number of the candidate window, is the ranging signal strength variance of the step-th candidate window, The acceleration variance of the step-th candidate window, α is the first weight, and β is the second weight.

8. The UAV cluster positioning method based on Kalman filtering in a satellite denial environment according to claim 1 is characterized in that: Also includes: After accurate positioning is obtained, Kalman filtering is exited and the positioning of multiple aircraft based on multiple ground base stations is returned to.

9. UAV cluster positioning device based on Kalman filtering in satellite denial environment, characterized in that: include: A positioning module, used to locate multiple aircraft based on multiple ground base stations, obtain the positioning coordinates of each aircraft at multiple time points; and obtain observation data of multiple sensors of each aircraft at multiple time points, wherein the multiple sensors include inertial sensors; A distance calculation module is used to calculate the relative distances of 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 distances at multiple time points, wherein the multiple aircraft are grouped in advance to obtain multiple aircraft groups, and each aircraft and its adjacent aircraft respectively form an aircraft group; An abnormality checking module is used to perform abnormality checking on 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 obtain the target aircraft with abnormal positioning coordinates; A positioning correction module is used to perform Kalman filtering based on the positioning coordinates of a target time period of a target aircraft and the observation data of multiple sensors to obtain precise positioning, wherein the target time period includes multiple time points within a preset time period before an abnormal time point where the positioning coordinates are abnormal; performing Kalman filtering based on the positioning coordinates of the target time period of the target aircraft and the observation data of multiple sensors to obtain precise positioning, including: 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 an initial state of the target aircraft with the positioning coordinates of the target aircraft at the target time point and the observation data of multiple sensors, and constructing an initial covariance matrix based on the initial state of the aircraft; performing Kalman filtering based on the initial state of the target aircraft and the initial covariance matrix to obtain precise positioning of the target aircraft at multiple time points after the target time point.

Images