第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 (3) 0 …s p(g,gobs)Po) V 1 Sm 0,其他 (13) 式中：n为斥力比例因子，p(q,qs)为机器人q与障碍 为了能够实现移动机器人的动态路径规划，在 物qs之间的最小距离，P为障碍物的影响范围。进 控制过程中采取分段规划的策略。因此，路径曲线 而斥力函数可表示为 还需满足起始角度和终止角度约束： y(s) Frep(q)= Frepi+Frep2,p(g.qots)<Po (4) x(s)sin- os0=0 (14) 0,p(q,qots)>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·