论文阅读《Feature-based visual navigation integrity monitoring for urban autonomous platforms》

1 摘要

视觉导航系统已越来越多地应用于许多城市安全至关重要的应用，如城市空中交通和高度自动化的车辆，它们必须不断地提供精确和安全的位姿估计。对于如何提供复杂环境下的视觉导航精度和鲁棒性，已有大量的研究，但对于如何在异常值存在的情况下保证导航安全的研究不足。从安全的角度来看，完好性是最重要的导航性能指标，因为它度量了对导航输出正确性的信任。通过利用完好性的概念，本文开发了一个完好性监测框架，以保护视觉导航系统不受误导性测量的影响，并量化导航输出的可靠性。首先，提出了基于迭代最小二乘（LS）的位姿估计算法，并推导了相关协方差的估计方法。然后，将随机采样一致性（RANSAC）与多假设解分离（MHSS）相结合，提出了一种双层故障检测方案，以实现高效性和可靠性。最后，该框架确定了导航输出的概率误差界，并严格捕获了未检测到的故障和测量不确定度。通过各种仿真验证了所提出算法的有效性，结果表明该算法具有良好的性能。

2 介绍

基于视觉或视觉辅助的导航系统因其在城市环境下的优异性能而受到人们的广泛关注，而城市环境是全球导航卫星（GNSS）严重脆弱的环境。因此，在城市空中交通（UAM）和高度自动化车辆（HAV）等城市安全关键应用中，视觉导航是实现定位的另一种选择，有望给社会带来巨大的利益。

对于这些应用，在设计导航算法时，确保视觉定位的安全性是重中之重。这是因为，一方面，视觉导航对操作和环境条件非常敏感，比如纹理、模糊的存在和光照变化。因此，导航系统可能在某些条件下性能良好，但在其它环境中，它可能变得不可靠。另一方面，如果定位任务执行不正确，可能会对车辆、乘客和附近的人造成灾难性的损害。

因此，不仅要提高导航精度和鲁棒性，而且要对导航输出的正确性进行量化。到目前为止，之前的工作主要是通过优化算法和开发一些外点剔除技术来解决前一项任务。虽然这些算法大大降低了故障率，但通过执行后一项任务，仍有可能进一步提高导航安全性，这正是本文研究的目标。

为此，本文提出了一种新的实现视觉导航自主完好性监测的方法，这在文献中鲜有提及。我们利用航空开发的完好性概念来评估UAM和HAV部署中视觉导航的安全性。完好性度量了对导航输出正确性的信任程度，而完好性风险则描述了导航系统在没有警告用户的情况下提供有害误导信息的概率。显然，完好性从导航的角度提供了一种直接量化安全水平的方法，因此，它被用于衡量城市自主系统的定位安全。此外，该指标和相关的分析方法完美地补充了基于测试的车辆安全演示方法。

完好性监测是保证导航完好性的重要手段：它涉及两个基本功能，一是检测故障测量，二是量化安全风险。建立了多种完好性监测方法，以确保基于GNSS的导航在民航应用中的完好性。最具代表性的方法包括接收机自主完好性监测（RAIM）和先进的接收机自主完好性监测（ARAIM），这两种方法都是基于冗余距离测量的一致性检验。

将这些技术引入到其它导航系统和其它应用中越来越有吸引力，并作出了一些显著的努力。然而，据我们所知，关于视觉导航完好性监测的研究还很少。Joerger等人提出了一种基于激光雷达导航的完好性监测方法，着重于特征提取和数据关联。此外，Bhamidipati等人为GNSS/视觉组合导航系统开发了基于同时定位与建图（SLAM）的完好性监测方案。Li等人提出了一种基于RAIM的视觉导航完好性监测方案，假设在外点剔除之后，最多存在一个故障测量（即特征）。但是，这些现有方法并不代表一般情况，也不能解决以下所述的挑战。

由于无线电导航与视觉导航的显著差异，使得在GNSS领域发展起来的算法很难引入到视觉导航环境中。在城市场景中，由于环境中有丰富的纹理，基于特征的视觉导航通常比其它方法，如光流法和模板匹配法效果更好。一幅图像的特征数目通常接近甚至超过100个，远远大于GNSS应用中使用的伪距数目。此外，故障率可能超过10%，严重超过了传统完好性监测算法的能力。并且，在视觉导航中，测量故障的空间相关性不可忽略，因此RAIM和ARAIM所采用的故障无关假设在此不成立。还有，在视觉导航中，位姿估计通常采用基于优化的方法来实现，而现有完好性监测算法主要针对基于最小二乘（LS）或滤波的导航系统。

为了解决上述问题，本文提出了一种实现视觉导航完好性监测的新方法。在本工作中，我们首先描述了基于特征的视觉导航的一般流程，重点介绍了已匹配特征的导航状态估计。然后，提出了基于迭代最小二乘的位姿估计算法，并推导了相关的实时协方差估计方法，在此基础上开发了完好性监测框架。该框架采用随机采样一致性（RANSAC）和多假设解分离（MHSS）两种方法，实现了高效和高可靠的故障检测。在该方案中，我们还利用故障分组技术来降低计算复杂度，并处理故障的空间相关性。为了评估导航完好性，该框架确定了捕获未检测故障的概率误差界和测量不确定度。

