CN109631894A - 一种基于滑动窗口的单目视觉惯性紧耦合方法 - Google Patents

Abstract

本发明属于电通信技术领域，公开了一种基于滑动窗口的单目视觉惯性紧耦合方法，具体包括以下步骤：获得原始的陀螺仪和加速度计观测值；利用相应的协方差描述IMU测量模型；使用视觉‑惯性光束平差公式，通过最小化先验和测量残差的马氏范数和，获得最大后验估计情况。本发明提出的基于滑动窗口的单目视觉惯性紧耦合方法，将视觉和惯性的观测量进行有效的组合实现高精度和鲁棒状态估计；本发明的算法经过在数据集EuRoC数据集上进行评测，可知本发明得到的轨迹与真实轨迹之间的绝对误差；为了比较本发明算法的有效性，通过与VIO方法的轨迹精度进行比较，获得本发明提供的算法的有效性。

Description

一种基于滑动窗口的单目视觉惯性紧耦合方法

技术领域

本发明属于电通信技术领域，尤其涉及一种基于滑动窗口的单目视觉惯性紧耦合方法。

背景技术

目前，业内常用的现有技术是这样的：

单目视觉导航定位系统由于无法测量空间物体的距离，限制了它们在实际场景中的应用。使用低成本惯性测量单元(IMU)协助单目视觉系统的主要优点是具有可观察的绝对尺度，可以测量滚转角和俯仰角，这使导航系统可以估计运动状态。此外，对IMU测量值积分可以缩小由光照变化、无纹理区域或运动模糊而导致的视觉轨迹偏移，从而极大地改善运动跟踪性能。

按照是否把图像特征信息加入状态向量来进行分类，融合视觉和惯性传感器的方法可以分为松耦合与紧耦合。较为简单的是松耦合方法，融合通常由扩展卡尔曼滤波器(EKF)完成，其中IMU用于状态传播，仅依赖视觉对位姿更新。紧密耦合的视觉惯性算法是有基于EKF或非线性优化，其中相机和IMU的测量是由原始测量值共同优化的。与松耦合算法相比，紧耦合算法精度较高，与基于EKF方法关心的是当前时刻的状态估计相比，图优化方法使用所有时刻采集到的数据进行状态估计，被认为优于传统滤波器。

综上所述，现有技术存在的问题是：现有的松耦合算法，精度较差；导航系统不可以估计运动状态。

发明内容

针对现有技术存在的问题，本发明提供了一种基于滑动窗口的单目视觉惯性紧耦合方法。

本发明是这样实现的，一种基于滑动窗口的单目视觉惯性紧耦合方法，具体包括以下步骤：

步骤一：获得原始的陀螺仪和加速度计观测值；

步骤二：利用相应的协方差来描述IMU测量模型；

步骤三：使用视觉-惯性光束平差公式，通过最小化先验和测量残差的马氏范数和，来获得最大后验估计情况。

进一步，步骤一中，原始的陀螺仪和加速度计观测值由IMU给出:

其中，是加速度测量值，a_t是加速度真实值，b_at是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，是陀螺仪偏移，n_ω是陀螺仪噪声项。

进一步，步骤二中，IMU测量模型为：

本发明的另一目的在于提供的基于滑动窗口的单目视觉惯性紧耦合方法，具体为：

原始的陀螺仪和加速度计观测值由IMU，和给出:

其中，是加速度测量值，a_t是加速度真实值，是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，是陀螺仪偏移，n_ω是陀螺仪噪声项。

假定加速度计和陀螺仪中的附加噪声服从高斯分布： 加速度和陀螺仪偏移服从随机游走模型： 可得：

设定两个时间序列对应的图像帧为b_k和b_k+1，时间间隔为[t_k,t_k+1]，通过对式积分就可以计算出下一时刻的无人机的位置P、速度v和方向状态q。

其中，

Δt是时间间隔[t_k，t_k+1]。

等式(0-4)左边的值都是世界坐标系(·)^ω下b_k+1帧的值，两边同时乘以从世界坐标到b_k帧坐标系的转换和就可以从世界坐标系转换到机体坐标系b_k中：

其中和仅对与加速度和角速度有关的量进行积分：

当偏移变化特别小的时候，可以使用一阶线性展开来调整临时状态量 如式(0-7)所示：

一开始将的值设为0；旋转设为单位旋转；噪声项n_a和n_ω未知并在整个过程把噪声设为0；用表示预积分项的估计值，将上式用欧拉积分展开可得：

其中，i是与IMU在[t_k,t_k+1]内测量值对应的离散时刻；δt是两个IMU测量值i和i+1之间的时隔。

