CN110827395B - 一种适用于动态环境的即时定位与地图构建方法 - Google Patents

Abstract

本发明公开了一种适用于动态环境的即时定位与地图构建方法，包括下述步骤：首先对深度相机和编码器进行外参标定，获得编码器坐标到深度相机坐标的一个变换矩阵；然后对深度相机采集的每一帧图像使用图像金字塔分层，然后从图像金字塔的每一层提取ORB特征点；将编码器获得的数据作为追踪模块的恒速模型，然后将前一帧关键帧的特征点投影到当前帧上，与当前帧的特征点进行匹配，匹配成功的关键点通过三角测量得到两帧之间的位姿；本发明追踪部分使用了编码器数据作为相机重投影时的初始位姿，实现了在动态环境中精准定位追踪，比传统的语义slam追踪模块需要每一帧剔除动态像素，节省了大量时间。

Description

一种适用于动态环境的即时定位与地图构建方法

技术领域

本发明涉及移动机器人定位导航和建图技术领域，具体涉及一种适用于动态环境的即时定位与地图构建方法。

背景技术

Slam(即时定位与地图构建)被广泛应用于移动机器人、无人机和增强现实等领域。然而在动态环境下，传统的slam***不具有鲁棒性。在充斥着动态物体的现实场景中，经典的slam***像ORB_SLAM中的追踪模块会失效，更糟糕的是，经典slam***建立的是静态地图，无法将动态物体从地图当中剔除出去，导致地图根本无法使用。为了解决如何使slam***在动态场景适用这个问题，很多论文文献给出了不同的解决方案，DS_SLAM是其中一个行之有效的方案，该方案对输入的每一帧图像进行语义分割，利用分割后的图像计算本质矩阵，然后使用本质矩阵判断是否为动态物体，如果是就将他们剔除。但是该方案由于对每一帧图像进行语义分割，***难以在移动平台达到实时性，而且建立的是稠密点云地图，内存占用高。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种适用于动态环境的即时定位与地图构建方法，该方法在特定改装的移动机器人上可达到实时性，跟踪部分在动态环境下具有鲁棒性，能根据语义分割的结果，能建立一个三维语义八叉树地图。

本发明的目的通过下述技术方案实现：

一种适用于动态环境的即时定位与地图构建方法，包括下述步骤：

步骤一，首先对深度相机和编码器进行外参标定，获得编码器坐标到深度相机坐标的一个变换矩阵；然后对深度相机采集的每一帧图像使用图像金字塔分层，然后从图像金字塔的每一层提取ORB特征点；将编码器获得的数据作为追踪模块的恒速模型，然后将前一帧关键帧的特征点投影到当前帧上，与当前帧的特征点进行匹配，匹配成功的关键点通过三角测量得到两帧之间的位姿；

步骤二，追踪模块通过最近一次全局优化离当前帧是否已经过去20帧以上和当前帧是否能检测到50个地图点的原则，判定当前帧是否应选取为关键帧，然后将关键帧传到动态物体剔除模块，动态物体剔除模块使用网络对关键帧进行语义分割；

步骤三，追踪模块选取的关键帧在局部地图优化模块投影得到地图点，然后使用lacol BA进行优化；然后在回环检测模块中使用DBow2词袋模型进行回环检测，将之前所有的地图点和关键帧用BA进行全局优化，既优化机器人的位姿也优化地图点；

步骤四，接收全局优化后的关键帧和语义分割后的结果，将关键帧投影到3d点云，根据不同的语义分割的标签为点云着色，然后在语义八叉树地图模块中建立语义八叉树地图。

优选地，所述追踪模块的工作步骤如下：

深度相机以30帧每秒的频率采集彩色图像和深度图像，通过ros中iai_kinect2软件包将彩色图和深度图以话题形式发布出来，然后slam***就监听这两个话题来接收采集到的彩色和深度图；移动机器人以90hz频率发布编码器数据，slam***通过监听话题获取编码器数据，然后***分别对编码器数据和图像进行预处理。

优选地，所述对编码器数据预处理具体为：

首先在t时刻，机器人的位姿为ξ_t＝[x_t y_t z_t]^T，根据里程计运动模型，可以得出t+1时刻机器人的位姿为：

其中Δs为机器人中心移动的距离，Δθ为机器人移动的角度值；通过监听编码器话题可以获得编码器左右码盘的读数，编码器左右码盘读数和机器人移动之间满足以下关系：

