论文阅读《Visual Measurement Integrity Monitoring for UAV Localization》

1 摘要

无人机（UAV）越来越多地应用于安全、安保和救援任务中，因为它们需要对所处环境进行精确和可靠的位姿估计。为了保证视觉定位时的任务安全，有必要对视觉定位方案的完好性进行评估。然而，据我们所知，这种基于优化的视觉定位的完好性监测算法并不存在。接收机自主完好性监测（RAIM）在全球卫星导航系统（GNSS）中得到了广泛的应用，如飞机自主着陆等。本文受RAIM的启发，提出了一种新的方法来监测基于优化的视觉定位的完好性，并计算估计的位姿的保护水平，即各个方向上最大可能的平移误差。我们还提出了一个定量评估误差包络性能的度量。最后，我们使用EuRoC数据集验证了该保护水平，并证明了所提出的保护水平比常用的3σ3\sigma3σ方法提供了一个更可靠的包络。

2 介绍

近年来，无人机（UAV）在事件安全、搜索和救援、安全监视等具有挑战性的应用中取得了成功。对于某些特定的操作领域，例如室内、地下或城市中心，在UAV上的全球导航卫星系统（GNSS）信息并不总是可用或可靠。因此，无人机在这些地区部署的可行性在很大程度上取决于基于视觉的定位的准确性，作为GNSS系统的补充和替代。UAV未能正确执行定位任务可能导致严重损害车辆或附近的人。因此，不仅要利用视觉信息对无人机进行精确定位，而且要了解当前定位估计的可靠性，最大限度地减少视觉定位过程中未检测出的故障的可能性。

许多无人机视觉定位或SLAM算法已被开发出来，能够提供较长时间的鲁棒状态估计，如[1]和[2]。虽然这些算法能够以较低的故障率运行，但通过不断评估无人机上计算的状态估计的完好性，仍有可能进一步提高无人机状态估计的鲁棒性。完好性衡量的是对定位解决方案正确性的信任程度。

在机器人视觉定位中，3σ3\sigma3σ（正负3倍标准差）常被用来度量状态估计的不确定性边界，这相当于99.7%的概率下真值在该区域内。然而，3σ3\sigma3σ仅限定了在测量中没有外点的情况下的状态误差，即没有从建模的测量分布之外获得的测量值。对于现实生活中的无人机视觉定位，假设视觉前端工作良好，在任何时间测量中都没有外点是不合理的，因为视觉前端依赖于帧与帧之间的特征对应，这一过程很容易出错。因此，3σ3\sigma3σ方法往往过于激进，且不能包络无人机真实状态的误差。

本文工作的灵感来自于接收机自主完好性监测（RAIM），这是一种用于评估GNSS信号完好性的监测算法。它特别用于自主飞行着陆和其它对安全至关重要的GNSS应用中。RAIM首先使用冗余的测量数据来检查接收到的测量数据的一致性，并从单个卫星中检测出可能的错误测量数据。然后，确定水平方向保护水平（HPL）和垂直方向保护水平（VPL），这是故障检测算法在水平方向和垂直方向上不希望检测到的最大误差。近几年，将RAIM用于机器人定位的思想主要用于基于滤波的算法，而不是基于优化的算法。

虽然完好性监测是一个成熟的GNSS应用领域，但将其应用到基于优化的无人机视觉定位中仍具有重要意义。首先，在大多数无人机视觉定位算法中，系统依靠特征来代替卫星提供测量，与GNSS应用相比，测量数目要多得多。其次，在RAIM中，两颗卫星同时出现故障是很少见的。然而，对于视觉定位，不正确的测量值或外点的绝对数量要高得多，外点的比例通常大于10%。因此，需要一种不同的外点剔除方法来处理多个外点。此外，在GNSS应用中，每个卫星提供1个测量，但在视觉定位中，每个特征分别为单目或双目提供2个或3个测量。

在本文中，我们提出了一种新的状态估计的完好性监测方法，用于基于优化的无人机视觉定位中。我们首先使用基于[7]的统计方法来检测和排除不一致的视觉测量，并扩展了我们之前的工作[8]。然后计算出无人机状态下可能存在的最大平移误差。本文的主要贡献有：
（1）一种新的度量称为松弛包络紧度，它定量地评估了在高斯分布误差假设的应用中误差包络的性能。
（2）提出了一种基于优化的视觉定位的无人机状态估计误差近似上界确定方法，该方法比典型的3σ3\sigma3σ方法可靠得多。

本文的其余部分概述如下。相关工作将在第二节中讨论。在第三节中详细讨论了外点剔除和保护水平计算算法。提出的评估误差包络的度量如第四节所示。最后，本文方法的验证结果如第五节所示。

3 相关的工作

3.1 接收机自主完好性监测

