CN114964276B - 一种融合惯导的动态视觉slam方法 - Google Patents

Abstract

本发明涉及动态视觉定位与建图方案领域，更具体的说是一种融合惯导的动态视觉SLAM方法，利用惯导IMU与相机互补的提升SLAM方法的精度和鲁棒性，该方法包括以下步骤：S1：计算IMU位姿；S2：鲁棒性的相机位姿估计；S3：融合IMU的紧耦合优化；首先，根据IMU积分获得IMU坐标系下的位姿、速度；其次，在相机视觉条件不足，即车辆快速运动、物体遮挡、光线变化等，可使用IMU位姿数据提供车辆定位结果，增强动态视觉SLAM系统的鲁棒性；采用基于贝叶斯网络的因子图搭建融合优化算法，引入IMU预积分因子进行全局约束；通过在KITTI数据集上的实验结果对比，可知，本方法在精度上优于纯视觉动态SLAM。

Description

一种融合惯导的动态视觉SLAM方法

技术领域

本发明涉及动态视觉定位与建图方案领域，更具体地说是一种融合惯导的动态视觉SLAM方法。

背景技术

实时精准的定位技术是无人驾驶汽车的关键技术之一，其提供了车辆在世界坐标系中的位置以及自身的朝向等关键信息。实时定位通常使用以下三种方法：1)使用卫星信号与惯性测量单元(IMU)的高精度组合导航设备；2)高精度地图对齐技术；3)同时定位与建图(SLAM，Simultaneous Localization And Mapping)技术。高精度组合导航设备虽定位结果稳定，但存在一些严重缺陷，例如卫星信号遮挡、硬件成本过高、算法黑盒子等问题。高精度地图对齐依赖于提前绘制的高精度地图，对环境变化的鲁棒性较差。因此，SLAM方法成为了满足实时精准定位需求的研究热点。SLAM研究方向已经出现了一些很不错的成果，包括基于视觉SLAM定位方案和基于激光SLAM定位方案。

常规的SLAM方法假设环境中所有物体均处于静止的状态，而一些能够在动态环境中运行的SLAM系统，只是将环境的动态物体视为异常值并将它们从环境中剔除，再使用常规的SLAM方法进行处理。这严重影响了SLAM技术在无人驾驶场景中的应用。常规的SLAM方法跟踪帧间的视觉几何特征，即点、线、面，然后通过束调整(BA，Bundle Adjustment)最小化重投影或光度误差。但是，除了环境中的这些低级要素之外，物体也是SLAM系统中未被很好探索的重要组成部分。物体对象的引入可以为BA调整阶段提供更多的约束条件。由此，近两年出现了对象级别的动态视觉SLAM的研究，然而它们仅仅使用相机作为传感器，车辆定位结果在运动速度过快，转弯或光线变化等场景依旧存在鲁棒性问题。

发明内容

本发明的目的是提供一种融合惯导的动态视觉SLAM方法，可以利用惯导与相机互补的优势提升SLAM方法的精度和鲁棒性；其定位精度和鲁棒性在KITTI数据集上均优于纯动态视觉SLAM方法。

本发明的目的通过以下技术方案来实现：

一种融合惯导的动态视觉SLAM方法，利用惯导IMU与相机互补的提升SLAM方法的精度和鲁棒性，该方法包括以下步骤：

S1：计算IMU位姿；

S2：鲁棒性的相机位姿估计；

S3：融合IMU的紧耦合优化；

所述计算IMU位姿为根据IMU积分获得IMU坐标系下的位姿、速度。

所述计算IMU位姿的步骤如下：

IMU的测量值为陀螺仪读数和加速度计读数测量时的加性噪声为高斯白噪声，且偏置建模为随机游走，即导数服从高斯分布，则有：

其中，变量的角标a：IMU中的加速计；g：IMU中的陀螺仪；b：IMU坐标系；w：世界坐标系；q_bw表示从世界坐标系到IMU坐标系的旋转四元数；g_w为重力加速度；

k帧到k+1帧，IMU的位移变化速度变化姿态变化为：

那么k+1时刻，IMU的位姿估计：

所述鲁棒性的相机位姿估计的步骤如下：

从当前输入的图像中获取当前帧，并从图片中提取点特征，将相邻帧的特征点进行匹配，根据针孔相机模型和相邻帧匹配结果最小化视觉重投影误差来估计相机位姿；由此看出，良好的视觉条件是位姿估计成功的重要条件，在车辆运动过快或者光线变化的场景，无法获得足够的特征点；

