CN109387204B - 面向室内动态环境的移动机器人同步定位与构图方法 - Google Patents

Abstract

本发明提出了一种面向室内动态环境的移动机器人同步定位与构图方法，该方法可以有效解决室内动态环境下移动机器人的定位和导航地图构建问题。具体地，通过最小化图像光度误差实现对相机位姿的初步估计。接着，估计对图像进行运动补偿，并分割出图像中的运动物体，剔除投影在图像运动区域的局部地图点，对相机位姿进行进一步优化。最后，使用优化后的相机位姿和相机图像信息构建TSDF稠密地图，利用图像和地图两个层次的运动检测，完成地图在动态环境下的实时更新。实验结果表明，在室内动态环境下，本发明能够有效提高相机定位精度，实现稠密地图的实时更新，在提升系统鲁棒性的同时也丰富了机器人感知到的环境信息。

Description

面向室内动态环境的移动机器人同步定位与构图方法

技术领域

本发明涉及一种数字图像处理领域及计算机视觉领域，具体涉及一种面向室内动态环境的移动机器人同步定位与构图方法。

背景技术

视觉SLAM(Simultaneous Localization and Mapping，同时定位与地图构建)是利用单个或者多个相机输出的连续图像序列对机器人进行位姿估计的方法。为了实现移动机器人实时准确的定位，基于视觉的实时定位与构图系统得到了广泛应用。

尤其是基于RGB-D相机的SLAM系统，可以直接使用相机提供的彩色和深度信息实现相机的定位和环境信息的感知。为了简化问题，现阶段大部分的视觉SLAM系统都假设相机所处环境是静态的或者是图像信息的变化仅依赖于相机自身的运动。但是，在实际环境中不可避免地存在着动态物体，如走动的行人、移动的桌椅等。尽管可以采取一些算法 (如随机采样一致性，RANSAC)对小的运动物体引入的外点进行处理，但是动态物体在图像中的占比较大的时候，就会给SLAM系统引入误差，得到错误的位姿信息，进而影响到地图的准确构建。因此，去除SLAM 地图中由环境动态物体引起的外点是提升SLAM定位精度和鲁棒性的一个十分重要的环节。

目前针对动态场景下的SLAM问题的解决方法可以分为两种：第一类是以里程计中的地图点为主，依据地图点的分布构造概率模型，剔除动态特征。这类方法一般是根据场景中静态特征点分布更均匀、数量更多、更加符合相机运动模型这一特性，迭代筛选出图像中的静态特征点。或者是为每一个地图点设置了静态权重，认为地图点静态权重符合t分布，根据帧间相机位姿计算当前地图点与目标地图点的欧式距离，进而更新地图点的静态权重。该算法的优点在于实时性高，缺点是当相机的运动较为复杂时，地图点的概率模型会含有较大的噪声，进而会在地图点的筛选中引入较大的误差。

第二类方法是引入运动物体检测算法(Motion Object Detection, MOD)，将图像区域分为静态特征区和动态特征区。在对图像进行运动检测时，可以使用稠密光流对图像序列进行运动分割，仅在静态特征区域提取特征点完成相机位姿解算。或者利用帧差法实现了运动物体的分割，然后使用量化的深度图像实现了对稠密点云地图的分割。该类算法的优点在于基本不受相机运动影响，利用分割后的图像对地图点的剔除较为彻底，缺点是算法实时性不高，甚至要借助于GPU加速才能完成。

综上所述，目前对于动态环境下的SLAM问题，现有的SLAM算法都有静态环境的假设，即使加入地图点静态模型或者使用稠密光流分割算法也无法兼顾准确性和实时性，而将高帧率的半直接法SLAM算法和图像运动检测算法融合可以有效的解决这一问题。

发明内容

针对现有技术的问题，一种面向室内动态环境的移动机器人同步定位与构图方法。

本发明的技术方案如下：

一方面，本发明提供一种面向室内动态环境的移动机器人同步定位与构图方法，包括：

S1、根据室内相机采集的图像，通过划分图像块的方式从图像的关键帧中提取角点并恢复出3D地图点转换到相机坐标系，通过最小化光度误差，获取当前帧相机的位姿；

S2、获取当前帧与参考关键帧之间的角点匹配关系，以及统计当前帧与参考关键帧共同观测的地图点的个数，作为两者之间的共视强度；

所述参考关键帧为与所述当前帧相邻的N个关键帧中时间间隔最小的关键帧，N为大于等于1的自然数；

S3、依据所述共视强度，对所述相机采集的图像进行运动检测，分割出每一张图像的静态区域和动态区域；

S4、将所述动态区域的特征点剔除，并更新所述相机的位姿；

S5、筛选用于构建地图的关键帧；

S6、根据所述相机所在室内空间的信息和筛选的关键帧、更新后的相机的位姿，构建稠密地图。

可选地，所述步骤S1包括：

S11、从相机采集分辨率为640*480的RGB图像I_k、I_k-1，当图像I_k或者I_k-1为关键帧时，将图像I_k、I_k-1分割成大小为P*P的图像块，在每一个图像块中提取角点，并筛选出响应值比较高的角点作为当前图像的角点；

S12、基于所述相机在深度图像中的深度值构造的一系列3D点P_w，在图像I_k、I_k-1上投影后的像素灰度差值δI(T_k,k-1,P_w)为：

δI(T_k,k-1,P_w)＝I_k(π(T_k,wP_w))-I_k-1(π(T_k-1,wP_w)) 公式一；

π表示用相机投影函数，T∈SE(3)为相机位姿变换，T_k-1,w和T_k,w分别表示第k-1与第k帧相机的位姿的逆；

S13、通过最小化光度误差，转换为公式二：

第i个地图点在世界坐标系下的坐标，T_k,k-1表示第k-1帧相机到第k 帧相机的位姿变换；