完好性监测的概念已经发展并应用于GNSS领域。它是安全关键应用的必要组件，在这些应用中，不可靠的解决方案可能导致严重的伤害或死亡。RAIM是航空应用中最常用的完好性监测算法之一。它有两个基本功能。一是利用故障检测与排除算法检测卫星是否存在故障。二是计算HPLHPLHPL和VPLVPLVPL，这是故障检测算法不期望检测到的最大水平和垂直误差。Brown等人提出了第一个数学上严格的RAIM算法，该算法利用统计理论来检查冗余GPS测量的一致性，并与提供足够一致性的测量组计算HPL。Walter等人以Brown的方法为基础，开发出了加权的RAIM。他们工作的主要思想是，来自每个单独卫星的测量数据有不同水平的噪声。因此，他们不假设所有卫星都有一个固定的协方差值，而是建议通过将噪声加权协方差赋值给不同卫星的测量模型，以不同的方式来信任来自卫星的测量结果。两种方法都假设在故障检测算法完成后，系统中最多存在一个故障测量，然后根据该假设计算出保护水平。Angus修改了Brown和Walter提出的算法，在测量中考虑到有多个故障卫星。在视觉定位场景中，处理多个故障测量的能力非常有用，因为根据相机设置，每个特征对应2个或者3个测量值。因此，为了在视觉定位中考虑一个故障的特征，我们需要考虑2或3个故障的测量。

3.2 基于视觉的定位

基于视觉的定位或SLAM算法可以分为两大类，直接法和基于特征的方法。直接法将整个图像作为测量，以最小化光度误差为目标。基于特征的方法从图像中提取关键点，目标是最小化重投影误差。ORB-SLAM2是一种最先进的基于特征的SLAM系统，适用于单目、双目和RGB-D相机。其SLAM模式通过光束平差法来构造地图。ORB-SLAM2也有一个定位模式，允许重用地图，通过位姿优化来确定主体的状态。此外，结合视觉和惯性测量也显示了性能的提升，特别是在无人机应用中，例如[2]等。虽然这些算法在实验中表现出了很好的性能，但假设测量在任何时候都是无故障的是不合理的，因为从帧到帧寻找特征的对应关系时一个很容易出错的过程，特别是在动态环境中。本文的工作主要适用于基于特征的视觉定位，我们使用ORB-SLAM2的定位模式来验证我们的方法，如第6节所示。

3.3 视觉测量的外点剔除

RANSAC已广泛应用于基于视觉的机器人定位中，用于剔除特征外点。RANSAC的基本思想是用随机样本集来形成假设，再用其它样本来验证这些假设，并选择一致性最高的假设作为内点集合。另一种方法是使用统计测试来检验测量集是否符合假设的统计模型，如奇偶空间法和归一化新息平方法。这两种方法都假设测量模型的噪声服从高斯分布，并检验加权平方残差和是否服从χ2\chi^2χ2分布。奇偶空间法不需要系统的状态，它通过将一组冗余测量值投影到奇偶空间来检查它们的一致性。另一方面，归一化新息平方法假设一个估计的状态是已知的，并且它通常用于检查单个测量值是否为外点。Tong等人提出了一种批量新息测试，该测试扩展了归一化新息平方法，以去除批量测量中的外点。最近，Tzoumas等人提出了一种自适应修剪算法，该算法也是基于残差值来剔除外点。与使用固定的阈值不同，该算法中的阈值在每次迭代中都会更新。在本文中，我们将Tong的方法与奇偶空间法相结合，迭代剔除了多个外点，从而计算出了保护水平。

4 完好性监测框架

4.1 问题表述