其中b为两轮之间的轴距，Δs_r和Δs_l为t到t+1时刻的左右码盘的读数差；因此可以得出t到t+1时刻机器人坐标的变换矩阵为：

优选地，所述对图像进行预处理分为对彩色图像和深度图像进行预处理，具体为：

从话题中获得的彩色图像是三通道的彩色图像，必须使用OpenCV库函数将彩色图像转为单通道的灰度图像；同时深度图也是三通道的图像，需要通过一个映射因子，将深度图转化为CV_32F的单通道图像；

对数据进行预处理后就进入追踪模块的初始化部分，将第一帧图像选取为关键帧；然后每当有新的一帧到来时，进入追踪模块的主线程：

(1)使用图像金字塔，在12个变换尺度下，提取orb特征点，包括orb关键点和描述子；

(2)使用编码器模型重投影特征点；数据预处理中我们得到了t到t+1时刻机器人坐标的变换矩阵，编码器采集频率是相机采集频率的3倍，而且编码器数据和相机数据具有相同的时间戳可以对应上，因此可以获得当前帧到上一帧的变换矩阵

假设p_c是当前帧某一个像素的相机坐标，u(·)定义相机针孔模型中，将2d像素坐标转换为3d点相机坐标的函数操作；则将当前帧像素p_c投影到前一帧的投影结果可以表示为

其中

为机器人的外参矩阵，由机器人和相机的相对位姿决定的；然后就可以在p_c′中3σ的像素区域内寻找匹配的特征点，编码器误差服从正态分布，其中σ为该正态分布的标准差；

(3)然后选取区域内描述子匹配得分最高的关键点为匹配点；遍历所有匹配点，对匹配点的深度值进行重投影，剔除因为动态环境所造成的误匹配；首先将匹配好的特征点对投影到世界坐标得到3d点p，然后将3d点投影到最近的一帧关键帧上，在关键帧上重建这个地图点

如果这个匹配点对应的地图点是静态的，p′和p之间的深度值是没变化的，所以如果深度值之差大于一个经验阈值d_th，则认为这个匹配点是动态点，把它的深度值置零；可以由以下公式得到d_th＝d_b+k_d*z′，其中k_d与尺度相关，d_b是一个基础阈值，一般让d_b＝0.2m和k_d＝0.025；

(4)调用OpenCV中提供的PNP函数，利用ePNP和匹配好的关键点，算出两帧图片之间的位姿；

(5)最后是关键帧的判定，判断当前帧是否满足称为关键帧的条件，如果同时满足以下条件就认为是关键帧；其中地图点是将关键帧上特征点转化到世界坐标下所得到的；

①距离上一次全局重定位超过20帧；

②局部建图线程处于空闲，或者距离上一次关键帧***超过20帧；

③当前帧至少能追踪50个地图点；

④当前帧能追踪到的地图点要少于参考关键帧中地图点的90％。

优选地，所述局部地图优化模块主要是管理地图点和关键帧，同时通过local BA优化地图点和关键帧之间的转换矩阵；局部地图优化模块的工作步骤如下：

(1)地图点剔除，将不满足下列条件的地图点从***中剔除：

①这个点要在可预测到能够观察到该点的关键帧中，有超过25％的关键帧能够跟踪到这个点；

②它必须被超过三个关键帧观察到；

(2)创建新的地图点；当产生一帧新的关键帧之后，将次关键帧和与之相连的关键帧进行特征匹配，将产生的特征点对进行三角化得到的3d点就是新的地图点；

(3)局部地图的BA优化，利用地图点和关键帧特征点之间重投影的关系，建立关于地图点和特征点之间的一个代价函数，然后相机外参数使得代价函数的误差值最小；

(4)关键帧剔除，如果90％关键点能被超过3个关键帧观察到，那么就认为多出来的关键帧是冗余的关键帧，将它们剔除。

优选地，所述动态物体剔除模块的工作步骤如下：

当追踪模块产生新的关键帧之后，会以ros***话题形式将关键帧对应的彩色图和深度图发布出去，然后动态物体剔除模块线程就会接受这个话题，从而获得需要剔除动态像素的彩色图和深度图；因为关键帧动态物体剔除模块中所用到的语义分割网络是在pytorch平台上实现的，必须用Python语言实现，其他模块都是用c++实现的，因此跨平台之间需要用到ros通信机制；

