摘要
视觉惯性里程计(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}
GCk??qˉ?和位置
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?=[XIMUk?T???GCk??qˉ?T??GpCk?T?]T,??where?XIMU?=[GIMU?qˉ?T??bgT???GvT??baT???GpT]T(1) 其中
G
I
M
U
q
ˉ
T
_G^{IMU}\bar{q}^T
GIMU?qˉ?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})
(GCk??qˉ?,?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?=CZi?1?[CXi?CYi??]+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?=???CXi?CYi?CZi?????=C(GCi??qˉ?)(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(GCi??qˉ?)是相机系和全局系之间的旋转矩阵。一旦得到特征位置的估计,我们就可以计算测量残差:
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?≈HC?X~+HiG?p~?i?+ni?,?where{HC?=?C1?pi??zi????XC1???C1?pi??+?C2?pi??zi????XC1???C2?pi??Hi?=?C1?pi??zi????Gpi??C1?pi??+?C2?pi??zi????Gpi??C2?pi???(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=??????HC?HC??HC??H1?H2??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+2Ix?Iy??Δ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?I?dtdx?+?y?I?dtdy?=??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?=CZi?1?[CXi??(CXi?+μt)CYi??(CYi?+vt)?]=CZi?t?[Ix?Iy??]?[ρ?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(HiT?Hi?)?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}
(HiT?Hi?)?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}
???Ci?Xj?+ΔxCi?Yj?+ΔyCi?Zj?+Δz????=C(GCi??qˉ?)(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?[CXi?CYi??]?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+21?a′Δ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?=[P11?P21??P12?P22??]。由于运动过程中产生的冲击,加速度计的观测结果可能导致不准确的偏差。我们首先分析了一维运动,
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=(∑?21??e)T(∑?21??e),??where?(∑?21??e)~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的。
|