基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质技术

本发明专利技术提出基于相机运动状态的双目

【技术实现步骤摘要】
基于相机运动状态的双目
‑
惯性融合的位姿估计方法、电子设备及存储介质


[0001]本申请涉及一种位姿估计方法，尤其涉及基于相机运动状态的双目
‑
惯性融合的位姿估计方法、电子设备及存储介质，属于位姿估计


技术介绍

[0002]同步定位与地图构建(SLAM,Simultaneous Localization and Mapping)一直是机器人技术和计算机视觉领域研究的热点之一。SLAM早期研究是为了实现移动机器人的自主定位和导航，为了能够在未知环境中完成智能化任务，移动机器人需要具备自身状态的认知能力以及周围环境感知能力。在面对一些更复杂的任务，比如在不受控制的条件下进行搜寻和救援任务时，机器人无法快速灵活地完成任务；同时机器人的运动受地形的影响，无法在一些特殊环境执行任务，因此仍需要训练有素的特种人员来执行。随着可穿戴式传感器的不断发展，基于可穿戴式传感器的辅助系统技术已经被引入到很多工业应用中。在上述背景下，可穿戴式SLAM系统的概念被提出。一套融合多传感器的可穿戴式SLAM系统可以在高度动态的条件下实现穿戴人员的精准实时定位，同时发送和记录环境信息，有效协助使用人员在未知环境下执行特殊任务。穿戴式计算平台与机器人平台不同，穿戴式计算平台受体积、重量等限制，通常计算能力非常有限，且对功耗更加敏感。
[0003]为此有研究人员提出了以下技术方案：
[0004]现有技术一、Campos和Elbira等学者又提出了ORB
‑
SLAM3，并设计了基于ORB
‑
SLAM3的可穿戴场景下的VI
‑
SALM系统，与VI
‑
ORB
‑
SLAM相比多了双目相机和IMU单元的视觉惯性融合支持。
[0005]该方法存在的缺点是该系统在后端优化模块特别是回环检测模块，无法在当设备长时间静态或者极小姿态运动时正确估计位姿，导致轨迹出现误差。
[0006]现有技术二、Viachaslau Kachurka等学者通过对视觉
‑
惯性SLAM(VI
‑
SLAM)与其它一些传感器之间的优化通信事件，实现了基于双目视觉
‑
IMU惯性的定位方法，并设计了一套可穿戴式协同SLAM系统。该系统的优点在于它能够同时执行两种互补的SLAM方法，以实现实时的位姿估计过程。
[0007]该方法存在的缺点是系统设计比较复杂，且硬件成本较高。

技术实现思路

