CN110533716B

CN110533716B - 一种基于3d约束的语义slam系统及方法

Info

Publication number: CN110533716B
Application number: CN201910768405.2A
Authority: CN
Inventors: 韩红; 张齐驰; 王毅飞; 唐裕亮; 迟勇欣; 范迎春
Original assignee: Xidian University
Current assignee: Xidian University
Priority date: 2019-08-20
Filing date: 2019-08-20
Publication date: 2022-12-02
Anticipated expiration: 2039-08-20
Also published as: CN110533716A

Abstract

本发明提出了一种基于3D约束的语义SLAM系统与方法，旨在解决在像素深度值不稳定情况下相机位姿计算不准确和当动态目标占据相机视野大部分空间时无法计算相机位姿的问题，通过深度稳定性检测方法提高了相机位姿估计的准确性，采用3D约束方法提高了相机轨迹的完整性。实现方法为：数据采集模块获取图像序列；神经网络模块获取目标检测图像和实例分割图像；3D约束模块获取不同类别的匹配对集合；目标分类模块获取静态目标实例分割图像和动态目标实例分割图像；视觉前端模块获取深度相机位姿和路标点集合；后端优化模块获取全局最优的深度相机位姿和路标点；语义地图模块获取语义点云地图。

Description

一种基于3D约束的语义SLAM系统及方法

技术领域

本发明属于计算机视觉技术领域，更进一步涉及一种基于3D约束的语义SLAM系统与方法，可用于复杂高动态环境中相机的位姿估计与语义地图的构建。

背景技术

同时定位与地图构建系统SLAM，在无人系统的自主导航避障上扮演着重要的角色，过去的三十年中，SLAM系统发展迅速，其主要目标是无人系统在对未知环境进行自主探索的过程中进行自身精确定位的同时能够构建环境地图。但是传统的SLAM系统建出的地图只包含环境中的点、线、面等低等级几何特征，而对于未来的无人系统，只包含简单空间信息的地图难以满足其发展需求。语义地图的独到之处在于包含了环境中物体的语义信息，三维空间的语义地图能够使无人系统正确感知周围情况，通过对环境的认知理解，能让SLAM系统提升一定的定位精度，弥补现有无人系统在环境感知，及理解的不足。语义SLAM系统在构建地图的过程中不仅获得环境中物体的几何结构信息，识别环境中物体，同时可以获取其位置、姿态和功能属性等语义信息，从而能有效的应对复杂场景及完成更加复杂的任务。

2018年10月，西班牙萨拉戈萨大学的Berta Bescos等人在IEEE Robotics andAutomation Letters第3卷第4期发表名称为“DynaSLAM:Tracking,Mapping,andInpainting in Dynamic Scenes”的文章，提出了一种基于实例分割的SLAM系统及方法，在ORB-SLAM2的基础上，增加了动态目标检测功能，将RGB-D图像数据输入到Mask R-CNN网络中对所有具备先验动态性质的目标进行逐像素的分割，得到动态目标实例，并采用多视图几何方法检测不包含于CNN网络输出类别中的真实移动物体，通过不属于这些动态目标实例和真实移动物体的特征点匹配对计算相机位姿，解决了ORB-SLAM2在环境中有动态目标情况下相机位姿估计不准确的问题。同时，在实例分割中将所有具备先验动态性质的目标实例分割出去，得到仅含有静态场景的图像，并使用静态场景图像构建点云地图。

然而，DynaSLAM将所有具有先验动态性质的目标去除，当这些目标在环境中是静态时，建立的静态场景地图将缺失这些物体的信息，从而使得地图构建不够准确，而且由于实例分割的精确度不够高，导致动态目标实例的边缘信息映射到地图中，从而在地图中出现动态目标边缘及边缘重影。另一方面，在深度值不稳定的情况下，利用深度值缺失或者突变的特征匹配对计算相机位姿将导致位姿估计误差较大，而且当动态目标占据相机视野中的大部分空间时，因为环境中的匹配点不足，会导致DynaSLAM无法计算相机位姿，从而出现丢帧现象，相机的轨迹将会不完整。

发明内容

本发明的目的在于克服上述已有技术的不足，提出了一种基于3D约束的语义SLAM系统及方法，用于解决在像素深度值不稳定情况下深度相机位姿计算不准确和当动态目标占据相机视野大部分空间时无法计算相机位姿的问题，以提高相机位姿的准确性和相机轨迹的完整性，同时解决动态目标在静止时无法在点云地图中构建和地图中出现动态目标边缘信息与边缘重影的问题，从而获取更准确的点云地图。

为实现上述目的，本发明采取的技术方案为：

