一种基于语义和几何的联合优化动态SLAM方法

摘要

本发明公开了一种基于语义和几何的联合优化动态SLAM方法，包括：将原始图像进行语义分割及几何分割得到二进制掩模maskseg与二进制掩模maskmvg；计算特征点权重；基于特征点权重确定语义和几何均为静态的特征点；初始化位姿；基于特征点权重对位姿进行优化求解；利用位姿优化的结果检测关键帧，利用关键帧进行局部建图及回环检测。与现有技术相比，本发明重点研究动态环境下的SLAM系统，综合分析几何分割和语义分割的优势与不足，将语义分割结果紧耦合进几何分割，提高动态特征点识别的准确率。此外，本发明提出了特征点权重的概念，基于动态特征点分割的结果初始化权重后，联合优化特征点权重和位姿，在优化相机位姿估计值的同时进一步权衡特征点的动态率。

说明书

技术领域

本发明涉及图像检测领域，具体涉及一种基于语义和几何的联合优化动态SLAM方法。

背景技术

视觉SLAM(同时定位与建图)是指在没有先验信息的情况下，利用搭载在机器人上的相机感知周围环境，在运动过程中建立环境模型，同时估计自身定位的技术。相机结构简单，安装方式多元化，无传感器探测距离的限制，应用范围广、成本低，可提取语义信息、地图信息丰富，视觉SLAM相对激光SLAM能获取更加准确和鲁棒的定位信息和环境地图。随着计算机硬件性能和图像处理相关算法的不断改进，近年来视觉SLAM以其独有的优点迅速发展，一系列应用视觉SLAM技术的产品也逐渐走进大众的视野中，如无人车、虚拟现实(VR)/增强现实(AR)、无人机、家用智能扫地机器人等等。

目前最广为人知的一些视觉SLAM，如ORB-SLAM2、LSD-SLAM等都是基于静态假设。但实际应用中必然会面对动态环境，如行走的人、行驶的汽车等，这些动态的物体会影响SLAM的特征点关联，进而降低运动估计的精度和算法的稳健性。为了处理动态物体，传统SLAM算法框架中常用随机抽样一致性检测(RANSAC)的方法迭代优化位姿，每次迭代过程中根据极线距离区分内点和外点，在后续位姿估计过程中将外点视为虚假数据滤除。但如果环境中动态物体占多数时，大多数的特征点位于动态物体上，RANSAC不能很好的区分出外点，进而影响估计的相机位姿。

因此，如何扩展静态假设下的SLAM以适应动态的环境成为了本领域技术人员急需解决的问题。

发明内容

本发明实际解决的技术问题为：如何扩展静态假设下的SLAM以适应动态的环境。

本发明采用了如下的技术方案：

一种基于语义和几何的联合优化动态SLAM方法，包括：

S1、将原始图像进行语义分割得到二进制掩模mask

S2、将原始图像进行几何分割得到二进制掩模mask

S3、基于二进制掩模mask

S4、提取原始图像中的特征点，基于特征点权重确定语义和几何均为静态的特征点；

S5、基于与原始图像对应的深度图及语义和几何均为静态的特征点初始化位姿；

S6、基于特征点权重对位姿进行优化求解；

S7、利用位姿优化的结果检测关键帧，利用关键帧进行局部建图及回环检测。

优选地，原始图像为RGB图像，原始图像的尺寸为m*n*3，步骤S1包括：

将原始图像输入Mask R-CNN，输出尺寸为m*n*l的矩阵，l为检测到的物体的个数，将每个物体的矩阵结合得到二进制掩模mask

优选地，步骤S2包括：

对原始图像进行特征点匹配；

去除与二进制掩模mask

基于剩余特征点求解矩阵F，并利用极线约束和矩阵F判别特征点为几何静态或几何动态；

生成二进制掩模mask

优选地，第i个特征点x

w

式中，(u,v)表示第i个特征点x

优选地，步骤S6中：

将特征点权重引入Bundle Adjustmet进行求解，目标函数为

式中，R

综上所述，本发明公开了一种基于语义和几何的联合优化动态SLAM方法，包括：将原始图像进行语义分割得到二进制掩模mask

附图说明

为了使发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：

图1为本发明公开的一种基于语义和几何的联合优化动态SLAM方法的流程示意图；

图2为Mask R-CNN架构示意图；

图3(a)及图3(b)分别为极线约束中连续帧间3D点和图像关系示意图及实际的极线约束示意图；

图4(a)和图4(b)分别为ORB-SLAM2获取的特征点及本发明的方法获取的特征点。

具体实施方式

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

如图1所示，本发明公开了一种基于语义和几何的联合优化动态SLAM方法，包括：

