本系列文章主要介绍全覆盖路径规划开源项目Clean-robot-turtlebot3的相关内容，包含如下四篇文章，分别介绍了开源项目Clean-robot-turtlebot3流程的概括总结、ROS坐标系常用坐标及其变换、Clean-robot-turtlebot3关键函数解析等内容。

  1、全覆盖路径规划开源项目Clean-robot-turtlebot3原理及流程概括总结

  https://blog.csdn.net/qq_44339029/article/details/136880603

  2、ROS坐标系常用坐标及其变换详细梳理

  https://blog.csdn.net/qq_44339029/article/details/136671867

  3、全覆盖路径规划开源项目Clean-robot-turtlebot3关键函数解析（上）

  https://blog.csdn.net/qq_44339029/article/details/136879605

  4、全覆盖路径规划开源项目Clean-robot-turtlebot3关键函数解析（下）

  https://blog.csdn.net/qq_44339029/article/details/136660766

---

  一、相关介绍

---

  Clean-robot-turtlebot3项目源码链接如下：

  https://github.com/li-haojia/Clean-robot-turtlebot3

–

  Clean-robot-turtlebot3效果演示如下：【图片来源于项目介绍】

  Clean-robot-turtlebot3系统结构如下：【图片来源于项目介绍】

---

  二、Clean-robot-turtlebot3原理及实现流程概括总结

---

  总的来说，Clean-robot-turtlebot3实现全覆盖清扫的流程可以分为初始化阶段、全覆盖路径规划阶段、执行覆盖阶段，这三个阶段，各个阶段的主要流程如下所示：

  1、初始化阶段

  在初始化阶段，通过类的构造函数CleaningPathPlanning()，及其调用或间接调用的initializeMats()、getCellMatAndFreeSpace()、initializeNeuralMat()、initializeCoveredGrid()等函数完成算法各项参数和数据结构的初始化。

  在初始化阶段有几个变量需要重点注意一下，首先是使用costmap栅格地图来初始化的新地图变量srcMap_，这里的转换仅仅是进行格式的转换（srcMap_的数据类型是OpenCV常用的Mat类型），其尺寸、分辨率和代价值均与costmap栅格地图相同。

  紧接着进一步按照设定的覆盖直径SIZE_OF_CELL，对srcMap_地图变量进行划分成一个个SIZE_OF_CELL x SIZE_OF_CELL个栅格大小的正方形区域。每个正方形区域将变成新地图cellMat_中的一个栅格，且若原地图中这个SIZE_OF_CELL x SIZE_OF_CELL个栅格的正方形区域中每个栅格均不存在障碍物，则认为转换后得到的新地图cellMat_中的这个栅格处没有障碍物，否则认为新地图中的这个栅格处有障碍物。

  上述转换将原有地图稀疏化，即认为机器人每走过原地图中一个栅格就完成了对其周围SIZE_OF_CELL个栅格的覆盖，所以按照SIZE_OF_CELL对地图进行稀疏化后得到的新地图，可以更简便的进行后续的全覆盖路径的规划。此外，得到了新地图cellMat_后，进一步标记了出新地图的自由空间（无障碍物）的栅格，并将其坐标存储到freeSpaceVec_中，方便后续覆盖规划时查找是否还存在没有覆盖的无障碍区域。

  最后，使用稀疏后的新地图cellMat_来初始化活力值地图neuralizedMat_，如果新地图中某个栅格处有障碍物，则其对应的活力值地图中存储一个较大的负数-100000，如果没有障碍物，则活力值地图中对应的位置存储的活力值为50/j，其中j是其所在的列数，这种活力值设置模式使得同一列无障碍物点的初始活力值相同，而随着所在列编号的增加，活力值依次降低，在后续的步骤中，机器人会倾向于向活力值高的区域移动，这样初始化活力值，将引导机器人按列形成从弓字型的覆盖路径，且避开障碍物。

