CN110490900B - 动态环境下的双目视觉定位方法及*** - Google Patents

Abstract

一种动态环境下的双目视觉定位方法和***，应用于机器人定位技术领域，提取图像特征点，通过对特征点聚类的方式区分运动物体点上特征点和非运动物体上特征点，这种方法可以有效剔除正在运动的物体上的特征点，避免剔除静止的动态属性物体上的特征点，进而保留更多可用特征点，通过IMU预积分算法得到的帧间位姿变换矩阵来筛选与IMU数据最接近的基于不同特征点集合估计的位姿，从而有效剔除一些外点比如属于运动物体的特征点；将单目的运动视觉与双目的立体视觉相融合的位姿与深度估计方法，可以有效提高估计的精度。

Description

动态环境下的双目视觉定位方法及***

技术领域

本发明涉及机器人定位技术领域，尤其涉及一种动态环境下的双目视觉定位方法及***。

背景技术

传统的GPS定位技术在对卫星信号依赖性强，在建筑群较多，遮挡严重的环境下精度大幅度下降，特别是在室内环境基本无法使用。基于惯导模块(陀螺仪，加速度计)的惯性导航技术不依赖外部信息，通过自身惯导测量信息的积分就可以得到载体当前的位姿信息，但是惯性导航定位误差随时间而增大，由于累积误差的存在，长期精度差。基于视觉的定位方法，则是通过对图像序列的分析来确定机器人的位姿，基于视觉的方法既可以用于室内等遮挡环境也可以利用视觉的丰富信息进行闭环检测消除累积误差实现长期高精度定位。基于视觉的的定位方法在机器人领域主要是基于视觉的同时定位与建图(SLAM，Simultaneous Localization and Mapping)，简称视觉SLAM，而不建立地图的视觉SLAM也被称为视觉里程计，这里我们统称为视觉SLAM。当前视觉SLAM根据传感器的不同分为单目SLAM、双目SLAM、RGB-D SLAM，由于单目SLAM存在尺度不确定性问题而RGB-D SLAM成本过高，精度较低，因此我们采用双目SLAM既不存在单目的尺度不确定性问题，成本也相对较低。目前SLAM定位技术大多都是基于静态环境假设，然而实际环境大多是复杂的动态场景，在动态场景中，这些基于静态假设的SLAM算法的精度大大降低，近几年一些基于语义的动态场景SLAM技术被提出用来，简称语义SLAM，语义SLAM通过深度学习或其他计算机视觉方法检测场景中具有动态属性的物体类别，在利用图像信息进行位姿估计前先剔除动态属性类别的物体，从而降低动态物体对位姿估计的影响。

但是这种方法有以下缺点：(1)是图像检测技术本身具有很高的计算复杂度，而定位技术对实时性具有很高的要求，在SLAM***中添加深度学习等框架会使的实时性受到很大的影响。(2)在SLAM***中，特别是不建立环境地图的视觉里程计，在利用图像信息进行位姿估计时，影响位姿估计精度的并不是具有动态属性的物体，而是正在运动的物体，例如停在停车场的汽车属于动态属性物体，但是该汽车可能会在停车场停留很久，至少不影响当前图像信息的位姿估计，反而为位姿估计提供更多的特征信息，特别是在特征稀少的场景，这种暂时静止的动态物体会提供更多的特征反而增加位姿估计的精确性。

发明内容

本发明的主要目的在于提供一种动态环境下的双目视觉定位方法及***。

为实现上述目的，本发明实施例第一方面提供一种动态环境下的双目视觉定位方法，包括：

获取相机双目视觉下的左目图像和右目图像，并提取所述左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，所述地图点为加入地图的特征点；

根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对，以及，立体视觉匹配特征点对；

筛选所述立体视觉匹配特征点对和运动视觉匹配特征点对的共有匹配特征点对；

分别对所述共有匹配特征点对集中的特征点对进行立体深度估计，得到立体深度估计值；

利用K-means++聚类算法，将所述左目和右目的运动视觉匹配特征点对按照运动视觉匹配特征点对之间的光流速度和旋转角进行聚类，得到聚类结果，并按照聚类结果将所述运动视觉匹配特征点对分为多个类别；

利用IMU预积分算法，计算所述左目和右目在当前帧与上一帧之间的时间段内的IMU的位姿估计结果，所述位姿估计结果包括所述当前帧与上一帧之间的帧间位姿；

对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果；

选取与IMU的位姿估计结果最相近的EPNP的位姿估计结果作为最佳EPNP位姿估计结果和地图点估计结果，并记录所述最佳EPNP位姿估计结果和地图点估计结果对应的有效匹配特征点对；