本文的布局安排如下。第三部分介绍了视觉导航的基本原理，明确了研究的范围。第四节给出了位姿估计算法和相关协方差估计方法。完好性监测框架的细节将在第五节中讨论。在第六节中，我们通过仿真来验证所提出的算法。最后，在第7节中得出结论。

3 视觉导航的基本原理

视觉导航是一种利用相机或激光雷达传感器的输出来估计自我运动（平移和旋转）的精确而鲁棒的技术。自20世纪80年代早期以来，已经开发了许多方法，它们可以大致分为表1所示的类别。
 

表1 典型视觉导航方法的分类

由于不同的方法在实现上存在明显的差异，因此本文不打算提供一个涵盖所有方法的完好性监测框架。对于在城市环境中运行的自主平台，基于特征的方法长期以来一直是主导的视觉导航解决方案。在城市场景下，图像中可以提取出丰富的独特特征，因此基于特征的方法在效率、准确性和鲁棒性方面都比其它方法表现出更好的性能。在传感器方面，由于单目视觉系统存在尺度不确定性，在没有外部传感器辅助的情况下，首选双目相机。因此，本文主要研究基于特征的双目视觉导航系统。我们将扩大这项工作，以支持未来的基于激光的导航。

如表1所示，基于特征的视觉导航系统有三种操作模式：视觉里程计、SLAM和基于地图的定位（文献中也称为重定位）。图1展示了这些模式下所需的程序。VO通过分析相机捕获的图像序列，提供了车辆位姿的增量在线估计。这是一种有效和方便的方法，不依赖于任何关于环境的先验知识，但定位误差会随着时间的推移而累积。SLAM是一种技术，它允许车辆在未知环境中进行自我定位，并逐步生成地图。如图1所示，VO是SLAM中必不可少的一部分，SLAM还包含后端优化、回环和建图。这些额外的程序有助于提高定位精度，但同时也使SLAM比VO复杂得多。通过使用预先建立的地图，基于地图的定位允许车辆估计其相对于该地图的位姿。实际上，在SLAM中，地图通常是通过地图提供者或者过去时间的建图程序来访问。

图1 基于特征的视觉导航系统所需步骤流程图  

在基于特征的方法中，特征提取是一个关键的步骤，它从图像中提取明显的特征（如直线、曲线和角点）。这个步骤可以用不同的方法实现，性能也不同。本文提出的方法普遍适用于各种特征，并以ORB特征为例验证了算法的有效性。作为一种广泛使用的特征，ORB对旋转、平移和缩放具有不变性，而且它还对相机设置和光照变化具有很强的鲁棒性。对于从当前图像中提取的特征，数据关联尝试在前一帧（VO模式）或局部地图（基于地图的定位模式）中找到对应的点。使用一组匹配的特征点，VO估计相机在不同历元之间的运动，而基于地图的定位计算主体相对于地图的位姿。本文主要研究基于地图的定位模式下的定位安全问题，但由于两者具有很大的相似性，该方法很容易推广到VO模式中。

对于基于地图的定位，位姿估计模块的直接输入是已匹配的三维点。在安全关键应用中，对视觉测量（即点对）中的潜在故障给予足够的重视是非常重要的。造成这些故障的原因主要有两个方面：一是由于环境条件不利导致数据关联失败，二是由于真实世界场景在地图构建后发生了变化。这些故障有可能导致较大的导航误差，并可能是车辆处于危险的情形中。为了确保操作安全，导航系统必须不断提供准确可靠的位姿估计。为此，本文旨在开发一个针对测量故障的完好性监测框架，以提供安全可靠的导航输出。

本节定义了这项工作的范围，并描述了在有代表性的可视化导航方法中所涉及到的必要过程。本文在不丧失一般性的情况下，假设从双目相机的输出中提取了一些显著特征，并将它们与预先建立的三维地图中的点进行匹配。为了清晰起见，这里不提供此流程的详细实现，它们不会影响以下派生。

4 位姿确定和协方差估计

4.1 测量方程

基于地图的定位的目的是确定主体的位姿，即相对于局部地图的位置和方向。位姿通常定义成$x\text{x}x=[{\phi \text{φ}\phi }^T,{t\text{t}t}^T{]}^T$，其中$\phi \text{φ}\phi =[\alpha \beta \gamma {]}^T$，$t\text{t}t=[t_x{t}_y{t}_z{]}^T$，它们分别表示旋转和平移。状态向量$x\text{x}x$，指的是4×4的变换矩阵$T\text{T}T$，
$$T\text{T}T=\left[\begin{array}{cc}R\text{R}R& t\text{t}t\\ 0\text{0}0& 1\end{array}\right]\text{\hspace{1em}(1)}$$
其中旋转矩阵$R\text{R}R$是由滚转角$\alpha$、俯仰角$\beta$和偏航角$\gamma$计算而来的，
$$R\text{R}R(\alpha ,\beta ,\gamma )=\left[\begin{array}{ccc}c\beta c\gamma & -c\alpha s\gamma +s\alpha s\beta c\gamma & s\alpha s\gamma +c\alpha s\beta c\gamma \\ c\beta s\gamma & c\alpha c\gamma +s\alpha s\beta s\gamma & -s\alpha c\gamma +c\alpha s\beta s\gamma \\ -s\beta & s\alpha c\beta & c\alpha c\beta \end{array}\right]\text{\hspace{1em}(2)}$$
其中$s$和$c$分别指$sin(\cdot )$函数和$cos(\cdot )$函数。

