消毒机器人全覆盖路径算法设计

邱春凯，王利恒

（武汉工程大学 电气信息学院，武汉430205）

随着科技的发展，机器人技术越来越成熟，新型冠状病毒的爆发使消毒机器人的使用越发频繁。如何让机器人高效率、低成本地执行任务体现了其智能化水平。其中关键的一步是机器完成消毒工作时的路径规划问题。路径规划就是在消毒机器人在工作范围内，从初始点开始工作，找到一条将所有位置覆盖并且可连续的路径，消毒机器人可在无人看守的情况下自主完成室内消毒工作，使某种性能指标达到最优。此技术的实现使得消毒机器人能代替人深入病房，彻底而快速地做好消毒工作，卫生死角都能妥善处理。不仅解决了前线人员不足等问题，还有效防止各类病原体在医院内的传播，降低了感染传播的风险，减少了不必要的人力和物力损耗。所以就目前整个社会对消毒机器人的需求是非常大的。其智能化程度对于人类的医疗工作意义非凡。为此，环境栅格地图[1]构建与路径规划算法是实现此目标的两项关键技术。本文使用栅格法对地图环境进行环境建模，采取SPDF 算法[2]与Theta* 算法[3]相结合进行路径规划与控制实现。

1 机器人的定位和环境的识别

机器人采用的是激光雷达[4]进行环境的识别，机器人通过自己机身的旋转和雷达托盘的旋转进行周围环境的扫描获取周围的环境信息。

1.1 机器人定位

以机器人为原点，通过雷达扫描得到周边各点到原点的距离，连接各点就可以构成周边区域的轮廓，通过坐标变换，就可以确定当前机器人相对周边环境的位置，如图1所示。

图1 机器人扫描环境Fig.1 Robot scanning environment

1.2 环境识别

环境的识别主要是确定空间内的障碍物，为后面的路径规划提供设计输入。在地图重建[5]过程中，根据边界计算的布尔运算，可以确定外联通区域和内联通区域。外联通区域可以确定为边界，内联通区域可以确定为障碍物，如图2所示。

图2 识别环境Fig.2 Identify environment

由于消毒机器人自身有高度，雷达扫描的是一个三维的立体环境[6]，考虑到机器人自身高度，高于机器人的障碍物，下空高可以让机器人顺利通过的，在地图建模中按自由空间处理，而下空高低于机器人高度的，按照障碍物处理，如图3所示。

图3 机器人环境识别判断Fig.3 Robot environment recognition and judgment

2 路径规划算法

2.1 环境模型的建立

环境建模[7]是将机器人工作的现实环境情况转化为自身可以识别的信息的过程，合适的环境建模方式将会明显缩减路径规划算法研究的难度。目前常用的有拓扑法、可视图法、自由空间法以及栅格法等多种环境建模方法。

拓扑法[8]是以环境的几何特征为依据，将消毒机器人的整个消毒空间划分成许多个子空间，然后将这些子空间连接起来。拓扑法通过使用拓扑特征显著地缩小了机器人的探索空间，并且对机器人的定位精度要求相对较低。可实现从高维向低维的转化，但是无法区分相似区域。

可视图法[9]在进行环境建模时，是将机器人看做一个质点，将障碍物转化为规则的多边形，然后利用直线把起点、障碍物多边形的顶点和目标点连结起来同时保证连接线不与障碍物多边形相交，由此得到的是可视图。该方法具有原理简单，易于操作等优点。但是随着障碍物增多，将会导致顶点之间的连线增多，进而导致路径规划的复杂度上升和消耗的时间增多。除此之外，如果可视图中顶点位置发生变化，则需要重新构建可视图。

自由空间法[10]原理简单，便于应用，并且构建出的连通图并不会随着起始点和目标点位置的变化而改变。但是其复杂程度与障碍物个数成正比，当障碍物数量较大时，构建连通图将变得十分困难，同时无法确保能否获得最优路径。

对于栅格法[11]而言，其原理简单，便于后续路径规划算法设计等优点。此外，栅格法有一个十分重要的参数，这个参数就是栅格尺寸大小，当选择栅格法去完成环境建模任务时，应该选择合适的栅格尺寸。

本文选用栅格法建立模型，需要考虑到4 个影响因子：①房间清扫面积及障碍物数量；②根据路径规划方法制定具体路径和栅格地图大小；③环境地图中栅格尺寸大小；④计算机的运算能力和反应能力。

栅格的行列号用坐标（x，y）表示，或者用索引值表示，如图4（a）所示，黄点可用坐标（2，2）或者索引值13 表示。索引值表示栅格位置如图4（c）所示，这里选择消毒机机器人的轮廓的大小。格栅表示方法栅格分为自由格栅与有障碍物格栅，这里将自由栅格设置为“1”，障碍物栅格设置为“2”，机器人覆盖后的栅格设置为“3”，如图4（b）所示。起初自由栅格为白色，障碍物为黑色，当机器人遍历自由栅格后，由栅格变成灰色，如图4（a）所示。

图4 栅格法Fig.4 Grid method

2.2 算法设计

通常机器人全覆盖的时候采用的是随机算法[12]，回旋算法[13]，但这些算法会有很大的重复率，机器人完成任务时间长，机器人进去死区后很难走到新区域，本文设计一套算法使机器人能够高效完成任务。

