视觉-惯性组合导航

单目视觉导航技术存在的一个很大的问题就是当系统只有相机一个外部传感器时无法很好的恢复尺度。除了借助常用的视觉手段如RGB-D相机或者双目相机之外，还有一些其他的方法，但也各有缺点。例如激光测距器传感器太重，红外线传感器又对太阳光太敏感，声呐的测距范围又受到限制。于是只剩下三种比较好的解决方案：双目相机、压力传感器和IMU。然而双目相机由于其基线太短而无法准确的测量远处的距离，压力传感器在室内又不太可靠（突然开关门或空调都会使压力发生突变）。于是似乎只有IMU成了一个比较合理的方案。而融合IMU数据和视觉导航系统输出可以使用最小二乘或者EKF来实现。（UKF计算量太大不考虑）然而最小二乘对粗差又不太敏感，而且通过最小二乘只能得到scale的估计值，不能得到载体速度和位置的最优估计值 [1][2]，于是使用EKF来作为数据融合的框架。

EKF框架

单目相机可以用来补偿IMU的暂时漂移，而磁力计、GPS又可以用于补偿单目相机的空间漂移。下图为几种(non-)传感器的一个简单分类。

上图为几种传感器的分类。单目相机或者带用激光扫描器的相机基于ICP进行姿态估计可以用于降低IMU的临时性漂移。而全局性传感器如激光跟踪器或GPS则可以用于削弱相机或者激光扫描器的空间误差。我们认为统一的自标定(self-)姿态估计方法非常重要，上述传感器为作者最终使用的传感器，它们的使用效果和局限将在下文介绍。

传统的紧组合方案使用EKF SLAM的方法，其计算复杂度为\(O(M^2)\) ，其中M为特征数目。相反，本文中提出的方法将计算复杂度降为常数。而且，因为将姿态估计当作一个黑盒子，我们的方法结果与姿态估计采用的方法无关。最重要的是，我们的方法可以检测姿态估计的错误和漂移。整体结构如下图所示

传感器安装与传感器模型

下面先讨论系统中最核心的两大传感器：用于预测更新状态的IMU和用于校正更新状态的单目相机。IMU可以提供三轴的加速度和角速率，而单目相机经过VO的解算后可以得到在视觉框架( frame 其实就是VO里的world frame，由第一个关键帧确定)下缺少尺度信息的3D位置以及与尺度无关的姿态。下图中显示了单目相机和IMU的安装以及相应的坐标系。其中，绿线表示的是由IMU框架到单目相机框架的系统标定参数，去取决于系统的变形，一般为常数；红线表示的是世界坐标系到IMU框架的转换参数，也是我们关注的重点；蓝线表示的是从视觉框架到相机坐标系的转换参数，由单目VO解算得到；灰线表示的是从视觉框架到世界坐标系的转换参数，在短期内也是不变的，但我们认为，其中的旋转参数（\(q_{v}^w\)）和平移参数（\(p_{v}^w\)）会随着时间不断漂移。引入这些漂移参数和建立世界坐标系的原因来源于两个方面

各个传感器使用的坐标系如上图所示，每两个坐标系之间都存在一个旋转\(q\)和平移\(p\) 。基本不变的参数用绿线表示，如IMU-cam的转换参数。红线表示的数据是我们最终需要的位姿。蓝线表示的是相机坐标系与是视觉框架的转换关系。灰线表示的是视觉框架与世界坐标系的转换参数，这些参数在短期内保持不变。注意\(q^w_{v}\)中微小的变化即为系统角度的漂移，\(p^w_{v}\)中微小的变化即为系统位置的漂移。

我们假定IMU测量值中包括一项零偏\(b\)和一项高斯随机噪声\(n\)，于是，对于IMU框架下的真实的角速度\(\omega\)和加速度\(a\)有

\[ \omega=\{m}-b_{\omega}-n_{\omega} ,\space a=a_{m}-b_{a}-n_{a}\tag{1} \]

