移动送料机器人地图构建与运动控制研究

郭隆臻 徐定明 李子信

摘 要：移动机器人的地图构建与运动控制是智能移动机器人进行自主导航的基础。针对工业装配生产线的不同工位需要送料的需求，本文以一台轮式差速移动送料机器人为研究对象，利用激光雷达作为传感器，搭建一个装配生产线，设计、实现了基于ROS的移动送料机器人的地图构建与运动控制策略。利用slam_gmapping算法包，实现了机器人在特定装配生产线环境下的同时定位与地图构建;在主程序代码中利用状态机的机制，实现了机器人到达指定工位的运动控制。实验结果表明，该算法和运动控制策略可以满足在所搭建的装配生产线中进行定位和运行。

关键词： 移动机器人;轮式差速;机器人操作系统;地图构建;运动控制

文章编号： 2095-2163（2021）01-0055-06 中图分类号：TP242 文献标志码：A

【Abstract】The map construction and motion control of mobile robots are the basis for autonomous navigation of intelligent mobile robots. In response to the needs of different stations in the industrial assembly line for feeding， this paper takes a wheeled differential mobile feeding robot as the research object， using lidar as a sensor to build an assembly production line， designing and implementing the ROS-based mobile feeding robot map construction and motion control strategy. The slam_gmapping algorithm package is used to realize the simultaneous positioning and map construction of the robot in a specific assembly line environment; the state machine mechanism is used in the main program code to realize the motion control of the robot to the designated station. The experimental results show that the algorithm and motion control strategy can satisfy the positioning and operation in the assembled assembly line.

【Key words】mobile robot; wheeled differential; robot operating system; map construction; motion control

0 引 言

随着制造业和机器人行业的快速发展，越来越多的机器人开始在制造业中发挥重要作用，将行业工人从单一和危险的工作环境中解放出来。特别是对于一些小型装配生产线，每个工位上都必须有原料供给，这一工作若由移动机器人来完成，将会大大提升生产效率，节约劳动力成本。而对于移动机器人领域来说，同时定位与建图（SLAM）和路径规划是目前两大研究热点，并且也是两大研究难点。而移动机器人的地图构建和运动控制是机器人在陌生环境中进行路径规划的基础。

在地图构建方面，基于激光雷达的SLAM技术起源于上世纪八十年代，Smith等人[1]创造性地提出了基于卡尔曼滤波的SLAM算法，将机器人的位姿与路标组成联合状态，并且通过激光雷达用最大似然估计将数据进行关联，从而完成对环境的一致性估计，这一研究的最大贡献就是使基于卡尔曼滤波的SLAM算法成为当时应用的主流方法。Gordon等人将Hammersley在20世纪中期提出的粒子滤波算法思想[2]应用于SIS的非线性滤波，使得粒子滤波在许多领域获得广泛使用，并得到长足发展。Murphy[3]使用Rao-Blackwell统计量寻找更优的统计估计，并且与粒子滤波相结合提出了RBPF算法。Montemerlo等人将粒子滤波引入了基于递归概率的SLAM设计思想中，提出了Fast SLAM算法。Grisetti等人[4]为了解决Fast SALM在大尺度环境中需要较多粒子来计算机器人后验概率分布的问题，提出了gmapping方案。该算法将粒子的数目进行限定，在一定程度上减小了内存消耗，目前在建立二维栅格地图中应用十分广泛。

国内关于SLAM的研究虽起步较晚，但其发展的速度非常快。陈白帆等人[5]对Fast SLAM做出了优化，使粒子群更为集中，提升了算法的运算速度。朱磊等人[6]将人工鱼群算法应用在对粒子分布预测的过程中，使更新粒子的准确度与机器人的真实状态分布更为相符。厉茂海等人[7]在RBPF算法的重采样过程中使用自适应方法，将进化策略和RBPF算法结合起来，得到了较好的估计结果。Yuan等人[8]提出了一种使用均匀分布的样本来表示粒子先验位姿的方法，改进了重采样过程中的粒子退化问题，并且将所有粒子样本进行批量处理，极大提高了激光SLAM算法的实时性。

以上的SLAM算法中，由于gmapping可以實时构建室内地图，在构建小场景地图时所需的计算量较小且精度较高，对激光雷达频率要求低、鲁棒性高，不需要太多的粒子，而且也没有回环检测，有效地利用了车轮里程计信息，里程计可以提供机器人的位姿先验。因此，本文选用gmapping对环境进行地图构建，建立的栅格地图可以满足该移动送料机器人的运动控制。

1 机器人平台设计

在硬件方面，考虑到稳定性与防撞性，移动机器人平台的下部分车体为钢材质，上部分车体为铝合金材质，前部为北阳雷达，后部为思岚雷达。采用六轮底盘结构，双轮差速驱动，4个万向轮支撑，弹性悬挂;在机械臂的选择上，采用了遨博i5协作机器人;电源方面，移动平台与机械臂采用2个独立的电池。其硬件参数配置信息见表1。末端执行器包括电磁铁模块、智能视觉多模板识别模块和双抓手模块，可以共同完成识别、抓取或吸附等一系列流程。移动操作机器人的实物如图1所示。