S1、将原始图像进行语义分割得到二进制掩模mask

S2、将原始图像进行几何分割得到二进制掩模mask

S3、基于二进制掩模mask

S4、提取原始图像中的特征点，基于特征点权重确定语义和几何均为静态的特征点；

S5、基于与原始图像对应的深度图及语义和几何均为静态的特征点初始化位姿；

S6、基于特征点权重对位姿进行优化求解；

S7、利用位姿优化的结果检测关键帧，利用关键帧进行局部建图及回环检测。

本发明中，始化位姿、关键帧检测以及关键帧检测后的步骤的实现均为现有技术，在此不再赘述。

动态SLAM问题的关键是如何精确检测动态物体并合理处理动态物体。图像语义分割以图像流作为输入，输出场景中物体的分类语义信息，同时检测物体的边缘。但语义分割并不是完美的，其一，语义分割不能实时运行；其二，物体的边缘检测不准确，部分边缘会被当做背景；其三，语义分割只能检测语义上动态的物体，而不能区分语义静态，实际动态的物体，如被扔出去的书。相反，几何分割刚好能弥补语义分割的缺点，实时运行且提供准确的物体边缘，同时几何分割也存在过分割的不足。

本发明采用语义信息和几何分割紧耦合的方式，将语义分割的结果作为先验信息，融合进几何分割的运算过程中，进一步提高定位的鲁棒性和精度。

另一方面，SLAM的位姿估计过程无差别对待每个特征点，通过最小化重投影误差和来优化位姿，对于存在潜在动态物体的环境来说，这样的处理策略可以进一步优化。因此，本发明的另一个创新点是为每个特征点设置权重值，根据前述语义和几何分割的结果设置初始值，将其和位姿估计一起作为联合估计的变量进行优化。

综上所述，与现有技术相比，本发明重点研究动态环境下的SLAM系统，综合分析几何分割和语义分割的优势与不足，将语义分割结果紧耦合进几何分割，提高动态特征点识别的准确率。此外，本发明提出了特征点权重的概念，基于动态特征点分割的结果初始化权重后，联合优化特征点权重和位姿，在优化相机位姿估计值的同时进一步权衡特征点的动态率。

具体实施时，原始图像为RGB图像，原始图像的尺寸为m*n*3，步骤S1包括：

将原始图像输入Mask R-CNN，输出尺寸为m*n*l的矩阵，l为检测到的物体的个数，将每个物体的矩阵结合得到二进制掩模mask

随着深度学习、计算机视觉等技术的深入研究，语义分割算法得到快速的发展。多层卷积神经网络可以自动学习抽取抽象的图像特征，在高维度上描述图像的本质，相对于传统图像分割准确率于效率都得到了较大提升。

本发明可采用Mask R-CNN框架实施语义分割，获取图像的先验信息。Mask R-CNN是一个非常灵活的框架，可以用于实现目标分类、目标检测、语义分割、实例分割等多个任务。Mask R-CNN以原始RGB图像作为输入，输出物体的掩模、物体检测框、分类ID。其中物体的掩模实际为二进制矩阵，区分了环境中哪些物体为潜在动态，如人、汽车、飞机、火车、猫、狗等。为得到最终的掩模，Mask R-CNN输出尺寸为m*n*l的矩阵，其中输入的原始图像尺寸为m*n*3，l为检测到的物体的个数，对于每个通道，其m*n的矩阵都为二进制掩模。通过结合这l个二进制掩模，可得到最终需要的m*n尺寸二进制掩模。

Mask R-CNN的网络结构是在经典目标检测算法Faster RCNN的基础上，融合进语义分割算法FCN来产生对应的Mask分支，使其可以获得高准确率。Faster RCNN作为目标检测领域的经典算法，利用一个神经网络将目标检测框架中关键模块：征提取网络、生成ROI、ROI分类、ROI回归融合，融合到一起。FCN是经典的语义分割算法，其总体架构是一个端到端的网络，首先对图像进行卷积和池化，使其特征图的尺寸不断减小，然后进行反卷积操作，不断增大特征图的尺寸，最后对每个像素值进行分类，从而实现对输入图像的准确分割。

图2为Mask R-CNN架构图，对于每个感兴趣区域(RoI)添加一个用于预测分割掩膜的分支。预测语义掩膜的分支是一个应用于RoI的小FCN，以像素到像素的的方式预测分割掩模。

具体实施时，步骤S2包括：

对原始图像进行特征点匹配；

去除与二进制掩模mask

基于剩余特征点求解矩阵F，并利用极线约束和矩阵F判别特征点为几何静态或几何动态；

