论文阅读《A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation》1_vision-aided inertial navigation with line feature-程序员宅基地

技术标签: 论文  VIO  

摘要

本文提出了一种基于扩展卡尔曼滤波(EKF)的实时视觉辅助惯性导航算法。这项工作的主要贡献是推导出一个测量模型,该模型能够表达当从多个相机位姿观察到一个静态特征时产生的几何约束。该测量模型不需要将三维特征位置包含在EKF的状态向量中,并且在线性化误差范围内是最优的。我们提出的视觉辅助惯性导航算法的计算复杂度在特征数量上仅为线性,并且能够在大规模真实环境中进行高精度的位姿估计。该算法的性能在广泛的实验结果中得到了证明,包括在城市区域内定位的相机/IMU系统。

1 介绍

在过去的几年中,视觉辅助惯性导航的研究受到了学术界的广泛关注。基于MEMS的惯性传感器制造的最新进展使得制造小型、廉价和非常精确的惯性测量单元(IMU)成为可能,适用于移动机器人和无人机等小规模系统的位姿估计。这些系统通常在GPS信号不可靠的城市环境(“城市峡谷”),以及室内、空间和其它一些无法进行全球定位测量的环境中运行。在无法依赖GPS测量的情况下,相机的低成本、重量轻和功耗小使其成为辅助惯性导航的理想选择。

视觉感知的一个重要优势是图像是高维测量,具有丰富的信息内容。特征提取方法通常可以检测和跟踪图像中的数百个特征,如果使用得当,可以得到良好的定位结果。然而,大量的数据也对估计算法的设计提出了重大挑战。在要求实时定位性能时,需要在算法的计算复杂度和估计精度之间进行权衡。

在本文中,我们提出了一种算法,能够优化地利用由视觉特征的多个测量提供的定位信息。我们的方法的动机是观察到,当一个静态特征从几个相机位姿观测到,它是可能定义涉及所有这些位姿的几何约束。我们工作的主要贡献是一个测量模型,该模型表达了这些约束,而不包括滤波器状态向量中的3D特征位置,导致计算复杂度仅与特征数量成线性关系。在对下一节的相关工作进行了简要讨论之后,第三节将介绍提出的估计器的细节。在第四节中,我们描述了在不受控制的城市环境中进行的大规模实验的结果,这表明所提出的估计器能够实现准确和实时的位姿估计。最后,在第五节得出了本工作的结论。

2 相关工作

一类融合惯性测量与视觉特征观测的算法遵循同时定位和建图(SLAM)范式。在这些方法中,当前IMU位姿以及所有视觉路标点的三维位置被联合估计。这些方法与基于SLAM的仅依靠相机定位的方法具有相同的基本原理,但不同的是,IMU测量用于状态传播,而不是统计运动模型。基于SLAM的算法的基本优势在于,它们可以解释相机的位姿和观测到的特征的3D位置之间存在的相关性。另一方面,SLAM的主要局限性是计算复杂度高;正确处理这些相关性的计算成本很高,因此在具有数千个特征的环境中执行基于视觉的SLAM仍然是一个具有挑战性的问题。

存在一些算法,与SLAM相反,仅估计相机的位姿(即不共同估计特征位置),以实现实时操作。这些方法中计算效率最高的是利用特征测量来推导成对图像之间的约束。例如,在[7]中,一种基于图像的运动估计算法应用于连续的图像对,以获得随后与惯性测量融合的位移估计。类似地,在[8]和[9]中,当前图像和以前图像之间的约束使用极线几何定义,并在扩展卡尔曼滤波器(EKF)中与IMU测量相结合。在[10]和[11]中,采用极线几何与统计运动模型相结合,而在[12]中则融合了极线约束与飞机动力学模型。使用特征测量对图像之间施加约束在哲学上与本文提出的方法相似。然而,一个根本的区别是,我们的算法可以表达多个相机位姿之间的约束,因此,在同一特征在两幅以上的图像中可见的情况下,可以获得更高的估计精度。

在保持由多个相机位姿组成的状态向量的算法中也采用了成对约束。在[13]中,实现了一个增广状态卡尔曼滤波,其中机器人位姿的滑动窗口保持在滤波状态。另一方面,在[14]中,所有相机位姿都是同时估计的。在这两种算法中,成对的相对位姿测量是从图像中导出的,并用于状态更新。该方法的缺点是,当一个特征在多幅图像中出现时,多个位姿之间的附加约束被丢弃,从而导致信息丢失。此外,当对相同的图像测量值进行处理以计算多个位移估计时,它们在统计上并不是独立的,如[15]所示。

[16]给出了一种算法,类似于本文提出的方法,直接使用路标测量值来施加多个相机位姿之间的约束。这是一个视觉里程计算法,它临时初始化路标,使用它们对连续相机位姿的窗口施加约束,然后丢弃它们。然而,这种方法不能用于协同惯性测量。此外,路标点估计和相机轨迹之间的相关性没有得到适当的解释,因此,该算法没有提供任何状态估计的协方差度量。

在可变状态维数滤波器(VSDF)中也保持一个相机位姿窗口。VSDF是一种混合批量/递归方法,(i)使用延迟线性化来提高对线性化准确性的鲁棒性,(ii)利用信息矩阵的稀疏性,这在没有使用动态运动模型时自然会出现。然而,在动态运动模型可用的情况下(如视觉辅助惯性导航),VSDF的计算复杂度最多为特征数量的二次型。

