基于ROS系统的迷宫机器人路径规划

技术领域

本发明涉及路径规划技术领域，尤其是涉及基于ROS系统的迷宫机器人路径规划。

背景技术

IEEE国际标准迷宫由16*16个、18*18cm大小的正方形单元格组成，其俯视图如图1所示。机器人走迷宫任务包括搜索和冲刺两个过程，其借助传感器、主控制器、电机控制等组件自主地向迷宫终点探索，并在搜索到迷宫目标点后，规划出一条从起点到目标点的最优路径。现有的机器人路径规划大多是集中在最优路径以及最少换向动作探索两个角度上，其缺乏对冲刺动作本身的改进。而且，现阶段的迷宫竞赛仍处于线下阶段，参赛者容易受硬件条件和地域限制。

发明内容

本发明针对传统迷宫机器人存在的硬件成本高昂、路径算法改良困难、比赛受硬件和地域限制的问题，提出了一种基于Ubuntu ROS系统搭建的线上迷宫机器人竞赛环境，使用者可以由软件编程验证算法，进而了解机器人的决策。同时本发明能够通过仿真评测计时系统，对选手成绩进行评估，实现了机器人的算法验证和线上比赛双重功能，摆脱了迷宫机器人硬件的限制，省去了硬件试错时间，同时在不受地域限制的情况下，能够实现用户的机器人比赛。

基于ROS系统的迷宫机器人路径规划，包括如下步骤：建模；基于ROS系统建立机器人模型和迷宫模型；底层运动控制；基于发布订阅通信模式，控制器为机器人进行前进/转弯动作规划、墙壁检测和位姿矫正；路径搜索；通过路径规划算法规划出起点到目标点的最优路径；冲刺动作规划；在最优路径上，基于当前迷宫格和前一个迷宫格的出口方向确定机器人在当前迷宫格的冲刺方向，然后基于当前迷宫格和前一个迷宫格的冲刺方向确定机器人在当前迷宫格的冲刺动作；冲刺动作整合；若相邻迷宫格的冲刺动作的变向趋势相同，则将两个迷宫格的冲刺动作合并，并重新确定冲刺动作的变向角度。

优选的，基于ROS系统建立机器人模型中，利用URDF模型文件里面的link标签语言来表征机器人底盘、轮子、传感器部件，利用URDF模型文件里面的joint标签语言表征部件之间的连接关系；利用libgazebo_ros_diff_drive.so运动插件将机器人左右两轮赋予差速驱动属性，用于控制机器人轮子关节的位置变化，同时获取轮子的位置以及速度的信息反馈，使机器人能够正常行走；利用libgazebo_ros_laser.so插件赋予激光雷达传感器部件测距功能，完成环境信息检测。

优选的，控制器通过订阅运动控制话题获取机器人的线速度与角速度信息，通过订阅里程计话题获取机器人的实时位置与角度，通过订阅激光雷达传感器话题获取机器人到障碍物的距离信息。

优选的，路径搜索的具体过程为：对路径上的迷宫格添加等高值，从而获得迷宫的等高图；按照迷宫格等高值由大到小的顺序找到起点到目标点的最优路径。

优选的，路径搜索与冲刺动作规划之间还包括八方向划分，其中，绝对方向的八个方向以迷宫地图为中心，相对方向的八个方向以机器人本体为中心。

优选的，冲刺动作规划的具体过程为：在最优路径上，确定每个迷宫格出口的绝对方向并将其表示为向量，将当前迷宫格和前一个迷宫格的向量和作为当前网格的冲刺方向；根据如下公式确定当前迷宫格的冲刺动作：

DiagonalwayAction[DiagonalwayCount]＝(8+cDirTemp-self.dir)％8

其中，DiagonalwayCount表示机器人的冲刺动作，其为{0,1,2,6,7}中数字，其中，0代表直行，1代表右转45度，2代表右转90°，6代表左转90°，7代表左转45°；DiagonalwayAction表示最优路径迷宫格上机器人冲刺动作的数组；cDirTemp表示当前迷宫格的冲刺方向；self.dir代表前一个迷宫格的冲刺方向。