将公式二转换为公式三：

M_k-1表示第k-1时刻相机坐标系下的空间点集合，

表示第i个地图点在k-1帧相机标系下的坐标，T(ξ)表示帧间位姿更新量；

求解公式三，获取相机位姿估计值

可选地，所述步骤S2包括：

S21、将当前帧图像分割为4*4的图像块；在离当前帧空间距离较小的关键帧中，选取在当前帧图像I_k上投影点最多的前m个关键帧，m为大于1的自然数；

S22、将这m个关键帧的所有地图点投影到当前帧图像上，每一个图像块中会包含多个投影点，将这些投影点对应的地图点集合记为P_src；

S23、在地图点集合P_src中选择在之前的图像帧中匹配成功次数最多的地图点P_i；

S24、在观测到P_i的所有关键帧中离当前帧最近的关键帧K_j上，取P_i的匹配点作为目标观测点x_i，构成已知量集合{P_i,K_j,x_i}；

S25、在步骤S1中得到相机初始位姿之后，根据当前帧与关键帧K_j之间的位姿将地图点P_i投影到到当前帧图像上作为当前帧的特征点x_i′。

可选地，所述步骤S25包括：

S251、引入仿射矩阵A，通过最小化光度误差来优化特征点x_i′的位置，

I_j为关键帧K_j对应的灰度图像，I_k(x_i′)表示第k帧图像在x_i′处的像素值，I_j(x_i)表示第j个关键帧的图像在x_i处的像素值；

S252、引入灰度值补偿量γ作为一个优化量处理，则特征点x_i′的位置算式变化为公式六：

利用逆向构造法将公式六写为

其中P_i是图像I_k中以特征点x_i′^l为中心，大小为4*4的图像块，I_k(x_i′^l)为图像块P_i中第l个像素点的像素值，Δx为x_i′的更新变化量,

将关于Δx和灰度补偿量γ的变化量Δγ合并为一个3*1列向量为Δp，根据公式八求解出Δp，

其中P_r是图像I_r中以特征点x_i为中心，大小为4×4的图像块；

使用高斯牛顿求解公式七更新匹配点x_i′，

x_i′＝x_i′-Δx 公式九。

可选地，所述步骤S3包括：

S31、在运动检测中，使用KLT跟踪算法寻找相邻图像上的匹配角点，然后计算单应性矩阵H_k,k-1，k和k-1表示前后两个时刻；对当前帧图像进行运动补偿

x_k＝H_k,k-1x_k-1 公式十；

其中x_k表示当前帧图像角点，x_k-1表示上一帧图像角点；

S32、在步骤S2中获得每一帧图像与参考关键帧之间的匹配点，进而会得到前后两帧之间的匹配点；接着，根据当前帧和上一帧的2D匹配点集，并利用Ransac算法计算出这两帧图像的H矩阵；

S33、为当前帧的每一个图像块建立两个SGM模型，一个是背景模型和另一个是候选背景模型，当新的图像到来后更新其均值

方差

以及模型更新次数η，

其中

定义为当前图像帧第i个图像块的均值和方差，图像块的大小取4×4像素点；

S34、在运动检测的过程中相机也会运动，图像块的高斯模型会受到周围图像块的影响，计算周围图像块对当前图像块的影响权重；

S35、根据计算的影响权重将周围图像块的均值和方差统一叠加到当前图像块上；

在计算出前后帧图像的H矩阵之后，根据公式十二进行图像运动补偿，即计算出图像块中心移动的方向和距离，并转换为当前图像块与原图像的图像块的重叠面积，则根据重叠面积利用插值法计算图像块高斯模型的均值和方差；

令背景模型的SGM参数为

候选背景模型的参数为

如果当前帧图像I_k的图像块P_i的均值

满足

则用图像图像块P_i更新背景模型，θ_s为更新阈值，取常数2；若

满足

则更新候选背景模型；

S36、当公式十四和公式十五都不满足时，选择背景模型，并将候选背景模型初始化；当候选背景模型的更新次数

大于背景模型的更新次数

时，交换前景模型和候选前景模型的参数，并将候选背景模型重新初始化；

S37、在得到背景模型SGM的参数之后，若图像块中的任意像素点灰度值满足

则该像素点属于背景模型，即静态区域，θ_d为阈值参数，取常数4。

可选地，所述步骤S4包括：

S41、在消除由场景中的运动物体引入的外点时，将当前帧图像中落在动态区域的特征点剔除；

在当前帧图像动态区域成功投影匹配的地图点中，属于动态物体引起，而非遮挡引起误匹配的地图点设置为外点，并将其从局部地图中剔除，得到静态地图点集合P_s；

S42、在获取了当前帧观测到的静态地图点集合P_s和地图点对应的角点集合x_s后，通过最小化重投影误差进一步优化相机位姿：

其中，

为世界坐标系到当前帧相机坐标系的变换矩阵；

地图点的优化在局部地图的优化中完成。

可选地，所述步骤S5包括：

S51、关键帧选择与局部地图管理；

局部地图由关键帧和地图点组成，关键帧选择遵从以下规则：

第一、当前帧至少跟踪到20个地图点；

第二、在上一次关键帧插入之后，跟踪线程已跟踪到20帧图像；

第三、在动态地图点剔除之前，当前帧与其所有共视关键帧的平均场景深度之差大于阈值D_thresh；

第四、当前帧与上一个关键帧的姿态角差值或位置差值大于阈值T_thresh。

可选地，所述步骤S6包括：

S61、以相机的初始位姿为基准，将整个室内空间分割为N个立方体，每个立方体代表TSDF地图中的一个体素块Voxel，并将体素块中心点的世界坐标系坐标当作该体素块的索引；