由于四自由度旋转四元数被过参数化，将其误差项看为均值扰动定义式:

其中是三维小扰动；

之后可以推导出式(0-6)连续时间线性化的误差项：

可以通过初始协方差的一阶离散时间协方差重递归计算：

其中Q是噪声的对角协方差矩阵

关于的的一阶雅可比矩阵也可以用初始雅可比递归计算：

J_t+δt＝I+F_tδtJ_t，t∈[k,k+1] (0-12)

因此，将式(0-11)和式(0-12)带入式(0-6)可得更精确的传播状态值

其中，和中的子矩阵的对应于 也有同样的含义。

现在可以用相应的协方差来描述IMU测量模型：

考虑滑动窗口中两个连续帧b_k和b_k+1内的IMU测量，与前面的预积分模型相同，协方差矩阵就是这里的协方差矩阵，而式左边的项就是预测的值，也被称为估计值，估计值和测量值之间的差值就是残差，根据式中定义的IMU测量模型，可以将预集成IMU测量的残差定义为：

其中[·]_xyz提取四元数q的矢量部分表示误差状态，表示四元数的三维误差，是在两个连续帧的时间间隔内仅使用有噪声的加速计和陀螺仪测量得的预积分的IMU测量项。加速度计和陀螺仪偏移也包括在线修正的残差项。

在已有在滑动窗口中将全状态矢量被定义为：

其中，第一个式子是滑动窗口内的整个状态向量，n是关键帧的总数，m是滑动窗口中的特征点总数，维度是15×n+6+m；第二个式子x_k是第k个图像被捕获时的IMU状态，包括位姿，速度，旋转，加速度计和陀螺仪偏移；第三个式子是相机外参，λ_l是第l个特征的逆深度。

相机的残差模型如下公式(0-17)所示，考虑到第i个图像中首先观察到的第l个特征，第j个图像中特征观察的残差被定义为：

其中，第一个式子是残差表达式；第二个式子是相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，由于视觉残差的自由度为2，可以将残差向量投影到切平面上，b₁，b₂是切平面上任意的两个正交基，是对第i个图像中的第l个特征的第一次观测，π_c ^-1是相机固有参数，P_l ^cj是在切空间中固定长度的标准协方差；第三个式子是重投影估计模型，是对第j个图像中相同特征的观察；

使用视觉-惯性光束平差公式；通过最小化先验和测量残差的马氏范数和，来获得最大后验估计：

其中，马氏范数的定义如下：

和分别是IMU和视觉测量的残差，β是IMU所有测量值的集合，C是在当前滑动窗口中观察到至少两次的特征点的集合，{r_p，H_p}是边缘化的先验信息，通过Ceres Solver解决这个非线性问题。

综上所述，本发明的优点及积极效果为：

本发明提出的基于滑动窗口的单目视觉惯性紧耦合方法，将视觉和惯性的观测量进行有效的组合实现高精度和鲁棒状态估计；本发明的算法经过在数据集EuRoC数据集上进行评测，可知本发明得到的轨迹与真实轨迹之间的绝对误差；为了比较本发明算法的有效性，通过与VIO方法的轨迹精度进行比较，获得本发明提供的算法的有效性，不停数据集对比结果见下表。本发明解决了初始化尺度问题；追踪中提供较好的初始位姿；提供重力方向；提供一个时间误差项以供优化。

表1数据集对比结果表

附图说明

图1是本发明实施例提供的基于滑动窗口的单目视觉惯性紧耦合方法流程图。

图2是本发明实施例提供的Ground Truth、VIO与本发明方法MH_01_easy上的对比示意图。

图3是本发明实施例提供的Ground Truth、VIO与本发明方法MH_02_easy上的对比示意图。

图4是本发明实施例提供的Ground Truth、VIO与本发明方法MH_03_medium上的对比示意图。

图5是本发明实施例提供的Ground Truth、VIO与本发明方法MH_04_difficult上的对比示意图。

图6是本发明实施例提供的Ground Truth、VIO与本发明方法MH_05_difficult上的对比示意图。

图7是本发明实施例提供的VIO算法与本发明优化VIO算法实验结果对比示意图；

图中：(a)MH_01_easy上的对比；(b)MH_02_easy上的对比；(c)MH_03_medium上的对比；(d)MH_04_difficult上的对比；(e)MH_05_difficult上的对比。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理做详细描述。

如图1所示，本发明实施例提供的基于滑动窗口的单目视觉惯性紧耦合方法，具体包括以下步骤：

S101：获得原始的陀螺仪和加速度计观测值；

S102：利用相应的协方差来描述IMU测量模型；

