CN109947097B - 一种基于视觉和激光融合的机器人定位方法及导航应用 - Google Patents

Abstract

本发明涉及一种基于视觉和激光融合的机器人定位方法及导航应用，利用融合激光扫描装置高精度的深度信息与图像捕获装置丰富的场景信息，将其结合起来，共同用于障碍物的检测，这种障碍物检测方法不仅具备二维激光雷达平面探测范围广、精度高、实时性好的特点,而且兼有深度图像捕获装置三维探测的优势，两者融合能够更精准实现机器人的定位，提高机器人的移动效率。

Description

一种基于视觉和激光融合的机器人定位方法及导航应用

技术领域

本发明涉及一种基于视觉和激光融合的机器人定位方法及导航应用，属于多传感器数据融合技术领域。

背景技术

近年来，人工智能技术不断发展，也带动了机器人产业的发展，使得机器人从原来的的主要面向工业应用逐渐向物流、服务等贴近人们生活的方向。机器人领域技术的研究是当今时代科研和应用中的热点。机器人正在向医疗、工业制造、航空航天、军事和民用服务多方面渗透。随着科技的迅速发展，机器人的应用前景非常广阔，特别是在人口老龄化加剧、生活节奏加快、人们对生活质量要求更高的未来，服务机器人必将更多地走进人类的生活，成为人类生产、生活中的重要帮手。机器人的行走是依靠其自身对其前方路况进行分析，并获得行走路线的，但是现有机器人的障碍物分析方法比较单一，智能化水平有限，进而使得分析准确性低，这也是影响机器人发展的重要因素之一。

发明内容

本发明所要解决的技术问题是提供一种基于视觉和激光融合的机器人定位方法，能够克服传统单一定位方式所造成的精度低问题，有效提高了机器人的行走效率。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种基于视觉和激光融合的机器人定位方法，应用设置于机器人上的图像捕获装置和激光扫描装置，实现机器人自身的定位，其中，图像捕获装置的主光轴指向与激光扫描装置的主光轴指向同向，所述机器人定位方法包括如下：

首先执行如下步骤A至步骤B，完成初始化；

步骤A.获取图像捕获装置所采集的图像、以及激光扫描装置所采集的激光扫描数据，分别作为相机样本图像与激光样本数据，然后进入步骤B；

步骤B.根据相机样本图像与激光样本数据，建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的转换模型，作为坐标转换模型，用于图像捕获装置所采集图像与激光扫描装置所采集激光扫描数据之间的转换；

然后实时执行如下步骤C操作，实现机器人自身的定位；

步骤C.根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，分机器人运动状态实现定位。

作为本发明的一种优选技术方案，所述步骤B包括如下步骤：

步骤B1.基于图像捕获装置主光轴指向与激光扫描装置主光轴指向同向，针对图像捕获装置所采集图像中与激光扫描装置所采集激光扫描数据中的同一个目标P，初始化目标P在图像捕获装置所对应坐标系O_c-X_cY_cZ_c下的坐标(X_c,Y_c,Z_c)，以及初始化目标P在激光扫描装置所对应坐标系O_l-X_lY_lZ_l下的坐标(X_l,Y_l,Z_l)，然后进入步骤B2；

步骤B2.建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的坐标关系如下：

其中，R代表旋转矩阵，T代表平移矩阵；坐标(X_c,Y_c,Z_c)对应于图像捕获装置的采集数据为目标P在所采集图像的平面坐标系中的投影P'的坐标(u,v)，以及该投影P'所对应的深度值Z_c；坐标(X_l,Y_l,Z_l)对应于激光扫描装置的采集数据为目标P距激光扫描装置工作端的距离r、以及目标P所对应的激光束偏角值α；然后进入步骤B3；

步骤B3.构建坐标(X_c,Y_c,Z_c)与像素点三元组之间的关系如下：

其中，f_x、f_y分别是图像捕获装置所对应坐标系中X轴和Y轴方向上、以像素为单位的等效焦距，c_x、c_y是图像捕获装置所采集图像的基准点，然后进入步骤B4；

步骤B4.构建坐标(X_l,Y_l,Z_l)与像素点三元组之间的关系如下：

其中，激光扫描装置所对应坐标系与平面直角坐标系在同一个平面内，原点为激光扫描装置中心；激光扫描装置的激光束与Z_l轴正向夹角为极坐标角度α，即目标P所对应的激光束偏角值；激光扫描装置激光束测距值为极坐标径向值r，即目标P距激光扫描装置工作端的距离；然后进入步骤B5；