本发明采用上述基于ROS系统的迷宫机器人路径规划，首先为迷宫机器人路径规划提供了新的改进方向，使机器人的执行动作变得简化，节省冲刺运动时间，提高机器人的探索效率；其次，为迷宫竞赛提供了线上举办方案，可以有效降低硬件条件和地域对选手的限制，并且为相关衍生技术例如协同作战、协同搜救、协同巡检等具有重要研究价值。

附图说明

图1为IEEE国际标准迷宫的俯视图；

图2为本发明实施例的方法流程图；

图3为本发明实施例中迷宫描述文本(a)和迷宫环境(b)；

图4为本发明实施例中机器人模型的Link标签属性；

图5为本发明实施例中机器人模型的部件和关节的连接关系；

图6为本发明实施例中激光雷达传感器的原理图；

图7为本发明实施例中迷宫绝对方向(a)和机器人相对方向(b)；

图8为本发明实施例中机器人执行迷宫任务的探索路径示例图；

图9为本发明实施例中基于图8路径生成的等高图；

图10为本发明实施例中基于图8路径生成的迷宫格出口方向图；

图11为本发明实施例中冲刺动作的十二种状况图；

图12为本发明实施例中基于图10生成的冲刺动作图；

图13为本发明实施例中单迷宫格冲刺动作的示例图；

图14为本发明实施例中冲刺动作整合的示例图；

图15为本发明实施例中基于图12生成的最终路径图。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步说明。

本发明整体流程图如图2所示，其基于Ubuntu 18.04ROS系统实现对迷宫机器人的运动控制、算法设计、迷宫任务实现，首先规划出最优路径，然后对最优路径迷宫格上的冲刺动作进行规划与整合，从而使得机器人能以最短距离和最少冲刺动作到达终点。本方案主要包含建模、底层运动控制设计、机器人激光雷达检测设计、八方向划分、最优路径规划、冲刺动作规划、冲刺动作整合等部分，下面进行具体阐述。

一、基于ROS的迷宫及机器人建模

在ROS系统环境中，提供了丰富的模型可视化平台，如RVIZ、Gazebo等，能够实现对机器人模型的物理模拟，同时还能仿照真实环境搭建模拟环境。模型通常利用URDF文件的形式表述，URDF文件内使用标签语言构建模型的组成部件、位姿、偏移、质量、功能属性等，并可以建立不同部件的连接关系。根据研究需要，从两部分对迷宫机器人模型进行搭建，一部分是机器人本体部件的建立，另一部分是部件功能配置。

1、迷宫建模

ROS系统里面的gazebo可视化平台能够构建复杂的室内或室外环境，提供高保真的物理模拟和一整套传感器模型，准确的模拟机器人的运行。在终端打开gazebo，通过加载一个.mz的迷宫描述文本文件，如图3(a)所示。将文件里面的“|”符号转化为迷宫对应的竖直墙壁，将文件里面的“_”符号转化为迷宫对应的横向墙壁，将文件里面的“空格”符号转化为出口，生成的迷宫模型如图3(b)所示。通过给环境添加光照，使迷宫充分达到保真效果，迷宫一共分为16*16个迷宫格，每个迷宫格都有迷宫墙壁(挡板)，每块迷宫挡板为一个长度为18cm、宽度为1cm、高为5cm的长方体，与实际的迷宫大小一致，环境中设置有重力，地面上设置有摩擦力。

2、迷宫机器人建模

机器人本体部件包含link标签和join标签。如图4所示，link标签语言来表征机器人底盘、轮子、传感器部件，其规定了机器人部件结构外观，一个link标签代表一个部件，每个

部件功能配置主要通过gazebo标签来描述，利用libgazebo_ros_diff_drive.so运动插件将迷宫机器人左右两轮赋予差速驱动属性，用于控制机器人轮子关节的位置变化，同时获取轮子的位置以及速度的信息的反馈，使机器人能够正常行走，利用libgazebo_ros_laser.so插件赋予激光雷达传感器部件测距功能，完成环境信息检测。此外，利用gazebo标签将迷宫机器人本体显示到gazebo环境中。