从所述共有匹配特征点对中选出所述有效匹配特征点对对应的特征点对，并对所述特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，得到估计位姿和估计地图点。

进一步地，所述IMU预积分算法：

其中，

分别表示在第k帧相机坐标系下从第k帧到k+1帧时间段内的位移增量，速度增量、以及旋转角度变化的四元数，a_t表示加速计测量值，w_t表示角速度测量值，b_at和b_wt分别表示加速度计和陀螺仪的偏置，n_a和n_w分别表示噪声，

表示时刻t相对于第k帧相机坐标系的旋转变换矩阵，可由

相关公式得到，Ω是与四元数计算相关的矩阵，Δt_k表示t_k到t_k+1的时间间隔，g^w表示在世界坐标系下的重力向量。

进一步地，所述按照聚类结果将所述运动视觉匹配特征点对分为多个类别之后，包括：

对各类别下的特征点的个数做直方统计；

对于各类别下对应的特征点的数量小于第一预设个数且与其它类别下的特征点的个数的差值大于第二预设个数的类别，删除所述类别，以及，所述类别下对应的特征点。

进一步地，所述对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果包括：

对各类别下特征点分别进行EPNP位姿估计和地图点估计，通过RANSAC随机采用一致性算法，对每个类别的位姿估计结果进行外点剔除，得到每个类别相应的EPNP的位姿估计结果和地图点估计结果。

进一步地，所述得到估计位姿和估计地图点之后，包括：

根据所述IMU的帧间位姿，利用基于图的优化算法，对所述估计位姿和估计地图点进行优化，得到优化后的地图点；

所述图的优化算法为：

其由

表示EPNP位姿估计得到的估计位姿变换矩阵，

表示IMU预积分算法得到的位姿变换矩阵，z^k+1表示相机图像的像素坐标，P^k表示在第k帧相机坐标系下地图点3D坐标，π_c(.)是基于相机的模型的投影变换，用于将相机坐标系下3D坐标变化为相机图像中的2D像素坐标。

进一步地，所述对所述估计位姿和估计地图点进行优化，得到优化后的地图点之后，包括：

利用所述优化后的地图点替换优化前的地图点，更新所述局部地图，剔除不属于所述局部地图的地图点；

比较当前帧与次新关键帧之间的共视特征点是否高于预设阈值；

若高于所述预设阈值，则利用当前关键帧替换最新关键帧；

若低于所述预设阈值，则将当前帧加入关键帧集合。

进一步地，所述根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对包括：

对所述左目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到左目的匹配特征点对；

对所述右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到右目的匹配特征点对；其中，所述左目和右目的运动视觉匹配特征点对包括所述左目的匹配特征点对和所述右目的匹配特征点对。

进一步地，所述根据所述特征点，得到所述左目和右目的立体视觉匹配特征点对包括：

分别对所述左目图像所属的当前帧中的特征点和所述右目图像所属的当前帧中的特征点分别进行ORB描述子匹配和SAD立体匹配，得到所述左目和右目的立体视觉匹配特征点对。

本发明实施例第二方面提供一种动态环境下的双目视觉定位***，包括：

主线程、位姿估计线程、回环线程和后端优化线程；

所述主线程，用于获取相机双目视觉下的左目图像和右目图像，并进行初始化，当初始化成功后，接收所述位姿估计线程或回环线程发送的重启信号，以根据所述重启信号，再次进行初始化；

所述位姿估计线程，用于获取相机双目视觉下的左目图像和右目图像，并提取所述左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，根据所述特征点，计算当前帧的估计位姿和估计地图点；

所述回环线程，用于检测回环帧，估计当前帧与所述回环帧之间的位姿变换矩阵，并以所述回环帧的位姿为基准，通过所述位姿变换矩阵对所述当前帧的位姿进行更新；

所述后端优化线程，用于根据IMU预积分算法的位姿估计结果以及当前帧与回环帧之间的位姿变换矩阵对所有帧的位姿进行优化。

进一步地，所述根据所述特征点，计算当前帧的估计位姿和估计地图点包括：

根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对，以及，立体视觉匹配特征点对；

筛选所述立体视觉匹配特征点对和运动视觉匹配特征点对的共有匹配特征点对；

分别对所述共有匹配特征点对集中的特征点对进行立体深度估计，得到立体深度估计值；

利用K-means++聚类算法，将所述左目和右目的运动视觉匹配特征点对按照运动视觉匹配特征点对之间的光流速度和旋转角进行聚类，得到聚类结果，并按照聚类结果将所述运动视觉匹配特征点对分为多个类别；