步骤B5.获得激光测距点二元组(r,α)与坐标(X_l,Y_l,Z_l)的关系如下：

Z_l＝rcos(α)，X_l＝rsin(α) (4)

根据投影P'所对应的图像坐标(u,v)，保持激光扫描装置坐标原点与图像捕获装置坐标原点在Y轴方向的垂直高度差为l，则有像素点三元组(u,v,Z_c)与激光雷达测距点二元组(r,α)的关系如下：

将多对像素点三元组和测距点二元组代入式(5)，通过求解线性方程组获得旋转矩阵R和平移矩阵T，进而确定图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的旋转平移关系，获得坐标转换模型。

作为本发明的一种优选技术方案，所述步骤C中，根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，按如下各种机器人的运动状态，分别实现定位；

运动状态(1).当机器人处于静止状态时，舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位；

运动状态(2).当机器人处于运动状态时，首先由图像捕获装置采集环境的RGB图像，对RGB图像进行灰度变换，获取该灰度图像的SURF特征点、以及所对应的特征向量R_i，然后计算特征向量R_i上每个SURF特征点分别与特征向量R_i之间的欧式距离，将欧式距离作为SURF特征点的相似性度量标准，使用KNN搜索算法，在数据库中查找最邻近特征点，根据最邻近特征点所属的特征模板，统计出特征向量R_i与每个特征模板匹配到最邻近特征点的个数M；

判断M是否小于预设特征点个数阈值，是则舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位；否则按如下步骤C(2)-1至步骤步骤C(2)-3，实现机器人定位：

步骤C(2)-1.根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，获得激光扫描装置所对应坐标系下的定位结果X₁，同时，根据图像捕获装置所采集的图像、以及对应视觉里程，获得图像捕获装置所对应坐标系下的定位结果，然后进入步骤C(2)-2；

步骤C(2)-2.根据所述坐标转换模型，将图像捕获装置所对应坐标系下的定位结果，转换至激光扫描装置所对应坐标系下，获得该定位结果X₂，然后进入步骤C(2)-3；

步骤C(2)-3.按如下公式：

X＝αX₁+βX₂

针对定位结果X₁和定位结果X₂进行加权，获得定位结果X，即实现机器人定位；其中，α表示激光扫描装置定位结果所对应预设权重，β表示图像捕获装置定位结果所对应预设权重。

作为本发明的一种优选技术方案：所述图像捕获装置为深度图像捕获装置。

作为本发明的一种优选技术方案：所述激光扫描装置为二维激光扫描装置。

与上述相对应，本发明还要解决的技术问题是提供一种基于视觉和激光融合的机器人定位方法的导航应用，能够高效实现障碍物侦测，保证导航路径规划，提高机器人行走效率。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一种基于视觉和激光融合的机器人定位方法的导航应用，包括如下步骤D至步骤E：

步骤D.通过所述图像捕获装置实时所采集图像、以及对应视觉里程，激光扫描装置实时所采集的激光扫描数据、以及对应激光里程，获得机器人正前方预设距离内是否存在障碍物的信息，作为机器人正前方路况，并进入步骤E；

步骤E.基于机器人的实时定位，实时根据目标点，以及机器人正前方路况，实时获得机器人至目标点的导航路径规划结果，并按该导航路径规划结果，控制机器人行进。

本发明所述一种基于视觉和激光融合的机器人定位方法及导航应用，采用以上技术方案与现有技术相比，具有以下技术效果：

本发明所设计基于视觉和激光融合的机器人定位方法及导航应用，利用融合激光扫描装置高精度的深度信息与图像捕获装置丰富的场景信息，将其结合起来，共同用于障碍物的检测，这种障碍物检测方法不仅具备二维激光雷达平面探测范围广、精度高、实时性好的特点,而且兼有深度图像捕获装置三维探测的优势，两者融合能够更精准实现机器人的定位，提高机器人的移动效率。

附图说明

图1是本发明设计基于视觉和激光融合的机器人定位方法的流程示意图。

具体实施方式

下面结合说明书附图对本发明的具体实施方式作进一步详细的说明。

本发明设计了一种基于视觉和激光融合的机器人定位方法，应用设置于机器人上的图像捕获装置和激光扫描装置，实现机器人自身的定位，其中，图像捕获装置的主光轴指向与激光扫描装置的主光轴指向同向，实际应用中，图像捕获装置为深度图像捕获装置，并具体可以选择Kinect相机，激光扫描装置为二维激光扫描装置。