进一步的，为了增强相机位姿估计的鲁棒性，在无法获得在足够视觉信息时，这时使用IMU积分获得的IMU位姿通过相机和IMU外参矩阵的转换来获得较好的相机位姿估计。通常以相机位姿估计结果为车辆的定位结果。

即：T_cw＝T_cbT_bw；

其中，T_cw：世界坐标系到相机坐标系的变换矩阵，即相机世界坐标系位姿；T_cb：IMU坐标系到相机坐标系的变换矩阵，也称为IMU到相机的外参；T_bw：世界坐标系到IMU坐标系下的变换矩阵，即IMU世界坐标系位姿；

变换矩阵T由旋转矩阵R和平移向量t构成，旋转四元数q是一种更紧凑的旋转运动表达方式，与旋转矩阵R可相互转换；

所述融合IMU的紧耦合优化的步骤如下：

设p_i,k(i＝1,2,...,s；k＝1,2,...,n)为第i个静态特征点在第k帧图像中的2d关联，则重投影误差：

其中f表示相机模型，为第k-1帧观察到的第i个特征点在世界坐标系下的三维坐标值；T_k为第k帧图片捕获时相机的世界坐标位姿；

设对象运动k-1H_k为第k-1帧到k帧动态对象的相对运动，动态对象上的动态特征点在第k帧图像中的2d关联为p_j,k(j＝1,2,...,d；k＝1,2,...,n)，则重投影误差为：

进一步的为了获得相对实时的位姿，位姿优化采用一种基于滑动窗的紧耦合VIO进行高精度和鲁棒的状态估计；

设滑动窗口内有N帧，完整状态向量定义如下：

χ＝[y₀,y₁,…,y_N,T_cb,m₁,m₂,…,m_l]

T_cb＝[p_cb,q_cb]

y_k为第k帧图像被捕获时的IMU状态，包括世界坐标系下的位移、速度、姿态、加速度计偏置和陀螺仪偏置；l为窗口中的视觉静动态特征总数；

对滑动窗口内的状态向量进行非线性优化：

其中p(s)为核函数；具体取值为：

Σ_s为静态三维点测量噪声协方差矩阵；Σ_b为IMU噪声协方差矩阵；Σ_d为动态三维点测量噪声协方差矩阵；n_b是当前窗口中所有IMU测量值的集合；n_s和n_d是在当前窗口中至少被观察到两次的三维点的集合；{r_p,H_p}是边缘化的先验信息；

在完成所有相机和IMU数据的相关状态量估计后，统一进行视觉和IMU的全局优化来提升定位和建图的精度。

本发明的优点及有益效果如下：

首先，根据IMU积分获得IMU坐标系下的位姿、速度；其次，在相机视觉条件不足，即车辆快速运动、物体遮挡、光线变化等，可使用IMU位姿数据提供车辆定位结果，增强动态视觉SLAM系统的鲁棒性；采用基于贝叶斯网络的因子图搭建融合优化算法，引入IMU预积分因子进行全局约束；通过在KITTI数据集上的实验结果对比，可知，本方法在精度上优于纯视觉动态SLAM。

附图说明

下面结合附图和具体实施方法对本发明做进一步详细的说明。

图1是本发明的全局优化因子图示意图；

图2是本发明的定位精度示意图；

图3是纯视觉动态SLAM的定位精度示意图；

图4是本发明的轨迹示意图；

图5是纯视觉动态SLAM的轨迹示意图。

具体实施方式

下面结合附图对本发明做进一步详细说明。

如图2和3所示，通过图2和图3的定位精度对比，可知本方法在精度上优于纯视觉动态SLAM；如图4和5所示，通过图4和图5的轨迹对比，可知本方法在鲁棒性上优于纯视觉动态SLAM；

如图1至5所示，为了解决“如何提升SLAM方法的精度和鲁棒性”这一技术问题，下面对一种融合IMU的视觉动态SLAM方法的步骤和功能进行详细的说明；

本发明所述的一种融合IMU的视觉动态SLAM方法，使用的动态视觉SLAM框架为VDO-SLAM，并将其实例分割算法替换成更先进的BlendMask算法，其次引入IMU数据的积分计算，估计IMU的平移、速度、姿态。再其次，当视觉条件不好时，用IMU位姿弥补相机定位。同时，当滑动窗口数达到20时，引入IMU状态变量结合动态视觉紧耦合优化，得到相对实时和准确的定位结果，最后，增加IMU的因子进行全局约束。