与VSDF相比,我们在本文中提出的多状态约束滤波器能够利用延迟线性化的优点,而复杂度在特征数量上仅为线性。通过直接表示多相机之间的几何约束,避免了成对位移估计带来的计算负担和信息丢失。此外,与SLAM类型的方法相比,它不需要在滤波器状态向量中包含3D特征位置,但仍然获得了最优的位姿估计。由于这些特性,所述算法是非常有效的,并如第四节所示,能够实时高精度视觉辅助惯性导航。

3 估计器描述

提出的基于EKF的估计器的目标是跟踪IMU固定的坐标系 { I } \{I\} { I}相对于全局参考坐标系 { G } \{G\} { G}的3D位姿。为了简化处理地球自转对IMU测量结果的影响(参见公式(7) ~(8)),全局坐标系选择地心地固坐标系(ECEF)。算法的概述在算法1中给出。IMU测量值在可用时立即进行处理,以传播EKF状态和协方差(参见第3-B节)。另一方面,每次记录图像时,当前相机位姿估计被附加到状态向量(参见第3-C节)。状态增广对于处理特征测量是必要的,因为在EKF更新过程中,每个被跟踪特征的测量值被用于在所有相机位姿之间施加约束。因此,在任何时刻,EKF状态向量包括(i)不断变化的IMU状态, X I M U X_{IMU} XIMU,和(ii)一个历史到 N m a x N_{max} Nmax过去的相机的位姿。下面,我们将详细描述算法的各个组成部分。

在这里插入图片描述

A EKF状态向量的结构

IMU状态的演变由向量描述:
X I M U = [ G I q ˉ T   b g T   G v I T   b a T   G p I T ] T (1) X_{IMU}=[^I_G\bar{q}^T \ b_g^T \ ^Gv_I^T\ b_a^T \ ^Gp_I^T]^T \tag{1} XIMU=[GIqˉT bgT GvIT baT GpIT]T(1)
其中 G I q ˉ T ^I_G\bar{q}^T GIqˉT是单位四元数,用来描述从坐标系 { G } \{G\} { G}到坐标系 { I } \{I\} { I}的旋转; G p I ^Gp_I GpI G v I ^Gv_I GvI是IMU相对于坐标系 { G } \{G\} { G}的位置和速度; b g b_g bg b a b_a ba 3 × 1 3\times1 3×1向量,它们分别用来描述陀螺仪和加速度计测量的偏差。将IMU偏差建模为随机游走过程,分别由高斯白噪声向量 n w g n_{wg} nwg n w a n_{wa} nwa驱动。根据公式(1),IMU的误差状态定义如下:
X ~ I M U = [ δ θ I T   b ~ g T   G v ~ I T   b ~ a T   G p ~ I T ] T (2) \tilde{X}_{IMU}=[\delta \theta_I^T \ \tilde{b}_g^T \ ^G\tilde{v}_I^T \ \tilde{b}_a^T \ ^G\tilde{p}_I^T]^T \tag{2} X~IMU=[δθIT b~gT Gv~IT b~aT Gp~IT]T(2)
对于位置、速度和偏差,使用标准加性误差定义(即变量 x x x的估计 x ^ \hat{x} x^的误差定义为 x ~ = x − x ^ \tilde{x}=x-\hat{x} x~=xx^)。然而,对于四元数则采用了不同的定义。具体而言,如果四元数 q ˉ \bar{q} qˉ的估计值为 q ˉ ^ \hat{\bar{q}} qˉ^,然后用误差四元数 δ q ˉ \delta \bar{q} δqˉ描述姿态误差,这是由这个关系定义的 q ˉ = δ q ˉ ⨂ q ˉ ^ \bar{q}=\delta \bar{q} \bigotimes \hat{\bar{q}} qˉ=δqˉqˉ^。在这个表达式中,符号 ⨂ \bigotimes 表示四元数的乘积。误差四元数为,
δ q ˉ ≃ [ 1 2 δ θ T   1 ] T (3) \delta \bar{q} \simeq \bigg[\frac{1}{2} \delta \theta^T \ 1 \bigg]^T \tag{3} δqˉ[21δθT 1]T(3)
直观上,四元数 δ q ˉ \delta \bar{q} δqˉ描述了导致真实和估计姿态重合的小旋转。由于姿态对应3个自由度,使用 δ θ \delta \theta δθ来描述姿态误差是一种最小表示。

