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

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

【技术实现步骤摘要】
一种融合惯导的动态视觉SLAM方法


[0001]本专利技术涉及动态视觉定位与建图方案领域，更具体地说是一种融合惯导的动态视觉SLAM方法。

技术介绍

[0002]实时精准的定位技术是无人驾驶汽车的关键技术之一，其提供了车辆在世界坐标系中的位置以及自身的朝向等关键信息。实时定位通常使用以下三种方法：1)使用卫星信号与惯性测量单元(IMU)的高精度组合导航设备；2)高精度地图对齐技术；3)同时定位与建图(SLAM，Simultaneous Localization And Mapping)技术。高精度组合导航设备虽定位结果稳定，但存在一些严重缺陷，例如卫星信号遮挡、硬件成本过高、算法黑盒子等问题。高精度地图对齐依赖于提前绘制的高精度地图，对环境变化的鲁棒性较差。因此，SLAM方法成为了满足实时精准定位需求的研究热点。SLAM研究方向已经出现了一些很不错的成果，包括基于视觉SLAM定位方案和基于激光SLAM定位方案。
[0003]常规的SLAM方法假设环境中所有物体均处于静止的状态，而一些能够在动态环境中运行的SLAM系统，只是将环境的动态物体视为异常值并将它们从环境中剔除，再使用常规的SLAM方法进行处理。这严重影响了SLAM技术在无人驾驶场景中的应用。常规的SLAM方法跟踪帧间的视觉几何特征，即点、线、面，然后通过束调整(BA，Bundle Adjustment)最小化重投影或光度误差。但是，除了环境中的这些低级要素之外，物体也是SLAM系统中未被很好探索的重要组成部分。物体对象的引入可以为BA调整阶段提供更多的约束条件。由此，近两年出现了对象级别的动态视觉SLAM的研究，然而它们仅仅使用相机作为传感器，车辆定位结果在运动速度过快，转弯或光线变化等场景依旧存在鲁棒性问题。

技术实现思路

