1.一种基于3D约束的语义SLAM系统，其特征在于，包括数据采集模块、神经网络模块、

3D约束模块、目标分类模块、视觉前端模块、后端优化模块和语义地图模块，其中：

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

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

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

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

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

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

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

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

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

数据采集模块采用深度相机，对室内环境进行持续拍摄，得到N帧彩色图像和N帧深度图像，并按照拍摄时间由前到后顺序分别对N帧彩色图像和N帧深度图像进行排序，得到彩色图像序列C1,C2,...,Ci,...,CN和深度图像序列D1,D2,...,Di,...,DN，i＝1,2,...,N，N≥

100；

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

神经网络模块采用通过COCO数据集训练的BlitzNet网络模型，对C1,C2,...,Ci,...,CN中的N帧彩色图像逐帧进行前向传播处理，对具有运动性质的物体进行检测和分割，得到带有潜在动态目标框的检测图像CD1,CD2,...,CDi,...,CDN，以及带有潜在动态目标实例的实例分割图像CS1,CS2,...,CSi,...,CSN；

(3)3D约束模块获取不同类别的匹配对集合SEMP1、SSMP1、SMP1和DMP1：(3a)3D约束模块对C1和C2分别提取ORB特征，得到C1的特征点集合FP1和C2的特征点集合FP1′，并采用深度稳定性检测方法对FP1和FP1′进行检测，得到深度稳定特征点集合DSP1和DSP1′；

(3b)3D约束模块对DSP1和DSP1′进行特征匹配，将得到的多个匹配对归为匹配对集合DSMP1，DSMP1中每一个匹配对dsmp1包含属于C1的特征点dsp1和属于C2的特征点dsp1′，并将像素坐标位于CD2潜在动态目标框内的dsp1′对应的dsmp1归为潜在动态匹配对集合PMP1，将其余的dsmp1归为环境匹配对集合EMP1；

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

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

(3e)3D约束模块采用方向约束方法计算LSEMP1中每一个匹配对lsemp1的方向向量，将方向一致的lsemp1归为稳定环境匹配对集合SEMP1，并通过SEMP1方向向量的方向角对LSSMP1进行方向约束，得到稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1；

(3f)3D约束模块将SEMP1和SSMP1合并，得到稳定匹配对集合SMP1，并对DDMP1、LDMP1和MDMP1进行合并，得到动态匹配对集合DMP1；

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

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

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

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

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

(5c)视觉前端模块按照获取T2和L2的方法获取C3,C4,...,Ci,...,CN的深度相机位姿T3,T4,...,Ti,...,TN和路标点集合L3,L4,...,Li,...,LN；

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

后端优化模块将L2,L3,...,Li,...,LN合并为路标点集合L，L中含有路标点l1,l2,...,lj,...,lM，构建以深度相机位姿T2,T3,...,Ti,...,TN和路标点l1,l2,...,lj,...,lM为变量的代价函数Err，并利用列文伯格‑马夸尔特法对Err进行非线性优化，得到全局最优深度相机位姿T2′,T3′,...,Ti′,...,TN′和路标点l1′,l2′,...,lj′,...,lM′；

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

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

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

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

(7d)语义地图模块对语义点云PL2′,PL3′,...,PLi′,...,PLN′进行拼接，得到全局语义点云地图PL。

3.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3a)中所述的采用深度稳定性检测方法对FP1和FP1′进行检测，实现步骤为：(3a1)3D约束模块以FP1中每一个特征点fp1的像素坐标为中心构建大小为3×3的图像块，检测图像块中是否存在深度值为0的像素，将存在零深度值的图像块对应的特征点fp1删除，保留其余的fp1；

(3a2)3D约束模块计算保留下来所有特征点fp1对应图像块内9个深度值的标准差，得到一组数量与特征点fp1数量相同的标准差值SD，SD＝{SD1,SD2,...,SDi,...,SDn}，计算SD的比例中位数绝对偏差SMADSD：SMADSD＝c×median(|SDi‑median(SD)|)    (1)其中，c为常数值1.4826，median(·)为一组数值的中位数；

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

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

4.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3c)中所述的深度稳定静态匹配对集合DSSMP1和深度动态匹配对集合DDMP1，获取方法为：(3c1)3D约束模块计算EMP1中每一个匹配对emp1的深度变化Δdepe：