在进行语义分割之前，必须要对语义分割的网络进行训练，这里采用vocpascal2012数据集对ICNet网络进行训练，选用的原因在于该数据集将背景标注为一类，将其他不同物体，如人、桌子和椅子标成其他类，方便对图像进行语义分割的时候，将背景和动态物体分离；

接收到彩色图像和深度图之后就可以用ICNet对彩色图进行图像分割，分割后的结果是单通道的灰度图，必须先用OpenCV将它处理成三通道的彩色图，然后使用经过预先制作好的色条为彩色图像上标注的标签着色，着色完成后就是最终语义分割的结果；

语义分割会将图片以预先标注好的标签分类进行分割，其中第0类标签为背景，第15类为人，对于室内的动态场景，要剔除的动态对象主要是人；因此先对图像进行二值化，将属于第15类的标签灰度值设为255，然后使用OpenCV区域膨胀函数，使得15类标签的像素区域扩大；然后在深度图中，将对应二值化图像中为255的像素的深度值置为零，实现动态像素的剔除；

最后将剔除动态像素的深度图和经过语义分割处理的彩色图以话题形式发回给slam***。

优选地，所述回环检测模块的工作步骤如下：

局部地图优化模块结束之后，会将关键帧传到回环检测模块，然后回环检测模块会调取当前关键帧的词袋向量，和其他关键帧的词袋向量作相似性检验，如下述公式所示：

其中v₁和v₂是词袋向量，反映的是一副图像中所具有的特征点信息，可以通过调用词袋模型的库函数计算得出；如果当前关键帧和之前某一帧关键帧的相似度，比一个相似度阈值还要高1.1倍，则认为当前关键帧和之前的关键帧具有相同的位姿，回环检测成功，以此作为全局关键帧优化的一个条件；相似度阈值是通过关键帧和邻近四个关键帧进行相似度计算得出的，相似度最高的值被设为相似度阈值。

优选地，所述语义八叉树地图模块的工作步骤如下：

当回环检测结束之后，语义八叉树地图模块接收语义分割后得到的彩色图和深度图；p_a＝[x_a y_a 1]^T为彩色图像中的一个像素，将p_a转化到世界坐标下

遍历彩色图中的每一个像素将它们都转换到世界坐标之下，得到世界坐标系下的3d点；然后将3d点转化为彩色点云数据类型pcl::PointXYZRGB，这个点云数据类型是一个六维变量，其中前三维是点云的世界坐标，后三维是点云的颜色；分割后不同标签在彩色图像上有不同的颜色，根据这个结果为点云数据着色；第0类标签为背景，因此第0类标签对应的像素转化为点云数据之后，就将这些点云设为银白色；其他标签依次设为不同颜色；

有了包含语义信息的点云数据之后，就可以直接转换为八叉树地图；调用八叉树地图在c++中的库，创建一个ColorOcTree的八叉树对象，然后将点云数据***这个八叉树地图中，最后将八叉树地图分辨率设为0.05，命中概率设为0.7，未命中概率设为0.55；然后将建立好的八叉树以ros话题/octomap形式发布出来。

本发明与现有技术相比具有以下的有益效果：

(1)本发明追踪部分使用了编码器数据作为相机重投影时的初始位姿，实现了在动态环境中精准定位追踪，比传统的语义slam追踪模块需要每一帧剔除动态像素，节省了大量时间；把语义分割部分仅放在了关键帧，能够剔除地图中的动态像素，建立可靠的静态地图；同时创新性地将语义分割部分放在了TensorFlow平台，有别于传统语义slam的caffe平台，不仅提高语义分割的时间，还提高了语义分割的精度；

(2)和融合编码器的slam方案不一样，本发明仅把编码器作为一个“恒速模型”——在重投影彩色图特征点的时候用到，传统的融合编码器数据的动态环境slam会将编码器作为主传感器，在地图局部优化和回环检测里面都会优化编码器数据；但由于编码器容易发生打滑，数据有可能会失效，因此把编码器当作主传感器可能会在某些场景存在相当大的误差，而仅把编码器当作恒速模型就可以有效的避免这个问题。

附图说明

图1为本发明的流程图；

图2为本发明采用ICNet网络与常用语义分割网络性能对比图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

