Centimeter-accurate vehicle navigation in urban environments with a tightly integrated PPP-RTK/MEMS/vision system

Abstract

Emerging applications like autonomous cars and unmanned aerial vehicles demand accurate, continuous and reliable positioning. The PPP-RTK technique, which could provide a rapid centimeter-accurate positioning service using a single GNSS receiver, has been recognized as a promising tool for mass-market and automotive applications. Nevertheless, the positioning performance of PPP-RTK degrades in urban environments because of the frequent signal deteriorating and blocking. Inertial navigation system (INS) is commonly integrated with GNSS to improve the continuity, accuracy and reliability of precise positioning as it has several advantages of all-environment operability and high temporal resolution, but it is limited by rapid error accumulation in long-term operation, especially when a microelectromechanical system inertial measurement unit (MEMS-IMU) is employed. Fortunately, the camera, another low-cost sensor, which provides rich information about the surrounding environment, is expected to improve the navigation performance further. This contribution develops a tightly integrated PPP-RTK/MEMS/vision model to achieve continuous and accurate positioning in urban environments. The raw data of MEMS-IMU and a stereo camera, as well as the high-precision GNSS phase measurements, are fused based on a multistate constraint Kalman filter to fully exploit the complementary properties from heterogeneous sensors. On this basis, a fast ambiguity resolution is achievable with the augmentation of the high-precision INS/vision information and the precise atmospheric corrections. The proposed integrated system is validated by several vehicle experiments conducted in urban areas. Results indicate that a centimeter-level accuracy of 4.1, 2.2 and 7.3 cm in the east, north and up components, respectively, and a high fixing percentage of 96.8% can be achieved for PPP-RTK/MEMS/vision in an urban environment, which exhibits comparable performance with respect to the tight integration of PPP-RTK and a tactical IMU. Besides, ambiguity refixing can be implemented instantaneously for the integrated system when going under three consecutive overpasses in 25 s.

Appendix 1: Jacobians of the estimated camera states

This section introduces the specific expression of Jacobians of estimated camera states. The linearized visual observation equations for a single feature $f_{j}$ observed at the time step $i$ can be written as:

$${\mathbf{r}}_{{{\text{vis}},i}}^{j} {\mathbf{ = z}}_{{{\text{vis}},i}}^{j} - {\hat{\mathbf{z}}}_{{{\text{vis}},i}}^{j} {\mathbf{ = H}}_{{C_{i} }}^{j} \delta {\mathbf{x}}_{{C_{i} }} + {\mathbf{H}}_{{f_{i} }}^{j} \delta {\mathbf{p}}_{j}^{e} {\mathbf{ + n}}_{{{\text{vis}},i}}^{j}$$

(19)

where $\delta {\mathbf{x}}_{{C_{i} }} { = }\left[ {\delta {{\varvec{\uptheta}}}_{{C_{i} }}^{e} ,\delta {\mathbf{p}}_{{C_{i} }}^{e} } \right]^{{\text{T}}}$ denotes the error state vector of the camera pose; $\delta {\mathbf{p}}_{j}^{e}$ is the feature position error; and ${\mathbf{H}}_{{C_{i} }}^{j}$ and ${\mathbf{H}}_{{f_{i} }}^{j}$ represent the corresponding Jacobians, respectively, which can be written as:

$${\mathbf{H}}_{{C_{i} }}^{j} { = }\left( {\begin{array}{*{20}c} {{\mathbf{H}}_{1} \left( {{\hat{\mathbf{p}}}_{j}^{{C_{i,l} }} \times } \right)} & { - {\mathbf{H}}_{1} \left( {{\hat{\mathbf{R}}}_{{C_{i,l} }}^{e} } \right)^{{\text{T}}} } \\ {\left( {{\mathbf{H}}_{2} \cdot {\mathbf{R}}_{{C_{i,l} }}^{{C_{i,r} }} } \right)\left( {{\hat{\mathbf{p}}}_{j}^{{C_{i,l} }} \times } \right)} & { - \left( {{\mathbf{H}}_{2} \cdot {\mathbf{R}}_{{C_{i,l} }}^{{C_{i,r} }} } \right)\left( {{\hat{\mathbf{R}}}_{{C_{i,l} }}^{e} } \right)^{{\text{T}}} } \\ \end{array} } \right),\quad {\mathbf{H}}_{{f_{i} }}^{j} { = }\left( {\begin{array}{*{20}c} {{\mathbf{H}}_{1} \left( {{\hat{\mathbf{R}}}_{{C_{i,l} }}^{e} } \right)^{{\text{T}}} } \\ {{\mathbf{H}}_{2} {\mathbf{R}}_{{C_{i,l} }}^{{C_{i,r} }} \left( {{\hat{\mathbf{R}}}_{{C_{i,l} }}^{e} } \right)^{{\text{T}}} } \\ \end{array} } \right)$$