一种基于3D约束的语义SLAM系统，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：

数据采集模块，采用深度相机，用于采集室内环境的多帧彩色图像和深度图像，以获取彩色图像序列和深度图像序列；

神经网络模块，用于通过训练好的BlitzNet网络模型，对彩色图像序列逐帧进行前向传播处理，以获取带有潜在动态目标框的目标检测图像和带有潜在动态目标实例的实例分割图像；

3D约束模块，用于对相邻两帧彩色图像进行特征匹配，并对获取的匹配对进行3D约束，将3D约束得到的多个匹配对进行归类，以获取各类别匹配对集合；

目标分类模块，用于通过目标检测图像和实例分割图像的内容信息与各类别匹配对集合的联系对潜在动态目标实例进行分类，以获取静态目标实例分割图像和动态目标实例分割图像；

视觉前端模块，用于通过各类别匹配对集合中的稳定匹配对集合计算深度相机位姿，并利用深度相机位姿和深度相机内参计算三维空间中的路标点坐标，以获取深度相机位姿和路标点集合；

后端优化模块，用于通过深度相机位姿和路标点集合构建代价函数，并对代价函数进行非线性优化，以获取全局最优的深度相机位姿和路标点集合；

语义地图模块，用于根据全局最优的深度相机位姿建立点云地图，并将静态目标实例分割图像中静态目标实例像素点的语义信息映射到点云地图上，以获取语义点云地图。

一种基于3D约束的语义SLAM的实现方法，包括如下步骤：

(1)数据采集模块获取图像序列：

数据采集模块采用深度相机，对室内环境进行持续拍摄，得到N帧彩色图像和N帧深度图像，并按照拍摄时间由前到后顺序分别对N帧彩色图像和N帧深度图像进行排序，得到彩色图像序列C₁,C₂,...,C_i,...,C_N和深度图像序列D₁,D₂,...,D_i,...,D_N，i＝1,2,...,N，N≥100；

(2)神经网络模块获取目标检测图像和实例分割图像：

神经网络模块采用通过COCO数据集训练的BlitzNet网络模型，对C₁,C₂,...,C_i,...,C_N中的N帧彩色图像逐帧进行前向传播处理，对具有运动性质的物体进行检测和分割，得到带有潜在动态目标框的检测图像CD₁,CD₂,...,CD_i,...,CD_N，以及带有潜在动态目标实例的实例分割图像CS₁,CS₂,...,CS_i,...,CS_N；

(3)3D约束模块获取不同类别的匹配对集合SEMP₁、SSMP₁、SMP₁和DMP₁：

(3a)3D约束模块对C₁和C₂分别提取ORB特征，得到C₁的特征点集合FP₁和C₂的特征点集合FP₁′，并采用深度稳定性检测方法对FP₁和FP₁′进行检测，得到深度稳定特征点集合DSP₁和DSP₁′；

(3b)3D约束模块对DSP₁和DSP₁′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP₁，DSMP₁中每一个匹配对dsmp₁包含属于C₁的特征点dsp₁和属于C₂的特征点dsp₁′，并将像素坐标位于CD₂潜在动态目标框内的dsp₁′对应的dsmp₁归为潜在动态匹配对集合PMP₁，将其余的dsmp₁归为环境匹配对集合EMP₁；

(3c)3D约束模块采用深度约束方法计算EMP₁中每一个匹配对emp₁的深度变化，将深度变化稳定的emp₁归为深度稳定环境匹配对集合DSEMP₁，并通过DSEMP₁深度变化的上界和下界对PMP₁进行深度约束，得到深度稳定静态匹配对集合DSSMP₁和深度动态匹配对集合DDMP₁；

(3d)3D约束模块采用距离约束方法计算DSEMP₁中每一个匹配对dsemp₁的距离变化，将距离变化稳定的dsemp₁归为距离稳定环境匹配对集合LSEMP₁，并通过LSEMP₁距离变化的上界和下界对DSSMP₁进行距离约束，得到距离稳定静态匹配对集合LSSMP₁和距离动态匹配对集合LDMP₁；

(3e)3D约束模块采用方向约束方法计算LSEMP₁中每一个匹配对lsemp₁的方向向量，将方向一致的lsemp₁归为稳定环境匹配对集合SEMP₁，并通过SEMP₁方向向量的方向角对LSSMP₁进行方向约束，得到稳定静态匹配对集合SSMP₁和方向动态匹配对集合MDMP₁；

(3f)3D约束模块将SEMP₁和SSMP₁合并，得到稳定匹配对集合SMP₁，并对DDMP₁、LDMP₁和MDMP₁进行合并，得到动态匹配对集合DMP₁；