本发明提出了一种适用于动态环境的即时定位与地图构建方法，在DS_SLAM***的基础上，融入了编码器数据，使得在动态环境中的跟踪部分更具鲁棒性，仅对关键帧部分进行图像分割，将图像分割结果仅用于建图部分，极大地缩短了时间；此外还创新性的结合了TensorFlow平台上的语义分割网络，比起传统的caffe框架下的语义分割网络，新网络具有更好的性能；新网络引入ros***多节点通信机制，为语义分割单独开辟一个节点线程；最后根据语义分割的结果建立语义八叉树地图。

具体来说，如图1～2所示，一种适用于动态环境的即时定位与地图构建方法，包括下述步骤：

步骤一，首先对深度相机和编码器进行外参标定，获得编码器坐标到深度相机坐标的一个变换矩阵；然后对深度相机采集的每一帧图像使用图像金字塔分层，然后从图像金字塔的每一层提取ORB特征点；将编码器获得的数据作为追踪模块的恒速模型，然后将前一帧关键帧的特征点投影到当前帧上，与当前帧的特征点进行匹配，匹配成功的关键点通过三角测量得到两帧之间的位姿；

其中，追踪模块的工作步骤如下：

深度相机以30帧每秒的频率采集彩色图像和深度图像，通过ros中iai_kinect2软件包将彩色图和深度图以话题形式发布出来，然后slam***就监听这两个话题来接收采集到的彩色和深度图；移动机器人以90hz频率发布编码器数据，slam***通过监听话题获取编码器数据，然后***分别对编码器数据和图像进行预处理。

对编码器数据预处理具体为：

首先在t时刻，机器人的位姿为ξ_t＝[x_t y_t z_t]^T，根据里程计运动模型，可以得出t+1时刻机器人的位姿为：

其中Δs为机器人中心移动的距离，Δθ为机器人移动的角度值；通过监听编码器话题可以获得编码器左右码盘的读数，编码器左右码盘读数和机器人移动之间满足以下关系：

其中b为两轮之间的轴距，Δs_r和Δs_l为t到t+1时刻的左右码盘的读数差；因此可以得出t到t+1时刻机器人坐标的变换矩阵为：

对图像进行预处理分为对彩色图像和深度图像进行预处理，具体为：

从话题中获得的彩色图像是三通道的彩色图像，必须使用OpenCV库函数将彩色图像转为单通道的灰度图像；同时深度图也是三通道的图像，需要通过一个映射因子，将深度图转化为CV_32F的单通道图像；

对数据进行预处理后就进入追踪模块的初始化部分，将第一帧图像选取为关键帧；然后每当有新的一帧到来时，进入追踪模块的主线程：

(1)使用图像金字塔，在12个变换尺度下，提取orb特征点，包括orb关键点和描述子；

(2)使用编码器模型重投影特征点；数据预处理中我们得到了t到t+1时刻机器人坐标的变换矩阵，编码器采集频率是相机采集频率的3倍，而且编码器数据和相机数据具有相同的时间戳可以对应上，因此可以获得当前帧到上一帧的变换矩阵

假设p_c是当前帧某一个像素的相机坐标，u(·)定义相机针孔模型中，将2d像素坐标转换为3d点相机坐标的函数操作；则将当前帧像素p_c投影到前一帧的投影结果可以表示为

其中

为机器人的外参矩阵，由机器人和相机的相对位姿决定的；然后就可以在p_c′中3σ的像素区域内寻找匹配的特征点，编码器误差服从正态分布，其中σ为该正态分布的标准差；

(3)然后选取区域内描述子匹配得分最高的关键点为匹配点；遍历所有匹配点，对匹配点的深度值进行重投影，剔除因为动态环境所造成的误匹配；首先将匹配好的特征点对投影到世界坐标得到3d点p，然后将3d点投影到最近的一帧关键帧上，在关键帧上重建这个地图点

如果这个匹配点对应的地图点是静态的，p′和p之间的深度值是没变化的，所以如果深度值之差大于一个经验阈值d_th，则认为这个匹配点是动态点，把它的深度值置零；可以由以下公式得到d_th＝d_b+k_d*z′，其中k_d与尺度相关，d_b是一个基础阈值，一般让d_b＝0.2m和k_d＝0.025；

(4)调用OpenCV中提供的PNP函数，利用ePNP和匹配好的关键点，算出两帧图片之间的位姿；

(5)最后是关键帧的判定，判断当前帧是否满足称为关键帧的条件，如果同时满足以下条件就认为是关键帧；其中地图点是将关键帧上特征点转化到世界坐标下所得到的；

