Quadrotor unmanned aerial vehicle positioning system in GNSS denial environment

Abstract

A quad-rotor unmanned aerial vehicle is high in maneuverability and wide in application, meter-scale positioning can be provided outdoors in combination with a GNSS, but positioning fails in complex environments such as tunnels. The UWB technology is anti-interference and low in cost, can provide positioning information in a complex environment, but needs to optimize robustness and fusion efficiency. The invention provides a quad-rotor unmanned aerial vehicle positioning system in a GNSS (Global Navigation Satellite System) denial environment in order to solve the positioning problem of a quad-rotor unmanned aerial vehicle in a satellite denial environment. The construction of the system comprises the following steps of: carrying out IIR (Infinite Impulse Response) low-pass filtering preprocessing on IMU data; calibrating UWB anchor point distance measurement by using a linear regression method; calibrating UWB anchor point positions based on a nonlinear least square method; constructing an unscented Kalman filter model containing IMU prediction, UWB and radar updating; uWB outliers are removed through double criterion detection, and the six-degree-of-freedom pose is output by fusing multi-source data. According to the method, real-time centimeter-level calculation of the six-degree-of-freedom pose of the unmanned aerial vehicle can be realized with low-cost hardware in a GNSS denial environment, and reliable estimation is provided for autonomous navigation of a complex scene.

Description

Technical Field

[0001] The present invention relates to the fields of unmanned aerial vehicle (UAV) navigation and control, embedded systems, and sensor networks, and particularly to a positioning and navigation system for a small quadrotor UAV, which solves the problem of UAV positioning based on multi-sensor fusion in a GNSS (Global Navigation Satellite System) denied environment. Background Art

[0002] In recent years, small quadrotor UAVs have become one of the hottest research topics due to their high mobility and overlooking perspective, and have been gradually applied to daily military and civilian fields, such as search and rescue, real-time mapping, and enemy surveillance. Outdoor positioning and navigation technologies for UAVs have been basically mature. In outdoor environments, the GNSS global positioning system is mostly used to provide position navigation for UAVs, and the positioning accuracy can reach the meter level. In the case of deploying a fixed differential base station, the positioning accuracy of the UAV can reach the centimeter level. However, in scenarios such as tunnels, jungles, and indoors, due to non-line-of-sight measurements of GNSS, the positioning error is extremely large. Therefore, the positioning and navigation problem of UAVs in a GNSS denied environment remains a challenge.

[0003] Currently, in a GNSS denied environment, solutions that can provide position information for UAVs, such as indoor positioning systems, can achieve centimeter-level positioning accuracy by deploying multiple positioning cameras indoors. However, the disadvantages are high cost and limited coverage. The visual SLAM solution uses an on-board monocular / binocular camera to fuse devices such as an IMU to reconstruct the surrounding environment structure and perform autonomous positioning. The disadvantages are complex algorithms, high computing power requirements for the on-board computer, and cumulative error problems over time. Positioning solutions based on radio waves, such as Wifi and Zigbee, have too small a coverage area and poor positioning accuracy.

[0004] Ultra-wideband (UWB) is a novel radio wave ranging technology in recent years. The UWB technology calculates the distance between two modules by measuring the time of arrival, time difference of arrival, or angle of arrival of radio waves. Since the emitted radio wave band is between 3.1 GHz and 4.8 GHz, it can effectively overcome the interference of other radio wave signals. In addition, its high bandwidth can easily overcome the multipath effect and weaken the influence of non-line-of-sight measurements. The P440 UWB module produced by Time domain company measures distance and communication using the two-way time of flight (TW-ToF) method. Due to its good performance and low price, this module can be used to build a positioning system to provide positioning information for UAVs in a GNSS denied environment.