(4)目标分类模块获取静态目标实例分割图像CSS₂和动态目标实例分割图像CDS₂：

(4a)目标分类模块对实例分割图像CS₂分割出来的潜在动态目标实例进行分类，将标签为人和动物的潜在动态目标实例归为柔性目标实例，其余的潜在动态目标实例归为刚性目标实例；

(4b)目标分类模块计算动态匹配对比率τ_d和潜在动态匹配对比率τ_p，并将CS₂中τ_d和τ_p均小于预设阈值的刚性目标实例归为静态目标实例，得到静态目标实例分割图像CSS₂，其余的实例归为动态目标实例，同时对动态目标实例进行膨胀处理，得到动态目标实例分割图像CDS₂；

(5)视觉前端模块获取深度相机位姿和路标点集合：

(5a)视觉前端模块将C₁的相机坐标系设定为世界坐标系，并采用迭代最近点ICP方法通过SMP₁中的匹配对smp₁计算C₂的深度相机位姿T₂；

(5b)视觉前端模块通过深度相机内参和T₂，将匹配对smp₁中C₂特征点sp₁′的像素坐标转化为世界坐标，得到三维空间中的路标点集合L₂；

(5c)视觉前端模块按照获取T₂和L₂的方法获取C₃,C₄,...,C_i,...,C_N的深度相机位姿T₃,T₄,...,T_i,...,T_N和路标点集合L₃,L₄,...,L_i,...,L_N；

(6)后端优化模块获取全局最优的深度相机位姿和路标点集合：

后端优化模块将L₂,L₃,...,L_i,...,L_N合并为路标点集合L，L中含有路标点l₁,l₂,...,l_j,...,l_M，构建以深度相机位姿T₂,T₃,...,T_i,...,T_N和路标点l₁,l₂,...,l_j,...,l_M为变量的代价函数Err，并利用列文伯格-马夸尔特法对Err进行非线性优化，得到全局最优深度相机位姿T₂′,T₃′,...,T_i′,...,T_N′和路标点l₁′,l₂′,...,l_j′,...,l_M′；

(7)语义地图模块获取语义点云地图PL：

(7a)语义地图模块对彩色图像序列C₂,C₃,...,C_i,...,C_N进行逐帧处理，采用深度稳定性检测方法将第i帧彩色图像C_i中深度稳定的像素点归为深度稳定像素点集合YP_i，并利用CDS_i中的动态目标实例信息将YP_i中不属于动态目标实例的像素点归为静态场景像素点集合CP_i；

(7b)语义地图模块通过深度相机内参和T_i计算CP_i的世界坐标，利用点云库PCL生成CP_i对应的三维空间点，并将所有三维空间点合并成点云PL_i；

(7c)语义地图模块利用CSS_i中的静态目标实例信息，对静态目标实例像素点对应的三维空间点进行语义标注，得到语义点云PL_i′；

(7d)语义地图模块对语义点云PL₂′,PL₃′,...,PL_i′,...,PL_N′进行拼接，得到全局语义点云地图PL。

本发明与现有的技术相比，具有以下优点：

第一，本发明采用深度稳定性检测方法剔除深度值缺失和深度值不稳定的特征点，通过深度稳定特征点进行特征匹配，并利用深度稳定匹配对构建深度约束，与现有技术中通过环境中所有的特征匹配对计算相机位姿相比，不仅减少了计算的数据量，而且提高了相机位姿估计的准确性；

第二，本发明采用3D约束方法实现对特征匹配对深度变化的深度约束、距离变化的距离约束和方向向量的方向约束，得到稳定静态匹配对集合和动态匹配对集合，并通过稳定静态匹配对集合和稳定环境匹配对集合共同计算相机位姿，与现有技术中仅通过环境匹配对集合计算相机位姿相比，解决了动态目标占据相机视野中大部分空间时无法计算相机位姿的问题，从而绘制更完整的相机轨迹，同时提高了相机位姿估计的准确性；

第三，本发明计算具有运动性质目标的动态匹配对比率和潜在动态匹配对比率，将潜在动态目标实例分为静态目标实例和动态目标实例，并对动态目标实例进行膨胀处理，同时将静态目标实例映射到点云地图中，与现有技术中将潜在动态目标实例均归为动态目标实例并在构建地图时没有利用动态目标实例相比，得到内容更丰富的语义点云地图，解决了地图中出现动态目标边缘信息与边缘重影的问题。

附图说明

图1是本发明语义SLAM系统的结构示意图；

图2是本发明语义SLAM方法的实现流程图；

图3是本发明方法中3D约束模块的实现流程图。