为了估计位姿，我们假设有$N$对匹配的特征点，如下：
P = { p 1 , p 2 , ⋯   , p N } ,    Q = { q 1 , q 2 , ⋯   , q N } (3) \pmb{P}=\{\pmb{p}_1,\pmb{p}_2,\cdots,\pmb{p}_N\},\ \ Q=\{\pmb{q}_1,\pmb{q}_2,\cdots,\pmb{q}_N\}\tag{3} PPP={p​p​​p1​,p​p​​p2​,⋯,p​p​​pN​},  Q={q​q​​q1​,q​q​​q2​,⋯,q​q​​qN​}(3)
其中${p\text{p}p}_i$和${q\text{q}q}_i$分别表示在第$i$个特征点在相机系和导航系中的三维坐标。这两个坐标系通过$R\text{R}R$和$t\text{t}t$联系，因此我们可以得到如下的测量方程，
$$\forall i,R\text{R}R\cdot ({p\text{p}p}_i-{ϵ}_{pi})+t={q\text{q}q}_i-{ϵ}_{qi}\text{\hspace{1em}(4)}$$
$${ϵ}_{pi}\sim \mathbb{N}(0,{C\text{C}C}_{pi})\text{\hspace{1em}(5)}$$
$${ϵ}_{qi}\sim \mathbb{N}(0,{C\text{C}C}_{qi})\text{\hspace{1em}(6)}$$
其中${ϵ}_{pi}$和${ϵ}_{qi}$是零均值的噪声项，它们的协方差分别是${C\text{C}C}_{pi}$和${C\text{C}C}_{qi}$。注意，在以往的研究中，特征点的误差通常假设为零均值高斯噪声。这项工作也是在这种假设下进行的，我们将在今后的工作中对误差模型进行检验。

然后，我们用非线性优化的方法来表示位姿估计。具体来说，导航状态，$\alpha$、$\beta$、$\gamma$、$t_x$、$t_y$和$t_z$通过最小化特征的重投影误差估计为：
{ R ∗ , t ∗ } = a r g m i n R , t ∑ i = 1 N ∣ ∣ R ⋅ p i + t − q i ∣ ∣ 2 2 (7) \{\pmb{R}^*,\pmb{t}^*\}=\underset{R,t}{argmin}\sum_{i=1}^{N}||\pmb{R}\cdot\pmb{p}_i+\pmb{t}-\pmb{q}_i||^2_2 \tag{7} {RRR∗,ttt∗}=R,targmin​i=1∑N​∣∣RRR⋅p​p​​pi​+ttt−q​q​​qi​∣∣22​(7)
公式（7）本质上是一个最小二乘优化问题。但由于$\alpha$、$\beta$和$\gamma$是旋转矩阵$R\text{R}R$中非线性三角函数的参数，不能直接应用线性最小二乘估计方法求解。在下一节中，我们将介绍如何修改这个优化问题，使其可以通过线性最小二乘方法解决。

4.2 基于迭代最小二乘的位姿确定

