CN112767546B - 移动机器人基于双目图像的视觉地图生成方法 - Google Patents

Abstract

本发明公开了一种移动机器人基于双目图像的视觉地图生成方法，具体根据双目序列图片的视觉特征，将相关图像进行视觉匹配并且检测回环，同时，为了更好地恢复场景尺度，充分利用双目的基线约束，改进点云的参数化表达方式，设计了一种针对双目相机设计的建图初始化算法，增量算法以及对应的全局平滑调整过程。本发明的方法相比现有的双目建图方法，速度更快，复杂度更低，相比基于SLAM的建图算法，地图漂移更小，而且更具备场景鲁棒性，增强了复杂环境的适应能力，满足现有机器人场景构图需求，同时具备高度可扩展性，能够结合SLAM的输出结果加速整个建图环节步骤，同时可以进一步增强鲁棒性。

Description

移动机器人基于双目图像的视觉地图生成方法

技术领域

本发明属于视觉地图技术领域，具体涉及一种移动机器人基于双目图像的视觉地图生成方法。

背景技术

从一系列图片中恢复场景的结构建立定位使用的地图，是机器人导航中的关键问题之一。从运动中恢复场景结构，是利用运动对传感器产生的观测信息，逐步利用多视图几何的信息计算传感器自身的位置同时恢复地图点坐标。其挑战如下：

(1)传统的视觉SLAM(Simultaneous Localization and Mapping，同步定位与地图构建)算法，在精度和鲁棒性方面往往高度依赖纹理，同时也存在精度差，闭环不充分，姿态存在累积误差等问题。在一些极端情况(例如机器碰撞，越坎等条件)下容易出现跟踪丢失，导致建图不完整。引入IMU(惯性测量单元)等惯性传感器能够一定程度上缓解跟踪丢失的问题，但姿态累积误差依然严重，同时在一些极端情况会带来噪声极大的观测导致建图整体算法受到影响。

(2)为了避免SLAM算法的精度误差给最终地图带来影响，基于纯粹的增量式建图方法被提出。该类方法只利用图像信息，在关键帧上离线地使用更强更耗时的视觉匹配方式，不依赖SLAM的信息建立地图。但该建图方法依然存在大量的姿态漂移，同时也由于在增量过程中多次进行平滑计算，计算不仅结果存在不佳的情况，过程往往也特别耗时。

发明内容

有鉴于此，本发明的目的在于避免现有技术中的不足而提供一种移动机器人基于双目图像的视觉地图生成方法，当机器人在未知场景采集了双目数据后，利用视觉特征进行匹配以及双目基线作为约束，在本方案提供的算法策略下建立出符合真实场景的稀疏地图，并且适用于多种多样的环境。

本发明的目的通过以下技术方案实现：提供一种移动机器人基于双目图像的视觉地图生成方法，包括以下步骤：

S1、构建视觉地图生成方法的前端模块，具体表现为：

S11、使用双目相机，在需要建立地图的场景中，采集一系列图片作为场景样本，双目相机在使用前进行双目标定；

S12、选取一定间隔长度的双目图片序列，并提取其中的SIFT视觉特征点以及描述子；

S13、匹配同一时刻的双目图片，同时也进行相邻图片之间的视觉匹配，在所有匹配进行完后，根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配；

S14、对所有匹配的图片进行Essential Matrix的噪点剔除，过滤得到更好的匹配关联；

S15、遍历所有关联，保存每个地图点在每张图片中出现的位置，至此前端模块完成；

S2、构建视觉地图生成方法的后端模块，具体表现为：

S21、双目建图初始化：令集合{s_i}是所有等待建图的图片，寻找关联最紧密的两对双目图片作为起始两帧进行初始化，生成的初始化地图称之为局部地图；

