移动机器人全覆盖路径规划及仿真（四.全覆盖路径规划）

算法流程

1.规划到从初始点到第一个cell左上角路径；
2.深度优先搜索链接每一个cell;
3.对每个cell内做BoustrophedonPath全覆盖路径规划；
4.寻找进入下一个区域入口；
5.寻找路口连接路径。

std::deque<std::deque<Point2D>> StaticPathPlanning(const cv::Mat& map, std::vector<CellNode>& cell_graph, const Point2D& start_point, int robot_radius, bool visualize_cells, bool visualize_path, int color_repeats=10)
{
    cv::Mat3b vis_map;
    cv::cvtColor(map, vis_map, cv::COLOR_GRAY2BGR);

    std::deque<std::deque<Point2D>> global_path;
    std::deque<Point2D> local_path;
    int corner_indicator = TOPLEFT;

    int start_cell_index = DetermineCellIndex(cell_graph, start_point).front();

    std::deque<Point2D> init_path = WalkInsideCell(cell_graph[start_cell_index], start_point, ComputeCellCornerPoints(cell_graph[start_cell_index])[TOPLEFT]);
    local_path.assign(init_path.begin(), init_path.end());

    std::deque<CellNode> cell_path = GetVisittingPath(cell_graph, start_cell_index);


    std::deque<cv::Scalar> JetColorMap;
    InitializeColorMap(JetColorMap, color_repeats);


    std::deque<Point2D> inner_path;
    std::deque<std::deque<Point2D>> link_path;
    Point2D curr_exit;
    Point2D next_entrance;

    std::deque<int> return_cell_path;
    std::deque<Point2D> return_path;

    for(int i = 0; i < cell_path.size(); i++)
    {
        inner_path = GetBoustrophedonPath(cell_graph, cell_path[i], corner_indicator, robot_radius);
        local_path.insert(local_path.end(), inner_path.begin(), inner_path.end());


        cell_graph[cell_path[i].cellIndex].isCleaned = true;

        if(i < (cell_path.size()-1))
        {
            curr_exit = inner_path.back();
            next_entrance = FindNextEntrance(curr_exit, cell_path[i+1], corner_indicator);
            link_path = FindLinkingPath(curr_exit, next_entrance, corner_indicator, cell_path[i], cell_path[i+1]);



            local_path.insert(local_path.end(), link_path.front().begin(), link_path.front().end());
            global_path.emplace_back(local_path);
            local_path.clear();
            local_path.insert(local_path.end(), link_path.back().begin(), link_path.back().end());
        }
    }
    global_path.emplace_back(local_path);

    if(visualize_cells||visualize_path)
    {
        cv::waitKey(0);
    }

    return global_path;
}

---------------------
作者：tutu_321
来源：CSDN
原文：https://blog.csdn.net/sinat_38625360/article/details/106125889
版权声明：本文为作者原创文章，转载请附上博文链接！
内容解析By：CSDN,CNBLOG博客文章一键转载插件

移动机器人全覆盖路径规划及仿真（四.全覆盖路径规划）

算法流程

1.规划到从初始点到第一个cell左上角路径；
2.深度优先搜索链接每一个cell;
3.对每个cell内做BoustrophedonPath全覆盖路径规划；
4.寻找进入下一个区域入口；
5.寻找路口连接路径。

std::deque<std::deque<Point2D>> StaticPathPlanning(const cv::Mat& map, std::vector<CellNode>& cell_graph, const Point2D& start_point, int robot_radius, bool visualize_cells, bool visualize_path, int color_repeats=10)
{
    cv::Mat3b vis_map;
    cv::cvtColor(map, vis_map, cv::COLOR_GRAY2BGR);

    std::deque<std::deque<Point2D>> global_path;
    std::deque<Point2D> local_path;
    int corner_indicator = TOPLEFT;

    int start_cell_index = DetermineCellIndex(cell_graph, start_point).front();

    std::deque<Point2D> init_path = WalkInsideCell(cell_graph[start_cell_index], start_point, ComputeCellCornerPoints(cell_graph[start_cell_index])[TOPLEFT]);
    local_path.assign(init_path.begin(), init_path.end());

    std::deque<CellNode> cell_path = GetVisittingPath(cell_graph, start_cell_index);


    std::deque<cv::Scalar> JetColorMap;
    InitializeColorMap(JetColorMap, color_repeats);


    std::deque<Point2D> inner_path;
    std::deque<std::deque<Point2D>> link_path;
    Point2D curr_exit;
    Point2D next_entrance;

    std::deque<int> return_cell_path;
    std::deque<Point2D> return_path;

    for(int i = 0; i < cell_path.size(); i++)
    {
        inner_path = GetBoustrophedonPath(cell_graph, cell_path[i], corner_indicator, robot_radius);
        local_path.insert(local_path.end(), inner_path.begin(), inner_path.end());


        cell_graph[cell_path[i].cellIndex].isCleaned = true;

        if(i < (cell_path.size()-1))
        {
            curr_exit = inner_path.back();
            next_entrance = FindNextEntrance(curr_exit, cell_path[i+1], corner_indicator);
            link_path = FindLinkingPath(curr_exit, next_entrance, corner_indicator, cell_path[i], cell_path[i+1]);



            local_path.insert(local_path.end(), link_path.front().begin(), link_path.front().end());
            global_path.emplace_back(local_path);
            local_path.clear();
            local_path.insert(local_path.end(), link_path.back().begin(), link_path.back().end());
        }
    }
    global_path.emplace_back(local_path);

    if(visualize_cells||visualize_path)
    {
        cv::waitKey(0);
    }

    return global_path;
}

全覆盖路径规划

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

输入条件

输入：

• 一张地图
• 清扫边界

    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