二、底层运动控制设计

机器人是一种高度复杂的系统性实现，其需要实现各种传感器以及运动控制功能。在ROS中每一个功能点都是一个进程，每一个进程都是独立运行的。为了建立不同进程间的通信，ROS拥有话题通信、服务通信、参数服务器三种实现策略，其中，话题通信是ROS中使用频率最高也是最高效的一种通信模式。话题通信是基于发布订阅模式的，也即一个节点发布消息，另一个节点订阅该消息。因此，本文采用话题通信研究建立不同节点间的通信，从而实现控制功能。

利用python语言编写控制文本，通过发布和订阅运动控制话题(cmd_vel)实现对迷宫机器人的线速度和角速度控制，通过发布和订阅激光雷达传感器话题(/my_mm_robot/laser/scan)实现激光雷达传感器检测信息发布，通过发布和订阅里程计话题(odom)实现迷宫机器人的位置信息、角度信息的获取，最终完成机器人前进与转弯动作规划、位姿矫正、测距以及坐标更新等任务。

三、机器人激光雷达检测设计

机器人在迷宫中行走，通过2D激光雷达传感器感知与障碍物墙壁的距离。在迷宫环境中，将激光雷达传感器分为三部分，分别是前方距离感器、左右距离传感器、左右斜45°传感器，如图6所示。前方距离传感器取激光雷达正90度的激光射线，进行机器人前方距离(F_dis)检测，判断前方是否有路；左右两侧红外传感器取激光雷达与水平夹角10°的激光射线，进行机器人左右两侧墙壁信息检测；左右斜45°传感器呈水平45°分布，用于迷宫机器人直行时的横向误差与角度误差纠正。

四、八方向划分

斜向45°冲刺规定了机器人行进的八个绝对方向和八个相对方向。八个绝对方向以迷宫地图为中心，定义为上方、右上方、右方、右下方、下方、左下方、左方、左上方，分别用0-7的数字来表示，如图7(a)所示。八个相对方向以机器人本体为中心，分别为前方、右前方、右方、右后方、后方、左后方、左方、左上方，同样用0-7的数字来表示，如图7(b)所示。

五、最优路径规划

本方案的45°斜向冲刺算法首先需要规划出起点到目标点的最优路径，和传统运动算法一样需要制作等高图，添加路径上的迷宫格等高值，如图8所示。黑色线条为机器人搜索路径，根据搜索路径和搜索得到的墙壁信息，可以规划出最优路径，将规划的路径数值化，可得到如下二维等高值矩阵：

。二维等高值矩阵每一个元素代表一个迷宫坐标等高值，第一个元素‘47’为起点(0，0)坐标的等高值，沿着等高值由大到小，可以得到一条由起点到目标点的最优路径，如图9所示。

六、冲刺动作规划

在最优路径基础上，进一步对冲刺动作进行规划。

第一步，由搜索时的每个迷宫格的墙壁信息确定冲刺路径上每个迷宫格的出口绝对方向，如下图10所示。

第二步，根据当前迷宫格的出口方向和前一个迷宫格的出口方向确定当前格的冲刺方向。冲刺方向为前一格开口方向与当前格开口方向的向量和的方向，主要分为如图11所示十二种状况。其中，细虚线箭头表示当前迷宫格出口方向，细实线箭头表示前一个迷宫格出口方向，粗实线箭头表示合成的冲刺方向。合成冲刺方向用二维数组self.MapDiagonalway[cX][cY]来保存，冲刺方向图如图12所示。

第三步，确定单格冲刺动作。冲刺动作由当前机器人车身的绝对方向和冲刺方向转换而来，且遵循下面的公式：

DiagonalwayAction[DiagonalwayCount]＝(8+cDirTemp-self.dir)％8