利用IMU预积分算法，计算所述左目和右目在当前帧与上一帧之间的时间段内的IMU的位姿估计结果，所述位姿估计结果包括所述当前帧与上一帧之间的帧间位姿；

对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果；

选取与IMU的位姿估计结果最相近的EPNP的位姿估计结果作为最佳EPNP位姿估计结果和地图点估计结果，并记录所述最佳EPNP位姿估计结果和地图点估计结果对应的有效匹配特征点对；

从所述共有匹配特征点对中选出所述有效匹配特征点对对应的特征点对，并对所述特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，得到估计位姿和估计地图点。

本发明提取图像特征点，通过对特征点聚类的方式区分运动物体点上特征点和非运动物体上特征点，这种方法可以有效剔除正在运动的物体上的特征点，避免剔除静止的动态属性物体上的特征点，进而保留更多可用特征点；通过IMU预积分算法得到的帧间位姿变换矩阵来筛选与IMU数据最接近的基于不同特征点集合估计的位姿，从而有效剔除一些外点比如属于运动物体的特征点；将单目的运动视觉与双目的立体视觉相融合的位姿与深度估计方法，可以有效提高估计的精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例提供的动态环境下的双目视觉定位方法的流程示意图；

图2为本发明一实施例提供的动态环境下的双目视觉定位方法的总框架示意图；

图3为本发明一实施例提供的动态环境下的双目视觉定位方法的主线程的流程示意图；

图4为本发明一实施例提供的动态环境下的双目视觉定位方法的位姿估计线程的流程示意图；

图5为本发明一实施例提供的动态环境下的双目视觉定位方法的位姿估计线程的流程示意图的局部放大图；

图6为本发明一实施例提供的动态环境下的双目视觉定位方法的位姿估计线程的流程示意图的局部放大图；

图7为本发明一实施例提供的动态环境下的双目视觉定位方法的位姿估计线程的流程示意图的局部放大图；

图8为本发明一实施例提供的动态环境下的双目视觉定位方法的特征点对相对速度与角度示意图；

图9为本发明一实施例提供的动态环境下的双目视觉定位方法的回环线程的流程示意图；

图10为本发明一实施例提供的回环检测示意图；

图11为本发明一实施例提供的后端优化流程示意图；

图12为本发明一实施例提供的为后端优化效果示意图。

具体实施方式

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本申请中，筛选运动物体的方法不仅仅适用于双目相机，对于单目、多目、RGB-D相机也同样使用，因此采用单目、多目、RGB-D相机也同样能达到本发明的目的。本申请实施例以视觉SLAM算法为例，但是对于激光传感器来说，基于本发明提出的方法筛选运动物体而非动态物体机制在激光SLAM中也能完成，因此在激光SLAM中采用对类似的特征进行聚类方法也能完成动态环境下提出运动物体的位姿估计。

请参阅图1，图1为本发明一实施例提供的动态环境下的双目视觉定位方法的流程示意图该方法主要包括：

S101、获取相机双目视觉下的左目图像和右目图像；

S102、提取该左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，该地图点为加入地图的特征点；

S103、根据该特征点，得到该左目和右目的运动视觉匹配特征点对，以及，立体视觉匹配特征点对；

S104、筛选该立体视觉匹配特征点对和运动视觉匹配特征点对的共有匹配特征点对；

S105、分别对该共有匹配特征点对集中的特征点对进行立体深度估计，得到立体深度估计值；

S106、利用K-means++聚类算法，将该左目和右目的运动视觉匹配特征点对按照运动视觉匹配特征点对之间的光流速度和旋转角进行聚类，得到聚类结果，并按照聚类结果将该运动视觉匹配特征点对分为多个类别；

S107、利用IMU预积分算法，计算该左目和右目在当前帧与上一帧之间的时间段内的IMU的位姿估计结果；

S108、对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果；

S109、选取与IMU的位姿估计结果最相近的EPNP的位姿估计结果作为最佳EPNP位姿估计结果和地图点估计结果，并记录该最佳EPNP位姿估计结果和地图点估计结果对应的有效匹配特征点对；

S110、从该共有匹配特征点对中选出该有效匹配特征点对对应的特征点对，并对该特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，得到估计位姿和估计地图点。

进一步地，该IMU预积分算法：

其中，

分别表示在第k帧相机坐标系下从第k帧到k+1帧时间段内的位移增量，速度增量、以及旋转角度变化的四元数，a_t表示加速计测量值，w_t表示角速度测量值，b_at和b_wt分别表示加速度计和陀螺仪的偏置，n_a和n_w分别表示噪声，