在软件方面，由于机器人操作系统（ROS）框架具有灵活、高效、集成度高、代码开源等优点，可以简化机器人平台下的复杂任务创建和稳定行为控制，帮助用户快速完成机器人应用的建立、编写和多机整合，所以被应用得越来越广泛。本文采用ROS框架，利用gazebo（仿真环境）进行仿真环境的创建，利用rviz（三维可视化平台）进行可视化显示，采用C++语言编写控制程序。

2 运动学模型的建立

在移动机器人的转向设计中，差速转向由于结构紧凑，重量轻，不需要专门的转向机构，而且可实现较小半径、甚至零半径的转向，所以近年来差速转向越来越多地运用于各式移动机器人中。本文选用轮式差速转向机器人，其运动学模型如图2所示，一共6个轮子，中间2个为驱动轮，前后4个为万向轮。这种结构简单，在理论上的旋转半径可为零，但由于转动过程中的速度瞬心位于两驱动轮轴心连线上，因而即使旋转半径为零，旋转中心也与车体几何中心不一致[9]。由于移动平台上需要加装机械臂和移动电源，需要移动平台部分有较大的承载力，而此结构具有4个万向轮，能增大平台的承载能力和稳定性，可以满足要求。

在图2中，假设其内部布局均匀对称，质心与几何中心重合，在两驱动轮轴线中心位置。设在t时刻，机器人速度瞬心为点O，其余各物理量见表2。

在图2中，由运动学知识可得：

由公式（1）～（5），结合图2，可求得机器人在局部坐标系中的运动学模型为：

机器人在全局坐标系中的运动学模型为：

在实际的差速模型中，可以通过左右驱动轮编码器的读数，实现移动机器人的轨迹推算。差速机器人的运动模型有3种，即：切线模型、割线模型和圆弧模型。其中，ROS中采用的是切线模型，故本文也选取切线模型，假设机器人在原来方向上沿着切线运动到下一点，再转过一定的角度。已知机器人在t时刻的位姿，通过t+1时刻编码器的读数，能够计算出移动机器人在t+1时刻的位姿。建立模型如图3所示，将时间离散化，取采样间隔为Ts，机器人从点A运动到点B为一个采样间隔，可得到移动机器人的轨迹推算模型为：

3 环境地图建模

环境地图建模需要将创建的地图模型与传感器所获得的环境信息进行整合，对环境模型的要求是便于机器人控制器进行存储、处理、更新和应用。在建立地图的过程中，机器人主要的数据来源是里程计信息和激光传感器的信息。在 ROS 中使用较为广泛的地图构建方法是栅格地图法（Grid mapping），由德国研究者 Grisetti 等人[10]提出并优化，是一种基于粒子滤波器的能同时定位与构建地图的算法。算法的优点是建模方便、易于编程、运算速度快并且精度高。因此，本文选用该方法进行环境地图的建模。

一个10×10 大小的栅格地图区域如图4所示。研究中将该区域划分为2种状态区域：自由状态区域和障碍状态区域。其中，白色栅格体现为自由状态区域，机器人可自由行驶;黑色栅格体现为障碍状态区，机器人不可通过。

要确定各栅格的坐标，可以令U={1，2，3，…，100}为栅格的序列集，以地图的左下角点（0，0）为原点，再由左至右、由下至上，对地图按序编号，从1～100。序号j的坐标通过下式确定：

其中，mod表示求余运算;int表示取整运算;R表示栅格行数。

本文选用gmapping功能包，分别在仿真环境和真实环境进行地图建模。gmapping功能包集成了Rao-Blackwellized粒子滤波算法，其核心运行节点为slam_gmapping，slam_gmapping的总体框架如图5所示。gmapping功能包订阅机器人的深度信息、IMU信息和里程计信息，同时完成一些必要参数的设置，即可创建并输出基于概率的二维栅格地图。

3.1 仿真环境建图

在ROS中gazebo上所搭建的仿真实验环境如图6所示。每个工位之间保持着一定的距离，需要移动机器人到每个工位进行送料。移动机器人底盘部分的模型在rviz中的显示如图7所示。

在构建地图时，首先启动gazebo仿真环境，接着启动rviz可视化环境，稍后将启动键盘控制器来移动机器人。然后，通过遥控界面控制移动操作机器人在仿真环境内运动，环绕全场进行地图构建，在可视化环境中可以看到地图构建的实时特征。最后， 在地图构建成功后，通过地图服务节点（map server）将地图进行保存，用于后续的定位导航，建好的仿真环境地图如图8所示。在地图构建过程中，ROS 节点会不断地校正机器人在环境中的位置信息，由此确保地图的准确性，进而通过键盘控制机器人的移动。考虑到仿真环境与真实环境是等比例的，所以仿真环境总体尺寸较大，在有些地方机器人与障碍物之间的距离超过了激光雷达的有效距离，会造成地图发生偏移。故在建图过程中，将指挥机器人环绕全场完成总体建图，再进行中间障碍物的扫描，如此可使建立的地图更加精确。

