CN108460779A - 一种动态环境下的移动机器人图像视觉定位方法 - Google Patents

Abstract

本发明公开了一种动态环境下的移动机器人图像视觉定位方法。Kinect深度相机固定于移动机器人，针对相邻两帧图像，获得depth深度图像的三维点云，获得RGB彩色图像的匹配点对；通过迭代最近点算法得到相邻两帧图像之间的初始位姿变换矩阵；对三维点云聚类变换，计算相邻两帧图像的三维点云聚类之间的残差进行分割；利用静态背景中的匹配点对和三维点云重复处理得到更准确的位姿变换矩阵，计算所有相邻两帧图像之间的位姿变换矩阵，实现移动机器人图像视觉定位。本发明降低了硬件成本，克服了恢复像素深度值的复杂性，修正了机器人的定位误差，更加准确地估计了移动机器人在室内的运动轨迹。

Description

一种动态环境下的移动机器人图像视觉定位方法

技术领域

本发明属于视觉定位导航领域，具体涉及一种动态环境下的移动机器人图 像视觉定位方法。

背景技术

随着机器人技术的快速发展，移动机器人在工业中得到了广泛应用。传统 的定位方法中，绝对定位主要采用导航信标、标识和卫星导航技术等进行定位， 但是信标、标识的建设和维护成本较高，GPS只能用于室外。相对定位主要采 用轮式编码器、惯性测量单元和激光雷达等，轮式编码器和惯性测量单元都是 测量一些间接的物理量，高精度的激光雷达价格过于昂贵，这些因素给自主定 位的移动机器人的商业化、进入日常生活带来了不小的挑战和困难。

近几年来，以视觉传感器为中心的视觉SLAM技术得到了明显的转变和突 破。多种基于单目、双目和深度相机的理论方法被提出，其中，Kinect深度相机 相比于激光雷达虽然精度较差、噪声大，但是成本低，能够采集彩色图像信息， 因而更加适合在普通的办公、家庭等室内场景使用。然而目前主流的方法中， 都是基于机器人所处环境为静态的假设条件，随机采样一致性算法对于存在运 动物体的场景具有一定的鲁棒性，然而随着运动物体在场景中所占比例变大， 算法的定位精度就会变差甚至导致定位失败。

发明内容

为了解决背景技术中存在的问题，本发明为解决现有算法在动态环境下鲁 棒性差的问题，提出了一种动态环境下的移动机器人图像视觉定位方法。

本发明采用的技术方案是包括以下步骤：

步骤一，将Kinect深度相机固定安装在移动机器人上，通过相机获取机器 人所处环境的RGB彩色图像和depth深度图像；

步骤二，针对相邻两帧图像，根据depth深度图像，得到每一帧图像的包含 环境信息的三维点云；

步骤三，针对相邻两帧图像，采用ORB特征(Oriented FAST and Rotated BRIEF)方式对每一帧RGB彩色图像依次进行提取关键点和计算描述子的操作， 并对描述子进行特征匹配，然后采用随机采样一致性算法剔除误匹配点，获得 匹配点对；

步骤四，根据相邻两帧图像的三维点云和匹配点对通过迭代最近点算法进 行运动估计得到相邻两帧图像之间的初始位姿变换矩阵T_coarse；

步骤五，对步骤二得到的三维点云进行聚类，并根据步骤四得到的初始位 姿变换矩阵T_coarse将已聚类的三维点云进行变换，计算相邻两帧图像的三维点云 聚类之间的残差进行静态背景和动态前景的分割；

步骤六，利用静态背景中的匹配点对和对应的三维点云重复步骤四通过迭 代最近点算法进行处理得到更准确的位姿变换矩阵T_fine；

步骤七，针对每相邻两帧图像重复上述步骤二～步骤六，计算所有相邻两帧 图像之间的位姿变换矩阵，实现移动机器人图像视觉定位。

所述的步骤五中，以相邻两帧图像的前一帧作为参考帧，以相邻两帧图像 的后一帧作为当前帧，采用以下方式进行处理：

