基于深度摄像头的机器人快速定位与姿态估计方法

摘要

本发明公开了一种基于深度摄像头的机器人快速定位与姿态估计方法，所述深度摄像头固定安装在机器人主体上，并且保持相对于机器人主体的位置不变，所述方法通过深度摄像头获取机器人在运动过程环境中的深度图像，再对深度图像进行缩放、角点检测、角点特征提取，然后对相似特征进行匹配，最后通过最小二乘法获取机器人在智能移动过程中的旋转矩阵R与平移矩阵T。本发明的基于深度摄像头的机器人快速定位与姿态估计方法能够提高机器人定位及姿态估计中的准确性，能够有效降低图像处理时间并且提升处理图像速度。

说明书

技术领域

本发明涉及机器人，尤其涉及机器人的快速定位与姿态估计。

背景技术

机器人定位与姿态估计是机器人应用的基本问题，定位能够确定机器人在工作环境中的位置信息，姿态能够获取机器人移动方向，准确快速获取机器人的位置及姿态，能够提高机器人作业的效率及安全性。

基于彩色摄像机的机器人定位及姿态估计常常受工作环境的光照影响，使得机器人定位及姿态估计误差较大。目前有些专利通过图像预处理的技术能够克服光照亮暗的影响，但对于一部分有阴影的情况，不能起到优化效果。

此外，目前虽然已经有一些技术可以针对摄像机拍摄的图像进行一系列处理，从而获得机器人的实时运动姿态与平移距离，但是存在运行过程复杂，计算速度缓慢，还会出现一定的误差等问题。

为此本领域迫切需要一种能够基于深度摄像头的机器人快速定位与姿态估计的方法。

发明内容

本发明旨在克服以上技术问题，提出一种准确率高、能够有效降低图像处理时间且处理速度较快的基于深度摄像头的机器人快速定位与姿态估计方法。

为解决上述技术问题，本发明提供一种基于深度摄像头的机器人快速定位与姿态估计方法，所述深度摄像头固定地安装在机器人主体上，并且保持相对于机器人主体的位置不变，所述方法包括以下步骤：

(1)、深度摄像头获取机器人在运动过程环境中的深度图像P1、P2；

其中，深度图像P1为时刻t采集的数据信息，深度图像P2为时刻t之后采集的数据信息；

(2)、利用高斯金字塔方法对(1)中获取的深度图像P1、P2进行缩放，其中，

将深度图像P1缩小为p1，缩放系数为k1；

将深度图像P2缩小为p2，缩放系数为k2；

(3)、利用Fast角点检测算法对(2)中缩放后的图像p1、p2进行角点检测；

(4)、利用Sift算法对检测到的角点进行特征提取；

(5)、利用Knn算法对相似特征进行匹配；

(6)、利用(5)中的匹配结果，通过最小二乘法获取机器人在智能移动过程中的旋转矩阵R与平移矩阵T，R与T的矩阵形式如下：

其中，r₁₁，r₁₂，…，r₃₃这9个参数表示深度摄像头随着机器人旋转导致图像的旋转及缩放；

其中，T_x，T_y，T_z分别表示在x、y、z方向的平移，机器人的位置为机器人从原点出发时每次位置变化T的累计。

优选地，对于某一个点X(x，y，z)，该点变换后的位置X’利用R、T矩阵来计算，计算方法如下：

X＝[x，y，z，1]^T，

X'＝RX+T。

优选地，所述深度摄像头为Kinect深度相机。

优选地，所述深度摄像头计算旋转或平移的频率根据不同环境设置为不同参数。

本发明的基于深度摄像头的机器人快速定位与姿态估计方法相比于现有技术具有如下优点:该方法能够提高机器人定位及姿态估计中的准确性，同时能够有效降低图像处理时间，提升处理图像的速度。

附图说明

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

图1为本发明的基于深度摄像头的机器人快速定位与姿态估计方法的流程结构示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构及技术的描述，以避免不必要地混淆本发明的概念。

