1. INTRODUCTION
Intelligent mobile robots play a key role in exploration, search and rescue and transportation, and they are receiving more attention from major developed countries (Yudanto and Petre, Reference Yudanto and Petre2015). Autonomous navigation technology is the basis of research into intelligent mobile robots, but also has difficulties. In the past few decades, there have been many research studies on mobile robot localisation technology. There are many positioning methods for the various types of sensor technologies, which can be roughly classified into two categories: relative positioning techniques and absolute positioning technologies (Huang et al., Reference Huang, Zhu, Yang, Xue, Wu and Zhao2015). Relative positioning determines the positioning based on the condition that the robot is told its initial position. An Inertial Navigation System (INS) or odometer is usually used to provide relative positioning information to the robot (Barshan and Durrant-Whyte, Reference Barshan and Durrant-Whyte1995; Mautz and Tilch, Reference Mautz and Tilch2011; Borenstein et al., Reference Borenstein, Miller and Borrell2012). This positioning method is not dependent on the external environment or the reference but relies only on its positioning system. However, it cannot be used for navigation over a long period, since the positioning error accumulates over time. Absolute positioning determines the position without the robot being told its initial position. In theory, it requires multiple reference points of known locations, such as trilateral localisation or triangulation localisation. Common positioning technologies, including ultrasonic, Wireless Fidelity (WIFI), ZigBee, Bluetooth and Radio Frequency Identification (RFID) technology (Liu et al., Reference Liu, Darabi, Banerjee and Liu2007; Wang et al., Reference Wang, Yoshida, Zhou and Jing2015; Fu and Retscher, Reference Fu and Retscher2009), all belong to the absolute positioning category. The drawback of these positioning technologies is that they are easily affected by the external environment, such as the Non-Line of Sight (NLOS) factor and the multi-path effect, which can result in large positioning errors.
To achieve high precision positioning in a closed environment, many scholars have used a combined positioning method to address the shortcomings of each individual technology. For example, Zhao et al. (Reference Zhao, Guan, Landry, Cheng and Sydorenko2015) used the Federated Kalman Filter (FKF) to fuse INS and laser technology and improved the fault tolerance. Ko and Kuc (Reference Ko and Kuc2015) used an Unscented Kalman Filter (UKF) algorithm to fuse ultrasonic technology and laser technology, and adjusted the parameters to make full use of the advantages of these sensors. Du et al. (Reference Du, Zhang and Li2015) analysed robot kinematics and installed an inertial sensor and a position sensor on the robot to realise self-tuning based on the EKF algorithm. Atia et al. (Reference Atia, Liu, Nematallah and Karamat2015) proposed a ground vehicle navigation system that integrates INS, a laser radar, a Wireless Local Area Network (WLAN) and an odometer using both tight coupling and loose coupling. Wang et al. (Reference Wang, Yoshida, Zhou and Jing2015) designed a new adaptive algorithm to achieve real-time tracking of a mobile robot by using visual odometry and inertial sensors. Most of these combined positioning systems can be classed as a combination of both relative positioning and absolute positioning. These methods can address the shortcomings of both individual technologies to a certain degree and can be used to realise self-calibration of robot navigation.
In absolute positioning systems, Ultra Wide Band (UWB) wireless radio systems have a high positioning accuracy, strong anti-jamming performance as well as other advantages, and have been the focus of much attention. Some scholars have undertaken research in combination with an INS. For example, Hol et al. (Reference Hol, Dijkstra, Luinge and Schon2009) proposed a Six Degree Of Freedom (6DOF) tracking system combining UWB measurements with low-cost MEMS inertial measurements. However, this includes many quaternions and integral calculations. Fan et al. (Reference Fan, Wu, Hui, Wu, Yu and Zhou2014) designed a new method based on INS/UWB for the attitude angle and position synchronous tracking of an indoor carrier, but their method involves many variables and includes some coordinate transformation equations. Yudanto and Petre (Reference Yudanto and Petre2015) presented a novel sensor fusion approach using UWB and INS for real-time indoor navigation and tracking of Autonomous Ground Vehicles (AGVs) and mobile robots in factories and warehouses. However, the system model they propose is a nonlinear system, and needs a lot of calculation to convert the nonlinear problem into a linear problem. The above researchers have focused on modifying the system coupling model and improving the optimal filtering algorithm. However, the calculation process of the above methods is too complex and they fail to fully collect the motion information of the mobile robot. In view of the above problems, this paper proposes a linear system model based on the kinematics of a mobile robot. The model contains many zero variables and reduces the calculation cost.
From the aspect of the filtering algorithm, the most commonly used algorithm for integrated navigation is the Kalman Filter (KF). However, the conventional Kalman filtering algorithm needs an accurate system model and the statistical characteristics of noise. It also lacks immunity to error measurement data and tolerance of sudden failures of a sensor, which can reduce the filtering accuracy and may even lead to divergence of the filter. During the actual process of motion, it is not possible to determine the noise characteristics of the positioning system as the measurement is easily affected by the complex closed environment and there will be some outliers in the observation values. To solve these problems, adaptive filtering algorithms such as Fuzzy Adaptive Filtering (Yang et al., Reference Yang, Li and Luo2015), Sage-Husa Adaptive Filtering (SHAF) (Guo et al., Reference Guo, Guo, Yu, Hong, Xiong and Tian2015) and Strong Tracking Filtering (STF) (Zhou and Xiao, Reference Zhou and Xiao2012), are always used in the actual system. The Sage-Husa adaptive filtering algorithm can estimate and correct the statistical characteristics of noise, but it does not have the ability to perform online estimation of the system noise and the measurement noise, which easily leads to divergence in high-order systems. It is widely accepted that system noise in an integrated navigation system is stable, so it usually only estimates the measurement noise. Guo et al. (Reference Guo, Guo, Yu, Hong, Xiong and Tian2015) applied the Sage-Husa adaptive Kalman filter with non-holonomic constraints and forward/backward filtering to an IMU/GPS integrated system, but due to the high dimensions of the combined system state, filtering divergence may still occur. Song and Liu (Reference Song and Liu2015) introduced a weighted adaptive filtering algorithm for autonomous radio, which solves the problem of the negative noise covariance matrix. However, Q and R have been estimated for each filter process, which will affect the real-time performance. Li et al. (Reference Li, Zhang, Wang and Chao2015) proposed an interacting multiple model adaptive algorithm based on the Sage-Husa Kalman filter and a strong tracking Kalman filter to solve the problem of filtering divergence. However, the measuring outliers of the system were not analysed, reducing the system fault tolerance. At present, many algorithms have been proposed to deal with outliers. The least squares estimation and the difference technique have problems such as missed detection, error detection and real-time problems (Zhu et al., Reference Zhu, Qiu, Li and Huang2004). Wei and Wu (Reference Wei and Wu2003) proposed a wavelet de-noising technique based on threshold processing, but it can only be used for post-processing. Zheng et al. (Reference Zheng, Cheng and Wan2005) used the Kalman filter based on a fuzzy controller to deal with outliers. Simulation results show that this method depends strongly on fuzzy rules. Liu et al. (Reference Liu, Yao, Lu and Ma2003) modified the orthogonality of KF innovation to restrain outliers, but they did not take the multi-input and multi-output system into account. In this paper, the corresponding theoretical derivation is carried out and applied to the multi-input and multi-output positioning system of a mobile robot.
To realise high precision positioning of a mobile robot in a closed environment, we propose a mobile robot autonomous navigation system based on tightly coupled INS/UWB. INS is used within the integrated positioning system to obtain the robot's position, velocity and azimuth information. UWB is used to provide the robot's position and velocity measurements and correct the position and the velocity of the robot. A two-dimensional kinematic model of the mobile robot is established based on kinematics analysis. An Auto-Regressive (AR) algorithm is used to establish third-order error equations for the gyroscope and the accelerometer. An Improved Adaptive Kalman Filter (IAKF) algorithm is also proposed, based on a simplified Sage-Husa adaptive filtering algorithm. An orthogonality judgment method of innovation values is used to identify outliers, and a covariance matching technique is introduced to judge the filter state. The proposed IAKF algorithm improves the robustness and fault tolerance of the integrated positioning system, and achieves high precision real-time tracking of the two-dimensional plane. The experimental results show that the combined positioning system can achieve positioning accuracy of within 0·24 m, which proves the validity of the combined system.
2. SYSTEM MODEL
2.1. The Design of INS/UWB Tightly Coupled Model
The principle framework for the combined positioning system based on INS and UWB is shown in Figure 1, which displays the overall process for the combined positioning system from signal collection through to attitude output. The INS obtains the acceleration and angular velocity information through a MEMS sensor, proposes the recursive gyro data using the real-time attitude angles of the system obtained using the quaternion method through initial calibration of the magnetic compass, and uses the accelerometer data to obtain the speed and position information of the carriers by integration.
Principle framework of combined positioning system.
The UWB wireless positioning system uses wireless positioning base stations to obtain the signal of the positioning tag, and the Time Difference Of Arrival (TDOA) algorithm is applied to obtain the position and velocity information of the carriers. Data fusion is realised by establishing a system error equation, taking the difference between the velocity and position values of the INS and the UWB wireless positioning system as the measurement vector and the error value inside the system as the state vector, and estimating and correcting the error within the system using a Kalman fusion filtering algorithm, which improves the positioning accuracy.
2.2. Kinematic Analysis
The East-North-Up coordinates are selected as the reference coordinates to establish a kinematics model of the mobile robot through kinematic analysis, as shown in Figure 2.
Kinematic model of the mobile robot.
A mobile robot shows very small height changes in indoor environments, so the motion of the mobile robot can be simplified to motion in a 2D plane. The kinematics equation of the mobile robot can be obtained from Figure 2.
$$\left[\matrix{{{\dot P}_e}\cr{\dot{P}_n } \cr {\dot{V}_e } \cr {\dot{V}_n } \cr {\dot{\theta}} \cr }\right]=\left[\matrix{{V_e } \cr {V_n } \cr {a_e } \cr {a_n } \cr \omega \cr }\right]+\left[\matrix{0 & 0 & 0 \cr 0 & 0 & 0 \cr {-1} & 0 & 0 \cr 0 & {-1} & 0 \cr 0 & 0 & {-1} \cr }\right]\left[\matrix{{w_{ae} } \cr {w_{an} } \cr {w_\omega } \cr }\right]$$
where
$P=\lsqb P_{e} \quad P_{n}\rsqb ^{\rm T}$
is the position of the moving carrier in the reference coordinate system,
$V=\lsqb V_{e} \quad V_{n}\rsqb ^{\rm T}$
is the velocity of the moving carrier in the reference coordinate system, θ is the azimuth angle of the moving carrier, a
e
and a
n
are the linear acceleration values of the moving carrier in the reference coordinate system, ω is the angular velocity of the moving carrier in the reference coordinate system and w
ax
, w
ay
and w
ω are the measurement noise values for each of the inertial sensors.
The sampling period of the system is denoted by T and Equation (1) is processed using first order linearization in the vicinity of the estimated state, as shown by Equation (2).
$$\left[\matrix{{\Delta \dot{P}_e } \cr {\Delta \dot{P}_n } \cr {\Delta \dot{V}_e } \cr {\Delta \dot{V}_n } \cr {\Delta \dot{\theta }} \cr}\right]=\left[\matrix{0 & 0 & 1 & 0 & 0 \cr 0 & 0 & 0 & 1 & 0 \cr 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 \cr }\right]\left[\matrix{{\Delta P_e } \cr {\Delta P_n } \cr {\Delta V_e } \cr {\Delta V_n } \cr {\Delta \theta } \cr }\right]+\left[\matrix{0 & 0 & 0 \cr 0 & 0 & 0 \cr {-1} & 0 & 0 \cr 0 & {-1} & 0 \cr 0 & 0 & {-1} \cr }\right]\left[\matrix{{w_{ae} } \cr {w_{an} } \cr {w_\omega } \cr }\right]$$
where Δ P e and Δ P n are the position errors of the mobile robot in the east and north direction, respectively, Δ V e and Δ V n are the velocity errors of the mobile robot in the east and north direction, respectively, and Δθ is the heading error of the mobile carrier. After discretization of Equation (2), a linear discrete model can be obtained as shown by Equation (3):
$$\left[\matrix{{\Delta P_{e\comma k} } \cr {\Delta P_{n\comma k} } \cr {\Delta V_{e\comma k} } \cr {\Delta V_{n\comma k} } \cr {\Delta \theta_k } \cr }\right]=\left[\matrix{1 & 0 & T & 0 & 0 \cr 0 & 1 & 0 & T & 0 \cr 0 & 0 & 1 & 0 & 0 \cr 0 & 0 & 0 & 1 & 0 \cr 0 & 0 & 0 & 0 & 1 \cr }\right]\left[\matrix{{\Delta P_{e\comma k-1} } \cr {\Delta P_{n\comma k-1} } \cr {\Delta V_{e\comma k-1} } \cr {\Delta V_{n\comma k-1} } \cr {\Delta \theta_{k-1} } \cr }\right]+\left[\matrix{0 & 0 & 0 \cr 0 & 0 & 0 \cr {-T} & 0 & 0 \cr 0 & {-T} & 0 \cr 0 & 0 & {-T} \cr }\right]\left[\matrix{{w_{ae} } \cr {w_{an} } \cr {w_\omega } \cr }\right]$$
2.3. Random Error Model of the Inertial Sensor
Random errors of the inertial sensor are often time-variant and do not follow any specific change law. They are important factors that affect the positioning accuracy of the integrated navigation system. Therefore, it is necessary to consider the random error characteristics of the inertial sensors in the modelling process of the integrated navigation system. Third-order AR models of the random errors of the gyros and accelerometers are established, as shown in Equation (4):
$$a_k =-m_1 a_{k-1} -m_2 a_{k-2} -m_3 a_{k-3} +w_k $$
where a k is the random error of the inertial sensors at time k, m 1, m 2 and m 3 are the parameters of the AR model, which can be calculated by the Yule-Walker equation and w is white noise with a zero mean value. Equation (4) can be transformed into a state space model, as shown by Equation (5):
$$\left[\matrix{{a_k } \cr {a_{k-1} } \cr {a_{k-2} } \cr }\right]=\left[\matrix{{-m_1 } & {-m_2 } & {-m_3 } \cr 1 & 0 & 0 \cr 0 & 1 & 0 \cr }\right]\left[\matrix{{a_{k-1} } \cr {a_{k-2} } \cr {a_{k-3} } \cr }\right]+\left[\matrix{1 \cr 0 \cr 0 \cr }\right]w_k $$
It can be seen from Equation (5) that the three states need to be increased to describe the random error using the third-order AR model, and therefore the state of the fusion filter will be increased.
3. OPTIMAL FILTERING STRATEGY
3.1. System Equation
The errors of position, velocity, azimuth, acceleration and angular velocity of the combined positioning system are set as state variables, which are selected as follows:
$$\lsqb X=\lsqb {\delta P_e \comma \; \delta P_n \comma \; \delta V_e \comma \; \delta V_n \comma \; \delta \theta \comma \; \delta a_e \comma \; \delta a_{e-1} \comma \; \delta a_{e-2} \comma \; \delta a_n \comma \; \delta a_{n-1} \comma \; \delta a_{n-2} \comma \; \delta \omega_d \comma \; \delta \omega_{d-1} \comma \; \delta \omega_{d-2} } \rsqb ^{\rm T} \rsqb$$
The random error state space models of the north and east accelerometers and the azimuth angular velocity of the integrated navigation system are built based on the AR random error model of the inertial sensor:
$$\left[\matrix{{\delta a_e \lpar k \rpar } \cr {\delta a_e \lpar {k-1} \rpar } \cr {\delta a_e \lpar {k-2} \rpar } \cr }\right]=\left[\matrix{{-m_{e1} } & {-m_{e2} } & {-m_{e3} } \cr 1 & 0 & 0 \cr 0 & 1 & 0 \cr }\right]\left[\matrix{{\delta a_e \lpar {k-1} \rpar } \cr {\delta a_e \lpar {k-2} \rpar } \cr {\delta a_e \lpar {k-3} \rpar } \cr }\right]+\left[\matrix{1 \cr 0 \cr 0 \cr }\right]w_{ae} \lpar k\rpar $$
$$\left[\matrix{{\delta a_n \lpar k \rpar } \cr {\delta a_n \lpar {k-1} \rpar } \cr {\delta a_n \lpar {k-2} \rpar } \cr }\right]=\left[\matrix{{-m_{n1} } & {-m_{n2} } & {-m_{n3} } \cr 1 & 0 & 0 \cr 0 & 1 & 0 \cr }\right]\left[\matrix{{\delta a_n \lpar {k-1} \rpar } \cr {\delta a_n \lpar {k-2} \rpar } \cr {\delta a_n \lpar {k-3} \rpar } \cr }\right]+\left[\matrix{1 \cr 0 \cr 0 \cr }\right]w_{an} \lpar k \rpar $$
$$\left[\matrix{{\delta \omega_d \lpar k \rpar } \cr {\delta \omega_d \lpar {k-1} \rpar } \cr {\delta \omega_d \lpar {k-2} \rpar } \cr }\right]=\left[\matrix{{-n_{d1} } & {-n_{d2} } & {-n_{d3} } \cr 1 & 0 & 0 \cr 0 & 1 & 0 \cr }\right]\left[\matrix{{\delta \omega_d \lpar {k-1} \rpar } \cr {\delta \omega_d \lpar {k-2} \rpar } \cr {\delta \omega_d \lpar {k-3} \rpar } \cr }\right]+\left[\matrix{1 \cr 0 \cr 0 \cr }\right]w_\omega \lpar k \rpar $$
From Equation (3) and Equations (6)–(8), the following can be obtained:
$$X_k =FX_{k-1} +W_k $$
where:
$$F=\left[\matrix{1 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 1 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 1 & 0 & 0 & {-T} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & {-T} & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & {-T} & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & {-m_{e1} } & {-m_{e1} } & {-m_{e1} } & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & {-m_{n1} } & {-m_{n2} } & {-m_{n3} } & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & {-n_{d1} } & {-n_{d2} } & {-n_{d3} } \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \cr 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \cr }\right]$$
$$W=\lsqb {w_{pe} \comma \; w_{pn} \comma \; w_{ve} \comma \; w_{vn} \comma \; w_\theta \comma \; w_{ae} \comma \; 0\comma \; 0\comma \; w_{an} \comma \; 0\comma \; 0\comma \; w_\omega \comma \; 0\comma \; 0} \rsqb ^{\rm T} $$
where (δP e , δP n ) and (δV e , δV n ) are the position error and the velocity error of INS in the north and east directions, respectively, δθ is the heading error of the moving-carrier, F is the system state transition matrix, T is the system period, W k is the system process noise and the covariance matrix is Q.
Z is the observation vector of the system. The differences between the position and the velocity that are output from INS and UWB are chosen as the measurements.
$$Z_k =HX_k +\nu _k $$
where
$$Z=\lsqb {\Delta P_e\ \Delta P_n\ \Delta V_e\ \Delta V_n } \rsqb ^{\rm T} $$
$$H=\left[\matrix{1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \cr }\right]$$
$$\Delta P=P_{INS} -P_{UWB} \comma \; \Delta V=V_{INS} -V_{UWB} $$
where Δ P e and Δ P n are the position differences in the north and east directions, Δ V e and Δ V n are the velocity differences in the north and east directions, H is the observation matrix of the filter, υ k is the observation noise and the covariance matrix is R.
3.2. Simplified Sage-Husa Adaptive Filtering Algorithm
The adaptive Kalman filtering algorithm is often used when the system noise is uncertain. Compared with the traditional Kalman filter, a measurement noise estimator is added to the simplified Sage-Husa adaptive filtering algorithm (Guo et al., Reference Guo, Guo, Yu, Hong, Xiong and Tian2015). The specific algorithm is as follows:
$$\hat {X}_k =\hat {X}_{k/k-1} +K_k v_k $$
$$\hat {X}_{k/k-1} =F_{k/k-1} \hat {X}_{k-1} $$
$$v_k =Z_k -H_k \hat {X}_{k/k-1} $$
$$K_k =P_{k/k-1} H_k^T \lsqb {H_k P_{k/k-1} H_k^T +R_k } \rsqb ^{-1} $$
$$P_{k/k-1} =F_{k/k-1} P_{k-1} F_{k/k-1}^T +Q_k $$
$$P_k =\lsqb {I-K_k H_k } \rsqb P_{k/k-1} \lsqb {I-K_k H_k } \rsqb ^T+K_k R_{k-1} K_k^T $$
$$R_k =\lpar {1-d_k } \rpar R_{k-1} +d_k \lcub {\lsqb {I-H_k K_{k-1} } \rsqb v_k v_k^T \lsqb {I-H_k K_{k-1} } \rsqb ^T+H_k P_{k-1} H_k^T }\rcub $$
where
$d_{k}=\lpar 1-b\rpar /\lpar 1-b^{k+1}\rpar $
, b is the forgetting factor, usually between 0·95 and 0·99. The simplified Sage-Husa adaptive algorithm can estimate the state of the system while performing the online calculation of the measurement noise R, and thus can achieve an adaptive effect.
3.3. Improved Adaptive Filtering (IAKF) Algorithm
An integrated positioning system has a high sensitivity to carrier movement. The real-time measurements will have outliers because the UWB wireless positioning system is highly sensitive to environmental changes, which may result in low accuracy of the adaptive filtering algorithm and filter divergence. Therefore the measurements need to be first identified and corrected, so that the state of the filter can be judged. Different filtering algorithms will be used for different states to ensure that the filter is stable and convergent.
As can be seen from Equations (16) and (18), the measured information Z k influences the linear combination of the state estimation values. These outliers will change the state predictive values incorrectly over time K k . The state estimate value will be offset from its normal value, which will cause the filter results to appear to deviate or even diverge. To solve this problem, the following equation is used which is based on the orthogonality of innovation (Zhu et al., Reference Zhu, Shan, Yang, Nian and Yang2015):
$$E\lpar Z_kZ_k^T\rpar =H_k P_{k/k-1} H_k^T +R_k +H_k X_{k/k-1} X_{k/k-1}^T H_k^T $$
Assume that:
$$D_k =H_k P_{k/k-1} H_k^T +R_k +H_k X_{k/k-1} X_{k/k-1}^T H_k^T $$
Using the diagonal elements of the two matrices in Equation (24), it can be judged whether component Z i in the measurement Z k is an outlier or not using the following equation:
$$G_{i\comma k} \in \lsqb {D_{i\comma k} -\varepsilon _i \comma \; D_{i\comma k} +\varepsilon _i }\rsqb $$
where, G i, k and D i, k are the ith elements on the diagonals of E(Z k Z k T ) and D k , respectively and ε is the disturbance factor, which is set depending on the actual demand. If Z i, k meets the condition in Equation (25), Z i, k is considered to be normal. However, if it does not meet this condition, it is recognised as the outlier, which should be limited by adjusting the activation function to eliminate any adverse effects caused by this outlier. The activation function is as follows:
$$\lambda _i =\sqrt {\lpar D_{i\comma k} +\varepsilon _i \rpar /G_{i\comma k}} $$
For each filter cycle, outlier identification is performed for each component of the measurement Z k . When Z k is not an outlier, the weight factor is set to one and will not change the innovation sequence; when Z k is an outlier, the activation function is set to less than one to limit the measurement and to maintain orthogonality of innovation.
It can be seen from Equation (26) that there will be missed outliers when ε is too large. Therefore, ε should be set as small as possible. In fact, ε caused by the calculation error would be very small. When ε is zero, G i, k is very close to D i, k and the weighted value λ i is 1. This will give rise to a very small measurement correction function. In practical application, ε is set depending on the application requirements and the measurement precision.
To prevent filter divergence, it is necessary to judge the state of the filter. The filter state is closely related to covariance matching, so covariance matching technology (Wang et al., Reference Wang, Tan, Chen and Cheng2013) can be used to determine whether the current filter is exhibiting divergence. The filter divergence criterion is as follows:
$$v_k v_k^T >\gamma Tr\lsqb {H_k P_{k/k-1} H_k^T +R_k } \rsqb $$
where γ is the reserve coefficient, γ > 1, Tr(·) are the matrix traces, v k is the innovation of Kalman Filtering, H k is the observation matrix of the system, P k/k−1 is the predicted covariance matrix of the filter and R k is the measurement noise variance of the system.
If Equation (27) is met, this means that the actual error exceeds the theoretical expected value of a factor of γ, and the filter will diverge. If Equation (27) is not met, this means that the filter is normal and the simplified Sage-Husa adaptive filtering algorithm can be used. The convergence condition is the most stringent when γ is equal to one. A robust filtering strategy is used to judge the divergence of the filter, and a fading factor f is introduced to change the prediction covariance:
$$P_{k/k-1} =fF_{k/k-1} P_k F_{k/k-1}^T +Q_k $$
When the measured variance of innovation is larger than the theoretical value, this means that there is an increase in the external disturbance. Thus the fading factor f can be defined as follows:
$$f =\max \lcub {1\comma \; Tr\lpar N_k \rpar /Tr\lpar M_k \rpar } \rcub $$
$$N_k =v_k v_k^T -H_k Q_k H_k^T -R_k $$
$$M_k =H_k F_{k/k-1} P_{k/k-1} F_{k/k-1}^T H_k^T $$
When the model error increases, v k will increase and N k will also increase, which means that f will be larger than one. This will increase the prediction covariance and change the filter gain matrix. The innovation weight will increase after increasing the prediction covariance and changing the filter gain matrix, which will improve the system's tracking ability. Figure 3 shows the flow chart of the IAKF algorithm.
Flow chart of the IAKF algorithm.
3.4. Simulation Analysis
The proposed INS/UWB integrated navigation system has been simulated by applying the improved adaptive filtering algorithm to the combined system, and comparing this to the Kalman filtering algorithm that is widely used in engineering. The East-North-Up coordinate system is selected as the navigation coordinates. The initial position is set at a 120·3° eastern longitude, a 31·6° northern latitude and 10 metres high. The gyro constant error is 0·5°/h with a random walk of 0·05°/h1/2 and 10−3 g with a random walk of 10−4g·s1/2. The initial position error of INS is 1 m and the initial velocity error is 1 m/s. The initial position is P 0 = (0, 0). The channels provided by the IEEE 802.15.4a standard are used in UWB to perform the positioning algorithm simulation. The Signal to Noise Ration (SNR) of the channel model is set at 30 dB. There are four reference nodes used, located at (−5,5), (5, − 5), (−5,5) and (5,5). The TDOA algorithm is used in the UWB system. The simulation duration is 15 s and the sampling frequency is 100 Hz. The simulation results are shown in Figure 4. This figure shows a comparison of the simulation trajectories, and the UWB measurement values which contain some outliers. The INS/UWB combined positioning system is implemented using the IAKF filtering algorithm.
Comparison of the simulation trajectories.
Table 1 shows the position errors of the different trajectories. Since the simulation time is short, the cumulative error of INS cannot be reflected and the INS will not be analyzed. The results show that KF, STF, SHAF and IAKF can all reduce the Root Mean Square Error (RMSE) and position error range of the INS/UWB system. It can be seen that: (1) the RMSE of the proposed INS/UWB tight coupling system with the IAKF filter is 0·0257 m, which is a reduction of approximately 62·59%, 55·30%, 50·67% and 54·43% compared with the UWB solution, the KF solution, the STF solution and the SHAF solution, respectively. (2) The eastbound position error range of the proposed system with the IAKF filter is a reduction of approximately 70·95%, 67·29%, 52·69% and 62·32% compared with the UWB, the KF, the STF and the SHAF, respectively. The northbound position error range of the proposed system with the IAKF filter is a reduction of approximately 76·47%, 73·09%, 56·30% and 69·12% compared with the UWB, the KF, the STF and the SHAF, respectively.
The position errors of the different trajectories.
Figures 5 and 6 show the eastbound and northbound positioning errors, respectively. It can be seen from the figures that the values of the KF, the STF and the SHAF algorithm have outliers of at least 0·3 m at approximately 900, 1,100 and 1,300 sampling points. The ability of the STF algorithm to deal with outliers is the best of the three filtering algorithms, but it cannot completely eliminate the impact of outliers and can also produce some position fluctuations. However, since the IAKF algorithm can adjust the measurement noise online, it can detect the outliers and reduce their influence on the position accuracy. Figure 7 shows the distance error range with different algorithms. It can be seen from the figure that the distance error with IAKF is maintained within 0·3 m, while the maximum distance error with KF, STF or SHAF is at least 0·6 m.
The eastbound positioning error of the different methods.
The northbound positioning error of the different methods.
The distance error ratio diagram.
The simulation results show that the proposed INS/UWB tightly coupled system is better than any of the independent subsystems, and the divergence problem does not exist. The simulation results also show that the IAKF algorithm is better than the other filter algorithms, and it is more suitable for the INS/UWB tightly coupled positioning system.
4. EXPERIMENT AND ANALYSIS
4.1. Hardware Design
The inertial navigation system in this paper is using the AH100B series navigation module developed by RION TECHNOLOGY in China. The module is designed using a MEMS inertial sensing unit with a “micro 9” axis. This unit adopts the latest MEMS technology which integrates a high-accuracy three-axis gyro sensor, a three-axis acceleration sensor and a three-axis magnetic field sensor; and performs high-speed calculation using a 32-bit ARM CortexM3 MCU, which can measure the position and attitude information of the carriers in either a static or a dynamic state without relying on any external signals in the three-dimensional space. The data refresh rate is 100 Hz, as shown in Table 2. The UWB wireless positioning system is a LINK UWB wireless positioning system developed by the ANGXUN Electronics Company in China. The positioning system is designed using a high-accuracy UWB wireless positioning module with four referenced base-stations, in conjunction with a network synchronous control unit (with a 10 Hz refresh rate of the mobile nodes) and a TDOA measurement method.
Basic performance parameters of INS.
The hardware of the combined positioning system is shown in Figure 8. The INS communicates with the host computer through the RS232 connection, and the UWB communicates with the host computer through the RJ-45 connection. The final data acquisition, fusion filtering and interface display are completed on the host computer.
Hardware structure of the combined positioning system.
4.2. Experimental Research
The experimental environment shown in Figure 9 was set up to verify the performances of the combined positioning system based on UWB and INS. The main devices in this test included one mobile robot (to simulate the moving target), four UWB positioning base-stations, one UWB wireless synchronous controller, one INS module, one upper computer and one positional tag. The experimental configuration is the same as the previous work (Fan et al., Reference Fan, Sun and Wu2015). The main difference is in the system model and data filtering algorithm. In the previous work, the position error and velocity error of the inertial navigation system were selected as the state variables and the traditional KF was used to fuse data. The outliers were measured by the UWB system before fusion had been artificially removed, and did not have real-time characteristics. In this paper, the errors of position, velocity, azimuth, acceleration and angular velocity of the combined positioning system are set as state variables. The filter is improved by adding an outliers' identification and divergence judgment mechanism, which improves the real-time positioning accuracy of the system.
Indoor positioning environment.
The sampling frequency of the INS is 100 Hz, and the sampling frequency of the UWB is 10 Hz. Therefore, the fused system frequency is 10 Hz, that is, the filter cycle is 0·1 s. The reference nodes of the UWB system are installed at (0,0), (7,0), (5,0), (7,5) positions.
A large cumulative error of the INS positioning builds up over a longer period, which does not occur for the UWB. Therefore, error analysis of INS is not undertaken in this paper, and we only analyse the UWB system and the combined positioning system.
4.3. Static Positioning Test
In the static test, the mobile robot is placed at point (3,2). The static data from the UWB system and the INS are used to calculate the static position of the mobile robot by using each of the different solution strategies.
As shown in Figure 10, during the static positioning test, the eastbound error range based only on the UWB measurements is between −0·02 m and 0·04 m, the RMSE is 0·0125 m, the residual rate is 0·003, and the confidence level is 99·7%. However, under the same conditions, the eastbound error range using INS/UWB with the KF algorithm is between −0·0095 m and 0·0002 m, the RMSE is 0·0043 m, the residual rate is 0·0012, and the confidence level is 99·88%. The eastbound error range under the same conditions using INS/UWB with the IAKF algorithm is between −0·0124 m and 0·0058 m, the RMSE is 0·008 m, the residual rate is 0·0024, and the confidence level is 99·76%.
The eastbound and northbound error of static tracking.
The northbound error range using UWB is between −0·08 m and 0·04 m, the RMSE is 0·0389 m, the residual rate is 0·0166, and the confidence level is 98·34%. However, under the same conditions, the northbound error range using INS/UWB with the KF algorithm is between −0·0376 m and 0·0031 m, the RMSE is 0·0133 m, the residual rate is 0·0045, and the confidence level is 99·55%. The northbound error range using INS/UWB with the IAKF algorithm is between −0·0221 m and 0·0001 m, the RMSE is 0·0103 m, the residual rate is 0·0044, and the confidence level is 99·56%.
4.4. Dynamic Positioning Test
For the dynamic experiment, the nodes and the robot trajectory are as shown in Figure 11. The initial position of the mobile robot is P0 = (3, 4), and the initial speed is 0 m/s. The trajectory passes through points (2,2) and (4,1) and finally reaches point (5,3). The dynamic experimental trajectory is shown in Figure 12.
Framework of node deployment.
Dynamic tracking performance of position.
The dynamic tracking results for the attitude angle and position for the dynamic experiment are shown in Figure 12, Figure 13, and Figure 14. The error range using UWB is between −0·4159 m and 0·5456 m, the RMSE is 0·1022 m, the residual rate is 0·0436, and the confidence level is 95·64%. However, under the same conditions, the error range using INS/UWB with the KF algorithm is between −0·4117 m and 0·3457 m, the RMSE is 0·0886 m, the residual rate is 0·0241, and the confidence level is 97·59%. The error range using INS/UWB with the IAKF algorithm is between −0·2313 m and 0·1183 m, the RMSE is 0·0636 m, the residual rate is 0·0176, and the confidence level is 98·24%.
Dynamic tracking error of distance.
Dynamic tracking performance of attitude angle.
4.5. Analysis of Results
In the static experiment, the distance error range of the INS/UWB tightly coupled system with either the KF or IAKF algorithms decreases and the trajectory tends to be smooth, compared with the INS or the UWB positioning system. The eastbound RMSE of KF and IAKF are reduced by approximately 65·6% and 36% compared with the UWB solution, respectively. The northbound RMSE of KF and IAKF are reduced by approximately 65·81% and 75·52% compared with the UWB solution, respectively. Therefore, the KF and IAKF algorithms have a generally beneficial effect when the carrier is in a static state.
In the dynamic experiment, the distance error range of the INS/UWB tight coupling positioning system with IAKF algorithm is significantly reduced. The RMSE is reduced by approximately 28·22% and 37·77% compared with the KF solution and the UWB solutions, respectively. At approximately 3 s and 8 s, the measurement values of UWB have some outliers, which introduce some large errors to the KF method. However, the IAKF algorithm can detect these outliers and eliminate their influence, thus improving the fault tolerance and the robustness of the system.
The above analysis shows that the IAKF algorithm is more suitable for the INS/UWB coupled system in achieving high precision tracking of the mobile robot.
5. CONCLUSIONS
The INS method produces diverging localisation errors after being used for a long time. To compensate for this disadvantage, this paper has proposed a mobile robot autonomous navigation system based on a tightly coupled INS/UWB. In the integrated positioning system, the differences between the position and the velocity values that are output from INS and UWB were chosen as the measurement values to establish a tightly coupled model using INS/UWB. The kinematics of a robot have been analysed and the AR algorithm was used to establish the third-order error equations of the gyroscope and the accelerometer. The system filter equations were established based on the coupled model. The IAKF algorithm was proposed for the coupled system. The simulation results have shown that the IAKF algorithm has a higher positioning accuracy than the KF filter and is more suitable for the INS/UWB coupled system. Static and dynamic experiments have been undertaken and the results show that the INS/UWB integrated navigation system can track the position and attitude angle of the mobile robot in real time and the positioning accuracy satisfies the requirements of practical applications.
ACKNOWLEDGMENT
The authors would like to thank the National Natural Science Foundation of China (Grant No.51405198), the National Natural Science Foundation of China (Grant No.61503156), the National Natural Science Foundation of China (Grant No.61572237), the Jiangsu Province Graduate Students Practice Innovation Project (SJLX15_0567), Project funded by China Postdoctoral Science Foundation(2016M590406) and the Fundamental Research Funds for the Central Universities (JUSRP11464) for the support given to the research.