线性求解公式（7）的主要困难是由于旋转矩阵而产生的强非线性。幸运的是，小角度近似（例如$s\theta \approx \theta$，$c\theta \approx 1$，当$\theta \approx 0$时）是消除非线性的有效方法。当$\Delta \alpha$、$\Delta \beta$和$\Delta \gamma$约等于0时，有，
$$\Delta R\text{R}R\approx \left[\begin{array}{ccc}1& -\Delta \gamma & \Delta \beta \\ \Delta \gamma & 1& -\Delta \alpha \\ -\Delta \beta & \Delta \alpha & 1\end{array}\right]\triangleq I\text{I}I-(\Delta \phi \text{φ}\phi \times )\text{\hspace{1em}(8)}$$
其中$(\Delta \phi \text{φ}\phi \times )$是叉乘矩阵，$I\text{I}I$表示3×3单位矩阵。通过这个近似，我们可以在工作点 { R l , t l } \{\pmb{R}_l,\pmb{t}_l\} {RRRl​,tttl​}线性化测量方程，
$$\forall i,({p\text{p}p}_i{|}_l\times )\cdot \Delta \phi \text{φ}\phi +\Delta t\text{t}t=\Delta {p\text{p}p}_i\text{\hspace{1em}(9)}$$
$$\forall i,{p\text{p}p}_i{|}_l={R\text{R}R}_l\cdot {p\text{p}p}_i+{t\text{t}t}_l\text{\hspace{1em}(10)}$$
因此，原非线性优化问题可通过迭代求解，如下所示。在每次迭代时，我们将测量方程线性化，并将其堆叠为，
$$\Delta y\text{y}y=G\text{G}G\cdot \Delta x\text{x}x+ϵ\text{ϵ}ϵ\text{\hspace{1em}(11)}$$
其中几何矩阵$G\text{G}G$，重投影误差向量$\Delta y\text{y}y$和测量噪声$ϵ\text{ϵ}ϵ$由下式给出，
$$G\text{G}G=\left[\begin{array}{cc}{p\text{p}p}_1^{\prime }\times & I\text{I}I\\ ⋮& ⋮\\ {p\text{p}p}_N^{\prime }& I\text{I}I\end{array}\right]\text{\hspace{1em}(12)}$$
$$\Delta y\text{y}y=\left[\begin{array}{c}{q\text{q}q}_1-{p\text{p}p}_1^{\prime }\\ ⋮\\ {q\text{q}q}_N-{p\text{p}p}_N^{\prime }\end{array}\right]\text{\hspace{1em}(13)}$$
$${p\text{p}p}_i^{\prime }={\widehat{R\text{R}R}}^{\prime }\cdot {p\text{p}p}_i+{\widehat{t\text{t}t}}^{\prime }\text{\hspace{1em}(14)}$$
$$ϵ\text{ϵ}ϵ=\left[\begin{array}{c}{\widehat{R\text{R}R}}^{\prime }\cdot {ϵ\text{ϵ}ϵ}_{p1}-{ϵ\text{ϵ}ϵ}_{q1}\\ ⋮\\ {\widehat{R\text{R}R}}^{\prime }\cdot {ϵ\text{ϵ}ϵ}_{pN}-{ϵ\text{ϵ}ϵ}_{qN}\end{array}\right]\text{\hspace{1em}(15)}$$
其中${\widehat{R\text{R}R}}^{\prime }$和${\widehat{t\text{t}t}}^{\prime }$表示由之前迭代给出的估计的变换参数。

其次，执行最小二乘估计，状态更新量$\Delta \widehat{x\text{x}x}$计算如下，
$$\Delta \widehat{x\text{x}x}=({G\text{G}G}^{T}G\text{G}G{)}^{-1}{G\text{G}G}^T\cdot \Delta y\text{y}y\text{\hspace{1em}(16)}$$
然后，我们用$\Delta \widehat{x\text{x}x}$来更新变换矩阵，
$$\left[\begin{array}{cc}\widehat{R\text{R}R}& \widehat{t\text{t}t}\\ 0\text{0}0& 1\end{array}\right]=\left[\begin{array}{cc}\Delta \widehat{R\text{R}R}& \Delta \widehat{t\text{t}t}\\ 0\text{0}0& 1\end{array}\right]\cdot \left[\begin{array}{cc}{\widehat{R\text{R}R}}^{\prime }& {\widehat{t\text{t}t}}^{\prime }\\ 0\text{0}0& 1\end{array}\right]\text{\hspace{1em}(17)}$$
最后，当解收敛时，我们输出估计的位姿$\widehat{x\text{x}x}$，最后的$\Delta y\text{y}y$被定义为残差向量$y\text{y}y$。

4.3 协方差计算

在导航系统中，协方差矩阵常被用来衡量状态估计的不确定性，协方差估计是精度评估和完好性监测的基础。位姿不确定是由测量误差引起的，协方差估计的关键是推导误差传播公式。依据公式（15），第$i$个特征对应的测量噪声协方差矩阵$C_i$计算为：
$${C\text{C}C}_i=\widehat{R\text{R}R}\cdot {C\text{C}C}_{pi}\cdot {\widehat{R\text{R}R}}^T+{C\text{C}C}_{qi}\text{\hspace{1em}(18)}$$
假设不同特征的测量误差不相关，则所有特征的协方差矩阵堆叠为分块对角矩阵，
$$C\text{C}C=\left[\begin{array}{ccc}{C\text{C}C}_1& & \\ & \ddots & \\ & & {C\text{C}C}_N\end{array}\right]\text{\hspace{1em}(19)}$$
基于公式（16），我们可以直接确定状态扰动$\delta x\text{x}x$如下：
$$\delta x\text{x}x=[\delta {\phi \text{φ}\phi }^T,\delta {t\text{t}t}^T{]}^T=({G\text{G}G}^{T}G\text{G}G{)}^{-1}{G\text{G}G}^T\cdot ϵ\text{ϵ}ϵ\text{\hspace{1em}(20)}$$
状态扰动$\delta x\text{x}x$的协方差矩阵由下式给出，
$${C\text{C}C}_{\delta }={S\text{S}S}_{\delta }\cdot C\text{C}C\cdot {S\text{S}S}_{\delta }^T\text{\hspace{1em}(21)}$$
其中${S\text{S}S}_{\delta }=({G\text{G}G}^{T}G\text{G}G{)}^{-1}{G\text{G}G}^T$。