S62、将m×m个Voxel存入到一个较大的块Chunk中，并将其索引以 Hash结构存储；在t时刻，相机定位阶段有位姿更新时，将相机视野内的Voxel投影到深度相机上，设Voxel中心点到相机的距离与投影点深度值的差值为μ_t，称为截断距离；

S63、假设有，Voxel A和B均投影在深度图和彩色图中像素点x处， x的深度值实际为曲面Φ＝0上P点的深度值；当Voxel处于曲面与相机中间时，阶段距离为正值，反之为负值，处于曲面上时截断距离为0；Voxel在t时刻颜色的测量值即为投影点的像素值；Voxel截断距离的更新，表示为

其中，

为Voxel在t时刻的截断距离，α为更新权重，取常数0.5，Voxel颜色的更新和截断距离同理；所以每个Voxel会带有截断距离

颜色信息

和权重

三种信息；

S64、在地图的动态更新过程中，首先检测出受物体运动影响的Voxel块；这部分运动检测的结果来自两个方面；一方面，将里程计阶段检测出的图像动态区域，反投影到地图中，将这部分Voxel标记为动态；另一方面通过Voxel包含的截断距离和颜色的变化判断，即

和

表示前后两个时刻Voxel v_i的截断距离值，δ_d表示前后两个时刻，Voxel v_i的截断距离的变化值，Voxel的灰度变化值δ_c计算同理；若δ_d和δ_c同时大于各自的阈值时，则将该Voxel标记为动态；在动态检测结束之后，对整个三维地图做一次开运算，接着，将标记为动态的Voxel的权重

置0，使其在地图中处于未定义的状态，等待下一次的更新；最后，提取TSDF模型中的零值面，构造三角网格地图，并渲染显示。

可选地，相机为RGB-D相机；

在上述公式中，(·)_w表示世界坐标系，(·)_c表示相机坐标系，(·)_cw表示世界坐标系到相机坐标系的变换；

I_k为k时刻RGB-D相机的灰度数据；x＝(u,v)^T表示像素坐标系像素点坐标，其在相机坐标下对应的3D点用齐次坐标表示为P_c＝(x,y,z)^T，两者对应的关系用相机投影函数π表示为

其中c_x,c_y,f_x,f_y为RGB相机内参，相应的π^-1为相机反投影函数，表示将像素点反投影至相机坐标系；

对于相机坐标系下的空间点P_c，使用变换矩阵T_w,c∈SE(3)将其变换到世界坐标系下，具体可以表示为

相机帧间的位姿变换表示为

本发明具有的有益效果：

本发明方法充分考虑了由环境内动态物体引入到SLAM地图中的动态外点对系统定位精度和鲁棒性造成的影响。通过建立关于图像块的双高斯模型，对图像进行运动区域分割，有效地剔除了投影在图像运动区域的地图点，再一次地对相机位姿做了精求解，同时也有效提高了系统的鲁棒性。最后，建立了基于TSDF模型的稠密地图，利用图像中检测到的运动区域和地图端检测到的运动区域对地图中运动物体进行动态剔除，实现地图的实时更新。构建完成的地图即可以为使用者展示机器人所处的实时三维环境，也可以作为机器人精确导航的初步地图。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1为本发明具体实施方式的系统结构图；

图2为本发明具体实施方式的运动补偿示意图；

图3为本发明具体实施方式的运动检测最终效果图；

图4为本发明具体实施方式的定位精度对比图；

图5为本发明具体实施方式的TSDF模型构造图；

图6为本发明具体实施方式的建图效果对比图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

在以下的描述中，将描述本发明的多个不同的方面，然而，对于本领域内的普通技术人员而言，可以仅仅利用本发明的一些或者全部结构或者流程来实施本发明。为了解释的明确性而言，阐述了特定的数目、配置和顺序，但是很明显，在没有这些特定细节的情况下也可以实施本发明。在其它情况下，为了不混淆本发明，对于一些众所周知的特征将不再进行详细阐述。

实施例一

针对现有SLAM算法无法有效解决动态场景下相机位姿求解的问题，本发明对SLAM算法进行了实时性提升，并且加入了图像运动检测结果，提供了一种融合运动检测的半直接法RGB-D SLAM算法(即面向室内动态环境的移动机器人同步定位与构图方法)。本发明可以有效解决SLAM中的定位和建图问题，为动态变化的室内场景中机器人的定位与导航问题，提供了有效的解决方案。

一方面，在利用半直接法视觉里程计初步求解出相机位姿之后，再借助图像运动检测的结果，对局部稀疏地图内由运动物体引起的外点进行剔除，进而有效提升整个系统的定位精度。

另一方面，可以充分利用深度相机提供的深度图像，对环境进行稠密重建，并且借助图像和地图两个方面的运动检测结果实现对地图的动态实时更新。

实验表明，这种算法可以高效地利用半直接法里程计速度快、精度高的特点，借助图像运动检测结果，实现对运动外点的有效剔除，在提高相机定位精度同时，可以实时更新稠密地图。由于该算法兼顾了实时性和准确性，所以其应用场景也更加普遍，尤其对于室内导航机器人，在提升了定位系统鲁棒性的同时，也极大程度上丰富了机器人感知到的环境信息。

本发明的技术方案是这样实现的：首先，通过最小化图像光度误差，利用稀疏图像对齐算法实现对相机位姿的初步估计。然后，使用里程计的位姿估计对图像进行运动补偿，建立基于图像块实时更新的高斯模型，依据方差变化分割出图像中的运动物体，进而剔除投影在图像运动区域的局部地图点，通过最小化重投影误差对相机位姿进行进一步优化，提升相机位姿估计精度。最后，使用相机位姿和RGB-D相机图像信息构建 TSDF稠密地图，利用图像运动检测结果和地图体素块的颜色变化，完成地图在动态环境下的实时更新。

在一种可能的实现方式中，一种面向室内动态环境的半直接法 RGB-D SLAM算法，包含如下步骤：