在本节中，我们将展示双目相机设置下的问题表述。我们将基于视觉的定位作为一种非线性位姿优化。目标是通过最小化特征的重投影误差来确定相机当前帧的状态。我们遵循Barfoot引入的符号。有两个坐标系，即惯性坐标系Fi\mathcal{F}_iFi和与左相机中心相对应的相机坐标系Fc\mathcal{F}_cFc。相机的状态定义为x={rici,Cci}x=\{r_i^{ci},C_{ci}\}x={rici,Cci}，它是指惯性坐标系到相机坐标系的变换矩阵，其中rici∈R3r_i^{ci}\in \mathbb{R}^3rici∈R3且Cci∈SO(3)C_{ci} \in \mathbb{SO}(3)Cci∈SO(3)。在进行定位之前，先用特征构建一张地图，ripjir_i^{p_ji}ripji表示特征jjj在惯性坐标系中的位置。首先将特征位置从惯性坐标系变换到相机坐标系，然后再将特征位置投影到图像坐标平面，公式如下：
rcpjc=[xyz]=Cci(ripji−rici)(1)\pmb{r}_c^{p_jc}=\begin{bmatrix} x \\ y\\ z \end{bmatrix}=\pmb{C}_{ci}(\pmb{r}_i^{p_ji}-\pmb{r}_i^{ci}) \tag{1} rrrcpjc=⎣⎡xyz⎦⎤=CCCci(rrripji−rrrici)(1)
[ulvlδd]=π(rcpjc)=1z[fuxfvyfub]+[cucv0]+ej(2)\begin{bmatrix} u_l \\ v_l \\ \delta_d \end{bmatrix}=\pi(\pmb{r}_c^{p_jc})=\frac{1}{z}\begin{bmatrix} f_ux \\ f_vy \\ f_ub\end{bmatrix} + \begin{bmatrix} c_u \\ c_v \\ 0 \end{bmatrix} +\pmb{e}_j \tag{2} ⎣⎡ulvlδd⎦⎤=π(rrrcpjc)=z1⎣⎡fuxfvyfub⎦⎤+⎣⎡cucv0⎦⎤+eeej(2)
ej∼N(0,Qj)(3)\pmb{e}_j \sim \mathcal{N}(\pmb{0},\pmb{Q}_j) \tag{3} eeej∼N(000,QQQj)(3)
其中ulu_lul和vlv_lvl是左边图像中的坐标，δd\delta_dδd是视差，fuf_ufu和fvf_vfv是焦距，cuc_ucu和cvc_vcv是光心坐标，bbb是基线，π\piπ是双目相机投影函数，ej\pmb{e}_jeeej是测量噪声，它服从零均值协方差为Qj\pmb{Q}_jQQQj的高斯分布。在每一个图像帧中，位姿优化可以表述如下：
x∗={rici∗,Cci∗}=argminrici,Cci∑jρ(ey,jTQj−1ey,j)(4)\pmb{x}^*=\{\pmb{r}_i^{ci*},\pmb{C}_{ci}^*\}=\underset{{r}_i^{ci},{C}_{ci}}{argmin}\sum_j\rho(\pmb{e}^T_{y,j}\pmb{Q}_j^{-1}\pmb{e}_{y,j}) \tag{4} xxx∗={rrrici∗,CCCci∗}=rici,Cciargminj∑ρ(eeey,jTQQQj−1eeey,j)(4)
其中ρ\rhoρ是Huber损失函数，ey,j\pmb{e}_{y,j}eeey,j是特征jjj测量误差，可用下式确定，
ey,j(x)=yj−π(Cci(ripji−rici))(5)\pmb{e}_{y,j}(\pmb{x})=\pmb{y}_j-\pi(\pmb{C}_{ci}(\pmb{r}_i^{p_ji}-\pmb{r}_i^{ci})) \tag{5} eeey,j(xxx)=yyyj−π(CCCci(rrripji−rrrici))(5)
其中yjy_jyj是特征jjj的测量。

4.2 故障检测与排除