S22、计算下一对双目的姿态：选择和当前局部地图共视最大的图像，作为下一帧，并将该帧从集合中取出并放入集合{I_i}当中，如果此时该帧有SLAM的求解姿态，则与时间上最近的图片比对相对姿态的结果，如果结果相似，则进入步骤S23，如果结果不相似，则进入步骤S24；

S23、生成更多的点云，并根据点云生成情况决定是否全局平滑；

S24、进行全局光束平滑调整操作，判断是否有更多双目图片，若是，则返回步骤S22；反之，则进入步骤S25；

S25、所有图片都有重建结果，构成全局地图。

作为进一步的改进，所述步骤S13具体表现为：

S131、匹配同一时刻的双目图片；

S132、判断是否检测到闭环图像，若是，则将检测到的闭环图像进行匹配后进入步骤S133；若否，则直接进入步骤S133；

S133、将步骤S132检测到的图像与相邻帧图像进行匹配；

S134、判断是否有没匹配到的图片，若是，则返回步骤S12；若否，则根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配后，进入步骤S14。

作为进一步的改进，所述步骤S23具体表现为：寻找集合{I_i}当中还未被计算位置的点云，计算视差角条件，对大于阈值的点云进行线性三角化求解，如果新增的点云数目大于现有点云数目的25％，则进入步骤S24，否则返回步骤S22；如果集合{s_i}中没有更多图片，则进入步骤S24。

作为进一步的改进，所述步骤S21具体包括如下步骤：

步骤a：从图片数据库中选择一帧的双目图片作为起始图片，若其条件满足：具备最多双目匹配合格的地图点，即视差角大于0.05弧度，同时这些地图点出现在其它图片的观测数量大于5，则视为情况A；

步骤b：如果没有找到满足情况A的一对双目图片作为起始帧，只从左目数据集中选取一张图片作为第一帧，该选取图片与其他图片的观测次数最多且观测次数大于15次，视为情况B；

步骤c：选取第二帧双目图片，第二帧的双目图片要求左目图片与第一帧的共同观测数量最多，如果是情况A，则对第二帧的双目相机进行3点pnp姿态求解；如果是情况B，则处理情况分为以下两类：

步骤1)、若此时能够得到SLAM的姿态求解，则通过极限约束来判断姿态能否使用，若可以，则直接进行去除噪点操作后，进入步骤d，否则进入步骤2)；

步骤2)、如此时没有SLAM的求解姿态，则利用双目约束进行点云缩放求解：具体通过第一帧左目和第二帧左目之间的观测进行本质矩阵求解，然后分解得到无尺度姿态，并且对点云进行三角化；再通过第一帧左右目的匹配计算点云坐标，同时除以无尺度点云坐标的数值，得到点云的尺度，并将点云尺度乘上无尺度姿态，进入步骤d；

步骤d：采用下述公式将点云投影到图像平面上，根据投影与观测之间的差值判断是否为噪点，该处观测视为干扰观测：

式中，π(·)为相机投影函数，uv_pre表示预测值，

是相机的姿态变量，表示从世界坐标系到相机坐标系的关系，

表示3×3单位旋转矩阵，

表示3×1平移向量，X_W表示是世界坐标系下的点云坐标；

步骤e：将余下观测所涉及到的图像帧和点云加入到全局光束平滑调整，进行优化全局光束平滑调整，优化的目标函数为：

式中，C_1，2表示第一帧、第二帧的相机姿态，

i的取值为1或2，j的取值为所有点云的编号，从1到N,N表示点云的数目,i≠j,

表示旋转矩阵，

表示平移向量，

表示第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，k的取值为1或2，π₁，π₂是左、右目的投影函数，左右目的姿态关联如下：

是标定环节中给出的参数，表示双目相机之间物理上的旋转和平移，且

步骤f：如果每条约束的平均残差在2个像素值以内，则初始化成功，将目前两帧双目图片从集合{s_i}取出放入集合{I_i}当中，进入步骤S22；否则进入步骤a，重新初始化。

作为进一步的改进，所述步骤a中视差角的计算公式如下：