假设EKF在时间步长 k k k时的状态向量中包含 N N N个相机位姿,该向量的形式为:
X ^ k = [ X ^ I M U k T       G C 1 q ˉ ^ T      G p ^ C 1 T     ⋯      G C N q ˉ ^ T      G p ^ C N T ] T (4) \hat{X}_k=[\hat{X}_{IMU_k}^T\ \ \ \ ^{C_1}_G\hat{\bar{q}}^T\ \ \ ^G\hat{p}_{C_1}^T \ \ \ \cdots \ \ \ ^{C_N}_G\hat{\bar{q}}^T \ \ \ ^G\hat{p}_{C_N}^T ]^T \tag{4} X^k=[X^IMUkT    GC1qˉ^T   Gp^C1T      GCNqˉ^T   Gp^CNT]T(4)
其中 G C i q ˉ ^ ^{C_i}_G\hat{\bar{q}} GCiqˉ^ G p ^ C i , i = 1 ⋯ N ^G\hat{p}_{C_i},i=1\cdots N Gp^Ci,i=1N分别是相机姿态和位置的估计。EKF误差状态向量相应地定义为如下:
X ~ k = [ X ~ I M U k T     δ θ C 1 T      G p ~ C 1 T     ⋯     δ θ C N T      G p ~ C N T ] T (5) \tilde{X}_k=[\tilde{X}_{IMU_k}^T \ \ \ \delta \theta^T_{C_1}\ \ \ ^G\tilde{p}_{C_1}^T \ \ \ \cdots \ \ \ \delta \theta_{C_N}^T \ \ \ ^G\tilde{p}_{C_N}^T]^T \tag{5} X~k=[X~IMUkT   δθC1T   Gp~C1T      δθCNT   Gp~CNT]T(5)

B 传播

对连续时间IMU系统模型进行离散化,得到滤波器传播方程,具体如下:

1)连续时间系统建模:IMU状态的时间演化由[20]给出:
G I q ˉ ˙ ( t ) = 1 2 Ω ( ω ( t ) ) G I q ˉ ( t ) ,     b ˙ g ( t ) = n w g ( t ) G v ˙ I ( t ) = G a ( t ) ,     b ˙ a ( t ) = n w a ( t ) ,      G p ˙ I ( t ) = G v I ( t ) (6) \begin{matrix} ^I_G\dot{\bar{q}}(t)=\frac{1}{2}\Omega(\omega(t))^I_G\bar{q}(t), \ \ \ \dot{b}_g(t)=n_{wg}(t) \\ \\ ^G\dot{v}_I(t)=^Ga(t), \ \ \ \dot{b}_a(t)=n_{wa}(t), \ \ \ ^G\dot{p}_I(t)=^Gv_I(t) \end{matrix} \tag{6} GIqˉ˙(t)=21Ω(ω(t))GIqˉ(t),   b˙g(t)=nwg(t)Gv˙I(t)=Ga(t),   b˙a(t)=nwa(t),   Gp˙I(t)=GvI(t)(6)
在这些表达式中, G a ^Ga Ga是在全局坐标系中的机体加速度, ω = [ ω x   ω y   ω z ] T \omega=[\omega_x \ \omega_y \ \omega_z]^T ω=[ωx ωy ωz]T是在IMU坐标系下的旋转速度,
Ω ( ω ) = [ − ⌊ ω × ⌋ ω − ω T 0 ] ,     ⌊ ω × ⌋ = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \Omega(\omega)=\begin{bmatrix} -\lfloor \omega \times \rfloor & \omega \\ -\omega^T & 0 \end{bmatrix}, \ \ \ \lfloor \omega \times \rfloor = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} Ω(ω)=[ω×ωTω0],   ω×=0ωzωyωz0ωxωyωx0
ω m \omega_m ωm a m a_m am分别表示陀螺仪和加速度计测量,由[20]给出:
ω m = ω + C ( G I q ˉ ) ω G + b g + n g (7) \omega_m=\omega+C(^I_G\bar{q})\omega_G+b_g+n_g \tag{7} ωm=ω+C(GIqˉ)ωG+bg+ng(7)
a m = C ( G I q ˉ ) ( G a − G g + 2 ⌊ ω G × ⌋ G v I + ⌊ ω G × ⌋ 2 ⋅ G p I ) + b a + n a (8) a_m=C(^I_G\bar{q})(^Ga-{^Gg}+2\lfloor \omega_G \times \rfloor^Gv_I + {\lfloor \omega_G \times \rfloor}^2\cdot {^Gp_I}) + b_a + n_a \tag{8} am=C(GIqˉ)(GaGg+2ωG×GvI+ωG×2GpI)+ba+na(8)
其中 C ( ⋅ ) C(\cdot) C()表示旋转矩阵, n g n_g ng n a n_a na是零均值的高斯白噪声,用来建模测量噪声。值得注意的是,IMU的测量纳入了地球自转的影响, ω G \omega_G ωG。此外,加速度计测量包括重力加速度, G g ^Gg Gg,在局部坐标系中表示。

