论文阅读《Pedestrian Dead Reckoning-Assisted Visual Inertial Odometry Integrity Monitoring》

摘要
1 介绍
2 背景
3 视觉误差分析和自主完好性监测
- 3.1 视觉误差分析
- - 3.1.1 不足的特征
  - 3.1.2 光照导致特征跟踪失败
  - 3.1.3 特征分布不均匀
  - 3.1.4 运动特征
- 3.2 PDR辅助视觉完好性监测
4 实验与评估
- 4.1 评估环境影响
- - 4.1.1 缺少特征
  - 4.1.2 光照导致特征跟踪失败
  - 4.1.3 特征分布不均匀
  - 4.1.4 移动特征点
- 4.2 建议的PDR辅助视觉完好性监测的评估
- - 4.2.1 A段
  - 4.2.2 B段
  - 4.2.3 C段
5 总结和讨论
参考文献

摘要

视觉惯性里程计(VIOs)由于相机的通用性和便捷性，在室内定位领域受到越来越多的关注。然而，VIO的视觉观测更容易受到环境的影响，观测误差影响最终的定位精度。针对这一问题，我们分析了不同场景下产生视觉观测误差的原因及其对定位精度的影响。提出了一种利用短时可靠性的行人航迹推算(PDR)来辅助视觉完好性监测和减少定位误差的新方法。该方法通过VIO和PDR输出的自动切换来选择最优的定位。实验对所提出的PDR辅助视觉完好性监测进行了测试和评价。传感器实验套件包括一个双目摄像机和一个惯性测量单元(IMU)。结果表明，该系统在低光照、背景纹理信息较少或运动目标较少的环境下具有较好的室内定位性能。

1 介绍

在现代社会，人们越来越依赖定位服务。提供准确的室内定位服务的需求越来越迫切。Wi-Fi、蓝牙、相机、惯性传感器等基于不同类型传感器的室内定位技术正在迅速发展。由于相机传感器可以提供丰富的环境视觉信息，且相机可以很容易获得，因此基于视觉的室内定位技术越来越受到关注。根据相机的工作模式，可以分为三种：单目、双目和RGB-D。单目相机只有一个摄像头，具有结构简单、成本低的优点，但存在尺度不确定的缺点。双目相机和RGB-D相机的目的是克服单目模式无法确定距离的缺点。双目相机由两个相机组成。我们可以利用两个相头之间的距离来估计每个像素的空间位置，这个过程非常类似于人眼。RGB-D相机通过物理测量获得深度信息，比双目相机节省了大量的计算量。然而，RGB-D相机存在噪声大、视场小、容易暴露在阳光下和无法测量透明材料等问题。因此，我们选择双目相机作为我们的基于视觉的室内定位系统的传感器。

基于视觉的室内定位技术可分为视觉里程计(VO)和视觉惯性里程计(VIO)。VO根据所捕获图像中的特征运动估计相机的运动。VIO的体系结构包括两个主要组件：前端和后端。前端将传感器数据抽象成便于估计的模型，而后端则对前端产生的抽象数据进行位置估计。根据是否需要在前端提取特征，VO可以分为特征点法和直接法两种。基于特征方法的前端是主流的视觉里程计，如ORB-SLAM、S-PTAM。然而，VO难以处理动态障碍物，且极易受环境影响。惯性测量单元(IMU)通过接收运动信息来减轻动态物体的影响。虽然IMU可以测量角速度和加速度，但这些测量中存在显著的漂移，这使得IMU两次积分得到的位姿非常不可靠。相机数据可以有效地估计和校正IMU引起的漂移。尽管相机和惯性传感器的使用分别由于视线内的动态障碍和漂移积累而具有挑战性，但这两种传感器的集成可以弥补其缺点，提供更精确的定位解决方案。VIO基于IMU和相机之间的互补性，为室内定位服务提供了可靠的选择。参考文献[17]中提出了一种带有可穿戴相头和惯性传感器的行人定位系统。采用基于惯性测量的航迹推算法估计相对位移，通过图像匹配法修正累积误差。Li等人[18]提出了一种使用相机和IMU的移动设备上的3D运动跟踪和重建系统。根据位姿估计方法的不同，VIO可分为两类：滤波估计方法和基于优化的估计方法。基于滤波器的经典VIO包括多状态约束卡尔曼滤波器(MSCKF)和鲁棒视觉惯性里程计(ROVIO)。一种基于优化方法的VIO是视觉惯性状态估计器(VINS)。

基于视觉的定位系统非常容易受到环境影响。提取稳定和准确的视觉观测信息的能力是影响定位精度的关键因素。当视觉观测(图像特征)很少或分布不均匀时，基于视觉的定位系统的定位误差会非常大，甚至会导致系统崩溃。Alexander[22]提出了一种整合IMU数据的有效方法来克服这些视觉观测问题。本工作利用IMU数据来预测运动，通过假设恒定速度来获得相机的初始猜测。Tashfeen[23]提出了一种基于扩展卡尔曼滤波(EKF)的松散耦合三维惯性传感器系统和VO的一种集成方案。然而，EKF的估计结果取决于视觉观测的质量。一些研究更多地关注于系统和算法的鲁棒性，而不是量化和可验证的完好性，特别是基于特征的处理。Veth[25]为时序图像帧之间的特征对应引入了区域边界的概念，并加入了一些独特的特征准则，可以提供一些保护，避免特征对应错误。Koch[26]和Kyriakoulis[27]专注于使用额外的视觉测量，如颜色。然而，这些方法只提高了视觉观测的质量，并没有解决视觉观测稀少的问题。

