Four-rotor GNSS / SINS / dynamics fusion algorithm based on SKF

Abstract

The invention discloses a quadrotor GNSS / SINS / dynamics fusion algorithm based on an SKF, and is suitable for the dynamic state estimation and positioning of a quadrotor unmanned aerial vehicle. The method comprises the following steps: firstly, constructing a simplified four-rotor kinetic model, and obtaining a thrust coefficient estimation initial value; secondly, deriving a dynamic error state propagation equation considering a thrust coefficient error; then, an SKF algorithm is used for updating dynamic speed measurement to estimate the dynamic state, a self-adaptive dynamic-gravity alignment constraint scheme considering the uncertainty of external force estimation is provided, and according to the constraint, horizontal attitude constraint is carried out on the system by aligning the external force, the motor thrust and the gravity. Results show that the method ensures that the dynamics does not damage the positioning performance of the GNSS / SINS system, can accurately estimate the four-rotor dynamics state in an outdoor scene, and meanwhile, the dynamics-gravity alignment constraint provided by the invention significantly improves the horizontal attitude estimation precision and positioning performance of the system, especially in a GNSS interruption scene.

Description

Technical Field

[0001] The present invention belongs to the field of GNSS / SINS / dynamics navigation of multi-rotor unmanned aerial vehicles, and relates to a GNSS / SINS / dynamics state estimation algorithm based on SKF (Schmidt Kalman Filter). It takes the GNSS / SINS integrated navigation of quad-rotor unmanned aerial vehicles, the external force estimation of quad-rotor unmanned aerial vehicles, and the dynamic assisted navigation of quad-rotors as the actual application background, and can be used for the dynamic state estimation of quad-rotor unmanned aerial vehicles and the positioning enhancement application direction of the navigation system in the GNSS signal loss environment. Background Art

[0002] Multi-rotor unmanned aerial vehicles represented by quad-rotors have currently been applied to undertake various tasks, such as aerial manipulation, inspection, and collaborative transportation. However, in these tasks, the unmanned aerial vehicle navigation system often faces problems such as satellite signal occlusion and external force interference. In response to these problems, traditional solutions usually adopt the strategy of adding redundant sensors for multi-source fusion, but this significantly increases the complexity and cost of the navigation system. In fact, in addition to relying on additional sensors, a large amount of available navigation information is also contained in the dynamics of unmanned aerial vehicles. If the dynamic state of the unmanned aerial vehicle can be accurately estimated and effective motion constraints can be constructed using it, the performance of the navigation system can theoretically also be improved.

[0003] The navigation system is one of the core systems of an unmanned aerial vehicle (UAV). Currently, quadrotor UAVs generally carry low-cost satellite and inertial integrated navigation systems. Among them, GNSS (Global Navigation Satellite System) provides users with high-precision position and velocity information by receiving signals from multiple satellites. It has many advantages such as error not accumulating over time and all-weather operation. However, in occluded environments such as mountains and cities, GNSS may encounter problems such as insufficient observable satellites and satellite signal loss. The strapdown inertial navigation system SINS (Starpdown Interial Navigation System) is an integral recursive navigation method based on Newton's laws of motion. SINS can work autonomously in all scenarios and has the advantage of high short-term accuracy. However, due to the existence of zero-bias errors in sensors, the error will accumulate rapidly over time. In summary, GNSS and SINS have good complementary characteristics. GNSS can suppress the long-term error divergence of SINS, while SINS, due to its high short-term accuracy and full autonomy advantages, can well handle the gross error problem of GNSS in abnormal observation scenarios. Currently, the UAV GNSS / SINS navigation system usually uses EKF (Extended Kalman Filter) to fuse GNSS and SINS data. However, whether it is GNSS or SINS, both ignore the potential information source of rotor dynamics, and the dynamic information is not fully utilized.