具体实施方式

以下结合附图和具体实施例，对本发明作进一步的详细说明：

参照图1，本发明基于3D约束的语义SLAM系统，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：

参照图2，本发明基于3D约束的语义SLAM方法，包括如下步骤：

步骤(1)数据采集模块获取图像序列：

步骤(2)神经网络模块获取目标检测图像和实例分割图像：

步骤(3)3D约束模块获取不同类别的匹配对集合SEMP₁、SSMP₁、SMP₁和DMP₁：

步骤(3a)3D约束模块对C₁和C₂分别提取ORB特征，得到C₁的特征点集合FP₁和C₂的特征点集合FP₁′，并采用深度稳定性检测方法对FP₁和FP₁′进行检测，得到深度稳定特征点集合DSP₁和DSP₁′，深度值存在突变的区域主要集中在物体的边缘，同时深度相机获得的处于较远距离的物体表面的深度值也会不精确，求删除这些深度值不稳定的特征点既减少特征匹配数据量，在往后求解ICP问题时也会降低对求解精度的影响；

采用深度稳定性检测方法对FP₁和FP₁′进行检测的实现步骤为：

步骤(3a1)3D约束模块以FP₁中每一个特征点fp₁的像素坐标为中心构建大小为3×3的图像块，检测图像块中是否存在深度值为0的像素，将存在零深度值的图像块对应的特征点fp₁删除，保留其余的fp₁；

步骤(3a2)3D约束模块计算保留下来所有特征点fp₁对应图像块内9个深度值的标准差，得到一组数量与特征点fp₁数量相同的标准差值SD，SD＝{SD₁,SD₂,...,SD_i,...,SD_n}，计算SD的比例中位数绝对偏差SMAD_SD：

SMAD_SD＝c×median(|SD_i-median(SD)|) (1)

其中，c为常数值1.4826，median(·)为一组数值的中位数；

步骤(3a3)3D约束模块将与median(SD)相差超过三倍SMAD_SD的标准差值SD_i定义为异常标准差值，并删除异常标准差值对应的特征点fp₁，将其余的fp₁归为深度稳定特征点集合DSP₁；

步骤(3a4)3D约束模块按照步骤(3a1)-(3a3)的方法对FP₁′进行相同的处理，得到深度稳定特征点集合DSP₁′。

参照图3，步骤(3b)3D约束模块对DSP₁和DSP₁′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP₁，DSMP₁中每一个匹配对dsmp₁包含属于C₁的特征点dsp₁和属于C₂的特征点dsp₁′，并将像素坐标位于CD₂潜在动态目标框内的dsp₁′对应的dsmp₁归为潜在动态匹配对集合PMP₁，将其余的dsmp₁归为环境匹配对集合EMP₁；

参照图3，步骤(3c)3D约束模块采用深度约束方法计算EMP₁中每一个匹配对emp₁的深度变化，将深度变化稳定的emp₁归为深度稳定环境匹配对集合DSEMP₁，并通过DSEMP₁深度变化的上界和下界对PMP₁进行深度约束，得到深度稳定静态匹配对集合DSSMP₁和深度动态匹配对集合DDMP₁，相机运动的过程中，相邻两帧图像中环境匹配对的深度变化理论上等于相机沿光轴方向的移动距离，考虑到噪声干扰等因素的影响，环境匹配对的深度变化值会集中在一个区间内，剔除异常值即可用来对潜在动态匹配对进行深度约束；

深度稳定静态匹配对集合DSSMP₁和深度动态匹配对集合DDMP₁的获取方法为：

步骤(3c1)3D约束模块计算EMP₁中每一个匹配对emp₁的深度变化Δdep_e：