表示时刻t相对于第k帧相机坐标系的旋转变换矩阵，可由

相关公式得到，Ω是与四元数计算相关的矩阵，Δt_k表示t_k到t_k+1的时间间隔，g^w表示在世界坐标系下的重力向量。

进一步地，该按照聚类结果将该运动视觉匹配特征点对分为多个类别之后，包括：

对各类别下的特征点的个数做直方统计；

对于各类别下对应的特征点的数量小于第一预设个数且与其它类别下的特征点的个数的差值大于第二预设个数的类别，删除该类别，以及，该类别下对应的特征点。

进一步地，该对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果包括：

对各类别下特征点分别进行EPNP位姿估计和地图点估计，通过RANSAC随机采用一致性算法，对每个类别的位姿估计结果进行外点剔除，得到每个类别相应的EPNP的位姿估计结果和地图点估计结果。

进一步地，该得到估计位姿和估计地图点之后，包括：

根据该IMU的位姿估计结果，利用基于图的优化算法，对该估计位姿和估计地图点进行优化，得到优化后的地图点；

该图的优化算法为：

其由

表示EPNP位姿估计得到的估计位姿变换矩阵，

表示IMU预积分算法得到的位姿变换矩阵，z^k+1表示相机图像的像素坐标，Pk表示在第k帧相机坐标系下地图点3D坐标，π_c(.)是基于相机的模型的投影变换，用于将相机坐标系下3D坐标变化为相机图像中的2D像素坐标。

进一步地，该对该估计位姿和估计地图点进行优化，得到优化后的地图点之后，包括：

利用该优化后的地图点替换优化前的地图点，更新该局部地图，剔除不属于该局部地图的点；

比较当前帧与次新关键帧之间的共视特征点是否高于预设阈值；

若高于该预设阈值，则利用当前关键帧替换最新关键帧；

若低于该预设阈值，则将当前帧加入关键帧集合。

进一步地，该根据该特征点，得到该左目和右目的运动视觉匹配特征点对包括：

对该左目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到左目的匹配特征点对；

对该右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到右目的匹配特征点对；其中，该左目和右目的运动视觉匹配特征点对包括该左目的匹配特征点对和该右目的匹配特征点对。

进一步地，该根据该特征点，得到该左目和右目的立体视觉匹配特征点对包括：

分别对该左目图像所属的当前帧中的特征点和该右目图像所属的当前帧中的特征点分别进行ORB描述子匹配和SAD立体匹配，得到该左目和右目的立体视觉匹配特征点对。

在本发明实施例中，本发明在剔除正在运动的物体而非动态物体，通过聚类的方式来区分运动物体和非运动物体的方法，通过对帧间特征点对速度和角度进行聚类，从而对运动物体的匹配特征点集与非运动物体特征点集区分开来；通过IMU预积分算法得到的帧间位姿变换矩阵来筛选与IMU数据最接近的基于不同特征点集合估计的位姿，从而有效剔除一些外点比如属于运动物体的特征点；将单目的运动视觉与双目的立体视觉相融合的位姿与深度估计方法，可以有效提高估计的精度。

请参阅图2，图2是本发明又一实施例提供的动态环境下的双目视觉定位***的结构示意图，该***主要包括：

主线程、位姿估计线程、回环线程和后端优化线程；

该主线程，用于获取相机双目视觉下的左目图像和右目图像，并进行初始化，当初始化成功后，接收该位姿估计线程或回环线程发送的重启信号，以根据该重启信号，再次进行初始化；

具体的，如图3所示，***主线程的功能主要是开启其他线程和初始化地图，为后续其他线程提供初始可信的数据，主要流程与工作原理如下：

分别从双目相机左目和右目获取图像，利用FAST角点检测提取左目图像和右目图像的特征点，然后将图像划分为10个子图像区域，在每个子图像区域按照Harris响应值从大到小选取N个具有最大响应值的角点，作为最终的特征点集合。并且计算每个特征点的旋转方向，特征点的旋转方向定义为以改特征点为中心的图像块的几何中心到质心的方向向量。然后计算特征点的描述子，描述子采用的是与ORB(Oriented FAST and RotatedBRIEF)相同的二进制BRIEF描述子。

对左目图像特征点与右目图像特征点先进行ORB描述子匹配，然后进行SAD(Sumof absolute differences)亚像素级匹配，接着利用特征点旋转角一致性进行有效特征点筛选从而得到左右相机图像精确的特匹配特征点对。

利用双目相机模型