生成二进制掩模mask

通过使用Mask R-CNN网络，大部分动态物体可以被分割出来，但分割的边缘准确性有待提高，同时一些静态语义但实际运动的物体无法被检测到，几何分割则刚好可以弥补上述两个问题。

极线约束可以用于判断一个特征点是静态或是动态，即静态的特征点一定会满足极线约束。极线约束可以表述为图3(a)和图3(b)，对于连续的两个图像帧I1和I2，C1和C2分别为两个图像帧的光心，C1和C2的连线被称为基线。3D点X在帧I1和I2上的投影分别为x1和x2。X和C1、C2决定的平面为极面π，π和图像平面I1和I2的交线为极线，即为图中的L1和L2。基线和图像平面I1和I2的交点为极点e1和e2。

如图3(b)中所示，假设已知I1中的特征点x1，试图找出I2中的对应的特征点x2，此时若深度信息未知，则只能推断出X点在C1,x1的延长线上，具体位置未知，由此可以推断出x2在I2的极线L2上相应的移动，称为对极约束。极限约束的物理意义是C1、X、C2共面，另外也描述了两个匹配点的空间位置关系，用基础矩阵F可表述为：

p2

其中，基础矩阵F是极线约束的关键，可表示为：

F矩阵有9个未知数，由于F的尺度缩放特性，其自由度可以被减少为8个，即只需要8对匹配的特征点就可以计算F。

求解F之后，根据是否满足极线约束可以判定图像帧的任意一对特征点x1、x2属于动态或静态。对于其次表达式下的特征点x1(u1,v1,1)、x2(u2,v2,1)，极线L1可以表示为：

则，帧I2中匹配点x2到对应极线L2的极线距离为：

上述利用极线约束的几何分割方法中，对于每个输入的帧，可以通过当前帧和上一帧的多对匹配到的特征点计算F矩阵，然后利用所得到的F矩阵计算像素点的极线距离来判定该特征点是否为动态。几何分割的关键问题在于如何选择成对的特征点，特征点匹配通常可以用光流法实现，但由于环境中存在动态物体，一旦选择的特征点位于动态物体上，后续估计F矩阵的准确性必然降低，进而导致无法准确识别物体的动静态。因此，本发明将语义分割的结果紧耦合进几何分割，提高F矩阵的估计准确性，从而确保几何分割的有效性。

语义分割之后，会得到m*n尺寸的二进制掩模mask

具体实施时，第i个特征点x

w

式中，(u,v)表示第i个特征点x

本发明中，w

在SLAM的位姿估计部分，提取特征点后对特征点对应的权重值进行判定，若权重值等于1则说明该点语义和几何均为静态，则将其用于后续位姿估计部分，若特征点权重小于1摒弃。

具体实施时，步骤S6中：

将特征点权重引入Bundle Adjustmet进行求解，目标函数为

式中，R

SLAM位姿估计可以用Bundle Adjustmet，即BA进行求解，这里BA是通过最小化图像帧中所有特征点的重投影误差总和来达到优化相机位姿的目的，即：

在这种做法中，所用的特征点，无论其运动状态如何都是平等的，对重投影误差的贡献一样大。而由于前述特征点动态检测不能保证百分百的准确，用于BA的特征点中难免会存在少量动态物体对应的特征点，该特征点的重投影误差会对位姿的估计产生一定的消极影响。

因此，本发明提出基于特征点权重BA的位姿估计算法，该算法引入特征点权重值w

联合优化相机的位姿和图像中特征点权重矩阵W，通过给特征点附加权重值合理分配该特征点对目标函数的贡献，提高位姿估计的精确度，另一方面优化后的权重矩阵W更精确的表明了该特征点的为动态物体的可靠性，取值越大则说明越可靠。

带权重BA问题求解可理解为联合BA问题，若将位姿用李代数ξ表示，首先需要计算每个误差项e对优化变量位姿ξ和特征点权重值w

为验证本发明公开的方法在动态环境下的可行性和有效性，本发明对所构建的算法在动态场景下进行位姿定位精度进行试验。实验运行平台配置为：CPU为Intel i7-8700K处理器，12核，主频为3.2GHz，内存为15.6GiB，使用GPU(GTX1080Ti)加速，操作系统为Ubuntu 16.04发行版。

本发明采取ORB-SLAM2作为对比基准，另外采取如下几个动态SLAM算法进行对比试验：

DynaSLAM：RGB-D数据集下，用几何分割补充语义分割产生的掩膜，取二者分割结果的并集作为最终分割结果。

DS-SLAM：将SegNet分割的像素级语义标签和极线约束相结合，二者均为动态时把对应的特征点看作动态并滤除。