α＝acos＜R_ib_i，R_jb_j>

其中，acos<·>表示反cos函数，R_i表示第i帧相机姿态中的旋转矩阵，R_j表示第j帧相机姿态中的旋转矩阵，b_i表示该点在第i帧相机中的反投影向量，b_j表示第j帧相机中的反投影向量，反投影向量可以通过反投影函数获得，具体地：

对于针孔相机，反投影函数为：

其中，P_c＝{f_x，f_y，c_x，c_y}，f_x，f_y，c_x，c_y是相机的内参，u，v是该点在图像平面的观测，norm(·)是向量欧式长度归一化算子；

对于鱼眼相机，则有反投影函数：

θ^*＝d^-1(r_θ)

其中，(u，v)是图片上的特征点观测，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数，

为实数集，θ^*是方程θ+k₁θ³+k₂θ⁵+k₃θ⁷+k₄θ⁹＝r_θ的解，θ表示畸变程度；

反投影向量则通过如下公式获得：

b＝π^-1(u，v)。

作为进一步的改进，所述步骤d中π(·)为相机投影函数，具体来说：对于针孔相机前的坐标点

为实数集，x、y、z分别为横坐标、纵坐标和竖坐标，则其相机投影函数为：

其中，X_c表示点云的三维坐标，P_c＝{f_x，f_y，c_x，c_y}，

是相机的内参，f_x、f_y、c_x、c_y分别表示相机x方向的焦距、相机y方向的焦距、横轴的光心、纵轴的光心；

对于鱼眼相机，其投影函数有下列公式：

其中，X_c表示点的空间坐标，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数。

作为进一步的改进，所述步骤S24中全局光束平滑调整优化的目标函数如下：

其中C_i是集合{I_i}中第i帧的相机姿态，

是第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，左右目姿态依然满足所述步骤e的线性约束。

与现有技术相比，本发明所具有的优点主要体现在以下几个方面：

(1)传统方法将双目相机当作两个独立的姿态节点进行优化，本发明充分利用双目约束，将双目相机用统一的姿态节点进行优化，使得优化过程中相机变量数目少了一半，优化的求解复杂度降低，同时能大幅减少建图环节所需要的时间消耗，且可以成功建立场景的真实尺度地图。

(2)该发明独创性地提出一套完整的初始化流程，能够自动从最有可能建图成功的局部场景开始构建地图，使得整体地图准确可靠，鲁棒性强。

(3)该发明不光能有效地利用了双目基线约束，同时可以适配鱼眼或小孔相机模型，适合于大部分传感器构建稳定无漂移的场景稀疏地图，应用范围广。

(4)本发明可以在几乎没有SLAM或者先验姿态的情况下独立建图，在有SLAM的输出的情况下会自行判断姿态可靠性，从而解决了SLAM姿态误差对整体建图的负面影响。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1是本发明移动机器人基于双目图像的视觉地图生成方法的流程图。

图2是本发明前端模块构建的流程图。

图3是本发明后端模块构建的流程图。

图4是初始化环节的流程图。

图5是本发明在kitti数据集上的建图效果。

图6为本发明kitti各个数据集上的样例图片。

图7为本发明在kitti数据集计算的轨迹与真实轨迹对比。

图8为本发明在EuRoC上的建图效果。

图9为EuRoC数据集的样例图片。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

结合图1至图3所示，本发明提供了一种移动机器人基于双目图像的视觉地图生成方法，具体包括以下步骤：

S1、构建视觉地图生成方法的前端模块，具体表现为：

S11、使用双目相机，在需要建立地图的场景中，采集一系列图片作为场景样本，双目相机在使用前进行双目标定；

S12、选取一定间隔长度的双目图片序列，并提取其中的SIFT视觉特征点以及描述子；需要说明的是，间隔长度只需要保持相邻图片共视区域不小于70％即可；

