本发明涉及移动机器人同步定位与地图构建(SLAM)技术领域，尤其是适用于动态环境的基于鱼眼图像与深度图像的视觉里程计方法。

背景技术：

定位技术是移动机器人实现各项复杂任务的技术基础。里程计便是一类简单有效的定位方法，目前主要的里程计方法有轮式里程计和视觉里程计等。轮式里程计通过安装在车轮上的编码器，对各个车轮的行驶距离进行采集，根据轮式机器人运动学模型解算出机器人相对于初始位置的位姿。显然这种方法只适用于平面运动的轮式移动机器人，无法应用于腿式、轮腿式、履带式等结构形式的移动机器人。另外，由于轮子打滑、传动误差等因素的影响，造成轮式里程计的累计误差增大。视觉里程计采用视觉传感器，利用特征匹配、摄影几何、最优化等技术估计机器人的位姿，可以实现三维空间六自由度的运动估计，且不受轮子打滑的影响。鉴于上述优点，视觉里程计已经在水下机器人、无人机、腿式机器人、野外轮式机器人等领域得到了广泛的应用。

自从美国学者Nister于2004年提出视觉里程计的概念以来，涌现出了大量的视觉里程计算法。但是，大部分主流算法均具有一个很强的假设：机器人的运行环境是静态不变的。然而，实际环境中充满了各种各样运动的物体，如办公室内行走的职员、公路上运动的车辆等。这些运动物体，阻碍了传统算法在实际环境中的应用。

视觉里程计采用的相机有单目、双目、多目、RGB-D相机等。相比之下，RGB-D相机不仅能够采集彩色图像，还能够提供每个像素的深度信息，可以有效减少算法的复杂性，提高运动估计的精度。但是，大部分RGB-D相机的视场范围小。当相机进行快速旋转时，由于帧间图像重叠率低和运动模糊等原因，极易发生运动估计失败。

技术实现要素：

本发明的目的在于克服传统视觉里程计无法适应动态环境的缺点，结合鱼眼图像与深度图像的信息实现一种动态环境下的视觉里程计。

本发明所提出的一种融合鱼眼图像与深度图像的动态环境视觉里程计方法，同时使用鱼眼图像和深度图像的信息，由跟踪线程、地图构建线程和地图点滤波线程组成，三个线程并行运行。

本发明所提出的一种融合鱼眼图像与深度图像的动态环境视觉里程计方法，首先对地图进行初始化，将地图划分为关键帧和地图点两部分，每个地图点具有一个静态指数si，初始化为0；每个地图点具有一个用于标识该地图点为静态、动态或者未知的地图点属性。

所述跟踪线程包括以下步骤：

步骤1，同步采集鱼眼图像Ifn和深度图像Idn，构建成图像帧Fn；n＝2,3,4，……。

步骤2，对图像帧Fn中的鱼眼图像Ifn提取ORB特征集kn；

步骤3，将图像帧Fn中的特征集kn中所有特征点与前一图像帧Fn-1的特征集中指向静态地图点的特征点进行匹配，形成3D静态地图点到图像帧2D特征的匹配点对集合，采用RANSAC框架下的PnP算法解算出图像帧Fn的初始位姿

步骤4，将地图中所有的静态地图点投影到图像帧Fn，与图像帧Fn中的特征集kn进行匹配，形成3D静态地图点到2D图像特征ui之间的匹配点对集合{pw u}，利用鱼眼相机投影模型π，构建优化方程如下：

其中，i＝1,2,3,4……，表示匹配点对的个数，为图像帧Fn的位姿，为优化后的图像帧Fn的最优位姿。以步骤3得到的初始位姿为初值，利Levenberg-Marquardt方法得到图像帧Fn的最优位姿

步骤5，如果步骤4中匹配点对集合{pw u}中点对数量少于阈值Thm，或图像帧Fn与前一图像帧Fn-1的距离大于阈值Thd，则将图像帧Fn构建为关键帧，并加入关键帧队列。

所述地图构建线程包括以下步骤：

步骤A，从关键帧队列中取出关键帧KFn，由于深度相机的视场只占鱼眼相机视场的一部分，鱼眼图像中只有中间部分的像素能在深度图像中找到对应的深度，因此对鱼眼图像Ifn中间大小w×h像素区域的图像提取ORB特征；

步骤B，对步骤A中提取到的每个ORB特征，在深度图像Idn中寻找深度，计算对应的3D世界坐标，构建成地图点，初始化地图点的静态指数si＝0，属性为未知；

步骤C，对鱼眼图像Ifn中间w×h像素区域外的特征点，利用极线搜索的方法在其他关键帧上搜索配对特征，利用三角方法构建地图点的3D世界坐标，构建成地图点，初始化地图点的静态指数si＝0，属性为未知；

步骤D，将步骤B和步骤C中新构建的地图点，反投影到地图中其他的关键帧KFo的鱼眼图像上，计算KFo上投影点的ORB描述子计算描述子与关键帧KFn对应特征的描述子的汉明距离，j＝1,2,3,……，表示描述子个数。如果汉明距离少于阈值Thh，则静态指数加1，否则静态指数减1，如果静态指数大于阈值Ths，该地图点的属性变为静态，如果静态指数小于阈值Thdy，则地图点的属性变为动态；

步骤E，遍历地图中所有的关键帧，剔除与关键帧KFn共视特征点少于阈值Thc的关键帧；

步骤F，对地图中所有的静态地图点和关键帧，利用光束平差法进行全局优化。

所述地图点滤波线程包括以下步骤：

步骤(I)，将地图中每个属性为未知的地图点投影到图像帧Fn的鱼眼图像Ifn上，形成投影点