---

  2、全覆盖路径规划阶段

  全覆盖路径规划阶段，核心程序在mainPlanningLoop()函数中，首先将机器人的当前位置作为全覆盖路径规划的起点，然后进行循环迭代，核心流程是，在每次迭代中，先计算机器人在当前点周围八个方向上相邻栅格（新地图中）的活动值，然后选择活动值最大的方向作为下一步的移动方向。然后按照该方向移动一个栅格，得到全覆盖路径中下一个点的位置，将其加入存储全覆盖路径的队列pathVec_中，并将该点坐标更新为新的当前点坐标，方便进行下一轮循环迭代信息。在特定条件下，还要处理距离关系，防止自锁现象，在当前点周围没有合适的点作为下一个路径点时，前往下一个区域继续规划。循环执行以上步骤，直到达到规划循环的最大次数或出现自锁现象(全覆盖路径规划完成)。

  上述流程有两个关键地方，即如何计算活力值，以及当前点周围没有合适的点进行移动时，如何前往下一个区域继续规划。

  先来看如何计算活力值，基于当前点的位置（这里的当前点是指当前规划中的当前点，不是机器人的当前位置）在姿态角分别为0, 45, 90, 135, 180, 225, 270, 315这八个方向上计算这八个相邻栅格的活动值，具体而言，计算每个方向上活动值时，先计算当前方向角度与上一步的角度lasttheta之间的角度差deltaTheta，如果差值超过180度，进行调整，确保差值在0到180度之间。然后将其转换为当前方向的角度与上一步的角度的偏离程度e。e的值在0到1之间，极端的0表示方向相同，1表示方向相反。若沿当前方向移动一步后，会超出地图边界，则当前方向需要给一个很小的活力值，如程序中的v = -100000，来使机器人不要向该方向移动。若不会超出地图边界则采活力值的计算分为两种情况，对于0，90，180，270这四个横平竖直的移动方向，活力值由两部分构成，第一部分是从活力值地图neuralizedMat_中查询的沿当前方向移动一步后的位置的活力值，第二部分是c_0 * e，其中c_0 是常系数50。附加项 c_0 * e的作用是更倾向于选取与上一次移动方向相同或者相近的方向。对于所有对角方向，即45, 135, 225, 315这四个方向计算活力值时，后面还有一个附加项-200，这个-200的附加项的作用可能是尽量选取上下左右这四个方向，所以对对角的四个方向施加了额外的代价。沿着0度和45度方向的计算表达式如下所示

v = neuralizedMat_.at<float>(currentPoint.row, currentPoint.col + 1) + c_0 * e;

 v = neuralizedMat_.at<float>(currentPoint.row - 1, currentPoint.col + 1) + c_0 * e - 200;

  接下来介绍当前点周围没有合适的点进行移动时，如何前往下一个区域继续规划。在计算完当前点周围八个相邻点的活力值后，会进行判断，若他们这的最大活力值依然小于0，说明周围上下左右四个移动方向上的点要么是障碍物，要么已经在覆盖路径里面了，在该区域已经不能继续维持弓字形继续规划覆盖路径了，则考虑跳到另一个区域进行规划。

  此时，需要依次遍历存储新地图无障碍点坐标的变量freeSpaceVec_中的点，查询该点的活力值，若活力值大于0，则找到了一个新的活跃的点，则进一步判断该点周围相邻的位置是否存在活力值为-250的点（即已经在覆盖路径中的点），若存在，则计算当前点与找到的这个活跃点之间的距离，如果这个距离小于当前的最小距离（初始化为一个较大值100000000），则更新最小距离及其索引，也就是在freeSpaceVec_中的位置，遍历完freeSpaceVec_中的点后，判断最小距离是否为初始化的100000000，若不是，则找到了距离当前点最近的活跃点，进行跳转，即更新当前点位置为该活跃点位置，并将其添加到路径点队列pathVec_中，继续在新的区域进行全覆盖路径的规划。若没有找到，则说明产生了自锁现象，已经完成对地图可行区域的全覆盖路径的规划，退出循环，结束规划。mainPlanningLoop()函数结束后，就得到了规划的全覆盖路径点存放在pathVec_中，然后赋值给GetPathInROS()中的变量cellvec，转换成合适的格式后以话题/path_planning_node /cleaning_plan_nodehandle/cleaning_path发布出去。

  3、执行覆盖阶段

  next_goal节点会接收上面发布的规划完成的全覆盖路径，然后依次发布这个全覆盖路径中的点作为目标点发送出去，当机器人当前位置与当前目标点位置小于阈值时，发送下一个。发送出去的目标点通过点对点的全局和局部规划算法进行跟踪来完成对地图的覆盖的，项目里面全局规划算法用的ROS提供的NavfnROS包中的Dijkstra算法。局部算法用的ROS提供的DWA算法。

---

  三、换地图测试效果

  我使用自己的地图对该开源程序进行测试，发现算法存在一些缺陷，并不能完成对地图中所以可行区域的探索，有很大的空白区域，如下所示：

---

---