①距离上一次全局重定位超过20帧；

②局部建图线程处于空闲，或者距离上一次关键帧***超过20帧；

③当前帧至少能追踪50个地图点；

④当前帧能追踪到的地图点要少于参考关键帧中地图点的90％。

步骤二，追踪模块通过最近一次全局优化离当前帧是否已经过去20帧以上和当前帧是否能检测到50个地图点的原则，判定当前帧是否应选取为关键帧，然后将关键帧传到动态物体剔除模块，动态物体剔除模块使用网络对关键帧进行语义分割；

其中，动态物体剔除模块的工作步骤如下：

当追踪模块产生新的关键帧之后，会以ros***话题形式将关键帧对应的彩色图和深度图发布出去，然后动态物体剔除模块线程就会接受这个话题，从而获得需要剔除动态像素的彩色图和深度图；因为关键帧动态物体剔除模块中所用到的语义分割网络是在pytorch平台上实现的，必须用Python语言实现，其他模块都是用c++实现的，因此跨平台之间需要用到ros通信机制；

在进行语义分割之前，必须要对语义分割的网络进行训练，这里采用vocpascal2012数据集对ICNet网络进行训练，选用的原因在于该数据集将背景标注为一类，将其他不同物体，如人、桌子和椅子标成其他类，方便对图像进行语义分割的时候，将背景和动态物体分离；

接收到彩色图像和深度图之后就可以用ICNet对彩色图进行图像分割，分割后的结果是单通道的灰度图，必须先用OpenCV将它处理成三通道的彩色图，然后使用经过预先制作好的色条为彩色图像上标注的标签着色，着色完成后就是最终语义分割的结果；

语义分割会将图片以预先标注好的标签分类进行分割，其中第0类标签为背景，第15类为人，对于室内的动态场景，要剔除的动态对象主要是人；因此先对图像进行二值化，将属于第15类的标签灰度值设为255，然后使用OpenCV区域膨胀函数，使得15类标签的像素区域扩大；然后在深度图中，将对应二值化图像中为255的像素的深度值置为零，实现动态像素的剔除；

最后将剔除动态像素的深度图和经过语义分割处理的彩色图以话题形式发回给slam***。

步骤三，追踪模块选取的关键帧在局部地图优化模块投影得到地图点，然后使用lacol BA进行优化；然后在回环检测模块中使用DBow2词袋模型进行回环检测，将之前所有的地图点和关键帧用BA进行全局优化，既优化机器人的位姿也优化地图点；

其中，局部地图优化模块主要是管理地图点和关键帧，同时通过local BA优化地图点和关键帧之间的转换矩阵；局部地图优化模块的工作步骤如下：

(1)地图点剔除，将不满足下列条件的地图点从***中剔除：

①这个点要在可预测到能够观察到该点的关键帧中，有超过25％的关键帧能够跟踪到这个点；

②它必须被超过三个关键帧观察到；

(2)创建新的地图点；当产生一帧新的关键帧之后，将次关键帧和与之相连的关键帧进行特征匹配，将产生的特征点对进行三角化得到的3d点就是新的地图点；

(3)局部地图的BA优化，利用地图点和关键帧特征点之间重投影的关系，建立关于地图点和特征点之间的一个代价函数，然后相机外参数使得代价函数的误差值最小；

(4)关键帧剔除，如果90％关键点能被超过3个关键帧观察到，那么就认为多出来的关键帧是冗余的关键帧，将它们剔除。

其中，回环检测模块的工作步骤如下：

局部地图优化模块结束之后，会将关键帧传到回环检测模块，然后回环检测模块会调取当前关键帧的词袋向量，和其他关键帧的词袋向量作相似性检验，如下述公式所示：

其中v₁和v₂是词袋向量，反映的是一副图像中所具有的特征点信息，可以通过调用词袋模型的库函数计算得出；如果当前关键帧和之前某一帧关键帧的相似度，比一个相似度阈值还要高1.1倍，则认为当前关键帧和之前的关键帧具有相同的位姿，回环检测成功，以此作为全局关键帧优化的一个条件；相似度阈值是通过关键帧和邻近四个关键帧进行相似度计算得出的，相似度最高的值被设为相似度阈值。

步骤四，接收全局优化后的关键帧和语义分割后的结果，将关键帧投影到3d点云，根据不同的语义分割的标签为点云着色，然后在语义八叉树地图模块中建立语义八叉树地图。