[0005] Regarding the problem of UAV positioning using UWB technology, major universities at home and abroad have carried out research. At present, the teams of Xie Lihua at Nanyang Technological University in Singapore, Chen Benmei at the National University of Singapore, the team of Raffaello D’Andrea at the Swiss Federal Institute of Technology in Zurich, the University of Würzburg in Germany, etc. have all achieved certain results successively; domestic universities such as Tsinghua University and Beihang University have also carried out relevant research. Thus, it can be seen that positioning using UWB modules is a promising solution. Summary of the Invention

[0006] Aiming at the technical bottlenecks of low positioning accuracy of quadrotor UAVs in satellite-denied environments, high cost of existing solutions or dependence on complex computing power, the present invention proposes a quadrotor UAV positioning system in GNSS-denied environments. By integrating ultra-wideband (UWB) two-way time-of-flight ranging technology, IMU inertial navigation, and radar altimeter data, an unscented Kalman filter fusion framework is designed to real-time analyze the six-degree-of-freedom pose of the UAV. Combining the quadrotor dynamics model and anti-interference outlier detection mechanism, centimeter-level positioning is achieved in complex scenarios such as tunnels, jungles, and indoors, solving the contradictions of traditional solutions in terms of cost, environmental adaptability, and real-time performance, and having significant engineering application value.

[0007] The present invention provides a quadrotor UAV positioning system in GNSS-denied environments, as shown in the appendix Figure 1 The specific steps are as follows:

[0008] Step S1: Perform IIR low-pass filtering on the original IMU data with a cut-off frequency of approximately 30 Hz.

[0009] Step S2: Place more than 4 UWB anchor modules around the environment, and use the linear regression method to calibrate the ranging data of the UWB anchor modules.

[0010] Step S3: Use the echo ranging function of the UWB anchor modules to measure the distances between the UWB anchor modules, and use the nonlinear least squares method to calibrate the positions of the anchors.

[0011] Step S4: Construct an unscented Kalman filter state space model, including an IMU prediction module, a UWB ranging update module, and a radar altimeter update module.

[0012] Step S5: Detect outliers in the UWB distance data through the dual criteria of measurement threshold and Mahalanobis distance. If there are anomalies, reject the introduction of the unscented Kalman filter for update. At the same time, real-time fuse the preprocessed IMU data, valid UWB ranging data, and radar altimeter data, and finally output the six-degree-of-freedom pose estimation result of the UAV.

[0013] So far, by running the multi-sensor fusion algorithm on the quadrotor UAV platform, the six-degree-of-freedom pose of the UAV can be solved in real time through unscented Kalman filtering based on IMU inertial measurement, UWB ranging data, and radar altimeter information, realizing centimeter-level positioning and stable navigation in a GNSS-denied environment.

[0014] The beneficial effects of the present invention are as follows: The research on the positioning and navigation algorithm of the quadrotor UAV in a GNSS-denied environment by the present invention is of great significance. The present invention is stable and reliable, not affected by weather, light, etc. At the same time, the algorithm saves computing resources and has low requirements for hardware, with high theoretical and practical value. The present invention mainly has the following characteristics and advantages:

[0015] (1) The UWB ranging UAV positioning method adopted by the present invention has high positioning accuracy, low cost, and wide coverage. The traditional UAV positioning and navigation methods in a GNSS-denied environment, such as using indoor positioning systems, are expensive. Generally, multiple positioning cameras need to be arranged on the roof and require good installation and calibration. In addition, the operating conditions are relatively harsh, the coverage is limited, and it cannot be applied to outdoor scenarios.

[0016] (2) The present invention proposes a positioning algorithm based on the fusion of IMU / UWB, supplemented by radar altimeter measurement. The position update rate of the traditional UWB positioning algorithm is limited by the ranging rate of the UWB module. Since the frequency of the UWB module is relatively low, the sawtooth is obvious, and the UAV attitude information cannot be obtained. The above problems can be significantly improved after fusing the IMU. In addition, the algorithm also fuses radar altimeter data, which can effectively improve the problem of insufficient height direction estimation accuracy. The fusion framework using Kalman filtering has high real-time performance, occupies less computer resources, and has a fast operation speed.

