一种室内动态场景下基于目标检测的语义视觉SLAM定位方法

一种室内动态场景下基于目标检测的语义视觉slam定位方法
技术领域
1.本发明属于人工智能与机器人和计算机视觉领域，具体涉及一种室内动态 场景下基于目标检测的语义视觉slam定位方法。


背景技术：

2.同步定位与地图构建(simultaneous localization and mapping,slam)是一种 应用在移动机器人上的技术，可使机器人在无环境先验信息的前提下，同时估 计自身运动状态并建立周围的环境模型。slam技术是人工智能与机器人以及 计算机视觉领域的重要研究内容，在室内服务机器人、自动驾驶等方面应用广 泛。针对室内动态场景下的定位与建图是slam技术研究的一个热门方向。
3.目前大多数视觉slam方法基于静态环境假设，而绝大多数现实环境中都 会含有动态物体，所以传统的slam方法很容易发生特征点跟踪错误导致特征 匹配的准确性降低，使位姿估计的误差不断累积。所以传统的slam方法在动 态环境下精度低、鲁棒性差。
4.现有应对动态环境的slam方法主要依赖特征点的几何信息，例如将前一 帧的特征点投影到当前帧，计算特征跟踪的重投影误差，如果误差很大，则被 认为是动态特征点；或者通过地图点之间的相关性来区分特征点的动静状态。 这类方法因为仅仅使用了几何信息，忽略了场景中物体的语义信息，所以无法 对周围环境有更高级的理解，从而导致系统在环境中的定位精度不够高。另一 类方法则主要依赖环境的语义信息，预先训练好用于识别环境中物体的神经网 络，通过目标检测或者语义分割获取动态物体的语义信息，剔除动态物体上提 取的特征点，但这类方法无法有效去除潜在动态物体上的不稳定特征点。
5.检测动态特征点是构建鲁棒视觉slam系统的关键步骤，动态特征点被准 确检测并剔除，才可以保证用于位姿估计的特征点均为静态特征点。由于室内 动态场景的构成十分复杂，可以通过一种先进的目标检测方法来检测环境中的 各个物体，获得动态对象的类别、位置及对应的类别概率，并通过对极几何约 束算法进一步识别潜在的动态对象，综合使用语义信息和几何信息来检测复杂 环境中的动态特征点，上述方法可显著提高检测的准确性并且保证整体slam 系统可以实时运行。


技术实现要素：