首先在机器人遍历消毒阶段设置8 个方向，分别是North，South，West，East，North West，North East、South West 以及South East，如图5所示，并且设置移动方向的顺序规则，水平方向和竖直方向优先于倾斜方向，趋向于起始点方向的优先级高于远离起始点方向的优先级，满足以上条件的同时，水平方向优先级权重高于竖直方向优先级权重，也就是West 方向和South 方向都是趋向于起始点的方向，East 方向和North 方向都是远离起始点的方向，所以方向的优先级依次为West、South、North、East。

图5 机器人移动方向Fig.5 Direction of robot moving

假设机器人起始点为室内的左下角，West 方向和South 方向是趋向于起始点的方向，因此方向的优先级顺序是West、South、North、East、South West、North West、South East、North East；当消毒机器人在室内右上角时，East 方向和North 方向是趋向于起始点的方向，因此方向的优先级顺序是 East、North、South、West、North East、South East、North West、South West。

这里就是按照SPDF 算法的工作原理，从机器人起始点出发，在当前位置上根据优先规则选出此刻的移动方向并沿着这个方向移动1 个栅格，循环这个规律一直运动到死点位置。在这个时候，机器人并未将所有位置遍历，需要机器人移动到一个新的位置重新工作。

2.3 回溯点的衔接方法

当运动到死点位置时，需要移动到另一个子区域空间进行消毒，在这个过程中，消毒机器人不进行消毒工作，在这个时候建立回溯机制，记录未覆盖的子区域和寻找子区域衔接的路径目标点。利用权值和代价值找到最近的未遍历点设置为回溯点，设A（XA，YA），B（XB，YB）权值和代价值的公式为

找到最优回溯点后，机器人重新使用SPDF 算法进行全覆盖。这样依次循环直到机器人遍历所有点。这里连接回溯点使用的是Theta*算法。

式（3）欧氏距离公式[11]比较适用于机器人可以沿着任意方向进行移动的情况，具有更加接近于真实距离的优点，但是需要开方导致计算量增大，这里区域衔接采用的是Theta* 算法[13]，机器人也可以向着任意角度移动，所以十分符合。

A*算法[15]中每个位置可以沿着East、North、South、West、North East、South East、North West、South West 8 个方向延伸，即除了边界节点之外，每个节点都有8 个相邻。如图6 从起点（S）到终点（G）的搜索过程中，最终形成的路径为C1-C2-B3-A4。而在Theta*算法中，由于取消了对于搜索节点必须沿相邻节点传播的限制，路径可以沿任意角度转换与延伸，因而最终搜索形成的路径为C1-B3-A4，如图7所示。

图6 A*算法Fig.6 A* algorithm

图7 Theta*算法Fig.7 Theta* algorithm

传统的A* 算法是一种启发的搜索算法，其算法流程为从初始点向附近点进行扩展，将不超过地图的可行区域子节点放入openlist，然后在openlist中选取最优点作为下一个父节点，并将此点放入closelist 中，重复此过程直至目标节点，完成路径规划，流程如图8所示。

图8 A*算法程序结构框图Fig.8 Program structure block diagram of A* algorithm for robot environment recognition and judgment

根据A* 算法得出来的路径可以进行优化，得到Theta* 算法的路径，从初始点开始与后面的路径点进行连接判断，如果该点不存在障碍物，则判断下一个点，直到某点存在障碍物，则设该点的前一个放入节点P 中，并设置为新得父节点，一次循环，直到目标点，其算法流程如图9所示。

图9 Theta*算法程序结构框图Fig.9 Program structure block diagram of Theta*algorithm for robot environment recognition and judgment

图10所示为用A* 算法和Theta* 算法从黄点到紫点的两条路径，从图中看出，很明显存在更短的路径抵达终点。图中蓝色线路路径是根据A* 算法路径优化的路径，路径长度d=21.49，根据两点之间直线最短的法则，显然得出蓝色路径更短，经过计算可以得出蓝色优化后的路径长度d=20。

图10 Theta*仿真实验图Fig.10 Theta* simulation experiment diagram

2.4 全覆盖路径规划

首先机器人建立环境模型，采用SPDF 局部区域覆盖路径规划算法进行覆盖，局部覆盖的过程中更新环境空间模型和回溯点，一直到局部区域的死点。利用回溯点列表筛选出最优回溯点，移动到最近的一个子区域空间中，并且更新当前位置，直到回溯列表中无回溯点，则表示空间被全覆盖，程序流程如图11所示。

图11 机器人全覆盖程序框图Fig.11 Robot full coverage program block diagram

3 仿真实验

图12所示为10×10 栅格的全局路径规划图，黑色区域表示障碍物，白色区域表示自由可移动区域且是未覆盖的，灰色区域表示已覆盖区域，设起点为（9，2），黑色粗线条为全覆盖路径，黑点为当机器人移动到死点区域时最短的衔接点，此时消毒机器人的路径覆盖率到达无死角全覆盖，并且保证了在衔接路径时最短。

图12 全覆盖仿真实验图Fig.12 Simulation diagram of full coverage algorithm

4 结语

本文利用雷达上下角度摆动，将二维坐标系转化为了三维立体的坐标系，根据消毒机器人的高度来判断机器人的通过性，然后利用栅格法建立环境地图栅格模型，结合全覆盖路径规划算法和Theta*算法进行全局路径规划，并对室内进行避障路径规划和仿真实验。结果表明，在静态的环境中机器人对室内可达到全面的消毒，覆盖率高，路径短，同时可以避开障碍物。