S13、匹配同一时刻的双目图片，同时也进行相邻图片之间的视觉匹配，在所有匹配进行完后，根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配；需要说明的是，步骤S13具体表现为：

S131、匹配同一时刻的双目图片；

S132、判断是否检测到闭环图像，若是，则将检测到的闭环图像进行匹配后进入步骤S133；若否，则直接进入步骤S133；

S133、将步骤S132检测到的图像与相邻帧图像进行匹配；

S134、判断是否有没匹配到的图片，若是，则返回步骤S12；若否，则根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配后，进入步骤S14。

S14、对所有匹配的图片进行Essential Matrix(基础矩阵)的噪点剔除，过滤得到更好的匹配关联；

S15、遍历所有关联，保存每个地图点在每张图片中出现的位置，至此前端模块完成；

S2、构建视觉地图生成方法的后端模块，具体表现为：

S21、双目建图初始化：令集合{s_i}是所有等待建图的图片，寻找关联最紧密的两对双目图片作为起始两帧进行初始化，生成的初始化地图称之为局部地图；优选地其具体步骤包括：

步骤a：从图片数据库中选择一帧的双目图片作为起始图片，若其条件满足：具备最多双目匹配合格的地图点，即视差角大于0.05弧度，同时这些地图点出现在其它图片的观测数量大于5，则视为情况A；其中，视差角的计算公式如下：

α＝acos＜R_ib_i，R_jb_j>

其中，acos<·>表示反cos函数，R_i表示第i帧相机姿态中的旋转矩阵，R_j表示第j帧相机姿态中的旋转矩阵，b_i表示该点在第i帧相机中的反投影向量，b_j表示第j帧相机中的反投影向量，反投影向量可以通过反投影函数获得，具体地：

对于针孔相机，反投影函数为：

其中，P_c＝{f_x，f_y，c_x，c_y}，f_x，f_y，c_x，c_y是相机的内参，u，v是该点在图像平面的观测，norm(·)是向量欧式长度归一化算子；

对于鱼眼相机，则有反投影函数：

θ^*＝d^-1(r_θ)

其中，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数，

为实数集，θ^*是方程θ+k₁θ³+k₂θ⁵+k₃θ⁷+k₄θ⁹＝r_θ的解，θ表示畸变程度；

反投影向量则通过如下公式获得：

b＝π^-1(u，v)。

步骤b：如果没有找到满足情况A的一对双目图片作为起始帧，只从左目数据集中选取一张图片作为第一帧，该选取图片与其他图片的观测次数最多且观测次数大于15次，视为情况B；

步骤c：选取第二帧双目图片，第二帧的双目图片要求左目图片与第一帧的共同观测数量最多，如果是情况A，则对第二帧的双目相机进行3点pnp(pespective-n-point，全局姿态估计)姿态求解；如果是情况B，则处理情况分为以下两类：

步骤1)、若此时能够得到SLAM的姿态求解，则通过极限约束来判断姿态能否使用，若可以，则直接进行去除噪点操作后，进入步骤d，否则进入步骤2)；

步骤2)、如此时没有SLAM的求解姿态，则利用双目约束进行点云缩放求解：具体通过第一帧左目和第二帧左目之间的观测进行本质矩阵求解，然后分解得到无尺度姿态，并且对点云进行三角化；再通过第一帧左右目的匹配计算点云坐标，同时除以无尺度点云坐标的数值，得到点云的尺度，并将点云尺度乘上无尺度姿态，进入步骤d；

步骤d：采用下述公式将点云投影到图像平面上，根据投影与观测之间的差值判断是否为噪点，该处观测视为干扰观测：

式中，π(·)为相机投影函数，uv_pre表示预测值，

是相机的姿态变量，表示从世界坐标系到相机坐标系的关系，

表示3×3单位旋转矩阵，

表示3×1平移向量，X_W表示是世界坐标系下的点云坐标；