S103：使用视觉-惯性光束平差公式，通过最小化先验和测量残差的马氏范数和，来获得最大后验估计情况。

步骤S101中，本发明实施例提供的原始的陀螺仪和加速度计观测值由IMU和给出:

其中，是加速度测量值，a_t是加速度真实值，是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，是陀螺仪偏移，n_ω是陀螺仪噪声项。

步骤S102中，本发明实施例提供的IMU测量模型为：

本发明实施例提供的基于滑动窗口的单目视觉惯性紧耦合方法，具体为：

原始的陀螺仪和加速度计观测值由IMU给出:

其中，是加速度测量值，a_t是加速度真实值，是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，是陀螺仪偏移，n_ω是陀螺仪噪声项。

假定加速度计和陀螺仪中的附加噪声服从高斯分布： 加速度和陀螺仪偏移服从随机游走模型： 可得：

设定两个时间序列对应的图像帧为b_k和b_k+1，时间间隔为[t_k，t_k+1]，通过对式积分就可以计算出下一时刻的无人机的位置P、速度v和方向状态q。

其中，

Δt是时间间隔[t_k，t_k+1]。

等式左边的值都是世界坐标系(·)^ω下b_k+1帧的值，两边同时乘以从世界坐标到b_k帧坐标系的转换和就可以从世界坐标系转换到机体坐标系b_k中：

其中和仅对与加速度和角速度有关的量进行积分：

当偏移变化特别小的时候，可以使用一阶线性展开来调整临时状态量 如式(0-7)所示：

一开始将的值设为0；旋转设为单位旋转；噪声项n_a和n_ω未知并在整个过程把噪声设为0；用表示预积分项的估计值，将上式用欧拉积分展开可得：

其中，i是与IMU在[t_k，t_k+1]内测量值对应的离散时刻；δ_t是两个IMU测量值i和i+1之间的时隔。

由于四自由度旋转四元数被过参数化，将其误差项看为均值扰动定义式:

其中是三维小扰动；

之后可以推导出式(0-6)连续时间线性化的误差项：

可以通过初始协方差的一阶离散时间协方差重递归计算：

其中Q是噪声的对角协方差矩阵

关于的的一阶雅可比矩阵也可以用初始雅可比递归计算：

J_t+δt＝I+F_tδtJ_t，t∈[k，k+1]

因此，(0-6)可得更精确的传播状态值

其中，和中的子矩阵的对应于 也有同样的含义；

现在可以用相应的协方差来描述IMU测量模型：

考虑滑动窗口中两个连续帧b_k和b_k+1内的IMU测量，可以将预积分IMU测量的残差定义为：

其中[·]_xyz提取四元数q的矢量部分表示误差状态，表示四元数的三维误差，是在两个连续帧的时间间隔内仅使用有噪声的加速计和陀螺仪测量得的预积分的IMU测量项。加速度计和陀螺仪偏移也包括在线修正的残差项。

在已有在滑动窗口中将全状态矢量被定义为：

其中，第一个式子是滑动窗口内的整个状态向量，n是关键帧的总数，m是滑动窗口中的特征点总数，维度是15×n+6+m；第二个式子X_k是第k个图像被捕获时的IMU状态，包括位姿，速度，旋转，加速度计和陀螺仪偏移；第三个式子是相机外参，λ_l是第l个特征的逆深度。

相机的残差模型如下所示，考虑到第i个图像中首先观察到的第l个特征，第j个图像中特征观察的残差被定义为：

其中，第一个式子是残差表达式；第二个式子是相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，由于视觉残差的自由度为2，可以将残差向量投影到切平面上，b₁，b₂是切平面上任意的两个正交基，是对第i个图像中的第l个特征的第一次观测，π_c ^-1是相机固有参数，P_l ^cj是在切空间中固定长度的标准协方差；第三个式子是重投影估计模型，是对第j个图像中相同特征的观察；

使用视觉-惯性光束平差公式；通过最小化先验和测量残差的马氏范数和，来获得最大后验估计：

其中，马氏范数的定义如下：

和分别是IMU和视觉测量的残差，β是IMU所有测量值的集合，C是在当前滑动窗口中观察到至少两次的特征点的集合，{r_p，H_p}是边缘化的先验信息，通过Ceres Solver解决这个非线性问题。

下面结合具体实施例对本发明的应用原理进行详细说明；

实施例1；

在数据集EuRoC数据集上对发明的算法进行评测，可以得到本发明得到的轨迹与真实轨迹之间的绝对误差；

为了比较本发明算法的有效性，与VIO方法的轨迹精度进行比较，对比结果如表1所示。