Δdepe＝|depth(ep1)‑depth(ep1')|    (2)

其中，ep1为emp1中属于C1的特征点，ep1′为emp1中属于C2的特征点，depth(·)为特征点的深度值，得到一组数量与匹配对emp1数量相同的深度变化值Δdep，Δdep＝{Δdepe1,Δdepe2,...,Δdepei,...,Δdepen′}；

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

(3c3)3D约束模块将Δdep'中的最大值作为深度变化上界Udep，Δdep′中的最小值作为深度变化下界Ddep，并计算PMP1中每一个匹配对pmp1的深度变化Δdepp，将Δdepp大于Ddep且小于Udep的pmp1归为深度稳定静态匹配对集合DSSMP1，其余的pmp1归为深度动态匹配对集合DDMP1。

5.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3d)中所述的距离稳定静态匹配对集合LSSMP1和距离动态匹配对集合LDMP1，获取方法为：(3d1)3D约束模块计算DSEMP1中每一个匹配对dsemp1的距离变化Δdise：其中，dsep1为dsemp1中属于C1的特征点，dsep1'为dsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对dsemp1数量相同的距离变化值Δdis，Δdis＝{Δdise1,Δdise2,...,Δdisei,...,Δdisen″}；

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

(3d3)3D约束模块将Δdis'中的最大值作为深度变化上界Udis，Δdis'中的最小值作为深度变化下界Ddis，并计算DSSMP1中每一个匹配对dssmp1的距离变化Δdisp，将Δdisp大于Ddis且小于Udis的dssmp1归为距离稳定静态匹配对集合LSSMP1，其余的dssmp1归为距离动态匹配对集合LDMP1。

6.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(3e)中所述的稳定静态匹配对集合SSMP1和方向动态匹配对集合MDMP1，获取方法为：(3e1)3D约束模块计算LSEMP1中每一个匹配对lsemp1的方向向量

其中，lsep1为lsemp1中属于C1的特征点，lsep1′为lsemp1中属于C2的特征点，(u,v)为特征点的像素坐标，得到一组数量与匹配对lsemp1数量相同的方向向量(3e2)3D约束模块计算 中各方向向量 的方向角θe：

得到一组数量与 数量相同的方向角θ，θ＝{θe1,θe2,...,θei,...,θen″′}；

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

为区间I3， 为区间I4， 为区间I5， 为区间I6，

为区间I7， 为区间I8，π为区间I9；

(3e4)3D约束模块对方向向量 进行分类，将θe∈I1的 归为方向集合NN，θe∈I2的归为方向集合YN，θe∈I3的 归为方向集合PN，θe∈I4的 归为方向集合XP，θe∈I5的归为方向集合PP，θe∈I6的 归为方向集合YP，θe∈I7的 归为方向集合NP，θe∈I8的归为方向集合XN，θe＝I9的 归为方向集合ZERO，并将除ZERO之外的所有方向集合按照{NN,YN,PN,XP,PP,YP,NP,XN,NN}的循环顺序设定方向集合之间的顺序关系；

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

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

(3e7)3D约束模块计算LSSMP1中每一个匹配对lssmp1的方向向量 并计算 的方向角θe，将θe∈I的lssmp1归为稳定静态匹配对集合SSMP1，其余的lssmp1归为方向动态匹配对集合MDMP1。

7.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(4b)中所述的目标分类模块计算动态匹配对比率τd和潜在动态匹配对比率τp，实现步骤为：(4b1)目标分类模块统计SEMP1的匹配对数量NSEMP、SSMP1中位于CS2各潜在动态目标实例的匹配对数量NSSMP1,NSSMP2,...,NSSMPi,...,NSSMPm和DMP1中位于CS2各潜在动态目标实例的匹配对数量NDMP1,NDMP2,...,NDMPi,...,NDMPm；

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

8.根据权利要求2所述的基于3D约束的语义SLAM的实现方法，其特征在于，步骤(6)中所述的构建代价函数Err，实现步骤为：(6a)后端优化模块根据相机位姿Ti中的旋转矩阵R和平移向量t，将L中的路标点lj三维T坐标pj＝[X,Y,Z]转化为相机坐标pj′：

T

pj'＝Rpj+t＝[X',Y',Z']    (8)

T

(6b)后端优化模块通过相机坐标pj′计算归一化坐标[uc,vc,1]：

(6c)后端优化模块通过深度相机内参x轴缩放因子fx、y轴缩放因子fy、x轴平移因子cx、Ty轴平移因子cy和归一化坐标计算像素坐标Pj＝[us,vs]：

T

(6d)后端优化模块通过lj对应的Ci特征点spi‑1像素坐标Pj′＝[us′,vs′]计算误差erri：(6f)后端优化模块对err2,err3,...,erri,...,errN进行求和，得到代价函数Err：