需要说明的是，本步骤中步骤d中π(·)为相机投影函数，具体来说：对于针孔相机前的坐标点

为实数集，x、y、z分别为横坐标、纵坐标和竖坐标，则其相机投影函数为：

其中，X_c表示点云的三维坐标，P_c＝{f_x，f_y，c_x，c_y}，

是相机的内参，f_x、f_y、c_x、c_y分别表示相机x方向的焦距、相机y方向的焦距、横轴的光心、纵轴的光心；

对于鱼眼相机，其投影函数有下列公式：

其中，X_c表示点的空间坐标，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数。

步骤e：将余下观测所涉及到的图像帧和点云加入到全局光束平滑调整，进行优化全局光束平滑调整，优化的目标函数为：

式中，C_1，2表示第一帧、第二帧的相机姿态，

i的取值为1或2，j的取值为所有点云的编号，从1到N,N表示点云的数目,i≠j,

表示旋转矩阵，

表示平移向量，

表示第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，k的取值为1或2，π₁，π₂是左、右目的投影函数，左右目的姿态关联如下：

是标定环节中给出的参数，表示双目相机之间物理上的旋转和平移，且

步骤f：如果每条约束的平均残差在2个像素值以内，则初始化成功，将目前两帧双目图片从集合{s_i}取出放入集合{I_i}当中，进入步骤S22；否则进入步骤a，重新初始化。

S22、计算下一对双目的姿态：选择和当前局部地图共视最大的图像，作为下一帧，并将该帧从集合中取出并放入集合{I_i}当中，如果此时该帧有SLAM的求解姿态，则与时间上最近的图片比对相对姿态的结果，如果结果相似，则进入步骤S23，如果结果不相似，则进入步骤S24；

S23、生成更多的点云，并根据点云生成情况决定是否全局平滑；

S24、进行全局光束平滑调整操作，判断是否有更多双目图片，若是，则返回步骤S22；反之，则进入步骤S25；

优选地，全局光束平滑调整优化的目标函数如下：

其中，C_i是集合{I_i}中第i帧的相机姿态，

是第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，左右目姿态依然满足所述步骤e的线性约束。

S25、所有图片都有重建结果，构成全局地图。

同时，作为进一步优选的实施方式，步骤S23具体表现为：寻找集合{I_i}当中还未被计算位置的点云，计算视差角条件，对大于阈值的点云进行线性三角化求解，如果新增的点云数目大于现有点云数目的25％，则进入步骤S24，否则返回步骤S22；如果集合{s_i}中没有更多图片，则进入步骤S24。

本发明中用于建图的场景分为两类，一类是室外大型场景，另一类是室内小型场景，室外以公开数据集Kitti为例。

对于室外场景，首先按照图2的方式，对所有图片进行匹配生成前端模型，包括以下几个步骤：

S11：使用双目相机，在需要建立地图的场景中，采集一系列图片作为场景样本，双目相机的标定参数从kitti数据集官网给出，由于相机没有畸变，可以采用标准的小孔模型。

S12：对双目图片序列间隔8张采样，对每个图片提取SIFT视觉特征点以及描述子。

S13：匹配同一时刻的双目图片，同时也进行相邻图片之间(前后10帧)进行视觉匹配，在所有匹配进行完后，根据描述子计算词汇树，寻找相似图片，同时再次对得到相似图片进行两两视觉匹配。

S14：对所有的匹配进行Essential Matrix的噪点剔除，过滤得到更好的匹配关联，噪点剔除的判断标准如下，计算下列value数值，对于value数值大于2.0的匹配对剔除：

其中，value表示噪点指数，越大越容易是噪点，x₁,x₂是匹配像素值的homogenous(归一化平面坐标)向量，T表示矩阵转置，E₁₂表示本质矩阵。

S15：遍历所有关联，保存每个地图点在每张图片中出现的位置，至此前端模块完成。