1)点云聚类阶段：利用参考帧I₁和当前帧I₂对应的三维点云采用K-means 聚类方法进行聚类将三维点云中的点分为k类，得到当前帧I₂的聚类结果C和参 考帧I₁的聚类结果C′：

C＝{c₁,…,c_k}

C′＝{c′₁,…,c′_k}

其中，c₁表示当前帧I₂的第一类点集合，c_k表示当前帧I₂的第k类点集合；c′₁表示参考帧I₁的第一类点集合，c′_k表示参考帧I₁的第k类点集合；

2)背景分割阶段：

根据初始位姿变换矩阵T_coarse将当前帧I₂的聚类结果C变换到参考帧坐标系 得到位姿变换后的聚类结果C_T＝{c_1T,…,c_kT}，c_1T表示当前帧I₂的第一类点集合 变换到参考帧坐标系后的集合，c_kT表示当前帧I₂的第k类点集合变换到参考帧 坐标系后的集合；

并计算位姿变换后的聚类结果C_T与参考帧I₁的聚类结果C′中各同类点集合 c′_i与c_iT之间的残差δ_i，残差为点集合c′_i中各点与点集合c_iT中各自对应点之间差的 平均值，取所有残差中的中间值作为分割阈值T；

最后将残差δ_i与分割阈值T进行比较，将小于分割阈值T的残差δ_i对应的聚类 点集合作为静态背景，反之作为动态前景，表示为：

其中，δ_i表示聚类结果C_T与聚类结果C′中第i类点集合之间的残差。

所述步骤1)中，采用K-means聚类方法进行聚类将三维点云中的点分为k 类，具体为：设置聚类数为k，设定k个聚类中心，根据最小距离原则将每个点 分配至各个聚类，然后重新计算聚类中心的位置，再根据最小距离原则多次迭 代进行聚类直至收敛。

所述步骤2)中的参考帧坐标系是指参考帧下以相机光心为基准建立的三维 坐标系。

所述的参考帧I₁和当前帧I₂对应的三维点云分别是在各自的坐标系下，即参 考帧坐标系和当前帧坐标系，当前帧坐标系是指当前帧下以相机光心为基准建 立的三维坐标系。

所述步骤一中，针对足式移动机器人，Kinect深度相机固定安装在机器人顶 部，针对轮式移动机器人，Kinect深度相机固定安装在机器人前部。

本发明的有益效果是：

1.本发明采用了Kinect深度相机，相比于激光雷达传感器，大大降低了硬件 成本，相比于单目和双目相机，克服了恢复像素深度值的复杂性。

2.本发明采用特殊的聚类分割前景和背景方式对两帧之间的初始位姿变换 矩阵进行修正优化，进行了动态场景下的背景分割，通过静态背景内的特征点， 进一步修正了机器人的定位误差，更加准确地估计了移动机器人在室内的运动 轨迹。

附图说明

图1为本发明的视觉SLAM原理框图；

图2为depth深度图像转化为三维点云时的针孔相机模型；

图3为相邻两帧的深度图像和对应的三维点云；

图4为相邻两帧图像的三维点云经过K-means聚类后的结果；

图5为相邻两帧图像背景分割结果图。

具体实施方式

下面结合附图对本发明的技术方案做进一步详细的说明。

步骤一，将Kinect深度相机固定安装在轮式移动机器人前部，通过相机获 取机器人所处环境的各帧图像，每帧图像包含了RGB彩色图像和depth深度图 像。

步骤二，根据针孔相机模型对前后相邻两帧的每帧depth深度图像处理得到 每一帧depth深度图像的包含环境信息的三维点云；

如图2所示，空间点经过相机成像后的像素点在图像坐标系中的坐标为 [u v]，在世界坐标系中的坐标为[x y z]，世界坐标系为参考帧坐标系或者 当前帧坐标系(若处理的参考帧，则为参考帧坐标系)，则二者对应关系如下：

式中，f_x和f_y为Kinect深度相机的焦距，(c_x,c_y)为Kinect深度相机的光心， s为尺度因子，d为深度值。