Kinect相机能以简捷方式和较低成本提供环境深度数据。利用这一优点，本文提出在移动机器人上同时安装二维激光雷达和Kinect相机，把激光扫描和深度图像结合起来检测障碍物。这种障碍物检测方法不仅具备二维激光雷达平面探测范围广、精度高、实时性好的特点，而且兼有Kinect深度相机三维探测的优势。

Kinect相机相比于激光雷达可以获得丰富的色彩、纹理、形状等场景信息，便于进行图像处理操作。激光雷达与相机相比具有深度信息精度高，视场范围大，数据处理简单等优势，然而其只能获得离散的点云数据。激光雷达高精度的深度信息与Kinect相机丰富的场景信息具有很强的互补性，结合起来检测障碍物。这种障碍物检测方法不仅具备二维激光雷达平面探测范围广、精度高、实时性好的特点,而且兼有Kinect深度相机三维探测的优势。两者融合能够更精准地进行移动机器人的位姿估计，提高机器人的行走效率。

本发明所设计一种基于视觉和激光融合的机器人定位方法，在实际应用当中，如图1所示，机器人定位方法包括如下。

首先执行如下步骤A至步骤B，完成初始化。

步骤A.获取图像捕获装置所采集的图像、以及激光扫描装置所采集的激光扫描数据，分别作为相机样本图像与激光样本数据，然后进入步骤B。

步骤B.根据相机样本图像与激光样本数据，建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的转换模型，作为坐标转换模型，用于图像捕获装置所采集图像与激光扫描装置所采集激光扫描数据之间的转换。

上述步骤B在实际应用中，具体包括如下步骤B1至步骤B5，实现坐标转换模型的获得，用于图像捕获装置所采集图像与激光扫描装置所采集激光扫描数据之间的转换。

步骤B1.基于图像捕获装置主光轴指向与激光扫描装置主光轴指向同向，针对图像捕获装置所采集图像中与激光扫描装置所采集激光扫描数据中的同一个目标P，初始化目标P在图像捕获装置所对应坐标系O_c-X_cY_cZ_c下的坐标(X_c,Y_c,Z_c)，以及初始化目标P在激光扫描装置所对应坐标系O_l-X_lY_lZ_l下的坐标(X_l,Y_l,Z_l)，然后进入步骤B2。

步骤B2.建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的坐标关系如下：

其中，R代表旋转矩阵，T代表平移矩阵；坐标(X_c,Y_c,Z_c)对应于图像捕获装置的采集数据为目标P在所采集图像的平面坐标系中的投影P'的坐标(u,v)，以及该投影P'所对应的深度值Z_c；坐标(X_l,Y_l,Z_l)对应于激光扫描装置的采集数据为目标P距激光扫描装置工作端的距离r、以及目标P所对应的激光束偏角值α；然后进入步骤B3。

步骤B3.构建坐标(X_c,Y_c,Z_c)与像素点三元组之间的关系如下：

其中，f_x、f_y分别是图像捕获装置所对应坐标系中X轴和Y轴方向上、以像素为单位的等效焦距，c_x、c_y是图像捕获装置所采集图像的基准点，然后进入步骤B4。

步骤B4.构建坐标(X_l,Y_l,Z_l)与像素点三元组之间的关系如下：

其中，激光扫描装置所对应坐标系与平面直角坐标系在同一个平面内，原点为激光扫描装置中心；激光扫描装置的激光束与Z_l轴正向夹角为极坐标角度α，即目标P所对应的激光束偏角值；激光扫描装置激光束测距值为极坐标径向值r，即目标P距激光扫描装置工作端的距离；然后进入步骤B5。

步骤B5.获得激光测距点二元组(r,α)与坐标(X_l,Y_l,Z_l)的关系如下：

Z_l＝rcos(α)，X_l＝rsin(α) (4)

根据投影P'所对应的图像坐标(u,v)，保持激光扫描装置坐标原点与图像捕获装置坐标原点在Y轴方向的垂直高度差为l，则有像素点三元组(u,v,Z_c)与激光雷达测距点二元组(r,α)的关系如下：

将多对像素点三元组和测距点二元组代入式(5)，通过求解线性方程组获得旋转矩阵R和平移矩阵T，进而确定图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的旋转平移关系，获得坐标转换模型。

然后实时执行如下步骤C操作，实现机器人自身的定位。

步骤C.根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，分机器人运动状态实现定位。

实际应用中，上述步骤C根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，按如下各种机器人的运动状态，分别实现定位。

运动状态(1).当机器人处于静止状态时，舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位。