Detect-SLAM:用物体检测网络SSD检测可移动的物体，不管实际运动状态如何，把落在该物体上的特征点删除。

为定量比较我们的系统在动态环境下对ORB-SLAM2框架的提升，我们采取绝对轨迹误差(Absolute Trajectory Error，ATE)RMSE作为我们的定量评价指标之一。实验中我们在每个数据集序列上进行5次实验，记录中值、平均、最大和最小RMSE结果。

此外，针对主流的动态SLAM算法对比试验中，除了用RMSE ATE之外，还采取相对旋转误差(Relative Rotation Error，RRE)和相对平移误差(Relative Translation Error，RTE)作为评价指标。

TUM RGB-D数据集由39个视频序列组成，这些序列是在不同室内环境下用MicrosoftKinect传感器采集的，采集的频率是30Hz。每个序列由RGB图像集、深度图集以及真实轨迹值三部分组成。以sitting为开头命名的序列中是两个人坐在桌子前聊天，以walking开头的序列是两个人前景和背景中走动，也会坐在桌子前面，相对而言walking开头的序列比sitting开头的序列动态率更好，环境更具有挑战。无论是sitting还是walking开头的序列都有四个类型：相机围绕着x-y-z轴运动的xyz序列、相机旋转的rpy序列、相机保持大致静态的static序列、以及相机在1m为直径的半圆轨迹上运动的halfsphere序列。

本发明在TUM RGB-D数据集进上分别进行定性和定量实验。首先，定性的观察和对比本发明的SLAM算法和ORB-SLAM2算法所选择的特征点，以动态率较高的walking-xyz序列中的特征点选择结果为例，图4(a)为ORB-SLAM2的特征点选择，图4(b)为本发明SLAM算法的选择结果。从图中可以看出，本发明公开的方法在特征点选择中能较好的区分动态物体，如图中运动的人，几乎所有的特征点均落在静态部分。

此外，本发明以RMSE ATE作为评价指标，在多个视频序列中分别用ORB-SLAM2和本发明SLAM算法重复进行5次实验。下表1中分别记录了ORB-SLAM2和本发明SLAM算法平均值、中值、最大和最小RMSE结果以及对应的提升百分比，表2中则分别记录了二者的RPE和RRE的结果。提升百分比η定义为：

其中o表示原始ORB-SLAM2的值，a表示本发明SLAM算法的值。

表1ORB-SLAM2和本发明算法的RMSE ATE对比

表2ORB-SLAM2和本发明算法的RPE、RRE对比

从表中可看出动态率较高的序列，如walking_xyz，walking_rpy等，本发明所提出的SLAM算法对原始ORB-SLAM2有相当大的提升。对于平均RMSE ATE指标而言，提升率最高可达到98.15％，最低也能达到93.2％，而对于RTE、RRE指标最高提升率分别为99.84％和97.80％。另外，我们也发现相对静态的序列，如表中的sitting_xyz序列，本发明的SLAM算法相对ORB-SLAM2来说表现稍差。分析原因，本发明认为是原始ORB-SLAM2算法能很好的应对静态环境，表现为RMSE ATE、RTE和RRE指标值均很小。而本发明提出的算法会摒弃语义上动态的特征点，在静态物体占大多数的环境中，这样的做法相对原始OEB-SLAM2会滤掉部分特征点，对定位精度有一定的消极影响。

此外，本发明将我们的SLAM算法和最先进的动态SLAM算法，即DynaSLAM、DS-SLAM和Detetect-SLAM等进行对比试验，下表3中记录中若干个序列中RMSE ATE结果对比。其中，DynaSLAM、DS-SLAM和Detetect-SLAM的RMSE ATE结果采用原论文中给出的数据。

表3本发明SLAM算法和最先进的SLAM算法RMSE ATE对比

对比分析表3，我们可以发现在动态率较低的环境中，即sitting_xyz序列，本发明SLAM算法的表现只比DS-SLAM差，和DynaSLAM算法相当且均优于Detetect-SLAM。分析其原因是DS-SLAM趋于将几何和语义分割中只有一种为动态的特征点进行保留，而本发明SLAM算法则趋于摒弃。在动态率较低的环境中，动态特征点的占比少，对定位精度的影响更小，因此更多的特征点在一定程度上意味着更高的定位精度。在动态率较高的剩下四个序列中，本发明算法在其中2个序列中表现最好，在walking_static序列中只比DynaSLAM略差，并且在这个序列上所有算法精度都较高。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管通过参照本发明的优选实施例已经对本发明进行了描述，但本领域的普通技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离所附权利要求书所限定的本发明的精神和范围。