[发明专利]一种适用于动态环境的即时定位与地图构建方法

权利要求书

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形式发布出来。