然而，${C\text{C}C}_{\delta }$并不是位姿估计的误差协方差矩阵，因为位姿误差${\epsilon \text{ε}\epsilon }_x=\widehat{x\text{x}x}-x\text{x}x$并不等于状态扰动$\delta x\text{x}x$。位姿估计值$\widehat{x\text{x}x}$和状态扰动$\delta x\text{x}x$之间的关系可以通过变换矩阵来建立，
$$\left[\begin{array}{cc}\widehat{R\text{R}R}& \widehat{t\text{t}t}\\ 0\text{0}0& 1\end{array}\right]=\left[\begin{array}{cc}\delta R\text{R}R& \delta t\text{t}t\\ 0\text{0}0& 1\end{array}\right]\cdot \left[\begin{array}{cc}R\text{R}R& t\text{t}t\\ 0\text{0}0& 1\end{array}\right]\text{\hspace{1em}(22)}$$
这个公式可以进一步重写为，
$$\left[\begin{array}{cc}\widehat{R\text{R}R}& \widehat{t\text{t}t}\\ 0\text{0}0& 1\end{array}\right]=\left[\begin{array}{cc}\delta R\text{R}R\cdot R\text{R}R& \delta R\text{R}R\cdot t\text{t}t+\delta t\text{t}t\\ 0\text{0}0& 1\end{array}\right]\text{\hspace{1em}(23)}$$
然后，我们有，
$${\epsilon \text{ε}\epsilon }_{\phi }=\widehat{\phi \text{φ}\phi }-\phi \text{φ}\phi =\left[\begin{array}{ccc}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 \phi \text{φ}\phi \cdot {A\text{A}A}_{\phi }\cdot \delta x\text{x}x\text{\hspace{1em}(24)}$$
$${\epsilon \text{ε}\epsilon }_t=\widehat{t\text{t}t}-t\text{t}t=(t\text{t}t\times )\cdot \delta \phi \text{φ}\phi +\delta t\text{t}t\triangleq {A\text{A}A}_t\cdot \delta x\text{x}x\text{\hspace{1em}(25)}$$
公式（24）详细的证明在附件中给出。

因此，${\epsilon \text{ε}\epsilon }_{\phi }$和${\epsilon \text{ε}\epsilon }_t$的协方差矩阵由以下两式给出，
$${C\text{C}C}_{\phi }={A\text{A}A}_{\phi }\cdot {C\text{C}C}_{\delta }\cdot {A\text{A}A}_{\phi }^T\text{\hspace{1em}(26)}$$
$${C\text{C}C}_t={A\text{A}A}_t\cdot {C\text{C}C}_{\delta }\cdot {A\text{A}A}_t^T\text{\hspace{1em}(27)}$$

5 视觉导航完好性监测框架

5.1 输入参数

如前所述，基于特征的视觉导航系统偶尔会遇到测量故障，这极大地威胁了系统的运行安全。本文将完好性的概念引入到视觉导航环境中，并开发了相应的完好性监测框架。该框架不仅能处理容易引起误解的特征，而且能够确定位姿估计的概率误差包络。该框架需要一些必要的输入来确保其可靠性，包括测量误差模型、先验故障概率和预定义的导航性能要求。

表2给出了每个特征的误差模型和先验故障概率。误差模型来源于对基于KITTI数据集的测量误差的初步分析，该数据集提供了地面真值和双目相机的输出。另外，需要注意的是，这里的故障概率表示的是经过第一层异常值排除（即RANSAC）后，内点发生故障的概率。两层故障检测方案将在第5.2节中详细说明。由于先验故障概率可能会随着RANSAC的设置而变化，本工作还对参数进行了灵敏度分析，其值见表2。

表2 误差模型和先验故障概率

导航需求参数主要包括目标完好性风险和虚警带来的连续性风险。完好性风险定义为导航系统在没有警告用户的情况下提供危险误导信息（HMI）的概率。误报警是故障检测器在无故障状态下检测到故障状态时发生的事件，是导致导航连续性丧失的主要原因。表3显示了这些参数的初步值，它们代表了安全关键应用的典型要求。由于这些需求实际上依赖于应用，我们将在未来的工作中确定特定车辆应用的导航需求。

表3 导航性能要求列表

5.2 双层故障检测方案设计

对于城市环境的车辆应用，视觉导航系统可以应用在测量数目大、故障率高的情况下。这种情况超出了传统的GNSS完好性监测方案的能力。为此，我们开发了一种双层故障检测方案，结合了RANSAC和MHSS的优点，提高了检测效率，保证了导航的完好性。

在描述这个方案之前，我们首先澄清基本假设。我们假设在地图生成过程中进行了质量控制。更具体地说，非静态物体是视觉导航系统的一大威胁，我们假设大多数这些不需要的物体都被排除在地图之外。这个排除步骤可以由地图提供者利用地面真实数据和一些物体分类技术来实现。这个步骤的详细实现超出了本文的范围。在这个假设下，不太可能有大量具有强一致性的异常值。

第一层采用RANSAC算法剔除原始视觉测量中的异常值。RANSAC通过重复以下步骤来达到目的：

1. 随机选择一个特征对应的子集作为初始一致集。
2. 利用这个子集来估计导航参数。
3. 然后，所有其它特征都根据估计参数进行测试，那些很好地符合估计参数的点（与预定义的阈值相比）被添加到一致集中。

这个过程重复固定的次数，有最多内点的一致集作为内点集输出。RANSAC是一种在异常值比例较高的情况下仍能有效去除大部分异常值的方法，因此在视觉导航中得到广泛应用。在上述假设下，RANSAC不会被异常值所误导，可以将异常值比例降低到一个足够低的值。迭代次数和外点识别的阈值对RANSAC后的故障率有很大的影响，即内点发生故障的概率。因此，我们需要通过指定RANSAC设置来正确地确定故障概率。这超出了本文的范围，但涉及到我们的另一项工作。

在适当的RANSAC设置下，故障概率预计会非常低（例如$10^{-3}$、$10^{-4}$或$10^{-5}$）。但是，仅仅使用RANSAC并不能保证视觉导航系统的安全性。这是因为，一方面，在安全关键型应用程序中仍然需要监视低概率事件。另一方面，RANSAC虽然可以大大降低导航误差，但很难推导出完好性监测算法所需的概率误差包络。

因此，我们采用MHSS技术作为第二层，进一步检测故障，更重要的是便于对导航安全进行量化。MHSS在完好性监测算法中得到了广泛的应用，因为它具有两个显著的优点：它能有效地处理多个同时发生的故障，并提供了完好性的直接证明。MHSS的分布步骤详细说明如下。

MHSS通过建立子集列表来实现单故障和多故障的检测，每个子集对应一个故障模式。需要注意的是，RANSAC中子集的确定过程与MHSS中子集的确定过程有明显的区别。RANSAC的目的是找到特征点的一个非常小的子集，使它们都是内点。在MHSS中，每个子集包含了大部分的观测信息，因为它假设在进行RANSAC后只有少量的故障特征。

利用MHSS，可以根据每个特征的先验故障概率确定需要监控的子集。每个故障模式（或子集）的相关概率是通过乘以该模式下假定为故障的特征的故障概率来确定的。对于概率相对较低的故障模式，我们采用一个预定义的阈值$P_{THRES}$来包络来自这些无监控模式的完好性风险。在此过程中，我们还可以得到需要监控的最大并发故障数$N_{f,max}$。可以按照ARAIM基线算法中所示的详细步骤来实现子集确定，方法是将每个特征视为“卫星”，并将星座故障概率设置为0。这一步的输出包括：监控子集（索引为$j=0,1,\cdots ,N_s$）、$N_{f,max}$、每个子集的故障概率$p_{fs}^{(j)}$以及来自未监控模式的总完好性风险$p_{nm}$。

然后计算检验统计量和相应的阈值。对于子集$j$，容错解${x\text{x}x}^{(j)}$和全视图解${x\text{x}x}^{(0)}$之间的差值$\Delta {x\text{x}x}^{(j)}$为：
$$\Delta {x\text{x}x}^{(j)}={x\text{x}x}^{(j)}-{x\text{x}x}^{(0)}=({S\text{S}S}^{(j)}-{S\text{S}S}^{(0)})\cdot y\text{y}y\text{\hspace{1em}(28)}$$
其中系数矩阵${S\text{S}S}^{(j)}$计算如下：
$${S\text{S}S}^{(j)}=\left[\begin{array}{c}{A\text{A}A}_{\phi }\\ {A\text{A}A}_t\end{array}\right]({G\text{G}G}^T{W\text{W}W}^{(j)}G\text{G}G{)}^{-1}{G\text{G}G}^T{W\text{W}W}^{(j)}\text{\hspace{1em}(29)}$$
其中$3n\times 3n$维对角矩阵${W\text{W}W}^{(j)}$可由下式获得：
$${W\text{W}W}^{(j)}(3i-2:3i,3i-2:3i)=\{\begin{array}{l}0,特征i在子集j中被认为是故障\\ 1,其它\end{array}\text{\hspace{1em}(30)}$$
让下标$q=1,2,3$分别表示那3个角度分量，$q=4,5,6$分别表示那3个平移分量。${x\text{x}x}^{(j)}$和$\Delta {x\text{x}x}^{(j)}$的协方差矩阵分别由以下两式给出，
$${\sigma }_q^{(j)2}=cov(x_q^{(j)},x_q^{(j)})={e\text{e}e}_q^T{S\text{S}S}^{(j)}C\text{C}C{S\text{S}S}^{(j)T}{e\text{e}e}_q\text{\hspace{1em}(31)}$$
$${\sigma }_{ss,q}^{(j)2}=cov(\Delta x_q^{(j)},\Delta x_q^{(j)})={e\text{e}e}_q^T({S\text{S}S}^{(j)}-{S\text{S}S}^{(0)})C\text{C}C({S\text{S}S}^{(j)}-{S\text{S}S}^{(0)}{)}^T{e\text{e}e}_q\text{\hspace{1em}(32)}$$
其中${e\text{e}e}_q$是一个向量，除了第$q$个元素为1外其余都是0。

对于每种故障模式，有6个解分离测试，每个旋转和平移分量各一个。对应的阈值计算如下：
$${T\text{T}T}_q^{(j)}={K\text{K}K}_{fa,q}\cdot {\sigma }_{ss,q}^{(j)}\text{\hspace{1em}(33)}$$
其中，
$${K\text{K}K}_{fa,q}=\{\begin{array}{l}{Q\text{Q}Q}^{-1}(\frac{P_{FA,R}}{2N_s}),q=1,2,3\\ {Q\text{Q}Q}^{-1}(\frac{P_{FA,T}}{2N_s}),q=4,5,6\end{array}\text{\hspace{1em}(34)}$$

${Q\text{Q}Q}^{-1}(p)$是零均值单位方差高斯分布的$(1-p)$分位数。系数$K_{fa,q}$反映了虚警的概率。只有当对所有$j$和$q$有以下条件时，说明导航系统通过第二层故障检测，
$${\tau }_q^{(j)}=\frac{|x_q^{(j)}-x_q^{(0)}|}{{T\text{T}T}_q^{(j)}}\le 1\text{\hspace{1em}(35)}$$

如果任何测试失败，导航系统将失去连续性。为了提高故障条件下的航行连续性，应尝试排除故障。我们在这里不提供排除方案，但我们将在今后的工作中开发一个准确、高效的排除算法。

对于实时应用，必须考虑算法的复杂性。从这个角度看，由于测量量大，上述方案的计算成本可能很高。为了解决这个问题，我们引入了故障分组技术，它可以大大减少被监控子集的数量。这种技术的基本思想是将图像分成几个不重叠的区域，称为超像素，每个超像素都包含一组特征。图2用一个2D示例说明了超像素的生成。

图2 二维图像分割示例（每个矩形都是单个超像素）  

在引入故障分组技术后，我们可以按照与传统MHSS方法相同的过程确定监测子集。但在这种情况下，我们需要将每个超像素视为单独的“特征”。超像素的先验故障概率表示超像素中任何特征发生故障的概率：
$$p_{sp}=1-(1-p_f{)}^{n_p}\text{\hspace{1em}(36)}$$
其中$p_f$和$p_{sp}$分别表示每个特征和每个超像素的先验故障概率；$n_p$是这个超像素中的特征个数。

5.3 保护水平计算

保护水平（PL）是为了保证位姿误差超过所述数字的概率小于或等于目标完好性风险而计算的概率误差包络。本文对每个位姿分量分别定义了PL。第$q$个分量的保护水平$P{L}_q$可通过求解下式确定：
$$P_{HMI,q}(1-\frac{p_{nm}}{P_{HMI}})=2Q(\frac{P{L}_q}{{\sigma }_q^{(0)}})+\sum _{j=1}^{N_s}{p}_{fs}^{(j)}Q(\frac{P{L}_q-T_q^{(j)}}{{\sigma }_q^{(j)}})\text{\hspace{1em}(37)}$$
其中，
$$P_{HMI,q}=\{\begin{array}{l}{P}_{HMI,R},q=1,2,3\\ P_{HMI,t},q=4,5,6\end{array}\text{\hspace{1em}(38)}$$
与该保护水平相关的安全性证明和求解公式（37）的方法在ARAIM基线算法中显示。在该式中，右边的每一项都是每个故障模式对完好性风险贡献的上界，左边的一项是分配给监控子集的目标完好性风险。

6 实验和结果

在本节中，进行了几个仿真来验证所提出的算法。为了表示真实城市环境中的特征几何，我们从KITTI数据集提供的图像中提取ORB特征。每个特征的深度也是通过比较左右相机的输出来计算的。图3显示了在相机系中提取的特征的几何形状。我们假定这幅图像来自一辆在良好建图环境中行驶的车辆，并且该图像与地图之间的特征对应关系已经确定。为了模拟噪声测量，我们根据表2所示的测量误差协方差对特征加入白噪声。由于所提出的位姿估计和完好性监测方案均为“快照”方法，即结果仅依赖于当前观测数据，因此图3所示的静态场景足以进行性能评估。

图3 图像中的特征点及其空间分布  

在模拟特征对应的基础上，首先验证了所提出的位姿确定算法和相关的协方差估计方法。为了捕捉车辆位姿的协方差变化，表4中所示的10个场景被用于执行此分析。该表中的参数表示车辆相对于局部地图的位姿。对于每个场景，我们进行蒙特卡罗模拟，生成5000个随机场景，用于统计上确定误差标准差。图4给出了统计误差和估计误差标准差。研究结果表明，该算法能较好地估计导航状态参数，证明了协方差估计方法的可行性。从图中可以看出，位姿不确定性对车辆姿态很敏感，但对平移保持不变。

表4 模拟中使用的案例列表

 

图4 统计（实线）和估计（虚线）误差标准差  

然后，我们进行了另一个仿真，以揭示故障分组对计算复杂度和导航性能的影响。在这个模拟中，我们假设如图3所示的特征构成了RANSAC生成的内点集。将每个特征的故障概率设置为不同的值进行灵敏度分析。此外，通过将三维图像分割为几个长方体来进行故障分组，长方体的大小对分组结果影响很大。表5显示了不同分组结果的CPU时间比较，图5显示了相关的保护水平。仿真实验在MATLAB 2018b上运行，笔记本电脑配置为Intel Core i5-8300H和8GB内存。注意，我们不计算案例1的保护水平，因为这个过程会消耗大量的时间。
 

表5 不同分组结果的计算复杂度比较

 

图5 保护水平随故障分组结果和先验故障概率的变化  

根据表5和图5的结果，我们可以得出以下结论。首先，如果不采用故障分组，完好性监测框架就不能应用于实时应用程序。其次，故障分组可以通过减少子集的数量而大大减少时间消耗，同时以提供保护水平为代价。第三，对故障概率的灵敏性分析表明，高故障概率会导致大量的子集和大的保护水平。因此，必须进行第一层故障检测，因为它可以有效地降低第二层检测中使用的特征的故障概率。

最后，通过仿真两种故障场景验证了所提出的故障检测方案。如图3所示，从图像中央的摩托车上提取部分特征（标记为超像素A），另一部分特征（标记为超像素B），位于图像左侧的树中。在第一种场景中，我们手动注入一个10米的故障到超像素A的深度分量中。这表示在地图中有一个非静态路标点的情况。第二种是多故障场景，在超像素A的故障基础上，往超像素B的深度分量上增加5米的故障。超像素B的故障可能是由于特征误匹配造成的。蒙特卡罗模拟采用的参数如表6所示。

表6 仿真中使用的参数

图6显示了$Z$方向（通常是沿街道方向）在标称和故障条件下的位置误差。可见，与无故障情况相比，视觉故障存在时，位置误差显著增大。由于较大的导航误差可能导致严重的事故，因此在安全关键应用中有必要进行故障检测。图7给出了三种场景下的故障检测结果。当统计值超过阈值时，故障检测器将会发出告警。如图所示，本次实验没有出现漏检或虚警现象。因此，结果证明了该故障检测方案在单故障和多故障情况下都具有良好的性能。

图6 在无故障和故障情况下Z方向的位置误差  

图7 在无故障和故障条件下检验统计量和相关阈值

7 结论

为了保证基于特征的视觉导航的安全性，本文开发了一个完好性监测框架。在此框架下，我们设计了一个双层故障检测方案，以应对误导特征，并严格推导保护水平，以量化导航安全。为了支持实时应用，我们还利用故障分组技术来降低该框架的计算复杂度。通过几个蒙特卡罗仿真来验证所提出的算法。首先，验证了基于最小二乘的位姿确定算法和相关协方差估计方法的可行性。此外，还指出了计算复杂度与导航完好性之间的一个关键权衡：故障分组可以大大减少算法的时间消耗，同时以提高保护水平为代价。高的先验故障概率同时会导致较大的计算量和较差的导航性能。最后，仿真结果表明了该故障检测算法的良好性能。我们未来的工作将集中在使用开源开源数据集验证所提出的框架，研究一种有效的故障排除算法，以及优化双层故障检测方案。

8 附件：公式（24）的证明

$\delta \phi \text{φ}\phi$通常被称为失准角，而${\epsilon }_{\phi }\text{εφ}{\epsilon }_{\phi }$表示方位角误差。它们之间的关系由下式给出，
$$\delta \phi \text{φ}\phi ={C\text{C}C}_b^n\cdot {C\text{C}C}_A^{\omega }\cdot {\epsilon }_{\phi }\text{εφ}{\epsilon }_{\phi }\text{\hspace{1em}(39)}$$
其中${C\text{C}C}_b^n={R\text{R}R}^T$，而${C\text{C}C}_A^{\omega }$由下式给出，
$${C\text{C}C}_A^{\omega }=\left[\begin{array}{ccc}1& 0& -s\beta \\ 0& c\alpha & s\alpha c\beta \\ 0& -s\alpha & c\alpha c\beta \end{array}\right]\text{\hspace{1em}(40)}$$
该方程由姿态动力学方程得到。将公式（40）代入公式（39），
$$\delta \phi \text{φ}\phi =\left[\begin{array}{ccc}c\beta c\gamma & -s\gamma & 0\\ c\beta s\gamma & c\gamma & 0\\ -s\beta & 0& 1\end{array}\right]\cdot {\epsilon }_{\phi }\text{εφ}{\epsilon }_{\phi }\text{\hspace{1em}(41)}$$
因此，我们有，
$${\epsilon }_{\phi }\text{εφ}{\epsilon }_{\phi }=\left[\begin{array}{ccc}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 \phi \text{φ}\phi \text{\hspace{1em}(42)}$$

9 参考文献

略