3.2 真实环境建图

利用实验室內的设备与器材，搭建了一个装配生产线的场景，其场景与仿真环境相似，用来进行真实环境的建图，如图9所示。

所不同的是，真实环境会与工业装配线更为贴切，有些工位的底座部分是镂空的，而本文所用的是2D激光雷达，会造成激光所在平面扫描不到障碍物的情况，如图10所示，有些工位只显示一些离散的点。

针对这个问题，先选用Matlab中的imread和imwrite函数进行格式转换，将ROS中保存的默认格式.pgm转换为.png格式，而后使用Photoshop等画图软件进行人工修正，最后用Matlab将图片转换成标准黑白格式，替换掉之前的地图文件。修正后的地图如图11所示。

4 运动控制实验

移动机器人主程序使用自定义ROS消息类型ControlCommand进行控制，sendMotionCommand函数向控制程序发送指令，并将last_motion_command_done变量置为false。当控制程序完成sendMotionCommand申明的动作，会将last_motion_command_done变量置为true。可通过定时轮询该变量控制机器人的状态机转换，实现各种功能。主程序中用到的command包括MOVE_TO_TARGET、MOVE_TO_TARGET_WITH_PATH、TURN_TO_TARGET、TURN_TO_ANGLE和JOKSTICK_CONTROL。前两个为移动命令，中间两个为转动命令，最后一个为切换手柄控制命令。

在主程序代码中利用了状态机的机制，来实现机器人到达指定工位的运动控制。至关重要的函数是move_loop（），将会以 100 ms的频率被调用。该函数下面有int类型的全局变量move_process，每一个move_process代表机器人的一种状态机，通过对move_process赋值，使机器人跳转状态。

基于修正后的在真实环境下构建的环境地图，在地图中选取了5个工位进行实验，第一个工位为取料处，后四个工位为送料处。为了提高安全性，设定机器人的最大直线速度为0.7 m/s，最大角速度为0.4 rad/s。与仿真实验不同的是，增加了机械臂来完成工件的识别与抓取。本次实验选取膨胀螺栓作为被抓取的工件，垫片用电磁铁来吸取，其余由电动手来抓取，电动手抓实物视像如图12所示。为了提高送料的效率，将膨胀螺栓的4类组件抓取后分别放入对应的盒子中，在送料时直接将盒子放到对应工位即可。

设定5个工位分别为A、B1、B2、B3、B4，其对应关系见表3。机器人到达抓取工位后，由移动平台给机械臂发送信号，此时，移动平台原地不动，由机械臂进行目标的识别与抓取。待抓取动作完成后，机械臂给移动平台发送信号，机器人按照事先规划好的路线到达每个送料处，送料的次序为垫圈、螺母、螺栓、套筒。到达每个送料处后，也是先由移动平台向机械臂发送指令，待送料完成后，再由机械臂向移动平台发送指令。直到最后一次送料完成后，实验结束。其送料流程图如图13所示。

5 结束语

本研究基于机器人操作系统，以一台轮式差速移动送料机器人为研究对象，设计并得到了定位导航系统。首先搭建了类似汽车装配生产线的室内环境;然后利用ROS系统中的开源slam_gmapping算法包，实现了机器人在该环境下的同时定位与地图构建;最后，利用状态机的机制，实现了机器人到达指定工位的运动控制。实验结果验证了该算法和运动控制策略的可行性和可靠性，能够进行准确定位和运动控制，便于后续在系统中加入路径规划算法，实现更加智能化的自动送料，具有良好的工业应用前景。

参考文献

[1]SMITH R， SELF M， CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence & Pattern Recognition， 1988， 5：435-461.

[2]Chen Zhe. Bayesian filtering： From Kalman filters to particle filters， and beyond[R]. Hamilton：McMaster University， 2003.

[3]MURPHY K. Bayesian map learning in dynamic environments[C]//Advances in Neural Information Processing Systems. San Mateo， CA： Morgan Kaufman， 1999，12：1015-1021.

[4]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.

[5]陈白帆， 蔡自兴， 袁成. 基于粒子群优化的移动机器人SLAM方法[J]. 机器人， 2009， 31（6）： 513-517.

[6]朱磊， 樊继壮， 赵杰， 等. 未知环境下的移动机器人SLAM方法[J]. 華中科技大学学报（自然科学版）， 2011， 39（7）： 9-13.

[7]厉茂海， 洪炳熔， 罗荣华. 用改进的Rao-blackwellized粒子滤波器实现移动机器人同时定位和地图创建[J]. 吉林大学学报（工学版）， 2007， 30（2）： 401-406.

[8]YUAN Rupeng， ZHANG Fuhai， QU Jiadi， et al. An enhanced pose tracking method using progressive scan matching[J]. Industrial Robot-The International Journal of Robotics Research and Application， 2019， 46（2）： 235-246.

[9]杨俊驹， 林睿， 王振华， 等. 轮式移动机器人运动控制系统研究与设计[J]. 现代电子技术， 2016， 39（2）： 22-27.

[10]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.