Robust navigation system for UGV based on the IMU/vision/geomagnetic fusion

Abstract

The autonomous navigation system is of vital importance and great urgency for unmanned ground vehicles (UGVs) in GNSS-challenged/denied environments. This paper aims to develop a robust IMU/vision/geomagnetic integrated navigation system, which can provide position and attitude estimation for UGVs during long endurance. The core idea of this paper is to integrate the navigation information estimated by continuous vision, IMU mechanization, and Geomagnetic measurement based on a federated Kalman filter (FKF). Considering the instability of the continuous vision system, the global position generated by its matching with the prior map is introduced to the IMU/vision subfilter. Together with the local attitude and position, the measurement of the subfilter is formed. In order to improve the robustness of the global attitude, the north angle and the geomagnetic increment are utilized together to form an IMU/geomagnetic subfilter. Then, the FKF algorithm is designed to integrate the two subfilters. Experiments in the urban roads are carried out and the results demonstrate the effectiveness of the proposed system. The statistical results of dozens of experiments show that the attitude error of the proposed system is 0.384$^{\circ }$ (2d RMS, 95%) and the positioning error is 3.77 m (2d RMS, 95%).

Appendix 1: Matrix in the mechanization of SINS

$$\begin{aligned} M_1&=\begin{bmatrix} 0 &{} 0 &{} 1/(R_N+h) \\ 0 &{} 0 &{} \tan L/(R_N+h) \\ -1/(R_M+h) &{} 0 &{} 0 \\ \end{bmatrix} \end{aligned}$$

(30)

$$\begin{aligned} M_2&=\begin{bmatrix} -\omega _{ie}\sin L &{} 0 &{} 0 \\ \omega _{ie}\cos L &{} 0 &{} 0 \\ 0 &{} 0 &{} 0 \\ \end{bmatrix} \end{aligned}$$

(31)

$$\begin{aligned} M_3&=\begin{bmatrix} 0 &{} 0 &{} -\frac{v_E}{(R_N+h)^2} \\ -\frac{v_E \sec ^2L}{R_N+h} &{} 0 &{} \frac{v_E \tan ^L}{(R_N+h)^2} \\ 0 &{} 0 &{} \frac{v_N}{(R_M+h)^2} \\ \end{bmatrix} \end{aligned}$$

(32)

$$\begin{aligned} M_5&=\begin{bmatrix} \frac{1}{R_M+h} &{} 0 &{} 0 \\ 0 &{} 0 &{} \frac{\sec L}{R_N+h} \\ 0 &{} 1 &{} 0 \\ \end{bmatrix} \end{aligned}$$

(33)

$$\begin{aligned} M_6&=\begin{bmatrix} 0 &{} 0 &{} -\frac{v_N}{(R_M+h)^2} \\ \frac{v_E \sec L \tan L}{R_N+h} &{} 0 &{} -\frac{v_E \sec L}{(R_N+h)^2} \\ 0 &{} 0 &{} 0 \\ \end{bmatrix} \end{aligned}$$

(34)