第13卷第3期 智能系统学报 Vol.13 No.3 2018年6月 CAAI Transactions on Intelligent Systems Jun.2018 D0:10.11992/tis.201702005 网络出版t地址：http:/kns.cnki.net/cms/detail/23.1538.TP.20180404.1343.010.html 基于RGB-D信息的移动机器人SLAM和路径规划 方法研究与实现 李元2，王石荣”，于宁波2 (L.南开大学机器人与信息自动化研究所，天津300353,2.南开大学天津市智能机器人技术重点实验室，天津 300353) 摘要：移动机器人在各种辅助任务中需具备自主定位、建图、路径规划与运动控制的能力。本文利用RGBD信息 和ORB-SLAM算法进行自主定位，结合点云数据和GMapping算法建立环境栅格地图，基于二次规划方法进行平滑 可解析的路径规划，并设计非线性控制器，实现了由一个运动底盘、一个RGBD传感器和一个运算平台组成的自主 移动机器人系统。经实验验证，这一系统实现了复杂室内环境下的实时定位与建图、自主移动和障碍物规避。由此 为移动机器人的推广应用提供了一个硬件结构简单、性能良好、易扩展、经济性好、开发维护方便的解决方案。 关键词：移动机器人；RGB-D:SLAM;GMapping;路径规划；二次规划；曲线拟合；非线性控制 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2018)03-0445-07 中文引用格式：李元，王石荣，于宁波.基于RGB-D信息的移动机器人SLAM和路径规划方法研究与实现J.智能系统学报，2018， 13(3:445-451. 英文引用格式：LI Yuan,.VANG Shirong,YU Ningbo.RGB-D-based SLAM and path planning for mobile robots[J.CAAI trans-. actions on intelligent systems,2018,13(3):445-451. RGB-D-based SLAM and path planning for mobile robots LI Yuan'2,WANG Shirong'2,YU Ningbo'2 (1.Institute of Robotics and Automatic Information Systems,Nankai University,Tianjin 300353,China;2.Tianjin Key Laboratory of Intelligent Robotics,Nankai University,Tianjin 300353,China) Abstract:To accomplish various tasks,mobile robots need the capabilities of autonomous localization,mapping,path planning,and motion control.In this paper,an autonomous robot system was realized using a mobile platform,an RGB- D sensor,and a computing unit.Localization was achieved using the ORB-SLAM algorithm based on RGB-D informa- tion,and the 2D grid map was built-up using the GMapping algorithm with localization and point cloud data.A smooth path was produced by point generation and curve fitting with quadratic programming.The robot motion was regulated by a nonlinear controller.The proposed system and methods were validated by experiments and the results show that the system can realize real-time localization and mapping,autonomous movement,and obstacle avoidance in a complex in- door environment.Therefore,the research provides a solution with the following merits for promotion and application of mobile robots:simple hardware structure,excellent performance,easy extension,as well as economical and convenient development and maintenance. Keywords:mobile robot;RGB-D;SLAM;GMapping,path planning;quadratic programming;curve fitting;nonlinear control 收稿日期：2017-02-16.网络出版日期：2018-04-04. 近年来，智能移动机器人技术得到迅速发展， 基金项目：国家自然科学基金项目(61720106012)：机器人学国家 重点实验室开放课题项目(2016-003). 已广泛应用于工业、军事、物流、办公和家庭服务等 通信作者：于宁波.E-mail:nyu@nankai..edu.cn 领域。智能移动机器人在环境中自主运行，通常
DOI: 10.11992/tis.201702005 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20180404.1343.010.html 基于 RGB-D 信息的移动机器人 SLAM 和路径规划 方法研究与实现 李元1,2，王石荣1,2，于宁波1,2 （1. 南开大学 机器人与信息自动化研究所，天津 300353; 2. 南开大学 天津市智能机器人技术重点实验室，天津 300353） 摘 要：移动机器人在各种辅助任务中需具备自主定位、建图、路径规划与运动控制的能力。本文利用 RGB-D 信息 和 ORB-SLAM 算法进行自主定位，结合点云数据和 GMapping 算法建立环境栅格地图，基于二次规划方法进行平滑 可解析的路径规划，并设计非线性控制器，实现了由一个运动底盘、一个 RGB-D 传感器和一个运算平台组成的自主 移动机器人系统。经实验验证，这一系统实现了复杂室内环境下的实时定位与建图、自主移动和障碍物规避。由此， 为移动机器人的推广应用提供了一个硬件结构简单、性能良好、易扩展、经济性好、开发维护方便的解决方案。 关键词：移动机器人；RGB-D；SLAM；GMapping；路径规划；二次规划；曲线拟合；非线性控制 中图分类号：TP242.6 文献标志码：A 文章编号：1673−4785(2018)03−0445−07 中文引用格式：李元, 王石荣, 于宁波. 基于 RGB-D 信息的移动机器人 SLAM 和路径规划方法研究与实现[J]. 智能系统学报, 2018, 13(3): 445–451. 英文引用格式：LI Yuan, WANG Shirong, YU Ningbo. RGB-D-based SLAM and path planning for mobile robots[J]. CAAI transactions on intelligent systems, 2018, 13(3): 445–451. RGB-D-based SLAM and path planning for mobile robots LI Yuan1,2 ，WANG Shirong1,2 ，YU Ningbo1,2 (1. Institute of Robotics and Automatic Information Systems, Nankai University, Tianjin 300353, China; 2. Tianjin Key Laboratory of Intelligent Robotics, Nankai University, Tianjin 300353, China) Abstract: To accomplish various tasks, mobile robots need the capabilities of autonomous localization, mapping, path planning, and motion control. In this paper, an autonomous robot system was realized using a mobile platform, an RGBD sensor, and a computing unit. Localization was achieved using the ORB-SLAM algorithm based on RGB-D information, and the 2D grid map was built-up using the GMapping algorithm with localization and point cloud data. A smooth path was produced by point generation and curve fitting with quadratic programming. The robot motion was regulated by a nonlinear controller. The proposed system and methods were validated by experiments and the results show that the system can realize real-time localization and mapping, autonomous movement, and obstacle avoidance in a complex indoor environment. Therefore, the research provides a solution with the following merits for promotion and application of mobile robots: simple hardware structure, excellent performance, easy extension, as well as economical and convenient development and maintenance. Keywords: mobile robot; RGB-D; SLAM; GMapping; path planning; quadratic programming; curve fitting; nonlinear control 近年来，智能移动机器人技术得到迅速发展， 已广泛应用于工业、军事、物流、办公和家庭服务等 领域[1-2]。智能移动机器人在环境中自主运行，通常 收稿日期：2017−02−16. 网络出版日期：2018−04−04. 基金项目：国家自然科学基金项目（61720106012）；机器人学国家 重点实验室开放课题项目（2016-003）. 通信作者：于宁波. E-mail：nyu@nankai.edu.cn. 第 13 卷第 3 期 智 能 系 统 学 报 Vol.13 No.3 2018 年 6 月 CAAI Transactions on Intelligent Systems Jun. 2018
·446· 智能系统学报 第13卷 需要搭载激光雷达、GPS、声呐、单目或双目视觉设 1硬件平台与系统架构 备等以获取环境信息，辅助移动机器人进行自主定 位、建图、环境探测等任务。然而，采用大量传感 本文建立的移动机器人系统由一个运动底盘 器虽然可以提高系统的精度和鲁棒性，但也显著提 一个RGB-D传感器和一个移动运算平台组成，如 高了机器人硬件和软件系统的复杂性以及成本，限 图1所示。Yujin Kobuki运动底盘由两轮差速驱 制了移动机器人的进一步推广应用。为此，建立 动，最大线速度可达70cms,最大旋转速度为180(ys, 一个硬件结构简单、性能良好、经济性和通用性好 最大运载能力5kg。采用Kinect V2传感器，TOF 的移动机器人系统，成为一个迫切需要解决的问题。 (time of flight)测量，水平可视角度为70.6°，垂直可 随着计算机视觉技术的进步，单目或双目视觉 视角度为60°，最大探测距离为8m,最小探测距离 设备成为移动机器人的一种选择。RGB-D传感 为0.5m,探测精度为2cm。采用惠普ENVY15- 器能同时获取色彩与深度信息，易安装使用，成本 j105tw作为移动运算平台，配备了core i74702m中 低，被广泛应用于目标识别、跟踪等领域⑧，利用 央处理器以及8GB运行内存。基于Ubuntu14.04 RGB-D传感器进行SLAM的研究迅速发展。美国 LTS操作系统，安装配置了机器人操作系统ROS 华盛顿大学与德国弗菜堡大学各自实现了基于 Yujin Kobuki Kinect V2 RGB-D传感器的SLAM系统o1,利用图像特征估 运动底盘 RGB-D传感器 计图像间的变换关系，并利用ICP(iterative closest point)等算法优化。卡耐基梅隆大学开发出利用多 运动和位姿 边形重构和融合算法，在原始数据中提取平面特 征，显著提高了算法的实时性☒。2007年Klein等] 首次提出了并行跟踪与建图算法PTAM(parallel tracking and mapping),关键帧以及多线程处理方法 被广泛采用。ORB-SLAM算法由西班牙萨拉戈萨 大学Raul Mur-.Artal等提出，由跟踪、局部建图和 线速度和角速度 RGB-D数据 闭环检测3个并行线程组成，具备重新定位能力， 移动运算平台 取得了良好的稳定性、鲁棒性和实时性。 Ubuntu 14.04 LTS+ROS Indigo+ORB-SLAM+GMapping 国内基于RGB-D信息的研究也迅猛发展。清 图1移动机器人系统的硬件平台 华大学孙富春教授8-团队利用Kinect传感器和 Fig.1 Hardware platform of the mobile robot system RGB-D信息完成了场景建模、机器人自主导航和人 机协作等工作。北京工业大学贾松敏教授团队 Indigo、.libfreenect2、PCL、OpenCV以及不莱梅大学 提供的Kinect V2的ROS驱动等基础工具包。在 通过提取Kinect RGB信息的特征点进行粗匹配，并 结合深度信息，通过迭代最近点算法实时获取位 此环境下，可方便地开发后续的移动机器人自主定 姿，采用关键帧选取机制及线程并行处理的方法， 位、障碍物检测规避与运动控制等算法。 提高了算法运行效率。上海交通大学曹其新教授团 本文的移动机器人工作流程如图2所示，主要 队I),从两个方面对Kinect Fusion算法进行改进提 包括以下功能单元。 高，通过使用环境中的边线特征点匹配提高了算法 I)实时定位：根据Kinect V2获取的环境RGB- 的定位鲁棒性，并通过在点云模型中预设地面点云 D信息，利用ORB-SLAM算法计算当前位姿。 提高了算法精度。第二炮兵工程大学张国良教授课 2)环境地图创建：结合Kinect V2点云信息和 题组提出了一种基于快速视觉里程计及大回环局 当前位姿，利用GMapping建立二维栅格地图。 部优化模型，提高配准精度并实现对累积误差快速 3)路径点生成：利用栅格地图，结合定位信息， 优化的同步定位与三维点云地图创建方法。 利用改进人工势场法规划运动路径点。 本文使用基础运动底盘，搭载通用RGB-D传 4)路径曲线拟合：根据规划的路径点，利用二 感器，通过开发整合相应算法，建立了一个硬件结 次规划进行曲线拟合，获得可解析的路径曲线。 构简单、易开发维护的自主移动机器人系统，实现 5)路径跟踪：根据路径曲线，考虑运动约束，生 了自主定位、障碍规避、运动规划与控制等功能。 成控制量，控制移动机器人运动
需要搭载激光雷达、GPS、 声呐、单目或双目视觉设 备等以获取环境信息，辅助移动机器人进行自主定 位、建图、环境探测等任务[3-4]。然而，采用大量传感 器虽然可以提高系统的精度和鲁棒性，但也显著提 高了机器人硬件和软件系统的复杂性以及成本，限 制了移动机器人的进一步推广应用[5]。为此，建立 一个硬件结构简单、性能良好、经济性和通用性好 的移动机器人系统，成为一个迫切需要解决的问题。 随着计算机视觉技术的进步，单目或双目视觉 设备成为移动机器人的一种选择[6-7]。RGB-D 传感 器能同时获取色彩与深度信息，易安装使用，成本 低，被广泛应用于目标识别、跟踪等领域[8-9] ，利用 RGB-D 传感器进行 SLAM 的研究迅速发展。美国 华盛顿大学与德国弗莱堡大学各自实现了基于 RGB-D 传感器的 SLAM 系统[10-11] ，利用图像特征估 计图像间的变换关系，并利用 ICP (iterative closest point) 等算法优化。卡耐基梅隆大学开发出利用多 边形重构和融合算法，在原始数据中提取平面特 征，显著提高了算法的实时性[12]。2007 年 Klein 等 [13] 首次提出了并行跟踪与建图算法 PTAM (parallel tracking and mapping)，关键帧以及多线程处理方法 被广泛采用。ORB-SLAM 算法由西班牙萨拉戈萨 大学 Raul Mur-Artal 等 [14]提出，由跟踪、局部建图和 闭环检测 3 个并行线程组成，具备重新定位能力， 取得了良好的稳定性、鲁棒性和实时性[15]。 国内基于 RGB-D 信息的研究也迅猛发展。清 华大学孙富春教授[8-9]团队利用 Kinect 传感器和 RGB-D 信息完成了场景建模、机器人自主导航和人 机协作等工作。北京工业大学贾松敏教授团队 [16] ， 通过提取 Kinect RGB 信息的特征点进行粗匹配，并 结合深度信息，通过迭代最近点算法实时获取位 姿，采用关键帧选取机制及线程并行处理的方法， 提高了算法运行效率。上海交通大学曹其新教授团 队 [17] ，从两个方面对 Kinect Fusion 算法进行改进提 高，通过使用环境中的边线特征点匹配提高了算法 的定位鲁棒性，并通过在点云模型中预设地面点云 提高了算法精度。第二炮兵工程大学张国良教授课 题组[18]提出了一种基于快速视觉里程计及大回环局 部优化模型，提高配准精度并实现对累积误差快速 优化的同步定位与三维点云地图创建方法。 本文使用基础运动底盘，搭载通用 RGB-D 传 感器，通过开发整合相应算法，建立了一个硬件结 构简单、易开发维护的自主移动机器人系统，实现 了自主定位、障碍规避、运动规划与控制等功能。 1 硬件平台与系统架构 本文建立的移动机器人系统由一个运动底盘、 一个 RGB-D 传感器和一个移动运算平台组成，如 图 1 所示。Yujin Kobuki 运动底盘由两轮差速驱 动，最大线速度可达 70 cm/s，最大旋转速度为 180 (°)/s， 最大运载能力 5 kg。采用 Kinect V2 传感器，TOF (time of flight) 测量，水平可视角度为 70.6°，垂直可 视角度为 60°，最大探测距离为 8 m，最小探测距离 为 0.5 m，探测精度为 2 cm。采用惠普 ENVY 15- j105tx 作为移动运算平台，配备了 core i7 4 702 m 中 央处理器以及 8 GB 运行内存。基于 Ubuntu 14.04 LTS 操作系统，安装配置了机器人操作系统 ROS Indigo、libfreenect2、PCL、OpenCV 以及不莱梅大学 提供的 Kinect V2 的 ROS 驱动等基础工具包[19]。在 此环境下，可方便地开发后续的移动机器人自主定 位、障碍物检测规避与运动控制等算法。 本文的移动机器人工作流程如图 2 所示，主要 包括以下功能单元。 1) 实时定位：根据 Kinect V2 获取的环境 RGBD 信息，利用 ORB-SLAM 算法计算当前位姿。 2) 环境地图创建：结合 Kinect V2 点云信息和 当前位姿，利用 GMapping 建立二维栅格地图。 3) 路径点生成：利用栅格地图，结合定位信息， 利用改进人工势场法规划运动路径点。 4) 路径曲线拟合：根据规划的路径点，利用二 次规划进行曲线拟合，获得可解析的路径曲线。 5) 路径跟踪：根据路径曲线，考虑运动约束，生 成控制量，控制移动机器人运动。 线速度和角速度 运动和位姿 Yujin Kobuki 运动底盘 移动运算平台 Ubuntu 14.04 LTS+ROS Indigo+ORB−SLAM+GMapping Kinect V2 RGB-D传感器 RGB−D 数据 图 1 移动机器人系统的硬件平台 Fig. 1 Hardware platform of the mobile robot system ·446· 智 能 系 统 学 报 第 13 卷
第3期 李元，等：基于RGB-D信息的移动机器人SLAM和路径规划方法研究与实现 ·447· 运动控制命令 期望 位姿 路径点 路径曲线 路径 Yujin Kobuki 实际 生成 拟合 跟踪 运动底盘 位姿 ,当前 一维环墙 位姿ORB-SLAM 栅格地图 算法 RGB-D 数据 Kinect V2 点云数据判断并 RGB-D传感器 GMapping 激光类型 生成激光类型数据 数据 图2系统架构与工作流程 Fig.2 Architecture and workflow of the system 2主要研究方法 跟踪模块 Kinect V2RGB-节 传感器 2.1基于ORB-SLAM的实时定位 基于采用的移动机器人系统软硬件平台，实现 RGB图像☐ 了如图3所示的ORB-SLAM算法。 基于ORB算法的 深度图像 不同帧图像特征 提取RGB-D数据中彩色图像的ORB图像特 提取与匹配 征，与上一帧图像特征点做初始匹配，得到初始点 对，同时利用深度信息进行尺度判断。若存在足够 前后两帧是否有 的匹配点对，则对包含深度信息的当前帧与前一帧 足够匹配的特征 点对数？ 匹配点对进行基于RANSAC算法的前后两帧位姿 N 计算当前帧的词袋向量 估计。若没有足够的匹配点对，则首先使用DBoW2 库建立关键帧ORB特征的词袋向量，在关键帧词 基于RANSAC算法的 将当前帧的词袋向量 袋数据库进行搜索匹配，选取最佳位姿估计。进而 前后两帧位姿估计 与关键帧数据进行 搜索匹配 利用covisibility graph获取局部地图进行当前帧位 姿优化，获得当前帧位姿等信息，然后判断是否为 选取基于RANSAC算法 的当前帧与各关键帧间 新的关键帧，用于进行局部建图，并采用g20对当 最佳位姿估计 前正在处理的关键帧与相关联的covisibility graph 内不同时刻的关键帧进行局部集束调整(bundle ad- 利用covisibility graph 获取局部地图进行当 justment)),最后进行闭环检测。由此，实现了移动机 前帧位姿优化 器人的自主实时定位与特征地图创建。 当前帧信息 2.2基于GMapping的二维栅格地图创建 (位姿、特征点、相机参数) 栅格地图用于表示二维平面坐标点存在障碍物 判断新的关键幀 的概率值，具有构建速度快、存储空间小、易扩展等 优势2。为实现路径规划，有必要建立二维栅格地 关键帧 covisibility graph 数据库 图，对此GMapping算法具有良好性能2u。 GMapping算法的输入为当前位姿及二维激光 局部建图 闭环检测 关键顿插人及 类型观测数据，输出为二维栅格地图。当前实时位 优化后的最新帧 地图修正 姿通过ORB-SLAM获取，而二维激光类型观测数 据由RGB-D数据生成。利用获取的点云信息，根 基于go对关键帧进行局部 闭环候选帧检测及候选帧 集束调整(bundle adjustment) 之间的位姿估计与优化 据移动机器人高度、RGB-D的实际观测视角及深度 范围添加采样约束，取约束范围内离移动机器人最 局部关键帧剪枝 闭环融合与图优化 近的点集，降维生成在指定坐标系平面（移动机器 人基坐标系平面)的激光类型数据。这种方法保留 图3基于RGB-D信息的ORB-SLAM算法流程 了环境立体信息，有利于自主移动机器人系统及时 Fig.3 The ORB-SLAM algorithm workflow for RGB-D 根据环境变化进行响应。 data
2 主要研究方法 2.1 基于 ORB-SLAM 的实时定位 基于采用的移动机器人系统软硬件平台，实现 了如图 3 所示的 ORB-SLAM 算法。 提取 RGB-D 数据中彩色图像的 ORB 图像特 征，与上一帧图像特征点做初始匹配，得到初始点 对，同时利用深度信息进行尺度判断。若存在足够 的匹配点对，则对包含深度信息的当前帧与前一帧 匹配点对进行基于 RANSAC 算法的前后两帧位姿 估计。若没有足够的匹配点对，则首先使用 DBoW2 库建立关键帧 ORB 特征的词袋向量，在关键帧词 袋数据库进行搜索匹配，选取最佳位姿估计。进而 利用 covisibility graph 获取局部地图进行当前帧位 姿优化，获得当前帧位姿等信息，然后判断是否为 新的关键帧，用于进行局部建图，并采用 g2o 对当 前正在处理的关键帧与相关联的 covisibility graph 内不同时刻的关键帧进行局部集束调整 (bundle adjustment)，最后进行闭环检测。由此，实现了移动机 器人的自主实时定位与特征地图创建。 2.2 基于 GMapping 的二维栅格地图创建 栅格地图用于表示二维平面坐标点存在障碍物 的概率值，具有构建速度快、存储空间小、易扩展等 优势[20]。为实现路径规划，有必要建立二维栅格地 图，对此 GMapping 算法具有良好性能[21]。 GMapping 算法的输入为当前位姿及二维激光 类型观测数据，输出为二维栅格地图。当前实时位 姿通过 ORB-SLAM 获取，而二维激光类型观测数 据由 RGB-D 数据生成。利用获取的点云信息，根 据移动机器人高度、RGB-D 的实际观测视角及深度 范围添加采样约束，取约束范围内离移动机器人最 近的点集，降维生成在指定坐标系平面 (移动机器 人基坐标系平面) 的激光类型数据。这种方法保留 了环境立体信息，有利于自主移动机器人系统及时 根据环境变化进行响应。 期望 位姿 路径点 生成 Yujin Kobuki 运动底盘 实际 位姿 ORB−SLAM 算法 Kinect V2 RGB-D传感器 运动控制命令 当前 位姿 GMapping 二维环境 栅格地图 点云数据判断并 激光类型 生成激光类型数据 数据 路径 跟踪 路径曲线 拟合 RGB-D 数据 xd yd θd xm ym θm vd ωd xa ya θa 图 2 系统架构与工作流程 Fig. 2 Architecture and workflow of the system 前后两帧是否有 足够匹配的特征 点对数？ 跟踪模块 N 局部建图 闭环融合与图优化 闭环检测 Y RGB 图像 深度图像 计算当前帧的词袋向量 将当前帧的词袋向量 与关键帧数据进行 搜索匹配 判断新的关键帧 关键帧插入及 地图修正 优化后的最新帧 局部关键帧剪枝 Kinect V2 RGB−D 传感器 基于ORB算法的 不同帧图像特征 提取与匹配 基于RANSAC算法的 前后两帧位姿估计 选取基于RANSAC算法 的当前帧与各关键帧间 最佳位姿估计 利用covisibility graph 获取局部地图进行当 前帧位姿优化 当前帧信息 (位姿、特征点、相机参数) 关键帧 数据库 covisibility graph 闭环候选帧检测及候选帧 之间的位姿估计与优化 基于g 2o对关键帧进行局部 集束调整(bundle adjustment) 图 3 基于 RGB-D 信息的 ORB-SLAM 算法流程 Fig. 3 The ORB-SLAM algorithm workflow for RGB-D data 第 3 期 李元，等：基于 RGB-D 信息的移动机器人 SLAM 和路径规划方法研究与实现 ·447·
·448· 智能系统学报 第13卷 建图过程中，GMapping把订阅ORB-SLAM生 的问题。为此，利用二次规划方法，将路径点进行 成的移动机器人实时位姿作为里程计信息，通过 曲线拟合，生成一条平滑可解析的路径曲线，并最 RGB-D点云生成的激光类型数据，实时建立环境的 终生成对移动机器人的控制量。 二维栅格地图。基于建图环节的优化和闭环检测环 将生成的路径点表示为 节的约束，ORB-SLAM提供的实时位姿具有较好的 ∫x(s)=o+a1s1+a2s2+…+ans (8) 稳定性与精确性，且位姿更新频率能达到20Hz,满 y(s)=bo+b1s+b2s2+...+bs" 足需求；同时，根据移动机器人高度约束，取符合高 式中：s为参数，假设路径点为m个，进而可以将等式 度范围内的RGB-D点云用于地图构建。由于GMap (8)表示为矩阵形式，即 ping栅格地图构建的方式实时性较好，在本平台上 AX=B (9) 能达到最少5Hz的更新频率，为自主移动机器人平 式中： 台实时进行路径规划与控制提供了保证。 1 后…6 2.3基于人工势场法的路径点生成 0 对于移动机器人的路径规划，人工势场法是一 种通用算法，传统人工势场法需处理势场中存在的 A 10 局部最小、目标不可达等问题2。为避免上述问 1 ss 题，本文采用改进人工势场法进行路径规划四。 0 在空间中某个位置X=[x,引力势场函数为 1 sh Um(q)=2p(4.9ema） (1) (10) X=aoa1a2…an bo by b2… 式中：ε是引力比例因子，p(q,9a)为机器人q与目标 (11) qa之间的距离。引力可表示为势场的负梯度函 B=x0X…xm%y1… (12) 数，如式(2)所示： 限定移动机器人路径曲线必须通过初始点以及 Fan(q）=-VUam(q）=ep(qeal-q) (2) 终止点，有如下约束： 斥力对应的势场函数为 [1s站哈…哈 ea(q)= 0 Xm 112 X 1 p"(q.qgoa),p(q,qobs)Po 式中： 根据式（⑧）可得 1 p"(q.9goa) as=c01x ox(s) (15) (5) p(q,qobs)po p2(q,qobs) (16) n 12 =[0c]x os Fp2=2 -三p-1(q,qgoa）) (6) 2p(g,gobs)Po 式中： 分别为排斥机器人远离障碍物的分量和吸引机器人 C=[012s… ns- (17) 向目标运动的分量。 设tanO,为路径曲线上某一点角度的正切值，进 因此，移动机器人承受的总作用力由引力和斥 而可得约束，即 力的合力构成，可表示为 Rtan0o -R Fsum(q)=Fum(q)+Frepl Vp(q.qobs)-Frep2Vp(q.qgoal)() Ttane =0 (18) 根据上述方法，结合定位信息以及栅格地图， 式中： 可获得移动机器人的路径点。 R=012s6 … ns哈1 (19) 2.4基于二次规划的运动路径曲线拟合 前述获得的路径点存在曲线不平滑、不可解析 T=[012x…nsg] (20)
建图过程中，GMapping 把订阅 ORB-SLAM 生 成的移动机器人实时位姿作为里程计信息，通过 RGB-D 点云生成的激光类型数据，实时建立环境的 二维栅格地图。基于建图环节的优化和闭环检测环 节的约束，ORB-SLAM 提供的实时位姿具有较好的 稳定性与精确性，且位姿更新频率能达到 20 Hz，满 足需求；同时，根据移动机器人高度约束，取符合高 度范围内的 RGB-D 点云用于地图构建。由于 GMapping 栅格地图构建的方式实时性较好，在本平台上 能达到最少 5 Hz 的更新频率，为自主移动机器人平 台实时进行路径规划与控制提供了保证。 2.3 基于人工势场法的路径点生成 对于移动机器人的路径规划，人工势场法是一 种通用算法，传统人工势场法需处理势场中存在的 局部最小、目标不可达等问题[22]。为避免上述问 题，本文采用改进人工势场法进行路径规划[23]。 X = [x y] 在空间中某个位置 T，引力势场函数为 Uatt(q) = 1 2 ερ(q,qgoal) (1) ε ρ(q,qgoal) q qgoal 式中： 是引力比例因子， 为机器人 与目标 之间的距离。引力可表示为势场的负梯度函 数，如式 (2) 所示： Fatt(q) = −∇Uatt(q) = ερ(qgoal −q) (2) 斥力对应的势场函数为 Ureq(q) = 1 2 η ( 1 ρ(q,qobs) − 1 ρ0 )2 ρ n (q,qgoal), ρ(q,qobs) ⩽ ρ0 0, 其他 (3) η ρ(q,qobs) q qobs ρ0 式中： 为斥力比例因子， 为机器人 与障碍 物 之间的最小距离， 为障碍物的影响范围。进 而斥力函数可表示为 Frep(q) = { Frep1 + Frep2, ρ(q,qobs) ⩽ ρ0 0, ρ(q,qobs) > ρ0 (4) 式中： Frep1 = η ( 1 ρ(q,qobs) − 1 ρ0 ) ρ n (q,qgoal) ρ 2 (q,qobs) (5) Frep2 = n 2 η ( 1 ρ(q,qobs) − 1 ρ0 )2 ρ n−1 (q,qgoal) (6) 分别为排斥机器人远离障碍物的分量和吸引机器人 向目标运动的分量。 因此，移动机器人承受的总作用力由引力和斥 力的合力构成，可表示为 Fsum(q) = Fatt(q)+ Frep1∇ρ(q,qobs)− Frep2∇ρ(q,qgoal) (7) 根据上述方法，结合定位信息以及栅格地图， 可获得移动机器人的路径点。 2.4 基于二次规划的运动路径曲线拟合 前述获得的路径点存在曲线不平滑、不可解析 的问题。为此，利用二次规划方法，将路径点进行 曲线拟合，生成一条平滑可解析的路径曲线，并最 终生成对移动机器人的控制量。 将生成的路径点表示为 { x(s) = a0 +a1 s 1 +a2 s 2 +···+an s n y(s) = b0 +b1 s 1 +b2 s 2 +···+bn s n (8) 式中： s 为参数，假设路径点为m个，进而可以将等式 (8) 表示为矩阵形式，即 AX = B (9) 式中： A = 1 s 1 0 s 2 0 ··· s n 0 1 s 1 1 s 2 1 ··· s n 1 . . . . . . . . . . . . 1 s 1 m s 2 m ··· s n m 0 0 1 s 1 0 s 2 0 ··· s n 0 1 s 1 1 s 2 1 ··· s n 1 . . . . . . . . . . . . 1 s 1 m s 2 m ··· s n m (10) X = [ a0 a1 a2 ··· an b0 b1 b2 ··· bn ]T (11) B = [ x0 x1 ··· xm y0 y1 ··· ym ]T (12) 限定移动机器人路径曲线必须通过初始点以及 终止点，有如下约束： x0 xm y0 ym = 1 s 1 0 s 2 0 ··· s n 0 1 s 1 m s 2 m ··· s n m 0 0 1 s 1 0 s 2 0 ··· s n 0 1 s 1 m s 2 m ··· s n m X (13) 为了能够实现移动机器人的动态路径规划，在 控制过程中采取分段规划的策略。因此，路径曲线 还需满足起始角度和终止角度约束： ∂x(s) ∂s sinθ− ∂y(s) ∂s cos θ = 0 (14) 根据式 (8) 可得 ∂x(s) ∂s = [ C T 0 ] X (15) ∂y(s) ∂s = [ 0 C T ] X (16) 式中： C = [ 0 1 2s 1 ··· nsn−1 ] (17) 设 tanθt为路径曲线上某一点角度的正切值，进 而可得约束，即 [ Rtanθ0 −R Ttanθm −T ] X = 0 (18) 式中： R = [ 0 1 2s 1 0 ··· nsn−1 0 ] (19) T = [ 0 1 2s 1 m ··· nsn−1 m ] (20) ·448· 智 能 系 统 学 报 第 13 卷
第3期 李元，等：基于RGB-D信息的移动机器人SLAM和路径规划方法研究与实现 ·449· 在上述条件下进行路径曲线拟合，即在满足上 分别将移动机器人置于以下8个初始位姿： 述约束下求解最优参数使得式(21)取最小值： [0-2.40,[-2.4-2.40,[-2.400,[-2.42.40] minllAX-B2= [02.40,[2.42.40,[2.400,[2.4-2.40,期望 min(AX-B)T(AX-B)= 21) min(XTATAX-2BTAX)+BTB 位姿设为[00π/2]。x,y单位为m,0单位为弧度。 式中BB为标量。进而可将上述曲线拟合问题转化 图5所示为实时定位与运动控制实验结果。虚 为二次规划问题，目标函数为 线为基于理想模型和控制器的理论运动路径，实线 为实验得到的实际运行路径。可以看出，两种路径 min(XATAX-2BTAX) (22) 趋势一致，且路径曲线平滑，定位平稳。 需同时满足式(13)和式(18)的线性约束。 300 由此，可将路径点拟合成可解析的路径曲线。 2.5移动机器人运动控制 200 移动机器人从初始位姿到目标位姿，可用经典 100 的控制方法，如基于极坐标的点镇定控制方法2。 0 本文生成了可解析的路径曲线，能够采用非线性跟 踪控制器2。 -100 使用的两轮差速驱动底盘运动学模型为 -200 =vcos0 y=vsin (23) -300 300-200-1000100200300 0=w X/cm 式中[xy为当前位姿。位姿开环误差为 图5理论计算路径（虚线）与实验运行路径（实线） cos0 Xd-X Fig.5 The calculated paths(dashed lines)and experiment- -sin cos0 0 (24) al paths(full lines) 0 0 1 8-0 图6为8次实验中移动机器人到达期望位姿的 式中：[e.e,e为误差，[xayu0u为期望位姿。对其 位置和姿态误差，实心圆点代表位置误差，箭头方 进行求导并简化，可得 向代表姿态误差。移动机器人距离目标点X轴方向 ex=Vacoseo-v+eyw 的平均距离偏差为0.43cm,y轴方向的平均距离偏 ey=Vasineo-exw (25) eo=@d-W 差为1.12cm,总体距离误差都在3cm以内。姿态 采用如下的非线性控制器： 角偏差的绝对平均值为2.16°。实验表明，采用RGB- v=vacoseo+k e D信息和ORB-SLAM算法进行移动机器人实时定 nee (26) w=Wd+eyvd- 位确实有效，为后续的运动规划与控制打下了良好 基础。 式中k,k为参数，可以保证闭环系统渐进稳定。 3实验结果与分析 利用本文的自主移动机器人系统进行实验，以 验证本文方法的有效性，如图4所示。 移动运算平台 Kinect V2 RGB-D传感器 0 3 Kobuki X/cm 运动底盘 图6移动机器人到达目标点的位置和姿态误差 Fig.6 Position and orientation errors at the target pose 图4自主移动机器人系统 为验证本文方法和系统的有效性，在障碍环境 Fig.4 The autonomous mobile robot system 下进行实验，结果如图7所示。图7(a)为实验环
在上述条件下进行路径曲线拟合，即在满足上 述约束下求解最优参数使得式 (21) 取最小值： min∥AX− B∥ 2 = min(AX− B) T (AX − B) = min( X TA TAX −2B TAX) + B TB (21) B T 式中 B 为标量。进而可将上述曲线拟合问题转化 为二次规划问题，目标函数为 min( X TA TAX −2B TAX) (22) 需同时满足式 (13) 和式 (18) 的线性约束。 由此，可将路径点拟合成可解析的路径曲线。 2.5 移动机器人运动控制 移动机器人从初始位姿到目标位姿，可用经典 的控制方法，如基于极坐标的点镇定控制方法[24]。 本文生成了可解析的路径曲线，能够采用非线性跟 踪控制器[25]。 使用的两轮差速驱动底盘运动学模型为 x˙ = v cos θ y˙ = v sinθ θ˙ = ω (23) [ x y θ 式中 ]T为当前位姿。位姿开环误差为 ex ey eθ = cos θ sinθ 0 −sinθ cos θ 0 0 0 1 xd − x yd −y θd −θ (24) [ ex ey eθ ]T [ xd yd θd 式中： 为误差， ]T为期望位姿。对其 进行求导并简化，可得 e˙x = vd cos eθ −v+eyω e˙y = vd sineθ −exω e˙θ = ωd −ω (25) 采用如下的非线性控制器： v = vd cos eθ +k1ex ω = ωd +eyvd sineθ eθ +k2eθ (26) 式中 k1, k2为参数，可以保证闭环系统渐进稳定[25]。 3 实验结果与分析 利用本文的自主移动机器人系统进行实验，以 验证本文方法的有效性，如图 4 所示。 ⼧ߔ䓼ッज Kobuki 䓼ߔᏁⰄ Kinect V2 RGB-Dьᙋஔ 图 4 自主移动机器人系统 Fig. 4 The autonomous mobile robot system [0 −2.4 0] T ,[−2.4 −2.4 0] T ,[−2.4 0 0] T ,[−2.4 2.4 0] T [0 2.4 0] T ,[2.4 2.4 0] T ,[2.4 0 0] T ,[2.4 −2.4 0] T [0 0 π/2] T x, y θ 分别将移动机器人置于以下 8 个初始位姿： ， ，期望 位姿设为 。 单位为 m， 单位为弧度。 图 5 所示为实时定位与运动控制实验结果。虚 线为基于理想模型和控制器的理论运动路径，实线 为实验得到的实际运行路径。可以看出，两种路径 趋势一致，且路径曲线平滑，定位平稳。 300 200 100 0 −100 −200 −300 Y/cm −300 −200 −100 0 100 200 300 X/cm 图 5 理论计算路径 (虚线) 与实验运行路径 (实线) Fig. 5 The calculated paths (dashed lines) and experimental paths (full lines) X Y 图 6 为 8 次实验中移动机器人到达期望位姿的 位置和姿态误差，实心圆点代表位置误差，箭头方 向代表姿态误差。移动机器人距离目标点 轴方向 的平均距离偏差为 0.43 cm， 轴方向的平均距离偏 差为 1.12 cm，总体距离误差都在 3cm 以内。姿态 角偏差的绝对平均值为 2.16°。实验表明，采用 RGBD 信息和 ORB-SLAM 算法进行移动机器人实时定 位确实有效，为后续的运动规划与控制打下了良好 基础。 3 2 1 0 −1 −2 −3 Y/cm X/cm −3 −2 −1 0 1 2 3 图 6 移动机器人到达目标点的位置和姿态误差 Fig. 6 Position and orientation errors at the target pose 为验证本文方法和系统的有效性，在障碍环境 下进行实验，结果如图 7 所示。图 7(a) 为实验环 第 3 期 李元，等：基于 RGB-D 信息的移动机器人 SLAM 和路径规划方法研究与实现 ·449·
·450· 智能系统学报 第13卷 境，图7(b)为移动机器人在此存在障碍物的环境下 参考文献： 进行地图创建和运动规划与控制的实验结果。可以 看出，根据RGB-D点云生成的激光类型数据，通过 [1]LEE S,JUNG S.Novel design and control of a home ser- GMapping算法成功建立了二维环境栅格地图，利 vice mobile robot for Korean floor-living life style:KOBOKER [C]//Proceedings of the 8th International Conference on 用改进人工势场法以及二次规划和轨迹跟踪等算 Ubiquitous Robots and Ambient Intelligence.Incheon, 法，有效规避了障碍物，最终成功实现了从初始位 South Korea,2011:863-867. 置运动到目标位置运动，运动路径平滑。 [2]HABIB MK,BAUDOIN Y,NAGATA F.Robotics for res- cue and risky intervention[C]//Proceedings of the 37th An- nual Conference on IEEE Industrial Electronics Society. Melbourne,Australia,2011:3305-3310. [3]权美香，朴松吴，李国.视觉SLAM综述).智能系统学 报，2016.11(6)：768-776 QUAN Meixiang,PIAO Songhao,LI Guo.An overview of visual SLAM[J].CAAI transactions on intelligent systems, 2016,11(6):768-776 [4张学习，杨宜民.基于多传感器信息融合的移动机器人快 (a)室内实验环境 速精确自定位[).控制理论与应用，2011,28(3)：443-448. 400 ZHANG Xuexi,YANG Yimin.Fast and accurate self-local- 350 ization of mobile robot based on multi-sensor[J].Control 300 theory&applications,2011,28(3):443-448. 250 [5]于宁波，李元，徐昌，等.一种基于RGB-D SLAM的移动 200 机器人定位和运动规划与控制方法.系统科学与数学， 150 2015,35(7):838-847. YU Ningbo,LI Yuan,XU Chang,et al.Localization and 100 motion planning and control of mobile robots with RGB-D 50 SLAMIJ].Journal of systems science and mathematical sci- 0 ences,.2015,35(7):838-847 -200 -100 0100200 X/cm [6)张雪波，方勇纯，刘玺.移动机器人自适应视觉伺服镇定 (b)建立的二维橱格地图和移动机器人 控制[J.控制理论与应用，2010,27(9)：1123-1130 避障并到达目标点的运动路径 ZHANG Xuebo,FANG Yongchun.LIU Xi.Adaptive visu- 图7移动机器人室内实验 al servo regulation of mobile robots[J].Control theory ap- Fig.7 Mobile robot experiment in indoor environment plications,2010,27(9)1123-1130 4结束语 [)王可，贾松敏，李秀智，等.基于地面特征的移动机器人单 目视觉里程计算法[.光学学报，2015,35(5)：0515002. 本文利用基础运动底盘，搭载通用RGB-D传 WANG Ke,JIA Songmin.LI Xiuzhi,et al.Mobile robot 感器与移动计算平台，建立了一个架构简单、经济 monocular visual odometry algorithm based on ground fea- 性好、易开发维护、性能强大、扩展性好的自主移动 tures[J].Acta optica sinica,2015,35(5):0515002. 机器人系统。采用ORB-SLAM算法、GMapping算 [8]张雪华，刘华平，孙富春，等，采用Kinect的移动机器人目 法、改进人工势场法、二次规划算法以及运动控制 标跟踪U.智能系统学报，2014,9(1)：3439 ZHANG Xuehua,LIU Huaping,SUN Fuchun,et al.Target 算法，实现了移动机器人的自主定位、路径规划、运 tracking of mobile robot using Kinect[J].CAAI transactions 动控制、障碍物规避等功能，并实时建立了环境二 on intelligent systems,2014,9(1):34-39 维栅格地图。由此，为移动机器人在家庭等各种场 [雷丽充，刘华平，孙富春，等.面向灵巧操作的视觉目标识 景下的推广应用提供了一个解决方案。 别).智能系统学报，2015,10(1少37-42. 将来可以基于该系统的硬件和软件架构，面向 LEI Lichong,LIU Huaping,SUN Fuchun,et al.Visual ob- 具体应用有针对性地开发整合相关算法，提高系统 ject recognition for smart manipulation[J].CAAI transac- 的智能化程度与控制性能，进一步拓展移动机器人 tions on intelligent systems,2015,10(1):37-42. 的功能和应用范围。 [10]HENRY P,KRAININ M,HERBST E,et al.RGB-D map-
境，图 7(b) 为移动机器人在此存在障碍物的环境下 进行地图创建和运动规划与控制的实验结果。可以 看出，根据 RGB-D 点云生成的激光类型数据，通过 GMapping 算法成功建立了二维环境栅格地图，利 用改进人工势场法以及二次规划和轨迹跟踪等算 法，有效规避了障碍物，最终成功实现了从初始位 置运动到目标位置运动，运动路径平滑。 (a) აڱ侸⣛ද 400 350 300 250 200 150 100 50 0 Y/cm X/cm −200 −100 0 100 200 (b) ᐦ⿷⮰θ㐠ᴱᵨపস⼧ߔᱦஔϦ 䖫䯈Ꭲݜ䓪Ⱊᴳ◥⮰䓼ߔ䌛ᒰ 图 7 移动机器人室内实验 Fig. 7 Mobile robot experiment in indoor environment 4 结束语 本文利用基础运动底盘，搭载通用 RGB-D 传 感器与移动计算平台，建立了一个架构简单、经济 性好、易开发维护、性能强大、扩展性好的自主移动 机器人系统。采用 ORB-SLAM 算法、GMapping 算 法、改进人工势场法、二次规划算法以及运动控制 算法，实现了移动机器人的自主定位、路径规划、运 动控制、障碍物规避等功能，并实时建立了环境二 维栅格地图。由此，为移动机器人在家庭等各种场 景下的推广应用提供了一个解决方案。 将来可以基于该系统的硬件和软件架构，面向 具体应用有针对性地开发整合相关算法，提高系统 的智能化程度与控制性能，进一步拓展移动机器人 的功能和应用范围。 参考文献： LEE S, JUNG S. Novel design and control of a home service mobile robot for Korean floor-living life style: KOBOKER [C]//Proceedings of the 8th International Conference on Ubiquitous Robots and Ambient Intelligence. Incheon, South Korea, 2011: 863–867. [1] HABIB M K, BAUDOIN Y, NAGATA F. Robotics for rescue and risky intervention[C]//Proceedings of the 37th Annual Conference on IEEE Industrial Electronics Society. Melbourne, Australia, 2011: 3305–3310. [2] 权美香, 朴松昊, 李国. 视觉 SLAM 综述[J]. 智能系统学 报, 2016, 11(6): 768–776. QUAN Meixiang, PIAO Songhao, LI Guo. An overview of visual SLAM[J]. CAAI transactions on intelligent systems, 2016, 11(6): 768–776. [3] 张学习, 杨宜民. 基于多传感器信息融合的移动机器人快 速精确自定位[J]. 控制理论与应用, 2011, 28(3): 443–448. ZHANG Xuexi, YANG Yimin. Fast and accurate self-localization of mobile robot based on multi-sensor[J]. Control theory & applications, 2011, 28(3): 443–448. [4] 于宁波, 李元, 徐昌, 等. 一种基于 RGB-D SLAM 的移动 机器人定位和运动规划与控制方法[J]. 系统科学与数学, 2015, 35(7): 838–847. YU Ningbo, LI Yuan, XU Chang, et al. Localization and motion planning and control of mobile robots with RGB-D SLAM[J]. Journal of systems science and mathematical sciences, 2015, 35(7): 838–847. [5] 张雪波, 方勇纯, 刘玺. 移动机器人自适应视觉伺服镇定 控制[J]. 控制理论与应用, 2010, 27(9): 1123–1130. ZHANG Xuebo, FANG Yongchun, LIU Xi. Adaptive visual servo regulation of mobile robots[J]. Control theory & applications, 2010, 27(9): 1123–1130. [6] 王可, 贾松敏, 李秀智, 等. 基于地面特征的移动机器人单 目视觉里程计算法[J]. 光学学报, 2015, 35(5): 0515002. WANG Ke, JIA Songmin, LI Xiuzhi, et al. Mobile robot monocular visual odometry algorithm based on ground features[J]. Acta optica sinica, 2015, 35(5): 0515002. [7] 张雪华, 刘华平, 孙富春, 等. 采用 Kinect 的移动机器人目 标跟踪[J]. 智能系统学报, 2014, 9(1): 34–39. ZHANG Xuehua, LIU Huaping, SUN Fuchun, et al. Target tracking of mobile robot using Kinect[J]. CAAI transactions on intelligent systems, 2014, 9(1): 34–39. [8] 雷丽充, 刘华平, 孙富春, 等. 面向灵巧操作的视觉目标识 别[J]. 智能系统学报, 2015, 10(1): 37–42. LEI Lichong, LIU Huaping, SUN Fuchun, et al. Visual object recognition for smart manipulation[J]. CAAI transactions on intelligent systems, 2015, 10(1): 37–42. [9] [10] HENRY P, KRAININ M, HERBST E, et al. RGB-D map- ·450· 智 能 系 统 学 报 第 13 卷
第3期 李元，等：基于RGB-D信息的移动机器人SLAM和路径规划方法研究与实现 ·451· ping:using Kinect-style depth cameras for dense 3D mod- of 2014 IEEE/RSJ International Conference on Intelligent eling of indoor environments[J].The international journal Robots and Systems.Chicago,USA,2014:709-715. of robotics research,2012,31(5):647-663. [21]GRISETTI G,STACHNISS C,BURGARD W.Improved [11]ENDRES F,HESS J,ENGELHARD N,et al.An evalu- techniques for grid mapping with rao-blackwellized ation of the RGB-D SLAM system[C]//Proceedings of particle filters[J].IEEE transactions on robotics,2007, 2012 IEEE International Conference on Robotics and 231):34-46. Automation.Saint Paul,USA,2012:1691-1696 [22]朱毅，张涛，宋靖雁.非完整移动机器人的人工势场法路 [12]BISWAS J,VELOSO M.Depth camera based localization 径规划[).控制理论与应用，2010,27(2)：152-158。 and navigation for indoor mobile robots[C//Proceedings of ZHU Yi,ZHANG Tao,SONG Jingyan.Path planning for the RGB-D Workshop at RSS.San Francisco,CA,USA, nonholonomic mobile robots using artificial potential field 2011. method[J].Control theory and applications,2010,27(2): [13]KLEIN G,MURRAY D.Parallel tracking and mapping for 152-158 small AR workspaces[C]//Proceedings of the 6th IEEE and [23]石为人，黄兴华，周伟.基于改进人工势场法的移动机器 ACM International Symposium on Mixed and Augmented 人路径规划[).计算机应用，2010,30(8)：2021-2023 Reality.Nara,Japan,2007:225-234. SHI Weiren,HUANG Xinghua,ZHOU Wei.Path plan- [14]MUR-ARTAL R,MONTIEL J MM,TARDOS J D.ORB- ning of mobile robot based on improved artificial potential SLAM:a versatile and accurate monocular SLAM field[J].Journal of computer applications,2010,30(8): system[J].IEEE transactions on robotics,2015,31(5): 2021-2023. 1147-1163 [24]SIEGWART R,NOURBAKHSH I R,SCARAMUZZA D [15]MUR-ARTAL R,TARDOS J D.ORB-SLAM2:an open- Introduction to autonomous mobile robots[M].2nd ed. source SLAM system for monocular,stereo,and RGB-D Cambridge,Massachusetts,England:MIT Press,2011. cameras[J].IEEE transactions on robotics,2017,33(5): [25]CHEN Jian,SUN Dong,YANG Jie,et al.Leader-follower 1255-1262 formation control of multiple non-holonomic mobile ro- [16贾松敏，王可，郭兵，等.基于RGB-D相机的移动机器人 三维SLAM).华中科技大学学报：自然科学版，2014， bots incorporating a receding-horizon scheme[J].The inter- 42(1)103-109. national journal of robotics research,2010,29(6):727-747. JIA Songmin,WANG Ke,GUO Bing,et al.Mobile robot 作者简介： 3D SLAM based on RGB-D camera[J].Journal of 李元，男，1990年生，硕士研究 Huazhong university of science and technology:natural 生，主要研究方向为移动服务机器人。 science edition,2014,42(1):103-109. [1刀朱笑笑，曹其新，杨扬，等.一种改进的KinectFusion三维 重构算法[).机器人，2014,36(2)：129-136. ZHU Xiaoxiao,CAO Qixin,YANG Yang,et al.An im- proved KinectFusion 3D reconstruction algorithm[J].Ro- bot,2014.36(2):129-136. 王石荣，男，1993年生，硕士研究 [18]李永锋，张国良，王蜂，等.基于快速视觉里程计和大回 生，主要研究方向为移动服务机器人。 环局部优化模型的改进VSLAM算法[).机器人，2015， 37(5):557-565 LI Yongfeng,ZHANG Guoliang,WANG Feng,et al.Im- proved VSLAM algorithm based on fast visual odometry and large loop local optimization model[J].Robot,2015, 37(5):557-565. 于宁波，男，1981年生，副教授， 博士，主要研究方向为机器人与智能 [19]WIEDEMEYER T.IAI Kinect2[EB/OL].GitHub,Inc, 系统、人与机器人的交互、协作与控 2015.(2015-06-12)[2018-03-08].https://github..com/code 制、智能服务和医疗辅助机器人。 iai/iai kinect2. [20]LU D V,HERSHBERGER D,SMART W D.Layered costmaps for context-sensitive navigation[Cl//Proceedings
ping: using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. The international journal of robotics research, 2012, 31(5): 647–663. ENDRES F, HESS J, ENGELHARD N, et al. An evaluation of the RGB-D SLAM system[C]//Proceedings of 2012 IEEE International Conference on Robotics and Automation. Saint Paul, USA, 2012: 1691–1696. [11] BISWAS J, VELOSO M. Depth camera based localization and navigation for indoor mobile robots[C]//Proceedings of the RGB-D Workshop at RSS. San Francisco, CA, USA, 2011. [12] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[C]//Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Nara, Japan, 2007: 225–234. [13] MUR-ARTAL R, MONTIEL J M M, TARDÓS J D. ORBSLAM: a versatile and accurate monocular SLAM system[J]. IEEE transactions on robotics, 2015, 31(5): 1147–1163. [14] MUR-ARTAL R, TARDÓS J D. ORB-SLAM2: an opensource SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE transactions on robotics, 2017, 33(5): 1255–1262. [15] 贾松敏, 王可, 郭兵, 等. 基于 RGB-D 相机的移动机器人 三维 SLAM[J]. 华中科技大学学报: 自然科学版, 2014, 42(1): 103–109. JIA Songmin, WANG Ke, GUO Bing, et al. Mobile robot 3D SLAM based on RGB-D camera[J]. Journal of Huazhong university of science and technology: natural science edition, 2014, 42(1): 103–109. [16] 朱笑笑, 曹其新, 杨扬, 等. 一种改进的 KinectFusion 三维 重构算法[J]. 机器人, 2014, 36(2): 129–136. ZHU Xiaoxiao, CAO Qixin, YANG Yang, et al. An improved KinectFusion 3D reconstruction algorithm[J]. Robot, 2014, 36(2): 129–136. [17] 李永锋, 张国良, 王蜂, 等. 基于快速视觉里程计和大回 环局部优化模型的改进 VSLAM 算法[J]. 机器人, 2015, 37(5): 557–565. LI Yongfeng, ZHANG Guoliang, WANG Feng, et al. Improved VSLAM algorithm based on fast visual odometry and large loop local optimization model[J]. Robot, 2015, 37(5): 557–565. [18] WIEDEMEYER T. IAI Kinect2[EB/OL]. GitHub, Inc, 2015. (2015-06-12)[2018-03-08] . https://github.com/codeiai/iai_kinect2. [19] LU D V, HERSHBERGER D, SMART W D. Layered costmaps for context-sensitive navigation[C]//Proceedings [20] of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago, USA, 2014: 709–715. GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE transactions on robotics, 2007, 23(1): 34–46. [21] 朱毅, 张涛, 宋靖雁. 非完整移动机器人的人工势场法路 径规划[J]. 控制理论与应用, 2010, 27(2): 152–158. ZHU Yi, ZHANG Tao, SONG Jingyan. Path planning for nonholonomic mobile robots using artificial potential field method[J]. Control theory and applications, 2010, 27(2): 152–158. [22] 石为人, 黄兴华, 周伟. 基于改进人工势场法的移动机器 人路径规划[J]. 计算机应用, 2010, 30(8): 2021–2023. SHI Weiren, HUANG Xinghua, ZHOU Wei. Path planning of mobile robot based on improved artificial potential field[J]. Journal of computer applications, 2010, 30(8): 2021–2023. [23] SIEGWART R, NOURBAKHSH I R, SCARAMUZZA D. Introduction to autonomous mobile robots[M]. 2nd ed. Cambridge, Massachusetts, England: MIT Press, 2011. [24] CHEN Jian, SUN Dong, YANG Jie, et al. Leader-follower formation control of multiple non-holonomic mobile robots incorporating a receding-horizon scheme[J]. The international journal of robotics research, 2010, 29(6): 727–747. [25] 作者简介： 李元，男，1990 年生，硕士研究 生，主要研究方向为移动服务机器人。 王石荣，男，1993 年生，硕士研究 生，主要研究方向为移动服务机器人。 于宁波，男，1981 年生，副教授， 博士，主要研究方向为机器人与智能 系统、人与机器人的交互、协作与控 制、智能服务和医疗辅助机器人。 第 3 期 李元，等：基于 RGB-D 信息的移动机器人 SLAM 和路径规划方法研究与实现 ·451·