之后，进入后端模块，后端模块的大致流程和逻辑如图1和图3所示。建图之前需要初始化模块，初始化按照图4给的流程进行，具体可以参考步骤S21，此时生成的初始化地图可以称之为局部地图。

S22：选择和当前局部地图共视最大的图像，作为下一帧，将该帧从集合{s_i}中取出并放入集合{I_i}当中，如果此时该有SLAM的求解姿态，则与时间上最近的图片比对相对姿态的结果，如果结果相似，则进入步骤S23，如果结果不相似，则进入步骤S24。

S23：寻找集合{I_i}当中还未被计算位置的点云，计算视差角条件，对大于阈值的点云进行线性三角化求解，如果新增的点云数目大于现有点云数目的25％，则进入步骤S24，否则返回步骤S22；如果集合{s_i}中没有更多图片，则进入步骤S24。

S24、进行全局光束平滑调整操作，判断是否有更多双目图片，若是，则返回步骤S22；反之，则进入步骤S25；

S25、所有图片都有重建结果，构成全局地图。

为了验证本发明方案的有效性和准确性，我们在图5附上Kitti多个场景的重建效果，并且在图6附上数据集的样例图片。从图5中能够清晰地看到街道规整的结构，同时从图7中可以结合相机的姿态真实数据和本方案的估计数据进行对比，数据显示姿态估计结果相当准确，误差可以达到厘米级。

此外，室内场景，我们使用EUROC数据集为例，验证鱼眼相机模型下的双目建图效果。

依然按照图2的方式，对所有图片进行匹配生成前端数据。包括以下几个步骤：

S11：读取EUROC场景图片，EUROC相机使用60Hz的双目拍摄，相机参数可以用从官方数据集给出；

S12：对双目图片序列间隔10张采样，对每个图片提取SIFT视觉特征点以及描述子；

S13：匹配同一时刻的双目图片，同时也进行相邻图片之间(前后10帧)进行视觉匹配。在所有匹配进行完后，根据描述子计算词汇树，寻找相似图片，同时再次对得到相似图片进行两两视觉匹配；

S14：对所有的匹配进行Essential Matrix的噪点剔除，过滤得到更好的匹配关联，噪点剔除的判断标准如下，计算下列value数值，对于value数值大于2.0的匹配对剔除：

其中，value表示噪点指数，越大越容易是噪点，b₁,b₂是匹配像素值的反投影向量，T表示矩阵转置，E₁₂表示本质矩阵，对于反投影向量，则根据鱼眼模型的反投影函数生成：

θ^*＝d^-1(r_θ)

其中，(u,v)是图片上的特征点观测，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}是相机内参，

为镜头畸变参数，

为实数集，θ^*是方程θ+k₁θ³+k₂θ⁵+k₃θ⁷+k₄θ⁹＝r_θ的解，θ表示畸变程度；

反投影向量则通过如下公式获得：

b＝π^-1(u，v)。

S15、遍历所有关联，保存每个地图点在每张图片中出现的位置，至此前端模块完成。

后端模块与室外场景相同，依然按照图1和3的流程进行，初始化环节按照图4的流程来进行。具体后端模块构建流程可以参考发明内容中的步骤S21～步骤25。

为了验证本发明方案的有效性和准确性，在图8附上EuRoC多个场景的重建效果，并且在图9附上数据集的样例图片。从图8中也能够清晰地看到完整的集装箱结构。