Δdep_e＝|depth(ep₁)-depth(ep₁')| (2)

其中，ep₁为emp₁中属于C₁的特征点，ep₁′为emp₁中属于C₂的特征点，depth(·)为特征点的深度值，得到一组数量与匹配对emp₁数量相同的深度变化值Δdep，Δdep＝{Δdep_e1,Δdep_e2,...,Δdep_ei,...,Δdep_en′}；

步骤(3c2)3D约束模块计算Δdep的比例中位数绝对偏差SMAD_Δdep，将与median(Δdep)相差超过三倍SMAD_Δdep的深度变化值Δdep_e定义为异常深度变化值，剔除Δdep中的异常深度变化值得到Δdep′，并删除异常深度变化值对应的匹配对emp₁，将其余的emp₁归为深度稳定环境匹配对集合DSEMP₁；

步骤(3c3)3D约束模块将Δdep'中的最大值作为深度变化上界U_dep，Δdep′中的最小值作为深度变化下界D_dep，并计算PMP₁中每一个匹配对pmp₁的深度变化Δdep_p，将Δdep_p大于D_dep且小于U_dep的pmp₁归为深度稳定静态匹配对集合DSSMP₁，其余的pmp₁归为深度动态匹配对集合DDMP₁。

参照图3，步骤(3d)3D约束模块采用距离约束方法计算DSEMP₁中每一个匹配对dsemp₁的距离变化，将距离变化稳定的dsemp₁归为距离稳定环境匹配对集合LSEMP₁，并通过LSEMP₁距离变化的上界和下界对DSSMP₁进行距离约束，得到距离稳定静态匹配对集合LSSMP₁和距离动态匹配对集合LDMP₁，相机运动过程中，两帧图像匹配对的距离变化会因为特征点在空间中距离相机的远近而有所不同，统计环境区域中的匹配对的距离变化，确定环境中匹配对距离的变化区间，同时剔除不符合统计特性的异常值；

距离稳定静态匹配对集合LSSMP₁和距离动态匹配对集合LDMP₁的获取方法为：

步骤(3d1)3D约束模块计算DSEMP₁中每一个匹配对dsemp₁的距离变化Δdis_e：

其中，dsep₁为dsemp₁中属于C₁的特征点，dsep₁'为dsemp₁中属于C₂的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对dsemp₁数量相同的距离变化值Δdis，Δdis＝{Δdis_e1,Δdis_e2,...,Δdis_ei,...,Δdis_en″}；

步骤(3d2)3D约束模块计算Δdis的比例中位数绝对偏差SMAD_Δdis，将与Δdis的中位数相差超过三倍SMAD_Δdis的距离变化值Δdis_e定义为异常距离变化值，剔除Δdis中的异常距离变化值得到Δdis′，并删除异常距离变化值对应的匹配对dsemp₁，将其余的dsemp₁归为距离稳定环境匹配对集合LSEMP₁；

步骤(3d3)3D约束模块将Δdis'中的最大值作为深度变化上界U_dis，Δdis'中的最小值作为深度变化下界D_dis，并计算DSSMP₁中每一个匹配对dssmp₁的距离变化Δdis_p，将Δdis_p大于D_dis且小于U_dis的dssmp₁归为距离稳定静态匹配对集合LSSMP₁，其余的dssmp₁归为距离动态匹配对集合LDMP₁。

参照图3，步骤(3e)3D约束模块采用方向约束方法计算LSEMP₁中每一个匹配对lsemp₁的方向向量，将方向一致的lsemp₁归为稳定环境匹配对集合SEMP₁，并通过SEMP₁方向向量的方向角对LSSMP₁进行方向约束，得到稳定静态匹配对集合SSMP₁和方向动态匹配对集合MDMP₁；

稳定静态匹配对集合SSMP₁和方向动态匹配对集合MDMP₁的获取方法为：

步骤(3e1)3D约束模块计算LSEMP₁中每一个匹配对lsemp₁的方向向量

其中，lsep₁为lsemp₁中属于C₁的特征点，lsep₁′为lsemp₁中属于C₂的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对lsemp₁数量相同的方向向量

步骤(3e2)3D约束模块计算

中各方向向量

的方向角θ_e：

得到一组数量与

数量相同的方向角θ，θ＝{θ_e1,θ_e2,...,θ_ei,...,θ_en″′}；

步骤(3e3)3D约束模块设置方向角区间，将

设为区间I₁，

为区间I₂，

为区间I₃，

为区间I₄，

为区间I₅，

为区间I₆，

为区间I₇，

为区间I₈，π为区间I₉；

步骤(3e4)3D约束模块对方向向量

进行分类，将θ_e∈I₁的

归为方向集合NN，θ_e∈I₂的

归为方向集合YN，θ_e∈I₃的

归为方向集合PN，θ_e∈I₄的

归为方向集合XP，θ_e∈I₅的

归为方向集合PP，θ_e∈I₆的

归为方向集合YP，θ_e∈I₇的

归为方向集合NP，θ_e∈I₈的

归为方向集合XN，θ_e＝I₉的

归为方向集合ZERO，并将除ZERO之外的所有方向集合按照{NN,YN,PN,XP,PP,YP,NP,XN,NN}的循环顺序设定方向集合之间的顺序关系；

步骤(3e5)3D约束模块统计各方向集合中方向向量的数量N，若N最大的方向集合为ZERO，则将ZERO所对应的匹配对lsemp₁设为方向一致匹配对semp₁，同时设定I₉为主方向区间I，并将semp₁归为稳定环境匹配对集合SEMP₁，否则，将N最大的方向集合设为主方向集合MD，MD的左相邻集合为MDL，MD的右相邻集合为MDR；

步骤(3e6)3D约束模块分别计算MDL中方向向量的数量与MD中方向向量的数量之比Prop_L和MDR中方向向量的数量与MD中方向向量的数量之比Prop_R，若Prop_L大于18.71％则将MDL和MD所对应的匹配对lsemp₁设为方向一致匹配对semp₁，若Prop_R大于18.71％则将MDR和MD所对应的lsemp₁设为semp₁，若Prop_L大于18.71％且Prop_R大于18.71％则将MDL、MDR和MD所对应的lsemp₁设为semp₁，否则，只将MD所对应的lsemp₁设为semp₁，然后设定semp₁对应的方向角区间为主方向区间I，并将semp₁归为稳定环境匹配对集合SEMP₁；

步骤(3e7)3D约束模块计算LSSMP₁中每一个匹配对lssmp₁的方向向量

并计算

的方向角θ_e，将θ_e∈I的lssmp₁归为稳定静态匹配对集合SSMP₁，其余的lssmp₁归为方向动态匹配对集合MDMP₁。

参照图3，步骤(3f)3D约束模块将SEMP₁和SSMP₁合并，得到稳定匹配对集合SMP₁，并对DDMP₁、LDMP₁和MDMP₁进行合并，得到动态匹配对集合DMP₁；

步骤(4)目标分类模块获取静态目标实例分割图像CSS₂和动态目标实例分割图像CDS₂：

步骤(4a)目标分类模块对实例分割图像CS₂分割出来的潜在动态目标实例进行分类，将标签为人和动物的潜在动态目标实例归为柔性目标实例，其余的潜在动态目标实例归为刚性目标实例；

步骤(4b)目标分类模块计算动态匹配对比率τ_d和潜在动态匹配对比率τ_p，并将CS₂中τ_d和τ_p均小于预设阈值的刚性目标实例归为静态目标实例，得到静态目标实例分割图像CSS₂，其余的实例归为动态目标实例，同时对动态目标实例进行膨胀处理，得到动态目标实例分割图像CDS₂；

目标分类模块计算动态匹配对比率τ_d和潜在动态匹配对比率τ_p的实现步骤为：

步骤(4b1)目标分类模块统计SEMP₁的匹配对数量N_SEMP、SSMP₁中位于CS₂各潜在动态目标实例的匹配对数量N_SSMP1,N_SSMP2,...,N_SSMPi,...,N_SSMPm和DMP₁中位于CS₂各潜在动态目标实例的匹配对数量N_DMP1,N_DMP2,...,N_DMPi,...,N_DMPm；

步骤(4b2)目标分类模块计算各潜在动态目标实例的动态匹配对比率τ_d和潜在动态匹配对比率τ_p：

设定τ_d的阈值为0.5，τ_p的阈值为0.15，将τ_d≤0.5且τ_p≤0.15的刚性目标实例归为静态目标实例，其余的实例归为动态目标实例。

步骤(5)视觉前端模块获取深度相机位姿和路标点集合：

步骤(5a)视觉前端模块将C₁的相机坐标系设定为世界坐标系，并采用迭代最近点ICP方法通过SMP₁中的匹配对smp₁计算C₂的深度相机位姿T₂；

步骤(5b)视觉前端模块通过深度相机内参和T₂，将匹配对smp₁中C₂特征点sp₁′的像素坐标转化为世界坐标，得到三维空间中的路标点集合L₂；

步骤(5c)视觉前端模块按照获取T₂和L₂的方法获取C₃,C₄,...,C_i,...,C_N的深度相机位姿T₃,T₄,...,T_i,...,T_N和路标点集合L₃,L₄,...,L_i,...,L_N；

步骤(6)后端优化模块获取全局最优的深度相机位姿和路标点集合：

构建代价函数Err的实现步骤为：

步骤(6a)后端优化模块根据相机位姿T_i中的旋转矩阵R和平移向量t，将L中的路标点l_j三维坐标p_j＝[X,Y,Z]^T转化为相机坐标p_j′：

p_j'＝Rp_j+t＝[X',Y',Z']^T (8)

