本发明属于视觉里程计领域的一种视觉里程计方法，具体涉及一种面向动态环境的轮速计辅助的视觉里程计方法。
背景技术：
现有的视觉里程计方法都假设图像中的特征点为不运动的，然而当运动物体出现在场景中，视觉里程计的结果会受到干扰。与此同时，轮式机器人常装配轮速计，如何利用轮速计来帮助视觉里程计在动态环境下实现更鲁棒的定位具有重要意义。
技术实现要素：
本发明为解决现有算法在动态环境下鲁棒性差的问题，提出了一种面向动态环境的轮速计辅助的视觉里程计方法。
本发明采用的技术方案是包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数；
步骤二，预测相机位姿
根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿，位姿包括位置和姿态的关系；
步骤三，无效特征点去除
对上一时刻的灰度图像提取特征点，以特征点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用lk光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点；
步骤四，位姿优化
根据所有的有效特征点进行位姿的优化计算，重新利用pnp算法处理获得当前时刻相机的姿态和位置；
步骤五，对于每相邻两帧图像重复上述步骤二～步骤四，以排除动态特征点的干扰后进行位姿的计算，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
所述的步骤二中，当前时刻位姿的预测采用以下方式处理获得：
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
所述步骤三中，将上一时刻的地图点投影到当前时刻的灰度图像进行跟踪并根据重投影误差的大小进行阈值分割采用以下方式进行处理：
1)将上一时刻的地图点投影到当前时刻的灰度图像：
其中，pi表示上一时刻的特征点xi对应的地图点的3d位置，x′j表示投影后的像素坐标，k表示相机的内参数矩阵，s表示地图点在第j帧中的深度
然后采用lk光流法在当前时刻的灰度图像上的x′j附近针对xi进行跟踪，得到xj。
2)计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值tset，d＜tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值tset，d＜tset，则跟踪点不作为有效特征点。
本发明的有益效果是：
1.本发明采用的轮速计辅助的方法能够帮助预测当前时刻的相机位姿，相比于其他方法，本发明不要求对于动态运动物体的先验知识，只需要轮式机器人中常见的轮速计便可以实现鲁棒的视觉里程计。
附图说明
图1为系统流程图；
图2为静态有效特征点的检测示意图。
具体实施方式
下面结合附图对本发明的技术方案做进一步详细的说明。
如图1所示，本发明的具体实施例及其实施过程如下：
步骤一，获取深度相机拍摄的相邻时刻的图像。将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数。
步骤二，预测相机位姿。根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿，位姿包括位置和姿态的关系。匀速运动的运动模型如下，当前时刻位姿的预测采用以下方式处理获得：
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
步骤三，运动特征点去除。对上一时刻的灰度图像提取角点，以角点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用lk光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点。
如图2所示，具体如下：
1)将上一时刻的地图点投影到当前时刻的灰度图像：
其中，pi表示上一时刻的特征点xi对应的地图点的3d位置，x′j表示投影后的像素坐标，
然后采用lk光流法在当前时刻的灰度图像上的x′j附近针对xi进行跟踪，得到xj。
2)计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值tset，d＜tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值tset，d＜tset，则跟踪点不作为有效特征点。
步骤四，位姿优化。根据所有的有效特征点进行位姿的优化计算，重新利用pnp算法处理获得当前时刻相机的姿态和位置。
步骤五，对于相邻两帧图像重复上述步骤二～步骤四，以排除动态特征点的干扰后进行位姿的计算，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
由此实施可见，本发明能够利用轮式机器人常配备的轮速计，检测环境中的动态特征点，保留有效的静态特征点，更加鲁棒地对机器人进行定位。
1.一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于，包括以下步骤：
步骤一，获取深度相机拍摄的相邻时刻的图像
将深度相机固定安装在轮式机器人上，轮式机器人的轮子上安装有轮速计，采集到前一时刻和当前时刻的灰度图像和深度图像，并读取轮式机器人上当前时刻的轮速计读数；
步骤二，预测相机位姿
根据匀速运动情况，根据上一时刻和当前时刻的轮速计读数预测在当前时刻的轮式机器人位姿；
步骤三，无效特征点去除
对上一时刻的灰度图像提取特征点，以特征点对应的三维空间坐标作为上一时刻的地图点，根据预测所得的轮式机器人位姿将上一时刻的地图点投影到当前时刻的灰度图像上作为投影点，然后在投影点周围的固定大小区域范围内对上一时刻灰度图像提取的角点采用lk光流法进行跟踪获得跟踪点，计算投影点和跟踪点之间的重投影误差；利用重投影误差的大小进行阈值分割获得有效特征点；
步骤四，位姿优化
根据所有的有效特征点进行位姿的优化计算，重新利用pnp算法处理获得当前时刻相机的姿态和位置；
步骤五，对于每相邻两帧图像重复上述步骤二～步骤四，获得各个时刻的位姿，由各个时刻的位姿衔接作为结果，实现在动态环境下的视觉里程计。
2.根据权利要求1所述的一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于：所述的步骤二中，当前时刻位姿的预测采用以下方式处理获得：
其中，ωi和ωj表示前一时刻和当前时刻的轮速计角速度测量值，vi和vj表示前一时刻和当前时刻的轮速计速度测量值，
3.根据权利要求1所述的一种面向动态环境的轮速计辅助的视觉里程计方法，其特征在于：所述步骤三中，计算投影点x′j和跟踪点xj之间的像素距离d作为重投影误差，利用重投影误差的大小和预设的像素距离阈值进行比较：若重投影误差d小于像素距离阈值tset，d＜tset，则以跟踪点作为有效特征点；若重投影误差d大于等于像素距离阈值tset，d＜tset，则跟踪点不作为有效特征点。
技术总结