扫地机器人：ROS-GAZEBO仿真系统与全覆盖规划算法_怎么将全局规划发送给机器人

目录

引言

ROS-GAZEBO仿真系统概述

 系统架构

地图预处理

全覆盖规划算法

局部规划算法

全局规划算法

决策

最终效果

---

引言

自己对于扫地机器人一直很感兴趣，也挺想去扫地机器人公司搬砖，奈何自己太菜了没人要，也一直想拥有一款自己的扫地机器人，奈何钱包干瘪，于是我开始着手搭建了一套强大的扫地机器人仿真系统，利用ROS-GAZEBO仿真环境，结合我自己编写的全覆盖规划算法和局部规划算法，成功实现了智能、高效的地图扫描与清理功能。本博客将详细介绍我的系统架构和关键技术

ROS-GAZEBO仿真系统概述

在实现自己的实物之前，我觉得应该把仿真系统先搭建好，上层算法都测试的差不多了，在着手设计机器人硬件， 这个仿真系统，我采用的是基于ros-gazebo， 自己手撸了一个urdf模型，当然你觉得困难你也可以现有的turtlebot，反正都是差速机器人。搭建好urdf后，为了让机器人好看点，我把身体和轮子的Visual标签部分替换成真实的扫地机模型（网上搜的），如下图

 系统架构

系统的架构参考的是ros的movebase结构图

地图预处理

 我自己也没有用过扫地机器人，只能根据自己的一些基本需求来去设计算法， 首先应该要有一个家！（真实的家我也买不起，只能在gazebo上自己搭建一个了- -）， 其次是可以根据环境进行建图， 这里我直接用的是gmapping功能包， 并且因为目前只是专注于实现整体的框架和算法，我把雷达传感器的误差设置的很小，所以建图效果就看起来不错。因为自己写的全覆盖算法是基于多边形的，所以这里还需要对栅格地图进行预处理，把边界和里面的多边形障碍物给提取出来并转换成多边形点集， 这个预处理部分直接用的是Opencv的开源算法，整体流程是：

1. 读取一个栅格地图，二值化，  去除噪声（我的地图噪声点比较少）， 对地图进行按照机器尺寸进行膨胀

2. 对地图进行开运算，闭运算，把连接比较近的障碍物，或者一些噪声点给去除掉

3. 用opencv的findContours找到所有的多边形轮廓

4. 把面积最大的作为外边界，其他的都是障碍物

这里随便贴一些预处理提取轮廓的效果图，效果一般般，但可以提取出来（- -）

 

 

全覆盖规划算法

 全覆盖规划算法是我系统的主要核心算法之一（只会这个），它使扫地机器人能够高效地清扫整个地图。这个部分的算法可以借鉴我的另外一篇博客，讲述的是室外机器人拖拉机的作业全覆盖，个人觉得扫地机，割草机，拖拉机的全覆盖算法其实都是差不多的，用的同一套核心算法，唯一不同的是掉头区域的处理，还有就是一些细枝末节，比如不可以重复走，不可以倒车，不可踩线之类，其他的不能出界，以及不能发生碰撞是一样的。这个部分算法主要逻辑如下

1. 对多边形分块，这里采用的方法是Cell decomposition，原理就是用一条自下而上的直线段，根据线段和多边形的交点数量变化来触发分块的逻辑， 这个分块方法取决于驾驶方向，目前是自己手动随机选择一个方向（- -有点蠢）

2. 分好块以后，每一个块会有进口和出口，把每一个字块的进和出口做成一张节点图，每一个节点之间的cost可以根据自己的需求计算，最简单就是欧氏距离， 把cost matrix计算好了以后可以用蚁群算法来求解最优的顺序， 这里也可以采用一些开源的tsp算法。

3. 提取蚁群算法的结果，恢复出所有的路线

话不多说，贴效果图

局部规划算法dwa源码https://github.com/Youhaochan/dwa-algorithm

路径都有了，当然少不了路径跟踪了是吧，室内扫地机器人或者清洁机器人的局部规划算法应该都是借鉴于ros的dwa算法或者teb算法吧，都有先成的，但是我本人对dwa算法比较了解，我不太了解ros的navigation功能包，不想去学习如何单独适用ros的dwa功能包，我就自己用c++手撸了一个dwa算法， 先撸一个cmake的版本，不依赖于ros的，方便调试，画图用的是matplotcpp， 其中障碍物搜索方面我特地去学习了kd_tree的用法（虽然我的障碍物就几十个，brute search也不影响速度）。这部分也少不了效果图！！

全局规划算法

扫地机器人可能在工作过程，或者在充电桩充电的时候，主人给他发一个指令，让他去定点干活，这个时候就需要根据已有的栅格地图规划处一条点到点的路径了，这部分我使用的是A星算法，这个算法已经非常成熟了，网络上也有很多图和先成算法，不过这部分我还是决定自己手撸，不然总觉得只是局限于理论，而不知道实现细节。

决策

扫地机要能正常工作，肯定少不了一个大脑，也就是决策部分，就像之前说的，我没有从事过相关工作，也没使用过扫地机，因此我就有点头疼该如何写这个决策部分的代码。 这个时候产品经理还是很重要的哈哈哈，我没有自己的产品经理，所以我选择去问chatgpt。gpt给的要求实在是太多了，我只能做到其中一两个，普通的机器人决策部分应该大多数是基于有限状态机来做决策的，也就是根据当前的状态，环境，结合指令做出相应的动作，都是程序员事先写好的逻辑。为了减少工作量，这部分我仅仅实现了，开始清洁，回充，检测电量，停止工作这几个状态。

最终效果

这里直接贴视频了

---

后续有优化在继续更新！