上市中下标m表示测量值，并且将零偏建模为动态随机过程，即

\[ \dot b_{\omega}=n_{b_{\omega}},\space\dot b_{a}=n_{b_{a}}\tag{2} \]

因此，我们可以总结噪声模型特性如下

\[ E(n_{a})=E(n_{\omega})=E(n_{b_{a}})=E(n_{b_{\omega}})=0 \\E(n_{\omega}(t+\tau)*n^T_{\omega}(t))=\sigma^2_{\omega}\delta(\tau) \\E(n_{b_{\omega}}(t+\tau)*n^T_{b_{\omega}}(t))=\sigma^2_{b_{\omega}}\delta(\tau) \\E(n_{a}(t+\tau)*n^T_{a}(t))=\sigma^2_{a}\delta(\tau) \\E(n_{b_{a}}(t+\tau)*n^T_{b_{a}}(t))=\sigma^2_{b_{a}}\delta(\tau) \tag{3} \]

定义\([\delta(x)]=\frac{1}{[x]}\)(式中\([x]\)表示\(x\)的单位)，同时高斯白噪声的单位和传感器测量值的单位应当是一致的，于是在连续时间下，我们可以得到各个噪声的单位

\[ \\ [\\omega^2]=\frac{\\omega^2\delta(\tau)}{[\delta(\tau)]}=\frac{rad^2}{sec} \\ [\{b_\omega}^2]=\frac{\{b_\omega}^2\delta(\tau)}{[\delta(\tau)]}=\frac{rad^2}{sec^3} \\ [\^2]=\frac{\^2\delta(\tau)}{[\delta(\tau)]}=\frac{m^2}{sec^3} \\ [\{b_a}^2]=\frac{\{b_a}^2\delta(\tau)}{[\delta(\tau)]}=\frac{m^2}{sec^5}\tag{4} \]

这些值应当由厂商提供，对于离散时间系统来说，因为需要在滤波器中使用这些值，我们需要根据它们的单位分别进行转换

\[ d\sigma^2_\omega=\frac{\sigma^2_\omega}{\Delta t}, \space d\sigma^2_{b_{\omega}}=\sigma^2_{b_{\omega}}*\Delta t \\ d\sigma^2_a=\frac{\sigma^2_a}{\Delta t}, \space d\sigma^2_{b_{a}}=\sigma^2_{b_{a}}*\Delta t\tag{5} \]

使用厂商提供的参数并结合上式进行转换，我们就能得到一个很好的离散滤波器噪声模型。

状态的表示

滤波器的状态有

以上符号当表示一般的量时上标表示某一框架，下标表示该量的参考框架，比如\(p^i_w\)表示i系在W系下的位置；而当表示转换关系时，下标为源坐标系，上标为目标坐标系，比如\(q^i_w\) 表示W系转换到IMU系的旋转参数。

此外需要说明的是，某一框架在某一参考框架下的姿态，是指参考框架的坐标轴（基）按照该姿态进行旋转后可得到该框架的坐标轴，而相反的该框架下的向量经过该姿态的旋转，得到参考框架下的向量。以\(q^i_w\) 为例，其表示W系下向量转换到IMU系下向量的旋转参数，同时也表示i系在W系下的姿态，即\(X_w=R_i^wX_i+p_w^i\)，式中\(R_i^w\)为\(q^w_i\)对应的旋转矩阵。

状态变量中并没有包括重力向量是因为（初始时）世界坐标系已经和水平面对齐。，最终我们得到包含31个元素的状态变量

\[ X=\{p_w^{i^T},v_w^{i^T},q_w^{i^T},b_\omega^{T},b_a^{T},\,p_i^c,q_i^c,p_v^w,q_v^w\}\tag{6} \]

相应的状态微分方程为