行人航迹推算(PDR)系统包含加速度计和陀螺仪的数据，可以在室内惯性导航系统中提供相对定位信息、速度和方向。Jimenez使用IMU对行人航迹推算算法进行了广泛的研究。Lee在考虑转向和握手事件的影响下，提出了一种基于识别六种位姿和四种模式的结构化环境下多姿态PDR室内定位实验启发式方法，以减少航向误差。但在长期运行中，PDR会产生累积误差，导致定位结果不可靠。Yan提出了一种融合VO和PDR的方法，该方法匹配VO和PDR的定位信息的时间步长，用于惯性定位校准。这种融合匹配方法在视觉观测质量较差的情况下，仍然保留了视觉误差引起的定位误差。Dae引入了一种选择性组合方法，在全球导航卫星系统(GNSS)受到挑战的环境下，应用于多个导航传感器，以提高定位精度。采用加权最小二乘法推导出仅衡量特征点几何分布优劣的性能指标。该方法不考虑特征点稀疏和移动的情况。

本文分析了视觉定位系统在特定环境下定位误差较大时的误差来源，将其分为四种情况。我们提出了一种基于视觉观测的行人航迹推算系统的自主完好性监测。根据PDR短期可靠的特点，当视觉观测不可靠时，PDR可以输出定位结果。实验结果表明，我们的定位系统在纹理较少、动态障碍或光照较低的室内环境下具有更强的鲁棒性。本研究的主要贡献如下：

我们分析了误差来源，并将其分为四种不同的误差情况，即视觉定位系统在特定的室内环境中，纹理较少、动态障碍物较少和光照较弱时的定位误差较大。
我们提出了一种基于视觉观测的行人航迹推算系统的自主完好性监测。针对PDR具有短期可靠性的特点，提出了一种PDR辅助视觉完好性监测系统，在室内环境中自动切换VIO(或VO)和PDR之间的状态，以提供更准确的位置。

2 背景

视觉惯性里程计一般由前端和后端两部分组成。前端主要处理传感器的观测信息，如特征提取、特征跟踪、特征筛选、IMU预积分处理、图像与IMU数据的组合等。后端主要对前端产生的抽象数据进行位置估计，方法是通过滤波器或优化方案最小化观测产生的残差。系统结构框图如图1所示。

图1 视觉惯性里程计的完整流程

后端的目标是为全局参考系 { G } \{G\} {G}估计相机系 { C } \{C\} {C}的三维姿态。由于一个双目相机由两个镜头组成，相机系用 { C k , k = 1 o r 2 } \{C_k,\ k = 1\ or\ 2\} {Ck, k=1 or 2}表示。为了更清晰、更简单地分析视觉观测对位姿估计的影响，我们定义了定位系统的状态向量和观测向量。视觉惯性里程计在 k k k时刻的状态向量 X k X_k Xk如公式（1）中所示，包括了IMU的状态 X I M U k X_{IMU_k} XIMUk和相机的位姿（姿态为 G C k q ˉ _G^{C_k}\bar{q} GCkqˉ和位置 G p C k ^Gp_{C_k} GpCk）。
X k = [ X I M U k T G C k q ˉ T G p C k T ] T , w h e r e X I M U = [ G I M U q ˉ T b g T G v T b a T G p T ] T (1) X_k=[X_{IMU_k}^T\ \ _G^{C_k}\bar{q}^T \ \ ^Gp_{C_k}^T]^T, \ \ where\ X_{IMU}=[^{IMU}_G\bar{q}^T \ \ b_g^T \ \ ^Gv^T \ \ b_a^T \ \ ^Gp^T]^T \tag{1} Xk=[XIMUkT GCkqˉT GpCkT]T, where XIMU=[GIMUqˉT bgT GvT baT GpT]T(1)
其中 G I M U q ˉ T _G^{IMU}\bar{q}^T GIMUqˉT是从坐标系 { G } \{G\} {G}到坐标系 { I M U } \{IMU\} {IMU}的旋转； G v ^Gv Gv和 G p ^Gp Gp是IMU相对于坐标系 { G } \{G\} {G}的位置和速度； b g b_g bg和 b a b_a ba是陀螺仪和加速度计测量的偏差。

