一种基于ros的移动机器人室内环境探索系统与控制方法
一种基于ros的移动机器人室内环境探索系统与控制方法
【技术领域】
[0001] 本发明涉及的是一种有关移动机器人的控制方法，具体基于R0S (机器人操作系 统，Robot Operating System)，探索室内环境的控制方法。
【背景技术】
[0002] 近年来，机器人领域发展迅速，例如灾难探测机器人、家用服务机器人、陪护机器 人、军用机器人等均得到广泛的应用。移动机器人作为一种高度自主的智能体，具有灵活的 运动能力和强大的观察设备，利用机器人的建图功能代替人员深入危险的未知环境进行探 索，已经成为救援工作的有效手段。尽管目前的各种救援机器人可以完成各种各样的任务， 但是救援机器人大部分依靠遥控操作，无法脱离人为控制在未知的复杂空间中移动。而在 灾难环境中，坍塌、烟尘充斥的环境空间是不可预知的，同时操作人员通过摄像机对灾难现 场的探索识别能力并不强，而且很大程度上受操作人员经验的影响。亟需一种可以对灾难 环境进行自主探索的智能移动机器人。
[0003] 同时，机器人也朝着复杂化、通用化以及工作环境不确定化发展，对代码的复用 性、模块化提出了较高的要求。现有的Player、MOOS、CARMEN、YARP、Orocos、微软Robotics Studio等操作系统无法满足实际应用的需求。ROS (Robot Operating System)是一种开 源机器人操作系统，能够提供类似于操作系统的功能，为机器人应用系统提供硬件抽象、底 层驱动、消息传递和包管理，以及一些辅助开发工具，例如建立、编写和运行多机通信系统 整合的程序。R0S操作系统的设计目标就是为了提高代码的复用性，所以采用一种分布式的 进程架构，使得程序具有高度的独立性和低耦合性。
[0004] 考虑上述因素，本发明引入R0S机器人操作系统设计和构建了一种移动机器人平 台，硬件采用了 iRobot差分驱动底盘以及信息采集单元、数据处理单元、运动控制单元和 远程监控单元4个模块。利用R0S分布式的进程框架优势，控制机器人更加方便快速地探 索室内环境，得到室内环境信息的二维地图，达到更加有效地进行机器人灾难救援。
【发明内容】
[0005] 本发明设计了基于R0S的移动机器人室内环境探索系统与控制方法。
[0006] 本发明采用的技术方案为：第一阶段，机器人的改装与基于R0S的程序配置；第二 阶段，激光雷达的环境信息获取与地图拼接匹配；第三阶段，构建机器人与上位机间的无线 通信；第四阶段，通过设置路径规划算法，使机器人自主行驶，并探索室内未知环境。
[0007] 所述的一种基于R0S的移动机器人室内环境探索系统与控制方法，通过以下步骤 实现。
[0008] 步骤一：以美国iRobot公司生产的iRobot Create为移动机器人平台，进行改装 与拓展。
[0009] 步骤二：采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置 TF坐标变换系统、Costmap占有栅格地图来完成导航栈的基本配置。
[0010] 步骤三：通过激光雷达获取地图信息，将激光雷达数据离散化。
[0011] 步骤四：将获取的激光雷达信息进行扫描匹配。
[0012] 步骤五：利用Zigbee协议构建机器人与上位机间的无线通讯，实现上位机对机器 人状态获取与对激光雷达获取的二维图进行实时监控。
[0013] 步骤六：对机器人进行同步定位与建图。
[0014] 步骤七：设置机器人的路径规划算法。
[0015] 步骤八：机器人进行自主行驶，同时探索室内未知环境。
[0016] 下面对各步骤进行具体说明。
[0017] 所述步骤一的iRobot Create移动机器人平台，保留了运动控制器和电源管理系 统，去掉了吸尘功能，并扩展了传感器。所构建的移动机器人系统由四个部分组成，包括信 息采集单元、数据处理单元、运动控制单元和远程监控单元。其系统组成框图如图1所示。 其中，控制核心为基于ATOM凌动N270中央处理器的工控机。
[0018] 所述步骤二的R0S的导航栈的基本思想为：得到里程计和激光雷达数据，同时输 出速度命令给机器人底盘，导航栈配置如图2所示。TF坐标变换系统如图3所示，TF系统 定义两个坐标系：一个坐标系的原点位于机器人的底盘中心，另一个坐标系位于激光雷达 的中心。将位于底盘的坐标系定义为base_link，因为导航栈需要把该TF坐标系放置为移 动机器人的旋转中心。将位于激光雷达的坐标系定义为base_laser。Costmap占有栅格 地图通过完成激光雷达数据离散化处理获得。
[0019] 所述步骤三的在地图获取阶段，首先定义单元栅格的大小。该尺度将确定离散化 的程度。离散化的基本思想为：从机器人当前所在坐标开始，以单元栅格的尺寸向四周扩 散，使得平面内铺满虚拟栅格，然后将激光雷达测距前端落到相应的栅格内，由栅格的占有 概率值来代表所有落到该栅格内的测距前端数据。该算法通过双线性滤波实现。
[0020] 所述步骤四的完成激光雷达数据离散化后，也就得到了局部占有栅格地图。然后 通过扫面匹配阶段对激光雷达数据进行扫描匹配，也就是通过局部地图对移动机器人位姿 进行估计，将局部环境地图融合为全局环境地图。
[0021] 所述步骤五的ZigBee通信协议为上位机和车和机器人之间的通信。包括机器人 的具体行进方向、运行路线等；包括机器人速度设定、启动/停止；包括获取的激光雷达数 据信息形成的环境二维地图。
[0022] 所述步骤六的同步定位与建图为基于Hector SLAM的自主建图算法。Hector为 R0S机器人操作系统的开源导航和建图库，其系统构架如图4所示。
[0023] 所述步骤七的路径规划算法为Di jkstra算法，Di jkstra为改进的广度优先搜索 算法，Di jkstra算法的流程图如图5所示。
[0024] 所述步骤八的探索算法结合了现在常用的两种策略。一种是向离机器人最近的前 端移动可以进行快速的地图获取，扩展未知区域。另一种同时考虑现有地图中前端的大小， 前段较长的地方，这样通过这里能够扩展较大的未知区域。本专利对两个策略取权重进行 加权。
[0025] 本发明的有益效果：移动机器人室内环境探索系统与控制方法基于R0S系统，利 用R0S提高了代码的复用性的优点，可快速遍历整个室内未知区域并进行探索。将未知区 域的二维环境图扫描构建出来并通过ZigBee无线协议传至上位机，实现了整个未知环境 区域的探测与监控。
【附图说明】
[0026] 图1是移动机器人系统构架图。
[0027] 图2是R0S的导航栈系统构架图。
[0028] 图3是TF坐标变换系统图。
[0029] 图4是移动机器人基本平台TF模型图。
[0030] 图5是格地图的双线性滤波。
[0031]图6是占有栅格地图离散化。
[0032] 图7是Hector SLAM系统构架图。
[0033] 图8是Hector mapping系统架构图。
[0034] 图9是Di jkstra算法流程图。
[0035] 图10是Di jkstra算法示例图。
【具体实施方式】
[0036] 下面结合附图对本发明进一步说明。
[0037] 基于R0S的移动机器人室内环境探索系统与控制方法由软、硬件两部分构成。
[0038] 硬件部分包括：iRobot差分驱动底盘以及信息采集单元、数据处理单元、运动控 制单元和远程监控单元4个模块。软件部分包括：R0S导航栈、Hector SLAM的自主建图算 法、上位机界面和程序、路径规划算法。
[0039] 具体实施步骤如下。
[0040] 步骤一：以美国iRobot公司生产的iRobot Create为移动机器人平台，进行改装 与拓展。
[0041] iRobot Create移动机器人平台最大的特点是可以进行编程，根据官方提供的通 信协议，可以控制Create底盘的运动和码盘数据的读取，并且该机器人底盘具有运行稳 定、续航时间长等特点。本专利在原有机器人的基础上进行改装，构建适应所需功能的移动 机器人，由四个部分组成，包括信息采集单元、数据处理单元、运动控制单元和远程监控单 元。数据采集单元主要为激光雷达，系统预留相关接口。运动控制单元中，Create机器人 底盘内建光电编码器，用于记录系统轮子所转过的角度，从而推算机器人的位姿。
[0042] 步骤二：采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置 TF坐标变换系统、Costmap占有栅格地图来完成导航栈的基本配置。
[0043] 栈为功能包的集合，同时栈也是R0S软件发布的主要形式。栈有Manifest文件， 其名字为Stack, xml，用来标识版本信息和功能包集的依赖信息。R0S导航栈需要使用TF 坐标变换系统来发布机器人的坐标变换树。而坐标变化树即系统中不同坐标系之间的偏 移。如图3所示，移动机器人平台拥有移动的底盘和位于底盘上方的激光雷达。在该机器 人上，TF系统定义两个坐标系：一个坐标系的原点位于机器人的底盘中心，另一个坐标系 位于激光雷达的中心。将位于底盘的坐标系定义为base_link，因为导航栈需要把该TF坐 标系放置为移动机器人的旋转中心。将位于激光雷达的坐标系定义为base_laser。本系统 的iRobot Create底盘所使用的TF变换由箭头标识。
[0044] 移动机器人得到从激光雷达采集的距离信息无法直接进行SLAM操作，因为这些 数据是基于激光雷达中心来表示的，即这些数据在basejaser坐标系下。为了能够使用这 些数据，需要进行数据的坐标变换，即从base_laser变换到base_link下。如图3所示的基 本模型中，已知激光雷达测得位于在其正前方0. 3m处有障碍物，而激光雷达安装于底盘前 方0. lm，上方0. 2m处。从而可以获取从base_link到basejaser的坐标变换关系，base_ link坐标系平移（X: 0.1m, y: 0.0m, z: 0.2m)。这样就可以得到障碍物到机器人底盘的 距离。如图4所示，假设base_link为父节点，因为所有 传感器都是基于底盘安装的。因 此，base_link 和 basejaser 之间的变换矩阵为（X: 0.1m, y: 0.0m, z: 0.2m)。设置完 TF系统后，需要创建用于发布变换信息的节点，然后创建收听变换信息的节点。ROS的TF 系统都可以很方便地进行管理。
[0045] 步骤三：通过激光雷达获取地图信息，将激光雷达数据离散化。
[0046] 地图获取阶段实际上就是激光雷达数据对环境进行描述，这个阶段也是将连续的 环境距离信息进行离散化的阶段。然后将这些离散的数据进行匹配，将局部环境地图融合 为全局环境地图。
[0047] 在地图获取阶段，首先就要定义单元栅格的大小。单元栅格越小，离散化程度越 低；相反，单元栅格越大，离散程度越高。离散化的基本思想为：从机器人当前所在坐标开 始，以单元栅格的尺寸向四周扩散，使得平面内铺满虚拟栅格，然后将激光雷达测距前端落 到相应的栅格内，由栅格的占有概率值来代表所有落到该栅格内的测距前端数据。该算法 通过双线性滤波实现，如图5所示，图中点P"是待插值的栅格地图某点。因为栅格地图其 自身离散的性质限制了地图所能达到的精度，所以要求先通过插值计算才能得到其地图数 据，即将亚栅格单元的点通过双线性插值转化到栅格地图。图6为占有栅格地图离散化的 效果。
[0048] 给定一个连续地图上的某点坐标Pm，占有值为M (Pm)，相应的梯度为：
可以使用四个最近的坐标PQQ.n近似：
[0049] 偏导数可以近似为：
[0050] 步骤四：将获取的激光雷达信息进行扫描匹配。
[0051] 完成激光雷达数据离散化后，得到了局部占有栅格地图。然后通过扫描匹配阶段 对激光雷达数据进行扫描匹配，也就是通过局部地图对移动机器人位姿进行估计。该算法 可以不使用里程计而进行建图，使算法可以应用于操作人员手持建图设备或者是使里程计 发生较大误差的不平坦环境中。
[0052] 求移动机器人位
需使得下式最小，也就是说要得到能够使激 光扫描最佳的校准：
[0053] 这里，SJ ζ )是激光雷达扫描端点SfGm，5"广在全局坐标系中的坐标。它们是 机器人全局坐标系位姿ζ的函数：
[0054] 方程ΜΒ^ζ))将返回坐标SiU)处的地图值。给定某个ζ的起始估计，估计 Α ζ使得测量误差最小：
[0055] 算法接下来对上式进行处理。首先对ΜΒ^ζ+Δ ζ))进行一阶泰勒展开得到：
[0056] 该式可以通过对Δ ζ求偏导得到：
[0057] 此时，求△ ζ的问题转换成为高斯牛顿方法求极小值的问题：
[0058] 其中，Η为海森矩阵：
[0059] ▽ M(Sje))为占有栅格地图的偏导：
[0060] 步骤五：利用Zigbee协议构建机器人与上位机间的无线通讯，实现上位机对机器 人状态获取与对激光雷达获取的二维图进行实时监控。
[0061] 在同一路由器下，配置操作员端的主机和移动机器人的从机域名解析，从而完成 机器人和上位机通信的配置。此时，两台PC机进行数据数据，只需要订阅同一个话题即可。 通过该无线传感器，构建从操作员端电脑到移动机器人端电脑的控制线路网络。操作员可 以发送位姿指令，移动机器人接收到通过无线网络传来的数据，然后在ROS的导航栈框架 下进行位姿跟踪。
[0062] 步骤六：对机器人进行同步定位与建图。
[0063] Hector为R0S机器人操作系统的开源导航和建图库，如图7所示。它主要用于城 市搜索和救援任务的移动机器人系统中，给出了基于R0S的程序架构与相应组件。Hector 各个模块可以进行方便的独立开发和整合，满足不同配置移动机器人和特定救援场合的使 用。其中最主要的组件为导航和建图组件（he Ct〇r_slam)，构建环境地图不仅能够将机器 人所处的环境信息给操作人员，而且能够给导航库上层的路径规划组件提供可行区域和定 位信息，hector_slam只负责进行2D建图。hector_slam的组件包括用于执行构图算法的 hector_mapping，为其他组件提供地图数据服务的hector_map_server，将地图信息转化为 图片信息的 hector_geotiff 和探索算法生成轨迹的 hector_trajectory_server。hector_ slam仅依靠激光雷达数据进行建图与定位，在道路特别崎岖的时候，还可以使用惯性导航 对SLAM算法进行补偿，提供更高的定位精度。另外，如图8所示，通过hector_elevation_ mapping，系统可以融合微软的Kinect深度摄像头，构建2. f5D地图信息。Kinect得到的点 云数据为2D地图中相应的栅格提供了高度信息及其方差。最后，hector_costmap可以融 合2. ?高度地图与2D的占有栅格地图构建代价地图。
[0064] 步骤七：设置机器人的路径规划算法。
[0065] 机器人路径规划算法使用Dijkstra算法。具体为对一个有权重的图G进行操作， 起点为S，V代表G中所有的定点的集合，E表示G中所有的边的集合。（u，v)表示从顶点u 到顶点v的路径，边的权重为w:E - [0，°° ]，则w(u, v)是从顶点u到顶点v的非负代价， 即两顶点之前的距离。一条路径的代价值等于从路径起点到终点所经过的边的代价值的 总和。Dijkstra算法可以找到图G中任意两个顶点如s、t的最低代价路径，即最短路径。 Dijkstra算法的流程图如图9所示。图10给出了 Dijkstra算法的示例。算法具体步骤如 下。
[0066] St印1 :如图10所示，算法开始时，开启列表中只有起点S，0= {S}，且nbest=S，对S节 点的相邻节点进行遍历，并且将S加入关闭列表，则，0= {2, 4, 1，5}，C={S}。相应的从起点到 当前点的代价函数分别为，f (2) =1，f (4) =1，f(l) =2, f (5) =4。代价函数最小的点位2和4， 任取其一，令nbest=2,而且此时反向指针均指向起点S。
[0067] Step2 :对节点2的相邻节点进行遍历，此时0={4, 1，5}，C={S, 2}，而节点2的相邻 节点只有S，而且S已经在关闭列表中，算法继续向下进行。
[0068] Step3 :对当前0={4, 1，5}中的代价函数最小的节点4进行相邻节点遍历，nbest=4 ; 同样，将4从开启列表移到关闭列表，0={1，5}，C={S，2,4}，4的相邻节点有S、3、5,其中 S已经在关闭列表，不对其进行处理，而在开启列表中加入3。由于开启列表中有5,而且 f (4)+c (4, 5)=1+2=3小于前面f (5) =4,需要更新开启列表中5的代价函数，则f (5) =3,且5 的反向指针指向4。
[0069] 3七印4:此时开启列表和关闭列表分别为0={5,3}，0={5,2,4}，11^=5。对节点5进 行处理，5的相邻节点有3个，其中4和S已经在关闭列表中，不做处理，只剩终点G，因此把 终点G放入开启列表，0={G，3}，C={S，2, 4, 5}。
[0070] St印5 :当前代价函数最小的节点即为nbest=G，f(G)=f(5)+c(5，G)=5。将相应节点 的反向指针取出，也就找到了一条从起点S到终点G的最短路径S - 4 - 5 - G。
[0071] 步骤八：机器人进行自主行驶，同时探索室内未知环境。
[0072] 基于Hector SLAM的环境地图构建的基础上，机器人进行自主行驶建图。移动机 器人在探索未知区域的时候，需要进行环境的建图，同时也需要设置建图规则，使得在相对 较短的时间内能够得到最大的环境地图。
[0073] 本系统所使用的探索算法结合了现在常用的两种策略。向离机器人最近的前端移 动可以进行快速的地图获取，扩展未知区域。同时考虑现有地图中前端的大小。前端较长 的地方，往往意味着通过这里能够扩展较大的未知区域。对两个策略去权重进行加权。移 动机器人取其中能够最快得到最大地图的原则进行地图构建，直到将所有的探索前端探索 完毕。
【主权项】
1. 一种基于ROS(机器人操作系统，RobotOperatingSystem)的移动机器人室内环 境探索系统与控制方法，包括上位机、改装的iRobot机器人、UTM30LX激光雷达；通过采用 R0S的导航栈与基于HectorSLAM的自主建图算法，实现移动机器人对室内环境的自主探 索并生成环境二维地图。2. 权利要求1所述的一种基于R0S的移动机器人室内环境探索系统与控制方法，其特 征在于：包含如下步骤： 步骤1、以美国iRobot公司生产的iRobotCreate为移动机器人平台，进行改装与拓 展； 步骤2、采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置TF坐 标变换系统、Costmap占有栅格地图来完成导航栈的基本配置； 步骤3、通过激光雷达获取地图信息并进行图像信息的处理匹配； 步骤4、上位机对机器人状态获取与对激光雷达获取的二维环境地图信息进行实时监 控； 步骤5、利用R0S内的Hector库对机器人进行同步定位与建图； 步骤6、设置机器人的Dijkstra路径规划算法； 步骤7、机器人进行自主行驶，同时探索室内未知环境。
【专利摘要】本发明公开了一种基于ROS（机器人操作系统，Robot？Operating？System）的移动机器人室内环境探索系统与控制方法。该系统基于ROS，能够实现机器人对室内环境的自主探索与定位。该系统主要采用iRobot差分驱动底盘和UTM？30LX激光雷达。上位机可实时得到机器人的位置定位信息与室内环境探索路径轨迹，并由激光雷达的数据匹配得到由局部地图连接形成的全局地图。本发明提供了更加智能化的未知环境探索系统，为灾难环境救援提供了极大的便利与安全保障，实现了室内环境感知的信息交互，具有很强的便捷性和实用性。
【IPC分类】G05D1/02
【公开号】CN105487535
【申请号】CN201410524184
【发明人】石成玉, 吴成东, 胡美玉, 陈建辉
【申请人】东北大学
【公开日】2016年4月13日
【申请日】2014年10月9日
【技术领域】
[0001] 本发明涉及的是一种有关移动机器人的控制方法，具体基于R0S (机器人操作系 统，Robot Operating System)，探索室内环境的控制方法。
【背景技术】
[0002] 近年来，机器人领域发展迅速，例如灾难探测机器人、家用服务机器人、陪护机器 人、军用机器人等均得到广泛的应用。移动机器人作为一种高度自主的智能体，具有灵活的 运动能力和强大的观察设备，利用机器人的建图功能代替人员深入危险的未知环境进行探 索，已经成为救援工作的有效手段。尽管目前的各种救援机器人可以完成各种各样的任务， 但是救援机器人大部分依靠遥控操作，无法脱离人为控制在未知的复杂空间中移动。而在 灾难环境中，坍塌、烟尘充斥的环境空间是不可预知的，同时操作人员通过摄像机对灾难现 场的探索识别能力并不强，而且很大程度上受操作人员经验的影响。亟需一种可以对灾难 环境进行自主探索的智能移动机器人。
[0003] 同时，机器人也朝着复杂化、通用化以及工作环境不确定化发展，对代码的复用 性、模块化提出了较高的要求。现有的Player、MOOS、CARMEN、YARP、Orocos、微软Robotics Studio等操作系统无法满足实际应用的需求。ROS (Robot Operating System)是一种开 源机器人操作系统，能够提供类似于操作系统的功能，为机器人应用系统提供硬件抽象、底 层驱动、消息传递和包管理，以及一些辅助开发工具，例如建立、编写和运行多机通信系统 整合的程序。R0S操作系统的设计目标就是为了提高代码的复用性，所以采用一种分布式的 进程架构，使得程序具有高度的独立性和低耦合性。
[0004] 考虑上述因素，本发明引入R0S机器人操作系统设计和构建了一种移动机器人平 台，硬件采用了 iRobot差分驱动底盘以及信息采集单元、数据处理单元、运动控制单元和 远程监控单元4个模块。利用R0S分布式的进程框架优势，控制机器人更加方便快速地探 索室内环境，得到室内环境信息的二维地图，达到更加有效地进行机器人灾难救援。
【发明内容】
[0005] 本发明设计了基于R0S的移动机器人室内环境探索系统与控制方法。
[0006] 本发明采用的技术方案为：第一阶段，机器人的改装与基于R0S的程序配置；第二 阶段，激光雷达的环境信息获取与地图拼接匹配；第三阶段，构建机器人与上位机间的无线 通信；第四阶段，通过设置路径规划算法，使机器人自主行驶，并探索室内未知环境。
[0007] 所述的一种基于R0S的移动机器人室内环境探索系统与控制方法，通过以下步骤 实现。
[0008] 步骤一：以美国iRobot公司生产的iRobot Create为移动机器人平台，进行改装 与拓展。
[0009] 步骤二：采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置 TF坐标变换系统、Costmap占有栅格地图来完成导航栈的基本配置。
[0010] 步骤三：通过激光雷达获取地图信息，将激光雷达数据离散化。
[0011] 步骤四：将获取的激光雷达信息进行扫描匹配。
[0012] 步骤五：利用Zigbee协议构建机器人与上位机间的无线通讯，实现上位机对机器 人状态获取与对激光雷达获取的二维图进行实时监控。
[0013] 步骤六：对机器人进行同步定位与建图。
[0014] 步骤七：设置机器人的路径规划算法。
[0015] 步骤八：机器人进行自主行驶，同时探索室内未知环境。
[0016] 下面对各步骤进行具体说明。
[0017] 所述步骤一的iRobot Create移动机器人平台，保留了运动控制器和电源管理系 统，去掉了吸尘功能，并扩展了传感器。所构建的移动机器人系统由四个部分组成，包括信 息采集单元、数据处理单元、运动控制单元和远程监控单元。其系统组成框图如图1所示。 其中，控制核心为基于ATOM凌动N270中央处理器的工控机。
[0018] 所述步骤二的R0S的导航栈的基本思想为：得到里程计和激光雷达数据，同时输 出速度命令给机器人底盘，导航栈配置如图2所示。TF坐标变换系统如图3所示，TF系统 定义两个坐标系：一个坐标系的原点位于机器人的底盘中心，另一个坐标系位于激光雷达 的中心。将位于底盘的坐标系定义为base_link，因为导航栈需要把该TF坐标系放置为移 动机器人的旋转中心。将位于激光雷达的坐标系定义为base_laser。Costmap占有栅格 地图通过完成激光雷达数据离散化处理获得。
[0019] 所述步骤三的在地图获取阶段，首先定义单元栅格的大小。该尺度将确定离散化 的程度。离散化的基本思想为：从机器人当前所在坐标开始，以单元栅格的尺寸向四周扩 散，使得平面内铺满虚拟栅格，然后将激光雷达测距前端落到相应的栅格内，由栅格的占有 概率值来代表所有落到该栅格内的测距前端数据。该算法通过双线性滤波实现。
[0020] 所述步骤四的完成激光雷达数据离散化后，也就得到了局部占有栅格地图。然后 通过扫面匹配阶段对激光雷达数据进行扫描匹配，也就是通过局部地图对移动机器人位姿 进行估计，将局部环境地图融合为全局环境地图。
[0021] 所述步骤五的ZigBee通信协议为上位机和车和机器人之间的通信。包括机器人 的具体行进方向、运行路线等；包括机器人速度设定、启动/停止；包括获取的激光雷达数 据信息形成的环境二维地图。
[0022] 所述步骤六的同步定位与建图为基于Hector SLAM的自主建图算法。Hector为 R0S机器人操作系统的开源导航和建图库，其系统构架如图4所示。
[0023] 所述步骤七的路径规划算法为Di jkstra算法，Di jkstra为改进的广度优先搜索 算法，Di jkstra算法的流程图如图5所示。
[0024] 所述步骤八的探索算法结合了现在常用的两种策略。一种是向离机器人最近的前 端移动可以进行快速的地图获取，扩展未知区域。另一种同时考虑现有地图中前端的大小， 前段较长的地方，这样通过这里能够扩展较大的未知区域。本专利对两个策略取权重进行 加权。
[0025] 本发明的有益效果：移动机器人室内环境探索系统与控制方法基于R0S系统，利 用R0S提高了代码的复用性的优点，可快速遍历整个室内未知区域并进行探索。将未知区 域的二维环境图扫描构建出来并通过ZigBee无线协议传至上位机，实现了整个未知环境 区域的探测与监控。
【附图说明】
[0026] 图1是移动机器人系统构架图。
[0027] 图2是R0S的导航栈系统构架图。
[0028] 图3是TF坐标变换系统图。
[0029] 图4是移动机器人基本平台TF模型图。
[0030] 图5是格地图的双线性滤波。
[0031]图6是占有栅格地图离散化。
[0032] 图7是Hector SLAM系统构架图。
[0033] 图8是Hector mapping系统架构图。
[0034] 图9是Di jkstra算法流程图。
[0035] 图10是Di jkstra算法示例图。
【具体实施方式】
[0036] 下面结合附图对本发明进一步说明。
[0037] 基于R0S的移动机器人室内环境探索系统与控制方法由软、硬件两部分构成。
[0038] 硬件部分包括：iRobot差分驱动底盘以及信息采集单元、数据处理单元、运动控 制单元和远程监控单元4个模块。软件部分包括：R0S导航栈、Hector SLAM的自主建图算 法、上位机界面和程序、路径规划算法。
[0039] 具体实施步骤如下。
[0040] 步骤一：以美国iRobot公司生产的iRobot Create为移动机器人平台，进行改装 与拓展。
[0041] iRobot Create移动机器人平台最大的特点是可以进行编程，根据官方提供的通 信协议，可以控制Create底盘的运动和码盘数据的读取，并且该机器人底盘具有运行稳 定、续航时间长等特点。本专利在原有机器人的基础上进行改装，构建适应所需功能的移动 机器人，由四个部分组成，包括信息采集单元、数据处理单元、运动控制单元和远程监控单 元。数据采集单元主要为激光雷达，系统预留相关接口。运动控制单元中，Create机器人 底盘内建光电编码器，用于记录系统轮子所转过的角度，从而推算机器人的位姿。
[0042] 步骤二：采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置 TF坐标变换系统、Costmap占有栅格地图来完成导航栈的基本配置。
[0043] 栈为功能包的集合，同时栈也是R0S软件发布的主要形式。栈有Manifest文件， 其名字为Stack, xml，用来标识版本信息和功能包集的依赖信息。R0S导航栈需要使用TF 坐标变换系统来发布机器人的坐标变换树。而坐标变化树即系统中不同坐标系之间的偏 移。如图3所示，移动机器人平台拥有移动的底盘和位于底盘上方的激光雷达。在该机器 人上，TF系统定义两个坐标系：一个坐标系的原点位于机器人的底盘中心，另一个坐标系 位于激光雷达的中心。将位于底盘的坐标系定义为base_link，因为导航栈需要把该TF坐 标系放置为移动机器人的旋转中心。将位于激光雷达的坐标系定义为base_laser。本系统 的iRobot Create底盘所使用的TF变换由箭头标识。
[0044] 移动机器人得到从激光雷达采集的距离信息无法直接进行SLAM操作，因为这些 数据是基于激光雷达中心来表示的，即这些数据在basejaser坐标系下。为了能够使用这 些数据，需要进行数据的坐标变换，即从base_laser变换到base_link下。如图3所示的基 本模型中，已知激光雷达测得位于在其正前方0. 3m处有障碍物，而激光雷达安装于底盘前 方0. lm，上方0. 2m处。从而可以获取从base_link到basejaser的坐标变换关系，base_ link坐标系平移（X: 0.1m, y: 0.0m, z: 0.2m)。这样就可以得到障碍物到机器人底盘的 距离。如图4所示，假设base_link为父节点，因为所有 传感器都是基于底盘安装的。因 此，base_link 和 basejaser 之间的变换矩阵为（X: 0.1m, y: 0.0m, z: 0.2m)。设置完 TF系统后，需要创建用于发布变换信息的节点，然后创建收听变换信息的节点。ROS的TF 系统都可以很方便地进行管理。
[0045] 步骤三：通过激光雷达获取地图信息，将激光雷达数据离散化。
[0046] 地图获取阶段实际上就是激光雷达数据对环境进行描述，这个阶段也是将连续的 环境距离信息进行离散化的阶段。然后将这些离散的数据进行匹配，将局部环境地图融合 为全局环境地图。
[0047] 在地图获取阶段，首先就要定义单元栅格的大小。单元栅格越小，离散化程度越 低；相反，单元栅格越大，离散程度越高。离散化的基本思想为：从机器人当前所在坐标开 始，以单元栅格的尺寸向四周扩散，使得平面内铺满虚拟栅格，然后将激光雷达测距前端落 到相应的栅格内，由栅格的占有概率值来代表所有落到该栅格内的测距前端数据。该算法 通过双线性滤波实现，如图5所示，图中点P"是待插值的栅格地图某点。因为栅格地图其 自身离散的性质限制了地图所能达到的精度，所以要求先通过插值计算才能得到其地图数 据，即将亚栅格单元的点通过双线性插值转化到栅格地图。图6为占有栅格地图离散化的 效果。
[0048] 给定一个连续地图上的某点坐标Pm，占有值为M (Pm)，相应的梯度为：
可以使用四个最近的坐标PQQ.n近似：
[0049] 偏导数可以近似为：
[0050] 步骤四：将获取的激光雷达信息进行扫描匹配。
[0051] 完成激光雷达数据离散化后，得到了局部占有栅格地图。然后通过扫描匹配阶段 对激光雷达数据进行扫描匹配，也就是通过局部地图对移动机器人位姿进行估计。该算法 可以不使用里程计而进行建图，使算法可以应用于操作人员手持建图设备或者是使里程计 发生较大误差的不平坦环境中。
[0052] 求移动机器人位
需使得下式最小，也就是说要得到能够使激 光扫描最佳的校准：
[0053] 这里，SJ ζ )是激光雷达扫描端点SfGm，5"广在全局坐标系中的坐标。它们是 机器人全局坐标系位姿ζ的函数：
[0054] 方程ΜΒ^ζ))将返回坐标SiU)处的地图值。给定某个ζ的起始估计，估计 Α ζ使得测量误差最小：
[0055] 算法接下来对上式进行处理。首先对ΜΒ^ζ+Δ ζ))进行一阶泰勒展开得到：
[0056] 该式可以通过对Δ ζ求偏导得到：
[0057] 此时，求△ ζ的问题转换成为高斯牛顿方法求极小值的问题：
[0058] 其中，Η为海森矩阵：
[0059] ▽ M(Sje))为占有栅格地图的偏导：
[0060] 步骤五：利用Zigbee协议构建机器人与上位机间的无线通讯，实现上位机对机器 人状态获取与对激光雷达获取的二维图进行实时监控。
[0061] 在同一路由器下，配置操作员端的主机和移动机器人的从机域名解析，从而完成 机器人和上位机通信的配置。此时，两台PC机进行数据数据，只需要订阅同一个话题即可。 通过该无线传感器，构建从操作员端电脑到移动机器人端电脑的控制线路网络。操作员可 以发送位姿指令，移动机器人接收到通过无线网络传来的数据，然后在ROS的导航栈框架 下进行位姿跟踪。
[0062] 步骤六：对机器人进行同步定位与建图。
[0063] Hector为R0S机器人操作系统的开源导航和建图库，如图7所示。它主要用于城 市搜索和救援任务的移动机器人系统中，给出了基于R0S的程序架构与相应组件。Hector 各个模块可以进行方便的独立开发和整合，满足不同配置移动机器人和特定救援场合的使 用。其中最主要的组件为导航和建图组件（he Ct〇r_slam)，构建环境地图不仅能够将机器 人所处的环境信息给操作人员，而且能够给导航库上层的路径规划组件提供可行区域和定 位信息，hector_slam只负责进行2D建图。hector_slam的组件包括用于执行构图算法的 hector_mapping，为其他组件提供地图数据服务的hector_map_server，将地图信息转化为 图片信息的 hector_geotiff 和探索算法生成轨迹的 hector_trajectory_server。hector_ slam仅依靠激光雷达数据进行建图与定位，在道路特别崎岖的时候，还可以使用惯性导航 对SLAM算法进行补偿，提供更高的定位精度。另外，如图8所示，通过hector_elevation_ mapping，系统可以融合微软的Kinect深度摄像头，构建2. f5D地图信息。Kinect得到的点 云数据为2D地图中相应的栅格提供了高度信息及其方差。最后，hector_costmap可以融 合2. ?高度地图与2D的占有栅格地图构建代价地图。
[0064] 步骤七：设置机器人的路径规划算法。
[0065] 机器人路径规划算法使用Dijkstra算法。具体为对一个有权重的图G进行操作， 起点为S，V代表G中所有的定点的集合，E表示G中所有的边的集合。（u，v)表示从顶点u 到顶点v的路径，边的权重为w:E - [0，°° ]，则w(u, v)是从顶点u到顶点v的非负代价， 即两顶点之前的距离。一条路径的代价值等于从路径起点到终点所经过的边的代价值的 总和。Dijkstra算法可以找到图G中任意两个顶点如s、t的最低代价路径，即最短路径。 Dijkstra算法的流程图如图9所示。图10给出了 Dijkstra算法的示例。算法具体步骤如 下。
[0066] St印1 :如图10所示，算法开始时，开启列表中只有起点S，0= {S}，且nbest=S，对S节 点的相邻节点进行遍历，并且将S加入关闭列表，则，0= {2, 4, 1，5}，C={S}。相应的从起点到 当前点的代价函数分别为，f (2) =1，f (4) =1，f(l) =2, f (5) =4。代价函数最小的点位2和4， 任取其一，令nbest=2,而且此时反向指针均指向起点S。
[0067] Step2 :对节点2的相邻节点进行遍历，此时0={4, 1，5}，C={S, 2}，而节点2的相邻 节点只有S，而且S已经在关闭列表中，算法继续向下进行。
[0068] Step3 :对当前0={4, 1，5}中的代价函数最小的节点4进行相邻节点遍历，nbest=4 ; 同样，将4从开启列表移到关闭列表，0={1，5}，C={S，2,4}，4的相邻节点有S、3、5,其中 S已经在关闭列表，不对其进行处理，而在开启列表中加入3。由于开启列表中有5,而且 f (4)+c (4, 5)=1+2=3小于前面f (5) =4,需要更新开启列表中5的代价函数，则f (5) =3,且5 的反向指针指向4。
[0069] 3七印4:此时开启列表和关闭列表分别为0={5,3}，0={5,2,4}，11^=5。对节点5进 行处理，5的相邻节点有3个，其中4和S已经在关闭列表中，不做处理，只剩终点G，因此把 终点G放入开启列表，0={G，3}，C={S，2, 4, 5}。
[0070] St印5 :当前代价函数最小的节点即为nbest=G，f(G)=f(5)+c(5，G)=5。将相应节点 的反向指针取出，也就找到了一条从起点S到终点G的最短路径S - 4 - 5 - G。
[0071] 步骤八：机器人进行自主行驶，同时探索室内未知环境。
[0072] 基于Hector SLAM的环境地图构建的基础上，机器人进行自主行驶建图。移动机 器人在探索未知区域的时候，需要进行环境的建图，同时也需要设置建图规则，使得在相对 较短的时间内能够得到最大的环境地图。
[0073] 本系统所使用的探索算法结合了现在常用的两种策略。向离机器人最近的前端移 动可以进行快速的地图获取，扩展未知区域。同时考虑现有地图中前端的大小。前端较长 的地方，往往意味着通过这里能够扩展较大的未知区域。对两个策略去权重进行加权。移 动机器人取其中能够最快得到最大地图的原则进行地图构建，直到将所有的探索前端探索 完毕。
【主权项】
1. 一种基于ROS(机器人操作系统，RobotOperatingSystem)的移动机器人室内环 境探索系统与控制方法，包括上位机、改装的iRobot机器人、UTM30LX激光雷达；通过采用 R0S的导航栈与基于HectorSLAM的自主建图算法，实现移动机器人对室内环境的自主探 索并生成环境二维地图。2. 权利要求1所述的一种基于R0S的移动机器人室内环境探索系统与控制方法，其特 征在于：包含如下步骤： 步骤1、以美国iRobot公司生产的iRobotCreate为移动机器人平台，进行改装与拓 展； 步骤2、采用R0S的导航栈（Navigation)来实现导航系统的具体框架，通过设置TF坐 标变换系统、Costmap占有栅格地图来完成导航栈的基本配置； 步骤3、通过激光雷达获取地图信息并进行图像信息的处理匹配； 步骤4、上位机对机器人状态获取与对激光雷达获取的二维环境地图信息进行实时监 控； 步骤5、利用R0S内的Hector库对机器人进行同步定位与建图； 步骤6、设置机器人的Dijkstra路径规划算法； 步骤7、机器人进行自主行驶，同时探索室内未知环境。
【专利摘要】本发明公开了一种基于ROS（机器人操作系统，Robot？Operating？System）的移动机器人室内环境探索系统与控制方法。该系统基于ROS，能够实现机器人对室内环境的自主探索与定位。该系统主要采用iRobot差分驱动底盘和UTM？30LX激光雷达。上位机可实时得到机器人的位置定位信息与室内环境探索路径轨迹，并由激光雷达的数据匹配得到由局部地图连接形成的全局地图。本发明提供了更加智能化的未知环境探索系统，为灾难环境救援提供了极大的便利与安全保障，实现了室内环境感知的信息交互，具有很强的便捷性和实用性。
【IPC分类】G05D1/02
【公开号】CN105487535
【申请号】CN201410524184
【发明人】石成玉, 吴成东, 胡美玉, 陈建辉
【申请人】东北大学
【公开日】2016年4月13日
【申请日】2014年10月9日
最新回复(0)