\[ \begin{array} \dot p^i_w=v^i_w \\ \dot v^i_w=C^T_{q_w^i}(a_m-b_a-n_a)-g \\ \dot q_w^i = \frac{1}{2}\Omega(\-b_\omega-n_\omega)q_w^i \end{array}\tag{7} \]

\[ \dot b_\omega=n_{b_\omega} \quad \dot b_a=n_{b_a}\quad \dot \=0 \\ \dot p_i^c=0 \quad \dot q_i^c=0 \quad \dot p^w_v =0 \quad \dot q^w_v=0\tag{8} \]

上式中\(C_{(q)}\)为四元数\(q\)对应的旋转矩阵，\(g\)为世界坐标系下的重力向量，\(\Omega(\omega)\)为\(\omega\)的四元数更新矩阵（是相应的斜对称矩阵skew？）。我们假设尺度和视觉框架V的漂移是空间上的，并且随时间变化(We the scale and frame V to drift and not ???)。因此我们可以使用静止模型(zero model)。有一些状态需要额外的注意因为它们的确定性需要根据具体的空间事件动态调整。比如，当使用基于关键帧的VO算法计算相机的姿态时，尺度\(\\)和视觉框架漂移参数\(p^w_v,q_v^w\)的方差只能在关键帧创建时改正并且和算法具体的性能有关。考虑以上因素以及前文提到的滤波器状态估计噪声模型，我们得到

\[ \begin{array} \\ \hat{\dot {p} }^i_w=\hat v^i_w \\ \hat{\dot v}^i_w=C^T_{\hat q_w^i}(a_m-\hat b_a)-g \\ \hat{\dot q} _w^i = \frac{1}{2}\Omega(\-\hat b_\omega)\hat q_w^i \end{array}\tag{9} \]

\[ \hat {\dot b}_\omega=0 \quad \hat {\dot b}_a=0 \quad \hat{ \dot \}=0 \\ \hat{\dot p}_i^c=0 \quad \hat{\dot q}_i^c=0 \quad \hat{\dot p}^w_v =0 \quad \hat{\dot q}^w_v=0\tag{10} \]

误差状态表示

在上述的系统状态表示中，我们使用四元数来描述姿态。表示姿态误差和相应的方差时，一般使用四元数的误差形式，而不直接进行算数上的作差。这能够提高系统的数值稳定性并且用最少的状态来描述姿态。因此，我们可以得到28元素的误差状态向量\(\tilde x\)

\[ \tilde x =\{\Delta p_w^{i^T}\space \Delta v_w^{i^T} \space \delta \^{i^T} \space \Delta b_\omega^{T} \space \Delta b_a^{T} \space \Delta \ \space \Delta p_i^{c^T} \space \delta \^{c^T} \space \Delta p_v^{w^T} \space \Delta \^{w^T}\}\tag{11} \]

以上除了四元数之外的误差变量均满足\(\tilde x=x-\hat x\)，而四元数的误差变量则满足

\[ \delta q^i_w=q^i_w \ \hat q^{i^{-1}}_w \[\frac{1}{2} \delta \^{i^T}\]^T \\ \delta q^c_i=q^c_i \ \hat q^{c^{-1}}_i \[\frac{1}{2} \delta \^{c^T}\]^T \\ \delta q^v_w=q^w_v \ \hat q^{w^{-1}}_v \[\frac{1}{2} \delta \^{w^T}\]^T\tag{12} \]

连续时间下的误差状态微分方程为

\[ \begin{array} \\ \Delta \dot p^i_w=\Delta v^i_w \\ \Delta \dot v^i_w = -C^T_{\hat q^i_w}[\hat a_\times]\delta \theta-C^T_{\hat q^i_w}\Delta b_a-C^T_{\hat q^i_w}n_a \\ \delta \dot \theta^i_w = -[\hat \\times]\delta \theta-\Delta b_\omega-n_\omega \\ \end{array} \\ \Delta \dot b_\omega=n_{b_\omega} \quad \Delta \dot b_a=n_{b_a} \quad \Delta \dot \=0 \\ \Delta \dot p^c_i = 0 \quad \delta \dot q^c_i=0 \quad \Delta \dot p^w_v=0 \quad \delta \dot q^w_v=0\tag{13} \]