计算匹配特征点的真实深度，这里z表示在相机参考系下，特征点沿z轴方向的真实坐标，f表示相机的焦距，b为相机的基线，u_L、u_R分别为特征点在左目图像和右目图像中的像素横坐标。检测z是否为正，若为正则深度估计成功，还原该特征点的3D坐标，此3D坐标我们叫做地图点，不为正则是该特征点深度估计失败，剔除该特征点对。

如果成功匹配特征点对大于一定阈值，则认为初始化成功，否则重新进行初始化。

一旦初始化确认成功，则将地图点加入局部地图和全局地图类当中以及将当前帧添加为关键帧，并且将当前帧标记为上一帧，以便进行后续处理。局部地图是指当前图像帧以及与当前图像帧有共视关系的过去帧上所有匹配特征点对对应的地图点组成的地图点集合，而全局地图则是过去所有地图点组成的地图点集合。关键帧集合则是为了维护数据集规模，只将符合条件的图像帧保存到本地作为关键帧，为后续闭环检测和后端优化所使用。

整个***初始化成功之后，便进入轮询其他线程信号状态，若接收到其他线程发来的重启信号，便再次进入分别从双目相机左目和右目获取图像开始重新初始化***。

该位姿估计线程，用于获取相机双目视觉下的左目图像和右目图像，并提取该左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，根据该特征点，计算当前帧的估计位姿和估计地图点；

具体的，请参阅图4、图5、图6和图7，位姿估计线程的工作过程如下：

分别从双目相机的左目和右目获取图像，

分别从双目相机左目和右目获取图像，利用FAST角点检测提取左目图像和右目图像的特征点，然后将图像划分为10个子图像区域，在每个子图像区域按照Harris响应值从大到小选取N个具有最大响应值的角点，作为最终的特征点集合。并且计算每个特征点的旋转方向，特征点的旋转方向定义为以改特征点为中心的图像块的几何中心到质心的方向向量。然后计算特征点的描述子，描述子采用的是与ORB(Oriented FAST and RotatedBRIEF)相同的二进制BRIEF描述子。从预置惯导模块(IMU模块)读取加速度a_t和角速度w_t，并且对加速度与角速度做滤波处理。

其中，IMU惯导模块作为辅助手段用于动态环境下筛选合适的视觉定位结果，利用码盘、编码器、电子罗盘等能够得到机器人位移、速度、角速度、角度的传感器作为辅助手段一样也能完成本发明的动态环境下下配合视觉传感器的处理效果。

分别将左目和右目所属的当前帧特征点与所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，这里采用DBOW3词袋技术对特征点匹配进行加速，然后利用特征点旋转角一致性进行有效特征点筛选，分别得到左目匹配特征点对，和右目特征点对，这里我们称之为左目和右目的运动视觉匹配特征点对。将左目当前帧与右目当前进行ORB描述子匹配和SAD立体匹配优化得到更加精确的左目和右目立体匹配特征点对，我们称为立体视觉匹配特征点对，然后筛选出运动视觉匹配特征点对与提提视觉匹配特征点对的共有匹配特征点对集，对相应的特征点进行立体深度估计，还原特征点3D坐标。

计算左目和右目的运动视觉匹配特征点对之间的光流速度v和旋转角θ，光流速度与旋转角原理示意如图8所示，计算公式为：

利用K-means++聚类算法将左目和右目的运动视觉匹配特征点对按照光流速度和旋转角聚成M个类别。在实际环境中，运动物体的速度与方向与背景元素的速度与方向是有很大差别的，通过聚类可以将不同速度的运动物体与静止物体的特征点进行隔离，在得到聚类结果后，对每个类别的特征点数做直方图统计，筛去统计数量过小且与别的数据差距较大的类以及对应的特征点对。

利用IMU预积分技术计算左目和右目相机在上一帧与当前帧时间段内的帧间位移，IMU预积分原理核心公式如下

其中，

分别表示在第k帧相机坐标系下从第k帧到k+1帧时间段内的位移增量，速度增量、以及旋转角度变化的四元数，a_t表示加速计测量值，w_t表示角速度测量值，b_at和b_wt分别表示加速度计和陀螺仪的偏置，n_a和n_w分别表示噪声，

表示时刻t相对于第k帧相机坐标系的旋转变换矩阵，可由

相关公式得到，Ω是与四元数计算相关的矩阵，Δt_k表示t_k到t_k+1的时间间隔，g^w表示在世界坐标系下的重力向量。