其中，语义八叉树地图模块的工作步骤如下：

当回环检测结束之后，语义八叉树地图模块接收语义分割后得到的彩色图和深度图；p_a＝[x_a y_a 1]^T为彩色图像中的一个像素，将p_a转化到世界坐标下

遍历彩色图中的每一个像素将它们都转换到世界坐标之下，得到世界坐标系下的3d点；然后将3d点转化为彩色点云数据类型pcl::PointXYZRGB，这个点云数据类型是一个六维变量，其中前三维是点云的世界坐标，后三维是点云的颜色；分割后不同标签在彩色图像上有不同的颜色，根据这个结果为点云数据着色；第0类标签为背景，因此第0类标签对应的像素转化为点云数据之后，就将这些点云设为银白色；其他标签依次设为不同颜色；

有了包含语义信息的点云数据之后，就可以直接转换为八叉树地图；调用八叉树地图在c++中的库，创建一个ColorOcTree的八叉树对象，然后将点云数据***这个八叉树地图中，最后将八叉树地图分辨率设为0.05，命中概率设为0.7，未命中概率设为0.55；然后将建立好的八叉树以ros话题/octomap形式发布出来。

传统的语义分割slam一般是基于caffe框架下的，caffe框架下可选择的语义分割网络不多，不仅速度慢分割效果也不好；而本发明如图2所示，横坐标为每秒钟处理的帧数，纵坐标为图像的精度分割，单位是mloU％，ICNet(ours)无论是在网络前向运行时间还是在精确度上都远远领先于传统语义分割框架用的Segnet网络，而且性能可以达到实时性。

相对于现有技术中动态环境下基于激光雷达的SLAM方法、动态稀疏环境下机器人SLAM物体状态检测方法和基于SLAM与步态IMU融合的室内人员自主定位方法，本发明的主要创新点在于：

1、使用了一种创新性融合编码器数据的方法，仅把编码器数据作为相机当前帧和关键帧之间重投影的变换矩阵，实现了编码器数据和相机数据的浅层融合；避免了编码器数据因打滑存在误差的时候对***的巨大影响。

2、利用ros***实现c++和Python多节点通信，从而创新性地将TensorFlow框架下的语义分割网络加入到slam***中，有效提高了语义分割的性能。

3、创新性地剔除了语义八叉树地图，充分利用语义分割得到的图像信息，建立一个更完整更实用性的语义地图。

本发明追踪部分使用了编码器数据作为相机重投影时的初始位姿，实现了在动态环境中精准定位追踪，比传统的语义slam追踪模块需要每一帧剔除动态像素，节省了大量时间；把语义分割部分仅放在了关键帧，能够剔除地图中的动态像素，建立可靠的静态地图；同时创新性地将语义分割部分放在了TensorFlow平台，有别于传统语义slam的caffe平台，不仅提高语义分割的时间，还提高了语义分割的精度。

和融合编码器的slam方案不一样，本发明仅把编码器作为一个“恒速模型”——在重投影彩色图特征点的时候用到，传统的融合编码器数据的动态环境slam会将编码器作为主传感器，在地图局部优化和回环检测里面都会优化编码器数据；但由于编码器容易发生打滑，数据有可能会失效，因此把编码器当作主传感器可能会在某些场景存在相当大的误差，而仅把编码器当作恒速模型就可以有效的避免这个问题。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种适用于动态环境的即时定位与地图构建方法，其特征在于，包括下述步骤：

步骤一，首先对深度相机和编码器进行外参标定，获得编码器坐标到深度相机坐标的一个变换矩阵；然后对深度相机采集的每一帧图像使用图像金字塔分层，然后从图像金字塔的每一层提取ORB特征点；将编码器获得的数据作为追踪模块的恒速模型，然后将前一帧关键帧的特征点投影到当前帧上，与当前帧的特征点进行匹配，匹配成功的关键点通过三角测量得到两帧之间的位姿；

步骤二，追踪模块通过最近一次全局优化离当前帧是否已经过去20帧以上和当前帧是否能检测到50个地图点的原则，判定当前帧是否应选取为关键帧，然后将关键帧传到动态物体剔除模块，动态物体剔除模块使用网络对关键帧进行语义分割；