根据上式求得空间点的三维坐标如下：

如图3所示为相邻两帧深度图像，根据上式分别计算得到三维点云。

步骤三，采用ORB特征(Oriented FAST and Rotated BRIEF)方式对RGB 彩色图像依次进行提取关键点和计算描述子的操作，并对描述子进行特征匹配， 然后采用随机采样一致性算法剔除误匹配点，获得匹配点对；

步骤四，根据相邻两帧图像的三维点云和匹配点对通过迭代最近点算法得 到相邻两帧图像之间的初始位姿变换矩阵T_coarse：

利用相邻两帧的RGB彩色图像I₁和I₂，通过特征提取和匹配得到的ORB特 征点表示如下：

P＝{p₁,…,p_n}，p′＝{p′₁,…,p′_n}

其中，P为参考帧中的特征点集合，p′为当前帧中的特征点集合，p₁为参考 帧中特征点集合的第一个点，p_n为参考帧中特征点集合的第n个点，p′₁为当前 帧中特征点集合的第一个点，p＇_n为当前帧中特征点集合的第n个点。

然后建立最小二乘问题的目标函数，如下：

其中，R为当前帧变换到参考帧的旋转矩阵，t为当前帧变换到参考帧的平 移向量，n为匹配特征点对的数量，表示二范数。

通过求解上式得到旋转矩阵R和平移向量t，二者构成了相邻两帧图像之间 的初始位姿变换矩阵T_coarse，如下式：

步骤五，对步骤二得到相邻两帧图像的三维点云进行聚类，并根据步骤四 得到的初始变换矩阵将已聚类的三维点云进行变换，计算相邻两帧图像的三维 点云聚类之间的残差进行静态背景和动态前景的分割；

以相邻两帧图像的前一帧作为参考帧，以相邻两帧图像的后一帧作为当前 帧，采用以下方式进行处理：

1)点云聚类阶段：利用参考帧I₁和当前帧I₂对应的三维点云采用K-means 聚类方法进行聚类将三维点云中的点分为k＝6类，如图4所示，得到当前帧I₂的 聚类结果C和参考帧I₁的聚类结果C＇：

C＝{c₁,…,c_k}

C′＝{c′₁,…,c′_k}

其中，c₁表示当前帧I₂的第一类点集合，c_k表示当前帧I₂的第k类点集合；c′₁表示参考帧I₁的第一类点集合，c＇_k表示参考帧I₁的第k类点集合；

2)背景分割阶段：

根据初始位姿变换矩阵T_coarse将当前帧I₂的聚类结果C变换到参考帧坐标系 得到位姿变换后的聚类结果C_T＝{c_1T,…,c_kT}，c_1T表示当前帧I₂的第一类点集合 变换到参考帧坐标系后的集合，c_kT表示当前帧I₂的第k类点集合变换到参考帧 坐标系后的集合；

并计算位姿变换后的聚类结果C_T与参考帧I₁的聚类结果C′中各同类点集合 c′_i与c_iT之间的残差δ_i，残差为点集合c′_i中各点与点集合c_iT中各自对应点之间差的 平均值，取所有残差中的中间值作为分割阈值T；

最后将残差δ_i与分割阈值T进行比较，将小于分割阈值T的残差δ_i对应的聚类 点集合作为静态背景，反之作为动态前景，表示为：

其中，δ_i表示聚类结果C_T与聚类结果C′中第i类点集合之间的残差。

如图5所示，将背景分割后的静态背景用黑色表示，可以发现，图像中走 动的人被很好地分割出来。

步骤六，利用静态背景中的匹配点对和三维点云重复步骤四通过迭代最近 点算法进行处理得到更准确的位姿变换矩阵T_fine；

步骤七，针对每相邻两帧图像重复上述步骤二～步骤六，计算所有相邻两帧 图像之间的位姿变换矩阵，实现移动机器人图像视觉定位。

Claims (5)

1.一种动态环境下的移动机器人图像视觉定位方法，其特征在于，包括以下步骤：

