基于目标检测和K-means聚类的联合优化动态SLAM方法技术

本发明专利技术公开基于目标检测和K

【技术实现步骤摘要】
基于目标检测和K
‑
means聚类的联合优化动态SLAM方法


[0001]本专利技术涉及定位与地图构建，尤其涉及基于目标检测和K
‑
means聚类的联合优化动态SLAM方法。

技术介绍

[0002]同时定位与地图构建(Simultaneous localization and mapping,SLAM)技术是实现机器人自主运动的关键技术，在移动机器人、自动驾驶、无人机和增强现实与虚拟现实(Augmented reality,Virtual reality,AR/VR)等领域有广泛的应用。SLAM的任务是通过机器人采集的传感器数据，在运动过程中同时完成对自身运动的估计和对环境地图的建立，即实现定位和建图两个目标。目前，SLAM技术可以分为动态SLAM和静态SLAM，其中ORB
‑
SLAM2,LSD
‑
SLAM,DSO等传统SLAM算法主要应用于静态场景下，而DS
‑
SLAM和Dynamic
‑
SLAM算法主要应用于动态场景下。
[0003]ORBSLAM2是最经典的基于静态场景下的SLAM系统之一，该系统包括三个线程，分别是跟踪、建图和回环检测。跟踪线程在跟踪参考关键帧时，通过随机采样一致性(RANSAC)算法将环境中的动态特征作为外点进行滤除，用剩下的特征匹配点对求解相机初始位姿。建图线程在局部地图中求解优化问题，将局部地图中的地图点重投影到当前帧，获得更多的3D
‑
2D匹配点对，求解更为精准的相机位姿。回环检测线程闭合回环，利用位姿图优化消除累积误差。
[0004]ORBSLAM2中所采用滤除误匹配点对的RANSAC算法，是从当前帧中所有的3D
‑
2D匹配点对中随机选取一组3D
‑
2D点对来求解当前帧的位姿，用剩下的3D
‑
2D点对来验证这个求得的位姿，如果满足这个求得的位姿的3D
‑
2D点对数量大于设定的阈值，说明这个位姿是准确的，如果小于设定的阈值，则重新选取一组3D
‑
2D点对来求解位姿，重复上述步骤。这种算法在低动态场景中，动态点数量较少时，可以达到一定效果。但是在高动态场景中，动态点数量很多，甚至场景中动态点数目大于静态点数目，此时求解出的位姿就会出现很大的误差，甚至导致跟踪和定位失败。
[0005]DS
‑
SLAM是目前最经典的基于动态场景下的SLAM系统之一，该算法处理动态场景下的动态特征主要分为三个部分。第一，首先采用SegNet神经网络对当前帧进行像素级语义分割，并假定人最有可能是动态对象，所以从中分割出人的区域。第二，在语义分割的同时，计算光流金字塔，得到当前帧中匹配的特征点，用RANSAC求解当前帧的位姿和基础矩阵，然后利用基本矩阵计算当前帧中的极线。最后，确定匹配点与其对应的极线之间的距离是否小于某个阈值。如果距离大于阈值，则该匹配点被判定为动态点。第三，如果落在人区域内的动态点数量大于设定阈值，则说明人是运动的，然后剔除掉人所在区域内的所有特征点，用剩下的特征点求解当前帧位姿。
[0006]DS
‑
SLAM中也存在着许多缺点，第一，由于此时的场景存在着大量的动态特征，所以用RANSAC算法求解出的当前帧的位姿和基础矩阵存在很大误差，这样进而会导致后面用极线约束判定的动态点不准。第二，极线约束的外点检测方法并不能找到所有外点，当物体
沿极线方向运动时这种方法会失效。第三，用物体区域特征点中的外点的比例来判断该物体是否运动，这用方法存在局限性，特征点的数量受物体纹理的影响较大。第四，SegNet语义分割网络运算成本大，耗时长，难以满足SLAM系统的实时性。

技术实现思路