利用得到的运动视觉匹配特征点集的不同的聚类，按照不同类别的匹配特征点集分别进行EPNP位姿估计和地图点估计，通过RANSAC随机采用一致性算法，对每个类别的位姿估计结果进行外点剔除。得到每个类别相应的位姿估计结果后，与IMU位姿结果进行比较，一般来说经过滤波优化的IMU短期位姿估计时准确的，因此利用IMU帧间位姿估计可以有效筛选真实的EPNP，排除运动物体特征点集的位姿估计结果，从而使得图像位姿估计结果更加真实精确，减少运动物体对估计结果的影响。通过结合IMU数据的筛选机制，我们可以得到较为准确的EPNP位姿和地图点估计结果，我们称为最佳EPNP位姿与地图点估计，对应的匹配特征点集称做运动视觉有效匹配特征点对集。

从共有匹配特征点集中筛选出属于有效匹配特征点对集的特征点，也就是说选出属于最佳EPNP位姿估计结果对应的特征点类的特征点对，然后对该特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，从而得到更加精确的地图点。

利用基于图的优化算法得到的位姿与地图点进行优化，优化的过程中融入IMU预积分得到的结果，将IMU预积分结果也作为优化约束条件之一，从而达到融合优化的效果，优化的核心公式如下：

其中

表示EPNP位姿估计得到的估计位姿变换矩阵，

表示IMU预积分算法得到的位姿变换矩阵，z^k+1表示相机图像的像素坐标，Pk表示在第k帧相机坐标系下地图点3D坐标，π_c(.)是基于相机的模型的投影变换，用于将相机坐标系下3D坐标变化为相机图像中的2D像素坐标。

在完成位姿估计之后，将正确的地图点加入局部地图与全局地图当中，对局部地图进行一次更新与替换，对于不在属于局部地图的点进行剔除，同时比较当前帧与次新关键帧之间的共视特征点是否高于阈值，如果高于阈值说明当前帧与最近的关键帧之间重合信息过多，不必要加入新关键帧，而是利用当前关键帧去替换最新关键帧，并且对局部地图点和全局地图点也进行部分替换与删除。如果低于阈值说明当前帧与最新关键帧之间信息差异较大，将当前帧加入关键帧集合以增加对环境的信息容量。

对与最新关键帧有足够共视特征点的关键帧的位姿以及与这些关键帧相关的地图点进行BundleAdjustment优化以及外点剔除。以及将上一帧以及与当前帧有共视关系的关键帧加入当前帧的共视图当中，更新数据图结构。然后进入下轮循环当中。

在上述过程中，都是考虑有上一帧地图点与当前帧足够匹配特征点的前提下进行的，如果匹配特征点不足，则需要补充特征点，补充特征点的方式主要有两种，一种是搜索局部地图，增加可匹配地图点，另一种则是对上一帧与当前帧重新进行特征匹配，匹配特征点范围不在局限于地图点对应的上一帧特征点，而是上一帧中所有特征点都参与匹配，从而有效弥补特征点空缺，一旦特征点满足需求则继续进行位姿估计，如果不满足需求，则利用IMU记录短期位姿变换以保持位姿轨迹的连续性，进入下一轮循环，直到找到足够特征点，如果连续很长时间没有找到足够特征点说明定位失效，此时IMU也因累积误差而不精确，需要重新初始化***。

该回环线程，用于检测回环帧，估计当前帧与该回环帧之间的位姿变换矩阵，并以该回环帧的位姿为基准，通过该位姿变换矩阵对该当前帧的位姿进行更新；

具体的，请参阅图9和图10，回环检测是指检测机器人是否回到曾经到过的地方，如果检测到回到曾经来过的地方，这可以建立起当前帧与回环帧之间的几何约束，通过优化消除从回环帧到当前帧这中间的累积误差。回环检测与特征匹配过程类似，主要采用DBOW3词袋加速，在过去所有关键帧中检测与最新关键帧有非常高的匹配率的帧，一旦检测到，该帧很可能就是回环帧，然后根据一系列筛选机制比如几何一致性、组匹配、时间一致性能方法确定是否为回环帧，一旦在关键帧集合中找到回环帧则估计当前帧与回环帧之间的位姿变换矩阵，对回环帧与当前帧之间的位姿变换矩阵以及当前帧的地图点进行BundleAdjustment优化以及外点剔除，最后以回环帧的位姿为基准通过回环帧与当前帧之间的位姿变换矩阵对当前帧的位姿进行更新，对与当前帧共视程度高的共视帧进行位姿进行更新，以及对相关帧相关的地图点进行更新，整个回环优化的作用消除部分累积误差，建立集合约束，为全局一致性优化提供约束条件。

该后端优化线程，用于根据IMU预积分算法的位姿估计结果以及当前帧与回环帧之间的位姿变换矩阵对所有帧的位姿进行优化。