步骤一：最小化光度误差，初步估计相机位姿。

虑到场景中动态物体对位姿求取的影响，本申请在提取角点时将图像分割成10*10的图像块，每一个图像块只提取一个角点。由于场景中的动态物体通常集中在图像的某个区域出现，这样可以有效降低运动物体引起的位姿误差。

RGB-D相机可以直接获取图像像素点对应的深度，本申请在里程计的初始化阶段即把第一帧图像作为关键帧，然后在图像中提取关键点，利用其在深度图像中的深度值构造一系列3D点。对于世界坐标系下的一 3D点P_w，设其在图像I_k,I_k-1均具有投影点，那么投影之后的像素灰度差值为

δI(T_k,k-1,P_w)＝I_k(π(T_k,wP_w))-I_k-1(π(T_k-1,wP_w)) (1)

其中π表示用相机投影函数，T∈SE(3)为相机位姿变换，T_k-1,w和T_k,w分别表示第k-1与第k帧相机的位姿的逆。

通过最小化光度误差，可以将其转换为最小二乘问题，

第i个地图点在世界坐标系下的坐标，T_k,k-1表示第k-1帧相机到第k 帧相机的位姿变换；

为了避免在求解(2)式过程中反复计算海森矩阵H_e，使用逆向构造法 (InverseCompositional Algorithm)将(2)式写为

M_k-1表示第k-1时刻相机坐标系下的空间点集合，

表示第i个地图点在k-1帧相机标系下的坐标，T(ξ)表示帧间位姿更新量；

使用高斯牛顿算法迭代求解(3)式，并更新相机位姿估计值

步骤二：求取当前帧和参考关键帧之间的角点匹配关系；

半直接法里程计在提取特征点的时候，只需提取角点，不需计算描述子，因此无法通过特征匹配得到当前帧与关键帧的特征点匹配关系，仅能通过投影关键帧包含的地图点到当前帧图像上，采用最小化光度误差的方式求解匹配关系。

首先，把当前帧图像分割为10*10大小的图像块。在局部地图中选取与在当前帧上投影点最多的k个关键帧，然后将其包含的地图点投影到当前帧图像上，每个图像块中包含多个投影点，对应地图点集合P_src。

在P_src中选择跟踪次数最多的地图点P_i，选取P_i的观测量中离当前帧最近的关键帧K_j对应的匹配点作为目标观测点x_i，构成集合{P_i,K_j,x_i}在当前帧中寻找匹配点。

在上一步得到相机的初始位姿之后，可以根据当前帧与关键帧K_j之间的位姿将地图点P_i投影到到当前帧图像上作为当前帧的特征点。即上一帧得到的相机的初始位姿是在世界坐标系下的绝对位姿，而关键帧的位姿也是世界坐标系下的绝对位姿，所以可以求解出当前帧与关键帧之间的相对位姿，然后把关键帧的地图点转到当前帧的坐标系下。

但是，由于相机位姿和地图点位置的不准确性，会对匹配点的求解引入较大的误差。所以进一步引入仿射矩阵A，通过最小化光度误差来优化特征点x_i′的位置，

I_j为关键帧K_j对应的灰度图像，A为仿射矩阵，I_k(x_i′)表示第k帧图像在x_i′处的像素值，I_j(x_i)表示第j个关键帧的图像在x_i处的像素值；

为了弥补在两个时刻相机不同的曝光时间造成的误差，本申请引入灰度值补偿量γ，并将其也作为一个优化量处理，则特征点x_i′的位置算式为

为了简化计算过程，利用逆向构造法将(6)式写为

其中P_i是图像I_k中以特征点x_i′^l为中心，大小为4*4的图像块，I_k(x_i′^l)为图像块P_i中第l个像素点的像素值，Δx为x_i′的更新变化量；

将关于Δx和灰度补偿量γ的变化量Δγ合并为一个3*1列向量为Δp，可以求解出Δp，

其中P_r是图像I_r中以特征点x_i为中心，大小为4×4的图像块。最后使用高斯牛顿求解(7)式，更新匹配点x_i′，

x_i′＝x_i′-Δx (9)

这里的x_i是指关键帧上的角点，当前帧上的角点表示为x_i′。

步骤三：图像运动检测，分割出图像中的静态和动态区域；

SLAM中的运动物体检测属于基于自由移动相机的运动物体检测，传统的基于静态背景的运动检测方法无法解决此类问题。因此，提出一种基于双单高斯模型(SingleGaussian Model,SGM)，用以构建图像块的灰度值变化模型。两个SGM分别表示背景高斯模型和候选背景高斯模型，在有新的图像输入时，同时更新两个SGM可以避免前景对背景模型的影响，当图像块灰度值的均值变化超过阈值时，便交换背景和候选背景模型。依据图像块方差的变化，区分前景和背景。

本申请在运动检测中，使用KLT跟踪算法寻找相邻图像上的匹配角点，然后计算单应性矩阵H_t,t-1，对图像进行运动补偿：

x_k＝H_k,k-1x_k-1 (10)

其中x_k表示当前帧图像角点，x_k-1表示上一帧图像角点；

然而，在动态环境下使用KLT算法会引入过多的外点，造成H矩阵求解的不准确性。本申请上一个步骤会求解出每一帧图像与参考关键帧之间的匹配点，进而会得到前后两帧之间的匹配关系，即前后帧图像的 2D匹配点集。接着根绝2D匹配点集，并利用Ransac算法计算出前后两帧图像的H矩阵，在一定程度上提升了H矩阵求解的准确性。而且使用Ransac算法可以进一步剔除由运动物体或者跟踪匹配错误造成的外点，为下一个步骤的位姿精求解做准备。

当前帧的每一个图像块建立两个SGM模型，一个是背景模型和另一个是候选背景模型，当新的图像到来后更新其均值