步骤(II)，对每个投影点计算ORB描述子计算描述子与地图点对应的描述子的汉明距离

步骤(III)，如果步骤(II)中汉明距离小于阈值Thh，则地图点的静态指数加1，否则静态指数减1，如果静态指数大于阈值Ths，则地图点的属性变为静态，如果静态指数小于阈值Thdy，则地图点的属性变为动态。

本发明的优点在于：

1.能够有效排除动态物体对视觉里程计的干扰，可在实际动态环境下运行。

2.相比于传统视觉里程计，在动态环境下的运动估计精度更高。

3.对快速运动、纯旋转等运动形式具有更强的稳健性。

4.利用鱼眼相机+深度相机的硬件配置方式，图像处理工作量小，效率高。

附图说明

图1为本发明的一种融合鱼眼图像与深度图像的动态环境视觉里程计方法流程图。

图2为本发明所采用的地图中地图点的构成示意图。

具体实施方式

下面将结合附图对本发明做进一步的详细说明。

如图1所示，本发明同时使用鱼眼图像和深度图像的信息，由跟踪线程、地图构建线程和地图点滤波线程组成，三个线程并行运行。

本发明采用的相机为Intel Realsense ZR300，该相机同时提供鱼眼图像和深度图像，鱼眼图像和深度图像的大小均为640×480像素。

本发明提供的一种融合鱼眼图像与深度图像的动态环境视觉里程计方法，包括地图初始化、跟踪线程、地图构建线程和地图点滤波线程四个部分，其中三个线程并行运行。本发明将地图划分为关键帧和地图点两部分，如图2所示，每个地图点具有一个静态指数si，以及一个用于标识该地图点为静态、动态或者未知的地图点属性。

系统的运行依赖于地图，地图初始化具体步骤如下：

步骤1，同步采集鱼眼图像If1和深度图像Id1，构建成图像帧F1；

步骤2，由于深度相机的视场只占鱼眼相机视场的一部分，鱼眼图像中只有中间部分的像素能在深度图像中找到对应的深度，因此对鱼眼图像If1中间大小w×h(如320×240像素)区域的图像提取ORB特征；

步骤3，对步骤2中提取到的每个ORB特征，在深度图Id1中寻找深度，计算对应的3D世界坐标，构建成地图点，初始化所述的地图点的静态指数si＝0，属性为静态。

所述跟踪线程的具体步骤如下：

步骤1，同步采集鱼眼图像Ifn和深度图像Idn，构建成图像帧Fn；n＝2,3,4,…；

步骤2，对图像帧Fn中的鱼眼图像Ifn提取ORB特征集kn；

步骤3，将图像帧Fn中的特征集kn中所有特征点与前一帧Fn-1的特征集中指向属性为静态的地图点的特征点进行匹配，形成3D静态地图点到图像帧2D特征的匹配点对集合，采用RANSAC框架下的PnP算法(常用方法有EPnP、P3P、UPnP等)解算出图像帧Fn的初始位姿

步骤4，将地图中所有的属性为静态的地图点投影到图像帧Fn，与图像帧Fn中的特征集kn进行匹配，形成3D静态地图点到2D图像特征ui之间的匹配点对集合{pw u}，利用鱼眼相机投影模型π，构建优化方程：

为图像帧Fn的位姿，以步骤3得到的初始位姿为初值，利Levenberg-Marquardt方法得到图像帧Fn的最优位姿

步骤5，如果步骤4中匹配点对集合{pw u}中点对数量少于阈值Thm(如100)，或图像帧Fn与前一图像帧Fn-1的距离大于阈值Thd(如0.2m)，则将图像帧Fn构建为关键帧，并加入关键帧队列。

所述地图构建线程包括以下步骤：

步骤A，从关键帧队列中取出关键帧KFn，对鱼眼图像Ifn中间大小w×h(如320×240像素)区域的图像提取ORB特征；

步骤B，对步骤A中提取到的每个ORB特征，在深度图像Idn中寻找深度，计算对应的3D世界坐标，构建成地图点，初始化地图点的静态指数si＝0，属性为未知；

步骤C，对鱼眼图像Ifn中间w×h像素(如320×240像素)区域外的特征点，利用极线搜索的方法在其他关键帧(关键帧队列中除关键帧KFn以外的所有关键帧)上搜索配对特征，利用三角方法构建地图点的3D世界坐标，构建成地图点，初始化地图点的静态指数si＝0，属性为未知；

步骤D，将步骤B和步骤C中新构建的地图点，反投影到地图中其他的关键帧的鱼眼图像上获得投影点，计算所述投影点的ORB描述子计算每一个描述子与关键帧KFn对应特征的描述子的汉明距离，如果汉明距离少于阈值Thh(如30)，则静态指数加1，否则静态指数减1。如果累加后的静态指数大于阈值Ths(如10)，该地图点的属性变为静态，如果静态指数小于阈值Thdy(如-3)，则地图点的属性变为动态；

步骤E，遍历地图中所有的关键帧，剔除与关键帧KFn共视特征点少于阈值Thc(如20)的关键帧；

步骤F，对地图中所有的静态地图点和关键帧，利用光束平差法进行全局优化。

所述地图点滤波线程包括以下步骤：

步骤I，将地图中每个属性为未知的地图点投影到图像帧Fn的鱼眼图像Ifn上，行成投影点

步骤II，对每个投影点计算ORB描述子计算描述子与地图点对应的描述子的汉明距离

步骤III，如果步骤II中汉明距离小于阈值Thh(如30)，则地图点的静态指数加1，否则静态指数减1，如果静态指数大于阈值Ths(如10)，则地图点的属性变为静态，如果静态指数小于阈值Thdy(如-3)，则地图点的属性变为动态。