[0004] Taking a quadrotor UAV as an example, the dynamic states to be estimated are external forces and rotor thrust coefficients. Among them, the external forces mainly include wind forces and other interference forces. Existing external force estimation methods mainly focus on using external observations to obtain accurate external force estimation values. In reference [1] (B. Nisar, P. Foehn, D. Falanga, and D. Scaramuzza, “Vimo: Simultane-ous visual inertial model-based odometry and force estimation,” IEEE Robotics and Automation Letters, vol. 4, no. 3, pp. 2785–2792, 2019.), the external force estimation is incorporated into the visual-inertial-tight coupling framework, and the joint estimation of the motion state and external forces is achieved simultaneously. In reference [2] (Z. Ding, T. Yang, K. Zhang, C. Xu, and F. Gao, “Vid-fusion: Robust visual-inertial-dynamics odometry for accurate external force estimation,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. 14469–14475.), it is further expanded on this basis to develop VID-Fusion that can estimate long-term external forces. However, these studies mostly focus on indoor scenes with rich visual features. In outdoor open scenes, there are often few visual features, which affects the accuracy of visual information. In addition, since the thrust coefficient often changes during the flight of the UAV, traditional CAD (Computer Aided Design) models, static tests or offline calibration methods are difficult to meet the actual application requirements. To solve this problem, researchers have adopted an online estimation method to estimate the parameters of the dynamic system. Theoretically, by correctly constructing the state model, accurate estimation of both the navigation state and the thrust coefficient can be achieved within a single estimator.

[0005] Force is the fundamental cause of the motion change of the UAV. If the dynamic states such as external forces can be accurately estimated and the motion state is constrained according to them, theoretically, the robustness and positioning performance of the navigation system can be improved. However, it is very difficult to establish an accurate dynamic model, and there are many complex error sources in dynamics. Directly fusing dynamics in the EKF (Extended Kalman Filter) often damages the positioning performance of the original system. Summary of the Invention

[0006] The object of the present invention is to overcome the deficiencies of the prior art and propose a new reliable GNSS / SINS / dynamics state estimation algorithm based on SKF.

[0007] The specific idea of the present invention is as follows: Aiming at the problems that traditional dynamics state estimation schemes are mainly for indoor scenarios, visual sensors face scene degradation and dynamic interference in outdoor scenarios, and current dynamics-augmented GNSS / SINS integrated navigation has uncontrollable errors and unreliable performance, the following three improvements are made: (1) The error propagation of the quadrotor dynamics state considering the thrust coefficient error of the dynamics model and the model modeling error is derived. On this basis, a new GNSS / SINS / dynamics fusion integrated navigation method is proposed, which is applicable to outdoor scenarios and can estimate the dynamics thrust coefficient online; (2) A dynamics velocity fusion strategy based on SKF (Schmidt Kalman Filter) filtering is proposed, which avoids the unreliable dynamics state from damaging the positioning performance of the original GNSS / SINS system; (3) By analyzing the forces acting on the UAV, a dynamics-gravity alignment constraint is proposed to improve the attitude and position estimation accuracy of the GNSS / SINS system. This method first establishes a simplified quadrotor dynamics model and roughly calibrates the initial value of the thrust coefficient using the recursive least squares algorithm. Subsequently, the dynamics error state propagation equation including the thrust coefficient error is derived. On this basis, dynamics is incorporated into the existing GNSS / SINS loose integration algorithm, and SKF is used to fuse the dynamics velocity to estimate the dynamics state. Finally, an adaptive dynamics-gravity alignment constraint method considering the uncertainty of the external force estimation is proposed, which significantly improves the positioning performance of the GNSS / SINS system.

[0008] The technical solution of the present invention is as follows:

[0009] A GNSS / SINS / dynamics fusion algorithm based on SKF, comprising the following steps:

[0010] (1) Construct a simplified quadrotor dynamics model;

[0011] (2) Roughly calibrate the motor thrust coefficient c t using the recursive least squares algorithm. The specific steps are as follows:

[0012] (2.1) Data acquisition: In a windless scenario, let the quadrotor UAV maintain a hover state without any load for a period of time, and collect the rotor speed data during this period;

[0013] (2.2) Rough calibration: Construct a rotor gravity observation equation and use the recursive least squares to estimate c t ;

[0014] (2.3) Iterative calibration: Repeat the loop in step (2.2), taking the single-pass recursive least squares calibrated thrust coefficient as the initial value for the next recursive least squares algorithm, and calculate repeatedly until the finally obtained thrust coefficient no longer changes.