表1：数据集对比结果

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于滑动窗口的单目视觉惯性紧耦合方法，其特征在于，所述基于滑动窗口的单目视觉惯性紧耦合方法包括以下步骤：

步骤一：获得原始的陀螺仪和加速度计观测值；

步骤二：利用相应的协方差来描述IMU测量模型；

步骤三：使用视觉-惯性光束平差公式，通过最小化先验和测量残差的马氏范数和，来获得最大后验估计情况。

2.如权利要求1所述的基于滑动窗口的单目视觉惯性紧耦合方法，其特征在于，所述步骤一中，原始的陀螺仪和加速度计观测值由IMU，和给出:

其中，是加速度测量值，a_t是加速度真实值，b_at是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，b_ωt是陀螺仪偏移，n_ω是陀螺仪噪声项。

3.如权利要求1所述的基于滑动窗口的单目视觉惯性紧耦合方法，其特征在于，所述步骤二中，IMU测量模型为：

4.如权利要求1所述的基于滑动窗口的单目视觉惯性紧耦合方法，其特征在于，所述基于滑动窗口的单目视觉惯性紧耦合方法，具体为：

原始的陀螺仪和加速度计观测值由IMU，和给出:

其中，是加速度测量值，a_t是加速度真实值，b_at是加速度计的偏移，是重力加速度，n_a是加速度噪声项，是陀螺仪测量值，ω_t是陀螺仪真实值，b_ωt是陀螺仪偏移，n_ω是陀螺仪噪声项；

加速度计和陀螺仪中的附加噪声服从高斯分布：加速度和陀螺仪偏移服从随机游走模型：可得：

设定两个时间序列对应的图像帧为b_k和b_k+1，时间间隔为[t_k,t_k+1]，通过对式积分就计算出下一时刻的无人机的位置P、速度v和方向状态q；

其中：

Δt是时间间隔[t_k，t_k+1]；

等式左边的值都是世界坐标系(·)^ω下b_k+1帧的值，两边同时乘以从世界坐标到b_k帧坐标系的转换和从世界坐标系转换到机体坐标系b_k中：

其中和仅对与加速度和角速度有关的量进行积分：

当偏移变化特别小的时候，可以使用一阶线性展开来调整临时状态量

一开始将的值设为0；旋转设为单位旋转；噪声项n_a和n_ω未知并在整个过程把噪声设为0；用表示预积分项的估计值，将上式用欧拉积分展开可得：

其中，i是与IMU在[t_k,t_k+1]内测量值对应的离散时刻；δt是两个IMU测量值i和i+1之间的时隔；

由于四自由度旋转四元数被过参数化，将其误差项看为均值扰动定义式:

其中是三维小扰动；

之后可以推导出连续时间线性化的误差项：

可以通过初始协方差的一阶离散时间协方差重递归计算：

其中Q是噪声的对角协方差矩阵

关于的的一阶雅可比矩阵J_bk+1也可以用初始雅可比J_bk＝I递归计算：

J_t+δ_t＝I+F_tδtJ_t，t∈[k,k+1]；

传播状态值

其中，和J_bk+1中的子矩阵的对应于；也有同样的含义；

用相应的协方差来描述IMU测量模型：

考虑滑动窗口中两个连续帧b_k和b_k+1内的IMU测量，可以将预积分IMU测量的残差定义为：

其中[·]_xyz提取四元数q的矢量部分表示误差状态，表示四元数的三维误差，是在两个连续帧的时间间隔内仅使用有噪声的加速计和陀螺仪测量得的预积分的IMU测量项；加速度计和陀螺仪偏移也包括在线修正的残差项；

相机的残差模型，第i个图像中首先观察到的第l个特征，第j个图像中特征观察的残差被定义为：

其中，第一个式子是残差表达式；第二个式子是相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，由于视觉残差的自由度为2，可以将残差向量投影到切平面上，b₁，b₂是切平面上任意的两个正交基，是对第i个图像中的第l个特征的第一次观测，π_c ^-1是相机固有参数，P_l ^cj是在切空间中固定长度的标准协方差；第三个式子是重投影估计模型，是对第j个图像中相同特征的观察；

使用视觉-惯性光束平差公式；通过最小化先验和测量残差的马氏范数和，来获得最大后验估计：

其中，马氏范数的定义如下：

和分别是IMU和视觉测量的残差，β是IMU所有测量值的集合，C是在当前滑动窗口中观察到至少两次的特征点的集合，{r_p，H_p}是边缘化的先验信息，通过Ceres Solver解决这个非线性问题。

5.一种应用权利要求1～4任意一项所述基于滑动窗口的单目视觉惯性紧耦合方法的信息数据处理终端。