上式中\(\hat \omega=\-\hat b_\omega\)，\(\hat a = a_m-\hat b_a\)，于是可以得到连续时间下的线性误差状态方程。

\[ \dot {\tilde x}=F_c \tilde x+G_cn\tag{14} \]

上式中\(n\)为噪声向量即\(n=[n_a^T,n^T_{b_a},n^T_\omega,n^T_{b_\omega}]\)。为了加快计算速度，认为\(F_c\)和\(G_c\)在积分周期中均为常值，对上述状态方程进行离散化后可得

\[ F_d=\exp(F_c\Delta t)=\{I_d}+F_c \Delta t+ \frac{1}{2!}F^2_c\Delta t^2\tag{15} \]

经过分析和计算可以得到\(F_d\)的结构如下

\[ F_d= \begin{} \{I_{d_3}}&\Delta t&A&B&-C^T_{\hat q^i_w}\frac{\Delta t^2}{2}& \{0_{3\}}\\ \{0_{3}}&\{I_{d_3}}&C&D&-C^T_{\hat q^i_w}\Delta t& \{0_{3\}} \\ \{0_{3}}&\{0_{3}} &E&F& \{0_{3}} & \{0_{3\}} \\ \{0_{3}}& \{0_{3}}& \{0_{3}}&\{I_{d_3}}& \{0_{3}}& \{0_{3\}} \\ \{0_{3\}}&\{0_{3\}}&\{0_{3\}}&\{0_{3\}}&\{0_{3\}}&\{I_{d_3}} \end{}\tag{16} \]

使用小角度近似即\(|\omega|\to0\)，并使用洛必达法则可以得到\(A,B,C,D,E,F\)的解[3]

\[ \begin{array} \\ A=-C^T_{\hat q^i_w}[\hat a_\times](\frac{\Delta t^2}{2} - \frac{\Delta t^3}{3!}[\\times]+ \frac{\Delta t^4}{4!}[\\times]^2 ) \\ B=-C^T_{\hat q^i_w}[\hat a_\times](\frac{-\Delta t^3}{3!} + \frac{\Delta t^4}{4!}[\\times]-\frac{\Delta t^5}{5!}[\\times]^2 ) \\ C=-C^T_{\hat q^i_w}[\hat a_\times](\Delta t - \frac{\Delta t^2}{2!}[\\times]+\frac{\Delta t^3}{3!}[\\times]^2 ) \\ D=-A \\ E = \{I_d}-\Delta t[\\times]+\frac{\Delta t^2}{2!}[\\times]^2 \\ F = -\Delta t+ \frac{\Delta t^2}{2!}[\\times]-\frac{\Delta t^3}{3!}[\\times]^2 \end{array}\tag{17} \]

同时可以得到\(G_c\)为

\[ G_c= \begin{} \{0_3}&\{0_3}&\{0_3}&\{0_3} \\ -C^T_{\hat q^i_w}&\{0_3}&\{0_3}&\{0_3} \\ \{0_3}&\{0_3}&\{-I_{d_3}}&\{0_3} \\ \{0_3}&\{0_3}&\{0_3}&\{I_{d_3}} \\ \{0_3}&\{I_{d_3}} &\{0_3}&\{0_3}\\ \{0_{13\}}&\{0_{13\}}&\{0_{13\}}&\{0_{13\}}\\ \end{}\tag{18} \]

于是通过积分就可以得到矩阵\(Q_d\)为

\[ Q_d=\int_{\Delta t}{F_d(\tau)^(\tau)^Td\tau}\tag{19} \]

