A distributed and low-order GPS/SINS algorithm of flight parameters estimation for unmanned vehicle

1. Introduction

Flight control system and strapdown inertial navigation system are the key technologies of unmanned aerial vehicle (UAV). A properly designed flight control law and control structure can improve the stability and flight qualities of UAV. Because of that solving control law is based on the flight parameters from the onboard strapdown inertial navigation system, so the algorithm of strapdown inertial navigation is taken into consideration increasingly. The traditional platform-style inertial navigation system is difficult to be applied in UAV system due to the complex structure and cost-expensive, while the MEMS-based strapdown inertial navigation system with small-size, light-weight and low-cost improve the accuracy of flight control parameters by designing multi-sensors fusion algorithms [1-5]. Literature [6] described a linear Kalman filter by using differential equation of orientation cosine matrix with the state vector constructed by the measurements of tri-axial magnetometers and tri-axial accelerometers, which improved the computation cost effectively. Kim et al.[7] presented a real-time quaternion-based attitude estimation system, of which the state equation was developed according to the relationship between quaternion and attitude angular. The measurement models of those algorithms were derived from the onboard accelerometers, which lead to high deviation of attitude in accelerated flight condition. Bijker [8] proposed two low-order extended Kalman filters for estimating the attitude by using MEMS gyro, accelerometer and magnetometer, meanwhile, the velocity and position estimation algorithms were given by using a loosely integrated GPS receiver. The nonlinear algorithms were deduced by the quaternion kinematics equation, which should be linearized to get attitude with large computational expense, and the rounding error would be introduced in the process of linearization. In this paper, a distributed and low-order GPS/SINS algorithm for UAV flight control system is designed. The attitude estimation Kalman filter is developed according to angular rate integration. The drift noise of gyro is solved and compensated by state matrix in real time, and the integrated error of gyro is eliminated effectively in recursion. The differential velocity from GPS is used to improve attitude accuracy in flight condition. On the basis of attitude estimation, a cascaded concept is utilized in designing velocity and position algorithms to reduce the filter order effectively. The reliability and practicability of presented algorithms are verified by flight tests of a small UAV.

2. GPS/SINS algorithm structure

The series-connected structure of the algorithm is shown in Fig. 1. The original analog datum, collected from the MEMS gyro and accelerometer, are filtered through the onboard active low-pass filter to reduce the high frequency noise. The digital signals from the A/D are again filtered by FIR before entering the attitude Kalman filter. The body high-frequency vibration and noise in the circuit are reduced greatly. The differential velocity from GPS is used to compensate the continuous acceleration in order to improve the accuracy of the measured attitude. With the attitude angle as an input, the velocity estimation algorithm is developed according to the rigid body dynamics equations, and the position algorithm is also deduced by the relations between velocity and position.

Fig. 1GPS/SINS algorithm architecture

3. Algorithm of attitude determination

Attitude determination system is a core technology of the strapdown inertial navigation system for UAV. In this paper, a low-order Kalman filter is used for solving attitude which is derived from the integration of the angular rate and the drift characteristics of the rate gyro is estimated in each iteration in real time.

3.1. Measurement model of attitude

The measured attitude can be obtained from onboard accelerometer and magnetometer, so the measured noise of the measurement system depends on the features of accelerometer and magnetometer. Since the accelerometer simultaneously measures the gravity and the rigid body acceleration, the slight difference between the measured attitude from accelerometer and the actual flight attitude comes up under speed-up flight conditions. The differential velocity from GPS compensates the signal from accelerometer and improves the accuracy of measured attitude in flight condition, so the compensated specific force can be given as:

1

$\left[\begin{array}{c}{f}_n\\ f_e\\ f_d\end{array}\right]=\left[\begin{array}{c}{f}_{nb}\\ f_{eb}\\ f_{db}\end{array}\right]-{\mathbf{C}}_b^n\left[\begin{array}{c}{f}_{ng}\\ f_{eg}\\ f_{dg}\end{array}\right],$

where $f_{ng},f_{eg},f_{dg}$ are the differential velocities from GPS in earth frame, $f_{nb},f_{eb},f_{db}$ represent the measured specific force from body accelerations, and $f_n,f_e,f_d$ are the compensated specific force, ${\mathbf{C}}_b^n$ is the attitude transformation matrix from earth to body frame as follows:

2