本发明提供了一种基于深度摄像头的机器人快速定位与姿态估计方法，在使用该方法之前需要将深度摄像头固定安装在机器人主体上，保持深度摄像头相对于机器人主体的位置不变。需要说明的是，本申请中深度摄像头的旋转、平移就是机器人主体的旋转、平移。

图1为本发明的基于深度摄像头的机器人快速定位与姿态估计方法的一种结构示意图，如图1所示，基于深度摄像头的机器人快速定位与姿态估计方法采用以下步骤：首先是获取一幅深度图像P1，再获取另一幅深度图像P2，然后将图像P1缩小为p1，将图像P2缩小为p2，再对p1和p2分别进行角点测测，对p1和p2检测到的角点分别提取特征，之后进行特征匹配，最后计算深度摄像头的旋转和平移，以此来获取机器人的旋转和平移矩阵。

需要说明的是，图1中深度图像P1、P2的获取可以通过深度相机厂家提供的SDK获取，优选地，本发明中的深度摄像头采用Kinect深度相机。

具体地，上述基于深度摄像头的机器人快速定位与姿态估计方法包括以下步骤：

(1)、深度摄像头获取机器人在运动过程环境中的深度图像P1、P2，

其中，深度图像P1为时刻t采集的数据信息，深度图像P2为时刻t之后采集的数据信息；

(2)、利用高斯金字塔方法对(1)中获取的深度图像P1、P2进行缩放，其中，将深度图像P1缩小为p1，缩放系数为k1；将深度图像P2缩小为p2，缩放系数为k2；

(3)、利用Fast角点检测算法对(2)中缩放后的图像p1、p2进行角点检测；

(4)、利用Sift算法对检测到的角点进行特征提取；

(5)、利用Knn算法对相似特征进行匹配；

(6)、利用(5)中的匹配结果，通过最小二乘法获取机器人在智能移动过程中的旋转矩阵R与平移矩阵T，R与T的矩阵形式如下：

其中，r11，r12，…，r33这9个参数表示深度摄像头随着机器人旋转导致图像的旋转及缩放；

其中，Tx，Ty，Tz分别表示在x、y、z方向的平移，机器人的位置为机器人从原点出发时每次位置变化T的累计。

本发明的上述步骤(1)中，深度图像P1、P2的获取顺序不可以变换，因为在计算深度摄像头的旋转与平移时，用到了拍摄图像的先后顺序，并且深度图像P1为时刻t采集的数据信息，深度图像P2为时刻t之后采集的数据信息。

本发明的上述步骤(2)中，将深度图像P1、P2进行缩放的系数k1与k2可相同或不同，其具体值需要根据图像的大小决定。

另外，本发明的基于深度摄像头的机器人快速定位与姿态估计方法的上述步骤中，可以先获取深度图像P1，然后对深度图像P1进行缩小、角点检测、角点特征提取，这几步操作步骤在执行时顺序不可以变换。此外，再获取深度图像P2，然后对深度图像P2进行缩小、角点检测、角点特征提取，这几步操作步骤在执行时顺序也不可以变换。但是，对深度图像P1、P2的处理顺序可以变换，即本发明的基于深度摄像头的机器人快速定位与姿态估计方法中可以先对深度图像P1进行处理，也可以先对深度图像P2进行处理。

本发明的基于深度摄像头的机器人快速定位与姿态估计方法的上述步骤中，步骤(5)与(6)的顺序不可以交换，因为步骤(5)的结果要用于步骤(6)中，从而计算出深度摄像头的旋转矩阵R与平移矩阵T。

在本发明的进一步实施例中，对于某一个点X(x，y，z)，该点变换后的位置X’，也可以利用R、T矩阵来计算，计算方法如下：

X＝[x，y，z，1]^T，

X′＝RX+T。

在本发明的进一步实施例中，根据不同环境或具体场景可以将深度摄像头计算旋转或平移的频率设置为不同参数。其中，机器人实时的位姿就是深度摄像头实时获得的旋转参数R，机器人的位置为机器人从原点出发时每次位置变化T的累计。

应当理解的是，本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神及范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围及边界、或者这种范围及边界的等同形式内的全部变化及修改例。