上式中\(Q_c\)为连续时间系统下的噪声协方差矩阵\(Q_c=diag(\sigma^2_{n_a},\sigma^2_{n_{b_a}},\sigma^2_{n_\omega},\sigma^2_{n_{b_\omega}})\)。

得到离散时间系统下误差状态方程和误差协方差阵后就可以按照下面的流程来对误差状态进行估计更新

根据式(9)(10)进行IMU状态更新，对四元数使用一阶积分进行计算[3]

计算\(F_d\)和\(Q_d\)

使用滤波迭代方程计算IMU状态相应的协方差矩阵

\[ P_{k+1|k}=F_dP_{k|k}F^T_d+Q_d \]

观测校正

前文提到IMU的加速度和角速度用于状态的预测更新，而接下来要介绍的位姿测量（相机）则用于状态的校正更新。然而位姿测量值在空间上存在漂移。为了更好的理解校正过程，我们先忽略位置的漂移，并且假定仅存在姿态的漂移。即认为\(p^w_v\)为0并且在滤波器的方程中可以忽略。

协方差矩阵

在滤波器的校正过程中，我们首先对观测状态的测量噪声的协方差矩阵进行估计。例如，EKF-SLAM中相机位姿的协方差矩阵就是实时估计的，为了计算姿态的协方差可以使用五点法或者其他类似的方法[4]。而利用关键帧进行非线性优化得到的位姿将更加复杂：我们建议使用[4]中提供的利用初始两个关键帧的估计方法。此外，我们还建议使用[5]中给出的定义N个相邻的关键帧进行优化，剩下的关键帧固定的方法。我们假设真实位姿的不确定程度与N个关键帧经过加权平均的不确定度相同。注意N是根据快速计算需求而选取的参数。但当使用的VO算法完全闭源且不输出精度信息那么就比较麻烦了。

我们将VO计算得到的位置\(p\)和旋转\(q\)当作滤波器的观测量，他们的噪声为\(n_p\)\(n_q\)，于是可以得到具有6个元素的测量噪声向量

\[ n_m=[n_p \space n_q]^T \]

根据以上噪声向量可以得到观测量的协方差矩阵\(R\)。

观测模型

我们将VO解算得到的位置\(p_v^c\)和旋转\(q_v^c\)当作系统的观测量，于是对于位置，有一下观测模型

\[ z_p=p_v^c=C_{q^w_v}^T(p^i_w+C^T_{q^i_w}p^c_i)\+n_p \]

上式中\(C_{q^i_w}\)为IMU在世界坐标系下的姿态，而\(C_{q^w_v}\)则为从v系到W系的旋转矩阵。

同时，我们定义姿态误差为

\[ \begin{align*} \tilde z_p&=z_p-\hat z_p \\ &=C^T_{q^w_v}(p^i_w+C^T_{q_w^i}p^c_i)\+n_p -\\ &\quad C^T_{\hat q^w_v}(\hat p^i_w+C^T_{\hat q^i_w}\hat p^c_i)\hat \ \end{align*} \]

将上式写为线性形式

\[ \tilde z_{p_l}=H_p\tilde x \]

式中\(H_p\)为

\[ H_p= \begin{} C^T_{\hat q^w_v}\hat \ \\ -C^T_{\hat q_v^w}C^T_{\hat q^i_w}[\hat p^c_{i \times}]\hat \ \\ \{0_3} \\ \{0_3} \\ C^T_{\hat q^w_v}C^T_{\hat q^i_w}\hat p^c_i+C^T_{\hat q^w_v}\hat p \\ C^T_{\hat q^w_v}C^T_{\hat q^i_w}\hat \ \\ \{0_3} \\ -C^T_{\hat q ^w_v }[(p_w^i+C^T_{\hat q^i_w}p^c_i)\ _\times] \end{}^T \]

上式中\(\hat p = \hat p ^i_w\)，又跟据四元数的误差定义得到