故此，本发明公开了一种移动机器人基于双目图像的视觉地图生成方法，其是一种建立在未知环境下的稀疏地图构建方法，用于恢复场景结构。具体根据双目序列图片的视觉特征，将相关图像进行视觉匹配并且检测回环，同时，为了更好地恢复场景尺度，本发明充分利用双目的基线约束，改进点云的参数化表达方式，设计了一种针对双目相机设计的建图初始化算法，增量算法以及对应的全局平滑调整过程，整个算法的大体步骤为：(1)使用图像匹配方法，建立双目视觉关联；(2)建立双目初始局部地图以及双目增量建图；(3)全局平滑姿态和点云。本发明的方法相比现有的双目建图方法，速度更快，复杂度更低，相比基于SLAM的建图算法，地图漂移更小，而且更具备场景鲁棒性，增强了复杂环境的适应能力，满足现有机器人场景构图需求，同时具备高度可扩展性，能够结合SLAM的输出结果加速整个建图环节步骤，同时可以进一步增强鲁棒性。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明的范围，否则都应该包括在本发明的保护范围内。

Claims (7)

1.移动机器人基于双目图像的视觉地图生成方法，其特征在于，包括以下步骤：

S1、构建视觉地图生成方法的前端模块，具体表现为：

S11、使用双目相机，在需要建立地图的场景中，采集一系列图片作为场景样本，双目相机在使用前进行双目标定；

S12、选取一定间隔长度的双目图片序列，并提取其中的SIFT视觉特征点以及描述子；

S13、匹配同一时刻的双目图片，同时也进行相邻图片之间的视觉匹配，在所有匹配进行完后，根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配；

S14、对所有匹配的图片进行Essential Matrix的噪点剔除，过滤得到更好的匹配关联；

S15、遍历所有关联，保存每个地图点在每张图片中出现的位置，至此前端模块完成；

S2、构建视觉地图生成方法的后端模块，具体表现为：

S21、双目建图初始化：令集合{s_i}是所有等待建图的图片，寻找关联最紧密的两对双目图片作为起始两帧进行初始化，生成的初始化地图称之为局部地图；

S22、计算下一对双目的姿态：选择和当前局部地图共视最大的图像，作为下一帧，并将该帧从集合中取出并放入集合{I_i}当中，如果此时该帧有SLAM的求解姿态，则与时间上最近的图片比对相对姿态的结果，如果结果相似，则进入步骤S23，如果结果不相似，则进入步骤S24；

S23、生成更多的点云，并根据点云生成情况决定是否全局平滑；

S24、进行全局光束平滑调整操作，判断是否有更多双目图片，若是，则返回步骤S22；反之，则进入步骤S25；

S25、所有图片都有重建结果，构成全局地图。

2.根据权利要求1所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤S13具体表现为：

S131、匹配同一时刻的双目图片；

S132、判断是否检测到闭环图像，若是，则将检测到的闭环图像进行匹配后进入步骤S133；若否，则直接进入步骤S133；

S133、将步骤S132检测到的图像与相邻帧图像进行匹配；

S134、判断是否有没匹配到的图片，若是，则返回步骤S12；若否，则根据描述子计算相似图片，同时再次对得到的相似图片进行两两视觉匹配后，进入步骤S14。

3.根据权利要求1所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤S23具体表现为：寻找集合{I_i}当中还未被计算位置的点云，计算视差角条件，对大于阈值的点云进行线性三角化求解，如果新增的点云数目大于现有点云数目的25％，则进入步骤S24，否则返回步骤S22；如果集合{s_i}中没有更多图片，则进入步骤S24。

4.根据权利要求1所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤S21具体包括如下步骤：

步骤a：从图片数据库中选择一帧的双目图片作为起始图片，若其条件满足：具备最多双目匹配合格的地图点，即视差角大于0.05弧度，同时这些地图点出现在其它图片的观测数量大于5，则视为情况A；

步骤b：如果没有找到满足情况A的一对双目图片作为起始帧，只从左目数据集中选取一张图片作为第一帧，该选取图片与其他图片的观测次数最多且观测次数大于15次，视为情况B；

步骤c：选取第二帧双目图片，第二帧的双目图片要求左目图片与第一帧的共同观测数量最多，如果是情况A，则对第二帧的双目相机进行3点pnp姿态求解；如果是情况B，则处理情况分为以下两类：