将期望算子应用于状态传播方程(公式(6)),我们得到了IMU状态估计的传播方程:
G I q ˉ ^ ˙ = 1 2 Ω ( ω ^ ) G I q ˉ ^ ,     b ^ ˙ g = 0 3 × 1 , G v ^ ˙ I = C q ^ T a ^ − 2 ⌊ ω G × ⌋ G v ^ I − ⌊ ω G × ⌋ 2 ⋅ G p ^ I + G g b ^ ˙ a = 0 3 × 1 ,      G p ^ ˙ I = G v ^ I (9) \begin{matrix} ^I_G\dot{\hat{\bar{q}}}=\frac{1}{2}\Omega(\hat{\omega})^I_G\hat{\bar{q}}, \ \ \ \dot{\hat{b}}_g=\pmb{0}_{3\times1}, \\ \\ ^G\dot{\hat{v}}_I=C^T_{\hat{q}}\hat{a}-2\lfloor\omega_G \times\rfloor ^G\hat{v}_I - \lfloor \omega_G \times \rfloor^2 \cdot {^G\hat{p}_I} + ^Gg \\ \\ \dot{\hat{b}}_a=\pmb{0}_{3\times1}, \ \ \ ^G\dot{\hat{p}}_I=^G\hat{v}_I \end{matrix} \tag{9} GIqˉ^˙=21Ω(ω^)GIqˉ^,   b^˙g=0003×1,Gv^˙I=Cq^Ta^2ωG×Gv^IωG×2Gp^I+Ggb^˙a=0003×1,   Gp^˙I=Gv^I(9)
其中为了简洁,我们把做如下记号规定: C q ^ = C ( G I q ˉ ^ ) C_{\hat{q}}=C(^I_G\hat{\bar{q}}) Cq^=C(GIqˉ^) a ^ = a m − b ^ a \hat{a}=a_m-\hat{b}_a a^=amb^a ω ^ = ω m − b ^ g − C q ^ ω G \hat{\omega}=\omega_m-\hat{b}_g-C_{\hat{q}}\omega_G ω^=ωmb^gCq^ωG。IMU误差状态的线性化连续时间模型为:
X ~ ˙ I M U = F X ~ I M U + G n I M U (10) \dot{\tilde{X}}_{IMU}=F\tilde{X}_{IMU}+Gn_{IMU} \tag{10} X~˙IMU=FX~IMU+GnIMU(10)
其中 n I M U = [ n g T     n w g T     n a T     n w a T ] T n_{IMU}=[n_g^T \ \ \ n_{wg}^T \ \ \ n_a^T \ \ \ n_{wa}^T]^T nIMU=[ngT   nwgT   naT   nwaT]T是系统噪声。 n I M U n_{IMU} nIMU的协方差矩阵为 Q I M U Q_{IMU} QIMU,它取决于IMU的噪声特性和在传感器标定时的离线计算。最后,公式(10)中的 F F F G G G矩阵,由下式给出:
F = [ − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 − 2 ⌊ ω G × ⌋ − C q ^ T − ⌊ ω G × ⌋ 2 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 ] F=\begin{bmatrix} -\lfloor \hat{\omega}\times \rfloor & -\pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3}\\ -C_{\hat{q}}^T\lfloor \hat{a}\times \rfloor & \pmb{0}_{3\times 3} & -2\lfloor \omega_G \times \rfloor & -C_{\hat{q}}^T & -\lfloor \omega_G \times \rfloor ^2 \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3}\\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \end{bmatrix} F=ω^×0003×3Cq^Ta^×0003×30003×3III30003×30003×30003×30003×30003×30003×32ωG×0003×3III30003×30003×3Cq^T0003×30003×30003×30003×3ωG×20003×30003×3
其中 I 3 \pmb{I}_3 III3 3 × 3 3\times 3 3×3的单位矩阵。
G = [ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] G=\begin{bmatrix} -\pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{I}_3 & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & -C_{\hat{q}}^T & \pmb{0}_{3\times 3} \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{I}_3 \\ \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 3} \end{bmatrix} G=III30003×30003×30003×30003×30003×3III30003×30003×30003×30003×30003×3Cq^T0003×30003×30003×30003×30003×3III30003×3

2)离散时间实现:IMU对周期为T的信号 ω m \omega_m ωm a m a_m am进行采样,这些测量结果用于EKF中的状态传播。每次接收到新的IMU测量值时,通过公式(9)的5阶龙格-库塔数值积分传播IMU状态估计。此外,EKF的协方差矩阵也被传播。为此,我们引入下面的协方差划分:
P k ∣ k = [ P I I k ∣ k P I C k ∣ k P I C k ∣ k T P C C k ∣ k ] (11) P_{k|k}=\begin{bmatrix} P_{II_{k|k}} & P_{IC_{k|k}} \\ P_{IC_{k|k}}^T & P_{CC_{k|k}} \end{bmatrix} \tag{11} Pkk=[PIIkkPICkkTPICkkPCCkk](11)
其中 P I I k ∣ k P_{II_{k|k}} PIIkk为演化IMU状态的 15 × 15 15\times 15 15×15维协方差矩阵, P C C k ∣ k P_{CC_{k|k}} PCCkk是相机位姿估计的 6 N × 6 N 6N\times 6N 6N×6N的协方差矩阵, P I C k ∣ k P_{IC_{k|k}} PICkk是IMU状态下的误差与相机位姿估计之间的相关性。在此记号下,传播状态的协方差矩阵为:
P k + 1 ∣ k = [ P I I k + 1 ∣ k Φ ( t k + T , t k ) P I C k ∣ k P I C k ∣ k T Φ ( t k + T , t k ) T P C C k ∣ k ] P_{k+1|k}=\begin{bmatrix} P_{II_{k+1|k}} & \Phi(t_k+T,t_k)P_{IC_{k|k}} \\ P_{IC_{k|k}}^T \Phi (t_k + T, t_k)^T & P_{CC_{k|k}} \end{bmatrix} Pk+1k=[PIIk+1kPICkkTΦ(tk+T,tk)TΦ(tk+T,tk)PICkkPCCkk]
其中 P I I k + 1 ∣ k P_{II_{k+1|k}} PIIk+1k是通过李亚普诺夫方程的数值积分来计算的:
P ˙ I I = F P I I + P I I F T + G Q I M U G T (12) \dot{P}_{II}=FP_{II}+P_{II}F^T+GQ_{IMU}G^T \tag{12} P˙II=FPII+PIIFT+GQIMUGT(12)
在初始条件 P I I k ∣ k P_{II_{k|k}} PIIkk下,对时间区间 ( t k , t k + T ) (t_k,t_k+T) (tk,tk+T)进行了数值积分。状态转移矩阵 Φ ( t k + T , t k ) \Phi(t_k+T,t_k) Φ(tk+T,tk)也同样通过微分方程的数值积分得到
Φ ˙ ( t k + τ , t k ) = F Φ ( t k + τ , t k ) ,       τ ∈ [ 0 , T ] (13) \dot{\Phi}(t_k+\tau,t_k)=F\Phi(t_k+\tau,t_k), \ \ \ \ \ \tau\in [0, T] \tag{13} Φ˙(tk+τ,tk)=FΦ(tk+τ,tk),     τ[0,T](13)
其中初始条件 Φ ( t k , t k ) = I 15 \Phi(t_k,t_k)=\pmb{I}_{15} Φ(tk,tk)=III15

