The application provides a divisional application aiming at China application with the application date of 2024, the application number of 10, the month and the 24, the application number of 202411491316.5 and the name of positioning method and device based on multi-ground radio station ranging assistance under the satellite refusing environment.
Disclosure of Invention
In view of the above, the present invention is to provide a method and a device for locating unmanned aerial vehicle clusters in a satellite refusing environment, so as to solve the problems in the background art.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
the invention relates to an unmanned aerial vehicle cluster positioning method under a satellite refusing environment, which comprises the following steps:
Positioning a plurality of aircrafts based on a plurality of ground base stations to obtain positioning coordinates of each aircrafts at a plurality of time points, and obtaining observation data of a plurality of sensors of each aircrafts at a plurality of time points, wherein the plurality of sensors comprise inertial sensors;
Determining relative position data of each group of aircrafts in the flying process based on the relative distances of a plurality of time points, wherein the aircrafts are grouped in advance to obtain a plurality of aircrafts, and each aircrafts and the aircrafts adjacent to the aircrafts respectively form an aircrafts group;
Performing anomaly verification on the positioning coordinates of each group of aircrafts in the flight process based on the observation data of the inertial sensors of each group of aircrafts at a plurality of time points to obtain target aircrafts with abnormal positioning coordinates; the inertial sensor comprises a triaxial gyroscope and an acceleration sensor, wherein the anomaly verification is carried out on the positioning coordinates of each group of aircrafts in the flying process based on the observation data of the inertial sensor of each group of aircrafts at a plurality of time points, and the inertial sensor comprises the steps of acquiring the instantaneous speed and the acceleration of each group of aircrafts at the plurality of time points, wherein the instantaneous speed comprises a speed value and a speed direction, the instantaneous speed is obtained by integrating the observation data of the triaxial gyroscope and the acceleration sensor, and the relative distance of the next time point is predicted based on the instantaneous speed, the acceleration and the relative distance of the previous time point to obtain a predicted distance;
and performing Kalman filtering based on the positioning coordinates of a target time period of the target aircraft and the observation data of various sensors to obtain accurate positioning, wherein the target time period comprises a plurality of time points in a preset time period before an abnormal time point when the positioning coordinates are abnormal.
In an embodiment of the present application, positioning an aircraft based on a plurality of ground base stations, to obtain positioning coordinates of the aircraft at a plurality of time points, includes:
acquiring coordinates of a plurality of base stations;
Ranging the aircraft based on the ground base stations at a plurality of continuous time points to obtain the distances of the continuous time points;
Constructing a distance equation set of a plurality of continuous time points based on the coordinates of the plurality of base stations, the positioning coordinates of the aircraft and the distances of the plurality of continuous time points;
Fitting the distance equation set of each continuous time point based on a least square method to obtain positioning coordinates of the aircraft at a plurality of time points.
In an embodiment of the present application, performing anomaly verification on positioning coordinates of all aircraft based on a relative distance in which anomalies exist, includes:
Marking all the aircraft corresponding to the abnormal relative distance as candidate aircraft;
marking candidate aircraft corresponding to the relative distances of the plurality of anomalies as target aircraft;
Identifying a candidate aircraft satisfying a first target condition as a normal aircraft, the first target condition comprising corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance corresponding to the target aircraft;
And identifying the candidate aircraft meeting the second target condition as the target aircraft, wherein the second target condition comprises that the second target condition corresponds to the relative distance of only one anomaly and the corresponding relative distance of the anomaly does not correspond to the target aircraft.
In an embodiment of the present application, predicting a relative distance at a later time point based on an instantaneous speed, an acceleration, and the relative distance at the previous time point to obtain a predicted distance includes:
Connecting the positioning coordinates of two aircrafts in an aircraft group to obtain a reference transverse axis, and constructing a reference vertical axis and a reference longitudinal axis which are perpendicular to the reference transverse axis by taking one of the aircrafts as an origin;
Decomposing the instantaneous speeds and accelerations of two aircrafts into the reference transverse shaft, the reference vertical shaft and the reference vertical shaft, and respectively carrying out difference to obtain a transverse shaft initial speed vh, a transverse shaft accelerator ah, a vertical shaft initial speed vv, a vertical shaft acceleration av, a vertical shaft initial speed vl and a vertical shaft acceleration al;
Calculating a horizontal axis relative displacement amount Sh, a vertical axis relative displacement amount Sv and a vertical axis relative displacement amount Sl at a later time point based on a time period T between a previous time point and the later time point, the horizontal axis initial velocity vh, the horizontal axis acceleration ah, the vertical axis initial velocity vv, the vertical axis acceleration av, the vertical axis initial velocity vl and the vertical axis acceleration al;
Calculating a predicted distance Sk+1 'based on the horizontal axis relative displacement Sh and the vertical axis relative displacement Sl, wherein the mathematical expression of the predicted distance Sk+1' is:
Where Sk is the relative distance from the previous time point.
In an embodiment of the present application, performing kalman filtering based on positioning coordinates of a target time period of a target aircraft and observation data of various sensors to obtain accurate positioning includes:
establishing a process model and an observation model of the aircraft in the flight process;
selecting a target time point from the target time period, constructing an initial state of the target aircraft according to the positioning coordinates of the target aircraft at the target time point and the observation data of various sensors, and constructing an initial covariance matrix based on the initial state of the aircraft;
and performing Kalman filtering based on the initial state and the initial covariance matrix of the target aircraft to obtain accurate positioning of the target aircraft at a plurality of time points after the target time point.
In an embodiment of the present application, selecting a target time point from the target time period includes:
Acquiring stability basic data of the target aircraft at a plurality of time points in the target time period, wherein the stability basic data comprises ranging signal strength and acceleration values on each axis;
Mapping stability basic data of a plurality of time points into a two-dimensional coordinate system, wherein the vertical axis of the two-dimensional coordinate system is a data axis, and the horizontal axis of the two-dimensional coordinate system is a time axis;
Sliding along the time axis based on a pre-constructed sliding window, and calculating a ranging signal intensity mean value, a ranging signal intensity variance, an acceleration mean value and an acceleration variance of a plurality of time points in the sliding window when each sliding;
Screening out candidate windows of which the ranging signal intensity average value is larger than a preset signal intensity threshold value and the acceleration average value is smaller than or equal to a preset acceleration threshold value;
Carrying out weighted summation on the ranging signal intensity variance and the acceleration variance to obtain a stability value of each candidate window;
And taking the candidate window with the maximum stability value as a target window, and taking the middle time point of the target window as a target time point.
In one embodiment of the present application, the mathematical expression of the stability value Wstep is:
where step is the sequence number of the candidate window,Ranging signal strength variance for the step-th candidate window,Acceleration variance of the step-th candidate window, α is a first weight, and β is a second weight.
In an embodiment of the present application, further includes:
after the accurate positioning is obtained, the Kalman filtering is exited, and the method returns to positioning a plurality of aircrafts based on a plurality of ground base stations.
The application also provides an unmanned aerial vehicle cluster positioning device under the satellite refusing environment, which comprises:
The positioning module is used for positioning a plurality of aircrafts based on a plurality of ground base stations to obtain positioning coordinates of each aircrafts at a plurality of time points, and acquiring observation data of a plurality of sensors of each aircrafts at a plurality of time points, wherein the plurality of sensors comprise inertial sensors;
The distance calculation module is used for calculating the relative distance of the positioning coordinates of the same group of aircrafts at the same time point, and determining the relative position data of each group of aircrafts in the flying process based on the relative distances of a plurality of time points, wherein the aircrafts are grouped in advance to obtain a plurality of aircraft groups, and each aircraft and the aircrafts adjacent to the aircraft groups respectively form the aircraft groups;
The anomaly verification module is used for carrying out anomaly verification on the positioning coordinates of each group of aircrafts in the flight process based on the observation data of the inertial sensors of each group of aircrafts at a plurality of time points to obtain target aircrafts with abnormal positioning coordinates; the inertial sensor comprises a triaxial gyroscope and an acceleration sensor, wherein the anomaly verification is carried out on the positioning coordinates of each group of aircrafts in the flying process based on the observation data of the inertial sensor of each group of aircrafts at a plurality of time points, and the inertial sensor comprises the steps of acquiring the instantaneous speed and the acceleration of each group of aircrafts at the plurality of time points, wherein the instantaneous speed comprises a speed value and a speed direction, the instantaneous speed is obtained by integrating the observation data of the triaxial gyroscope and the acceleration sensor, and the relative distance of the next time point is predicted based on the instantaneous speed, the acceleration and the relative distance of the previous time point to obtain a predicted distance;
And the positioning correction module is used for performing Kalman filtering based on the positioning coordinates of a target time period of the target aircraft and the observation data of various sensors to obtain accurate positioning, wherein the target time period comprises a plurality of time points in a preset time period before an abnormal time point when the positioning coordinates are abnormal.
The unmanned aerial vehicle cluster positioning method and device under the satellite refusing environment have the advantages that the unmanned aerial vehicle cluster positioning method and device under the satellite refusing environment can be applied to unmanned aerial vehicle cluster positioning under the satellite refusing environment, the plurality of ground base stations are used for positioning the plurality of aircrafts first, and because the aircrafts adopt cluster operation, the unmanned aerial vehicle cluster positioning method and device utilize the observation data of the inertial sensors of the adjacent aircrafts during the cluster to check the distance change between the adjacent aircrafts, and utilize the calculated abnormal distance to quickly determine abnormal unmanned aerial vehicles when the calibration is abnormal. And then, the Kalman filtering is called to carry out positioning correction on the section of the abnormal unmanned aerial vehicle where the abnormal positioning data appear. The application performs Kalman filtering by screening the abnormal points, thereby realizing accurate positioning of the aircraft cluster by using the base station with lower calculation force and having stronger applicability.
Detailed Description
Other advantages and effects of the present invention will become apparent to those skilled in the art from the following disclosure, which describes the embodiments of the present invention with reference to specific examples. The invention may be practiced or carried out in other embodiments that depart from the specific details, and the details of the present description may be modified or varied from the spirit and scope of the present invention. It should be noted that the following embodiments and features in the embodiments may be combined with each other without conflict.
It should be noted that the illustrations provided in the following embodiments merely illustrate the basic concept of the present invention by way of illustration, and only the layers related to the present invention are shown in the drawings and are not drawn according to the number, shape and size of the layers in actual implementation, and the form, number and proportion of the layers in actual implementation may be arbitrarily changed, and the layer layout may be more complex.
In the following description, numerous details are discussed to provide a more thorough explanation of embodiments of the present invention, however, it will be apparent to one skilled in the art that embodiments of the present invention may be practiced without these specific details.
Fig. 1 is a view of an operation scenario in an embodiment of the present application, as shown in fig. 1, the present application measures distance of an aircraft 1 to be positioned, such as d1, d2, d3, d4, by a plurality of ground base stations, such as base station 1 (x 1, y1, z 1), base station 2 (x 2, y2, z 2), base station 3 (x 3, y3, z 3), and base station 4 (x 4, y4, z 4). The application can be used for positioning by utilizing the data, then the inertial measurement unit IMU is used for predicting the distance between adjacent aircrafts at the next time point, and the predicted distance is used for verifying and correcting the distance between the next time point, and the specific process is as follows:
Fig. 2 is a flowchart of a method for positioning a cluster of unmanned aerial vehicles in a satellite-repellent environment according to an embodiment of the present application, as shown in fig. 2, the method for positioning a cluster of unmanned aerial vehicles in a satellite-repellent environment according to the present embodiment may include steps S210 to S240:
s210, positioning a plurality of aircrafts based on a plurality of ground base stations to obtain positioning coordinates of each aircrafts at a plurality of time points, and obtaining observation data of a plurality of sensors of each aircrafts at a plurality of time points, wherein the plurality of sensors comprise inertial sensors;
in the present application the aircraft obtains the relative distance, i.e. the ranging value, by calculating the signal transfer time between the ground base station and the airborne station. The three-dimensional space positioning at least needs four groups of ground base stations for assistance, and the approximate position of the aircraft can be obtained after the ranging values between the airborne radio station and the ground base stations are obtained.
The approximate position of the aircraft is found as follows:
s211, acquiring coordinates of a plurality of base stations, such as base station 1 (x 1, y1, z 1), base station 2 (x 2, y2, z 2), base station 3 (x 3, y3, z 3), and base station 4 (x 4, y4, z 4);
S212, ranging the aircraft based on a plurality of ground base stations at a plurality of continuous time points to obtain the distances of the plurality of continuous time points, wherein the ranging values of 4 base station stations are d1, d2, d3 and d4 respectively in each time point;
s213, constructing a distance equation set of a plurality of continuous time points based on the coordinates of the plurality of base stations, the positioning coordinates of the aircraft and the distances of the plurality of continuous time points;
The distance equation set is: i is 1, 2, 3, 4;
where (x, y, z) is the position of the aircraft.
And S214, fitting a distance equation set of each continuous time point based on a least square method to obtain positioning coordinates of the aircraft at a plurality of time points.
The concrete solution is as follows:
Order ther=x2+y2+z2
Then
Order the
Is obtained according to the least square method
Pdt＝(ATA)-1ATY
The coarse position coordinates Pdt of the aircraft can be obtained.
S220, calculating the relative distance of the positioning coordinates of the same group of aircrafts at the same time point, and determining the relative position data of each group of aircrafts in the flying process based on the relative distances of a plurality of time points, wherein the aircrafts are grouped in advance to obtain a plurality of aircrafts groups, and each aircraft and the aircrafts adjacent to the aircrafts respectively form the aircrafts groups;
Wherein, assuming that the positioning coordinates of the same group of aircrafts at the same time point are (x, y, z), (x ', y ', z '), the corresponding relative distance is
The present application uses relative distance as a reference for verification because there may be a positioning error for each drone in the drone cluster. By checking with the relative distance as a reference, the errors can be found and corrected in time, and the errors are prevented from accumulating in the cluster, so that the overall positioning accuracy is improved.
In addition, the unmanned aerial vehicles in the unmanned aerial vehicle cluster are more focused on the cooperative work among the unmanned aerial vehicles, so that the adoption of the relative distance as a reference can provide a more effective data base for the subsequent control of the cooperative work among the unmanned aerial vehicles.
S230, carrying out anomaly verification on the positioning coordinates of each group of aircrafts in the flying process based on the observation data of the inertial sensors of each group of aircrafts at a plurality of time points;
In the application, in view of the fact that the radio ranging process is susceptible to measurement errors and signal multipath propagation, the accuracy of a positioning result and the robustness of a system are directly influenced. Therefore, the application starts from taking off, and utilizes the observation data of the inertial sensor at the previous time point at a plurality of time points to verify the relative position of the aircraft at the later time point. Therefore, locating points with larger deviation can be found in time, so that the follow-up correction can be conveniently carried out in time by utilizing Kalman filtering. Therefore, the calculation amount brought by Kalman filtering can be reduced while the positioning accuracy is improved, and the method specifically comprises the following steps:
S231, acquiring instantaneous speeds and accelerations of the aircraft at a plurality of time points, wherein the instantaneous speeds comprise speed values and speed directions, and the instantaneous speeds are obtained by integrating observation data of a triaxial gyroscope and an acceleration sensor;
The inertial detection unit IMU in the aircraft comprises an accelerometer and a triaxial gyroscope, and the acceleration on three axes is integrated so as to obtain the instantaneous speed of the aircraft in the flight process. Whereas the acceleration at a plurality of time points can be read directly by the accelerometer.
S232, for each group of aircrafts, predicting the relative distance of the next time point based on the instantaneous speed, the acceleration and the relative distance of the previous time point to obtain a predicted distance;
In the present application, the parameter of interest is the distance between adjacent aircrafts, so the change of the distance between adjacent aircrafts needs to be analyzed by using the instantaneous speed and the acceleration to obtain the predicted distance, which specifically includes:
S2321, connecting positioning coordinates of two aircrafts in an aircraft group to obtain a reference transverse axis (h axis), and constructing a reference vertical axis (v axis) and a reference longitudinal axis (l axis) which are perpendicular to the reference transverse axis by taking one of the aircrafts as an origin;
S2322, decomposing the instantaneous speeds and the accelerations of the two aircrafts into the reference transverse shaft, the reference vertical shaft and the reference vertical shaft, and respectively obtaining a transverse shaft initial speed vh, a transverse shaft accelerator ah, a vertical shaft initial speed vv, a vertical shaft acceleration av, a vertical shaft initial speed vl and a vertical shaft acceleration al;
In order to facilitate displacement analysis in the coordinate system, the flight direction and the acceleration direction of the aircraft do not overlap with the reference axis in most cases, so that the velocity vector and the acceleration vector are decomposed into a reference horizontal axis (h axis), a reference vertical axis (v axis), and a reference vertical axis (l axis) by orthogonal decomposition, thereby obtaining a horizontal axis initial velocity vh, a horizontal axis accelerator ah, a vertical axis initial velocity vv, a vertical axis acceleration av, a vertical axis initial velocity vl, and a vertical axis acceleration al.
S2323, calculating a transverse axis relative displacement amount Sh, a vertical axis relative displacement amount Sv and a vertical axis relative displacement amount Sl of the later time point based on a time period T of the former time point and the later time point, the transverse axis initial speed vh, the transverse axis accelerator ah, the vertical axis initial speed vv, the vertical axis acceleration av, the vertical axis initial speed vl and the vertical axis acceleration al;
The interval between two time points in the application is relatively short and is approximately between 0.1 and 0.5 seconds, so that each displacement is regarded as uniform acceleration movement, and the transverse axis relative displacement Sh, the vertical axis relative displacement Sv and the longitudinal axis relative displacement Sl are calculated by utilizing the transverse axis initial speed vh, the transverse axis accelerator ah, the vertical axis initial speed vv, the vertical axis acceleration av, the vertical axis initial speed vl and the vertical axis acceleration al which are obtained by orthogonal decomposition, wherein the mathematical expressions of the transverse axis relative displacement Sh and the longitudinal axis relative displacement Sl are as follows:
S2324, calculating a predicted distance Sk+1 'based on the horizontal axis relative displacement Sh and the vertical axis relative displacement Sl, wherein the mathematical expression of the predicted distance Sk+1' is:
Where Sk is the relative distance from the previous time point.
Finally, the predicted distance Sk+1' of the next time point is calculated, and the predicted distance of the adjacent aircraft from the inertial measurement unit angle can be obtained. Since the inertial measurement unit is not susceptible to noise interference, performing inertial prediction at each time point can accurately predict the relative displacement state at the next time point, and error accumulation caused by constant use of inertial navigation can be avoided.
S233, comparing the relative distance Sk+1 of the later time point with the predicted distance Sk+1', and judging that the relative distance of the later time point is abnormal when the difference value between the relative distance of the later time point and the predicted distance exceeds a preset threshold value Sth;
At |sk+1-Sk+1′|>Sth, it is explained that the relative displacement amount predicted by the inertial measurement unit is greatly deviated from the relative displacement amount measured by the ground base station, and the relative distance Sk+1 is verified based on the predicted distance Sk+1' in view of the accuracy of the inertial system in a short time. Thereby verifying the relative distance of the abnormality.
S234, screening out the relative distances with the anomalies, and performing anomaly verification on the positioning coordinates of all the aircrafts based on the relative distances with the anomalies to obtain the target aircrafts with the anomalies in the positioning coordinates.
Because each relative distance corresponds to two aircrafts, and the target aircraft with abnormal positioning cannot be directly determined at the moment, the application adopts the following process to screen out the aircrafts with abnormal positioning, and the method comprises the following steps:
Fig. 3 is a schematic diagram of a discrimination principle of an abnormal aircraft in an embodiment of the present application, as shown in fig. 3, the aircraft corresponding to the relative distances of all the anomalies are marked as candidate aircraft, and the target aircraft and the common aircraft are identified by the following discrimination conditions:
(1) If one aircraft corresponds to the relative distance of a plurality of anomalies, then the aircraft is more likely to be positioned abnormally, and the plurality of relative distances between the aircraft and adjacent aircraft are abnormal. For example, if the positioning of t9 in FIG. 3 is abnormal, then a high probability will cause the relative distances S9-S16 to all be abnormal.
(2) Identifying a candidate aircraft satisfying a first target condition as a normal aircraft, the first target condition comprising corresponding to only one abnormal relative distance, and the corresponding abnormal relative distance corresponding to the target aircraft;
for example, t9 in fig. 3 is a target aircraft, and when there is only S9 abnormality at t1, but there is no abnormality in S1 and S8, it is determined that the relative distance due to t9 is abnormal, and t1 is a normal aircraft.
(3) And identifying the candidate aircraft meeting the second target condition as the target aircraft, wherein the second target condition comprises that the second target condition corresponds to the relative distance of only one anomaly and the corresponding relative distance of the anomaly does not correspond to the target aircraft.
For example, in fig. 3, S1 is abnormal, but t1 and t2 are not abnormal in other relative distances, and at this time, it cannot be determined whether t1 or t2 is abnormal in positioning, so that both t1 and t2 are marked as target aircraft for positioning correction.
If there is only one abnormal relative distance between a candidate aircraft and the target aircraft, the reason for the abnormal relative distance is generalized to the target aircraft. If none of the relative distances that caused an anomaly are the target aircraft, then both are marked as target aircraft if it is not possible to determine which aircraft caused the relative distance anomaly.
And S240, performing Kalman filtering based on the positioning coordinates of a target time period of the target aircraft and the observation data of various sensors to obtain accurate positioning, wherein the target time period comprises a plurality of time points in a preset time period before an abnormal time point when the positioning coordinates are abnormal.
After determining the target aircraft, combining the time points with abnormal positioning coordinates, and performing Kalman filtering on the positioning coordinates of the target aircraft to obtain accurate positioning, wherein the method comprises the following steps of:
S241, establishing a process model and an observation model of the aircraft in the flight process;
Wherein, the process model is:
xk＝f(xk-1,uk-1,wk-1),wk～N(0,Qk)
the observation model is as follows:
zk＝h(xk,vk),vk～N(0,Rk)
where xk and xk-1 are the system state variables at time k and time k-1, respectively, and zk is the system observations at time k. f () is a nonlinear state transfer function, h () is a nonlinear observation function, uk-1 is a control vector at time k-1, wk is process noise, vk is measurement noise, all obey zero-mean gaussian white noise, and N (0, qk) and N (0, rk) represent zero-mean gaussian white noise.
S242, selecting a target time point from the target time period, constructing an initial state of the target aircraft according to the positioning coordinates of the target aircraft at the target time point and the observation data of various sensors, and constructing an initial covariance matrix based on the initial state of the aircraft;
Because the application does not carry out Kalman filtering on the whole 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, so that the Kalman filtering can be fitted as soon as possible and then exits. So as to avoid high calculation force for a long time. Therefore, the present application needs to select as accurate a positioning coordinate as possible as an initial value to perform kalman filtering.
In the positioning structure, the accuracy of positioning is related to the flight attitude and the signal of the aircraft in the flight process, so the application selects a target time point in a target time period based on the flight attitude and the signal, and carries out Kalman filtering by taking the positioning coordinates of the target time point as initial values, thereby enabling the filtering result to be fitted as soon as possible and obtaining an accurate result, and the application specifically comprises the following steps:
S2421, acquiring stability basic data of the target aircraft at a plurality of time points in the target time period, wherein the stability basic data comprises ranging signal intensity and acceleration value on each axis;
The application collects inertial data of the inertial observation unit IMU of the aircraft at each time point, including acceleration values. During positioning, the ranging signal intensity can be automatically acquired, so that the flight attitude and the ranging signal of the unmanned aerial vehicle at each time point are analyzed based on the two parameters, and whether the positioning coordinates have risks is judged.
S2422, mapping stability basic data of a plurality of time points into a two-dimensional coordinate system, wherein the vertical axis of the two-dimensional coordinate system is a data axis, and the horizontal axis of the two-dimensional coordinate system is a time axis;
s2423, sliding along the time axis based on a pre-constructed sliding window, and calculating a ranging signal intensity mean value, a ranging signal intensity variance, an acceleration mean value and an acceleration variance of a plurality of time points in the sliding window when each sliding;
The sliding window is adopted to collect signal intensity characteristics (mean value and variance) in a short period of time and flight attitude characteristics (acceleration mean value and variance) in a short period of time, so that analysis result errors caused by data fluctuation at a single time point are avoided.
S2424, screening out candidate windows of which the ranging signal intensity mean value is larger than a preset signal intensity threshold value and the acceleration mean value is smaller than or equal to the preset acceleration threshold value;
S2425, carrying out weighted summation on the ranging signal intensity variance and the acceleration variance to obtain a stability value of each candidate window, wherein the mathematical expression of the stability value Wstep is as follows:
where step is the sequence number of the candidate window,Ranging signal strength variance for the step-th candidate window,Acceleration variance of the step-th candidate window, α is a first weight, and β is a second weight.
In this embodiment, the intensity and stability of the ranging signal, the magnitude and stability of the acceleration are reflected by using the ranging signal intensity mean value, the ranging signal intensity variance, the acceleration mean value and the acceleration variance in a short period of time corresponding to each sliding of the sliding window. Firstly, windows with stronger ranging signals and smaller acceleration are screened out, and candidate windows are obtained. And selecting a window with the strongest stability from the candidate windows, and carrying out weighted summation on the ranging signal intensity variance and the acceleration variance to obtain the comprehensive stability.
S2426, the candidate window with the largest stability value is taken as the target window, and the middle time point of the target window is taken as the target time point.
The candidate window with the greatest stability, the corresponding ranging deviation risk value is the smallest, so the time point in this window is selected as the target time point to execute the subsequent kalman filtering.
S243, performing Kalman filtering based on the initial state and the initial covariance matrix of the target aircraft to obtain accurate positioning of the target aircraft at a plurality of time points after the target time point.
The application adopts the existing Kalman filtering, and the steps can be summarized as follows:
taking the positioning coordinates of the target aircraft corresponding to the target time point, the observation parameters of the inertial sensor, the sensor parameters such as the magnetometer and the like as the initial state of Kalman filtering;
A system covariance matrix and an observation noise covariance matrix are set, and the matrices are used for describing the uncertainty of the system state and the observation data.
And predicting the unmanned aerial vehicle state at the current moment by using a state change matrix according to the unmanned aerial vehicle motion state estimation at the previous moment. And simultaneously, predicting a system covariance matrix to reflect uncertainty of a prediction state.
And acquiring observation data at the current moment, such as GPS positioning information or other sensor data.
A kalman filter gain is calculated that is used to trade-off the confidence of the predicted and observed values.
And updating the estimated value of the unmanned aerial vehicle state by combining the Kalman filtering gain and the observed data.
The system covariance matrix is updated to reflect the uncertainty of the updated state.
And assuming that the motion of the target unmanned aerial vehicle in a short time is uniform motion, correcting the current motion state matrix of the unmanned aerial vehicle according to the optimal estimated value of the current unmanned aerial vehicle positioning system.
Repeating the steps of predicting and updating, and continuously and iteratively executing a Kalman filtering algorithm to obtain a continuous unmanned aerial vehicle positioning correction result.
And when the deviation between the continuous multiple observed values and the predicted value does not exceed a preset threshold value, fitting the judgment result, and obtaining accurate positioning. The point in time of the precise positioning is uncertain and can be performed in real time during the subsequent flight.
After the accurate positioning is obtained, the kalman filtering is exited, and the process returns to step S210. In the process, the Kalman filtering is continuously carried out on part of time periods of part of the aircrafts, and compared with the Kalman filtering carried out on all aircrafts in the whole process, the Kalman filtering method can reduce a large amount of calculation force requirements and has stronger adaptability.
The unmanned aerial vehicle cluster positioning method under the satellite refusing environment can be applied to unmanned aerial vehicle cluster positioning under the satellite refusing environment, a plurality of ground base stations are utilized to position a plurality of aircrafts, and because the aircrafts adopt cluster operation, the unmanned aerial vehicle cluster positioning method utilizes the observation data of the inertial sensors of the adjacent aircrafts during the cluster to check the distance change between the adjacent aircrafts, and utilizes the calculated abnormal distance to quickly determine the abnormal unmanned aerial vehicle when the check is abnormal. And then, the Kalman filtering is called to carry out positioning correction on the section of the abnormal unmanned aerial vehicle where the abnormal positioning data appear. The application performs Kalman filtering by screening the abnormal points, thereby realizing accurate positioning of the aircraft cluster by using the base station with lower calculation force and having stronger applicability.
As shown in fig. 4, the present application further provides an unmanned aerial vehicle cluster positioning device in a satellite rejection environment, including:
The positioning module is used for positioning a plurality of aircrafts based on a plurality of ground base stations to obtain positioning coordinates of each aircrafts at a plurality of time points, and acquiring observation data of a plurality of sensors of each aircrafts at a plurality of time points, wherein the plurality of sensors comprise inertial sensors;
The distance calculation module is used for calculating the relative distance of the positioning coordinates of the same group of aircrafts at the same time point, and determining the relative position data of each group of aircrafts in the flying process based on the relative distances of a plurality of time points, wherein the aircrafts are grouped in advance to obtain a plurality of aircraft groups, and each aircraft and the aircrafts adjacent to the aircraft groups respectively form the aircraft groups;
The anomaly verification module is used for carrying out anomaly verification on the positioning coordinates of each group of aircrafts in the flight process based on the observation data of the inertial sensors of each group of aircrafts at a plurality of time points to obtain target aircrafts with abnormal positioning coordinates;
And the positioning correction module is used for performing Kalman filtering based on the positioning coordinates of a target time period of the target aircraft and the observation data of various sensors to obtain accurate positioning, wherein the target time period comprises a plurality of time points in a preset time period before an abnormal time point when the positioning coordinates are abnormal.
The unmanned aerial vehicle cluster positioning device under the satellite refusing environment can be applied to unmanned aerial vehicle cluster positioning under the satellite refusing environment, a plurality of ground base stations are utilized to position a plurality of aircrafts, and because the aircrafts adopt cluster operation, the unmanned aerial vehicle cluster positioning device utilizes the observation data of the inertial sensors of the adjacent aircrafts during the cluster to check the distance change between the adjacent aircrafts, and utilizes the calculated abnormal distance to quickly determine the abnormal unmanned aerial vehicle when the check is abnormal. And then, the Kalman filtering is called to carry out positioning correction on the section of the abnormal unmanned aerial vehicle where the abnormal positioning data appear. The application performs Kalman filtering by screening the abnormal points, thereby realizing accurate positioning of the aircraft cluster by using the base station with lower calculation force and having stronger applicability.
The present embodiment also provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements any of the methods of the present embodiments, wherein the method is the execution logic of the present system.
The embodiment also provides an electronic terminal, which comprises a processor and a memory;
The memory is used for storing a computer program, and the processor is used for executing the computer program stored in the memory, so that the terminal executes any one of the methods in the embodiment.
The computer readable storage medium of the present embodiment, those of ordinary skill in the art will appreciate that all or part of the steps of implementing the above-described method embodiments may be implemented by computer program related hardware. The aforementioned computer program may be stored in a computer readable storage medium. The program, when executed, performs the steps comprising the method embodiments described above, and the storage medium described above includes various media capable of storing program code, such as ROM, RAM, magnetic or optical disk.
The electronic terminal provided in this embodiment includes a processor, a memory, a transceiver, and a communication interface, where 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 to perform communication, and the processor and the transceiver are used to run the computer program, so that the electronic terminal performs each step of the above method.
In this embodiment, the memory may include a random access memory (Random Access Memory, abbreviated as RAM), and may further include a non-volatile memory (non-volatile memory), such as at least one disk memory.
The processor may be a general-purpose processor, including a central Processing unit (CentralProcessing Unit, CPU), a network processor (Network Processor, NP), a digital signal processor (DIGITAL SIGNAL Processing, DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable gate array (FPGA) or other Programmable logic device, discrete gate or transistor logic device, or discrete hardware components.
In the above embodiments, while the present invention has been described in conjunction with specific embodiments thereof, many alternatives, modifications and variations of these embodiments will be apparent to those skilled in the art in light of the foregoing description. The embodiments of the invention are intended to embrace all such alternatives, modifications and variances which fall within the broad scope of the appended claims.
The above embodiments are merely illustrative of the principles of the present invention and its effectiveness, and are not intended to limit the invention. Modifications and variations may be made to the above-described embodiments by those skilled in the art without departing from the spirit and scope of the invention. Accordingly, it is intended that all equivalent modifications and variations of the invention be covered by the claims, which are within the ordinary skill of the art, be within the spirit and scope of the present disclosure.