具体实施步骤如下：

S1：计算IMU位姿；

IMU的测量值为陀螺仪读数和加速度计读数假设测量时的加性噪声为高斯白噪声，且偏置建模为随机游走，即导数服从高斯分布。则有：

其中，变量的角标a：IMU中的加速计，g：IMU中的陀螺仪，b：IMU坐标系，w：世界坐标系，选择第一帧图片的坐标系作为世界坐标系原点。g_w为重力加速度，取值9.8m/s²。

IMU的噪声和随机游走数据由IMU标定给出，本文的IMU测量值分别来自KITTI数据集对齐相机后的矫正数据，频率为10Hz。

设k表示捕获到第k帧图片的时刻，则k帧到k+1帧，IMU的位移变化速度变化姿态变化为：

式中的ω和a使用欧拉法，选择k时刻的测量值来计算k到k+1时刻的 则k+1帧时，IMU的位姿估计：

S2：鲁棒性的相机位姿估计；

从当前输入的图像中获取当前帧，并从图片中提取ORB点特征，将相邻帧的特征点进行匹配，根据相机模型和相邻帧匹配结果最小化视觉重投影误差来估计相机位姿。本文使用针孔相机模型，即：

其中K为相机内参矩阵，T_cw为世界坐标系到相机坐标系的变换矩阵，即相机的外参。P_w为世界点，[u,v]是世界点在图片中对应的像素值，Z_c为世界点到相机光心的深度值。

在车辆运动过快或者光线变化的场景，无法获得足够的特征点，为了相机位姿估计的鲁棒性，这时使用IMU积分获得的IMU位姿通过相机和IMU外参矩阵的转换来获得较好的相机位姿估计。以相机位姿估计结果为车辆的定位结果。

即

T_cw＝T_cbT_bw

T_cw：世界坐标系到相机坐标系的变换矩阵，即相机世界坐标系位姿；T_cb：IMU坐标系到相机坐标系的变换矩阵，也称为IMU到相机的外参；T_bw：世界坐标系到IMU坐标系下的变换矩阵，即IMU世界坐标系位姿。变换矩阵T由旋转矩阵R和平移向量t构成，旋转四元数q是一种更紧凑的旋转运动表达方式，与旋转矩阵R可相互转换；

S3：融合IMU的紧耦合优化；

假设p_i,k(i＝1,2,...,s；k＝1,2,...,n)为第i个静态特征点在第k帧图像中的2d关联，则重投影误差：

f表示针孔相机模型，为第k-1帧观察到的第i个特征点在世界坐标系下的三维坐标值。T_k为第k帧图片捕获时相机的世界坐标位姿。

假设对象运动_k-1H_k为第k-1帧到k帧动态对象的相对运动，动态对象上的动态特征点在第k帧图像中的2d关联为p_j,k(j＝1,2,...,d；k＝1,2,...,n),则重投影误差为：

进一步的为了获得相对实时的位姿，位姿优化采用一种基于滑动窗的紧耦合VIO进行高精度和鲁棒的状态估计。假设滑动窗口内有N帧，选定N＝20。完整状态向量定义如下：

χ＝[y₀,y₁,…,y_N,T_cb,m₁,m₂,…,m_l]

y_k为第k帧图像被捕获时的IMU状态，包括世界坐标系下的位移、速度、姿态、加速度计偏置和陀螺仪偏置。l为窗口中的视觉静动态特征总数。由此可构建出最大后验估计问题，即列文伯格－马夸尔特法最小化所有误差函数，从而对滑动窗口内的状态向量进行非线性优化：

其中p(s)为核函数。具体取值为：

Σ_s为静态三维点测量噪声协方差矩阵，Σ_b为IMU噪声协方差矩阵，Σ_d为动态三维点测量噪声协方差矩阵。n_b是当前窗口中所有IMU测量值的集合，n_s和n_d是在当前窗口中至少被观察到两次的三维点的集合。{r_p,H_p}是边缘化先验信息。

在完成所有相机和IMU数据的相关状态量估计后，统一进行视觉和IMU的全局优化来提升定位和建图的精度，全局优化因子图如图1所示。

