GPS/INS Integrated Navigation System for Autonomous Robot

Use your smartphone to scan this QR code and download this article ABSTRACT Nowadays, autonomous robots are capable of replacing people with hard work or in dangerous environments, so this field is rapidly developing. One of the most important tasks in controlling these robots is to determine its current position. The Global Positioning System (GPS) was originally developed for military purposes but is nowwidely used for civilian purposes such asmapping, navigation for land vehicles, marine, etc. However, GPS has some disadvantages like the update rate is low or sometimes the satellites' signal is suspended. Another navigation system is the Inertial Navigation System (INS) can allow you to determine position, velocity and attitude from the subject's status, like acceleration and rotation rate. Essentially, INS is a dead-reckoning system so it has a huge cumulative error. An effective method is to integrate GPS with INS, in which the center is a nonlinear estimator (e.g. the Extended Kalman filter) to determine the navigation error, from which it can update the position the object more accurately. To improve even better accuracy, this paper proposes new method which combines the original integrated GPS/INS with tri-axis rotation angles estimation and velocity constraints. The experimental system is built on a low-cost IMU with tri-axis gyroscope, accelerometer and magnetometer and a GPS module to verify the model algorithm. Experiment results have shown that the rotation angles estimator helps us to determine the Euler angles correctly, thereby increasing the quality of the position and velocity estimation. In practice, the accuracy of roll and pitch angle is 2 degrees, the error of yaw angle is still large. The achieved horizontal accuracy is 2m when the GPS signal is stable and 3m when the GPS signal is lost in a short period. Compared with individual GPS, the error of the integrated system is about 10% smaller.


INTRODUCTION
For autonomous robots (such as USV, UAV, AUV, etc.) to work in a stable and efficient manner, navigation is one of the most important issues to be aware of. The Global Positioning System (GPS) was originally developed for military purposes but is now widely used for civilian purposes such as mapping, navigation for land vehicles, marine, etc. However, GPS has some disadvantages like the update rate is low or sometimes the satellites' signal is suspended. Another navigation system is the Inertial Navigation System (INS) can allow you to determine position, velocity and attitude from the subject's status, like acceleration and rotation rate. Essentially, INS is a dead-reckoning system so it has a huge cumulative error. An effective method is to integrate GPS with INS, in which the center is a nonlinear estimator (e.g. the Extended Kalman filter) to determine the navigation error, from which it can update the position the object more accurately 1 .
Depending on the "depth" of the interaction and for the shared information between the GPS and INS, we have some integration methods: uncoupled integration, loosely coupled (LC), tightly coupled (TC) and deeply integrated 2 . For uncoupled method, GPS output is used as the "reset" signal for the INS. When there is no GPS solution (position and velocity), the integrated system uses INS to estimate. This method has the simplest structure, but the system cannot estimate the sensor's drift, so it is not commonly used. In LC method, GPS solutions will be compared with the inertial estimation in order to calculate the error state of the object 3,4 . In TC method the integration is "deeper", the raw measurements of the GPS (pseudorange and Doppler measurements) are directly combined with the calculated INS estimation results in an appropriate filter 4,5 . Both LC and TC systems operate in closed loop, i.e. position, velocity, attitude errors and sensor's drifts are fed back for the INS and GPS to make a navigation correction. The loosely coupled model is simpler than the tightly one. The structure of tightly coupled model and deeply integration are very complex, so we do not mention in this paper. In estimating Euler angles, conventional INS systems use tri-axis angular rate to calculate these angles. However, MEMS (Micro-Electro-Mechanical Systems) IMUs often have large disturbances, so their errors often accumulate rapidly. The INS mechanization method for update rotation angles is only available for a short period. In this paper we use a triangular Euler angles estimator. The centerpiece of this estimator is the two-stage Extended Kalman filter, using accelerometer and magnetic field values to correct the angles evaluated using rotation rate 6 . This paper introduces the building method of loosely coupled GPS/INS integrated navigation system. The Euler angles estimation and the velocity constraints are used to improve accuracy. We use MAT-LAB/Simulink software to simulate and analyze data. The experimental system is built on a low-cost IMU with tri-axis gyroscope, accelerometer and magnetometer and a GPS module to verify the model algorithm. The update rate of the integrated system is equal to the INS rate of 100 Hz and the rate of GPS is 10 Hz. The data acquisition and processing system is performed on an ARM Cortex-M4 microcontroller. INS is a navigation system that uses tri-axis inertial sensors (gyroscope, accelerometer and magnetometer) to calculate the orientation and position of an object. This system does not need an external reference, so it can continuously calculate without interruption. In this paper, outputs of inertial sensors are three components of the gyroscope, three components of the accelerometer and three components of the magnetometer in the body-frame, denoted by f b , ω b , m b respectively. The Figure 1 describes the INS mechanization in NED frame 3 . The INS uses rotation rate and acceleration values from the IMU sensor to update attitude, velocity and position by the following formula:

Inertial navigation system
In this formula, vector r n = [φ λ h] T is the position vector, whose components are geographic latitude, longitude and altitude (height) respectively.
Vector v n is the velocity vector in NED coordinate. Matrix C n b is the direction cosine matrix (DCM, or rotation matrix) from body-frame to NED frame. The symbols ω and Ω denote the angular rate and its skew symmetric form, matrix D is the transition matrix from NED frame to latitude, longitude and altitude: Formula (1) is written in continuous form. In experiment, we have to discretize it for simplicity in calculation. Because of this discretization, the update process always has error. On the other hand, IMU sensor has other types of error like bias and scale factor. Thus, the INS errors are rapidly accumulating.
To improve the accuracy of the navigation estimation, we use a tri-axis Euler angles estimator 6 . It is structured as a two-stage cascaded Extended Kalman filter (Figure 2). These filters use acceleration and magnetic field measured from the IMU to correct the Euler angles (roll, pitch and yaw). Precisely, first the EKF-1 combines the gyroscope and accelerometer measurements to calculate the Earth's gravity vector in NED frame, and then it can determine roll and pitch angles. Next, the EKF-2 uses the gyroscope, magnetic field measurements and the determined roll and pitch to calculate yaw angle. In experimental conditions, the accuracy of roll and pitch is 1 degree and accuracy of yaw angle is 3 degrees. Rotation rate and acceleration measurements can be affected by noises such as deviation, scale factor, nonorthogonality and some other types. Some types of error can be identified and calibrated in the laboratory environment. Some types are unpredictable, and have to be modeled as random noise. In the above factors, bias has the greatest effect on the measurement value of the IMU. So we can model the remaining types of noise (except bias) as white noise (denoted by the symbol η), we have the estimation equation: In (3), the symbols f and ω are estimated values of acceleration and angular velocity, f and ω are values measured from the IMU sensor, the symbol b is the bias and η is the other types of noise (modeled as white noise).

Loosely Coupled scheme
The loosely coupled model also referred to as "decentralized" filtering, consists of two estimators. The first one is a nonlinear estimator. It combines the INS estimation results with the GPS results to estimate the position, velocity, attitude error and the IMU sensor's error. The second is the GPS filter. It uses the pseudorange and Doppler measurement values from GPS module to determine the position, velocity. Figure 3 shows the diagram of loosely coupled model. In today's GPS modules, there is usually a built-in GPS data processor, which can calculate position, velocity and some other information from GPS raw data. In the LC model, position and velocity are fed into the nonlinear filter. The filter used in this paper is the Extended Kalman filter, which is suitable for nonlinear systems. Measurement values from IMU sensor (angular rate and acceleration) after being computed using the Euler angles estimation and INS mechanization, will be compared with the position and velocity of the GPS. The difference between two results is used as the input of the EKF. The integrated system works in closed loop, the estimated error values are fed back to adjust the state of the INS system and to compensate for the IMU measurements. This closeloop model is suitable for MEMS IMU, which has large disturbance. The error state vector δ x of the EKF filter in this model is composed of the position error δ r n , the velocity error δ v n , the attitude error ε, the acceleration bias error δ b a and the gyroscope bias error δ b g . Derive the INS mechanization function and take the first order elements 3 , we have the process model equation: In matrix F, τ ba and τ bg are the correlation time vectors of accelerometers and gyroscopes, determined based on the Gauss-Markov model. The components of vector u are white noises, with the covariance determined by the formula: , q = 2σ 2 τ (7) In the above formula, σ is the standard deviation of the Gauss-Markov noise. Matrix Q is called the spectral density matrix and its component, respectively, are covariance accelerometer, gyroscope, accelerometer bias and gyroscope bias. These values can be determined in the datasheet of the sensor 5 . The measurement model of the EKF is the difference of the INS results (position and velocity) and GPS results: In the above equation, symbol ε is the measurement noise. Its covariance matrix R can be obtained from GPS processing. The activation of the EKF is divided into 2 stages: update and prediction. The Kalman gain is computed first in the update stage. Then state variables (δ x) and error covariance (P) are updated based on prior estimates and its error covariance. After each correction, the error state vector should be reset to zero. When there is a GPS outage, we can use velocity constraints (Figure 4) to estimate errors 4 . Vehicles essentially move in forward direction. If the vehicle does not jump off the ground nor slide on the ground, its velocity in the axes perpendicular to the forward direction (y-axis and z-axis) is almost zero. So we have two velocity constraints:

SIMULATION RESULTS
In simulation, we use FlightGear simulation software 7 to create the data file and use MAT-LAB/Simulink to process it. The GPS signal is disturbed with noise to research about noise suppression of the estimator. The standard deviation of noise is 2.5 m in each horizontal axis and 5 m in vertical axis. Simulations were made in two cases: with and without the Euler angles estimator. We have the result   accuracy is about 0.64 meters. The velocity error is within 0.1 m/s. We can conclude that the estimator has good filtering capability. Next, we will examine the quality of the system when the GPS signal is lost in intervals of 3, 5 and 10 seconds. From Table 2, we can conclude that when there is a GPS outage, the error of the system will be larger than the normal case (GPS fix). In addition, if the GPS lost time is longer, the horizontal error is larger. Using an Euler angles estimator helps to make smaller errors.

Hardware development
We built an experimental system to verify the implemented algorithm. The hardware (Figure 5) consists of the IMU sensor ADIS16405 from Analog Devices 8 , the GPS module from U-blox 9 and the microcontroller STM32F407 (ARM Cortex-M4) from STMicroelectronics 10

Results
For MEMS IMU sensors, the amplitude of its noise is huge, so if we do not use the Euler angles estimator, the result is bad, the attitude, position, velocity errors are enormous. The estimated trajectory (red dots in Figure 6) does not have the same shape with the reference one (black line). In contrast, when we use the angles estimator, the errors are smaller, the accuracy is higher. The horizontal error of our GPS/INS system is 1.69 m, while the error of the individual GPS system is 1.93 m. For this reason, the GPS/INS algorithm can reduce over 10% of the error. On the other hand, the update rate of GPS is only 10 Hz. The integrated GPS/INS update rate is 10 times larger (100 Hz). We can see the effective of high update rate in Figure 6. Because the GPS has the low update rate of 10 Hz, there are visible spaces between the green dots (GPS-only). And if the vehicle moves very fast, the GPS cannot describe the vehicle's trajectory accurately. Differently, the blue dots (GPS/INS) approx-     imately form a continuous line. From the above results, it can be concluded that the angles estimator can improve the accuracy of the navigation system and the integrated GPS/INS system performs better than the single GPS system ( Table 3).
Next, assuming the GPS signal is lost for a period of 5 seconds, we will analyze the accuracy of implemented GPS/INS system in cases with and without speed constraints. We will simulate GPS outages in two cases: GPS lost in straight line and in curved line (

CONCLUSIONS
In this paper, we have implemented a loosely coupled GPS/INS integrated navigation system. The main al-gorithm in this system is the Extended Kalman filter. We combined the EKF with Euler angles estimator and velocity constraints to improve accuracy.
The rotation angles estimator helps us to determine the Euler angles correctly, thereby increasing the quality of the position and velocity estimation. In practice, the accuracy of roll and pitch angle is 2 degrees, the error of yaw angle is still large.
The achieved horizontal accuracy is 2m when the GPS signal is stable and 3m when the GPS signal is lost in a short period. Compared with individual GPS, the error of the integrated system is about 10% smaller. In addition, the positive point of the GPS/INS is its update rate reaches 100 Hz, which is 10 times larger than the initial system. When there is a long-period GPS outage, the LC algorithm's result is very bad, so we need to use the tightly coupled model. In the future, we will research about this model, point out its advantages and disadvantages, and compare with the original model. After that, we will find the optimal switching method between two models.