（3. 1)得到的平面点的法 向量和距离十分接近，以其平均值作为该平面在相机坐标系中的法向量和原点到平面的距 离，从而搜索出地面Π = {Pl，p2... pk}，利用最小二乘拟合求出地面的平面方程nT (X-p)= 〇,其中η为地面的法向量，ρ为平面上一点，X = (X，y, ζ)τ为广义坐标；
[0057] (3. 3)相机位姿参数求解阶段：给定地面的平面方程ητ(X-p) = 0,求相机原点到 平面的距离d，得到当前相机的高度h，求相机正前方的单位方向向量与平面法向量η之间 的夹角，得到当前Kinect相机的俯仰角度
[0058] (4)利用基于尺度不变、旋转不变的特征提取算法，得到RGB图像中的特征点，并 通过基于像素的特征描述子对其进行描述，通过对相邻时刻采集的两帧RGB图像中的特征 点进行特征匹配，获取相机在时间段内的相对运动关系，用三维旋转矩阵R和平移向量t来 描述，形式如下：
[0061] 下标c表示相机坐标系下的运动，相机坐标系方向分别是：x轴指向相机正右方，y 轴指向相机正下方，z轴指向相机正前方；
[0062] (5)根据步骤（3)中得到的相机高度h和俯仰角，，将步骤（4)中得到的相机运 动R和t，通过公式转换到地面坐标系，得到基于视觉的机器人在二维地面的相对运动 (△χ ν，^yv, △ θν)，其中，下标V表示该相对运动是基于视觉给出的结果，地面坐标系方向 为轴为机器人正前方，y轴为机器人正右方；转换过程中，先根据相机高度h和俯仰角辦 得到相机相对于机器人的相对位姿关系，
[0065] 左下标c和左上标r表示相机相对于机器人的位姿关系，然后再计算相机在机器 人坐标系的运动氺和公式如下：
[0068] 最后，将其投影到二维地面，得到相对运动（Δ χν，Δ yv，Δ θ v)，其中Δ \和Δ y 彳的前两维，△ θ J相对于z轴的转角；
[0069] (6)通过里程计获取机器人的相对运动信息（Δ X。，Δ y。，Δ Θ。)，下标〇表示里程 计给出的结果；
[0070] (7)利用在线信息融合算法，将视觉和里程计给出的两种机器人相对运动量，即 (Δ Χν，Δ yv, Δ θ v)和（△ X。，Δ y。，Δ Θ。）进行融合，准确估计机器人在t时刻的相对运动量 (Axt，^yt, Δ Θ t);信息融合公式为：
[0074] 其中，《。和Wv分别是里程计信息和视觉信息的权值，表示两种信息的可信程度；
[0075] (8)根据机器人上一时刻位姿（xt i，yt η Θ t J和相对运动信息 (AXt，Ayt, △ 0t)，估计机器人当前时刻的位姿（Xt，yt，0t)，从而实现机器人位姿的跟踪； 计算公式为：
[0079] (9)根据步骤（3)中提取的地面，将三维点云数据投影到地面，得到类似于激光数 据的二维点云；
[0080] (10)利用二维地图构建的SLAM算法，通过激光或Kinect传感器构建出二维环境 地图；
[0081] (11)通过地图匹配算法，将投影后的二维点云与步骤（10)进行匹配，从而修正机 器人跟踪过程中的累积误差，准确的估计机器人在二维地图中的位置和朝向角，实现实时 自定位。
【主权项】
1. 一种基于Kinect的机器人自定位方法，其特征在于，包括以下步骤： (1)将Kinect相机固定安装在机器人上，通过Kinect相机采集环境的RGB图像信息和 深度图像彳目息； ⑵将深度图像信息进行三维恢复，得到三维点云数据：设Kinect深度相机的焦距为f^fy，光心为（cx,cy)，通过公式⑴恢复深度图像上任一点（x，y)的三维坐标（X，Y，Z);其中z为深度值； (3)利用平面提取算法在三维点云中提取出地面部分的点云，并将地面部分的点云进 行平面拟合，得到地面方程；根据相机坐标系下的地面方程，通过坐标求解，计算Kinect相 机相对于地面的高度h和俯仰角口 5 ⑷利用基于尺度不变、旋转不变的特征提取算法，得到RGB图像中的特征点，并通过 基于像素的特征描述子对其进行描述，通过对相邻时刻采集的两帧RGB图像中的特征点进 行特征匹配，获取相机在时间段内的相对运动关系，用三维旋转矩阵R和平移向量t来描 述，形式如下：ct= (X，y，z) 下标c表示相机坐标系下的运动，相机坐标系方向分别是：x轴指向相机正右方，y轴指 向相机正下方，z轴指向相机正前方； (5)根据步骤（3)中得到的相机高度h和俯仰角声，将步骤⑷中得到的相机运 动R和t，通过公式转换到地面坐标系，得到基于视觉的机器人在二维地面的相对运动 (△Xv,Ayv,A0v)，其中，下标V表示该相对运动是基于视觉给出的结果，地面坐标系方向 为轴为机器人正前方，y轴为机器人正右方；转换过程中，先根据相机高度h和俯仰角贫 得到相机相对于机器人的相对位姿关系，左下标C和左上标r表示相机相对于机器人的位姿关系，然后再计算相机在机器人坐 标系的运动氺和彳，公式如下： rR=' R-, R rt=' 最后，将其投影到二维地面，得到相对运动（Axv，Ayv，A0v)，其中AxJPAyvSA的前两维，A0v为氺相对于z轴的转角； (6) 通过里程计获取机器人的相对运动信息（Axci,Ayci,A0J，下标〇表示里程计给 出的结果； (7) 利用在线信息融合算法，将视觉和里程计给出的两种机器人相对运动量，即 (Axv,Ayv,A0v)和（AX。，Ay。，A0。）进行融合，准确估计机器人在t时刻的相对运动量 (Axt，Ayt,A0 t);信息融合公式为：其中，《。和wv分别是里程计信息和视觉信息的权值，表示两种信息的可信程度； (8) 根据机器人上一时刻位姿（xtyt 9tD和相对运动信息（AXt，Ayt,A0 t)，估 计机器人当前时刻的位姿（xt，yt，0t)，从而实现机器人位姿的跟踪；计算公式为： Xt=Xt!+cos( 0ti) ?AXt-Sin( 0ti) ?Ayt Yt=yti+sin( 0t:) ?Axt+cos( 0ti) ?Ayt Qt=^ti+AQt (9) 根据步骤（3)中提取的地面，将三维点云数据投影到地面，得到类似于激光数据的 二维点云； (10) 利用二维地图构建的SLAM算法，通过激光或Kinect传感器构建出二维环境地 图； (11) 通过地图匹配算法，将投影后的二维点云与步骤（10)进行匹配，从而修正机器人 跟踪过程中的累积误差，准确的估计机器人在二维地图中的位置和朝向角，实现实时自定 位。2.根据权利要求1所述的一种基于Kinect的机器人自定位方法，其特征在于，所述步 骤（3)具体包括以下子步骤： (3. 1)地面点云分割阶段：给定点云（Pi=(X;，y;，ZiMi =i...n，n为点的个数，通过 法向量特征和距离特征来判断任意两点是否属于同一个平面，并以此作为扩展规则对所 有点进行扩展，将同一个平面的点拓展到一起，从而将点云分割成一系列的平面{n1 = {pu，pi2. ? ?pik}}i=L..m，m为平面的个数； (3. 2)地面方程拟合阶段：先粗略的给出相机初始高度h~和初始俯仰角度歹，在所有 平面中，搜索出与初始假设最接近的平面；搜索方法是，步骤（3. 1)得到的平面点的法向量 和距离十分接近，以其平均值作为该平面在相机坐标系中的法向量和原点到平面的距离， 从而搜索出地面TI= {Pl，p2...pk}，利用最小二乘拟合求出地面的平面方程nT(X-p) =0,其 中n为地面的法向量，p为平面上一点，X= (X，y,z)T为广义坐标； (3. 3)相机位姿参数求解阶段：给定地面的平面方程nT(X-p) = 0,求相机原点到平面 的距离d，得到当前相机的高度h，求相机正前方的单位方向向量与平面法向量n之间的夹 角，得到当前Kinect相机的俯仰角度识。
【专利摘要】本发明公开了一种基于Kinect的机器人自定位方法，通过Kinect获取环境的RGB图像和深度图像，通过融合视觉和物理里程计的信息，估计机器人的相对运动量，并根据上一时刻机器人位姿，实现位姿跟踪；将深度信息转换成三维点云，在点云中提取出地面，并根据地面自动标定Kinect相对于地面的高度和俯仰角，从而将三维点云投影到地面，得到类似于激光数据的二维点云，将其与事先构建的环境栅格地图进行匹配，从而修正机器人跟踪过程中的累积误差，准确的估计机器人的位姿。本发明通过Kinect替代激光进行自定位，成本低，融合了图像和深度信息，精度高，同时兼容激光地图，且无需事先标定Kinect的安装高度和姿态，使用方便，可满足机器人自主定位导航的需求。
【IPC分类】G05D1/02
【公开号】CN105045263
【申请号】CN201510400786
【发明人】熊蓉, 毛曙源
【申请人】杭州南江机器人股份有限公司
【公开日】2015年11月11日
【申请日】2015年7月6日