方差

以及模型更新次数η，

其中

定义为当前图像帧第i个图像块的均值和方差，图像块的大小取4×4像素点。

考虑到在运动检测的过程中相机也会运动，图像块的高斯模型会受到周围图像块的影响，因此需要计算周围图像块对当前图像块的影响权重，然后根据权重将周围图像块的均值和方差统一叠加到当前图像块上。在计算出前后帧图像的H矩阵之后，可以根据式(10)进行图像运动补偿，即计算出图像块中心移动的方向和距离，并转换为当前图像块与原图像的图像块的重叠面积，则可以根据重叠面积利用插值法计算图像块高斯模型的均值和方差。

令背景模型的SGM参数为

候选背景模型的参数为

如果当前帧图像的图像块均值

满足

则用图像I_t更新背景模型，θ_s为更新阈值。若

满足

则更新候选背景模型。当两个条件都不满足时，选择背景模型，并将候选背景模型初始化。当候选背景模型的更新次数

大于背景模型的更新次数

时，交换两模型参数，并将候选背景模型重新初始化。

在得到背景模型SGM的参数之后，若图像块中的任意像素点灰度值满足

则该像素点属于背景模型，即静态区域。θ_d为阈值参数。

步骤四：动态点剔除与相机位姿优化

经过上述处理，已成功分割出了图像中的动态区域和静态区域。为了消除由场景中的运动物体引入的外点，需要将当前帧图像中落在图像动态区域的特征点剔除。在当前帧图像动态区域成功投影匹配的地图点，属于动态物体引起的外点，而非遮挡引起的误匹配，因此也需要将这部分地图点设置为外点，并将其从局部地图中剔除，得到静态地图点集合P_s。

在获取了当前帧观测到的静态地图点集合P_s和其匹配点集合x_s后，通过最小化重投影误差进一步优化相机位姿

其中，

为世界坐标系到当前帧相机坐标系的变换矩阵；

因为单位姿节点的BA问题对地图点的约束过于单一，所以仅对相机位姿进行优化，地图点的优化在局部BA中完成。

步骤五：关键帧选择与局部地图管理

局部地图由关键帧和地图点组成，构建局部地图的目的是在尽可能节省计算资源和内存的情况下，加强相机位姿和地图点之间的约束关系，进一步对关键帧和地图点的位姿进行优化，以提升系统整体的输出精度。局部地图的好坏，很大程度上取决于插入的关键帧的质量，本文的关键帧选择遵从以下规则：

(1)当前帧至少跟踪到20个地图点。

(2)在上一次关键帧插入之后，跟踪线程已跟踪到20帧图像。

(3)在动态地图点剔除之前，当前帧与其所有共视关键帧的平均场景深度之差大于阈值D_thresh。

(4)当前帧与上一个关键帧的姿态角差值或位置差值大于阈值T_thresh。

条件(1)可以有效防止在里程计跟丢之后，插入关键帧引起较大的位姿跳动。条件(4)同时考虑了相机的平移和旋转运动带来的视差变化。条件(2)、(3)能够保证场景中有动态物体出现时，及时插入关键帧纠正相机位姿。

局部建图包括关键帧特征提取、地图点构造、地图点和关键帧筛选以及局部BA优化。特别地，在关键帧提取FAST角点构造地图点时，仅在图像的静态区域进行。然后依据地图点和关键帧的共视关系，对地图点和关键帧进行筛选。最后，选择与当前关键帧有共视关系的关键帧以及它们观测到的地图点，构建局部BA问题，对关键帧和地图点的位姿进行进一步优化。

步骤六：稠密地图构建

为了节省地图存储空间、提高地图搜索效率，在稠密地图构建时本文选择了基于TSDF模型的Mesh类地图，而非传统的点云地图。尤其是，基于TSDF模型的地图更加方便管理和动态物体的剔除。本发明以Google Tango的构图算法为基础，加入动态检测模块，实现地图的动态更新。

以相机的初始位姿为基准，将整个空间分割为N个立方体，每个立方体代表TSDF地图中的一个体素块(Voxel)，并将体素块中心点的世界坐标系坐标当作该体素块的索引。为了加快地图更新的搜索速度，将 m×m个Voxel存入到一个较大的块(Chunk)中，并将其索引以Hash结构存储。在t时刻，相机定位阶段有位姿更新时，将相机视野内的Voxel投影到深度相机上，设Voxel中心点到相机的距离与投影点深度值的差值为μ_t，称为截断距离(Truncated distance)。

TSDF模型构造图如图5所示。在图5中，Voxel A和B均投影在深度图和彩色图中像素点x处，x的深度值实际为曲面Φ＝0上P点的深度值。当Voxel处于曲面与相机中间时，阶段距离为正值，反之为负值，处于曲面上时截断距离为0。Voxel在t时刻颜色的测量值即为投影点的像素值。Voxel截断距离的更新，可表示为

其中

为Voxel在t时刻的截断距离，α为更新权重，在本文中取常量0.5。Voxel颜色的更新和截断距离同理。所以每个Voxel会带有截断距离

颜色信息

和权重

三种信息。

在地图的动态更新环节，首先检测出受物体运动影响的Voxel块。这部分运动检测的结果来自两个方面。一方面，将里程计阶段检测出的图像动态区域，反投影到地图中，将这部分Voxel标记为动态；另一方面通过Voxel包含的截断距离和颜色的变化判断，即

和

表示前后两个时刻Voxel v_i的截断距离值，δ_d表示前后两个时刻，Voxel v_i的截断距离的变化值，Voxel的灰度变化值δ_c计算同理。若δ_d和δ_c同时大于各自的阈值时，则将该Voxel标记为动态。在动态检测结束之后，本文对整个三维地图做一次开运算，有效避免了由相机测量噪声引起的假真性(False positive)动态标签。接着，将标记为动态的Voxel的权重