${\mathbf{C}}_b^n=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta \mathrm{c}\mathrm{o}\mathrm{s}\psi & \mathrm{c}\mathrm{o}\mathrm{s}\vartheta \mathrm{s}\mathrm{i}\mathrm{n}\psi & -\mathrm{s}\mathrm{i}\mathrm{n}\vartheta \\ \mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta \mathrm{c}\mathrm{o}\mathrm{s}\psi -\mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{s}\mathrm{i}\mathrm{n}\psi & \mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta \mathrm{s}\mathrm{i}\mathrm{n}\psi +\mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{c}\mathrm{o}\mathrm{s}\psi & \mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{c}\mathrm{o}\mathrm{s}\vartheta \\ \mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta \mathrm{c}\mathrm{o}\mathrm{s}\psi +\mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{s}\mathrm{i}\mathrm{n}\psi & \mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta \mathrm{s}\mathrm{i}\mathrm{n}\psi -\mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{c}\mathrm{o}\mathrm{s}\psi & \mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{c}\mathrm{o}\mathrm{s}\vartheta \end{array}\right],$

where $\phi ,\vartheta ,\psi$ stand for roll, pitch and yaw angles of the UAV respectively. The measured attitude of roll and pitch angles with the compensated specific force in the body frame are obtained as:

3

${\phi }_a=\arctan \left(\frac{f_e}{f_d}\right),$
${\vartheta }_a=-\arctan \left(\frac{f_n}{\sqrt{f_e^2+f_d^2}}\right).$

The Earth’s magnetic field $H_0$ is defined according to its orientation relative to the geographic frame, which known as the angle of magnetic declination $\chi$ and dip angle $\lambda$ relative to horizontal [9], as indicates in Fig. 2.

Fig. 2Distribution of Earth’s magnetic field

The magnetic field $H_0$ is mapped into the horizontal component ($H_0\mathrm{c}\mathrm{o}\mathrm{s}\lambda$) and vertical component ($H_0\mathrm{s}\mathrm{i}\mathrm{n}\lambda$), and then the magnetic measurements in body sensitive axis are:

4

$\left[\begin{array}{c}{m}_n^b\\ m_e^b\\ m_d^b\end{array}\right]=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\vartheta & 0& -\mathrm{s}\mathrm{i}\mathrm{n}\vartheta \\ \mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta & \mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{s}\mathrm{i}\mathrm{n}\phi \mathrm{c}\mathrm{o}\mathrm{s}\vartheta \\ \mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{s}\mathrm{i}\mathrm{n}\vartheta & -\mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\phi \mathrm{c}\mathrm{o}\mathrm{s}\vartheta \end{array}\right]\left[\begin{array}{c}{H}_{0}cos\lambda cos{\psi }_m\\ -H_{0}cos\lambda sin{\psi }_m\\ H_{0}sin\lambda \end{array}\right],$

where ${\psi }_m$ is the magnetic heading angle. Defining:

5

$H_x=H_0\mathrm{c}\mathrm{o}\mathrm{s}\lambda \mathrm{c}\mathrm{o}\mathrm{s}{\psi }_m,$
$H_y=-H_0\mathrm{c}\mathrm{o}\mathrm{s}\lambda \mathrm{s}\mathrm{i}\mathrm{n}{\psi }_m.$

Therefore, the measured magnetic heading angle is constructed as:

6

${\psi }_m=\arctan \left(-\frac{H_y}{H_x}\right)=\arctan \left(-\frac{m_e^b\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_a-m_d^b\mathrm{s}\mathrm{i}\mathrm{n}{\phi }_a}{m_n^b\mathrm{c}\mathrm{o}\mathrm{s}{\vartheta }_a+m_e^b\mathrm{s}\mathrm{i}\mathrm{n}{\vartheta }_a\mathrm{s}\mathrm{i}\mathrm{n}\phi +m_d^b\mathrm{s}\mathrm{i}\mathrm{n}{\vartheta }_a\mathrm{c}\mathrm{o}\mathrm{s}{\phi }_a}\right),$

where $m_n^b,m_e^b,m_d^b$ are the components of body frame magnetic filed, which measured by tri-axial magnetometers, respectively. Then the measured yaw angle ${\psi }_a$ as below:

7

${\psi }_a={\psi }_m+\chi .$

4. Algorithms for estimation of velocity and position

5. Numerical simulation of attitude estimation

Attitude estimation system could measure tri-axial angular rates, attitudes and heading parameters in real time, which corresponding to damping control loop and attitude control loop of flight control system for the UAV respectively. The accuracy of attitude estimation affects the stability of flight control system, and as the inner loop of the cascaded Kalman filter, it also influences the accuracy of velocity and position estimation. Therefore, the performance of the attitude estimation is verified in numerical simulation. The control plant is described by using six-degree rigid body equations of motion. In this simulation, the update rate of the attitude estimation system is 20 Hz, the measured noise covariance of MEMS gyro is 0.05 ^o/s, the measured noise of accelerometer is about 10 mg, and the GPS update rate is 4 Hz. Figure 3 shows the differences between the actual attitudes and the filter outputs, with roll and pitch moments apply on the control plant. The designed algorithm is satisfied to track the target movement in real time. When the controlled plant is applied on continuous nose-down torque, and two 25 s continuous longitudinal forces, as shown in Fig. 4, the simulation results indicate that the pitch angle of controlled plant decreases gradually. At 100 s, the onboard accelerometer measures the rigid body motion and the gravity component simultaneously, which results in a constant deviation pitch angle without GPS compensation. After the on-line acceleration compensation, the output from filter has a good accuracy of pitch angle in the flight condition.

