[发明专利]面向室内动态环境的移动机器人同步定位与构图方法

权利要求书

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)将其变换到世界坐标系下，具体可以表示为

相机帧间的位姿变换表示为公式二十三。