[0008]在下文中给出了关于本专利技术的简要概述，以便提供关于本专利技术的某些方面的基本理解。应当理解，这个概述并不是关于本专利技术的穷举性概述。它并不是意图确定本专利技术的关键或重要部分，也不是意图限定本专利技术的范围。其目的仅仅是以简化的形式给出某些概念，以此作为稍后论述的更详细描述的前序。
[0009]鉴于此，为解决现有技术中存在的无法在当设备长时间静态或者极小姿态运动时正确估计位姿，导致轨迹出现误差的技术问题，本专利技术提供基于相机运动状态的双目
‑
惯性
融合的位姿估计方法、电子设备及存储介质
[0010]方案一：基于相机运动状态的双目
‑
惯性融合的位姿估计方法，包括以下步骤：
[0011]S1.获取摄像头拍摄的视频流中相邻两帧图像特征点对，匹配相邻两帧图像的特征点对；
[0012]S2.对相邻两帧图像中的IMU测量数据进行预积分处理，得到IMU信息位姿；
[0013]S3.基于相邻两帧图像的特征点对进行相机初始位姿估计；
[0014]S4.将S2所述IMU信息位姿与S3所述相机初始位姿进行融合；
[0015]S5.对融合后的相机初始化位姿进行紧耦合位姿优化；
[0016]S6.对优化后的位姿进行闭环检测与重定位，方法是：判断相机是否回到之前到过的某个位置，当形成回环时进行回环校正和全局优化以消除误差；
[0017]S7.基于相机运动状态设置关键帧筛选阈值；
[0018]S8.基于相机运动状态的双目
‑
惯性融合的位姿估计。
[0019]优选的，S1具体方法是：
[0020]S11.提取当前帧图像的FAST角点作为关键点，用BRIEF特征点描述算法计算关键点的特征描述向量，提取方法是：
[0021]S111.选取图像中任一像素点p，获取其亮度lp；
[0022]S112.设置阈值T，T设为像素点p亮度的20％；
[0023]S113.取像素点p点为圆心的半径为3的圆上的16个像素点；
[0024]S114.比较像素点p和16个像素点的亮度，若出现连续N个像素点的亮度与像素点p点亮度差超过阈值T，则认为像素点p点是特征点，否则，不认为像素点p点是特征点，执行S111；N取9或12；
[0025]S115.依次对图像中的所有像素点重复S111
‑
S114步骤，直至遍历完图像的所有像素点，得到多个特征点；
[0026]S12.采用RANSAC算法匹配相邻两帧图像的特征点对，方法是，包括以下步骤；
[0027]S121.将采集到的特征点作为样本，将其分为正确数据和异常数据；
[0028]S122.将正确数据作为内点；将异常数据作为外点；
[0029]S123.随机匹配一组子集作为内点，将模型拟合至该组子集上；形成拟合模型；
[0030]S124.利用拟合模型测试其他数据，形成共识集，利用共识集的所有成员对模型进行迭代获得最优模型参数值。
[0031]优选的，S2具体方法是：
[0032]对系统输入的当前帧图像b
k
和相邻帧图像b
k+1
，对IMU的测量模型积分可得位姿：
[0033][0034]其中，上标g表示gyro，上标a表示acc，上标w表示在世界坐标系world，上标b表示imu本体坐标系body；则表示下标帧在世界坐标系下的位置，同理表示当前帧在世界
坐标系下的位置；表示当前帧在世界坐标系下的速度，同理表示标帧在世界坐标系下的位置；Δt
k
表示两帧间隔时间；g
w
为世界坐标系下的重力；dt时间微分；表示下标帧在世界坐标系下的姿态，同理表示当前帧在世界坐标系下的姿态；R
w
表示方向余弦矩阵；
[0035]加速度计测量值陀螺仪测量值bias游走误差测量值b
u
b
y
，随机白噪声n
u
n
y
[0036]其中：
[0037]对加速度和角速度进行积分并将式(1
‑
1)转换到IMU坐标系得：
[0038][0039]式(1
‑
2)中表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的位置增量；表示当前本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于相机运动状态的双目
‑
惯性融合的位姿估计方法，其特征在于，包括以下步骤：S1.获取摄像头拍摄的视频流中相邻两帧图像特征点对，匹配相邻两帧图像的特征点对；S2.对相邻两帧图像中的IMU测量数据进行预积分处理，得到IMU信息位姿；S3.基于相邻两帧图像的特征点对进行相机初始位姿估计；S4.将S2所述IMU信息位姿与S3所述相机初始位姿进行融合；S5.对融合后的相机初始化位姿进行紧耦合位姿优化；S6.对优化后的位姿进行闭环检测与重定位，方法是：判断相机是否回到之前到过的某个位置，当形成回环时进行回环校正和全局优化以消除误差；S7.基于相机运动状态设置关键帧筛选阈值；S8.基于相机运动状态的双目
‑
惯性融合的位姿估计。2.根据权利要求1所述的基于相机运动状态的双目
‑
惯性融合的位姿估计方法，其特征在于，S1具体方法是：S11.提取当前帧图像的FAST角点作为关键点，用BRIEF特征点描述算法计算关键点的特征描述向量，提取方法是：S111.选取图像中任一像素点p，获取其亮度lp；S112.设置阈值T，T设为像素点p亮度的20％；S113.取像素点p点为圆心的半径为3的圆上的16个像素点；S114.比较像素点p和16个像素点的亮度，若出现连续N个像素点的亮度与像素点p点亮度差超过阈值T，则认为像素点p点是特征点，否则，不认为像素点p点是特征点，执行S111；N取9或12；S115.依次对图像中的所有像素点重复S111
‑
S114步骤，直至遍历完图像的所有像素点，得到多个特征点；S12.采用RANSAC算法匹配相邻两帧图像的特征点对，方法是，包括以下步骤；S121.将采集到的特征点作为样本，将其分为正确数据和异常数据；S122.将正确数据作为内点；将异常数据作为外点；S123.随机匹配一组子集作为内点，将模型拟合至该组子集上；形成拟合模型；S124.利用拟合模型测试其他数据，形成共识集，利用共识集的所有成员对模型进行迭代获得最优模型参数值。3.根据权利要求2所述的基于相机运动状态的双目
‑
惯性融合的位姿估计方法，其特征在于，S2具体方法是：对系统输入的当前帧图像b
k
和相邻帧图像b
k+1
，对IMU的测量模型积分可得位姿：其中，上标g表示gyro，上标a表示acc，上标w表示在世界坐标系world，上标b表示imu本
体坐标系body；则表示下标帧在世界坐标系下的位置，同理表示当前帧在世界坐标系下的位置；表示当前帧在世界坐标系下的速度，同理表示标帧在世界坐标系下的位置；Δt
k
表示两帧间隔时间；g
w
为世界坐标系下的重力；dt时间微分；表示下标帧在世界坐标系下的姿态，同理表示当前帧在世界坐标系下的姿态；R
w
表示方向余弦矩阵；加速度计测量值陀螺仪测量值bias游走误差测量值b
u
b
y
，随机白噪声n
u
n
y
其中：对加速度和角速度进行积分并将式(1
‑
1)转换到IMU坐标系得：式(1
‑
2)中表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的位置增量；表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的增量；表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的旋转矩阵变化将IMU的离散数据，改为欧拉积分进行处理，积分之后可得：式(1
‑
3)中、R、a
i
、δt、ω
i
、分别表示，表示i+1时刻位置增量的四元数，表示当前图像i+1帧与图像b
k
时刻的位置增量；表示i+1时刻速度增量的四元数，表示图像i帧与图像b
k+1
时刻的位置增量；表示i+1时刻旋转矩阵变化的四元数，表示帧图像i+1与图像b
k
时刻的速度增量；R表示方向余弦矩阵，a
i
表示i时刻加速度，δt表示时间增量，ω
i
表示i时刻角速度，表示bias游走误差；式(1
‑
3)成立的条件是IMU的偏置不会发生改变，系统实际运行时IMU的偏置是不断变
化的，因此，对积分后的离散数据进行优化，在迭代计算的过程中，预积分量中有一个与偏置有关的误差：式(1
‑
4)中δb
a
、δb
w
、分别表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的位置增量预测值，当前帧图像b
k
与相邻帧图像b
k+1
时刻的位置增量实际值，δb
a
为加速度增量，δb
w
为角速度增量，表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的速度增量；表示当前帧图像b
k
与相邻帧图像b
k+1
时刻的旋转矩阵变化；分别为预积分项相对于偏置误差的雅克比；建立线性高斯误差状态递推方程计算方程的协方差矩阵，同时计算相应的雅可比矩阵，假定测量值与实际值的误差为δ，则在t时刻误差项的导数为：简写为：进一步整理可得：由当前时刻的值可以计算出下一时刻的协方差：其中初始的协方差矩阵且有同时得到对应的雅克布矩阵迭代公式：
J
t+δt
＝(I+F
t
δt)J
t
(1
‑
10)。4.根据权利要求3所述的基于相机运动状态的双目
‑
惯性融合的位姿估计方法，其特征在于，S3具体方法是：通过S1所述相邻两帧图像的特征点对估计相机的运动，利用已知的n个3D空间点以及投影位置求解相机位姿，具体包括：定义世界坐标系下的n个3D点：p
i
i＝1,...,n(2
‑
1)定义世界坐标系下的4个控制点：c
j
,j＝1,...,4(2
‑
2)使用世界坐标系下的4个控制点的线性组合表示该世界坐标系下的任意三维点：其中，α
ij
表示其次重心坐标系数，表示世界坐标系下选取的控制点，而空间中三维点在相机坐标系中也适用(2
‑
3)关系：关系：表示相机坐标系选取的控制点；设(u
i
,v
i
)为3D
...

【专利技术属性】
技术研发人员：张展，左德承，杨荣鑫，冯懿，封威，刘宏伟，董剑，舒燕君，温东新，罗丹彦，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：