6. Low-cost GPS/SINS hardware design

To demonstrate the effectiveness of the proposed algorithms, the onboard avionics of the GPS/SINS is developed and the prototype system is designed according to payload and performance requirements of the small tiltrotor. Figure 5 shows the block diagram of onboard GPS/SINS, which contains power supplier, micro-inertial measurement unit (MIMU) and navigation unit.

Fig. 3Attitude outputs under action of moments

Fig. 4Pitch angle output in flight condition

The main consideration in the power suppler design is to meet the whole experimental requirements and total system safety. Based on the detail power consumptions of the onboard avionics, the lithium–polymer battery with 2200 mAh capacity, 25 C discharge rate, 185 g weight and 11.1 V output voltage is selected, and the voltage is converted to 5 V by a DC/DC model.

The MIMU plays an important role in collecting signals from MEMS sensors, which include angular rate, acceleration and magnetic filed. The collecting process is complemented on an 8-bit Atmel ATmega16 micro-controller which utilizes six AD channels to acquire the analog digitals from gyro and accelerometer, also collects digital signal from magnetometer via standard SPI interface. In real environment, the outputs of MIMU often suffer from large sensor noise. In order to reduce the initial noise, the collected signals are preprocessed by low-pass filter, packaged and sent to the navigation unit via a standard RS232 serial port.

ARM2440 is used to collect datum from MIMU and GPS, execute attitude velocity and position algorithms, carry out control laws and drive actuators. It clocks at 533 MHz and supports various peripherals.

The specific hardware components in the airborne prototype are shown in Fig. 6. A properly sized aluminum box is designed to contain the onboard hardware components and protect them from being contaminated.

Fig. 5GPS/SINS block diagram

Fig. 6The onboard prototype of the GPS/SINS

The onboard avionics must be small, light and cost-effective enough for installing into the small tiltrotor aircraft. Table 1 lists the specifications of the MEMS sensors.

7. Flight validation

Experiments are performed on a small unmanned tiltrotor to validate the stability of the proposed algorithms. The attitude filter update rate is 100 Hz and GPS update rate is 4 Hz. Fig. 7 shows the small unmanned tiltrotor in flight tests, which consists of rotorcraft, wing, fuselage, engine nacelle, horizontal and vertical tails, driving mechanism, flight control system, strapdown inertial navigation system and landing gear et al.

Fig. 7Unmanned tiltrotor aircraft in the flight tests

7.2. Full envelop flight tests

The small unmanned tiltrotor has three modes: helicopter, tilting and airplane flights. The airspeed varies dramatically during the tilting mode. The designed strapdown navigation system can measure flight parameters in the speed-up flight condition in real time. Fig. 12 shows the feature of the pitch angle in the full flight envelop. The pitch angle varies smoothly in the helicopter and airplane flight modes, while varying drastically in the tilting flight mode due to complex flight condition and the center of gravity shifting with tilting nacelle angle. Fig. 13 shows the characteristics of the forward velocity. The small tiltrotor speed up from the helicopter to tilting mode, the airspeed increases fastly with the nacelle tilting. After 10 seconds, the tiltrotor enters into the airplane mode, and continues speed up until 44 m/s. Then, the nacelle starts tilting from horizon to vertical position and the speed slows down into hover flight of helicopter mode finally. The flight tests results show that the attitude changes severely during the tilting mode in that the designed control law in helicopter mode is not quite robust in tilting mode for the flight environment changes significantly.

Fig. 8The tri-axial attitude angles in hover

Fig. 9The tri-axial velocities in hover

Fig. 10GPS vs KF output of north

Fig. 11GPS vs KF output of east

Fig. 12Pitch angle of the full envelop flight

Fig. 13Forward speed of the full envelop flight

8. Conclusions

In this work, a whole set of navigation algorithm is developed for a small unmanned tiltrotor. The hardware of the onboard navigation system is designed and implemented successfully. A series of actual flight tests are conducted and the following conclusions could be given:

(1) The linear attitude estimation algorithm is designed on the basis of the integration of angular rate. The drift noise of gyro is solved in real time and compensated simultaneously, and the GPS is introduced to modify the accuracy of attitude estimation. The numerical simulation indicates a good performance of the measured attitude.

(2) The algorithms of the attitude, velocity and position for UAV adopt the cascaded linear Kalman filter, which fully use the output parameters from the former. The order of the full-state filter is reduced, and the computational expense is saved effectively.

(3) The onboard flight control law of the unmanned tiltrotor is implemented by utilizing the filtered data from strapdown inertial navigation system. The reliability of the proposed algorithms are verified under full flight envelop.