置0，使其在地图中处于未定义的状态，等待下一次的更新。最后，提取TSDF模型中的零值面，构造三角网格地图，并渲染显示。

上述实施例中，部分内容解释说明如下：

在上述公式中，(·)_w表示世界坐标系，(·)_c表示相机坐标系，(·)_cw表示世界坐标系到相机坐标系的变换；I_k为k时刻RGB-D相机的灰度数据。 x＝(u,v)^T表示像素坐标系像素点坐标，其在相机坐标下对应的3D点用齐次坐标表示为P_c＝(x,y,z)^T，两者对应的关系可用相机投影函数π表示为

其中c_x,c_y,f_x,f_y为RGB相机内参，相应的π^-1为相机反投影函数，表示将像素点反投影至相机坐标系。

对于相机坐标系下的空间点P_c，可以使用变换矩阵T_w,c∈SE(3)将其变换到世界坐标系下，具体可以表示为

相机帧间的位姿变换表示为

上述的半直接法RGB-D视觉SLAM(同时定位与构图)算法，由运动检测、相机定位、基于TSDF(Truncated Signed Distance Function)模型的稠密地图构建三个步骤组成，可以有效解决室内动态环境下移动机器人的定位和导航地图构建问题。特别地，在室内动态环境下，本发明能够有效提高相机定位精度，实现稠密地图的实时更新，在提升系统鲁棒性的同时也丰富了机器人感知到的环境信息。

实施例二

本实施方式的方法，硬件配置为Intel E3-1230 CPU，主频为3.30GHz，内存为12G，不使用GPU加速，测试的系统为Ubuntu14.04。使用的相机为一代Kinect。本发明的结构图如图1所示：

步骤一：从Kinect相机采集分辨率为640*480RGB图像I，在I上提取200个FAST角点。提取角点的过程可以简单描述为：首先将图像分割成8*8的图像块，然后单独在每一个图像块中提取FAST角点，只选取响应值最高的角点。如果图像某些区域纹理不够丰富，可能提取不到角点，则直接跳过即可。最后，在挑选出的所有的角点中，选取响应值在前200 的角点作为系统要跟踪的角点。

从Kinect相机采集分辨率为640*480RGB图像D，将前面得到的200 个角点转为3D地图点并加入到地图当中。如果当前图像帧为首帧图像，则将该帧图像当做关键帧处理，并加入到局部地图中。否则，将参考关键帧的地图点投影到当前帧图像上，通过最小化光度误差，求解出当前帧相机的位姿T_wc。

步骤二：在求解出相机的位姿T_wc之后，需要进一步求解当前帧与参考关键帧之间的特征点匹配关系。

首先，更新局部地图，局部地图包含了离当前帧时间和空间上比较近的一系列关键帧和地图点。根据解算出的当前帧的位姿T_wc选取离当前帧最近的10个关键帧加入到局部地图中。为了尽可能的增强帧间约束，将加入局部地图10个关键帧观测到的所有地图点加入到局部地图中，将这些地图点称为集合M_L

然后，局部地图点筛选。如果将全部M_L在当前帧上寻找匹配，会存在由于相机误差、深度值不准确而引起的重复匹配，进而影响到系统的实时性。所以，这里将图像继续分为8*8的图像块，将M_L在全部投影在当前帧上，然后仅筛选出每个图像块中，被跟踪次数最多的地图点。这样既提升了系统的效率，又保持了角点在图像上分布的均匀性。

最后，再次使用光度最小化算法在当前帧图像上求解出局部地图点对应的匹配角点。并且根据角点数目更新当前帧与参考关键帧之间的共视强度。

步骤三：对图像进行运动检测，分割出图像的静态区域和动态区域，进一步筛选地图点，进行位姿精求解。

首先，因为在运动检测的过程中，相机也发生了运动，所以需要对图像进行运动补偿。在步骤二中，计算出了当前帧与参考关键帧之间的角点匹配关系，进一步可以得到前后帧之间的角点匹配关系，进而求解前后帧之间的单应性矩阵H。

然后，为了细化运动检测效果，将图像分割为的图像块，为每一个图像块建立两个高斯模型，分为背景模型和候选背景模型。当有新的图像到来时，则更新当前图像块的均值和方差，并且根据单应性矩阵H，叠加上周围图像块的均值和方差。具体的叠加方式如图2所示。接着，根据每个图像块的均值与其两个高斯模型方差的关系，选择更新背景或者候选背景模型。

最后，判断图像块中任意像素点和背景模型方差的关系，进而判断出该像素点属于前景模型还是后景模型。最终的运动检测效果图如图3 所示。

步骤四：运动外点剔除与相机位姿精求解

表1里程计输出位姿RPE结果

在步骤三中，得到了图像中的运动区域，这里需要进一步将投影在图像运动区域的地图点删除，然后根据最小化重投影误差对相机位姿进行精求解。

完整的里程计在公开TUM的数据集上的测试结果如表1所示。测试的场景分为fr2和fr3两种图像序列，区别主要在于两种序列使用到的相机内参不同。

上述表1中，fr2/desk、fr3/long-house为静态场景，fr2/desk-person、 fr3/sitting-static、fr3/sitting-xyz、fr3/sitting-xyz、fr3/sitting-rpy和 fr3/sitting-halfphere为低动态场景，仅包含人物的肢体运动， fr3/walking-static、fr3/walking-xyz和fr3/walking-rpy为高动态场景，包含了走动的行人。本发明对运动外点剔除前后的SLAM系统的精度也做了对比，轨迹对比图如图4所示。

步骤五：筛选关键帧，维护局部地图。为了避免将每一个视觉帧都加入到后端的优化中，根据相邻关键帧之间共视关系以及距离筛选出一部分关键帧，仅将该部分关键帧加入到后端的优化中。除此之外，还要剔除地图当中共视关系较弱的关键帧和地图点，以此来提高系统效率

