Feature-based visual navigation integrity monitoring for urban autonomous platforms

Abstract

Visual navigation systems have increasingly been adopted in many urban safety–critical applications, such as urban air mobility and highly automated vehicle, for which they must continuously provide accurate and safety-assured pose estimates. Extensive studies have focused on improving visual navigation accuracy and robustness in complex environment, while insufficient attention has been paid to ensuring navigation safety in the presence of outliers. From safety perspective, integrity is the most important navigation performance criterion because it measures the trust that can be placed in the correctness of the navigation output. Through leveraging the concept of integrity, this paper develops an integrity monitoring framework to protect visual navigation system against misleading measurements and to quantify the reliability of the navigation output. We firstly present the iterative least squares (LS)-based pose estimation algorithm and derive the associated covariance estimation methodology. Then we develop a two-layer fault detection scheme through combining random sampling consensus (RANSAC) with multiple hypotheses solution separation (MHSS) to achieve high efficiency and high reliability. Finally, the framework determines the probabilistic error bound of the navigation output that rigorously captures the undetected faults and the measurement uncertainty. The proposed algorithms are validated using various simulations, and the results suggest the promising performance.

Appendix: Proof of Eq. (24)

$\delta \boldsymbol{\varphi }$ is usually called misalignment angles, while ${{\varvec{\varepsilon}}}_{\varphi }$ denotes the errors in attitude angles. The relationship between them is given by the following:

$$ \delta {\varvec{\varphi }} = {\varvec{C}}_{b}^{n} \, \cdot \,{\varvec{C}}_{A}^{\omega } \, \cdot \,{\varvec{\varepsilon}}_{\varphi } , $$

(39)

where ${{\varvec{C}}}_{b}^{n}={{\varvec{R}}}^{\mathrm{T}}$ and ${{\varvec{C}}}_{A}^{\omega }$ is shown as:

$$ {\varvec{C}}_{A}^{\omega } = \left[ {\begin{array}{*{20}c} 1 & 0 & { - s\beta } \\ 0 & {c\alpha } & {s\alpha c\beta } \\ 0 & { - s\alpha } & {c\alpha c\beta } \\ \end{array} } \right]. $$

(40)

This equation is obtained from the attitude kinematic equation. Substituting (40) to (39),

$$ \delta {\varvec{\varphi }} = \left[ {\begin{array}{*{20}c} {c\beta c\gamma } & { - s\gamma } & 0 \\ {c\beta s\gamma } & {c\gamma } & 0 \\ { - s\beta } & 0 & 1 \\ \end{array} } \right]\, \cdot \,{\varvec{\varepsilon}}_{\varphi } . $$

(41)

Therefore, we have:

$$ {\varvec{\varepsilon}}_{\varphi } = \left[ {\begin{array}{*{20}c} {c\gamma /c\beta } & {s\gamma /c\beta } & 0 \\ { - s\gamma } & {c\gamma } & 0 \\ {c\gamma s\beta /c\beta } & {s\gamma s\beta /c\beta } & 1 \\ \end{array} } \right]\, \cdot \,\delta {\varvec{\varphi }} $$

(42)