。其中DiagonalwayAction数组存放迷宫格的相对动作的数值，cDirTemp代表当前迷宫格的冲刺方向，self.dir代表上一迷宫格的冲刺方向，DiagonalwayAction数组存放的值是{0、1、2、6、7}中的数字，如表1所示。

表1整合前的冲刺动作编码及其含义

如图13(a)所示，当前机器人车身的绝对方向为0，下一格的冲刺方向为1，那么根据公式计算出相对动作值为1，保存到数组，即准备执行动作为右转45°；图13(b)，当前车身绝对方向为1，下一格的冲刺方向为1，那么计算出相对动作值为0，即将要直行；图13(c)，当前车身绝对方向为1，下一格的冲刺方向为0，计算出动作为7，即将要左转45°，以此方法直到处理完所有迷宫冲刺方向。以图7迷宫地图为例，计算出的单个动作数组DiagonalwayAction[]＝[0,0,1,0,7,0,0,0,1,0,6,0,2,0,6,0,1,1,2,0,6,0,1,1,1,0,0,0,0,0,7,7,1,0,0,0,7,7,7,0,7,0,2,0,6,7,0]。

七、冲刺动作整合

前一阶段已经确定了每个迷宫格的冲刺动作，但是如果一个一个执行起来动作较多，动作越多产生误差的可能性就越大，而且动作增多会降低机器人的效率，增加不必要的时间。例如，一个简单的右转90°动作将会分成两个右转45°来实现，由于每完成一个动作将会把速度清0，再去执行下一个动作，中间的加减速过程使转弯效率降低。因此，对冲刺动作进行一定的整合十分必要。

在冲刺动作整合过程中，首先需要对将要执行的单格冲刺动作进行判定，即对DiagonalwayAction数组里面存放的元素进行判定，可能会判定即将执行的一个动作，也可能两个甚至三个动作，根据具体情况有所不同，其次需要判断冲刺动作前冲刺方向是斜向还是垂直。

如图14(a)，即将执行的单格动作为右转45°，且当前的方向为垂直方向，那么整合后的动作为直线右转45°，左转45°同右转。如图14(b)，即将执行的单格动作左转45°，且当前的方向为斜向，那么整合后的动作为斜向左转45°。如图14(c),即将有两个连续的45°右转动作，且转弯前方向为垂直方向，那么可以将这两个动作整合为一个直线右转90°动作。如图14(d)，同样有两个连续右转45°的动作，但是转弯前方向为斜向，那么可以整合为一个斜向90°转弯。如图14(e)，即将执行的动作为右转45度且下一个动作是右转90°，又因为转弯前方向为垂直方向，那么可以将这两个动作整合为直转弯135°。如图14(f)，即将执行的动作为右转45度紧接着是右转90°，但是转弯前方向为斜线方向，那么可以将这两个动作整合为斜转弯135°。如图14(g)，即将执行的动作为右转45°、右转90°、右转45°，那么可以将其整合为一个直线180°的转弯。如图14(h)，当连续直行动作时，需要判定当前的方向是斜向还是垂直，如果是斜向需要执行45°直行，如果是垂直方向，需要执行90°直行。

将整合的动作用一维数组actionsequence[]保存，元素用1-16的整数表示，对应的含义如表2所示。

表2整合后的冲刺动作编码及其含义

以图8迷宫地图为例，整合前后的动作对比如表3所示。整合出的动作数组actionsequence[]＝[15,15,6,16,7,15,15,15,6,16,3,16,4,16,3,16,8,10,16,3,16,8,2,15,15,15,15,15,1,6,16,16,16,7,1,15,5,16,4,16,11,15]，整合后共42个动作，相较于整合前的47个少了5个动作。最终，生成的斜向冲刺路线如图15所示。

表3整合前与整合后的冲刺动作

/>

以上是本发明的具体实施方式，但本发明的保护范围不应局限于此。任何熟悉本领域的技术人员在本发明所揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内，因此本发明的保护范围应以权利要求书所限定的保护范围为准。