步骤(6b)后端优化模块通过相机坐标p_j′计算归一化坐标[u_c,v_c,1]^T：

步骤(6c)后端优化模块通过深度相机内参x轴缩放因子f_x、y轴缩放因子f_y、x轴平移因子c_x、y轴平移因子c_y和归一化坐标计算像素坐标P_j＝[u_s,v_s]^T：

步骤(6d)后端优化模块通过l_j对应的C_i特征点sp_i-1像素坐标P_j′＝[u_s′,v_s′]^T计算误差err_i：

步骤(6f)后端优化模块对err₂,err₃,...,err_i,...,err_N进行求和，得到代价函数Err：

步骤(7)语义地图模块获取语义点云地图PL：

步骤(7a)语义地图模块对彩色图像序列C₂,C₃,...,C_i,...,C_N进行逐帧处理，采用深度稳定性检测方法将第i帧彩色图像C_i中深度稳定的像素点归为深度稳定像素点集合YP_i，并利用CDS_i中的动态目标实例信息将YP_i中不属于动态目标实例的像素点归为静态场景像素点集合CP_i；

步骤(7b)语义地图模块通过深度相机内参和T_i计算CP_i的世界坐标，利用点云库PCL生成CP_i对应的三维空间点，并将所有三维空间点合并成点云PL_i；