步骤一，将Kinect深度相机固定安装在移动机器人上，通过相机获取机器人所处环境的RGB彩色图像和depth深度图像；

步骤二，针对相邻两帧图像，根据depth深度图像，得到每一帧图像的三维点云；

步骤三，针对相邻两帧图像，采用ORB特征(Oriented FAST and Rotated BRIEF)方式对每一帧RGB彩色图像依次进行提取关键点和计算描述子的操作，并对描述子进行特征匹配，然后采用随机采样一致性算法剔除误匹配点，获得匹配点对；

步骤四，根据相邻两帧图像的三维点云和匹配点对通过迭代最近点算法得到相邻两帧图像之间的初始位姿变换矩阵T_coarse；

步骤五，对步骤二得到的三维点云进行聚类，并根据步骤四得到的初始位姿变换矩阵T_coarse将已聚类的三维点云进行变换，计算相邻两帧图像的三维点云聚类之间的残差进行静态背景和动态前景的分割；

步骤六，利用静态背景中的匹配点对和对应的三维点云重复步骤四通过迭代最近点算法进行处理得到更准确的位姿变换矩阵T_fine；

步骤七，针对每相邻两帧图像重复上述步骤二～步骤六，计算所有相邻两帧图像之间的位姿变换矩阵，实现移动机器人图像视觉定位。

2.根据权利要求1所述的一种动态环境下的移动机器人图像视觉定位方法，其特征在于：所述的步骤五中，以相邻两帧图像的前一帧作为参考帧，以相邻两帧图像的后一帧作为当前帧，采用以下方式进行处理：

1)点云聚类阶段：利用参考帧I₁和当前帧I₂对应的三维点云采用K-means聚类方法进行聚类将三维点云中的点分为k类，得到当前帧I₂的聚类结果C和参考帧I₁的聚类结果C′：

C＝{c₁,…,c_k}

C′＝{c′₁,…,c′_k}

其中，c₁表示当前帧I₂的第一类点集合，c_k表示当前帧I₂的第k类点集合；c′₁表示参考帧I₁的第一类点集合，c′_k表示参考帧I₁的第k类点集合；

2)背景分割阶段：

根据初始位姿变换矩阵T_coarse将当前帧I₂的聚类结果C变换到参考帧坐标系得到位姿变换后的聚类结果C_T＝{c_1T,…,c_kT}，c_1T表示当前帧I₂的第一类点集合变换到参考帧坐标系后的集合，c_kT表示当前帧I₂的第k类点集合变换到参考帧坐标系后的集合；

并计算位姿变换后的聚类结果C_T与参考帧I₁的聚类结果C′中各同类点集合c′_i与c_iT之间的残差δ_i，残差为点集合c′_i中各点与点集合c_iT中各自对应点之间差的平均值，取所有残差中的中间值作为分割阈值T；

最后将残差δ_i与分割阈值T进行比较，将小于分割阈值T的残差δ_i对应的聚类点集合作为静态背景，反之作为动态前景，表示为：

其中，δ_i表示聚类结果C_T与聚类结果C′中第i类点集合之间的残差。

3.根据权利要求1所述的一种动态环境下的移动机器人图像视觉定位方法，其特征在于：所述步骤1)中，采用K-means聚类方法进行聚类将三维点云中的点分为k类，具体为：设置聚类数为k，设定k个聚类中心，根据最小距离原则将每个点分配至各个聚类，然后重新计算聚类中心的位置，再根据最小距离原则多次迭代进行聚类直至收敛。

4.根据权利要求2所述的一种动态环境下的移动机器人图像视觉定位方法，其特征在于：所述步骤2)中的参考帧坐标系是指参考帧下以相机光心为基准建立的三维坐标系。

5.根据权利要求1所述的一种动态环境下的移动机器人图像视觉定位方法，其特征在于：所述步骤一中，针对足式移动机器人，Kinect深度相机固定安装在机器人顶部，针对轮式移动机器人，Kinect深度相机固定安装在机器人前部。