[发明专利]一种基于IMU辅助的单目视觉里程计位姿处理方法

摘要

本发明公开了一种基于IMU辅助的单目视觉里程计位姿处理方法。对单目相机采集的连续图像进行ORB特征提取，同时在相邻两帧图像的时刻之间对IMU数据进行预积分，根据提取到的特征点数目与设定阈值的关系分情况输出各时刻下的相机位姿：若特征点的数目大于设定阈值，则判断该图像为正常图像序列，采用纯视觉模型解出相机的运动位姿，然后利用图像的重投影误差，优化相机位姿；若特征点的数目小于设定阈值，则判断该图像为特征点缺失图像，采用惯性测量单元(IMU)预积分模型，获得特征缺失图像之间的相机位姿。本发明计算速度快，而且对运动具有鲁棒性并建图的准确性有所提高。

主权项

1.一种基于IMU辅助的单目视觉里程计位姿处理方法，其特征在于包括以下步骤：步骤1：将单目相机和惯性测量单元IMU模块固定连接同一平面构成相机‑IMU传感器，对相机‑IMU传感器进行标定得到单目相机和IMU模块的采样周期、相机内参K和t时刻世界坐标系到IMU坐标系的旋转矩阵根据t时刻世界坐标系到IMU坐标系的旋转矩阵可以得到t时刻IMU坐标系到世界坐标系的旋转矩阵所述的IMU模块包括三轴加速度计和三轴陀螺仪，三轴加速度计采集加速度测量值，三轴陀螺仪采集角速度测量值，建立IMU坐标系下的IMU动力学模型，具体如下：式中，a_t、b_at、n_a分别表示三轴加速度计在t时刻的加速度测量值、加速度真值、加速度偏置噪声、加速度高斯白噪声，g^w表示三轴加速度计在世界坐标系w下的加速度值，为9.8m/s²；ω_t、b_wt和n_ω分别表示三轴陀螺仪在t时刻的角速度测量值、角速度真值、角速度偏置噪声和角速度高斯白噪声；步骤2：单目相机连续采集图像，记录每帧图像的采集时间点，IMU模块输出每个采集时间点对应的IMU测量值，IMU测量值包括加速度测量值和角速度测量值；对于单目相机采集的每帧图像，利用ORB特征提取算法提取当前帧图像的特征点，设置特征点数量阈值，若当前帧图像所包括的特征点总数小于设定的特征点数量阈值，进入步骤3；若当前帧图像所包括的特征点总数大于设定的特征点数量阈值，进入步骤5；步骤3：根据IMU输出的加速度测量值和角速度测量值对单目相机采集的当前帧图像与当前帧的前一帧图像之间的加速度真值和角速度真值进行预积分，计算得到当前帧图像在世界坐标系下的IMU位姿：式中，t表示时刻，[T_k,T_k+1]表示单目相机前后两帧图像采集时间点之间的时间段，和分别表示在IMU模块在对应当前帧图像采样时间点b_k+1和前一帧图像采样时间点b_k下相对世界坐标系w的位移，和分别表示IMU模块在对应当前帧图像采样时间点b_k+1和前一帧图像采样时间点b_k下相对世界坐标系的速度，△t_k表示IMU的采样频率，和分别表示IMU模块在对应当前帧图像采样时间点b_k+1和前一帧图像采样时间点b_k下相对世界坐标系w的旋转角度的四元数，运算符为四元数的乘法表示，Ω表示四元数运算；根据IMU模块在对应当前帧图像采样时间点b_k+1下的位移得到IMU位姿的IMU平移矩阵，根据IMU模块在对应当前帧图像采样点b_k+1下旋转角度的四元数得到IMU位姿的IMU旋转矩阵，从而得到IMU位姿；步骤4：将当前帧图像的IMU位姿乘以旋转矩阵得到当前帧图像的相机位姿[R|t]，将得到的相机位姿[R|t]输出作为当前帧图像的最终输出位姿；返回步骤2对单目相机采集的下一帧图像的特征点总数进行判断；步骤5：对图像当前帧的特征点采用BRIEF算法计算描述子并与当前帧的后一帧图像进行特征匹配，从而得到前后两帧图像的匹配特征点对集，根据匹配特征点对集计算得到前后两帧图像的基础矩阵F，对基础矩阵F进行SVD矩阵分解得到当前帧图像的第一相机位姿，再根据基础矩阵F和相机内参K，计算得到单应矩阵E，对单应矩阵E进行分解得到当前帧图像的第二相机位姿；步骤6：分别计算第一相机位姿和第二相机位姿的重投影误差并优化获得对应的第一最小化重投影误差值和第二最小化重投影误差值，两个最小化重投影误差的计算方式相同，具体如下：其中，i表示特征点对的序号，n表示特征点对的总数，p'_i表示后一帧图像中与前一帧图像特征点p_i对应的匹配特征点，P_i表示前一帧图像特征点p_i对应的空间点，ξ^*表示相机位姿的最小化误差，ξ为相机位姿的李代数表示形式，s_i为图像的缩放因子，K为相机内参，ξ^表示相机位姿ξ的反对称矩阵，表示最小化二乘，表示最小化计算；步骤7：将第一和第二最小化重投影误差值进行比较判断，输出误差值最小的相机位姿作为当前帧图像的最终输出位姿；返回步骤3对后一帧图像的特征点总数进行判断，直至将单目相机采集的所有帧图像判断完毕。