C 状态增广

记录新图像后,由IMU位姿估计计算出相机位姿估计为:
G C q ˉ ^ =   I C q ˉ   ⊗   G I q ˉ ^ G p ^ C =   G p ^ I + C q ^ T   I p C (14) \begin{matrix} ^C_G\hat{\bar{q}}=\ ^C_I\bar{q} \ \otimes\ ^I_G\hat{\bar{q}} \\ \\ ^G\hat{p}_C =\ ^G\hat{p}_I+C_{\hat{q}}^T\ ^Ip_C \end{matrix} \tag{14} GCqˉ^= ICqˉ  GIqˉ^Gp^C= Gp^I+Cq^T IpC(14)
其中 I C q ˉ ^C_I\bar{q} ICqˉ是四元数,表示IMU坐标系和相机坐标系之间的旋转, I p C ^Ip_C IpC是相机坐标系的原点相对于坐标系 { I } \{I\} { I}的位置,这两个都是已知的。将该相机位姿估计加到状态向量中,并相应增大EKF的协方差矩阵:
P k ∣ k ← [ I 6 N + 15 J ] P k ∣ k [ I 6 N + 15 J ] T (15) P_{k|k} \leftarrow \begin{bmatrix} \pmb{I}_{6N+15} \\ \pmb{J} \end{bmatrix} P_{k|k} \begin{bmatrix} \pmb{I}_{6N+15} \\ \pmb{J} \end{bmatrix}^T \tag{15} Pkk[III6N+15JJJ]Pkk[III6N+15JJJ]T(15)
其中 J \pmb{J} JJJ可由公式(14)得出:
J = [ C ( I C q ˉ ) 0 3 × 9 0 3 × 3 0 3 × 6 N ⌊ C q ^ T   I p C × ⌋ 0 3 × 9 I 3 0 3 × 6 N ] (16) \pmb{J}=\begin{bmatrix} C(^C_I\bar{q}) & \pmb{0}_{3\times 9} & \pmb{0}_{3\times 3} & \pmb{0}_{3\times 6N} \\ \lfloor C_{\hat{q}}^T\ ^Ip_C \times \rfloor & \pmb{0}_{3\times 9} & \pmb{I}_3 & \pmb{0}_{3\times 6N} \end{bmatrix} \tag{16} JJJ=[C(ICqˉ)Cq^T IpC×0003×90003×90003×3III30003×6N0003×6N](16)

D 测量模型

我们现在提出了用于更新状态估计的测量模型,这是本文的主要贡献。由于EKF用于状态估计,对于构建测量模型,它足以定义一个线性依赖于状态误差 X ~ \tilde{X} X~的残差 r r r,根据一般形式:
r = H X ~ + n o i s e (17) r = H\tilde{X} + noise \tag{17} r=HX~+noise(17)
在这个表达式中, H H H是测量雅克比矩阵,为了应用EKF框架,噪声项必须是零均值,白色的,并且与状态误差不相关。