[0004]本专利技术的目的是提供一种融合惯导的动态视觉SLAM方法，可以利用惯导与相机互补的优势提升SLAM方法的精度和鲁棒性；其定位精度和鲁棒性在KITTI数据集上均优于纯动态视觉SLAM方法。
[0005]本专利技术的目的通过以下技术方案来实现：
[0006]一种融合惯导的动态视觉SLAM方法，利用惯导IMU与相机互补的提升SLAM方法的精度和鲁棒性，该方法包括以下步骤：
[0007]S1：计算IMU位姿；
[0008]S2：鲁棒性的相机位姿估计；
[0009]S3：融合IMU的紧耦合优化；
[0010]所述计算IMU位姿为根据IMU积分获得IMU坐标系下的位姿、速度。
[0011]所述计算IMU位姿的步骤如下：
[0012]IMU的测量值为陀螺仪读数和加速度计读数测量时的加性噪声为高斯白噪
声，且偏置建模为随机游走，即导数服从高斯分布，则有：
[0013][0014][0015]其中，变量的角标a：IMU中的加速计；g：IMU中的陀螺仪；b：IMU坐标系；w：世界坐标系；q
bw
表示从世界坐标系到IMU坐标系的旋转四元数；g
w
为重力加速度；
[0016]k帧到k+1帧，IMU的位移变化速度变化姿态变化为：
[0017][0018][0019][0020]那么k+1时刻，IMU的位姿估计：
[0021][0022][0023][0024]所述鲁棒性的相机位姿估计的步骤如下：
[0025]从当前输入的图像中获取当前帧，并从图片中提取点特征，将相邻帧的特征点进行匹配，根据针孔相机模型和相邻帧匹配结果最小化视觉重投影误差来估计相机位姿；由此看出，良好的视觉条件是位姿估计成功的重要条件，在车辆运动过快或者光线变化的场景，无法获得足够的特征点；
[0026]进一步的，为了增强相机位姿估计的鲁棒性，在无法获得在足够视觉信息时，这时使用IMU积分获得的IMU位姿通过相机和IMU外参矩阵的转换来获得较好的相机位姿估计。通常以相机位姿估计结果为车辆的定位结果。
[0027]即：T
cw
＝T
cb
T
bw
；
[0028]其中，T
cw
：世界坐标系到相机坐标系的变换矩阵，即相机世界坐标系位姿；T
cb
：IMU坐标系到相机坐标系的变换矩阵，也称为IMU到相机的外参；T
bw
：世界坐标系到IMU坐标系下的变换矩阵，即IMU世界坐标系位姿；
[0029]变换矩阵T由旋转矩阵R和平移向量t构成，旋转四元数q是一种更紧凑的旋转运动表达方式，与旋转矩阵R可相互转换；
[0030]所述融合IMU的紧耦合优化的步骤如下：
[0031]设p
i,k
(i＝1,2,...,s；k＝1,2,...,n)为第i个静态特征点在第k帧图像中的2d关联，则重投影误差：
[0032][0033]其中f表示相机模型，为第k
‑
1帧观察到的第i个特征点在世界坐标系下的三维坐标值；T
k
为第k帧图片捕获时相机的世界坐标位姿；
[0034]设对象运动k
‑
1H
k
为第k
‑
1帧到k帧动态对象的相对运动，动态对象上的动态特征点在第k帧图像中的2d关联为p
j,k
(j＝1,2,...,d；k＝1,2,...,n)，则重投影误差为：
[0035][0036]进一步的为了获得相对实时的位姿，位姿优化采用一种基于滑动窗的紧耦合VIO进行高精度和鲁棒的状态估计；
[0037]设滑动窗口内有N帧，完整状态向量定义如下：
[0038]χ＝[y0,y1,
…
,y
N
,T
cb
,m1,m2,
…
,m
l
][0039][0040]T
cb
＝[p
cb
,q
cb
][0041]y
k
为第k帧图像被捕获时的IMU状态，包括世界坐标系下的位移、速度、姿态、加速度计偏置和陀螺仪偏置；l为窗口中的视觉静动态特征总数；
[0042]对滑动窗口内的状态向量进行非线性优化：
[0043][0044]其中p(s)为核函数；具体取值为：
[0045][0046]Σ
s
为静态三维点测量噪声协方差矩阵；Σ
b
为IMU噪声协方差矩阵；Σ
d
为动态三维点测量噪声协方差矩阵；n
b
是当前窗口中所有IMU测量值的集合；n
s
和n
d
是在当前窗口中至少被观察到两次的三维点的集合；{r
p
,H
p
}是边缘化的先验信息；
[0047]在完成所有相机和IMU数据的相关状态量估计后，统一进行视觉和IMU的全局优化来提升定位和建图的精度。
[0048]本专利技术的优点及有益效果如下本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合惯导的动态视觉SLAM方法，利用惯导IMU与相机互补的提升SLAM方法的精度和鲁棒性，其特征在于：该方法包括以下步骤：S1：计算IMU位姿；S2：鲁棒性的相机位姿估计；S3：融合IMU的紧耦合优化。2.根据权利要求1所述的一种融合惯导的动态视觉SLAM方法，其特征在于：所述计算IMU位姿为根据IMU积分获得IMU坐标系下的位姿、速度。3.根据权利要求1所述的一种融合惯导的动态视觉SLAM方法，其特征在于：所述计算IMU位姿的步骤如下：IMU的测量值为陀螺仪读数和加速度计读数测量时的加性噪声为高斯白噪声，且偏置建模为随机游走，即导数服从高斯分布，则有：且偏置建模为随机游走，即导数服从高斯分布，则有：其中，变量的角标a：IMU中的加速计；g：IMU中的陀螺仪；b：IMU坐标系；w：世界坐标系；q
bw
表示从世界坐标系到IMU坐标系的旋转四元数；g
w
为重力加速度。4.根据权利要求3所述的一种融合惯导的动态视觉SLAM方法，其特征在于：k帧到k+1帧，IMU的位移变化速度变化姿态变化为：为：为：那么k+1时刻，IMU的位姿估计：那么k+1时刻，IMU的位姿估计：那么k+1时刻，IMU的位姿估计：5.根据权利要求1所述的一种融合惯导的动态视觉SLAM方法，其特征在于：所述鲁棒性的相机位姿估计的步骤如下：从当前输入的图像中获取当前帧，并从图片中提取点特征，将相邻帧的特征点进行匹配，根据针孔相机模型和相邻帧匹配结果最小化视觉重投影误差来估计相机位姿。6.根据权利要求5所述的一种融合惯导的动态视觉SLAM方法，其特征在于：使用IMU积分获得的IMU位姿通过相机和IMU外参矩阵的转换来获得相机位姿估计。7.根据权利要求6所述的一种融合惯导的动态视觉SLAM方法，其特征在于：以相机位姿
估计结果为定位结果，即：T
cw
＝T
cb
T
bw
；其中，Tcw：世界坐标系到相机坐标系的变换矩阵，即相机世界坐标系位姿；Tcb：IMU坐标系到相机坐标系的变换矩阵，即IMU到相机的外参；Tbw：世界坐标系到IMU坐标系下的变换矩阵，即IMU世界坐标系位姿。8.根据权利要求7所述的一种融合惯导的动态视觉SLAM方法，其特征在于：变换矩阵T由旋转矩阵R和平移向量...

【专利技术属性】
技术研发人员：程绍武，彭忠翠，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：