第 k k k次相机测量包含了第 k k k次相机位姿 ( G C k q ˉ , G p C k ) (^{C_k}_G\bar{q},\ ^Gp_{C_k}) (GCkqˉ, GpCk)所观察到的一系列特征点。特征点 z i z_i zi的测量模型用如下公式表示：
z i = 1 C Z i [ C X i C Y i ] + n i , i ∈ f , C ∈ { C 1 , C 2 } (2) z_i=\frac{1}{^CZ_i}\begin{bmatrix} ^CX_i \\ ^CY_i \end{bmatrix} + n_i, \ \ i \in f,C\ \ \in \{C_1,C_2\} \tag{2} zi=CZi1[CXiCYi]+ni, i∈f,C ∈{C1,C2}(2)
其中 f f f表示所有特征点的集合； C 1 C_1 C1和 C 2 C_2 C2分别表示左右相机； n i n_i ni表示 2 × 1 2\times 1 2×1的图像噪声向量。特征位置在相机系的坐标， C p i ^Cp_i Cpi，如下所示：
C p i = [ C X i C Y i C Z i ] = C ( G C i q ˉ ) ( G p i − G p c ) (3) ^Cp_i=\begin{bmatrix} ^CX_i \\ ^CY_i \\ ^CZ_i \end{bmatrix}=C(^{C_i}_G\bar{q})(^Gp_i-^Gp_c) \tag{3} Cpi=⎣⎡CXiCYiCZi⎦⎤=C(GCiqˉ)(Gpi−Gpc)(3)
其中 G p i ^Gp_i Gpi是特征点在全局坐标系中的坐标值； G p c ^Gp_c Gpc是相机在全局坐标系中的坐标值； C ( G C i q ˉ ) C(^{C_i}_G\bar{q}) C(GCiqˉ)是相机系和全局系之间的旋转矩阵。一旦得到特征位置的估计，我们就可以计算测量残差：
r i = z i − z ^ i ≈ H C X ~ + H i G p ~ i + n i , w h e r e { H C = ∂ z i ∂ C 1 p i ⋅ ∂ C 1 p i ∂ X C 1 + ∂ z i ∂ C 2 p i ⋅ ∂ C 2 p i ∂ X C 1 H i = ∂ z i ∂ C 1 p i ⋅ ∂ C 1 p i ∂ G p i + ∂ z i ∂ C 2 p i ⋅ ∂ C 2 p i ∂ G p i (4) r_i=z_i-\hat{z}_i \approx H_C\tilde{X}+H_i^G\tilde{p}_i+n_i, \ where\begin{cases} H_C=\frac{\partial z_i}{\partial ^{C_1}p_i} \cdot \frac{\partial ^{C_1} p_i}{\partial X_{C_1}}+\frac{\partial z_i}{\partial ^{C_2}p_i} \cdot \frac{\partial ^{C_2}p_i}{\partial X_{C_1}} \\ H_i=\frac{\partial z_i}{\partial ^{C_1}p_i} \cdot \frac{\partial^{C_1}p_i}{\partial ^G p_i} + \frac{\partial z_i}{\partial ^{C_2}p_i}\cdot \frac{\partial ^{C_2}p_i}{\partial ^Gp_i} \end{cases} \tag{4} ri=zi−z^i≈HCX~+HiGp~i+ni, where{HC=∂C1pi∂zi⋅∂XC1∂C1pi+∂C2pi∂zi⋅∂XC1∂C2piHi=∂C1pi∂zi⋅∂Gpi∂C1pi+∂C2pi∂zi⋅∂Gpi∂C2pi(4)
其中 H c H_c Hc和 H i H_i Hi是测量 z i z_i zi对特征状态和位置估计的的雅可比矩阵。利用由特征点组成的所有测量方程集，通过最小化误差得到最优解，得到最优位置估计。
m i n ∣ ∣ r − H [ X ~ G p ~ i ] ∣ ∣ 2 , H = [ H C H 1 H C H 2 ⋮ ⋮ H C H i ] (5) min\ ||r-H\begin{bmatrix} \tilde{X} \\ ^G\tilde{p}_i \end{bmatrix}||^2, \ \ H=\begin{bmatrix} H_C &H_1 \\ H_C & H_2\\ \vdots & \vdots \\ H_C & H_i \end{bmatrix} \tag{5} min ∣∣r−H[X~Gp~i]∣∣2, H=⎣⎢⎢⎢⎡HCHC⋮HCH1H2⋮Hi⎦⎥⎥⎥⎤(5)

3 视觉误差分析和自主完好性监测

3.1 视觉误差分析

在纹理较少、光照较低或动态障碍物等特殊环境下，定位结果存在较大误差。在本研究中，我们深入研究了四种情况下的误差来源：室内环境中纹理较少导致特征不足；室内环境中光线昏暗导致特征跟踪失败；室内环境中纹理不均匀导致特征分布不均匀；室内环境中与动态障碍物产生运动特征。

3.1.1 不足的特征

常用的特征提取算法有SIFT、SURF、FAST和ORB算法。这些算法通常用于处理VIO项目。在一幅图像中，与周围像素有强烈对比的点被定义为特征点。点 P P P的对比度可以表示为：
V ( x , y ) = ∑ ∣ I ( x + Δ x , y + Δ y ) − I ( x , y ) ∣ V(x,y)=\sum |I(x+\Delta x, y + \Delta y)-I(x,y)| V(x,y)=∑∣I(x+Δx,y+Δy)−I(x,y)∣
, V ( x , y ) ∼ ∑ [ I x 2 ⋅ Δ x 2 + I y 2 ⋅ Δ y 2 + 2 I x I y ⋅ Δ x Δ y ] (6) , \ \ V(x,y)\sim \sum[I_x^2\cdot \Delta x^2+I_y^2\cdot \Delta y^2+2I_xI_y\cdot \Delta x \Delta y] \tag{6} , V(x,y)∼∑[Ix2⋅Δx2+Iy2⋅Δy2+2IxIy⋅ΔxΔy](6)
其中 x x x和 y y y表示 P P P的像素坐标值。 I ( x , y ) I(x,y) I(x,y)和 V ( x , y ) V(x,y) V(x,y)分别表示该点的灰度值和对比度。 V V V的值取决于点 P P P在 x x x和 y y y方向上的梯度， I x I_x Ix和 I y I_y Iy。梯度值越大，越容易被检测器检测到。很难从纹理较少的场景（如白墙）或室内环境中常见的昏暗灯光中获得足够的特征点。当特征点对超过8对时，可以进行位置估计。
[ δ X ^ δ G p ^ i ] = ( H T H ) − 1 H T r (7) \begin{bmatrix} \delta \hat{X} \\ \delta ^G\hat{p}_i \end{bmatrix}=(H^TH)^{-1}H^Tr \tag{7} [δX^δGp^i]=(HTH)−1HTr(7)

当特征点个数足够， r a n k ( H ) ≥ 8 rank(H)\geq8 rank(H)≥8时，公式（7）足以获得最优解。当特征点数量不足时，约束条件不足，估计误差（ δ X ^ \delta\hat{X} δX^和 δ p ^ i \delta \hat{p}_i δp^i）变大。这将导致定位误差的增加：
X ^ = X + δ X ^ , G p ^ i = G p i + δ G p ^ i (8) \hat{X}=X+\delta \hat{X}, \ \ \ ^G\hat{p}_i=^Gp_i+\delta ^G\hat{p}_i \tag{8} X^=X+δX^, Gp^i=Gpi+δGp^i(8)

3.1.2 光照导致特征跟踪失败

室内环境中光照经常发生变化，我们采用Lambertian模型作为照明模型。
I ( x , y ) = ρ ( x , y ) ⋅ h ( x , y ) T ⋅ S (9) I(x,y)=\rho (x,y)\cdot h(x,y)^T\cdot S \tag{9} I(x,y)=ρ(x,y)⋅h(x,y)T⋅S(9)
其中 I ( x , y ) I(x,y) I(x,y)是图像灰度值； ρ ( x , y ) \rho(x,y) ρ(x,y)是物体反射率； h ( x , y ) h(x,y) h(x,y)是曲面的法向量； S S S是光照强度。我们发现，在光照变化时，特征跟踪容易丢失，导致定位不准确。光流法是基于灰度不变的假设。将照明公式替换为：
∂ I ∂ x d x d t + ∂ I ∂ y d y d t = − ∂ I ∂ t → [ I x I y ] [ μ v ] = − ρ ⋅ [ ∂ h T ∂ t ⋅ S + ∂ S ∂ t ⋅ h T ] (10) \frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}=-\frac{\partial I}{\partial t} \rightarrow \ [I_x \ I_y]\begin{bmatrix} \mu \\ v \end{bmatrix}=-\rho\cdot[\frac{\partial h^T}{\partial t}\cdot S+\frac{\partial S}{\partial t}\cdot h^T] \tag{10} ∂x∂Idtdx+∂y∂Idtdy=−∂t∂I→ [Ix Iy][μv]=−ρ⋅[∂t∂hT⋅S+∂t∂S⋅hT](10)
其中 I x I_x Ix和 I y I_y Iy是特征点沿 x x x和 y y y方向上的梯度值； μ \mu μ和 v v v是沿 x x x和 y y y方向上的运动速度，代表特征点。如公式（11）所示，当光照强度变化和 ∂ S ∂ t \frac{\partial S}{\partial t} ∂t∂S变大时，特征的残差 δ r i \delta r_i δri将会变大。
δ r i = 1 C Z i [ C X i − ( C X i + μ t ) C Y i − ( C Y i + v t ) ] = t C Z i [ I x I y ] ⋅ [ ρ ∂ S ∂ t ⋅ h T ] (11) \delta r_i=\frac{1}{^CZ_i}\begin{bmatrix} ^CX_i-(^CX_i+\mu t) \\ ^CY_i-(^CY_i+vt) \end{bmatrix}=\frac{t}{^CZ_i}\begin{bmatrix} I_x \\ I_y \end{bmatrix} \cdot \begin{bmatrix} \rho \frac{\partial S}{\partial t}\cdot h^T\end{bmatrix} \tag{11} δri=CZi1[CXi−(CXi+μt)CYi−(CYi+vt)]=CZit[IxIy]⋅[ρ∂t∂S⋅hT](11)

3.1.3 特征分布不均匀

从图像的观测方程中可以看出，噪声的存在导致了图像中特征点的位置误差。在计算重投影误差时，特征点的位置误差会影响到相机的状态估计。为了更好的表示图像特征点与相机姿态之间的几何关系，我们使用了一个简单的二维例子来描述几何关系。如图2所示， P 1 P_1 P1和 P 2 P_2 P2表示两个图像特征点。在没有噪声影响的情况下，相机位姿可以通过以两个特征点为中心的两个圆的交点和两个投影距离来确定位置。但测量结果不理想，噪声的不确定度为 ± ε \pm \varepsilon ±ε。

图2 特征点几何关系的误差图。位置估计的精度取决于观测的位姿估计误差和几何角度，阴影部分表示位置估计的不确定性。

我们描述了基于相机状态特征点的雅可比矩阵 H i H_i Hi的位置估计的质量。假设测量误差为零均值，则定位误差也为零均值。得到位置计算中误差的期望值 E ( Δ X ) E(\Delta X) E(ΔX)和协方差 C o v [ Δ X ] Cov[\Delta X] Cov[ΔX]。
E ( Δ X ) = E ( X ^ − X ) = 0 , C o v [ Δ X ] = σ 2 ( H i T H i ) − 1 b 2 − 4 a c (12) E(\Delta X)=E(\hat{X}-X)=0,\ \ Cov[\Delta X]=\sigma^2(H_i^TH_i)^{-1}\sqrt{b^2-4ac} \tag{12} E(ΔX)=E(X^−X)=0, Cov[ΔX]=σ2(HiTHi)−1b2−4ac (12)

位置误差在 x x x、 y y y和 z z z方向上的变化量用 σ x 2 \sigma^2_x σx2、 σ y 2 \sigma_y^2 σy2和 σ z 2 \sigma_z^2 σz2来表示。 H i i H_{ii} Hii用于表示对角矩阵 ( H i T H i ) − 1 (H_i^TH_i)^{-1} (HiTHi)−1中对角上的第 i i i个元素。那么，可以表示如下：
S D ( Δ X ) = σ x 2 + σ y 2 + σ z 2 = H 11 + H 22 + H 33 (13) SD(\Delta X)=\sqrt{\sigma_x^2+\sigma_y^2+\sigma_z^2}=\sqrt{H_{11}+H_{22}+H_{33}} \tag{13} SD(ΔX)=σx2+σy2+σz2 =H11+H22+H33 (13)

3.1.4 运动特征

定位过程中，所有移动物体（如行人或车辆）都会影响定位结果。当特征点集中在运动物体上时，特征点的相对运动导致计算出的相机运动较大。这种情况可以用特征点的世界坐标表示为有额外的运动位移， Δ G p f j \Delta ^Gp_{f_j} ΔGpfj，它影响相机的观测，如公式（14）所示：
[ C i X j + Δ x C i Y j + Δ y C i Z j + Δ z ] = C ( G C i q ˉ ) ( G p f i + Δ G p f i − G p C i ) (14) \begin{bmatrix} ^{C_i}X_j+\Delta x \\ ^{C_i}Y_j+\Delta y\\ ^{C_i}Z_j+\Delta z \end{bmatrix}=C(^{C_i}_G\bar{q})(^Gp_{f_i}+\Delta ^Gp_{f_i}-^Gp_{C_i}) \tag{14} ⎣⎡CiXj+ΔxCiYj+ΔyCiZj+Δz⎦⎤=C(GCiqˉ)(Gpfi+ΔGpfi−GpCi)(14)
让我们分析特征的偏移量 ( Δ x , Δ y , Δ z ) (\Delta x,\ \Delta y,\ \Delta z) (Δx, Δy, Δz)产生的残差 r i r_i ri。
r i = Δ z C Z i ( C Z i + Δ Z ) [ C X i C Y i ] − 1 C Z i + Δ z [ Δ x Δ y ] (15) r_i=\frac{\Delta z}{^CZ_i(^CZ_i+\Delta Z)}\begin{bmatrix} ^CX_i \\ ^CY_i \end{bmatrix}-\frac{1}{^CZ_i+\Delta z}\begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} \tag{15} ri=CZi(CZi+ΔZ)Δz[CXiCYi]−CZi+Δz1[ΔxΔy](15)