运动状态(2).当机器人处于运动状态时，首先由图像捕获装置采集环境的RGB图像，对RGB图像进行灰度变换，获取该灰度图像的SURF特征点、以及所对应的特征向量R_i，然后计算特征向量R_i上每个SURF特征点分别与特征向量R_i之间的欧式距离，将欧式距离作为SURF特征点的相似性度量标准，使用KNN搜索算法，在数据库中查找最邻近特征点，根据最邻近特征点所属的特征模板，统计出特征向量R_i与每个特征模板匹配到最邻近特征点的个数M。

判断M是否小于预设特征点个数阈值，是则舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位；否则按如下步骤C(2)-1至步骤步骤C(2)-3，实现机器人定位。

步骤C(2)-1.根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，获得激光扫描装置所对应坐标系下的定位结果X₁，同时，根据图像捕获装置所采集的图像、以及对应视觉里程，获得图像捕获装置所对应坐标系下的定位结果，然后进入步骤C(2)-2。

步骤C(2)-2.根据所述坐标转换模型，将图像捕获装置所对应坐标系下的定位结果，转换至激光扫描装置所对应坐标系下，获得该定位结果X₂，然后进入步骤C(2)-3。

步骤C(2)-3.按如下公式：

X＝αX₁+βX₂

针对定位结果X₁和定位结果X₂进行加权，获得定位结果X，即实现机器人定位；其中，α表示激光扫描装置定位结果所对应预设权重，β表示图像捕获装置定位结果所对应预设权重。

基于上述所设计的基于视觉和激光融合的机器人定位方法，本发明可以进一步对其实现导航应用，即在实际应用当中，具体执行如下步骤D至步骤E。

步骤D.通过所述图像捕获装置实时所采集图像、以及对应视觉里程，激光扫描装置实时所采集的激光扫描数据、以及对应激光里程，获得机器人正前方预设距离内是否存在障碍物的信息，作为机器人正前方路况，并进入步骤E。

步骤E.基于机器人的实时定位，实时根据目标点，以及机器人正前方路况，实时获得机器人至目标点的导航路径规划结果，并按该导航路径规划结果，控制机器人行进。

上述技术方案所设计基于视觉和激光融合的机器人定位方法及导航应用，利用融合激光扫描装置高精度的深度信息与图像捕获装置丰富的场景信息，将其结合起来，共同用于障碍物的检测，这种障碍物检测方法不仅具备二维激光雷达平面探测范围广、精度高、实时性好的特点,而且兼有深度图像捕获装置三维探测的优势，两者融合能够更精准实现机器人的定位，提高机器人的移动效率。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (5)

1.一种基于视觉和激光融合的机器人定位方法，其特征在于：应用设置于机器人上的图像捕获装置和激光扫描装置，实现机器人自身的定位，其中，图像捕获装置的主光轴指向与激光扫描装置的主光轴指向同向，所述机器人定位方法包括如下：

首先执行如下步骤A至步骤B，完成初始化；

步骤A.获取图像捕获装置所采集的图像、以及激光扫描装置所采集的激光扫描数据，分别作为相机样本图像与激光样本数据，然后进入步骤B；

步骤B.根据相机样本图像与激光样本数据，建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的转换模型，作为坐标转换模型，用于图像捕获装置所采集图像与激光扫描装置所采集激光扫描数据之间的转换；

然后实时执行如下步骤C操作，实现机器人自身的定位；

步骤C.根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，分机器人运动状态实现定位；

上述步骤C中，根据图像捕获装置所采集图像、以及对应视觉里程，激光扫描装置所采集的激光扫描数据、以及对应激光里程，结合坐标转换模型，按如下各种机器人的运动状态，分别实现定位；

运动状态(1).当机器人处于静止状态时，舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位；

运动状态(2).当机器人处于运动状态时，首先由图像捕获装置采集环境的RGB图像，对RGB图像进行灰度变换，获得灰度图像，并获取该灰度图像的SURF特征点、以及所对应的特征向量R_i，然后计算特征向量R_i上每个SURF特征点分别与特征向量R_i之间的欧式距离，将欧式距离作为SURF特征点的相似性度量标准，使用KNN搜索算法，在数据库中查找最邻近特征点，根据最邻近特征点所属的特征模板，统计出特征向量R_i与每个特征模板匹配到最邻近特征点的个数M；

判断M是否小于预设特征点个数阈值，是则舍弃图像捕获装置所采集图像，根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，实现机器人定位；否则按如下步骤C(2)-1至步骤C(2)-3，实现机器人定位：