[0015] (3) Derive the dynamic state error equation, and augment the dynamic error state δx in the GNSS / SINS loose integration state model δx I ; and update the dynamic error state according to the dynamic error equation; D

[0016] (4) Construct the GNSS position observation equation and perform measurement update using EKF;

[0017] (5) Construct the dynamic velocity measurement equation and update the dynamic velocity measurement using SKF;

[0018] (6) Construct the dynamic-gravity alignment measurement, and adaptively activate the dynamic-gravity alignment constraint based on the uncertainty difference of the continuous-time external force estimation.

[0019] Furthermore, the rotor gravity observation equation described in step (2) is expressed as:

[0020]

[0021] where m represents the mass of the UAV, g w represents the gravitational acceleration, c t，i represents the thrust coefficient of the i-th rotor, r i represents the rotational speed of the i-th rotor.

[0022] Furthermore, the GNSS / SINS loose integration state δx in step (3) I is:

[0023]

[0024] where the superscript T represents the matrix transpose, φ w , respectively represent the misalignment angle error, velocity error, and position error of the rotor b-frame relative to the w-frame, δb g and δb a represent the zero bias errors of the gyroscope and accelerometer respectively. The augmented dynamic error state δx D is:

[0025]

[0026] where is the external force estimation error in the b-frame, δc t is the thrust coefficient error, is the dynamic velocity error in the w frame. The augmented state δx is expressed as:

[0027] δx = [δx I δx D T

[0028] Furthermore, the method for judging the difference in the uncertainty of the continuous-time external force estimation in step (6) is: calculate the difference Δu in the external force estimation covariance between t - 1 and t f , when Δu f is less than the given threshold, it indicates that the external force estimation error has converged at this time. The calculation method is as follows:

[0029]

[0030] In the formula and represent the external force estimation covariance matrices at times t - 1 and t respectively, tr(·) represents the trace of the matrix, represents the square root, and |·| represents the absolute value.

[0031] The present invention has the following advantages compared with the prior art:

[0032] First, the present invention aims at the problem of insufficient accuracy of the traditional vision-aided dynamic state estimation scheme in outdoor open environments. For outdoor open environments, a GNSS / SINS integrated navigation system is used as the observation source to estimate the quadrotor dynamic state. Compared with the traditional vision scheme, the GNSS / SINS system has good positioning accuracy in outdoor open scenes, and the dynamic state estimation has high accuracy.

[0033] Second, when the present invention fuses the dynamic state, it fully considers the influence of the dynamic model error, incorporates the thrust coefficient error δc t in the dynamic model error into the state variables to be estimated, and derives the dynamic state error propagation equation and measurement equation including δc t . Compared with the traditional dynamic fusion method that does not consider the dynamic model error, the present invention can calibrate the thrust coefficient online during the flight of the unmanned aerial vehicle even when the initial value of the thrust coefficient is unreliable, effectively reducing the influence of the unreliable dynamic model error on the system accuracy.

[0034] ​3. The present invention aims at the problem of uncontrollable error and unreliable performance faced by the traditional EKF method in fusing dynamics. The error characteristics of the dynamic state are analyzed in detail based on the derived dynamic error propagation equation. On this basis, the SKF algorithm is used to fuse the GNSS / SINS speed and the dynamic speed to estimate the dynamic state. The SKF algorithm not only ensures that the dynamics will not damage the positioning performance of the system, but also can accurately estimate the dynamic state. In addition, after estimating the external force using the SKF algorithm, the present invention proposes a dynamic-gravity alignment constraint that uses gravity as an observation to correct the motor thrust and external force based on the force conditions of the motor thrust, external force and gravity of the drone under uniform speed and small acceleration flight. The constraint corrects the horizontal attitude estimation accuracy of the GNSS / SINS system through dynamics, and significantly improves the positioning performance of the system when the GNSS is lost. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a flow chart of the present invention;

[0036] Figure 2 The external force estimation results of the present invention on the open source data set;

[0037] Figure 3 This is the thrust coefficient estimation result of the present invention on the open source data set;

[0038] Figure 4 This is a horizontal attitude estimation error diagram of different schemes in the experiment of improving navigation performance with measured data of the present invention;

[0039] Figure 5 This is the error diagram of point position error estimation of different schemes in the experiment of improving navigation performance with measured data of the present invention. DETAILED DESCRIPTION

[0040] The present invention will be further described below in conjunction with the accompanying drawings.

[0041] Reference Figure 1 The specific implementation steps of the present invention are as follows:

[0042] Step 1. Construct a simplified quadrotor dynamic model, assuming that all forces on the drone act on the rotor mass center, and regard forces other than rotor gravity and motor thrust as external forces. The quadrotor motors are symmetrically fixed to the drone body. When the motors rotate, they provide the drone with thrust along the z-axis of the body-frame. Assuming that the thrust provided by each motor acts on the geometric center of the drone, the mass-normalized total thrust generated by the motors is measured as In the b system, it can be expressed as:

[0043]

[0044] where \(e\) z = [0, 0, 0, 1] T is the unit vector along the z-axis of the b system, and \(c\) t = [c t,1 , c t,2 , c t,3 , c t,4 T are the thrust coefficients corresponding to the four rotors respectively. Since the model only considers the contribution of the motor thrust in the z direction, an additional measurement white noise \(n\) T is introduced to make up for the deficiency in the horizontal direction measurement of the dynamic model.

[0045] The Newton differential motion equation of the UAV in the w system (world-frame) is as follows:

[0046]

[0047] where represents the velocity of the b system relative to the w system deduced by dynamics, is the attitude rotation matrix from the b system to the w system, and \(g\) w = [0, 0, -9.8] T (unit: m / s 2 ) is the gravitational acceleration in the w system.

[0048] Step 2. Use the recursive least squares method to estimate the initial value of the thrust coefficient. First, collect the rotor speed data of the quadrotor UAV in the hovering state without any load in a windless scenario. Assuming that the thrust provided by the UAV rotors in the hovering state is completely used to offset the rotor gravity, the rotor gravity observation equation can be obtained:

[0049]

[0050] where \(m\) represents the mass of the UAV, and \(g\) w represents the gravitational acceleration, \(c\) t,i represents the thrust coefficient of the \(i\)-th rotor, and \(r\) i represents the rotational speed of the \(i\)-th rotor.

[0051] The update process of the recursive least squares algorithm is as follows:

[0052]

[0053]

[0054] where \(k\) t is the gain weight, and represent the thrust coefficient weights at times \(t - 1\) and \(t\) respectively, is the observation matrix, and \(R\) t is the measurement noise.​ and represent the estimated thrust coefficient values at times t-1 and t respectively, is the gravity measurement of the UAV, and I represents the identity matrix.

[0055] Subsequently, repeat the above recursive least squares method, and use the single-recursive least squares calibrated thrust coefficient c t，k-1 as the initial value for the next recursive least squares algorithm until the thrust coefficient c t，k obtained next t，k-1 and c t，k satisfy |c t，k-1 -c t，k | < α, where α is the thrust coefficient iteration threshold selected based on empirical values. At this time, it is judged that the thrust coefficient has converged, and c

[0056] Step 3. To derive the dynamic error state propagation equation, first give the continuous-time differential equation for the actual calculated dynamic state :

[0057]

[0058] where and represent the normalized external force, thrust coefficient, and dynamic velocity in the actual calculation respectively, represents the attitude rotation matrix calculated by the IMU, n f represents the external force random walk noise, 0 m×n represents the zero matrix of m rows and n columns, is the diagonal matrix of the measured rotor speed.

[0059] Subsequently, ignoring the sensor noise, the continuous-time differential equation for the ideal dynamic state is given as:

[0060]

[0061]

[0062] where c t and represent the normalized external force, thrust coefficient, and dynamic velocity in the ideal state respectively, is the attitude rotation matrix in the ideal state. D(r) = diag(r1, r2, r3, r4) is the diagonal matrix of the rotor speed in the ideal state.

[0063] The dynamic error state and x DThe relationship between them can be expressed as:

[0064] Then, subtracting the continuous-time differential equation of the ideal dynamic state from the continuous-time differential equation of the actual calculated dynamic state can obtain the continuous-time differential equation of the dynamic error state:

[0065]

[0066] In the formula, × represents a vector skew-symmetric matrix, D(c t ) = diag(c t，1 c t，2 , c t，3 , c t，4 ) represents a diagonal matrix with c t as the diagonal elements. The complete GNSS / SINS / dynamic state δx after amplification according to the dynamic error equation is expressed as:

[0067] δx = [δx I δx D T

[0068] In the formula is the IMU error state. At this time, the state transition matrix F, noise drive matrix G, and system noise n of δx can be obtained as:

[0069]

[0070]

[0071] In the formula, I m×n represents an m×n identity matrix. n g and respectively represent the gyroscope measurement white noise and the gyroscope bias drive white noise, n a and respectively represent the accelerometer measurement white noise and the accelerometer bias drive white noise, n r is the rotational speed measurement white noise.

[0072] Step 4. GNSS can provide the geodetic height position observation in the WGS84 coordinate system. First, convert it to the w coordinate system. The GNSS position measurement in the w coordinate system is expressed as The inertial navigation estimated position is subtracted from to obtain the position error observation:

[0073]

[0074] In the formula, V G is the GNSS position measurement white noise. The corresponding measurement matrix H G is:​

[0075] H G = [0 3×6 I 3×3 0 3×15

[0076] For GNSS position measurement, the traditional EKF method is still used for fusion. The measurement update process of EKF is specifically expressed as follows:

[0077] First, according to the predicted covariance P t / t-1 , H G and V G calculate the Kalman gain K t :

[0078] K t = P t / t-1 H G T (H G P t / t-1 H G T + V G ) -1

[0079] Subsequently, update the posterior state δx t and covariance P G according to K t and δz t :

[0080] δx t = δx t / t-1 + K t (δz G - H G δx t / t-1 )

[0081] P t = (I - K t H G )P t / t-1

[0082] where δx t / t-1 is the predicted state.

[0083] Step 5. Subtract the inertial navigation calculated velocity from the dynamics deduced velocity to construct the dynamics velocity error measurement. The GNSS / SINS system estimates the dynamics state by correcting the dynamics velocity. The dynamics velocity measurement δz D can be expressed as:

[0084]

[0085] where V D ​The dynamic velocity measurement white noise and the corresponding dynamic velocity measurement matrix H D is as follows:

[0086] H D = [0 3×3 I 3×3 0 3×12 -I 3×3 0 3×4

[0087] The process of SKF calculating the Kalman gain is exactly the same as the EKF calculation method in Step 4. The difference lies in the feedback method. The posterior state δx t and P t of the SKF can be updated as follows:

[0088]

[0089] In the above formula, K D represents the Kalman gain matrix for the dynamic state after the dynamic measurement. ΔP ID represents the cross-covariance matrix of δx I and δx D . ΔP DD represents the dynamic state covariance matrix.

[0090] As can be seen from the above formula, in the dynamic velocity measurement update of the SKF, neither δx I，t nor its covariance will change. In fact, the SKF ensures that the dynamic velocity measurement can still update the dynamic state and the dynamic state covariance without disturbing the performance of the original GNSS / SINS system.

[0091] Step 6. Multirotor UAVs usually use accelerometers to align with gravity for attitude constraint. However, the accelerometer measurement is easily disturbed by the high-frequency vibration of the rotorcraft. Using the dynamic alignment with gravity as a constraint avoids the influence of the high vibration of the airframe. When the UAV is in a uniform or stationary state, the force analysis of the UAV can be obtained as follows:

[0092]

[0093] Obtain the dynamic-gravity alignment observation δz g of the UAV state according to the force:

[0094]

[0095] In the formula, V g is the dynamic-gravity alignment measurement white noise, and the corresponding measurement matrix can be expressed as:

[0096] ​

[0097] Considering that the dynamic-gravity alignment measurement depends on the accuracy of the external force estimation, the constraint is adaptively determined according to the difference in the continuous-time external force estimation uncertainty. The specific determination method is as follows: Calculate the difference in the external force estimation covariance Δu between t-1 and t f , when Δu f is less than the given empirical threshold, it indicates that the external force estimation error has converged. The calculation method is as follows:

[0098]

[0099] In the formula and respectively represent the external force estimation covariance matrices at times t-1 and t, tr(·) represents the trace of the matrix, represents the square root, and |·| represents the absolute value.

[0100] After it is determined that the external force estimation has converged, for the dynamic-gravity alignment measurement, the EKF method in step 4 is still used for measurement update.

[0101] The effects of the present invention can be illustrated by the following experiments:

[0102] 1. Experimental data

[0103] The verification experiment of the present invention will use an open-source dataset and measured data respectively. The open-source dataset comes from the VID (The Visual-Inertial-Dynamical Multirotor Dataset) dataset provided by Zhejiang University. This dataset covers multi-sensor measurements and motor speed data, and at the same time provides the reference value of the motor thrust coefficient. In addition, the outdoor dataset also contains RTK (Real-Time Kinematic) position measurement data, which can be used to evaluate the GNSS / SINS integrated navigation algorithm. The drone uses an "X"-type symmetric quadrotor fuselage and weighs 3.15 kg after carrying the sensors. In the measured experiment, the drone flies smoothly and slowly along a rectangular trajectory, with the flight speed maintained at about 1 m / s, and the overall flight time is 240 s. The true pose reference value of the measured experiment is obtained through GNSS / SINS forward and backward filtering.

[0104] 2. Experimental results

[0105] The present invention designs 3 schemes for the control experiment. The first scheme is the GNSS / SINS integrated navigation algorithm under the traditional EKF fusion method, denoted as EKF. The second scheme fuses the dynamic velocity through EKF on the basis of the first scheme which is called DEKF. The third scheme is the GNSS / SINS / dynamics fusion scheme proposed by the present invention, which is fused through SKF The dynamic-gravity alignment constraint, called DSKF-g, is added simultaneously.

[0106] Experiment 1: The present invention verifies the external force estimation results on the outdoor figure-eight flight dataset of the VID dataset, and compares the external force estimation accuracies of VID-Fusion and DSKF-g. Figure 2 The external force estimation results of the two schemes are shown. The entire flight process can be divided into three stages: Takeoff stage: In this stage, the external force on the UAV mainly comes from the supporting force of the ground. As the rotational speed of the UAV's rotors gradually increases, the motor thrust gradually increases, and the magnitude of the supporting force provided by the ground slowly decreases from the gravity of the rotors themselves to 0. Second stage: The UAV is flying in the air. In this stage, the external forces on the UAV mainly include air resistance. In this stage, the present invention maintains the same accuracy as VID-Fusion in the x and y directions, and compared with VID-Fusion, the present invention has an obvious deburring effect. The third stage is the landing stage of the UAV. When the UAV touches the ground, it is again subject to the ground supporting force. As the rotors stop rotating, the ground supporting force increases from 0 to the magnitude of the UAV's own gravity. Experiments on the VID dataset show that the improved scheme proposed by the present invention can accurately estimate the external force state of a quadrotor UAV in an outdoor scene, and the accuracy is better than that of traditional vision schemes.

[0107] Experiment 2: The present invention conducts three independent initial thrust coefficient perturbation experiments on the figure-eight fast flight data in the VID dataset. In each independent experiment, the same initial errors are set for the four rotor thrust coefficient parameters. In addition, only the initial covariance of the thrust coefficient error is adjusted in different experiments. The convergence errors of the thrust coefficients in the three experiments are as Figure 3 shown. The experimental results show that by reasonably setting parameters, the method proposed by the present invention can ensure that the thrust coefficient error converges rapidly within 5s.

[0108] Experiment 3: In order to further verify the improvement of the present invention on the rotor navigation performance, starting from 90s of the actual measurement experiment, three 30s GNSS interruption times are simulated, and EKF, DEKF, and DSKF-g are respectively run. It should be noted that at the initial moment of each GNSS signal loss, the same error and covariance estimation values are ensured for the three schemes.

[0109] Figure 4 and Figure 5 respectively show the horizontal attitude error and positioning error sequences of the three schemes. Tables 1 and 2 statistically show the horizontal attitude error and positioning error statistical results of the estimated trajectories of the three schemes. In the first 90s of the experiment, GNSS has been providing effective observations. The pose estimation accuracies of EKF and DSKF-g are basically the same, while DEKF due to The influence is that the accuracy is lower than that of EKF and DSKF-g, especially in the elevation direction. Starting from 90s, GNSS signal loss scenarios begin to occur. EKF degrades to pure inertial navigation during GNSS signal loss, and the position error and horizontal attitude error continue to diverge. The horizontal attitude estimation error and positioning error of DEKF are the worst during all GNSS signal loss times, and the dynamic velocity measurement significantly accelerates the divergence speed of the system error. The DSKF-g scheme proposed in the present invention maintains the most excellent horizontal attitude estimation accuracy and horizontal positioning accuracy during all GNSS signal loss periods. Before 90s, the external force estimation error has converged, and the system adaptively activates the dynamic-gravity alignment constraint. The dynamic-gravity alignment constraint improves the horizontal attitude estimation accuracy of the system during GNSS signal loss by providing horizontal attitude observations, significantly suppressing the drift of the system in the horizontal direction. At the same time, the system maintains an accuracy similar to that of EKF in the elevation direction. During GNSS loss, compared with EKF, the RMSE and MAX of the roll angle estimation accuracy of the DSKF-g scheme are improved by 35.0% and 31.7% respectively, the RMSE and MAX of the pitch angle estimation accuracy are improved by 23.8% and 5.6% respectively, and the RMSE and MAX of the horizontal position estimation accuracy are improved by 43.1% and 35.3% respectively.

[0110] Table 1 Horizontal attitude error statistics during GNSS signal loss

[0111]

[0112] Table 2 Point position error statistics during GNSS signal loss

[0113]

Claims

1. A GNSS / SINS / power fusion algorithm based on SKF, characterized in that, It includes the following steps: (1) Construct a simplified quadrotor dynamics model, model the motor thrust, and derive the Newtonian differential motion equations of the UAV in the w frame; (2) Calibrate the motor thrust coefficient c based on the recursive least squares algorithm t , and the specific steps are as follows: (2.1) Data acquisition: In a windless scenario, let the quadrotor UAV maintain a hovering state without carrying any load for a period of time, and collect the rotor speed data during this period; (2.2) Coarse calibration: Construct the rotor gravity observation equation through the force analysis of the UAV, and use the recursive least squares method to estimate c t ; (2.3) Iterative calibration: Repeat step (2.2), and use the single - step recursive least - squares calibrated thrust coefficient c t，k-1 as the initial value for the next recursive least - squares algorithm, and iterate repeatedly until the thrust coefficient converges; (3) Derive the dynamic state error equation, and based on this, augment the dynamic error state δx in the GNSS / SINS loose integration state model δx l ; D ; (4) Construct GNSS position measurement constraints, derive the GNSS position observation equation, and fuse the GNSS position measurement using the EKF method; (5) Construct dynamic velocity measurement constraints, derive the dynamic velocity observation equation, and update the dynamic velocity measurement using the SKF method; (6) Construct dynamic-gravity alignment constraints, derive the dynamic-gravity alignment observation equation, and adaptively activate the dynamic-gravity alignment constraints based on the estimated uncertainty difference of the continuous-time external force.

2. The GNSS / SINS / kinematics fusion algorithm based on SKF according to claim 1, wherein The total mass-normalized thrust generated by the motor in step (1) can be expressed in the b-frame as follows: where e z = [0, 0, 0, 1] T is the unit vector along the z-axis of the b system, c t = [c t，1 , c t，2 , c t，3 , c t，4 T are the thrust coefficients corresponding to the four rotors respectively, n T is the measurement white noise of the model's thrust in the horizontal direction;​ The Newtonian differential equation of the UAV in the w frame is: where represents the velocity of the b-frame relative to the w-frame calculated dynamically, is the attitude rotation matrix from the b-frame to the w-frame, is the normalized external force acting on the UAV in the b-frame, g w = [0, 0, -9.8] T is the gravitational acceleration in the w-frame.

3. The GNSS / SINS dynamic fusion algorithm based on SKF according to claim 1, wherein The rotor gravity observation equation described in step (2.2) is expressed as: where m represents the mass of the UAV, g w represents the acceleration due to gravity, c t，i represents the thrust coefficient of the i-th rotor, r i represents the rotational speed of the i-th rotor; The update process of the recursive least squares algorithm is expressed as: where k t is the gain weight, and represent the thrust coefficient weights at times t-1 and t respectively, is the observation matrix, R t is the measurement noise, and represent the estimated values of the thrust coefficient at times t-1 and t respectively, is the gravity observation of the UAV, and I represents the identity matrix.

4. According to the SKF-based GNSS / SINS / dynamics fusion algorithm described in claim 1, wherein step (2.3) determines whether the thrust coefficient converges based on the difference between adjacent thrust coefficients, and the specific calculation formula is: |c t,k -c t,k-1 | <α where c t，k-1 and c t，k represent the estimated thrust coefficient values at t - 1 and t times respectively, and α is the thrust coefficient iteration threshold selected according to the empirical value.

5. The GNSS / SINS / kinematics fusion algorithm based on SKF according to claim 1, wherein The dynamic error state propagation equation in step (3) is: where × represents a vector skew-symmetric matrix, is the external force estimation error in the b frame, c t is the thrust coefficient error, is the dynamic velocity error in the w frame, φ w represents the attitude misalignment angle error, is the external force estimation error in the b frame, δc t is the thrust coefficient error, is the dynamic velocity error in the w frame, n f is the external force random walk noise, 0 m×n represents an m-by-n zero matrix, D(r) = diag(r1, r2, r3, r4) and D(c t ) = diag(c t，1 c t，2 , c t，3 , c t，4 ) represent the diagonal matrices of the rotor speeds and thrust coefficients in the ideal state, respectively; The GNSS / SINS loose integration state δx l is as follows: where the superscript T represents the matrix transpose, and φ w , respectively represent the misalignment angle error, velocity error, and position error of the rotor b-frame relative to the w-frame, and δb g and δb a represent the zero bias errors of the gyroscope and accelerometer, respectively. The augmented dynamic error state δx D is as follows: The augmented GNSS / SINS / dynamics state δx is expressed as: δx = [δx I δx D T ​ The state transition matrix F, noise drive matrix G, and system noise n of δx are expressed as: Where I m×n represents an identity matrix of m rows and n columns. n g and represent the gyroscope measurement white noise and the gyroscope bias drive white noise respectively, n a and represent the accelerometer measurement white noise and the accelerometer bias drive white noise respectively, n r is the rotational speed measurement white noise.

6. The GNSS / SINS / kinematics fusion algorithm based on SKF according to claim 1, wherein The GNSS position observation equation in step (4) is: where δz G is the position measurement, is the SINS-derived position, is the GNSS position observation in the w frame, V G is the white noise of the GNSS position measurement; The measurement update process of the EKF is expressed as: K t = P t / t-1 H G T (H G P t / t-1 H G T + V G ) -1 δx t = δx t / t-1 + K t (δz G - H G δx t / t-1 ) P t = (I - K t H G )P t / t-1 where H G = [0 3×6 I 3×3 0 3×15 is the GNSS position measurement matrix, V G is the GNSS position measurement white noise, K t represents the Kalman gain, P t / t-1 is the predicted covariance, δx t / t-1 is the predicted state, δx t and P t are the a posteriori estimated state and covariance.

7. The GNSS / SINS / Kinetics fusion algorithm based on SKF according to claim 1, wherein The dynamic velocity observation equation in step (5) is: where δz D is the dynamic velocity measurement, is the SINS-derived velocity, is the dynamically calculated velocity, V D is the white noise of the dynamic velocity measurement. The posterior state update process of the SKF can be expressed as: where K D represents the Kalman gain matrix for the dynamic state after dynamic measurement, ΔP ID represents δx I and the cross-covariance matrix of δx D , ΔP DD represents the dynamic state covariance matrix, H D = [0 3×3 I 3×3 0 3×12 -I 3×3 0 3×4 represents the dynamic velocity measurement matrix.

8. The GNSS / SINS / kinematics fusion algorithm based on SKF according to claim 1, wherein The dynamic-gravity alignment observation equation in step (6) is: where δz g is the dynamic-gravity alignment measurement, is for calculating the normalized total motor thrust, is for calculating the normalized external force state, V g is the white noise of the dynamic-gravity alignment measurement; The calculation method of the estimated uncertainty difference of the continuous-time external force is as follows: where tr(·) represents the trace of a matrix, denotes the square root, and |·| represents the absolute value; and represent the external force estimation covariance matrices at times t - 1 and t respectively, and Δu f represents the difference in the external force estimation covariance between t - 1 and t.

Images