\[ q^i_w=\delta q^i_w \ \hat q^i_w \\ C_{q^i_w}=C_{q^i_{\hat i}}C_{q^{\hat i}_w} \\ C_{q^i_{\hat i}} \ \{I_d}-[\delta \theta^i_{w \times}] \]

对于姿态测量，我们使用四元数的形式来表示姿态误差。首先VO提供的姿态为从视觉框架转换到相机坐标系的旋转参数\(q_v^c\)，可写为

\[ z_q=q^c_v=q^c_i \ q^i_w \ q^w_v \]

于是可以得到相应的误差方程（通过四元数的“除法”）

\[ \begin{align*} \tilde z_q&=z_q-\hat z_q \\ & =q^c_i \ q^i_w \ q^w_v \(q^c_i \ \hat q^i_w \ \hat q^w_v)^{-1} \\ & =H^{wi}_q\delta q^i_w = H^{ic}_q \delta q^c_i = H^{vw}_q \delta q^w_v \end{align*} \]

式中\(H^{wi}_q\)，\(H_q^{ic}\)和\(H^{vw}_q\)极为相应的线性转换矩阵，最终可以得到观测方程为

\[ \begin{} \tilde z_p \\ \tilde z_q \end{}=\begin{}& & H_p \\ \{0_{3\}} & \tilde H^{wi}_q & \{0_{3\}} & \tilde H^{ic}_q & \tilde H^{vw}_q \end{} \tilde x \]

using the ....(这句话想说啥？)

测量校正更新

一旦我们计算的到测量矩阵\(\{H}\)后，我们就可以根据滤波的流程对之前的估计状态进行校正更新

计算残差\(\tilde z=z-\hat z\)计算新息矩阵\(S=HPH^T+R\)计算滤波增益\(K=PH^TS^{-1}\)计算误差的估计值\(\hat{\tilde x}=K \tilde z\)

利用误差的估计值对状态进行改正即可得到校正后的状态值，而后对协方差矩阵进行更新

\[ P_{k+1|k+1}=(\{I_d}-KH)P_{k+1|k}(\{I_d}-KH)^T+KRK^T \]

VO位姿估计故障检测

前文提到我们认为短时间内世界坐标系和视觉框架的旋转\(q_v^w\)不变，因此在每个滤波估计更新的时刻我们可以得到该旋转为

\[ q_v^w(k)=\hat q^{i^{-1}}_w(k) \ \hat q^{c^{-1}}_i (k)\ q^c_v(k) \]

因为姿态的漂移相对于滤波估计和校正的周期来说非常慢，因此我们可以对\(q^w_v(k)\)作平滑。我们建议使用一个中值滤波器对\(q^w_v(k)\)序列进行处理，并且识别出其中的粗差。于是，使用一个窗口大小为N的中值滤波器对世界坐标系和视觉框架之间的转换参数进行滤波得到

\[ \hat q^w_v(k)=med(q^w_v(i))\quad i=k-N\to k \]

因为漂移非常慢，所以我们可以识别出突变的异常值。识别到异常值后，不对偏差和尺度更新，但对IMU的位置、速度、姿态进行更新防止IMU的状态发散。此外，相对与IMU的加速度计，其内部的陀螺仪测量比较可靠，对陀螺仪的角速度测量进行积分在长时间内可以得到较好的姿态。因此我们相应的减小VO位置测量的噪声，增大VO姿态测量的噪声。

[1]L. Kneip, A. , S. Weiss, D. , and R. , Y. -Form for Scale and a . In on and , , China, May 2011. URL .

[2]A. , C. , and A. . -aided : -form of scale, speed and . In on and , San , États-Unis, 2011. URL .

[3]N. and S. . for 3d . , of , of and , 2005.

[4]C. Beder and R. . an image pair for the scale of a 3d from an image . 4174 in LNCS. , 2006.