本小节展示了在视觉测量上如何应用故障检测与排除算法，它受RAIM启发，利用奇偶空间法。首先，对于每个特征jjj我们有如下测量模型，
yj=hj(x)+ej(6)\pmb{y}_j=h_j(\pmb{x})+\pmb{e}_j \tag{6} yyyj=hj(xxx)+eeej(6)
其中特征jjj的测量函数hj(x):R6⇒R3h_j(\pmb{x}): \mathbb{R}^6\Rightarrow \mathbb{R}^3hj(xxx):R6⇒R3定义如下，
hj(x)=π(Cci(ripji−rici))(7)h_j(\pmb{x})=\pi(\pmb{C}_{ci}(\pmb{r}_i^{p_ji}-\pmb{r}_i^{ci})) \tag{7} hj(xxx)=π(CCCci(rrripji−rrrici))(7)
然后将测量函数hj(x)h_j(\pmb{x})hj(xxx)关于某工作点x0\pmb{x}_0xxx0线性化，得到线性化的测量模型：
dyj=Hj⋅dx+ej(8)d\pmb{y}_j=\pmb{H}_j\cdot d\pmb{x}+\pmb{e}_j \tag{8} dyyyj=HHHj⋅dxxx+eeej(8)
其中dx∈R6d\pmb{x}\in\mathbb{R}^6dxxx∈R6表示状态扰动，H∈R3×6\pmb{H}\in\mathbb{R}^{3\times6}HHH∈R3×6是测量模型的雅克比，dyj∈R3d\pmb{y}_j\in\mathbb{R}^3dyyyj∈R3为特征jjj根据工作点x0\pmb{x}_0xxx0偏移的测量值。通过对当前帧中观察到的NNN个特征进行dyjd\pmb{y}_jdyyyj、Hj\pmb{H}_jHHHj和协方差矩阵Qj\pmb{Q}_jQQQj的叠加，我们做出如下定义：
dy=[dy1⋮dyN],Q=[Q1⋱QN],H=[H1⋮HN](9)d\pmb{y}=\begin{bmatrix} d\pmb{y}_1 \\ \vdots \\ d\pmb{y}_N \end{bmatrix},\ \pmb{Q}=\begin{bmatrix} \pmb{Q}_1 & & \\ & \ddots & \\ & & \pmb{Q}_N\end{bmatrix},\ \pmb{H}=\begin{bmatrix} \pmb{H}_1 \\ \vdots \\ \pmb{H}_N \end{bmatrix} \tag{9} dyyy=⎣⎢⎡dyyy1⋮dyyyN⎦⎥⎤, QQQ=⎣⎡QQQ1⋱QQQN⎦⎤, HHH=⎣⎢⎡HHH1⋮HHHN⎦⎥⎤(9)
公式（8）的叠加形式如下，
dy=H⋅dx+e(10)d\pmb{y}=\pmb{H}\cdot d\pmb{x}+\pmb{e} \tag{10} dyyy=HHH⋅dxxx+eee(10)
其中，
e∼N(0,Q)(11)e\sim \mathcal{N}(\pmb{0}, \pmb{Q}) \tag{11} e∼N(000,QQQ)(11)
我们将信息矩阵WWW定义为QQQ的逆，
W=Q−1(12)\pmb{W} = \pmb{Q}^{-1} \tag{12} WWW=QQQ−1(12)
估计扰动为公式（10）的加权最小二乘解，即
dx^=(HTWH)−1HTWdy(13)d\hat{x}=(\pmb{H}^T\pmb{W}\pmb{H})^{-1}\pmb{H}^T\pmb{W}d\pmb{y} \tag{13} dx^=(HHHTWWWHHH)−1HHHTWWWdyyy(13)
残差可以进一步计算如下，
ϵ=dy−Hdx^=(I−H(HTWH)−1HTW)dy(14)\epsilon=d\pmb{y}-\pmb{H}d\hat{x}=(\pmb{I}-\pmb{H}(\pmb{H}^T\pmb{WH})^{-1}\pmb{H}^T\pmb{W})d\pmb{y} \tag{14} ϵ=dyyy−HHHdx^=(III−HHH(HHHTWHWHWH)−1HHHTWWW)dyyy(14)
然而，如果测量中存在外点，公式（10）要改写成，
dy=H⋅dx+e+f(15)d\pmb{y}=\pmb{H}\cdot d\pmb{x}+\pmb{e} + \pmb{f}\tag{15} dyyy=HHH⋅dxxx+eee+fff(15)
其中f∈R3Nf\in \mathbb{R}^{3N}f∈R3N是在假设测量中存在外点的情况下，对测量中的误差进行建模的故障向量。那么，公式（14）需改写成，
ϵ=(I−H(HTWH)−1HTW)(dy−f)(16)\epsilon=(\pmb{I}-\pmb{H}(\pmb{H}^T\pmb{WH})^{-1}\pmb{H}^T\pmb{W})(d\pmb{y}-\pmb{f}) \tag{16} ϵ=(III−HHH(HHHTWHWHWH)−1HHHTWWW)(dyyy−fff)(16)
然后计算加权平方和残差：
λ=ϵTWϵ(17)\lambda=\epsilon^T\pmb{W}\epsilon \tag{17} λ=ϵTWWWϵ(17)
我们可以利用λ\lambdaλ来确定测量集中是否存在外点，且将λ\sqrt{\lambda}λ定义为检验统计量。如果测量集中没有外点，依据奇偶空间法，那么λ\lambdaλ服从n−mn-mn−m自由度的中心χ2\chi^2χ2分布。其中nnn表示测量的数量，mmm表示状态的数量，在视觉定位情况下，n=3Nn=3Nn=3N且m=6m=6m=6。如果测量集中存在外点，λ\lambdaλ服从非中心的χ2\chi^2χ2分布。利用加权平方和残差，λ\lambdaλ，来判断定位解是否无外点。通过计算中心χ2\chi^2χ2分布的1−Pfa1-P_{fa}1−Pfa分位数（自由度为3N−63N-63N−6），可以根据期望的虚警概率PfaP_{fa}Pfa解析地选择虚警阈值δ\deltaδ。如果λ<δ\lambda<\deltaλ<δ，则检验表明测量值之间是一致的，可以认为测量值中没有外点；如果λ>δ\lambda>\deltaλ>δ，有1−Pfa1-P_{fa}1−Pfa的概率认为测量集中至少存在一个故障。

如果测量集通过了外点检测算法，我们可以直接计算出保护水平，见第4.3节。如果发现测量集中包含外点，我们需要执行外点排除。在GNSS应用中，两颗卫星同时故障的情况很少见，但与此不同的是，视觉测量中的外点比例要高得多。因此，在基于视觉的定位中，需要一种不同的方法来排除测量中的外点。

我们对[7]中的方法进行了修改，提出了迭代奇偶空间外点拒绝法（IPSOR），排除多个外点用于完好性监测。两种算法都计算一组测量值的加权平方和残差，并迭代排除外点，直到其余测量值通过残差阈值检验。具体来说，IPSOR策略排除对统计检验量贡献最大的特征，并将其分类为外点，直到统计检验量λ\lambdaλ阈值δ\deltaδ。然后，我们将剩余的测量值分类为内点，并再次计算检验统计量。如果更新后的检验统计量小于更新后的阈值δ\deltaδ，则外点已完全排除。否则，我们重复此过程。在此过程中，如果内点数目小于阈值，则认为测量集中外点过多，定位解不可信。这种方法依赖于内部测量彼此一致和外点测量差异很大的假设。使用高斯牛顿法，IPSOR方法可以迭代应用，获得更好的线性化状态x0\pmb{x}_0xxx0，以避免排除内点。