步骤六：稠密地图构建与动态更新。

首先，将空间假设为N*N*N的大小，单位为m，然后将空。间分割为2*2*2的体素块，单位为cm，将其作为TSDF模型的最小单元。

然后，根据输入的RGB-D图像，将每一个RGB图像像素点根据其在深度图中的深度图反投影到TSDF地图中。根据反投影点与体素块之间的欧氏距离计算截断距离，当有新的图像输入之后不断的更新该截断距离。

最后，一方面，统计截断距离变化的方差，将方差较大的体素块作为运动体素块处理。另一方面，将图像运动区域的像素点反投影到地图中，将对应的体素块也作为运动体素块处理处理。运动剔除之后对整个地图做渲染显示。最终的构图效果如图6所示。图6(a)和图6(b)分别表示假如行人的地图和有行人走动的地图。

综上，本发明将基于半直接法的视觉里程计作为SLAM前端计算相机位姿，并且融入运动检测算法，完成了对场景中运动物体的检测和动态特征点的剔除，在有效提升了里程计精度的同时，避免了后端闭环检测环节受到动态物体影响。利用SLAM中解算出的相机位姿，基于TSDF 模型构建了Mesh状稠密地图，借助图像和地图两层的运动检测，完成了对地图在动态环境下的实时更新。对于动态环境下机器人定位和导航，本发明算法具有明显的优势。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (2)

1.一种面向室内动态环境的移动机器人同步定位与构图方法，其特征在于，包括：

S1、根据室内相机采集的图像，通过划分图像块的方式从图像的关键帧中提取角点并恢复出3D地图点转换到相机坐标系，通过最小化光度误差，获取当前帧相机的位姿；

S2、获取当前帧与参考关键帧之间的角点匹配关系，以及统计当前帧与参考关键帧共同观测的地图点的个数，作为两者之间的共视强度；

所述参考关键帧为与所述当前帧相邻的N个关键帧中时间间隔最小的关键帧，N为大于等于1的自然数；

S3、依据所述共视强度，对所述相机采集的图像进行运动检测，分割出每一张图像的静态区域和动态区域；

S4、将所述动态区域的特征点剔除，并更新所述相机的位姿；

S5、筛选用于构建地图的关键帧；

S6、根据所述相机所在室内空间的信息和筛选的关键帧、更新后的相机的位姿，构建稠密地图；

所述步骤S1包括：

S11、从相机采集分辨率为640*480的RGB图像I_k、I_k-1，当图像I_k或者I_k-1为关键帧时，将图像I_k、I_k-1分割成大小为P*P的图像块，在每一个图像块中提取角点，并筛选出响应值比较高的角点作为当前图像的角点；

S12、基于所述相机在深度图像中的深度值构造的一系列3D点P_w，在图像I_k、I_k-1上投影后的像素灰度差值δI(T_k,k-1,P_w)为：

δI(T_k,k-1,P_w)＝I_k(π(T_k,wP_w))-I_k-1(π(T_k-1,wP_w)) 公式一；

π表示用相机投影函数，T∈SE(3)为相机位姿变换，T_k-1,w和T_k,w分别表示第k-1与第k帧相机的位姿的逆；

S13、通过最小化光度误差，转换为公式二：

第i个地图点在世界坐标系下的坐标，T_k,k-1表示第k-1帧相机到第k帧相机的位姿变换；

将公式二转换为公式三：

M_k-1表示第k-1时刻相机坐标系下的空间点集合，

表示第i个地图点在k-1帧相机标系下的坐标，T(ξ)表示帧间位姿更新量；

求解公式三，获取相机位姿估计值

；

所述步骤S2包括：

S21、将当前帧图像分割为4*4的图像块；在离当前帧空间距离较小的关键帧中，选取在当前帧图像I_k上投影点最多的前m个关键帧，m为大于1的自然数；

S22、将这m个关键帧的所有地图点投影到当前帧图像上，每一个图像块中会包含多个投影点，将这些投影点对应的地图点集合记为P_src；

S23、在地图点集合P_src中选择在之前的图像帧中匹配成功次数最多的地图点P_i；

S24、在观测到P_i的所有关键帧中离当前帧最近的关键帧K_j上，取P_i的匹配点作为目标观测点x_i，构成已知量集合{P_i,K_j,x_i}；

S25、在步骤S1中得到相机初始位姿之后，根据当前帧与关键帧K_j之间的位姿将地图点P_i投影到到当前帧图像上作为当前帧的特征点x_i′；

所述步骤S25包括：

S251、引入仿射矩阵A，通过最小化光度误差来优化特征点x_i′的位置，

I_j为关键帧K_j对应的灰度图像，I_k(x_i′)表示第k帧图像在x_i′处的像素值，I_j(x_i)表示第j个关键帧的图像在x_i处的像素值；

S252、引入灰度值补偿量γ作为一个优化量处理，则特征点x_i′的位置算式变化为公式六：

利用逆向构造法将公式六写为

其中P_i是图像I_k中以特征点x_i′^l为中心，大小为4*4的图像块，I_k(x_i′^l)为图像块P_i中第l个像素点的像素值，Δx为x_i′的更新变化量,

将关于Δx和灰度补偿量γ的变化量Δγ合并为一个3*1列向量为Δp，根据公式八求解出Δp，

其中P_r是图像I_r中以特征点x_i为中心，大小为4×4的图像块；

使用高斯牛顿求解公式七更新匹配点x_i′，

x_i′＝x_i′-Δx 公式九；

所述步骤S3包括：

S31、在运动检测中，使用KLT跟踪算法寻找相邻图像上的匹配角点，然后计算单应性矩阵H_k,k-1，k和k-1表示前后两个时刻；对当前帧图像进行运动补偿

