[发明专利]基于IMU的SLAM运动模糊位姿跟踪算法

权利要求书

1.一种基于IMU的SLAM运动模糊位姿跟踪算法，其特征在于，包括如下步骤：

S1、对输入的图像序列进行ORB特征提取，根据提取到的特征点数目判断图像序列属于正常图像还是运动模糊图像，根据判断结果，择一进行S2或S3步骤；

S2、若判断结果为正常图像，利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整；

S3、若判断结果为运动模糊图像，利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，最后将估计位姿与优化位姿进行合并，得到相机的最终位姿；

S1中所述对输入的图像序列进行ORB特征提取，包括如下步骤：

S11、FAST角点检测，在图像中选取像素p，假设它的亮度为I_p，设置一个阈值T，以像素p为中心，选取半径为3的圆上的16个像素点，若选取的圆上有连续的N个点的亮度大于I_p+T或小于I_p-T，那么像素p则被认为是特征点，循环上述步骤，对每一个像素执行相同的操作；

S12、计算BRIEF描述子，利用特征点的主方向为BRIEF描述子增加旋转不变特性，计算特征点的主方向，计算公式为，

其中，m₀₁、m₁₀为图像块的矩，

所述BRIEF描述子为由0和1组成的向量，其中，0和1编码了关键点附近两个像素p和q的大小关系，若p大于q，则取1，反之取0；

S2中所述利用匀速运动模型估计相机初始位姿，然后执行运动参数的捆集调整，包括如下步骤：

S21、更新匀速运动模型中的速度矩阵，根据匀速运动模型，当前帧的初始位姿T_c＝VT_l，其中V为速度矩阵，T_l为上一帧的相机位姿；

S22、对世界坐标系中3-D点和图像中的关键点进行匹配，执行运动参数的捆集调整优化相机位姿，包括方向R∈SO(3)和位置t∈R³,针对匹配的世界坐标系中的3-D点Xⁱ∈R³以及关键点最小化它们之间的重投影误差，

其中，X为所有匹配点对的集合，ρ为鲁棒Huber代价函数，∑为与关键点尺度相关联的协方差矩阵，

投影函数π_s定义为，

其中，(f_x,f_y)为焦距，(c_x,c_y)为原点偏移量，b为基线；

所述估计位姿包括估计位置参数以及估计姿态参数，所述优化位姿包括优化位置参数以及优化姿态参数，所述最终位姿包括最终位置参数以及最终姿态参数；

S3中所述利用IMU运动方程得到估计位姿，然后利用扩展卡尔曼滤波器得到优化位姿，包括如下步骤：

S31、对惯性世界坐标系和IMU坐标系构建旋转矩阵，然后将该矩阵求导，得到旋转矩阵的微分方程，通过解微分方程，得到矩阵变量，最后转化成IMU的姿态四元数；

S32、通过坐标系之间的空间关系，能够利用IMU的位置、姿态得出相机的估计位姿；

S33、得到相机的估计位姿后，利用扩展卡尔曼滤波器对估计位姿进行优化，获得优化位姿；

所述S31具体包括如下步骤：

旋转矩阵C的表达式为，

其中，i，j，k为惯性世界坐标系的单位向量，i′，j′，k′为IMU坐标系的单位向量，对旋转矩阵C求导得到微分方程

解微分方程得到矩阵变量，并转化成IMU姿态四元数估计IMU位置，然后进行二次积分得到位移值，

其中，为惯性世界坐标系下IMU的位置，为其速度，g为世界坐标系下的引力向量；

所述S32具体包括如下步骤：

计算公式为，

其中，为估计位置参数，是由视觉坐标系到惯性世界坐标系的旋转，为IMU的姿态，n_p为相机位置测量噪声，为估计姿态参数，表示由IMU坐标系到相机坐标系的旋转；

所述S33具体包括如下步骤：

测量矩阵为，

其中，

和为线性化误差测量值的矩阵，

依据测量矩阵H，根据卡尔曼滤波过程更新估计值，由校正值计算得到优化位姿；

所述依据测量矩阵H，根据卡尔曼滤波过程更新估计值，具体包括如下步骤：

S331、计算余量计算公式为，

其中，

S332、计算新息S，计算公式为，S＝HPH^T+R，

其中，R为相机平移与旋转测量噪声的协方差矩阵；

S333、计算卡尔曼增益K，计算公式为，K＝PH^TS^-1；

S334、计算校正值计算公式为，

所述S3还包括，利用双目图像确定视觉尺度因子。