6.本发明的目的在于改善现有方法的缺陷，采用一种室内动态场景下基于目 标检测的语义视觉slam定位方法。该方法将目标检测技术与对极几何约束算 法相结合，首先使用yolov5s网络对图像中各先验物体进行语义识别，划分出 图像的动静区域，同时使用光流法跟踪提取到的特征点，然后通过对极几何约 束算法检测出其中的动态特征点。yolov5s目标检测算法可有效规避几何约束 对物体边缘特征点判断能力弱的缺点；同时几何约束也可以对yolov5s无法检 测到的潜在动态物体上所包含的动态特征点予以剔除，二者相互补充。保证用 于特征匹配和位姿估计环节的特征点均为静态特征点。本发明可以显著降低 slam系统的轨迹误差，同时也保证了系统的运行速度，可有效平衡好slam 系统位
姿估计的准确性、鲁棒性和快速性。
7.本发明提供了一种室内动态场景下基于目标检测的语义视觉slam定位方 法，包括：
8.本发明采用深度相机作为图像采集装置。
9.(1)对相机采集的图像序列利用目标检测网络将图像中的物体划分成静态物体 和动态物体，以动态物体所属的锚框作为图像的动态区域；
10.1.1)搭建yolov5s目标检测网络，该网络属单阶段的目标检测网络，分为 输入端、backbone、neck、输出端共4部分，该网络可进行自适应锚框计算和 自适应图片缩放，对不同大小的图片进行同一标准地缩放，标准的608*608*3 图像会输入focus结构，进行切片操作，变为304*304*12规格的特征图，经32 个卷积核的卷积操作，最终变为304*304*32规格的特征图；在backbone、neck 两个部分，yolov5s使用不同的csp(cross stage partial)跨阶段局部结构， 将特征图拆分，一部分用于卷积操作，一部分用于和以前的卷积结果关联；在 neck部分，利用fpn(feature pyramidnetwork)特征金字塔网络传递高层的语义 特征，利用pan(pathaggregationnetwork)路径聚合网络传递底层的定位特征； 在输出端，yolov5s使用ciou_loss作为边界框预测的损失函数，ciou_loss 的计算公式为：
11.ciou_loss＝1-ciou
ꢀꢀꢀꢀꢀꢀ
(1)
[0012][0013][0014]
其中iou代表预测框和真实框的交并比，distance_2代表两个中心点的欧氏距 离，distance_c代表识别对象最小外接矩形的对角线距离。
[0015]
1.2)利用ms coco数据集作为训练样本对yolov5s网络进行训练。mscoco数据集包含80种不同的物体类别，这足以包含室内环境中绝大多数的常 见物体。这些物体类别可分为三种：第一种是动态置信度高的移动对象，如人、 猫、狗等；第二种是静态置信度高的对象或潜在动态的移动对象，如椅子、书、 杯子、笔记本电脑等，第三种是绝对静态物体，如桌子、冰箱等。
[0016]
1.2.1)首先进行图片分类训练，使用默认的训练步长、批量大小和学习率。 利用训练数据集训练yolov5s网络的前20个卷积层、1个池化层和1个全连接 层，训练图像分辨率。
[0017]
1.2.2)利用预训练得到的网络参数初始化yolov5s网络的前20个卷积层， 然后进行整体模型训练，提高图像分辨率。
[0018]
训练结束后，进行检测效果的测试，到达满意程度即可用于完成slam系 统的语义分割任务。
[0019]
1.3)yolov5s网络为单阶段的目标检测网络，输入rgb图像，yolov5s 网络均匀地将图像平均分割，分割后的每个图像块独立进行物体检测并生成对 应的检测框，每个检测框含5个参数(x,y,w,h,c)，其中(x,y)代表检测框中心坐标， (w,h)代表检测框同图像整体的宽度和高度比值，c代表检测框预测物体类别的置 信度，这代表了检测框预测的准确程
度，最终识别的物体类别概率就是在该置 信度下产生的条件概率。yolov5s网络在图像的不同位置进行密集抽样，抽样 时可以采用不同尺度和长宽比，然后利用cnn提取特征，直接进行分类与回 归，输出所有先验物体的类别信息。在筛选多目标框时使用非极大值抑制获得 置信度最高的边界框。对于动态物体，会去除框内所有特征点；对于静态物体， 检测框内的特征点全部保留；对于潜在动态物体上提取的特征点需要配合对极 几何约束综合检测。
[0020]
(2)由于目标检测算法无法去除潜在动态物体所包含的特征点，因此需要利用 对极几何约束进一步检查这些来自潜在动态物体的特征点是否属于动态特征点， 具体步骤如下：
[0021]
2.1)对输入的图像序列提取orb特征点；
[0022]
2.2)采用光流法在不同图像帧中跟踪所提取到的特征点，具体步骤如下：
[0023]
2.2.1)将图像序列中的各帧图像视为时间f(t)的函数，t时刻坐标(x,y)的特征 点灰度值为f(x,y,t)与t+dt时刻的后续帧中匹配特征点的灰度值f(x+dx,y+dy,t +dt)基于灰度不变假设应符合：
[0024]
f(x,y,t)＝f(x+dx,y+dy,t+dt)
ꢀꢀꢀꢀꢀꢀꢀ
(4)
[0025]
经泰勒展开可得到：
[0026][0027]
计算公式(5)可得：
[0028][0029][0030]
其中的和是特征点灰度值沿x轴和y轴的梯度变化，和是特征点沿x轴 和y轴的速度矢量。令可以得到：
[0031][0032]
光流法假设同一子图像内像素点的运动相同，基于这样的假设可计算得到u和v， 也即得到后续帧中的位移变化dx和dy，最终实现特征点跟踪。
[0033]
2.2.2)根据前一帧特征点集合p1，利用光流法跟踪到当前帧中匹配的特征点 集合p2，如果匹配对太靠近图像的边缘，或者匹配对中心的3
×
3图像块的像素 差太大，则匹配对将被丢弃。
[0034]
2.3)使用经典的八点法估计基础矩阵f；
[0035]
2.4)假设p1和p2为不同帧中一对匹配的特征点，它们的齐次坐标如下所示：
[0036][0037]
利用基础矩阵和特征点坐标可得到极线i1：
[0038][0039]
2.5)通过比较p2到p1对应极线i1的距离与预定阈值的大小来判断特征点是否 属于动态特征点，其中点p2到极线i1之间的距离d可通过下式计算：
[0040][0041]
当距离d大于预定阈值，则将p2视为动态特征点，否则将其视为相对稳定的静态 特征点，该特征点可被用于后续的位姿估计。
[0042]
(3)跟踪线程确定关键帧后，会生成新的关键帧。通过插入关键帧、管理地图 点、局部集束优化、筛选局部关键帧进行局部建图，具体步骤如下：
[0043]
3.1)将新的关键帧作为新的节点加入图中，更新与关键帧节点相连接的边以 及关键帧的生长树，并计算表示关键帧的词袋；
[0044]
3.2)为了保留有效的地图点，如果能观察到该地图点的关键帧少于三帧，当 前地图点将被删除；
[0045]
3.3)对于未匹配的点，可与其他关键帧中未被匹配的特征点进行匹配，orb 特征点对三角化，检查正向景深、视差、反投影误差和尺度一致性后，创建新 的地图点；
[0046]
3.4)局部集束调整可优化当前处理的关键帧；
[0047]
3.5)为了实现准确地构建地图，通过筛选去除冗余的关键帧，若某关键帧中 90％的点可被另外超过三个关键帧观察到，则该帧被视为冗余关键帧，应予以删 除。
[0048]
(4)由于视觉里程计在跟踪的过程中会不断地累计定位误差，闭环检测可通过 图像匹配判断当前场景是否曾经来过，通过闭环优化，可将累计误差缩小到合 理的范围内，具体步骤如下：
[0049]
4.1)检测图像队列中是否有新的关键帧，若关键帧的数目小于10，则不必进 行闭环检测，反之则进入闭环检测线程；
[0050]
4.2)检测闭环：
[0051]
4.2.1)获取共视关键帧，并计算共视关键帧和当前关键帧之间的词袋分数， 获取最低分；
[0052]
4.2.2)通过获取到的最低分，到数据库中查找候选关键帧，这就相当于找 到了曾经到过当前位置的关键帧；
[0053]
4.2.3)对候选关键帧集进行连续性检测。
[0054]
4.3)如果上一步已经检测到了闭环，需要对闭环进行优化。主要工作是在当 前关键帧和闭环帧之间找到更多的对应点，通过这些对应点计算当前关键帧和 闭环帧之间的sim3变换，求解得到rt和s；
[0055]
4.4)计算闭环、融合位图。
附图说明
[0056]
图1为本发明方法的流程图。
[0057]
图2为本发明方法中物体检测及去除对应特征点的效果图。
[0058]
图3为本发明方法使用tum数据集的walking_xyz序列进行位姿估计的绝对误差轨迹图。
[0059]
图4为本发明方法使用tum数据集的walking_xyz序列进行位姿估计的相对误差轨迹图。
具体实施方式
[0060]
图1为本发明方法的流程图。参照图1，本发明提供一种室内动态场景下基于目标检测的语义视觉slam定位方法。整体系统包含跟踪、语义分割、局部建图、闭环检测共四个线程。原始的图像帧采集后同时进行语义分割线程的目标检测环节和跟踪线程中提取orb特征点，yolov5s将获取语义动态物体，跟踪线程对提取到的orb特征点进行跟踪和几何约束判断。yolov5s和几何约束共同将提取的orb特征点中的动态特征点进行精确剔除，使用剩余的静态特征点进行后续位姿估计。通过生成新的关键帧、插入关键帧、管理地图点、局部集束调整、筛选局部关键帧完成局部建图线程，通过闭环检测线程判断当前位置是否曾经来过，经过闭环优化，将累计误差缩小到合理范围。
[0061]
训练yolov5s目标检测网络时，在图像分类实验中设定步长为8,000,000，批大小设置为128，采用多项式衰减学习率调度策略，初始学习率为0.1。在整体模型训练时，批大小设置为64。
[0062]
tum数据集是评估rgb-dslam系统基准的常用数据集，该数据集含39个序列，每个序列含彩色图、深度图和相关的地面真实相机位姿。图像的采集分辨率为640*480，记录频率为30hz。walking序列是tum数据集的动态序列。采用walking序列对设计的slam系统进行实验时，yolov5s目标检测网络可自适应640*480分辨率的图像，经过检测可以识别所有的先验物体，划分出图像的动静区域，动态区域的特征点被全部删除，潜在的动态区域需要跟踪线程的对极几何进一步判断特征点的动静状态。图2显示检测场景中的物体并去除动态特征点的效果，因为人是动态物体，所以其检测框内的特征点均为动态特征点，而被全部剔除，而显示器等静态物体，其检测框内的特征点都被保留。
[0063]
图1显示输入图像帧后分别进入语义分割线程和跟踪线程，两个线程间的通信采用socket套接字。语义分割线程会返回一帧中各个物体的位置信息和预测概率，经过检测，语义分割线程每帧的检测数据可一次传输到跟踪线程，且传输的速度可满足实时性。在动态特征点被完全剔除后，保留的静态特征点可进行精确的位姿估计。而后进行的局部建图线程和闭环检测线程具体步骤已经在发明内容的步骤3和步骤4进行详细的说明。
[0064]
衡量slam系统定位精度的常用指标为相对位姿误差(relativeposeerror,rpe)和绝对轨迹误差(absolutetrajectoryerror,ate)。rpe可测量固定时间间隔内估计运动和真实运动之间的姿态变化量差值，ate通过比较估计运动和真实运动的绝对距离，可评估全局一致性。rpe需要同时考虑平移误差和旋转误差，ate只考虑平移误差。若定义时间步长为i，时间间隔为δ，以pi∈se(3)表示第i帧的估计位姿，以qi∈se(3)表示第i帧的真实位姿，则相对位姿误差计算为：
[0065][0066]
绝对位姿误差的计算为：
[0067][0068]
其中s为估计位姿到真实位姿的转换矩阵。
[0069]
使用tum数据集的walking_xyz序列进行测试，在rpe和ate的测试中 使用均方根误差、误差均值、误差中值、标准差共4个指标具体表示。具体数 据为表1所示。同时图3展示了本发明的绝对轨迹误差图，图4展示了本发明 的相对轨迹误差图，可以看出本发明的预估轨迹和真实轨迹十分相近，平移误 差也在合理范围内，对室内动态环境下的位姿估计影响可忽略。
[0070]
表1精度实验结果
[0071][0072][0073]
除精度测试外，通过时间测试，walking_xyz序列共859帧rgb图片，系 统对每帧的工作时间为0.0845s，本发明所阐述的方法可满足实时性要求。
[0074]
综上所述，本发明实例可显著降低轨迹误差，保证位姿估计的准确性，同 时保证系统可实时运行，综合平衡了室内动态环境中位姿估计的准确性、鲁棒 性和快速性。
[0075]
以上所述仅为本发明的实施例而已，并非用于限定本发明的保护范围。凡 在本发明的精神和原则之内，所作的任何修改，等同替换以及改进等，均应包 含在本发明的保护范围之内。