步骤(7c)语义地图模块利用CSS_i中的静态目标实例信息，对静态目标实例像素点对应的三维空间点进行语义标注，得到语义点云PL_i′；

步骤(7d)语义地图模块对语义点云PL₂′,PL₃′,...,PL_i′,...,PL_N′进行拼接，得到全局语义点云地图PL。

Claims

1.一种基于3D约束的语义SLAM系统，其特征在于，包括数据采集模块、神经网络模块、3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：

2.一种基于3D约束的语义SLAM的实现方法，其特征在于，包括如下步骤：

(1)数据采集模块获取图像序列：

(2)神经网络模块获取目标检测图像和实例分割图像：

(5)视觉前端模块获取深度相机位姿和路标点集合：

(7)语义地图模块获取语义点云地图PL：

3.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3a)中所述的采用深度稳定性检测方法对FP₁和FP₁′进行检测，实现步骤为：

(3a1)3D约束模块以FP₁中每一个特征点fp₁的像素坐标为中心构建大小为3×3的图像块，检测图像块中是否存在深度值为0的像素，将存在零深度值的图像块对应的特征点fp₁删除，保留其余的fp₁；

(3a2)3D约束模块计算保留下来所有特征点fp₁对应图像块内9个深度值的标准差，得到一组数量与特征点fp₁数量相同的标准差值SD，SD＝{SD₁,SD₂,...,SD_i,...,SD_n}，计算SD的比例中位数绝对偏差SMAD_SD：

SMAD_SD＝c×median(|SD_i-median(SD)|) (1)

其中，c为常数值1.4826，median(·)为一组数值的中位数；

(3a3)3D约束模块将与median(SD)相差超过三倍SMAD_SD的标准差值SD_i定义为异常标准差值，并删除异常标准差值对应的特征点fp₁，将其余的fp₁归为深度稳定特征点集合DSP₁；

(3a4)3D约束模块按照步骤(3a1)-(3a3)的方法对FP₁′进行相同的处理，得到深度稳定特征点集合DSP₁′。

4.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3c)中所述的深度稳定静态匹配对集合DSSMP₁和深度动态匹配对集合DDMP₁，获取方法为：

(3c1)3D约束模块计算EMP₁中每一个匹配对emp₁的深度变化Δdep_e：

Δdep_e＝|depth(ep₁)-depth(ep₁')| (2)

(3c2)3D约束模块计算Δdep的比例中位数绝对偏差SMAD_Δdep，将与median(Δdep)相差超过三倍SMAD_Δdep的深度变化值Δdep_e定义为异常深度变化值，剔除Δdep中的异常深度变化值得到Δdep′，并删除异常深度变化值对应的匹配对emp₁，将其余的emp₁归为深度稳定环境匹配对集合DSEMP₁；

(3c3)3D约束模块将Δdep'中的最大值作为深度变化上界U_dep，Δdep′中的最小值作为深度变化下界D_dep，并计算PMP₁中每一个匹配对pmp₁的深度变化Δdep_p，将Δdep_p大于D_dep且小于U_dep的pmp₁归为深度稳定静态匹配对集合DSSMP₁，其余的pmp₁归为深度动态匹配对集合DDMP₁。

5.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3d)中所述的距离稳定静态匹配对集合LSSMP₁和距离动态匹配对集合LDMP₁，获取方法为：

(3d1)3D约束模块计算DSEMP₁中每一个匹配对dsemp₁的距离变化Δdis_e：

(3d2)3D约束模块计算Δdis的比例中位数绝对偏差SMAD_Δdis，将与Δdis的中位数相差超过三倍SMAD_Δdis的距离变化值Δdis_e定义为异常距离变化值，剔除Δdis中的异常距离变化值得到Δdis′，并删除异常距离变化值对应的匹配对dsemp₁，将其余的dsemp₁归为距离稳定环境匹配对集合LSEMP₁；