下面根据数据集实验对本发明的技术方案的定位精度对比验证。利用KITTI跟踪数据集评估算法在驾驶平稳和光线良好的室外驾驶场景中位姿估计结果，评估策略为通过比较数据集真值与算法输出位姿结果，求相机和跟踪对象位姿估计的相对误差均方根，如表1所示。同时，相机位姿估计对比结果如图2和3所示，图中rmse表示位姿误差均方根，mean表示位姿误差均值，median表示位姿误差中位数。轨迹误差对比如图4和5所示，reference表示真值的轨迹，彩色为相机位姿轨迹。KITTI跟踪数据集有21个序列，VDO-SLAM仅适用于刚性动态物体跟踪，因此以上数据序列排除了存在非刚性对象的序列。

数据集测试结果如下表所示：

表1融合IMU的动态视觉SLAM和动态视觉SLAM方法的定位误差：

由上表可知，在KITTI数据集实验中，得益于融合IMU的相机位姿估计和VIO紧耦合优化算法的双重作用，本算法较纯动态视觉SLAM方法较优，在视觉良好的条件下得到较为精准的定位结果。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进和润饰，这些改进和润饰均落入本发明权力要求书的保护范围。

Claims (2)

1.一种融合惯导的动态视觉SLAM方法，利用惯导IMU与相机互补的提升SLAM方法的精度和鲁棒性，其特征在于：该方法包括以下步骤：

S1：计算IMU位姿；

S2：鲁棒性的相机位姿估计；

S3：融合IMU的紧耦合优化；

所述计算IMU位姿为根据IMU积分获得IMU坐标系下的位姿、速度；

所述计算IMU位姿的步骤如下：

IMU的测量值为陀螺仪读数和加速度计读数测量时的加性噪声为高斯白噪声，且偏置建模为随机游走，即导数服从高斯分布，则有：

其中，变量的角标a：IMU中的加速计；g：IMU中的陀螺仪；b：IMU坐标系；w：世界坐标系；qb_w表示从世界坐标系到IMU坐标系的旋转四元数；g_w为重力加速度；

k帧到k+1帧，IMU的位移变化速度变化姿态变化为：

那么k+1时刻，IMU的位姿估计：

从当前输入的图像中获取当前帧，并从图片中提取点特征，将相邻帧的特征点进行匹配，根据针孔相机模型和相邻帧匹配结果最小化视觉重投影误差来估计相机位姿；

使用IMU积分获得的IMU位姿通过相机和IMU外参矩阵的转换来获得相机位姿估计；

以相机位姿估计结果为定位结果，即：T_cw＝T_cbT_bw；

其中，Tcw：世界坐标系到相机坐标系的变换矩阵，即相机世界坐标系位姿；Tcb：IMU坐标系到相机坐标系的变换矩阵，即IMU到相机的外参；Tbw：世界坐标系到IMU坐标系下的变换矩阵，即IMU世界坐标系位姿；

所述融合IMU的紧耦合优化的步骤如下：

设p_i,k(i＝1,2,...,s；k＝1,2,…,n)为第i个静态特征点在第k帧图像中的2d关联，则重投影误差：

其中f表示相机模型，为第k-1帧观察到的第i个特征点在世界坐标系下的三维坐标值；T_k为第k帧图片捕获时相机的世界坐标位姿；

设对象运动_k-1H_k为第k-1帧到k帧动态对象的相对运动，动态对象上的动态特征点在第k帧图像中的2d关联为p_j,k(j＝1,2,…,d；k＝1,2,…,n)，则重投影误差为：

位姿优化采用一种基于滑动窗的紧耦合VIO进行高精度和鲁棒的状态估计；

设滑动窗口内有N帧，完整状态向量定义如下：

χ＝[y₀,y₁,…,y_N,T_cb,m₁,m₂,…,m_l]

T_cb＝[p_cb,q_cb]

y_k为第k帧图像被捕获时的IMU状态，包括世界坐标系下的位移、速度、姿态、加速度计偏置和陀螺仪偏置；l为窗口中的视觉静动态特征总数；

对滑动窗口内的状态向量进行非线性优化：

其中p(s)为核函数；具体取值为：

Σ_s为静态三维点测量噪声协方差矩阵；Σ_b为IMU噪声协方差矩阵；Σ_d为动态三维点测量噪声协方差矩阵；n_b是当前窗口中所有IMU测量值的集合；n_s和n_d是在当前窗口中至少被观察到两次的三维点的集合；{r_p,H_p}是边缘化的先验信息。

2.根据权利要求1所述的一种融合惯导的动态视觉SLAM方法，其特征在于：变换矩阵T由旋转矩阵R和平移向量t构成，旋转四元数q是一种更紧凑的旋转运动表达方式，与旋转矩阵R可相互转换。