具体的，请参阅图11和图12端优化线程是提高定位精度，减少全局累积误差的关键，在后端优化模块将对所有关键帧以及当前帧的共视帧以及全局地图点进行优化。整个优化流程又分为两大步，第一步位姿图优化，即只对关键帧的位姿进行优化，通过引入IMU预积分的帧间位姿约束以及当前帧与回环帧之间的约束对所有位姿进行优化，该步骤主要目的是融合IMU数据对关键帧的位姿进行优化，第二步全局一致性优化，在第一步的基础上回环优化为基础进一步消除累积误差，在回环约束下调整全局地图点和全部帧的位姿，从而减少轨迹漂移，保持全局一致性。

本发明改变现有处理动态环境的方式，通过剔除运动物体而非动态物体的方式，可以保留大量有效特征点，可以保留静止的动态物体特征点来提高位姿估计的精度，而且相比较剔除动态物体的方式，本发明可以适应极端情况下，比如场景中只有静止的动态物体，如果采用剔除动态物体的方式则无法获取足够特征点，而采用剔除运动物体的方法则可以保留静止的动态物体特征点用于位姿估计。

本发明改变现有基于深度学习的语义标签筛选的方式来应对动态环境的SLAM方法，提出一种对帧间匹配特征点对的速度和旋转方向进行聚类的方法区分运动物体与非运动物体(静态物体和静止的动态物体)，采用IMU数据作为辅助约束信息的方式，筛选不同聚类数据集位姿估计的结果中最接近IMU数据的位姿估计作为最佳位姿估计来剔除动态场景下运动物体对位姿估计的干扰，相比较基于深度学习的语义方式，大大降低了运算复杂度提高了定位算法的实时性。

另外，在本发明各个实施例中的各功能模块可以集成在一个处理模块中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。

以上为对本发明所提供的动态环境下的双目视觉定位方法及***的描述，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种动态环境下的双目视觉定位方法，其特征在于，包括：

获取相机双目视觉下的左目图像和右目图像，并提取所述左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，所述地图点为加入地图的特征点；

根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对，以及，立体视觉匹配特征点对；

筛选所述立体视觉匹配特征点对和运动视觉匹配特征点对的共有匹配特征点对；

分别对所述共有匹配特征点对集中的特征点对进行立体深度估计，得到立体深度估计值；

利用K-means++聚类算法，将所述左目和右目的运动视觉匹配特征点对按照运动视觉匹配特征点对之间的光流速度和旋转角进行聚类，得到聚类结果，并按照聚类结果将所述运动视觉匹配特征点对分为多个类别；

利用IMU预积分算法，计算所述左目和右目在当前帧与上一帧之间的时间段内的IMU的位姿估计结果，所述位姿估计结果包括所述当前帧与上一帧之间的帧间位姿；

对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果；

选取与IMU的位姿估计结果最相近的EPNP的位姿估计结果作为最佳EPNP位姿估计结果和地图点估计结果，并记录所述最佳EPNP位姿估计结果和地图点估计结果对应的有效匹配特征点对；

从所述共有匹配特征点对中选出所述有效匹配特征点对对应的特征点对，并对所述特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，得到估计位姿和估计地图点。

2.根据权利要求1所述的动态环境下的双目视觉定位方法，其特征在于，所述IMU预积分算法：

其中

分别表示在第k帧相机坐标系下从第k帧到k+1帧时间段内的位移增量，速度增量、以及旋转角度变化的四元数，a_t表示加速计测量值，w_t表示角速度测量值，b_at和b_wt分别表示加速度计和陀螺仪的偏置，n_a和n_w分别表示噪声，

表示时刻t相对于第k帧相机坐标系的旋转变换矩阵，可由

相关公式得到，Ω是与四元数计算相关的矩阵，Δt_k表示t_k到t_k+1的时间间隔，g^w表示在世界坐标系下的重力向量。

3.根据权利要求1所述的动态环境下的双目视觉定位方法，其特征在于，所述按照聚类结果将所述运动视觉匹配特征点对分为多个类别之后，包括：

对各类别下的特征点的个数做直方统计；

对于各类别下对应的特征点的数量小于第一预设个数且与其它类别下的特征点的个数的差值大于第二预设个数的类别，删除所述类别，以及，所述类别下对应的特征点。

4.根据权利要求3所述的动态环境下的双目视觉定位方法，其特征在于，所述对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果包括：

对各类别下特征点分别进行EPNP位姿估计和地图点估计，通过RANSAC随机采用一致性算法，对每个类别的位姿估计结果进行外点剔除，得到每个类别相应的EPNP的位姿估计结果和地图点估计结果。