3.2 PDR辅助视觉完好性监测

虽然PDR存在累积误差的问题，但短时间内的误差非常小。三轴陀螺仪可以构造IMU相对于世界坐标系的旋转矩阵。旋转三轴加速度后，进行积分运算即可得到 ( x , y , z ) (x, y, z) (x,y,z)的相对位置信息，如公式（16）所示。
s ( t + Δ t ) = s ( t ) + v ( t ) Δ t + 1 2 a ′ Δ t 2 (16) s(t+\Delta t)=s(t)+v(t)\Delta t+\frac{1}{2}a'\Delta t^2 \tag{16} s(t+Δt)=s(t)+v(t)Δt+21a′Δt2(16)

现在假设有两个采样点 O 1 O_1 O1和 O 2 O_2 O2，采样间隔是 Δ t \Delta t Δt；时间 O 1 O_1 O1的速度是 ν ν ν；位移是 s s s；状态协方差矩阵是 P 1 = [ P 11 P 12 P 21 P 22 ] P_1=\begin{bmatrix}P_{11} & P_{12} \\ P_{21} & P_{22} \end{bmatrix} P1=[P11P21P12P22]。由于运动过程中产生的冲击，加速度计的观测结果可能导致不准确的偏差。我们首先分析了一维运动， O 1 O_1 O1时刻的加速度为 f m e a = f t r u e + δ f f_{mea} = f_{true} + \delta f fmea=ftrue+δf。从 O 1 O_1 O1的状态量可以得到 O 2 O_2 O2的状态量的估计值。由 δ f \delta f δf引起的偏差为：
[ δ s δ v ] = [ δ f ⋅ Δ t ( 1 2 Δ t − p 12 + Δ t ⋅ p 22 p 22 + R ) δ f ⋅ Δ t ( 1 − p 22 p 22 + R ) ] (17) \begin{bmatrix} \delta s\\ \delta v \end{bmatrix} = \begin{bmatrix} \delta f \cdot \Delta t(\frac{1}{2}\Delta t-\frac{p_{12}+\Delta t \cdot p_{22}}{p_{22}+R}) \\ \delta f \cdot \Delta t(1-\frac{p_{22}}{p_{22}+R}) \end{bmatrix} \tag{17} [δsδv]=[δf⋅Δt(21Δt−p22+Rp12+Δt⋅p22)δf⋅Δt(1−p22+Rp22)](17)
其中 R R R为观测噪声的协方差。