(20)

$${\text{with}}\quad\quad {\mathbf{H}}_{1} = \frac{1}{{\left( {\hat{Z}_{j}^{{C_{i,l} }} } \right)^{2} }}\left( {\begin{array}{*{20}r} \hfill {\hat{Z}_{j}^{{C_{i,l} }} } & \hfill 0 & \hfill { - \hat{X}_{j}^{{C_{i,l} }} } \\ \hfill 0 & \hfill {\hat{Z}_{j}^{{C_{i,l} }} } & \hfill { - \hat{Y}_{j}^{{C_{i,l} }} } \\ \hfill {0} & \hfill {0} & \hfill {0} \\ \hfill {0} & \hfill {0} & \hfill {0} \\ \end{array} } \right),\quad {\mathbf{H}}_{2} = \frac{1}{{\left( {\hat{Z}_{j}^{{C_{i,r} }} } \right)^{2} }}\left( {\begin{array}{*{20}r} \hfill {0} & \hfill {0} & \hfill {0} \\ \hfill {0} & \hfill {0} & \hfill {0} \\ \hfill {\hat{Z}_{j}^{{C_{i,r} }} } & \hfill {0} & \hfill { - \hat{X}_{j}^{{C_{i,r} }} } \\ \hfill 0 & \hfill {\hat{Z}_{j}^{{C_{i,r} }} } & \hfill { - \hat{Y}_{j}^{{C_{i,r} }} } \\ \end{array} } \right)$$

(21)

Considering that the same feature will be tracked by multiple consecutive camera poses, therefore the linearized visual observation vector for this feature can be obtained by stacking all the individual equations together:

$$\underbrace {{\left[ {\begin{array}{*{20}c} {u_{1,l}^{j} } \\ \vdots \\ {v_{k,r}^{j} } \\ \end{array} } \right]}}_{{{\mathbf{z}}_{{{\text{vis}}}}^{j} }} = \underbrace {{\left[ {\begin{array}{*{20}c} {\hat{X}_{j}^{{C_{1,l} }} /\hat{Z}_{j}^{{C_{1,l} }} } \\ \vdots \\ {\hat{Y}_{j}^{{C_{k,r} }} /\hat{Z}_{j}^{{C_{k,r} }} } \\ \end{array} } \right]}}_{{{\hat{\mathbf{z}}}_{{{\text{vis}}}}^{j} }} + \underbrace {{\left[ {\begin{array}{*{20}c} {{\mathbf{H}}_{{C_{1} }}^{j} } \\ \vdots \\ {{\mathbf{H}}_{{C_{k} }}^{j} } \\ \end{array} } \right]}}_{{{\mathbf{H}}_{C}^{j} }}\delta {\mathbf{x}}_{C} + \underbrace {{\left[ {\begin{array}{*{20}c} {{\mathbf{H}}_{f,1}^{j} } \\ \vdots \\ {{\mathbf{H}}_{f,k}^{j} } \\ \end{array} } \right]}}_{{{\mathbf{H}}_{f}^{j} }}\delta {\mathbf{p}}_{{f_{j} }}^{e} + {\mathbf{n}}_{{{\text{vis}}}}^{j}$$

(22)

where $k$ denotes the $k$-th camera pose in the sliding window.

Since the feature positions are computed with the camera poses, the uncertainty of feature position is thereby correlated with the camera pose in the estimator. To eliminate the correlation between the feature position and camera poses, the linearized observation vector is reformulated by projecting it on the left null space (${\mathbf{A}}_{j}^{{\text{T}}}$) of the matrix ${\mathbf{H}}_{f}^{j}$, which can be rewritten as:

$${\mathbf{A}}_{j}^{{\text{T}}} {\mathbf{z}}_{{{\text{vis}}}}^{j} = {\mathbf{A}}_{j}^{{\text{T}}} {\hat{\mathbf{z}}}_{{{\text{vis}}}}^{j} + {\mathbf{A}}_{j}^{{\text{T}}} {\mathbf{H}}_{C}^{j} \delta {\mathbf{x}}_{C} + {\mathbf{n}}_{{{\text{vis}}}}^{j}$$

(23)

In this way, the visual observation equation is independent of the errors of the estimated feature position; therefore, the EKF update can be performed.