步骤C(2)-1.根据激光扫描装置所采集的激光扫描数据、以及对应激光里程，获得激光扫描装置所对应坐标系下的定位结果X₁，同时，根据图像捕获装置所采集的图像、以及对应视觉里程，获得图像捕获装置所对应坐标系下的定位结果，然后进入步骤C(2)-2；

步骤C(2)-2.根据所述坐标转换模型，将图像捕获装置所对应坐标系下的定位结果，转换至激光扫描装置所对应坐标系下的定位结果X₂，然后进入步骤C(2)-3；

步骤C(2)-3.按如下公式：

X＝αX₁+βX₂

针对定位结果X₁和定位结果X₂进行加权，获得定位结果X，即实现机器人定位；其中，α表示激光扫描装置定位结果所对应预设权重，β表示图像捕获装置定位结果所对应预设权重。

2.根据权利要求1所述一种基于视觉和激光融合的机器人定位方法，其特征在于，所述步骤B包括如下步骤：

步骤B1.基于图像捕获装置主光轴指向与激光扫描装置主光轴指向同向，针对图像捕获装置所采集图像中与激光扫描装置所采集激光扫描数据中的同一个目标P，初始化目标P在图像捕获装置所对应坐标系O_c-X_cY_cZ_c下的坐标(X_c,Y_c,Z_c)，以及初始化目标P在激光扫描装置所对应坐标系O_l-X_lY_lZ_l下的坐标(X_l,Y_l,Z_l)，然后进入步骤B2；

步骤B2.建立图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的坐标关系如下：

其中，R代表旋转矩阵，T代表平移矩阵；坐标(X_c,Y_c,Z_c)对应于图像捕获装置的采集数据为目标P在所采集图像的平面坐标系中的投影P'的坐标(u,v)，以及该投影P'所对应的深度值Z_c；坐标(X_l,Y_l,Z_l)对应于激光扫描装置的采集数据为目标P距激光扫描装置工作端的距离r、以及目标P所对应的激光束偏角值α；然后进入步骤B3；

步骤B3.构建坐标(X_c,Y_c,Z_c)与像素点三元组之间的关系如下：

其中，f_x、f_y分别是图像捕获装置所对应坐标系中X轴和Y轴方向上、以像素为单位的等效焦距，c_x、c_y是图像捕获装置所采集图像的基准点，然后进入步骤B4；

步骤B4.构建坐标(X_l,Y_l,Z_l)与像素点三元组之间的关系如下：

其中，激光扫描装置所对应坐标系与平面直角坐标系在同一个平面内，原点为激光扫描装置中心；激光扫描装置的激光束与Z_l轴正向夹角为极坐标角度α，即目标P所对应的激光束偏角值；激光扫描装置激光束测距值为极坐标径向值r，即目标P距激光扫描装置工作端的距离；然后进入步骤B5；

步骤B5.获得激光测距点二元组(r,α)与坐标(X_l,Y_l,Z_l)的关系如下：

Z_l＝rcos(α)，X_l＝rsin(α) (4)

根据投影P'所对应的图像坐标(u,v)，保持激光扫描装置坐标原点与图像捕获装置坐标原点在Y轴方向的垂直高度差为l，则有像素点三元组(u,v,Z_c)与激光雷达测距点二元组(r,α)的关系如下：

将多对像素点三元组和测距点二元组代入式(5)，通过求解线性方程组获得旋转矩阵R和平移矩阵T，进而确定图像捕获装置所对应坐标系与激光扫描装置所对应坐标系之间的旋转平移关系，获得坐标转换模型。

3.根据权利要求1所述一种基于视觉和激光融合的机器人定位方法，其特征在于：所述图像捕获装置为深度图像捕获装置。

4.根据权利要求1所述一种基于视觉和激光融合的机器人定位方法，其特征在于：所述激光扫描装置为二维激光扫描装置。

5.一种基于权利要求1所述一种基于视觉和激光融合的机器人定位方法的导航应用，其特征在于，包括如下步骤D至步骤E：

步骤D.通过所述图像捕获装置实时所采集图像、以及对应视觉里程，激光扫描装置实时所采集的激光扫描数据、以及对应激光里程，获得机器人正前方预设距离内是否存在障碍物的信息，作为机器人正前方路况，并进入步骤E；

步骤E.基于机器人的实时定位，实时根据目标点，以及机器人正前方路况，实时获得机器人至目标点的导航路径规划结果，并按该导航路径规划结果，控制机器人行进。

Images