在本文中，我们提出了一种自主PDR辅助的视觉完好性监测方法，以提高定位精度。根据PDR的短期可靠性特点，所提出的PDR辅助视觉完好性监测系统在室内环境中自动切换VIO(或VO)和PDR之间的状态，以提供更准确的位置。具体切换情况如图3所示。当VIO系统的定位结果超出PDR的误差范围时，使用PDR结果代替VIO结果。其中 X ^ i \hat{X}_i X^i表示在第 i i i个时刻由PDR获得的相机位姿； X ^ i + 1 \hat{X}_{i+1} X^i+1表示第 i + 1 i+1 i+1个时刻由PDR获得的相机位姿； X ^ i + 1 ′ \hat{X}'_{i+1} X^i+1′表示在第 i + 1 i+1 i+1时刻由VIO获得的位姿。 ε \varepsilon ε为PDR的误差范围。

图3 行人航迹推算辅助视觉完好性测试。\varepsilon为PDR定位的误差范围。当VIO系统的定位结果超出PDR的误差范围时，使用PDR结果代替VIO结果。

假设偏差服从高斯分布 e ∼ N ( 0 , Σ ) e\sim N(0,\Sigma) e∼N(0,Σ)。现在， e e e是三维向量。为了便于计算，将计算向量的内积以变换为标量。
r = e T ∑ e = ( ∑ − 1 2 e ) T ( ∑ − 1 2 e ) , w h e r e ( ∑ − 1 2 e ) ∼ N ( 0 , I ) (18) r=e^T\sum e=\Big(\sum^{-\frac{1}{2}}e\Big)^T\Big(\sum^{-\frac{1}{2}}e\Big),\ \ where\ \Big(\sum^{-\frac{1}{2}}e\Big) \sim N(0,\ I) \tag{18} r=eT∑e=(∑−21e)T(∑−21e), where (∑−21e)∼N(0, I)(18)

