【智能系统】基于RGB-D信息的移动机器人SLAM和路径规划方法研究与实现

第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 trans￾actions 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 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 近年来，智能移动机器人技术得到迅速发展， 已广泛应用于工业、军事、物流、办公和家庭服务等 领域[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 获取的环境 RGB￾D 信息，利用 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 ad￾justment)，最后进行闭环检测。由此，实现了移动机 器人的自主实时定位与特征地图创建。 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 点云用于地图构建。由于 GMap￾ping 栅格地图构建的方式实时性较好，在本平台上 能达到最少 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 experiment￾al paths (full lines) X Y 图 6 为 8 次实验中移动机器人到达期望位姿的 位置和姿态误差，实心圆点代表位置误差，箭头方 向代表姿态误差。移动机器人距离目标点 轴方向 的平均距离偏差为 0.43 cm， 轴方向的平均距离偏 差为 1.12 cm，总体距离误差都在 3cm 以内。姿态 角偏差的绝对平均值为 2.16°。实验表明，采用 RGB￾D 信息和 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 ser￾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. [1] HABIB M K, 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. [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-local￾ization 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 sci￾ences, 2015, 35(7): 838–847. [5] 张雪波, 方勇纯, 刘玺. 移动机器人自适应视觉伺服镇定 控制[J]. 控制理论与应用, 2010, 27(9): 1123–1130. ZHANG Xuebo, FANG Yongchun, LIU Xi. Adaptive visu￾al servo regulation of mobile robots[J]. Control theory & ap￾plications, 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 fea￾tures[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 ob￾ject recognition for smart manipulation[J]. CAAI transac￾tions 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 mod￾eling of indoor environments[J]. The international journal of robotics research, 2012, 31(5): 647–663. ENDRES F, HESS J, ENGELHARD N, et al. An evalu￾ation 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. ORB￾SLAM: 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 open￾source 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 im￾proved KinectFusion 3D reconstruction algorithm[J]. Ro￾bot, 2014, 36(2): 129–136. [17] 李永锋, 张国良, 王蜂, 等. 基于快速视觉里程计和大回 环局部优化模型的改进 VSLAM 算法[J]. 机器人, 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. [18] WIEDEMEYER T. IAI Kinect2[EB/OL]. GitHub, Inc, 2015. (2015-06-12)[2018-03-08] . https://github.com/code￾iai/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 plan￾ning 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 ro￾bots incorporating a receding-horizon scheme[J]. The inter￾national journal of robotics research, 2010, 29(6): 727–747. [25] 作者简介： 李元，男，1990 年生，硕士研究 生，主要研究方向为移动服务机器人。 王石荣，男，1993 年生，硕士研究 生，主要研究方向为移动服务机器人。 于宁波，男，1981 年生，副教授， 博士，主要研究方向为机器人与智能 系统、人与机器人的交互、协作与控 制、智能服务和医疗辅助机器人。 第 3 期 李元，等：基于 RGB-D 信息的移动机器人 SLAM 和路径规划方法研究与实现 ·451·