为了推导出我们的测量模型,我们的动机是这样一个事实:从多个相机位姿观测一个静态特征会导致涉及所有这些位姿约束。在我们的工作中,相机观察按跟踪特征分组,而不是按测量记录的相机位姿分组(后者是这种情况,例如,在计算位姿之间的成对约束的方法)。对同一个3D点的所有测量都用来定义一个约束方程(参见公式(24)),它涉及测量发生的所有相机位姿。这是在不包括滤波器状态向量中的特征位置的情况下实现的。我们提出了一个测量模型,考虑一个单一的特征, f j f_j fj,已观察到一组 M j M_j Mj相机位姿 ( G C i q ˉ ,   G p C i ) ,   i ∈ S j (^{C_i}_G\bar{q}, \ ^Gp_{C_i}),\ i \in S_j (GCiqˉ, GpCi), iSj。该模型描述了该特征的每个 M j M_j Mj观测值:
z i ( j ) = 1 C i Z j [ C i X j C i Y j ] + n i ( j ) ,    i ∈ S j (18) z_i^{(j)}=\frac{1}{^{C_i}Z_j}\begin{bmatrix} ^{C_i}X_j \\ ^{C_i}Y_j \end{bmatrix} + n_i^{(j)}, \ \ i \in S_j \tag{18} zi(j)=CiZj1[CiXjCiYj]+ni(j),  iSj(18)
其中 n i ( j ) n_{i}^{(j)} ni(j) 2 × 1 2\times 1 2×1的图像噪声向量,其协方差矩阵为 R i ( j ) = σ i m 2 I 2 \pmb{R}_i^{(j)}=\sigma_{im}^2\pmb{I_2} RRRi(j)=σim2I2I2I2。表示在相机坐标系中的特征位置, C i p f j ^{C_i}\pmb{p}_{f_j} Cipppfj由下式给出:
C i p f j = [ C i X j C i Y j C i Z j ] = C ( G C i q ˉ ) ( G p f j − G p C i ) (19) ^{C_i}\pmb{p}_{f_j}=\begin{bmatrix} ^{C_i}X_j \\ ^{C_i}Y_j \\ ^{C_i}Z_j \end{bmatrix} = C(^{C_i}_G\bar{q})(^Gp_{f_j}-^Gp_{C_i}) \tag{19} Cipppfj=CiXjCiYjCiZj=C(GCiqˉ)(GpfjGpCi)(19)
其中 G p f j ^Gp_{f_j} Gpfj为在全局坐标系中的3D特征位置。由于这是未知的,在我们的算法的第一步,我们使用最小二乘最小化来获得特征位置的估计, G p ^ f j ^G\hat{p}_{f_j} Gp^fj。这是在相应的时刻使用测量 z i ( j ) , i ∈ S j z_i^{(j)}, i \in S_j zi(j),iSj和滤波器相机位姿的估计值来实现的(参见附录)。

一旦得到特征位置的估计,我们计算测量残差:
r i ( j ) = z i ( j ) − z ^ i ( j ) (20) r_i^{(j)}=z_i^{(j)}-\hat{z}_i^{(j)} \tag{20} ri(j)=zi(j)z^i(j)(20)
其中,
z ^ i ( j ) = 1 C i Z ^ j [ C i X ^ j C i Y ^ j ] ,     [ C i X ^ j C i Y ^ j C i Z ^ j ] = C ( G C i q ˉ ^ ) ( G p ^ f j − G p ^ C i ) \hat{z}_i^{(j)}=\frac{1}{^{C_i}\hat{Z}_j}\begin{bmatrix} ^{C_i}\hat{X}_j \\ ^{C_i}\hat{Y}_j \end{bmatrix}, \ \ \ \begin{bmatrix} ^{C_i}\hat{X}_j \\ ^{C_i}\hat{Y}_j \\ ^{C_i}\hat{Z}_j \end{bmatrix} = C(^{C_i}_G\hat{\bar{q}})(^G\hat{p}_{f_j}-^G\hat{p}_{C_i}) z^i(j)=CiZ^j1[CiX^jCiY^j],   CiX^jCiY^jCiZ^j=C(GCiqˉ^)(Gp^fjGp^Ci)
对相机位姿估计和特征位置估计进行线性化,公式(20)的残差近似为:
r i ( j ) ≃ H X i ( j ) X ~ + H f i ( j )   G p ~ f j + n i ( j ) (21) r_i^{(j)} \simeq H_{X_i}^{(j)}\tilde{X}+H_{f_i}^{(j)}\ ^G\tilde{p}_{f_j}+n_i^{(j)} \tag{21} ri(j)HXi(j)X~+Hfi(j) Gp~fj+ni(j)(21)
在前面的表达式中 H X i ( j ) H_{X_i}^{(j)} HXi(j) H f i ( j ) H_{f_i}^{(j)} Hfi(j)分别表示测量 z i ( j ) z_i^{(j)} zi(j)相对于状态和特征位置的雅克比; G p ~ f j ^G\tilde{p}_{f_j} Gp~fj f j f_j fj位置估计的误差。这个表达式中雅可比矩阵的确切值在[21]中提供。通过堆叠该特征的所有 M j M_j Mj测量值的残差,我们得到:
r ( j ) ≃ H x ( j ) X ~ + H f ( j )   G p ~ f j + n ( j ) (22) r^{(j)} \simeq H_x^{(j)}\tilde{X} + H_f^{(j)}\ ^G\tilde{p}_{f_j} + n^{(j)} \tag{22} r(j)Hx(j)X~+Hf(j) Gp~fj+n(j)(22)
其中 r ( j ) r^{(j)} r(j) H X ( j ) H_X^{(j)} HX(j) H f ( j ) H_f^{(j)} Hf(j) n ( j ) n^{(j)} n(j)是块向量或矩阵,分别带有元素 r i ( j ) r_i^{(j)} ri(j) H X i ( j ) H_{X_i}^{(j)} HXi(j) H f i ( j ) H_{f_i}^{(j)} Hfi(j) n i ( j ) n_i^{(j)} ni(j),其中 i ∈ S j i\in S_j iSj。由于不同图像的特征观测是相互独立的,因此 n ( j ) n^{(j)} n(j)的协方差矩阵为 R ( j ) = σ i m 2 I 2 M j R^{(j)}=\sigma_{im}^2\pmb{I}_{2M_j} R(j)=σim2III2Mj