x_k＝H_k,k-1x_k-1 公式十；

其中x_k表示当前帧图像角点，x_k-1表示上一帧图像角点；

S32、在步骤S2中获得每一帧图像与参考关键帧之间的匹配点，进而会得到前后两帧之间的匹配点；接着，根据当前帧和上一帧的2D匹配点集，并利用Ransac算法计算出这两帧图像的H矩阵；

S33、为当前帧的每一个图像块建立两个SGM模型，一个是背景模型和另一个是候选背景模型，当新的图像到来后更新其均值

方差

以及模型更新次数η，

其中

定义为当前图像帧第i个图像块的均值和方差，图像块的大小取4×4像素点；

S34、在运动检测的过程中相机也会运动，图像块的高斯模型会受到周围图像块的影响，计算周围图像块对当前图像块的影响权重；

S35、根据计算的影响权重将周围图像块的均值和方差统一叠加到当前图像块上；

在计算出前后帧图像的H矩阵之后，根据公式十二进行图像运动补偿，即计算出图像块中心移动的方向和距离，并转换为当前图像块与原图像的图像块的重叠面积，则根据重叠面积利用插值法计算图像块高斯模型的均值和方差；

令背景模型的SGM参数为

候选背景模型的参数为

如果当前帧图像I_k的图像块P_i的均值

满足

则用图像图像块P_i更新背景模型，θ_s为更新阈值，取常数2；若

满足

则更新候选背景模型；

S36、当公式十四和公式十五都不满足时，选择背景模型，并将候选背景模型初始化；当候选背景模型的更新次数

大于背景模型的更新次数

时，交换前景模型和候选前景模型的参数，并将候选背景模型重新初始化；

S37、在得到背景模型SGM的参数之后，若图像块中的任意像素点灰度值满足

则该像素点属于背景模型，即静态区域，θ_d为阈值参数，取常数4；

所述步骤S4包括：

S41、在消除由场景中的运动物体引入的外点时，将当前帧图像中落在动态区域的特征点剔除；

在当前帧图像动态区域成功投影匹配的地图点中，属于动态物体引起，而非遮挡引起误匹配的地图点设置为外点，并将其从局部地图中剔除，得到静态地图点集合P_s；

S42、在获取了当前帧观测到的静态地图点集合P_s和地图点对应的角点集合x_s后，通过最小化重投影误差进一步优化相机位姿：

其中，

为世界坐标系到当前帧相机坐标系的变换矩阵；

地图点的优化在局部地图的优化中完成；

所述步骤S5包括：

S51、关键帧选择与局部地图管理；

局部地图由关键帧和地图点组成，关键帧选择遵从以下规则：

第一、当前帧至少跟踪到20个地图点；

第二、在上一次关键帧插入之后，跟踪线程已跟踪到20帧图像；

第三、在动态地图点剔除之前，当前帧与其所有共视关键帧的平均场景深度之差大于阈值D_thresh；

第四、当前帧与上一个关键帧的姿态角差值或位置差值大于阈值T_thresh；

所述步骤S6包括：

S61、以相机的初始位姿为基准，将整个室内空间分割为N个立方体，每个立方体代表TSDF地图中的一个体素块Voxel，并将体素块中心点的世界坐标系坐标当作该体素块的索引；

S62、将m×m个Voxel存入到一个较大的块Chunk中，并将其索引以Hash结构存储；在t时刻，相机定位阶段有位姿更新时，将相机视野内的Voxel投影到深度相机上，设Voxel中心点到相机的距离与投影点深度值的差值为μ_t，称为截断距离；

S63、假设有，VoxelA和B均投影在深度图和彩色图中像素点x处，x的深度值实际为曲面Φ＝0上P点的深度值；当Voxel处于曲面与相机中间时，阶段距离为正值，反之为负值，处于曲面上时截断距离为0；Voxel在t时刻颜色的测量值即为投影点的像素值；Voxel截断距离的更新，表示为

其中，

为Voxel在t时刻的截断距离，α为更新权重，取常数0.5，Voxel颜色的更新和截断距离同理；所以每个Voxel会带有截断距离

颜色信息

和权重

三种信息；

S64、在地图的动态更新过程中，首先检测出受物体运动影响的Voxel块；这部分运动检测的结果来自两个方面；一方面，将里程计阶段检测出的图像动态区域，反投影到地图中，将这部分Voxel标记为动态；另一方面通过Voxel包含的截断距离和颜色的变化判断，即

和

表示前后两个时刻Voxel v_i的截断距离值，δ_d表示前后两个时刻，Voxel v_i的截断距离的变化值，Voxel的灰度变化值δ_c计算同理；若δ_d和δ_c同时大于各自的阈值时，则将该Voxel标记为动态；在动态检测结束之后，对整个三维地图做一次开运算，接着，将标记为动态的Voxel的权重

置0，使其在地图中处于未定义的状态，等待下一次的更新；最后，提取TSDF模型中的零值面，构造三角网格地图，并渲染显示。

2.根据权利要求1所述的方法，其特征在于，

相机为RGB-D相机；

在上述公式中，(·)_w表示世界坐标系，(·)_c表示相机坐标系，(·)_cw表示世界坐标系到相机坐标系的变换；

I_k为k时刻RGB-D相机的灰度数据；x＝(u,v)^T表示像素坐标系像素点坐标，其在相机坐标下对应的3D点用齐次坐标表示为P_c＝(x,y,z)^T，两者对应的关系用相机投影函数π表示为

其中c_x,c_y,f_x,f_y为RGB相机内参，相应的π^-1为相机反投影函数，表示将像素点反投影至相机坐标系；

对于相机坐标系下的空间点P_c，使用变换矩阵T_w,c∈SE(3)将其变换到世界坐标系下，具体可以表示为

相机帧间的位姿变换表示为

公式二十三。

Images