[0017] (3) The UAV positioning platform built by the present invention in a GNSS-denied environment has strong scalability. In addition to sensor devices such as IMU, UWB, and radar altimeter, sensor devices such as lidar can be added according to the developers themselves, and secondary development can be carried out. Description of the Drawings

[0018] Appendix Figure 1 Flowchart of the construction of the quadrotor UAV positioning system in a GNSS-denied environment

[0019] Appendix Figure 2 Hardware architecture diagram of the quadrotor UAV positioning system in a GNSS-denied environment

[0020] Appendix Figure 3 Algorithm flowchart of the quadrotor UAV positioning system in a GNSS-denied environment

[0021] Appendix Figure 4 Structure diagram of the IMU low-pass filter

[0022] Appendix Figure 5 Schematic Diagram of UWB Module Arrangement in Anchor Point Calibration Link

[0023] Appendix Figure 6 Principle Diagram of Unscented Kalman Filter

[0024] Appendix Figure 7 Positioning Effect Diagram of Quadrotor UAV in GNSS Denied Environment (3D)

[0025] Appendix Figure 8 Positioning Effect of Quadrotor UAV in GNSS Denied Environment (XY Plane)

[0026] Appendix Figure 9 Comparison Diagram of Position Estimation and True Value of Quadrotor UAV in GNSS Denied Environment (3D) Detailed Implementation Manner

[0027] The following describes the detailed implementation manner of the present invention with reference to the accompanying drawings.

[0028] The pose estimation platform of the quadrotor UAV in the GNSS denied environment is shown in Appendix Figure 2 . First, a detailed introduction to the hardware composition of the system: The hardware composition of the system includes a quadrotor UAV, a UWB ranging sensor, an IMU inertial measurement unit, a radar altimeter, and an embedded airborne processor. Considering the reliability and positioning accuracy of the system, high-precision sensor devices are required. Among them, the UWB ranging sensor uses the P440 module produced by Time domain Corporation of the United States. For the IMU inertial measurement unit, the present invention uses the mti300 inertial measurement unit produced by Xsense Technology Company of the Netherlands. For the selection of the embedded airborne sensor, the present invention uses the eighth-generation NUC of Intel as the airborne processor. The quadrotor UAV platform is assembled from a carbon fiber frame, a Pixhawk open-source flight controller, a DJI E300 motor speed controller, a 6S high-voltage battery, and the above sensors. The algorithm program is developed based on the ROS (Robot Operating System) robot open-source framework under the Linux operating system. In the ROS framework, ROS Message is used to receive the data collected by the sensors in real time. The pose information parsed by the algorithm is sent to the Pixhawk flight control via the serial port using the Mavlink protocol for further attitude control to achieve the positioning and navigation of the UAV in the GNSS denied environment. The software algorithm is shown in Appendix Figure 2 , where the key algorithms are all written in C++ language.

[0029] The specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0030] The present invention proposes to perform pose estimation of a quadrotor UAV in a GNSS-denied environment, and the specific steps of the construction method are as follows:

[0031] Step S1: Perform IIR low-pass filtering on the original IMU data with a cut-off frequency of approximately 30 Hz. The design structure is shown in the appendix Figure 4 , and the data after low-pass filtering is sent to a Kalman filter for multi-sensor fusion;

[0032] Step S2: Place more than 4 UWB anchor modules around the environment, and use the linear regression method to calibrate the ranging data of the UWB anchor modules;

[0033] Step S2.1: Describe the relationship between the measured value r of UWB and the true distance d using a linear relationship:

[0034] r = ad + b

[0035] where a is the weight of the linear equation and b is the static error of the linear equation.