请注意由于状态估计, X X X,被用来计算特征位置的估计(参见附录),公式(22)中的误差 G p ~ f j ^G\tilde{p}_{f_j} Gp~fj和误差 X ~ \tilde{X} X~相关。因此,残差 r ( j ) r^{(j)} r(j)不是公式(17)的形式,它不能直接应用于EKF中的测量更新。为了克服这个问题,我们定义残差 r o ( j ) r_o^{(j)} ro(j),通过将 r ( j ) r^{(j)} r(j)投影至矩阵 H f ( j ) H_f^{(j)} Hf(j)的左零空间。具体而言,如果我们让 A A A表示酉矩阵,它的列构成 H f H_f Hf的左零空间的基,我们得到:
r o ( j ) = A T ( z ( j ) − z ^ ( j ) ) ≃ A T H X ( j ) X ~ + A T n ( j ) (23) r_o^{(j)}=A^T(z^{(j)}-\hat{z}^{(j)})\simeq A^TH_X^{(j)}\tilde{X} + A^Tn^{(j)} \tag{23} ro(j)=AT(z(j)z^(j))ATHX(j)X~+ATn(j)(23)
= H o ( j ) X ~ ( j ) + n o ( j ) (24) =H_o^{(j)}\tilde{X}^{(j)}+n_o^{(j)} \tag{24} =Ho(j)X~(j)+no(j)(24)
由于 2 M j × 3 2M_j\times 3 2Mj×3维矩阵 H f ( j ) H_f^{(j)} Hf(j)是列满秩的,它的左零空间的维数是 2 M j − 3 2M_j-3 2Mj3。因此, r o ( j ) r_o^{(j)} ro(j) ( 2 M j − 3 ) × 1 (2M_j-3)\times 1 (2Mj3)×1维向量。该残差与特征坐标中的误差无关,因此可以基于该残差进行EKF更新。公式(24)定义了观察特征 f j f_j fj的所有相机位姿之间的线性化约束。这表达了测量 z i ( j ) z_i^{(j)} zi(j) M j M_j Mj个状态提供的所有可用信息,因此得到的EKF更新是最优的,除了线性化引起的不准确性。需要指出的是,为了计算残差 r o ( j ) r_o^{(j)} ro(j)和测量矩阵 H o ( j ) H_o^{(j)} Ho(j),不需要显式地计算酉矩阵 A A A。相反,向量 r r r和矩阵 H X ( j ) H_X^{(j)} HX(j) H f ( j ) H_f^{(j)} Hf(j)的零空间上的投影可以使用Givens旋转[22],在 O ( M j 2 ) O(M_j^2) O(Mj2)运算中非常有效地计算。此外,由于矩阵 A A A是酉矩阵,那么噪声向量 n o ( j ) n_o^{(j)} no(j)的协方差矩阵可以表示为:
E { n o ( j ) n o ( j ) T } = σ i m 2 A T A = σ i m 2 I 2 M j − 3 E\{n_o^{(j)} {n_o^{(j)}}^T \}=\sigma_{im}^2 A^TA=\sigma_{im}^2I_{2M_j-3} E{ no(j)no(j)T}=σim2ATA=σim2I2Mj3

公式(23)中定义的残差并不是通过观察 M j M_j Mj图像中的静态特征而产生的几何约束的唯一可能表达。另一种方法是,例如,使用为每个 M j ( M j − 1 ) / 2 M_j(M_j-1)/2 Mj(Mj1)/2图像对定义的极线约束。然而,得到的 M j ( M j − 1 ) / 2 M_j(M_j-1)/2 Mj(Mj1)/2个方程仍然只对应 2 M j − 3 2M_j-3 2Mj3独立约束,因为每次测量都被多次使用,使得方程在统计上具有相关性。我们的实验表明,相比于上述方法,使用线性化的极线约束导致一个明显更复杂的实现,并产生较差的结果。

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/YMWM_/article/details/124982435

智能推荐

区块链分类_按准入机制划分区块链-程序员宅基地

文章浏览阅读3.8k次。按照区块链节点的准入机制,目前区块链可以分为如下三类:(1) 公有链:任何人都可以访问公有链的状态,任何节点都具有读写权限。目前主流的公有链是比特币和以太坊[54]。(2) 联盟链:由一些机构联合构建的区块链,可以设置访问权限和节点加入权限,只有授权节点才可以加入,主流的联盟链有Hyperledger Fabric[55]和Quorum[56]。(3) 私有链:所有节点都同属于一个组织,只有这个组织内部可以决定其使用目的,具有部分去中心化的特性。..._按准入机制划分区块链

Android事件分发机制完全解析,带你从源码的角度彻底理解(下)-程序员宅基地

文章浏览阅读500次。转载请注明出处:http://blog.csdn.net/guolin_blog/article/details/9153761记得在前面的文章中,我带大家一起从源码的角度分析了Android中View的事件分发机制,相信阅读过的朋友对View的事件分发已经有比较深刻的理解了。还未阅读过的朋友,请先参考 Android事件分发机制完全解析,带你从源码的角度彻底理解(上) 。

InnoDB的缓冲池(buffer pool)-程序员宅基地