步骤1)、若此时能够得到SLAM的姿态求解，则通过极限约束来判断姿态能否使用，若可以，则直接进行去除噪点操作后，进入步骤d，否则进入步骤2)；

步骤2)、如此时没有SLAM的求解姿态，则利用双目约束进行点云缩放求解：具体通过第一帧左目和第二帧左目之间的观测进行本质矩阵求解，然后分解得到无尺度姿态，并且对点云进行三角化；再通过第一帧左右目的匹配计算点云坐标，同时除以无尺度点云坐标的数值，得到点云的尺度，并将点云尺度乘上无尺度姿态，进入步骤d；

步骤d：采用下述公式将点云投影到图像平面上，根据投影与观测之间的差值判断是否为噪点，该处观测视为干扰观测：

式中，π(·)为相机投影函数，uv_pre表示预测值，

是相机的姿态变量，表示从世界坐标系到相机坐标系的关系，

表示3×3单位旋转矩阵，

表示3×1平移向量，X_W表示是世界坐标系下的点云坐标；

步骤e：将余下观测所涉及到的图像帧和点云加入到全局光束平滑调整，进行优化全局光束平滑调整，优化的目标函数为：

式中，C_1，2表示第一帧、第二帧的相机姿态，

i的取值为1或2，j的取值为所有点云的编号，从1到N,N表示点云的数目,i≠j,

表示旋转矩阵，

表示平移向量，

表示第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，k的取值为1或2，π₁，π₂是左、右目的投影函数，左右目的姿态关联如下：

是标定环节中给出的参数，表示双目相机之间物理上的旋转和平移，且

步骤f：如果每条约束的平均残差在2个像素值以内，则初始化成功，将目前两帧双目图片从集合{s_i}取出放入集合{I_i}当中，进入步骤S22；否则进入步骤a，重新初始化。

5.根据权利要求4所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤a中视差角的计算公式如下：

α＝acos<R_ib_i，R_jb_j>

其中，acos<·>表示反cos函数，R_i表示第i帧相机姿态中的旋转矩阵，R_j表示第j帧相机姿态中的旋转矩阵，b_i表示该点在第i帧相机中的反投影向量，b_j表示第j帧相机中的反投影向量，反投影向量可以通过反投影函数获得，具体地：

对于针孔相机，反投影函数为：

其中，P_c＝{f_x，f_y，c_x，c_y}，f_x,f_y,c_x,c_y是相机的内参，u,v是该点在图像平面的观测，norm(·)是向量欧式长度归一化算子；

对于鱼眼相机，则有反投影函数：

θ^*＝d^-1(r_θ)

其中，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数，

为实数集，θ^*是方程θ+k₁θ³+k₂θ⁵+k₃θ⁷+k₄θ⁹＝r_θ的解，θ表示畸变程度；

反投影向量则通过如下公式获得：

b＝π^-1(u，v)。

6.根据权利要求5所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤d中π(·)为相机投影函数，具体来说：对于针孔相机前的坐标点

为实数集，x、y、z分别为横坐标、纵坐标和竖坐标，则其相机投影函数为：

其中，X_c表示点云的三维坐标，P_c＝{f_x，f_y，c_x，c_y}，

是相机的内参，f_x、f_y、c_x、c_y分别表示相机x方向的焦距、相机y方向的焦距、横轴的光心、纵轴的光心；

对于鱼眼相机，其投影函数有下列公式：

其中，X_c表示点的空间坐标，P_c＝{f_x，f_y，c_x，c_y，k₁，k₂，k₃，k₄}，

是相机内参，

为镜头畸变参数。

7.根据权利要求4所述的移动机器人基于双目图像的视觉地图生成方法，其特征在于，所述步骤S24中全局光束平滑调整优化的目标函数如下：

其中C_i是集合{I_i}中第i帧的相机姿态，

是第j个点云，

表示第j个点云在第i帧的第k个相机上的观测，左右目姿态依然满足所述步骤e的线性约束。

Images