它是多维的标准正态分布。它可以被认为是服从三自由度的卡方分布，更具体的说，标准正态分布的两个独立随机变量的平方和。概率分布（累积分布函数）是 a = F ( x ) a=F(x) a=F(x)，给定 a a a，我们可以确定一个区间 [ 0 , F − 1 ( a ) ] [0,F^{-1}(a)] [0,F−1(a)]。 F − 1 ( a ) F^{-1}(a) F−1(a)是我们正在寻找的阈值，以确定视觉完好性。以上是理论分析部分，提出了一种PDR辅助视觉完好性监测方法的阈值。

我们的室内定位系统是基于多状态约束卡尔曼滤波(MSCKF)定位算法和PDR的。为了给室内定位系统提供一个准确的定位结果，自动切换MSCKF和PDR之间的状态是非常重要的。视觉观测在MSCKF上引起的问题可以归因于视觉观测没有更新或更新不正确。因此，当PDR辅助视觉完好性监测检测到异常时，视觉观测的更新频率和陀螺仪偏差估计会产生较大的异常波动(如图9所示)。我们利用视觉观测的更新频率 f u p d a t e f_{update} fupdate和陀螺仪偏差的估计值 b ^ g y r \hat{b}_{gyr} b^gyr来自动切换MSCKF和PDR的状态，如公式（19）所示：
P o u t = { P P D R b ^ g y r > τ 1 & & f u p d a t e < τ 2 P M S C K F o t h e r s (19) P_{out}=\begin{cases} P_{PDR} & \hat{b}_{gyr}>\tau_1\ \ \&\& \ f_{update}<\tau_2 \\ P_{MSCKF} & others \end{cases} \tag{19} Pout={PPDRPMSCKFb^gyr>τ1 && fupdate<τ2others(19)
其中 P o u t P_{out} Pout是我们定位系统的估计位姿； P P D R P_{PDR} PPDR表示PDR估计的位姿； P M S C K F P_{MSCKF} PMSCKF代表MSCKF估计的位姿； τ 1 \tau_1 τ1和 τ 2 \tau_2 τ2是系统自动转换MSCKF和PDR状态的超参数。MSCKF需要重新测量观测值，以防止视觉观测误差对后续切换操作的影响。在系统运行过程中，更换姿态角后，MSCKF不能立即恢复正常。为了防止陀螺仪误差影响后续的位姿估计，我们将在一段时间内继续输出PDR的估计位姿。这段时间将用于MSCKF重新测量观测数据并完成重启操作。

4 实验与评估

我们使用的传感器套件如图4所示。它由一个双目相机(ZED, 30HZ，来自美国旧金山的stereolabs)和一个IMU(来自荷兰XSENS的MTi-100)组成。室内定位系统基于MSCKF定位算法和PDR。在数据收集过程中，传感器套件拿在手上，参与者在室内环境中行走。实验的第一部分评估场景对定位结果的影响。实验的第二部分对所提出的PDR辅助视觉完好性监测进行了测试和评估，该监测可以自动在MSCKF和PDR之间切换状态，以提供准确的位置。

图4 用于室内实验的装置。它包含一个双目相机(ZED, 30HZ)，分辨率为672×376。

4.1 评估环境影响

下面的实验旨在评估在上一章节中确定的四个误差原因。

4.1.1 缺少特征

如图5a所示，我们对同一组数据改变每帧特征点数量的阈值来提取特征点。从图5a可以看出，阈值越低，特征点的数量越多。对于20和60两个阈值，在第1560帧特征点的数量都为0。这是因为遇到了白墙，无法提取特征点。我们绘制了相应的定位轨迹，如图5b所示。当特征点不足时，相机对IMU的校正能力较弱，路径的锯齿性不够，轨迹在 x x x轴和 y y y轴之间也表现出明显的偏差。

(a)

(b) 图5 (a)中红线为每帧阈值为20的特征点个数，绿线为每帧阈值为60的稀疏特征点个数。(b)中红叉标记起点，点A为特征点原始状态下轨迹的终点，点B为特征点稀疏时轨迹的终点。

4.1.2 光照导致特征跟踪失败

当左右相机光照不同时，左右相机获取的图像平均灰度值不同，匹配率较低。图6a是对左右相机采集的图像进行FAST特征提取得到的特征点分布图。但是左右相机的图像匹配率不高，匹配率仅为0.55。双目匹配后的图像中不存在特征点。如果特征检测模块没有特征点数据输出，则视觉惯性里程计无法进行姿态更新，导致轨迹累积偏移，可能出现如图6b所示的严重误差。

图6 (a)中顶部图像的平均灰度值为97.0845，底部图像的平均灰度值为183.946；顶部图像提取的特征个数为946，底部图像提取的特征数为1543。(b)中红色箭头所指的一条长线，表示左右相机光线不同，没有特征点，轨迹出现严重偏差。

4.1.3 特征分布不均匀

我们以特征点的分布为变量，将轨迹与原始特征分布进行对比，如图7a所示。而当特征点仅分布在红色区域时，特征点的运动轨迹指向图像的右侧。如图7b所示，分布不均匀的特征轨迹有明显的向左偏移。

图7 (a)中红色区域控制特征点分布。黄色圆圈表示所有提取的特征点，蓝色线段表示特征点的跟踪轨迹。(b)中红叉标记为起点，C点为特征点分布正态的轨迹终点，D点为特征点分布不均匀的轨迹终点。

4.1.4 移动特征点

行人在相机前行走，对比跟踪绘制轨迹如图8b所示。在圆圈中可以明显看出，绿色轨道因为行人的影响向左偏移了。我们分析了这一刻的细节。从图8a可以看出，当行人移动时，提取的特征点有一半以上集中在行人身上。因此，行人相对于相机的运动会导致定位结果的偏差。当行人向相机右侧移动时，行人身上的特征点累计相应的移动，导致估计位置产生向左偏移，如图8b中的黑色椭圆区域所示。

图8 (a)中空心圆代表移动特征点；实心圆代表固定的特征点。(b)中红叉为起点，E点为原轨迹终点。点F是受运动特征点影响的轨迹终点。

4.2 建议的PDR辅助视觉完好性监测的评估

本实验在一个长100米，高20米的大型办公大楼中进行，历时20分钟，跨越三层。视觉观测异常会导致视觉观测频率的更新，陀螺仪偏差估计会产生较大的异常波动。根据视觉观测更新频率和陀螺仪偏差估计，我们将路径分为三部分来引入基于视觉观测的PDR辅助效果，如图9 (A, B, C)所示。以下是我们的定位系统与提出的PDR辅助视觉完好性监测的实验结果的内容测试和评估。

图9 (a)视觉观察的更新频率曲线；(b)陀螺仪偏差估计。

4.2.1 A段

A段步行距离约340米。具体的动作轨迹是在走廊里直走，然后下到下一层，在走廊里走三次。由图10a可以看出，MSCKF的路径方向偏差较大。为了说明轨迹的变化，黄色区域的路径为放大路径，红色区域的路径为PDR辅助的路径。这里的场景如图10b所示，这是楼梯区域。当特征观测较少，出现一个转折点时，MSCFK的视觉更新频率会变低，这意味着在滤波器更新过程中，被丢弃的特征点的频率不够。这些情况会导致MSCKF方向的偏差。PDR辅助切换后，可有效改善系统，在视觉观测不足时提供可靠的路径输出。

图10 （a）MSCKF和MSCKF+PDR的轨迹比较。绿色是MSCKF，蓝色是MSCKF+PDR。黄色区域是楼梯区域路径的放大视图，红色部分是PDR辅助的输出路径。（b）楼梯场景图片，黄色点为提取的特征点。

4.2.2 B段

B段步行距离约214米，上楼至屋顶区域，涉及两圈屋顶空间。如图11a所示，MSCKF的路径有点不规则，即黄色区域。从图11b中我们可以看出，在向上移动的过程中总是存在着移动的特征点。楼梯上的特征跟踪多次失败，导致MSCKF的轨迹方向总是偏移。在这种情况下，IMU预测的相对位移与视觉观察的相对位移有很大的不同。针对这种情况，PDR的切换频率相对较高，可以更准确地保证定位方向。但是由于PDR的步长计算错误，导致了在上楼的过程中整体轨迹变长。

图11 （a）MSCKF和MSCKF+PDR的轨迹比较。（b）行人行走，黄色点为提取的特征点，绿色为特征点的跟踪路径。

4.2.3 C段

C段的步行距离约为166米，在一个空房间内绕行了两次，大部分是转弯。由于大量白墙场景的存在，使得MSCKF的视觉观察在数量和质量上都相对较差。滤波器估计了一个错误的陀螺仪的偏差，导致整个轨迹方向的偏差。利用PDR的短期可靠性，可以减少定位的方向偏差。由图12a可以看出，在第一次转弯时，没有PDR辅助，导致MSCKF方向发生偏移。但很明显，PDR在第二次转弯时进行了切换，有效地减小了系统的方向误差。

图12 （a）MSCKF和MSCKF+PDR的轨迹比较。（b）楼梯场景的图片和黄色的点是提取的特征点。

为了更清晰地描述定位结果，我们标记了32个地标，并记录了这些地标的位置信息。然后根据实验数据计算了MSCKF、PDR和我们的定位系统的定位误差。如图13所示，从定位误差折线图和累积分布函数(CDF)图可以看出，我们系统的定位精度得到了显著提高。MSCKF的定位误差主要是由于视觉观测质量较差造成的，而PDR的定位结果是由于行人转弯时的步长检测误差造成的累积误差造成的。

图13 （a）MSCKF、PDR与本系统的定位误差比较。（b）MSCK、PDR和我们的系统的位置结果的CDF。

在这一部分，我们进行了实时定位实验，如IPIN [37] (The International Conference on Indoor positioning and Indoor Navigation)比赛，并根据IPIN比赛的性能评价标准对定位情况进行了定量分析。如图14所示，我们在具有显著路径长度和持续时间的大型、具有挑战性的多层环境中测试我们的系统，以评估其性能。步行路线的总长度为1400米，步行区域跨越四层。然后，我们进行了数值分析，以说明我们的系统的准确性。

图14 这张地图有四层。绿点代表事先校准过的真正的地标点。蓝色的路线是我们系统的定位结果。

为了更好的展示实验结果，每层楼的定位轨迹如图15所示。本次实验定位结果的平均误差约为2.5838 m；我们还绘制了定位误差的CDF，如图16所示。最终的评分指标是IPIN中定位误差的第三个四分位数（即75%的概率），这使得精度结果不太容易受到异常值的影响，更符合商业系统所需的精度。所以我们系统的最终成绩是2.13米。

图15 显示各楼层的定位结果。蓝色轨迹是我们系统的定位结果，绿色点是真正的路标点。红线表示实际路标点与系统定位点之间的误差距离。

图16 系统定位结果的CDF。

5 总结和讨论

为了解决视觉定位系统易受环境影响的问题，我们分析了在特定的室内环境中，纹理较少、动态障碍物或光线昏暗时，视觉定位系统定位误差较大时的视觉观测误差来源。我们将视觉测量的误差来源分为四种情况，并进行了详细的分析和解释。实验的第一部分评估场景对定位结果的影响，直观展示特征观测的效果。为了解决这一问题，我们提出了一种基于行人航迹推算系统的视觉观测自主完好性监测。通过对PDR的误差分析，发现PDR在短时间内的误差很小且有界。根据PDR短期可靠性的特点，提出的PDR辅助视觉完好性监测方法自动切换MSCKF和PDR之间的状态，以在室内环境中提供更准确的位置。实验的第二部分对所提出的PDR辅助视觉完好性监测进行了测试和评估。最后，我们证明了我们的定位系统能够有效地提供更可靠和准确的定位结果。未来的研究应该更仔细地考虑视觉观测的潜在影响。此外，为了提高PDR系统的定位精度，还需要进一步研究提高PDR步长精度。

参考文献

略