[0007]本专利技术的目的在于提供基于目标检测和K
‑
means聚类的联合优化动态SLAM方法。
[0008]本专利技术采用的技术方案是：
[0009]基于目标检测和K
‑
means聚类的联合优化动态SLAM方法，其包括以下步骤：
[0010]步骤S1：获取输入图像得到RGB图像及对应的深度图像，
[0011]步骤S2：对当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算；同时将当前帧的RGB图像用Yolov5目标检测得到物体的边界框位置和类别；
[0012]步骤S3：将检测出的物体边界框进行分类，把人物所在的边界框标记为动态物体框，把非人物的物体所在的边界框标记为静态物体框；
[0013]步骤S4：根据深度值当前帧的深度图像中动态物体框内的像素聚成k类；
[0014]步骤S5：从K
‑
means聚类分割出的k类中找出像素点数量最大的类，把在最大类但不在静态物体框内的特征点标记为准动态特征点并剔除掉，把在其他类的但不在静态物体框内的特征点标记为疑似动态特征点，把其余剩下的特征点暂时标记为静态特征点；
[0015]步骤S6：将当前帧的RGB图像的静态特征点和疑似动态特征点与上一帧的RGB图像的特征点进行描述子匹配，得到N
i
个静态特征匹配点对和N
j
个疑似动态特征匹配点对；将静态特征匹配点对和疑似动态特征匹配点对进行加权最小化重投影误差，初步求解当前帧的位姿T
cw1
，公式如下：
[0016][0017]其中：∑1＝n
×
E,∑2＝0.4
×
n
×
E；x
i
为静态特征点坐标，p
i
为对应的投影坐标；x
j
为可疑动态特征点坐标，p
j
为对应的投影坐标，∑1为静态特征点的信息矩阵，∑2为可疑动态特征点点的信息矩阵，n为当前特征点所在图像金字塔的层数，E为2
×
2的单位矩阵；
[0018]步骤S7：在静态物体框中而实际运动但无法被Yolov5目标检测算法和K
‑
means聚类算法检测到的特征点，用极限约束和投影条件检测出剩下少量动态特征点；
[0019]步骤S8：不满足极线约束和投影条件的剩余动态特征点剔除后，把剩下的特征点标记为最终静态特征点，将最终静态特征点对应的地图点投影到当前帧，建立最小重投影误差求解当前帧的最终位姿：
[0020][0021]其中x
i
是准静态特征点对应的地图点坐标，p
i
为准静态特征点坐标，N为准静态特征点数量，T
cw
为当前帧最终位姿。
[0022]进一步地，步骤S1中的RGB图像通过RGB
‑
D相机采集得到。
[0023]进一步地，步骤S3中非人物的物体包括电脑、电视和椅子。
[0024]进一步地，步骤S4具体包括以下步骤：
[0025]步骤S41：随机本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于目标检测和K
‑
means聚类的联合优化动态SLAM方法，其特征在于：其包括以下步骤：步骤S1：获取输入图像得到RGB图像及对应的深度图像，步骤S2：对当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算；同时将当前帧的RGB图像用Yolov5目标检测得到物体的边界框位置和类别；步骤S3：将检测出的物体边界框进行分类，把人物所在的边界框标记为动态物体框，把非人物的物体所在的边界框标记为静态物体框；步骤S4：根据深度值当前帧的深度图像中动态物体框内的像素聚成k类；步骤S5：从K
‑
means聚类分割出的k类中找出像素点数量最大的类，把在最大类但不在静态物体框内的特征点标记为准动态特征点并剔除掉，把在其他类的但不在静态物体框内的特征点标记为疑似动态特征点，把其余剩下的特征点暂时标记为静态特征点；步骤S6：将当前帧的RGB图像的静态特征点和疑似动态特征点与上一帧的RGB图像的特征点进行描述子匹配，得到N
i
个静态特征匹配点对和N
j
个疑似动态特征匹配点对；将静态特征匹配点对和疑似动态特征匹配点对进行加权最小化重投影误差，初步求解当前帧的位姿T
cw1
，公式如下：其中：∑1＝n
×
E，∑2＝0.4
×
n
×
E；x
i
为静态特征点坐标，p
i
为对应的投影坐标；x
j
为可疑动态特征点坐标，p
j
为对应的投影坐标，∑1为静态特征点的信息矩阵，∑2为可疑动态特征点点的信息矩阵，n为当前特征点所在图像金字塔的层数，E为2
×
2的单位矩阵；步骤S7：在静态物体框中而实际运动但无法被Yolov5目标检测算法和K
‑
means聚类算法检测到的特征点，用极限约束和投影条件检测出剩下少量动态特征点；步骤S8：不满足极线约束和投影条件的剩余动态特征点剔除后，把剩下的特征点标记为最终静态特征点，将最终静态特征点对应的地图点投影到当前帧，建立最小重投影误差求解当前帧的最终位姿：其中x
i
是准静态特征点对应的地图点坐标，p
i
为准静态特征点坐标，N为准静态特征点数量，T
cw
为当前帧最终位姿。2.根据权利要求1所述的基于目标检测和K
‑
means聚类的联合优化动态SLAM方法，其特征在于：步骤S1中的RGB图像通过RGB
‑
D相机采集得到。3.根据权利要求1所述的基于目标检测和K
‑
means聚类的联合优化动态SLAM方法，其特征在于：步骤S3中非人物的物体包括电脑、电视和椅子。4.根据权利要求1所述的基于目标检测和K
‑
means聚类的联合优化动态SLAM方法，其特征在于：步骤S4具体包括以下步骤：步骤S41：随机在深度图像动态物体框内选出k个像素深度值作为聚类中心，初始化聚类中心为Z1(1)，Z2(1)
…
Z
k

【专利技术属性】
技术研发人员：刘丽桑，郭江峰，
申请(专利权)人：福建工程学院，
类型：发明
国别省市：