论文阅读《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

智能推荐

stm32 里的0xFFFFFFul-程序员宅基地

文章浏览阅读3.3k次。表示常量类型的方法,就是放后面的;定义变量放前面。 “u”表示“unsigned”,无符号 “l”表示“long”,长整型 如果不加UL的话那就是默认的int型,UL后缀.也是一种强制转换方式.“SysTick_LOAD_RELOAD_Pos”代表“0” “SysTick_LOAD_RELOAD_Msk”代表“(0xFFFFFFul << SysTick_LOAD_RELOAD_Pos)”_0xfffffful

php正则替换首字符,php正则 字符怎么替换-程序员宅基地

文章浏览阅读77次。php正则 字符怎么替换2021-02-07 12:35:39php正则字符的替换方法:首先创建一个PHP示例文件;然后定义一个字符串;接着通过正则表达式“'/(\w+) (\d+), (\d+)/i'”将指定字符串进行替换即可。本文操作环境:windows7系统、PHP7.1版,DELL G3电脑。preg_replace 函数执行一个正则表达式的搜索和替换。语法mixed preg_repla..._php 判断开头的字符并替换

PyQT 跟我学做密码管理器(5)_pyqt-tool 密码-程序员宅基地

文章浏览阅读2.3k次。PyQT 跟我学做密码管理器(5) —— 备份数据到邮箱声明:本文借鉴https://blog.csdn.net/bigbennyguo/article/details/50755207 修改而来前言借鉴的文章是基于python2+pyqt4,本文是基于python3+pyqt5环境编辑器:Sublime Text3操作系统:win10Python3所需要的库:主要..._pyqt-tool 密码

浅谈Kaggle 当前算法趋势_kaggle 什么模型流行-程序员宅基地

文章浏览阅读1k次。浅谈Kaggle 当前算法趋势一年前我还在悉尼大学着实花费不少在房价预测的kaggle竞赛上,然而之后的一年,先是去聚宽做量化研究实习,接着在悉尼大学的最后一个学期我又抽风非要把商学院的物流方向读下来。于是好多次在kaggle上看到喜欢的竞赛头脑一热报了名,然后就没有然后了:(。近来回国找工作,同时考驾照(一直没考),终于有时间再看看kaggle。准备着手把喜欢的竞赛项目做一做,学一学;同时把..._kaggle 什么模型流行

【微信小程序】云开发+三级联动选择器_三级联动选择器 bed-程序员宅基地

文章浏览阅读1.2k次,点赞2次,收藏9次。目录前言一、数据结构二、Wxml代码三、关键JS代码(一)从数据库中获取数据(二)bindchange事件(三)bindcolumnchange事件(四)处理宿舍数据A.获取宿舍楼栋数据B.获取房号数据(数组与宿舍楼栋数组相对应)C.获取床位数据(数组与宿舍楼栋数组、房号数组相对应)四、整合(一)JS代码(二)Wxml..._三级联动选择器 bed

在linux上安装mysql,最全的安装过程_warning: file /usr/lib64/mysql/plugin/mysql_clear_-程序员宅基地

文章浏览阅读512次。安装linux系统下的mysql1.查看是否安装了mysql;rpm -qa |grep mysql #如果没有输出任何东西,证明没有安装2.找到liunx的版本,下载对应版本的mysql。cat /ect/redhat - release3.下载适合版本的文件来源。#获取我们的文件,通过在linux系统里输入命令和地址wget https://dev.mysql.com/get/mysql75-community-release-el8-1.noarch.rpm#通过找到mysql的官_warning: file /usr/lib64/mysql/plugin/mysql_clear_password.so: remove failed

随便推点

java中可以把代码做为返回值 参数吗_java中如何用函数返回值作为post提交的参数?...-程序员宅基地

文章浏览阅读100次。1.我想实现的功能是在java程序中导入HttpURLConnection类,然后将函数的值作为post方法要提交的参数,最后显示在显示台上。2.要用到的函数是自己写的可以显示实时计算机cpu、内存、硬盘利用率的一个方法,返回值是String.3.这是调用HttpURLConnection的代码> package com.httpclient;import java.io.BufferedR..._java返回结果了可以作为参数吗

JNI技术---clojure 调用C++库的方法_glpk c++-程序员宅基地

文章浏览阅读1.2k次。本文目的:加深理解—to JNI and clojure。JNI概述JNI,是Java Native Interface的缩写,中文为Java本地调用。 JavaTM Native Interface (JNI) is a standard programming interface for writing Java native methods and embedding the JavaTM_glpk c++

Scala中的Map操作_scala中map的用法-程序员宅基地

文章浏览阅读1.8k次。Scala中Map的常用方法_scala中map的用法

将一个浮点型的数通过串口发送出去_串口发浮点数-程序员宅基地

文章浏览阅读6.4k次,点赞19次,收藏85次。可能大家对发送字符串,整数,数组等待的没有问题,也想的明白,可是对于浮点型的数,或许有些不知所措(大佬绕过,小白我是这样)今天搞懂了,就记录下!其实发送原理还是通过字符串的形式发送出去的。只不过是我们将浮点型的数进行的拆解。ps:除运算与或运算不太懂的可以自己查查就可以了。假如一个浮点型的数 d = 25.6345879999我想将这个浮点型的数保留2位小数发送到串口,那我该怎么做呢?..._串口发浮点数

易语言PHP非对称加密,RSA非对称加密通信源码-程序员宅基地

文章浏览阅读573次。RSA非对称加密通信非对称加密是非常安全的一类加密算法TXQQ客户Duan的通信也用了椭圆曲线非对称加密(ECC)非对称加密算法需要两个密钥:公开密钥(publickey)和私有密钥(privatekey)。公开密钥与私有密钥是一对,如果用公开密钥对数据进行加密,只有用对应的私有密钥才能解密;如果用私有密钥对数据进行加密,那么只有用对应的公开密钥才能解密。因为加密和解密使用的是两个不同的密钥,所以..._易语言rsa算法

linux重定向文件输出到文件,【教程】linux 下重定向输入/输出到文件-程序员宅基地

文章浏览阅读879次。1. 引言今天考试遇到一个提交答案题,已经给出了答案检验器(已经编译了的,没有源码),但是手动输命令检验答案文件效率很低,我们最好是让检验器本来输出到屏幕的东西输出到文件,方便我们写程序自动检验。但是我们没有检验器源码,没法 freopen 怎么办呢 QvQ其实是 kb 提出了这个问题啦,下面我就来讲一下 linux(ubuntu)下的重定向输入/输出的方法。2. 输出重定向现在我们有个已经编译好..._重定向输出文件时怎么输出到文件的某一行