全覆盖路径规划思想（1）

全覆盖路径规划

主要用于目前比较火的全自动扫地机、洗地机。需要要求机器人遍历一个地图中所有空间。本文章仅描述其基本原理。

输入条件

输入：

• 一张地图
• 清扫边界

    ox_outside = [0.0, 200.0, 200, 0.0, 0.0]     # 外边界，即清扫范围
    oy_outside = [0.0, 0.0,  60,  60.0, 0.0]

    ox_inside = [[50, 90, 75, 50],[100, 150, 130, 100],[160, 170, 130 , 160]]    # 内部障碍边界
    oy_inside = [[18, 48, 28, 18],[18 , 45,  28,  18] ,[20,  30,  10,   20]]

输出：

• 全覆盖路线

全局规划思想

1. 根据边界提取一张地图中所需覆盖范围，即清扫范围；
2. 根据外边界，提取最长边界，用于覆盖时的遍历的方向，并记录一个顶点，用于原点；

    max_dist = 0.0
    vec = [0.0, 0.0]
    sweep_start_pos = [0.0, 0.0]
    for i in range(len(ox) - 1):
        dx = ox[i + 1] - ox[i]
        dy = oy[i + 1] - oy[i]
        d = np.sqrt(dx ** 2 + dy ** 2)

        if d > max_dist:
            max_dist = d
            vec = [dx, dy]                 #最长边向量，即斜率
            ori_pos = [ox[i], oy[i]]       #最长边起始顶点

3. 根据最长边和一个顶点，将清扫边界和内部障碍物，进行坐标转换，即旋转和平移；将记录的一顶点，作为原点，将最长边作为X轴正方向，进行坐标转换。
4. 经过以上步骤，则目前清扫范围在x轴方向最长，因此为最适合遍历方向；
5. 构建栅格地图，将边界外以及障碍物内全部标记为1，而清扫区域标记为0；
6. 栅格地图进行膨胀，主要考虑机器人大小问题；
7. 栅格地图闭算法，主要是滤除掉
8. 定义清扫的主要方向，从下到上，第一次从左到右。（可更改，本文仅以此种情况说明）
9. 查找清扫起始点，方法是从栅格图最下一行从左到右进行遍历，查找第一个标记为0的栅格，则为起始坐标；
10. 查找终点，方法与9一样；
11. 从起点开始开始查找下一个点；
12. 判断是否为终点或者不存在下一个点，否则执行11步骤；
13. 记录每一个点坐标，根据原点和长边向量，还原坐标；
14. 同时9~12步可执行多次，如此可进行分区覆盖；采用python仿真效果图如下：

判断下一点

已知：

1. 栅格标记为0.5，表明已经走过；
2. 栅格标记为1，不能行走；
3. 栅格标记为0，表明需要清扫未清扫区域；
4. 初始位置为start_pos
5. 初始扫描方向为Direction_x = 1
   流程图如下：
   
   附python优化后的源码github，
   参考AtsushiSakai/PythonRobotics源工程github