当追踪模块产生新的关键帧之后，会以ros***话题形式将关键帧对应的彩色图和深度图发布出去，然后动态物体剔除模块线程就会接受这个话题，从而获得需要剔除动态像素的彩色图和深度图；因为关键帧动态物体剔除模块中所用到的语义分割网络是在pytorch平台上实现的，必须用Python语言实现，其他模块都是用c++实现的，因此跨平台之间需要用到ros通信机制；

在进行语义分割之前，必须要对语义分割的网络进行训练，这里采用vocpascal2012数据集对ICNet网络进行训练，选用的原因在于该数据集将背景标注为一类，将其他不同物体，包括人、桌子和椅子标成其他类，方便对图像进行语义分割的时候，将背景和动态物体分离；

接收到彩色图像和深度图之后就用ICNet对彩色图进行图像分割，分割后的结果是单通道的灰度图，必须先用OpenCV将它处理成三通道的彩色图，然后使用经过预先制作好的色条为彩色图像上标注的标签着色，着色完成后就是最终语义分割的结果；

语义分割会将图片以预先标注好的标签分类进行分割，其中第0类标签为背景，第15类为人，对于室内的动态场景，要剔除的动态对象是人；因此先对图像进行二值化，将属于第15类的标签灰度值设为255，然后使用OpenCV区域膨胀函数，使得15类标签的像素区域扩大；然后在深度图中，将对应二值化图像中为255的像素的深度值置为零，实现动态像素的剔除；

最后将剔除动态像素的深度图和经过语义分割处理的彩色图以话题形式发回给slam***；

步骤三，追踪模块选取的关键帧在局部地图优化模块投影得到地图点，然后使用局部地图BA进行优化；然后在回环检测模块中使用DBow2词袋模型进行回环检测，将之前所有的地图点和关键帧用BA进行全局优化，既优化机器人的位姿也优化地图点；

步骤四，接收全局优化后的关键帧和语义分割后的结果，将关键帧投影到3d点云，根据不同的语义分割的标签为点云着色，然后在语义八叉树地图模块中建立语义八叉树地图；

所述追踪模块的工作步骤如下：

深度相机以30帧每秒的频率采集彩色图像和深度图像，通过ros中iai_kinect2软件包将彩色图和深度图以话题形式发布出来，然后slam***就监听这两个话题来接收采集到的彩色和深度图；移动机器人以90hz频率发布编码器数据，slam***通过监听话题获取编码器数据，然后***分别对编码器数据和图像进行预处理；

对图像进行预处理分为对彩色图像和深度图像进行预处理，具体为：

从话题中获得的彩色图像是三通道的彩色图像，必须使用OpenCV库函数将彩色图像转为单通道的灰度图像；同时深度图也是三通道的图像，需要通过一个映射因子，将深度图转化为CV_32F的单通道图像；

对数据进行预处理后就进入追踪模块的初始化部分，将第一帧图像选取为关键帧；然后每当有新的一帧到来时，进入追踪模块的主线程包括步骤(1)至(5)：

(1)使用图像金字塔，在12个变换尺度下，提取orb特征点，包括orb关键点和描述子；

(2)使用编码器模型重投影特征点；数据预处理中得到了t到t+1时刻机器人坐标的变换矩阵，编码器采集频率是相机采集频率的3倍，而且编码器数据和相机数据具有相同的时间戳对应上，因此获得当前帧到上一帧的变换矩阵

T_rr表示t到t+1时刻机器人坐标的变换矩阵；假设p_c是当前帧某一个像素的相机坐标，u(·)定义相机针孔模型中，将2d像素坐标转换为3d点相机坐标的函数操作；则将当前帧像素p_c投影到前一帧的投影结果表示为

其中

为机器人的外参矩阵，由机器人和相机的相对位姿决定的；然后就在p_c′中3σ的像素区域内寻找匹配的特征点，编码器误差服从正态分布，其中σ为该正态分布的标准差；

(3)然后选取区域内描述子匹配得分最高的关键点为匹配点；遍历所有匹配点，对匹配点的深度值进行重投影，剔除因为动态环境所造成的误匹配；首先将匹配好的特征点对投影到世界坐标得到3d点p，然后将3d点投影到最近的一帧关键帧上，在关键帧上重建这个地图点

如果这个匹配点对应的地图点是静态的，p′和p之间的深度值是没变化的，所以如果深度值之差大于一个经验阈值d_th，则认为这个匹配点是动态点，把它的深度值置零；由以下公式得到d_th＝0.2+0.025*z′；