(3d3)3D约束模块将Δdis'中的最大值作为深度变化上界U_dis，Δdis'中的最小值作为深度变化下界D_dis，并计算DSSMP₁中每一个匹配对dssmp₁的距离变化Δdis_p，将Δdis_p大于D_dis且小于U_dis的dssmp₁归为距离稳定静态匹配对集合LSSMP₁，其余的dssmp₁归为距离动态匹配对集合LDMP₁。

6.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3e)中所述的稳定静态匹配对集合SSMP₁和方向动态匹配对集合MDMP₁，获取方法为：

(3e1)3D约束模块计算LSEMP₁中每一个匹配对lsemp₁的方向向量

(3e2)3D约束模块计算

中各方向向量

的方向角θ_e：

得到一组数量与

数量相同的方向角θ，θ＝{θ_e1,θ_e2,...,θ_ei,...,θ_en″′}；

(3e3)3D约束模块设置方向角区间，将

设为区间I₁，

为区间I₂，

为区间I₃，

为区间I₄，

为区间I₅，

为区间I₆，

为区间I₇，

为区间I₈，π为区间I₉；

(3e4)3D约束模块对方向向量

进行分类，将θ_e∈I₁的

归为方向集合NN，θ_e∈I₂的

归为方向集合YN，θ_e∈I₃的

归为方向集合PN，θ_e∈I₄的

归为方向集合XP，θ_e∈I₅的

归为方向集合PP，θ_e∈I₆的

归为方向集合YP，θ_e∈I₇的

归为方向集合NP，θ_e∈I₈的

归为方向集合XN，θ_e＝I₉的

(3e5)3D约束模块统计各方向集合中方向向量的数量N，若N最大的方向集合为ZERO，则将ZERO所对应的匹配对lsemp₁设为方向一致匹配对semp₁，同时设定I₉为主方向区间I，并将semp₁归为稳定环境匹配对集合SEMP₁，否则，将N最大的方向集合设为主方向集合MD，MD的左相邻集合为MDL，MD的右相邻集合为MDR；

(3e6)3D约束模块分别计算MDL中方向向量的数量与MD中方向向量的数量之比Prop_L和MDR中方向向量的数量与MD中方向向量的数量之比Prop_R，若Prop_L大于18.71％则将MDL和MD所对应的匹配对lsemp₁设为方向一致匹配对semp₁，若Prop_R大于18.71％则将MDR和MD所对应的lsemp₁设为semp₁，若Prop_L大于18.71％且Prop_R大于18.71％则将MDL、MDR和MD所对应的lsemp₁设为semp₁，否则，只将MD所对应的lsemp₁设为semp₁，然后设定semp₁对应的方向角区间为主方向区间I，并将semp₁归为稳定环境匹配对集合SEMP₁；

(3e7)3D约束模块计算LSSMP₁中每一个匹配对lssmp₁的方向向量

并计算

7.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(4b)中所述的目标分类模块计算动态匹配对比率τ_d和潜在动态匹配对比率τ_p，实现步骤为：

(4b1)目标分类模块统计SEMP₁的匹配对数量N_SEMP、SSMP₁中位于CS₂各潜在动态目标实例的匹配对数量N_SSMP1,N_SSMP2,...,N_SSMPi,...,N_SSMPm和DMP₁中位于CS₂各潜在动态目标实例的匹配对数量N_DMP1,N_DMP2,...,N_DMPi,...,N_DMPm；

(4b2)目标分类模块计算各潜在动态目标实例的动态匹配对比率τ_d和潜在动态匹配对比率τ_p：

8.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(6)中所述的构建代价函数Err，实现步骤为：

(6a)后端优化模块根据相机位姿T_i中的旋转矩阵R和平移向量t，将L中的路标点l_j三维坐标p_j＝[X,Y,Z]^T转化为相机坐标p_j′：

p_j'＝Rp_j+t＝[X',Y',Z']^T (8)

(6b)后端优化模块通过相机坐标p_j′计算归一化坐标[u_c,v_c,1]^T：

(6c)后端优化模块通过深度相机内参x轴缩放因子f_x、y轴缩放因子f_y、x轴平移因子c_x、y轴平移因子c_y和归一化坐标计算像素坐标P_j＝[u_s,v_s]^T：

(6d)后端优化模块通过l_j对应的C_i特征点sp_i-1像素坐标P_j′＝[u_s′,v_s′]^T计算误差err_i：

(6f)后端优化模块对err₂,err₃,...,err_i,...,err_N进行求和，得到代价函数Err：