文章浏览阅读295次。应用系统分层架构,为了加速数据访问,会把最常访问的数据,放在缓存(cache)里,避免每次都去访问数据库。操作系统,会有缓冲池(buffer pool)机制,避免每次访问磁盘,以加速数据的访问。MySQL作为一个存储系统,同样具有缓冲池(buffer pool)机制,以避免每次查询数据都进行磁盘IO。今天,和大家聊一聊InnoDB的缓冲池。InnoDB的缓冲池缓存什么?有什么用?缓存表数据与索引数据,把磁盘上的数据加载到缓冲池,避免每次访问都进行磁盘IO,起到加速..._innodb的缓冲池

【NISP一级】考前必刷九套卷(五)_waf的敏感信息检测规则可检测http信息中的哪部分?(2.5分)a. http请求头b.http-程序员宅基地

文章浏览阅读4.3k次。HTTP 请求是指从客户端到服务器端的请求消息。下列选项中不是 HTTP 请求方法的是_________ A.BODY B.POST C.HEAD D.GET HTTP 消息(HTTP HEADER)又称 HTTP 头,包括请求头等四部分。请求头只出现在 HTTP 请求中,请求头允许客户端向服务器端传递请求的附加信息以及客户端自身的信息,常用的 HTTP 请求报头不包括_________ A.Line B.Cookie C.Accept D.Host..._waf的敏感信息检测规则可检测http信息中的哪部分?(2.5分)a. http请求头b.http

LabVIEW数据采集:视频教程附录1手把手安装LabVIEW_labview教程视频下载-程序员宅基地

文章浏览阅读218次。《LabVIEW数据采集》视频教程附录1:手把手安装LabVIEW _labview教程视频下载

缓存工具类 使用IO流写入本地文件_io写入到本地文件-程序员宅基地

文章浏览阅读227次。/** * Copyright (c) 2012-2013, Michael Yang 杨福海 (www.yangfuhai.com). * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the ..._io写入到本地文件

随便推点

为 netbeans 6.9 安装 scala 插件-程序员宅基地

文章浏览阅读126次。最近想学习一下scala,因为看到有些牛人说这个语言很厉害(是gavin king说的么?),于是就去它的网站上了解了一下。发现有eclipse,netbeans插件,这个还是很不错的,现在很多语言都提供这两个IDE的插件,ruby,groovy。但失望的是两个插件都不能用,eclipse插件装上去无法打开新建的scala文件,装了netbeans插件后,直接无法启动。估计是我用的eclipse和..._netbeans ide有scala 吗

畅享能不能升级鸿蒙,Mate 9 也能升级!鸿蒙系统正式发布:老用户爽了-程序员宅基地

文章浏览阅读178次。6 月 2 日晚,HarmonyOS 2.0 及华为智能产品发布会正式召开。华为 CEO 余承东带来了全新的 HarmonyOS,并喊出了 " 一生万物,万物归一 " 的口号。关于 HarmonyOS,之前已经公布了许多信息,它不仅仅是手机系统,而是一个可以实现万物互联的分布式系统。在此之前,没有任何系统可以覆盖任何设备,毕竟许多设备的运行内存非常小,HarmonyOS 不同,即便是只有 128K..._畅享5会升级鸿蒙

精选10款超酷的HTML5/CSS3菜单-程序员宅基地

文章浏览阅读358次。今天向大家精选了10款超酷的HTML5/CSS3菜单,给你的网页添加不一样的精彩,一起来围观一下吧。1、CSS3手风琴菜单 下拉展开带弹性动画利用CSS3技术可以实现各种各样的网页菜单,我们之前也在CSS3菜单栏目中分享了许多CSS3菜单。今天我们分享的这款是CSS3手风琴菜单,菜单项在展开和收缩的时候菜单项会有弹性动画效果。每一层父级菜单有一个小三角,菜单项在展开的时候这个小三角也会出现动画,非..._css3 实现卷轴效果展开画面

前端架构: 从vue-cli探究脚手架原理-程序员宅基地

文章浏览阅读1.6k次,点赞25次,收藏24次。脚手架本质是一个操作系统的客户端在终端中去执行一个命令,这个命令本身它就是一个客户端我们其实可以把脚手架理解为操作系统的一个客户端通过命令去执行它的时候,这个命令往往是这样的一个构造,如下比如:要创建一个vue的项目的时候, $上面这条命令由3个部分组成主命令: vue这个 command 是子命令,实际上它向脚手架(主命令)发送一个请求这个请求,让我们的脚手架帮我们完成一个动作,完成这个动作,就是create创建项目command的param: vue-test-app。

SwitchyOmega 配置前端代理_switchy omeaga 前端-程序员宅基地

文章浏览阅读3.6k次,点赞2次,收藏3次。switchyomega 使用简介 - 解决前端开发与不同后端联调问题!不管前端开几个服务,不管后端接口多少对接人。 一招搞定。_switchy omeaga 前端

AD+radius完成802.1X认证_ad radius-程序员宅基地

文章浏览阅读2.9k次。实际部署中,AD域、radius,应在搭配交换机、无线路由器或AP,完成基于AD域验证通过有线和无线连接的802.1X认证。本次采取寻找华为交换机、AP产品文档添加相关配置,未经测试,不一定能通。尽管交换机端口已关闭,但用户可以通过身份验证协议与 RADIUS 服务器通信。验证通过,RADIUS服务器会告诉交换机打开端口,用户将获得对网络的访问权限。FreeRADIUS部署在centos7上,需要将RADIUS服务器加入域控(加入域控需要安装samba)。AD域中配置该账号,配置完毕后centos上验证。_ad radius