5.根据权利要求4所述的动态环境下的双目视觉定位方法，其特征在于，所述得到估计位姿和估计地图点之后，包括：

根据所述IMU的帧间位姿，利用基于图的优化算法，对所述估计位姿和估计地图点进行优化，得到优化后的地图点；

所述图的优化算法为：

其中，

表示EPNP位姿估计得到的估计位姿变换矩阵，

表示IMU预积分算法得到的位姿变换矩阵，z^k+1表示相机图像的像素坐标，P^k表示在第k帧相机坐标系下地图点3D坐标，π_c(.)是基于相机的模型的投影变换，用于将相机坐标系下3D坐标变化为相机图像中的2D像素坐标。

6.根据权利要求5所述的动态环境下的双目视觉定位方法，其特征在于，所述对所述估计位姿和估计地图点进行优化，得到优化后的地图点之后，包括：

利用所述优化后的地图点替换优化前的地图点，更新局部地图，剔除不属于所述局部地图的地图点；

比较当前帧与次新关键帧之间的共视特征点是否高于预设阈值；

若高于所述预设阈值，则利用当前关键帧替换最新关键帧；

若低于所述预设阈值，则将当前帧加入关键帧集合。

7.根据权利要求1所述的动态环境下的双目视觉定位方法，其特征在于，所述根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对包括：

对所述左目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到左目的匹配特征点对；

对所述右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点进行ORB描述子匹配，得到右目的匹配特征点对；其中，所述左目和右目的运动视觉匹配特征点对包括所述左目的匹配特征点对和所述右目的匹配特征点对。

8.根据权利要求1所述的动态环境下的双目视觉定位方法，其特征在于，所述根据所述特征点，得到所述左目和右目的立体视觉匹配特征点对包括：

分别对所述左目图像所属的当前帧中的特征点和所述右目图像所属的当前帧中的特征点分别进行ORB描述子匹配和SAD立体匹配，得到所述左目和右目的立体视觉匹配特征点对。

9.一种动态环境下的双目视觉定位***，其特征在于，包括：

主线程、位姿估计线程、回环线程和后端优化线程；

所述主线程，用于获取相机双目视觉下的左目图像和右目图像，并进行初始化，当初始化成功后，接收所述位姿估计线程或回环线程发送的重启信号，以根据所述重启信号，再次进行初始化；

所述位姿估计线程，用于获取相机双目视觉下的左目图像和右目图像，并提取所述左目图像和右目图像所属的当前帧中的特征点和所属的上一帧中的地图点对应的特征点，根据所述特征点，计算当前帧的估计位姿和估计地图点；

所述回环线程，用于检测回环帧，估计当前帧与所述回环帧之间的位姿变换矩阵，并以所述回环帧的位姿为基准，通过所述位姿变换矩阵对所述当前帧的位姿进行更新；

所述后端优化线程，用于利用IMU预积分算法，计算所述左目和右目在当前帧与上一帧之间的时间段内的IMU的位姿估计结果，所述位姿估计结果包括所述当前帧与上一帧之间的帧间位姿，根据IMU预积分算法的位姿估计结果以及当前帧与回环帧之间的位姿变换矩阵对所有帧的位姿进行优化。

10.根据权利要求9所述的动态环境下的双目视觉定位***，其特征在于，所述根据所述特征点，计算当前帧的估计位姿和估计地图点包括：

根据所述特征点，得到所述左目和右目的运动视觉匹配特征点对，以及，立体视觉匹配特征点对；

筛选所述立体视觉匹配特征点对和运动视觉匹配特征点对的共有匹配特征点对；

分别对所述共有匹配特征点对集中的特征点对进行立体深度估计，得到立体深度估计值；

利用K-means++聚类算法，将所述左目和右目的运动视觉匹配特征点对按照运动视觉匹配特征点对之间的光流速度和旋转角进行聚类，得到聚类结果，并按照聚类结果将所述运动视觉匹配特征点对分为多个类别；

对各类别下运动视觉匹配特征点对分别进行EPNP位姿估计和地图点估计，得到EPNP的位姿估计结果和地图点估计结果；

选取与IMU的位姿估计结果最相近的EPNP的位姿估计结果作为最佳EPNP位姿估计结果和地图点估计结果，并记录所述最佳EPNP位姿估计结果和地图点估计结果对应的有效匹配特征点对；

从所述共有匹配特征点对中选出所述有效匹配特征点对对应的特征点对，并对所述特征点对对应的立体深度估计值与EPNP估计的深度值进行卡尔曼滤波融合，得到估计位姿和估计地图点。

Images