一种基于Kinect的机器人自定位方法
【技术领域】
[0001] 本发明涉及一种基于Kinect的机器人自定位方法，属于机器人自主定位导航领 域。
【背景技术】
[0002] 随着计算机和机器人技术的发展，移动机器人得到了快速发展和广泛应用，几乎 渗透到了社会生活的各个领域。传统的自主定位方法中，基于磁条、磁钉等有轨的自定位方 式涉及到磁条的铺设，环境的改造和维护成本大，基于激光的无轨自定位方法，激光传感器 通常十分昂贵，无法实现普遍应用推广。由于技术和成本限制，对机器人无轨自定位，目前 还没有低成本的、稳定的解决方案。
[0003] 近年来，低成本的Kinect等RGB-D相机，已经越来越多的用于移动机器人的定位 和导航。Kinect相机由一个普通相机和一个红外相机组成，分别用于采集颜色（RGB)信 息和深度（Depth)信息，它的集成度高，价格低廉，因而更适和在一般的家庭和办公服务等 场合应用。然而，Kinect相机的精度较低，视野较小，可视范围较近，噪声较大，这些都为 Kinect相机的定位和导航，带来了巨大的困难和挑战。
【发明内容】
[0004] 本发明的目的是针对现有的激光定位的缺陷，提供一种基于Kinect相机的机器 人自定位方法。
[0005] 本发明的目的是通过以下技术方案来实现的：一种基于Kinect的机器人自定位 方法，包括以下步骤：
[0006] (1)将Kinect相机固定安装在机器人上，通过Kinect相机采集环境的RGB图像信 息和深度图像信息；
[0007] (2)将深度图像信息进行三维恢复，得到三维点云数据：设Kinect深度相机的 焦距为亡和f y，光心为（cx, cy)，通过公式（1)恢复深度图像上任一点（X，y)的三维坐标 (X, Y, Z)；
[0011] 其中z为深度值；
[0012] (3)利用平面提取算法在三维点云中提取出地面部分的点云，并将地面部分的 点云进行平面拟合，得到地面方程；根据相机坐标系下的地面方程，通过坐标求解，计算 Kinect相机相对于地面的高度h和俯仰角货；
[0013] (4)利用基于尺度不变、旋转不变的特征提取算法，得到RGB图像中的特征点，并 通过基于像素的特征描述子对其进行描述，通过对相邻时刻采集的两帧RGB图像中的特征 点进行特征匹配，获取相机在时间段内的相对运动关系，用三维旋转矩阵R和平移向量t来 描述，形式如下：
[0016] 下标c表示相机坐标系下的运动，相机坐标系方向分别是：x轴指向相机正右方，y 轴指向相机正下方，z轴指向相机正前方；
[0017] (5)根据步骤（3)中得到的相机高度h和俯仰角^将步骤（4)中得到的相机运 动R和t，通过公式转换到地面坐标系，得到基于视觉的机器人在二维地面的相对运动 (△χ ν，^yv, △ θν)，其中，下标V表示该相对运动是基于视觉给出的结果，地面坐标系方向 为轴为机器人正前方，y轴为机器人正右方；转换过程中，先根据相机高度h和俯仰角F 得到相机相对于机器人的相对位姿关系，
[0018]
[0019] 7-(0,0,/?)
[0020] 左下标c和左上标r表示相机相对于机器人的位姿关系，然后再计算相机在机器 人坐标系的运动氺和公式如下：
[0023] 最后，将其投影到二维地面，得到相对运动（Δ χν，Δ yv，Δ θ v)，其中Δ \和Δ y力 彳的前两维，△ θ J相对于z轴的转角；
[0024] (6)通过里程计获取机器人的相对运动信息（Δ X。，Δ y。，Δ Θ。)，下标〇表示里程 计给出的结果；
[0025] (7)利用在线信息融合算法，将视觉和里程计给出的两种机器人相对运动量，即 (Δ Χν，Δ yv, Δ θ v)和（△ X。，Δ y。，Δ Θ。）进行融合，准确估计机器人在t时刻的相对运动量 (Axt，^yt, Δ Θ t);信息融合公式为：
[0029] 其中，《。和Wv分别是里程计信息和视觉信息的权值，表示两种信息的可信程度；
[0030] (8)根据机器人上一时刻位姿（xt D yt η Θ t D和相对运动信息 (AXt，Ayt, △ 0t)，估计机器人当前时刻的位姿（Xt，yt，0t)，从而实现机器人位姿的跟踪； 计算公式为：
[0034] (9)根据步骤（3)中提取的地面，将三维点云数据投影到地面，得到类似于激光数 据的二维点云；
[0035] (10)利用二维地图构建的SLAM算法，通过激光或Kinect传感器构建出二维环境 地图；
[0036] (11)通过地图匹配算法，将投影后的二维点云与步骤（10)进行匹配，从而修正机 器人跟踪过程中的累积误差，准确的估计机器人在二维地图中的位置和朝向角，实现实时 自定位。
[0037] 进一步地，所述步骤（3)具体包括以下子步骤：
[0038] (3. 1)地面点云分割阶段：给定点云（Pi= (X ;，y;，ZiMi = 1...η，η为点的个数，通 过法向量特征和距离特征来判断任意两点是否属于同一个平面，并以此作为扩展规则对所 有点进行扩展，将同一个平面的点拓展到一起，从而将点云分割成一系列的平面{ Π 1 = {pu，pi2. · · pik}} i = L..m，m 为平面的个数；
[0039] (3. 2)地面方程拟合阶段：先粗略的给出相机初始高度瓦和初始俯仰角度I，在 所有平面中，搜索出与初始假设最接近的平面；搜索方法是，步骤（3. 1)得到的平面点的法 向量和距离十分接近，以其平均值作为该平面在相机坐标系中的法向量和原点到平面的距 离，从而搜索出地面Π = {Pl，p2... pk}，利用最小二乘拟合求出地面的平面方程nT (X-p)= 〇,其中η为地面的法向量，ρ为平面上一点，X = (X，y, ζ)τ为广义坐标；
[0040] (3. 3)相机位姿参数求解阶段：给定地面的平面方程ητ(X-p) = 0,求相机原点到 平面的距离d，得到当前相机的高度h，求相机正前方的单位方向向量与平面法向量η之间 的夹角，得到当前Kinect相机的俯仰角度炉_ &
[0041] 本发明的有益效果是：
[0042] 1.本发明采用廉价的Kinect相机替代昂贵的激光传感器，实现机器人的自主定 位，极大的降低了成本。同时，针对Kinect相机感知视野小、噪声大的缺点，通过融合视觉 特征和物理里程计，准确的估计机器人的运动并进行位姿跟踪，并通过深度信息恢复得到 的点云数据与地图进行匹配，进一步修正机器人的定位误差，可以实现实时的、高精度的、 稳定的机器人自定位。
[0043] 2.本发明通过观测到的地面，自动矫正相机的实时高度和俯仰角。不需要实现对 高度和俯仰角等参数进行标定，并且能够应付机器人颠簸、摇摆导致的相机高度和俯仰角 变化的问题。
[0044] 3.本发明虽然采用Kinect相机，但是同样可以兼容使用激光采集的地图进行自 定位。使用Kinect替代激光以后，不需要通过Kinect再次扫描地图，便可以进行机器人实 时自定位。
【附图说明】
[0045] 图1为地面平面方程与Kinect相机高度和俯仰角的坐标转换关系。
【具体实施方式】
[0046] 下面结合附图对本发明作进一步详细说明。
[0047] 本发明一种基于Kinect的机器人自定位方法，包括以下步骤：
[0048] (1)将Kinect相机固定安装在机器人上，通过Kinect相机采集环境的RGB图像信 息和深度图像信息；
[0049] (2)将深度图像信息进行三维恢复，得到三维点云数据：设Kinect深度相机的 焦距为亡和f y，光心为（cx, cy)，通过公式（1)恢复深度图像上任一点（X，y)的三维坐标 (X, Y, Z)；
[0053] 其中z为深度值；
[0054] (3)利用平面提取算法在三维点云中提取出地面部分的点云，并将地面部分的 点云进行平面拟合，得到地面方程；根据相机坐标系下的地面方程，通过坐标求解，计算 Kinect相机相对于地面的高度h和俯仰角如图1所示，具体包括以下子步骤：
[0055] (3. 1)地面点云分割阶段：给定点云（Pi= (X ;，y;，ZiMi = 1...η，η为点的个数，通 过法向量特征和距离特征来判断任意两点是否属于同一个平面，并以此作为扩展规则对所 有点进行扩展，将同一个平面的点拓展到一起，从而将点云分割成一系列的平面{ Π 1 = {pu，pi2. · · pik}} i = L..m，m 为平面的个数；
[0056] (3. 2)地面方程拟合阶段：先粗略的给出相机初始高度I和初始俯仰角度沒,在 所有平面中，搜索出与初始假设最接近的平面；搜索方法是，步骤