(4)调用OpenCV中提供的ePNP函数，利用ePNP函数和匹配好的关键点，算出两帧图片之间的位姿；

(5)最后是关键帧的判定，判断当前帧是否满足称为关键帧的条件，如果同时满足以下条件就认为是关键帧；其中地图点是将关键帧上特征点转化到世界坐标下所得到的；

①距离上一次全局重定位超过20帧；

②局部建图线程处于空闲，或者距离上一次关键帧***超过20帧；

③当前帧至少能追踪50个地图点；

④当前帧能追踪到的地图点要少于参考关键帧中地图点的90％；

所述语义八叉树地图模块的工作步骤如下：

当回环检测结束之后，语义八叉树地图模块接收语义分割后得到的彩色图和深度图；p_a＝[x_a y_a 1]^T为彩色图像中的一个像素，将p_a转化到世界坐标下

遍历彩色图中的每一个像素将它们都转换到世界坐标之下，得到世界坐标系下的3d点；然后将3d点转化为彩色点云数据类型pcl::PointXYZRGB，这个点云数据类型是一个六维变量，其中前三维是点云的世界坐标，后三维是点云的颜色；分割后不同标签在彩色图像上有不同的颜色，根据这个结果为点云数据着色；第0类标签为背景，因此第0类标签对应的像素转化为点云数据之后，就将这些点云设为银白色；其他标签依次设为不同颜色；

有了包含语义信息的点云数据之后，就直接转换为八叉树地图；调用八叉树地图在c++中的库，创建一个ColorOcTree的八叉树对象，然后将点云数据***这个八叉树地图中，最后将八叉树地图分辨率设为0.05，命中概率设为0.7，未命中概率设为0.55；然后将建立好的八叉树以ros话题/octomap形式发布出来。

2.根据权利要求1所述的适用于动态环境的即时定位与地图构建方法，其特征在于，所述对编码器数据预处理具体为：

首先在t时刻，机器人的位姿为ξ_t＝[x_t y_t θ_t]^T，根据里程计运动模型，得出t+1时刻机器人的位姿为：

其中Δs为机器人中心移动的距离，Δθ为机器人移动的角度值；通过监听编码器话题获得编码器左右码盘的读数，编码器左右码盘读数和机器人移动之间满足以下关系：

其中b为两轮之间的轴距，Δs_r为t到t+1时刻的左码盘的读数差，Δs_l为t到t+1时刻的右码盘的读数差；因此得出t到t+1时刻机器人坐标的变换矩阵为：

3.根据权利要求1所述的适用于动态环境的即时定位与地图构建方法，其特征在于，所述局部地图优化模块用于管理地图点和关键帧，同时通过局部地图BA优化地图点和关键帧之间的转换矩阵；局部地图优化模块的工作步骤如下：

A.地图点剔除，将不满足下列条件的地图点从***中剔除：

①这个点要在可预测到能够观察到该点的关键帧中，有超过25％的关键帧能够跟踪到这个点；

②它必须被超过三个关键帧观察到；

B.创建新的地图点；当产生一帧新的关键帧之后，将次关键帧和与之相连的关键帧进行特征匹配，将产生的特征点对进行三角化得到的3d点就是新的地图点；

C.局部地图的BA优化，利用地图点和关键帧特征点之间重投影的关系，建立关于地图点和特征点之间的一个代价函数，然后相机外参数使得代价函数的误差值最小；

D.关键帧剔除，如果90％关键点能被超过3个关键帧观察到，那么就认为多出来的关键帧是冗余的关键帧，将它们剔除。

4.根据权利要求1所述的适用于动态环境的即时定位与地图构建方法，其特征在于，所述回环检测模块的工作步骤如下：

局部地图优化模块结束之后，会将关键帧传到回环检测模块，然后回环检测模块会调取当前关键帧的词袋向量，和其他关键帧的词袋向量作相似性检验，如下述公式所示：

其中s(v₁,v₂)表示相似性检验结果，v₁和v₂是词袋向量，反映的是一幅 图像中所具有的特征点信息，通过调用词袋模型的库函数计算得出；如果当前关键帧和之前某一帧关键帧的相似度，比一个相似度阈值还要高1.1倍，则认为当前关键帧和之前的关键帧具有相同的位姿，回环检测成功，以此作为全局关键帧优化的一个条件；相似度阈值是通过关键帧和邻近四个关键帧进行相似度计算得出的，相似度最高的值被设为相似度阈值。

Images