[0036] Step S2.2: Obtain n sets of sample points (d i , r i ) at the same moment using a laser rangefinder and UWB anchor modules, where i = 1,..., n, and d i is the true distance measured by the laser rangefinder, and r i is the ranging reading of the UWB anchor module.

[0037] Step S2.3: Calculate the linear parameters and the static error

[0038]

[0039] where and are the mean values of d i and r i respectively.

[0040] Step S2.4: Calculate the calibrated distance

[0041]

[0042] where

[0043] Step S3: Use the echo ranging function of the UWB anchor module to measure the distance between the UWB anchor modules, and use the non-linear least squares method to calibrate the positions of the anchors;

[0044] Step S3.1: Establish a three-dimensional rectangular coordinate system as the positioning reference system. See the attached figure for the schematic diagram of anchor point calibration. Figure 5 , the origin of the coordinate system is set at the geometric center of the first UWB anchor point module; the X-axis reference is determined by the spatial connection line between the first and second UWB anchor point modules, where the positive direction of the X-axis is defined as the vector direction from the first UWB anchor point module to the second UWB anchor point module; the Z-axis reference is established perpendicular to the ground plane, and its positive direction is defined as the vertical axis extending along the reverse direction of gravity of the first UWB anchor point module; the Y-axis reference is automatically generated according to the right-hand orthogonal rule, and its positive direction satisfies the direction indicated by the thumb when the four fingers of the right hand bend from the X-axis to the Z-axis. The green arrow in the figure represents the ranging object, such as a UAV ranging to module 1.

[0045] Step S3.2: Turn on the echo ranging function of all UWB anchor point modules.

[0046] Step S3.3: The UAV platform collects the ranging information between UWB anchor point modules, the ranging information of UWB anchor point modules, and the ranging information of UWB tags, and constructs a non-linear least squares cost function:

[0047]

[0048] E2 = ||n - cosθ·t||

[0049] where E1 is the error term related to distance in the cost function, E2 is the error term related to direction in the cost function, d i is the i-th distance measurement, i = 1, 2,..., r; x m is the position of the m-th module, x n is the position of the n-th module; n = (1, 0) T represents the direction vector; t is the unit vector in the direction; cosθ is the angle between n and t;

[0050] Step S3.4: Construct a joint cost function according to the two cost functions:

[0051] F(x) = min(E1 + E2)

[0052] Step S3.5: Use the Gauss-Newton algorithm or the Levenberg-Marquardt algorithm to iteratively solve the optimal estimates of the UWB anchor point coordinates and the UWB tag coordinates.

[0053] Step S4: Construct an unscented Kalman filter state space model, including an IMU prediction module, a UWB ranging update module, and a radar altimeter update module. See the attached figure for the Kalman filter flow chart described in the present invention. Figure 6 ;

[0054] Step S4.1: Construct the discrete form of the kinematic equation of the drone:

[0055]

[0056] v k = v k-1 + Δt a k-1

[0057]