4.3 计算保护水平

在视觉定位中，我们把保护水平定义为外点检测算法不期望检测到的每个方向上的最大平移误差，标记为PLxPL_{x}PLx、PLyPL_{y}PLy和PLzPL_{z}PLz，它们必须考虑可能的外点和噪声引入的误差。同样的方法也可以计算出总距离的保护水平，但我们使用的是特定轴的保护水平，以便与3σ3\sigma3σ方法进行比较。我们遵循并修改了Walter和Angus提出的方法来计算保护水平。一旦测量通过外点排除算法，我们假设测量中至多有一个外点。如果测量中有超过一个的故障特征，那么测量就不太可能通过外点排除测试。此外，根据应用的不同，我们总是可以考虑更多的故障特征，以获得更保守的保护水平。

为了解析确定PLxPL_xPLx、PLyPL_yPLy和PLzPL_zPLz，我们遵循[11]中的问题表述。由于每个特征产生3个测量值，我们将公式（15）中的故障向量f\pmb{f}fff表示为，
f=Pjf∗(18)\pmb{f}=\pmb{P}_j\pmb{f}^*\tag{18} fff=PPPjfff∗(18)
其中f∗∈R3\pmb{f}^*\in\mathbb{R}^3fff∗∈R3，且Pj∈R3N×3\pmb{P}_j\in\mathbb{R}^{3N\times3}PPPj∈R3N×3。在Pj\pmb{P}_jPPPj中每个3×33\times33×3矩阵代表一个特征。对Pj\pmb{P}_jPPPj而言，第jjj个3×33\times33×3矩阵是单位阵，其余的都是零矩阵。例如，如果第一个特征是外点，而其它的特征是内点，那么Pj\pmb{P}_jPPPj为，
Pj=P1=[100010001000⋮⋮⋮000](19)\pmb{P}_j=\pmb{P}_1=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \\ \vdots & \vdots & \vdots \\ 0 & 0 & 0 \end{bmatrix} \tag{19} PPPj=PPP1=⎣⎢⎢⎢⎢⎢⎢⎢⎡1000⋮00100⋮00010⋮0⎦⎥⎥⎥⎥⎥⎥⎥⎤(19)
由故障向量f\pmb{f}fff引入的加权平方和残差λf\lambda_fλf可以计算为：
λf=fTSf=f∗TPTSPf∗(20)\lambda_f=\pmb{f}^T\pmb{Sf}={\pmb{f}^*}^T\pmb{P}^T\pmb{SP}\pmb{f}^* \tag{20} λf=fffTSfSfSf=fff∗TPPPTSPSPSPfff∗(20)
其中，
S=W(I−H(HTWH)−1HTW)(21)\pmb{S}=\pmb{W}(\pmb{I}-\pmb{H}(\pmb{H}^T\pmb{WH})^{-1}\pmb{H}^T\pmb{W}) \tag{21} SSS=WWW(III−HHH(HHHTWHWHWH)−1HHHTWWW)(21)
根据公式（13），由故障向量f\pmb{f}fff引入的定位解中的外点引起的误差的平方(εf)2(\varepsilon_f)^2(εf)2也可以计算为，
(εf)2=fTDif=f∗TPTDiPf∗(22)(\varepsilon_f)^2=\pmb{f}^T\pmb{D}_i\pmb{f}={\pmb{f}^*}^T\pmb{P}^T\pmb{D}_i\pmb{P}\pmb{f}^* \tag{22} (εf)2=fffTDDDifff=fff∗TPPPTDDDiPPPfff∗(22)
其中Di\pmb{D}_iDDDi依赖于方向，假设位姿的前三个分量对应于位置部分，则计算方法如下：
Di=WH(HTWH)−1AiTAi(HTWH)−1HTW(23)\pmb{D}_i=\pmb{WH}(\pmb{H}^T\pmb{WH})^{-1}\pmb{A}_i^T\pmb{A}_i(\pmb{H}^T\pmb{WH})^{-1}\pmb{H}^T\pmb{W}\tag{23} DDDi=WHWHWH(HHHTWHWHWH)−1AAAiTAAAi(HHHTWHWHWH)−1HHHTWWW(23)
A1=[100000](24)\pmb{A}_1=\begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} \tag{24} AAA1=[100000](24)
A2=[010000](25)\pmb{A}_2=\begin{bmatrix} 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix} \tag{25} AAA2=[010000](25)
A3=[001000](26)\pmb{A}_3=\begin{bmatrix}0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} \tag{26} AAA3=[001000](26)
其中i=1,2,3i=1,2,3i=1,2,3分别指xxx方向、yyy方向和zzz方向。从公式（22）中，我们发现向量f\pmb{f}fff越大，定位解的误差就越大。但由于通过了外点排除检验，f∗TPTSPf∗{\pmb{f}^*}^T\pmb{P}^T\pmb{SP}\pmb{f}^*fff∗TPPPTSPSPSPfff∗的最大值被限制在小于δ\deltaδ的范围内。因此，由外点引入的向量f∗\pmb{f}^*fff∗需要被分配，以使解中由外点引起的误差最大化，同时仍然满足f∗TPTSPf∗=δ{\pmb{f}^*}^T\pmb{P}^T\pmb{SP}\pmb{f}^*=\deltafff∗TPPPTSPSPSPfff∗=δ的条件。保护水平所考虑的由外点诱发的误差的平方可通过求解优化问题来确定：
{maxf∗,Pj∈Pf∗TPjTDiPjf∗s.t.f∗TPjTSPjf∗=δ(27)\begin{cases} \underset{f^*,P_j\in P}{max} {\pmb{f}^*}^T\pmb{P}_j^T\pmb{D}_i\pmb{P}_j\pmb{f}^* \\ s.t. \ \ \ {\pmb{f}^*}^T\pmb{P}_j^T\pmb{S}\pmb{P}_j\pmb{f}^*=\delta \end{cases} \tag{27} ⎩⎨⎧f∗,Pj∈Pmaxfff∗TPPPjTDDDiPPPjfff∗s.t.   fff∗TPPPjTSSSPPPjfff∗=δ(27)
根据参考文献[11]，我们可以从方程中消去f∗\pmb{f}^*fff∗，将约束优化问题简化为无约束优化问题，如下所示：
maxPj∈PΛmax(Di,Pj,S)δ(28)\underset{P_j\in P}{max}\ \ \pmb{\Lambda}_{max}(\pmb{D}_i,\pmb{P}_j,\pmb{S})\delta \tag{28} Pj∈Pmax  ΛΛΛmax(DDDi,PPPj,SSS)δ(28)
其中Λmax(Di,Pj,S)\pmb{\Lambda}_{max}(\pmb{D}_i,\pmb{P}_j,\pmb{S})ΛΛΛmax(DDDi,PPPj,SSS)是如下矩阵的最大特征值，
PjTDiPj(PjTSPj)−1(29)\pmb{P}_j^T\pmb{D}_i\pmb{P}_j(\pmb{P}_j^T\pmb{S}\pmb{P}_j)^{-1} \tag{29} PPPjTDDDiPPPj(PPPjTSSSPPPj)−1(29)
我们可以遍历所有的特征，找到使得公式（28）最大的那个Pj\pmb{P}_jPPPj，其定义为：
P∗=argmaxPj∈PΛmax(Di,Pj,S)δ(30)\pmb{P}^*=\underset{P_j\in P}{argmax}\ \ \ \pmb{\Lambda}_{max}(\pmb{D}_i,\pmb{P}_j,\pmb{S})\delta \tag{30} PPP∗=Pj∈Pargmax   ΛΛΛmax(DDDi,PPPj,SSS)δ(30)
因此，考虑外点引起的误差的保护水平分量为，
εf=Λmax(Di,P∗,S)δ(31)\varepsilon_f=\sqrt{\pmb{\Lambda}_{max}(\pmb{D}_i,\pmb{P}^*,\pmb{S})\delta} \tag{31} εf=ΛΛΛmax(DDDi,PPP∗,SSS)δ(31)
定位解中噪声引起的误差εn\varepsilon_nεn可以用下式来计算：
εn=k[(HTWH)−1]i,i(32)\varepsilon_n=k\sqrt{[(\pmb{H}^T\pmb{WH})^{-1}]_{i,i}} \tag{32} εn=k[(HHHTWHWHWH)−1]i,i(32)
其中i=1,2,3i=1,2,3i=1,2,3分别指xxx方向、yyy方向和zzz方向。并且kkk为指定检测概率PdP_dPd对应的标准差个数。在实际生活应用中，PdP_dPd通常设置为99.73%，相应的k=3k=3k=3。结合外点引起的误差和噪声引起的误差，我们可以定义如下的视觉定位保护水平：
PLi=εf+εn=Λmax(Di,P∗,S)δ+k[(HTWH)−1]i,i(34)PL_i=\varepsilon_f+\varepsilon_n=\sqrt{\pmb{\Lambda}_{max}(\pmb{D}_i,\pmb{P}^*,\pmb{S})\delta}+k\sqrt{[(\pmb{H}^T\pmb{WH})^{-1}]_{i,i}} \tag{34} PLi=εf+εn=ΛΛΛmax(DDDi,PPP∗,SSS)δ+k[(HHHTWHWHWH)−1]i,i(34)
其中i=1,2,3i=1,2,3i=1,2,3分别指xxx方向、yyy方向和zzz方向。PLxPL_xPLx、PLyPL_yPLy和PLzPL_zPLz分别等价于PL1PL_1PL1、PL2PL_2PL2和PL3PL_3PL3。

5 性能评估

一个估计过程的最佳误差包络应该尽可能得紧，同时仍然在任何时候包络误差。然而，对于有外点的非线性系统，不可能保证所提出的包络在任何时候都能成功地包络住真实误差。目前还没有定量地评估潜在的误差包络性能的度量。因此，我们提出了一种新的松弛包络紧度（RBT）度量，用于定量地评估误差包络在假设误差服从高斯分布的应用中的性能。指标Z\mathcal{Z}Z计算如下，
Z=∑i=1Nρ(vi−eiσi)2N(35)\mathcal{Z}=\sqrt{\frac{\sum_{i=1}^N\rho(\frac{v_i-e_i}{\sigma_i})^2}{N}} \tag{35} Z=N∑i=1Nρ(σivi−ei)2(35)
其中viv_ivi和eie_iei分别是对于样本iii的误差包络和误差，NNN是样本数量，σi\sigma_iσi是给定样本的误差协方差，ρ\rhoρ是权重函数，定义如下：
ρ={1vi≥eiτvi<ei(36)\rho=\begin{cases} 1 & v_i\geq e_i \\ \tau&v_i<e_i \end{cases} \tag{36} ρ={1τvi≥eivi<ei(36)
因为误差应该是一个上界，而不是对误差期望的预测，所以权重函数ρ\rhoρ应该惩罚误差包络故障，而不是放松包络。系数τ\tauτ决定了度量在多大程度上倾向于一个具有较高故障概率的紧密误差包络，而不是一个具有较低故障概率的保守包络，反之亦然，并取决于用户定义的检测概率PdP_dPd。这个概率PdP_dPd是该方法正确包络用户要求的误差的最小概率。

为了确定τ\tauτ的值，我们假设误差服从无外点的高斯分布。设Φ−1\Phi^{-1}Φ−1为高斯分布的分位数函数，并定义理想的误差包络v∗v^*v∗为，
v∗=Φ−1(1−1−Pd2)(37)v^*=\Phi^{-1}(1-\frac{1-P_d}{2}) \tag{37} v∗=Φ−1(1−21−Pd)(37)
这个包络的选择与高斯分布的检测概率PdP_dPd的定义完全一致。给定包络v∗v^*v∗，我们现在解出使度量Z\mathcal{Z}Z最小化的τ\tauτ值。请注意，在RBT度量中，由于我们用每个样本的协方差除以包络和真实误差的差，τ\tauτ的值与σi\sigma_iσi的值是独立的，因为假设误差服从高斯分布。

对于安全关键应用，PdP_dPd应该定义为接近100%，这样这个度量更倾向于保守的方法，但能够在更高的时间百分比内正确地包络误差。对于安全性不那么重要的应用程序，可以不那么积极地定义PdP_dPd，这样度量就倾向于提供更紧密包络的方法，但可能在某些时候无法对误差进行包络。

6 实验结果

为了验证所提出的方法，并确定保护水平是否正确包络住了各方向上的平移误差，我们在著名的EuRoC数据集上对machine hall序列进行了实验。EuRoC数据集由微型飞行器（MAV）采集，利用Leica MS50激光跟踪器捕捉MAV在machine hall的真实位姿。

我们利用了ORB-SLAM2的定位模式，并添加了提出的外点剔除和保护水平计算模块。我们首先使用真实位姿来构建地图，然后使用ORB-SLAM2的定位模式和提出的完好性监测算法来获得对每帧的估计位姿和保护水平。对于基于双目视觉的定位，不需要初始化就可以恢复尺度。因此，在初始化之后，每个序列开始时MAV处于静止状态，我们开始估计。然后，将真值和估计的轨迹进行对齐，并对误差进行评估。最后，我们将各方向的保护水平和3σ3\sigma3σ包络与每帧的真实平移误差进行比较。

在本实验中，我们设虚警概率PfaP_{fa}Pfa为0.05，设公式（32）中的kkk为3，这是机器人应用的典型值。在ORB-SLAM2中，每个特征的协方差矩阵Qj\pmb{Q}_jQQQj与提取关键点时的尺度相关。因为ORB-SLAM2是一种仅基于视觉的SLAM算法，所以决定它们在确定相机位姿时重要性的是特征的协方差之间的相对值，而不是协方差的绝对值。首先，我们使用默认的ORB-SLAM2协方差来进行实验，该协方差被设置为1像素，用于在基本尺度级别中检测到的特征。图1显示了利用1像素假设，MH_01_easy整个轨迹的zzz方向上的保护水平、3σ3\sigma3σ包络和真实误差。结果表明，保护水平能够包络95%以上的帧，而3σ3\sigma3σ只能包络50%左右的帧。3σ3\sigma3σ包络的这种惨烈失败的可能原因是，首先，在这些帧中，3σ3\sigma3σ没有包络误差，其次，测量模型上的内点测量噪声的协方差设置得过小。为了避免第二种情况，我们在假设了1、1.5和2像素协方差的基础尺度水平上进行实验。这些是用于相机测量模型的协方差的典型值。

图1 利用1像素协方差假设，对EuRoC数据集中的MH_easy_01序列计算z方向的保护水平、3sigma和误差。3sigma方法的包络范围仅在54%左右，保护水平的包络范围在95%以上。

在图2中，通过绘制在不同的测量协方差假设下误差包络与真实误差的差值在xxx方向上的累积分布，我们比较了保护水平方法和3σ3\sigma3σ方法。理想的误差包络将完全包含xxx正半轴，因为我们希望误差包络能够包络真正的误差，以及分布尽可能接近于0，因为我们希望误差包络一定会尽可能的紧密。曲线与垂直线x=0x=0x=0之间的交点给出了误差包络方法未能包络误差的帧数百分比。正如预期的那样，与3σ3\sigma3σ相比，保护水平提供了一个更保守的误差包络，但能够以更高的百分比包络误差。如果我们使用存在1个像素的假设，这是ORB-SLAM2中默认的协方差值，我们发现3σ3\sigma3σ只能以较低的比例包络xxx方向上误差，分别是MH_01_easy的35%，MH_02_easy的54%，MH_03_medium的22%，MH_04_difficult的43%和MH_05_difficult的45%。然而，在MH_03_medium中，保护水平能够以85%左右的比例包络误差，其余序列的在95%左右。如果我们使用1.5像素和2像素假设，3σ3\sigma3σ方法能够在一个很高百分比的情况下正确包络误差，但仍然明显低于保护水平方法。保护水平方法对于MH_03_medium约为95%，对于其余的接近100%。类似的结果也适用于yyy方向和zzz方向。

图2 EuRoC数据集中machine hall序列的误差包络和误差的差值的累积分布图。与3sigma方法相比，该保护水平方法以较高的比例正确地包络了所有序列的误差。

我们还将提出的保护水平与3σ3\sigma3σ方法进行了比较，使用第5节中提出的度量。我们将PdP_dPd参数设置为99.73%，因为这是现实生活中最常用的检测概率之一，也与用于比较的3σ3\sigma3σ包络相对应。由于我们利用高斯分布确定了度量中的惩罚系数τ\tauτ，并选择PdP_dPd为99.73%，因此如果误差真正遵循零均值高斯分布，则度量更倾向于3σ3\sigma3σ方法。然而，从表1中可以发现，在所有3个协方差假设下，提出的保护水平在所有序列都优于3σ3\sigma3σ方法。结果表明，基于特征的视觉测量容易产生误差，因此位置解的误差并不完全服从高斯分布。此外，该方法能够正确地将误差包络在较高的百分比上，并在所选的度量上优于3σ3\sigma3σ。

表1 使用提出的RBT指标的性能比较

对于MH_03_medium序列，保护水平和3σ3\sigma3σ的结果均明显低，因为该序列获得的真实误差明显大于其它序列。假设地图上的点可能包含这个序列的一些误差。为了验证这个假设，我们执行实验，使用由ORB-SLAM2的SLAM模式构建的地图，而不是由地面真值构造的地图，并评估相对于SLAM模式的定位解，而不是地面真值，这消除了解中地图的误差。对于MH_03_medium系列，我们发现在这种情况下，平移的RSME为0.011米，远小于使用地面真值获得的0.032米。在1像素、1.5像素和2像素的假设下，xxx方向上的保护水平的性能分别为17.8、18.4和18.7，而3σ3\sigma3σ方法的性能分别为90.0、43.2和23.2。在使用提出的RBT度量下，保护水平仍然优于3σ3\sigma3σ方法。这同样适用于其它两个方向和其它序列。

我们注意到，虽然保护水平能够更好地包络误差，但与3σ3\sigma3σ相比，它产生的误差包络较松散。潜在的问题是，在操作期间，它可能会导致更多的误报警。我们认为，误差包络的选择应取决于应用的安全标准，所提出的保护水平更适合生命安全关键应用。

7 结论及未来工作

本文提出了一种完好性监测算法来估计视觉定位解中最大可能的平移误差。该框架受到RAIM的启发，我们并进行了修改，以适应视觉定位问题的表述。它首先基于奇偶空间法来检测外点，然后计算外点检测算法不期望检测到的最大可能误差。此外，我们还提出了一种松弛包络紧密度来定量地评估误差包络的性能。最后，通过在EuRoC数据集上进行实验，并使用所提出的度量对结果进行评估，结果表明所提出的保护水平比典型的3σ3\sigma3σ方法产生了更可靠的包络，并为评估方案的完好性提供了一种方法。未来的工作将包络在计算保护水平时考虑地图的不确定性和可能不正确的特征协方差，以及进一步发展为视觉里程计提供完好性监测的概念。

8 参考文献

略