CN105045263B

CN105045263B - 一种基于Kinect深度相机的机器人自定位方法

Info

Publication number: CN105045263B
Application number: CN201510400786.0A
Authority: CN
Inventors: 熊蓉; 毛曙源
Original assignee: Hangzhou Ltd Co Of Nan Jiang Robot
Current assignee: HANGZHOU IPLUS TECH CO.,LTD.
Priority date: 2015-07-06
Filing date: 2015-07-06
Publication date: 2016-05-18
Anticipated expiration: 2035-07-06
Also published as: CN105045263A

Abstract

本发明公开了一种基于Kinect的机器人自定位方法，通过Kinect获取环境的RGB图像和深度图像，通过融合视觉和物理里程计的信息，估计机器人的相对运动量，并根据上一时刻机器人位姿，实现位姿跟踪；将深度信息转换成三维点云，在点云中提取出地面，并根据地面自动标定Kinect相对于地面的高度和俯仰角，从而将三维点云投影到地面，得到类似于激光数据的二维点云，将其与事先构建的环境栅格地图进行匹配，从而修正机器人跟踪过程中的累积误差，准确的估计机器人的位姿。本发明通过Kinect替代激光进行自定位，成本低，融合了图像和深度信息，精度高，同时兼容激光地图，且无需事先标定Kinect的安装高度和姿态，使用方便，可满足机器人自主定位导航的需求。

Description

一种基于Kinect深度相机的机器人自定位方法

技术领域

本发明涉及一种基于Kinect深度相机的机器人自定位方法，属于机器人自主定位导航领域。

背景技术

随着计算机和机器人技术的发展，移动机器人得到了快速发展和广泛应用，几乎渗透到了社会生活的各个领域。传统的自主定位方法中，基于磁条、磁钉等有轨的自定位方式涉及到磁条的铺设，环境的改造和维护成本大，基于激光的无轨自定位方法，激光传感器通常十分昂贵，无法实现普遍应用推广。由于技术和成本限制，对机器人无轨自定位，目前还没有低成本的、稳定的解决方案。

近年来，低成本的Kinect等RGB-D相机，已经越来越多的用于移动机器人的定位和导航。Kinect相机由一个普通相机和一个红外相机组成，分别用于采集颜色(RGB)信息和深度(Depth)信息，它的集成度高，价格低廉，因而更适和在一般的家庭和办公服务等场合应用。然而，Kinect相机的精度较低，视野较小，可视范围较近，噪声较大，这些都为Kinect相机的定位和导航，带来了巨大的困难和挑战。

发明内容

本发明的目的是针对现有的激光定位的缺陷，提供一种基于Kinect深度相机的机器人自定位方法。

本发明的目的是通过以下技术方案来实现的：一种基于Kinect深度相机的机器人自定位方法，包括以下步骤：

(1)将Kinect深度相机固定安装在机器人上，通过Kinect深度相机采集环境的RGB图像信息和深度图像信息；

(2)将深度图像信息进行三维恢复，得到三维点云数据：设Kinect深度相机的焦距为f_x和f_y，光心为(c_x,c_y)，通过公式(1)恢复深度图像上任一点(x,y)的三维坐标(X,Y,Z)；

X = (x - c_{x}) * \frac{1}{f_{x}} * z

Y = (y - c_{y}) * \frac{1}{f_{y}} * z - - - (1)

Z＝z

其中z为深度值；

(3)利用平面提取算法在三维点云中提取出地面部分的点云，并将地面部分的点云进行平面拟合，得到地面方程；根据相机坐标系下的地面方程，通过坐标求解，计算Kinect深度相机相对于地面的高度h和俯仰角

(4)利用基于尺度不变、旋转不变的特征提取算法，得到RGB图像中的特征点，并通过基于像素的特征描述子对其进行描述，通过对相邻时刻采集的两帧RGB图像中的特征点进行特征匹配，获取相机在时间段内的相对运动关系，用三维旋转矩阵R和平移向量t来描述，形式如下：