[0058] In the above equation represents quaternion multiplication; where the inertial system established in the aforementioned anchor point calibration step is b = [x w , y w , z w , and the body coordinate system of the drone in the inertial coordinate system is set as b = [x b , y b , z b ; for simplicity of representation, the position of the drone's body coordinate system in the inertial system can be expressed as is represented as a 3-dimensional real number space; the velocity can be expressed as The attitude of the drone in the inertial system can be represented by a unit quaternion is represented as a 4-dimensional real number space;

[0059] Step S4.2: Construct the IMU inertial measurement model of the drone:

[0060] ω m = ω b + b ω + e ω

[0061] a m = R -1 (a - g) + b a + e a

[0062] where ω b represents the angular velocity in the body coordinate system of the drone; a is the acceleration in the inertial system; g is the gravity vector in the inertial system; b ω , b a are the zero biases of the IMU; e ω , e a are zero-mean Gaussian white noises.

[0063] Step S4.3: Construct the UWB distance measurement model of the drone:

[0064] Y = ‖p + R t tag - xa ‖+v

[0065] Among them, p is the position of the UAV, R is the attitude of the UAV, and t tag is the position of the tag in the UAV body coordinate system, which is a known constant, and x a is the position of the anchor point in the inertial system.

[0066] Step S5: Detect outliers in the UWB distance data through the dual criteria of measurement threshold and Mahalanobis distance. If there are anomalies, reject the introduction of the unscented Kalman filter for update. At the same time, fuse the preprocessed IMU data, valid UWB ranging data, and radar altimeter data in real time, and finally output the six-degree-of-freedom pose estimation result of the UAV;

[0067] Step S5.1: Use the unscented Kalman filter to estimate the position, velocity, attitude of the UAV in the inertial system, and the zero bias of the IMU in a tightly coupled manner. Define the system state vector as:

[0068] X = [p T , v T , q T , b a T , b ω T T

[0069] Among them, b a , b ω is the zero bias of the IMU. The covariance matrix of the system is P, and the initial state x0 is determined by the calibration step;

[0070] Step S5.2: Construct the process equation of the unscented Kalman filter system:

[0071] X k = f(X k-1 , u k-1 , w k-1 )

[0072] Among them, is the input of the system, is a q-dimensional zero-mean Gaussian white noise;

[0073] Step S5.3: Perform the prediction step of the Kalman filter:

[0074] Step S5.3.1: Determine the initial value of the filter:

[0075]

[0076] Among them, the above formula is the augmented state and the corresponding covariance, ​P0 = E[(X0 - EX0)(X0 - EX0) T , which can be obtained by the anchor calibration step;

[0077] Step S5.3.2: When the accelerometer and gyroscope readings of the IMU arrive, for k = 1, 2,..., iterations, first apply the unscented transform to obtain the set matrix of Sigma points:

[0078]

[0079]

[0080] where (·) i represents taking the i-th column in the matrix ·, is a parameter of the unscented Kalman filter, L is the dimension of the state vector, representing the total number of the UAV position, velocity, and attitude parameters mentioned above;

[0081] Step S5.3.3: Perform time update through the process equation to obtain the predicted new Sigma point matrix:

[0082]

[0083] where α is a scaling factor used to control the Sigma points, that is, the sampling points, in the distribution range of the state space, and β is a prior distribution correction parameter used to adjust the covariance weight to compensate for the influence of non-Gaussian distribution;

[0084] Step S5.4: Perform the UWB ranging update step:

[0085] Step S5.4.1: When the filter iterates to the k-th moment and receives the distance measurement Y k , define the distance measurement equation Y k = h(X k , v k ) as follows:

[0086] Y k = ‖p k + R k t tag - x a ‖ + v k

[0087] where p is the position part of the system state vector X in Step S5.1, R is the attitude part in the state, t tag is the position of the UWB tag in the UAV body coordinate system, which is a known constant, and x a is the position of the corresponding anchor point;

[0088] Step S5.4.2: Apply the unscented transformation to obtain the set matrix of Sigma points:

[0089]

[0090] Step S5.4.3: Substitute the set matrix of Sigma points into the measurement equation for measurement update:

[0091]

[0092]

[0093] where, is the predicted covariance matrix of the measurement value, is the cross-covariance matrix between the state quantity and the measurement value, W i m , W i c is the same as in Step S5.3;

[0094] Step S5.4.4: Calculate the Kalman filter gain and obtain the state mean and covariance after distance update:

[0095]

[0096] Step S5.5: Fuse the radar altimeter data:

[0097] Step S5.5.1: At time k, obtain the state and covariance P k of the system, and find the data with the corresponding timestamp of the current system state in the radar altimetry data Buffer of the airborne computer; if the cosine value of the angle between the z b axis of the current UAV body coordinate system and the z w axis of the inertial system is approximately 1, then the radar altimeter can be updated, project the distance measurement from the ground into the inertial system, and obtain the observation quantity Z k of the radar altimetry data;

[0098] Step S5.5.2: Augment the state quantity and covariance of the system again:

[0099]

[0100] Step S5.5.3: According to the radar altitude measurement equation substitute it into the measurement equation for update:

[0101]

[0102]

[0103] Among them, is the predicted covariance matrix of the measurement values, is the cross-covariance matrix between the state quantity and the measurement values, W i m , W i c is the same as in step S5.3, ;.

[0104] Step S5.4.4: Calculate the Kalman filter gain and obtain the updated state mean and covariance of the radar, and obtain a good pose estimation of the UAV:

[0105]

[0106] As described above in the embodiments of the present invention, by performing UAV pose estimation on a multi-sensor fusion system, the positioning of a quadrotor UAV in a GNSS-denied environment is realized. The experimental results are referred to in the appendix Figure 7 - Appendix Figure 9 , Appendix Figure 7 The orange line corresponds to the UAV trajectory using multi-sensor fusion of IMU, UWB, and radar altimeter, and the green line corresponds to the UAV trajectory output by the indoor positioning system, which can be regarded as the trajectory ground truth. It can be seen from the figure that the estimation error of the method adopted by the present invention can be guaranteed within ±15 cm. Appendix Figure 8 is the comparison between the trajectory estimation and the ground truth in only the xy plane. Appendix Figure 9 is the comparison between the x, y, and z-axis position estimations of the UAV and the ground truth. It can be seen from the figure that the errors in the x and y directions are within ±10 cm, and due to the fusion of radar altimeter data, the estimation error in the z-axis direction is within ±5 cm.

[0107] Based on the construction method provided by the present invention, by fusing IMU inertial measurement, UWB distance measurement, and radar altimeter measurement, the pose information of the UAV is resolved, and by designing a quadrotor UAV platform and running the positioning algorithm, the rapid and precise positioning of the UAV is realized.

[0108] Without departing from the basic technical ideas and spirit principles of the present invention, any obvious modifications, equivalent replacements, or further optimizations made to the above details shall be included within the scope of the claims of the present invention.

Claims

1. A quadrotor drone positioning system in a GNSS-denied environment, characterized in that: The system is composed of a hardware platform and a multi-sensor fusion algorithm; the hardware platform includes a UWB anchor point module group deployed in the environment, a drone platform, a UWB tag module mounted on the drone platform, an IMU inertial measurement sensor radar altimeter, and an embedded airborne processor; the multi-sensor fusion algorithm includes an IMU low-pass filter unit, a distance calibration unit, an anchor point calibration unit, an outlier detection unit, and a Kalman filter unit; the system realizes drone positioning through the following steps: Step S1: Implement IIR low-pass filtering with a cutoff frequency of 30 Hz on the IMU raw data; Step S2: placing more than four UWB anchor modules around the environment, and calibrating the ranging data of the UWB anchor modules using a linear regression method; Step S3: using the echo ranging function of the UWB anchor point module to measure the distance between the UWB anchor point modules, and using the nonlinear least squares method to calibrate the position of the anchor point; Step S4: construct an unscented Kalman filter state space model, including an IMU prediction module, a UWB ranging update module, and a radar altimeter update module; Step S5: Detect outliers in the UWB distance data through the dual criteria of measurement threshold and Mahalanobis distance. If anomalies exist, the unscented Kalman filter is introduced for updating. At the same time, the preprocessed IMU data, valid UWB ranging data and radar altimeter data are integrated in real time to finally output the six-degree-of-freedom pose estimation result of the UAV.

2. The quadrotor drone positioning system in a GNSS-denied environment according to claim 1, characterized in that: The IIR low-pass filter of step S1 is designed as: The frequency domain representation of the IIR low-pass filtering algorithm is as follows: Where m is the molecular order, n is the molecular order, which is 2-8; a i , i∈{1,…,m} is the denominator coefficient, which is used to control the zero position of the filter; b j , j∈{1,…,n} is the denominator coefficient, which is used to control the pole position of the filter; the time domain form of the low-pass filter is as follows: y(t)=a0u(t)+a1u(t-1)+…+a m u(t-m)-b1y(t-1)-…-b n y(t-n)。 3. The quadrotor UAV positioning system in a GNSS-denied environment according to claim 1, characterized in that: In step S2, the specific steps are: Step S2.1: Use a linear relationship to describe the relationship between the UWB measurement value r and the actual distance d: r = ad + b Where a is the weight of the linear equation and b is the static error of the linear equation. Step S2.2: At the same time, use the laser rangefinder and the UWB anchor point module to obtain n groups of sample points (d i ,r i ), i = 1,…,n, where d i is the true value of the distance measured by the laser rangefinder, r i is the ranging reading of the UWB anchor module; Step S2.3: Calculate linear parameters using the linear regression method and static error in and They are d i The mean and r i The mean of Step S2.4: Calculate the calibrated distance in 4. The quadrotor drone positioning system in a GNSS-denied environment according to claim 1, characterized in that: In step S3, the specific steps are: Step S3.1: Establish a three-dimensional rectangular coordinate system as a positioning reference system; the origin of the coordinate system is set at the geometric center of the first UWB anchor module; the X-axis reference is determined by the spatial connection line of the first and second UWB anchor modules, wherein the positive direction of the X-axis is defined as the vector direction from the first UWB anchor module to the second UWB anchor module; the Z-axis reference is established perpendicular to the ground plane, and its positive direction is defined as the vertical axis extending in the opposite direction of gravity of the first UWB anchor module; the Y-axis reference is automatically generated according to the orthogonal right-hand rule, and its positive direction satisfies the direction in which the thumb points when the four fingers of the right hand are bent from the X-axis to the Z-axis; Step S3.2: Turn on the echo ranging function of all UWB anchor modules; Step S3.3: The UAV platform collects the ranging information between the UWB anchor modules, the ranging information of the UWB anchor modules, and the ranging information of the UWB tags, and constructs a nonlinear least squares cost function: E2=||n-cosθ·t|| Where E1 is the error term related to distance in the cost function, E2 is the error term related to direction in the cost function, and d i is the i-th distance measurement, i = 1, 2, …, r; x m is the position of the mth module, x n is the position of the nth module; n=(1,0) T represents the direction vector; t is The unit vector of direction; cosθ is the angle between n and t; Step S3.4: construct a joint cost function based on the two cost functions: F(x)=min(E1+E2) Step S3.5: Use the Gauss-Newton algorithm or the Levenberg-Marquardt algorithm to iteratively solve the optimal estimate of the UWB anchor point coordinates and the UWB tag coordinates.

5. The quadrotor drone positioning system in a GNSS-denied environment according to claim 1, characterized in that: In step S4, the specific steps are: Step S4.1: Construct the discrete form of the kinematic equation of the UAV: in k =in k-1 +Δta k-1 In the above formula represents quaternion multiplication; wherein the inertial system established in the above anchor point calibration step is b = [x w ,y w ,z w ], the coordinate system of the drone in the inertial coordinate system is set to b = [x b ,y b ,z b ]; To simplify the representation, the position of the drone body coordinate system in the inertial system can be expressed as Represented as a 3-dimensional real number space; the speed can be expressed as The attitude of the drone in the inertial system can be expressed as a unit quaternion Represented as a 4-dimensional real number space; Step S4.2: Construct the IMU inertial measurement model of the drone: oh m =ω b +b ω +e ω a m =R -1 (a-g)+b a +e a Among them, ω b represents the angular velocity of the drone in the coordinate system; a is the acceleration in the inertial system; g is the gravity vector in the inertial system; b ω 、b a is the zero bias of the IMU; e ω 、e a is zero-mean Gaussian white noise. Step S4.3: Constructing the UWB distance measurement model of the drone: Y=‖p+Rt tag -x a ‖+v Among them, p is the position of the UAV, R is the posture of the UAV, and t tag is the position of the tag in the coordinate system of the drone, which is a known constant. a is the position of the anchor point in the inertial frame.

6. The quadrotor UAV positioning system in a GNSS-denied environment according to claim 1, characterized in that: In step S5, the specific steps are: Step S5.1: Use the unscented Kalman filter to estimate the position, velocity, attitude and zero bias of the UAV in the inertial system in a tightly coupled manner, and define the system state vector as: X=[p T ,v T ,q T ,b a T ,b ω T ] T Among them, b a , b ω is the zero bias of IMU, the covariance matrix of the system is P, and the initial state x0 is determined by the calibration step; Step S5.2: Construct the process equation of the unscented Kalman filter system: X k =f(X k-1 ,u k-1 ,w k-1 ) in, is the input of the system, is q-dimensional zero-mean Gaussian white noise; Step S5.3: Perform the prediction step of Kalman filter: Step S5.3.1: Determine the initial value of the filter: Among them, the above formula is the augmented state and the corresponding covariance, P0=E[(X0-EX0)(X0-EX0) T ], which can be obtained by the anchor point calibration step; Step S5.3.2: When the accelerometer and gyroscope readings of the IMU arrive, for k = 1, 2, ..., iterations, first apply the unscented transformation to obtain the set matrix of Sigma points: in,(·) i It means taking the i-th column in the matrix ·, is the parameter of the unscented Kalman filter, L is the dimension of the state vector, which represents the total number of the position, velocity and attitude parameters of the UAV mentioned above; Step S5.3.3: Update the time through the process equation to obtain the predicted new Sigma point matrix: in, α is a scaling factor used to control the distribution range of Sigma points, i.e., sampling points, in the state space. β is a prior distribution correction parameter used to adjust the covariance weight to compensate for the impact of non-Gaussian distribution. Step S5.4: Perform UWB ranging update steps: Step S5.4.1: When the filter iteration reaches the kth moment, the distance measurement Y is received k , define the distance measurement equation Y k =h(X k ,v k )as follows: Y k =‖p k +R k t tag -x a ‖+v k Where p is the position part of the system state vector X in step S5.1, R is the attitude part of the state, and t tag is the position of the UWB tag in the coordinate system of the drone body, which is a known constant, x a is the position of the corresponding anchor point; Step S5.4.2: Apply the unscented transformation to obtain the set matrix of Sigma points: Step S5.4.3: Substitute the set matrix of Sigma points into the measurement equation for measurement update: in, is the predicted covariance matrix of the measured values, is the cross covariance matrix between the state quantity and the measurement value, W i m , W i c Same as in step S5.3; Step S5.4.4: Calculate the Kalman filter gain and obtain the state mean and covariance after the distance update: Step S5.5: Fusing radar altimeter data: Step S5.5.1: At time k, get the state of the system and covariance P k , find the data corresponding to the timestamp of the current system state in the airborne computer radar height measurement data Buffer; if the current drone body coordinate system z b Axis and inertial system z w The cosine value of the angle between the two axes is approximately 1, so the radar altimeter can be updated, and the distance measurement to the ground can be projected into the inertial system to obtain the observed value Z of the radar altimeter data. k ; Step S5.5.2: Augment the state quantity and covariance of the system: Step S5.5.3: Measurement equation based on radar altimetry data Substitute the measurement equation for update: in, is the predicted covariance matrix of the measured values, is the cross covariance matrix between the state quantity and the measurement value, W i m , W i c Same as in step S5.3; Step S5.5.4: Calculate the Kalman filter gain and obtain the updated radar state mean and covariance to obtain a good drone pose estimate:

Images