R_{c} = (\begin{matrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{matrix})

_ct＝(x,y,z)

下标c表示相机坐标系下的运动，相机坐标系方向分别是：x轴指向相机正右方，y轴指向相机正下方，z轴指向相机正前方；

(5)根据步骤(3)中得到的相机高度h和俯仰角将步骤(4)中得到的相机运动R和t，通过公式转换到地面坐标系,得到基于视觉的机器人在二维地面的相对运动(Δx_v,Δy_v,Δθ_v)，其中，下标v表示该相对运动是基于视觉给出的结果，地面坐标系方向为：x轴为机器人正前方，y轴为机器人正右方；转换过程中，先根据相机高度h和俯仰角得到相机相对于机器人的相对位姿关系，

t_{c}^{r} = (0, 0, h)

左下标c和左上标r表示相机相对于机器人的位姿关系，然后再计算相机在机器人坐标系的运动_rR和_rt，公式如下：

R_{r} = R_{r}^{c} \cdot R_{c}

t_{r} = R_{r}^{c} \cdot t_{c} + t_{r}^{c}

最后，将其投影到二维地面，得到相对运动(Δx_v,Δy_v,Δθ_v)，其中Δx_v和Δy_v为_rt的前两维，Δθ_v为_rR相对于z轴的转角；

(6)通过里程计获取机器人的相对运动信息(Δx_o,Δy_o,Δθ_o)，下标o表示里程计给出的结果；

(7)利用在线信息融合算法，将视觉和里程计给出的两种机器人相对运动量，即(Δx_v,Δy_v,Δθ_v)和(Δx_o,Δy_o,Δθ_o)进行融合，准确估计机器人在t时刻的相对运动量(Δx_t,Δy_t,Δθ_t)；信息融合公式为：

{Δx}_{t} = \frac{w_{o} \times {Δx}_{v} + w_{v} \times {Δx}_{o}}{w_{o} + w_{v}}

{Δy}_{t} = \frac{w_{o} \times {Δy}_{v} + w_{v} \times {Δy}_{o}}{w_{o} + w_{v}}

{Δθ}_{t} = \frac{w_{o} \times {Δθ}_{v} + w_{v} \times {Δθ}_{o}}{w_{o} + w_{v}}

其中，w_o和w_v分别是里程计信息和视觉信息的权值，表示两种信息的可信程度；

(8)根据机器人上一时刻位姿(x_t-1,y_t-1,θ_t-1)和相对运动信息(Δx_t,Δy_t,Δθ_t)，估计机器人当前时刻的位姿(x_t,y_t,θ_t)，从而实现机器人位姿的跟踪；计算公式为：

x_t＝x_t-1+cos(θ_t-1)·Δx_t-sin(θ_t-1)·Δy_t

y_t＝y_t-1+sin(θ_t-1)·Δx_t+cos(θ_t-1)·Δy_t

θ_t＝θ_t-1+Δθ_t

(9)根据步骤(3)中提取的地面，将三维点云数据投影到地面，得到类似于激光数据的二维点云；

(10)利用二维地图构建的SLAM算法，通过激光或Kinect深度相机构建出二维环境地图；

(11)通过地图匹配算法，将投影后的二维点云与步骤(10)进行匹配，从而修正机器人跟踪过程中的累积误差，准确的估计机器人在二维地图中的位置和朝向角，实现实时自定位。

进一步地，所述步骤(3)具体包括以下子步骤：

(3.1)地面点云分割阶段：给定点云{p_i＝(x_i,y_i,z_i)}_i＝1...n，n为点的个数，通过法向量特征和距离特征来判断任意两点是否属于同一个平面，并以此作为扩展规则对所有点进行扩展，将同一个平面的点拓展到一起，从而将点云分割成一系列的平面{∏_i＝{p_i1,p_i2...p_ik}}_i＝1...m，m为平面的个数；

(3.2)地面方程拟合阶段：先粗略的给出相机初始高度和初始俯仰角度在所有平面中，搜索出与初始假设最接近的平面；搜索方法是，步骤(3.1)得到的平面点的法向量和距离十分接近，以其平均值作为该平面在相机坐标系中的法向量和原点到平面的距离，从而搜索出地面∏＝{p₁,p₂...p_k}，利用最小二乘拟合求出地面的平面方程n^T(X-p)＝0，其中n为地面的法向量，p为平面上一点，X＝(x,y,z)^T为广义坐标；

(3.3)相机位姿参数求解阶段：给定地面的平面方程n^T(X-p)＝0，求相机原点到平面的距离d，得到当前相机的高度h，求相机正前方的单位方向向量与平面法向量n之间的夹角，得到当前Kinect深度相机的俯仰角度

本发明的有益效果是：

1.本发明采用廉价的Kinect相机替代昂贵的激光传感器，实现机器人的自主定位，极大的降低了成本。同时，针对Kinect相机感知视野小、噪声大的缺点，通过融合视觉特征和物理里程计，准确的估计机器人的运动并进行位姿跟踪，并通过深度信息恢复得到的点云数据与地图进行匹配，进一步修正机器人的定位误差，可以实现实时的、高精度的、稳定的机器人自定位。

2.本发明通过观测到的地面，自动矫正相机的实时高度和俯仰角。不需要实现对高度和俯仰角等参数进行标定，并且能够应付机器人颠簸、摇摆导致的相机高度和俯仰角变化的问题。

3.本发明虽然采用Kinect相机，但是同样可以兼容使用激光采集的地图进行自定位。使用Kinect替代激光以后，不需要通过Kinect再次扫描地图，便可以进行机器人实时自定位。

附图说明

图1为地面平面方程与Kinect相机高度和俯仰角的坐标转换关系。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明一种基于Kinect深度相机的机器人自定位方法，包括以下步骤：

X = (x - c_{x}) * \frac{1}{f_{x}} * z

Y = (y - c_{y}) * \frac{1}{f_{y}} * z - - - (1)

Z＝z

其中z为深度值；

(3)利用平面提取算法在三维点云中提取出地面部分的点云，并将地面部分的点云进行平面拟合，得到地面方程；根据相机坐标系下的地面方程，通过坐标求解，计算Kinect深度相机相对于地面的高度h和俯仰角如图1所示，具体包括以下子步骤：

R_{c} = (\begin{matrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{matrix})

_ct＝(x,y,z)

t_{c}^{r} = (0, 0, h)

R_{r} = R_{r}^{c} \cdot R_{c}

t_{r} = R_{r}^{c} \cdot t_{c} + t_{r}^{c}

{Δx}_{t} = \frac{w_{o} \times {Δx}_{v} + w_{v} \times {Δx}_{o}}{w_{o} + w_{v}}

{Δy}_{t} = \frac{w_{o} \times {Δy}_{v} + w_{v} \times {Δy}_{o}}{w_{o} + w_{v}}

{Δθ}_{t} = \frac{w_{o} \times {Δθ}_{v} + w_{v} \times {Δθ}_{o}}{w_{o} + w_{v}}

x_t＝x_t-1+cos(θ_t-1)·Δx_t-sin(θ_t-1)·Δy_t

y_t＝y_t-1+sin(θ_t-1)·Δx_t+cos(θ_t-1)·Δy_t

θ_t＝θ_t-1+Δθ_t

(10)利用二维地图构建的SLAM算法，通过激光或Kinec深度相机构建出二维环境地图；

Claims

1.一种基于Kinect深度相机的机器人自定位方法，其特征在于，包括以下步骤：

X = (x - c_{x}) * \frac{1}{f_{x}} * z

Y = (y - c_{y}) * \frac{1}{f_{y}} * z - - - (1)

Z＝z

其中z为深度值；

R_{c} = (\begin{matrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{matrix})

_ct＝(x,y,z)

t_{c}^{r} = (0, 0, h)

R_{r} = R_{r}^{c} \cdot R_{c}

t_{r} = R_{r}^{c} \cdot t_{c} + t_{r}^{c}

其中，_rR表示相机在机器人坐标系的三维旋转矩阵，_rt表示相机在机器人坐标系的平移向量；

{Δx}_{t} = \frac{w_{o} \times {Δx}_{v} + w_{v} \times {Δx}_{o}}{w_{o} + w_{v}}

{Δy}_{t} = \frac{w_{o} \times {Δy}_{v} + w_{v} \times {Δy}_{o}}{w_{o} + w_{v}}

{Δθ}_{t} = \frac{w_{o} \times {Δθ}_{v} + w_{v} \times {Δθ}_{o}}{w_{o} + w_{v}}

x_t＝x_t-1+cos(θ_t-1)·Δx_t-sin(θ_t-1)·Δy_t

y_t＝y_t-1+sin(θ_t-1)·Δx_t+cos(θ_t-1)·Δy_t

θ_t＝θ_t-1+Δθ_t

2.根据权利要求1所述的一种基于Kinect深度相机的机器人自定位方法，其特征在于，所述步骤(3)具体包括以下子步骤：

(3.1)地面点云分割阶段：给定点云{p_i＝(x_i,y_i,z_i)}_i＝1...n，n为点的个数，通过法向量特征和距离特征来判断任意两点是否属于同一个平面，并以此作为扩展规则对所有点进行扩展，将同一个平面的点拓展到一起，从而将点云分割成一系列的平面{Π_i＝{p_i1,p_i2...p_ik}}_i＝1...m，m为平面的个数；

(3.2)地面方程拟合阶段：先粗略的给出相机初始高度和初始俯仰角度在所有平面中，搜索出与初始假设最接近的平面；搜索方法是，步骤(3.1)得到的平面点的法向量和距离十分接近，以其平均值作为该平面在相机坐标系中的法向量和原点到平面的距离，从而搜索出地面Π＝{p₁,p₂...p_k}，利用最小二乘拟合求出地面的平面方程n^T(X-p)＝0，其中n为地面的法向量，p为平面上一点，X＝(x,y,z)^T为广义坐标；