移动机器人全覆盖路径规划及仿真（四.全覆盖路径规划）
算法流程
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;
}

 

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

 

算法流程

 

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）
全覆盖路径规划
输入条件
全局规划思想
判断下一点

全覆盖路径规划
主要用于目前比较火的全自动扫地机、洗地机。需要要求机器人遍历一个地图中所有空间。本文章仅描述其基本原理。
输入条件
输入：

一张地图
清扫边界

    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]]

输出：

全覆盖路线

全局规划思想

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

    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]]       #最长边起始顶点


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


判断下一点
已知：

栅格标记为0.5，表明已经走过；
栅格标记为1，不能行走；
栅格标记为0，表明需要清扫未清扫区域；
初始位置为start_pos
初始扫描方向为Direction_x = 1

流程图如下：



附python优化后的源码github，

参考AtsushiSakai/PythonRobotics源工程github

移动机器人全覆盖路径规划及仿真（一.地图预处理）
算法步骤
1.地图预处理；

2.选取合适的遍历方向；

3.地图分割；

4.子区域连接与遍历；

5.建立机器人模型，并将路径点发送给机器人。
原始地图

全覆盖路径

gazebo仿真




地图处理流程



图2，用wall_contours围成的区域做mask。



图3，mask与原地图做加运算（两者都是黑色时为黑，其余均为白色）。



图4，用wall_contours围成的区域画黑色填充的多边形，以多边形每个顶点，机器人半径画白色圆。


图5，在上图基础上用Obstacle_contours

围成的区域画黑色填充的多边形。



图6，以多边形每个顶点，机器人半径画白色圆，地图腐蚀和膨胀。



图7，地图内Obstacle_contours。



图8，地图Wall_contours


void ExtractRawContours(const cv::Mat& original_map, std::vector<std::vector<cv::Point>>& raw_wall_contours, std::vector<std::vector<cv::Point>>& raw_obstacle_contours)
{
    cv::Mat map = original_map.clone();
    cv::threshold(map, map, 128, 255, cv::THRESH_BINARY_INV);

    cv::cvtColor(map, map, cv::COLOR_GRAY2BGR);


    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(original_map.clone(), contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    std::vector<int> wall_cnt_indices(contours.size());
    std::iota(wall_cnt_indices.begin(), wall_cnt_indices.end(), 0);

//    std::sort(wall_cnt_indices.begin(), wall_cnt_indices.end(), [&contours](int lhs, int rhs){return contours[lhs].size() > contours[rhs].size();});
    std::sort(wall_cnt_indices.begin(), wall_cnt_indices.end(), [&contours](int lhs, int rhs){return cv::contourArea(contours[lhs]) > cv::contourArea(contours[rhs]);});

    std::vector<cv::Point> raw_wall_contour = contours[wall_cnt_indices.front()];
    raw_wall_contours = {raw_wall_contour};

    cv::Mat mask = cv::Mat(original_map.size(), original_map.type(), 255);
    cv::fillPoly(mask, raw_wall_contours, 0);



    cv::Mat base = original_map.clone();
    base += mask;



    cv::threshold(base, base, 128, 255, cv::THRESH_BINARY_INV);



    cv::findContours(base, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    raw_obstacle_contours = contours;
}

void ExtractContours(const cv::Mat& original_map, std::vector<std::vector<cv::Point>>& wall_contours, std::vector<std::vector<cv::Point>>& obstacle_contours, int robot_radius=0)
{
    ExtractRawContours(original_map, wall_contours, obstacle_contours);

    if(robot_radius != 0)
    {
        cv::Mat3b canvas = cv::Mat3b(original_map.size(), CV_8U);
        canvas.setTo(cv::Scalar(255, 255, 255));


        cv::fillPoly(canvas, wall_contours, cv::Scalar(0, 0, 0));

        for(const auto& point:wall_contours.front())
        {
            cv::circle(canvas, point, robot_radius, cv::Scalar(255, 255, 255), -1);
        }



        cv::fillPoly(canvas, obstacle_contours, cv::Scalar(255, 255, 255));


        for(const auto& obstacle_contour:obstacle_contours)
        {
            for(const auto& point:obstacle_contour)
            {
                cv::circle(canvas, point, robot_radius, cv::Scalar(255, 255, 255), -1);
            }
        }



        cv::Mat canvas_;
        cv::cvtColor(canvas, canvas_, cv::COLOR_BGR2GRAY);
        cv::threshold(canvas_, canvas_, 200, 255, cv::THRESH_BINARY_INV);


        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(robot_radius,robot_radius), cv::Point(-1,-1));
        cv::morphologyEx(canvas_, canvas_, cv::MORPH_OPEN, kernel);



        ExtractRawContours(canvas_, wall_contours, obstacle_contours);


        std::vector<cv::Point> processed_wall_contour;
        cv::approxPolyDP(cv::Mat(wall_contours.front()), processed_wall_contour, 1, true);

        std::vector<std::vector<cv::Point>> processed_obstacle_contours(obstacle_contours.size());
        for(int i = 0; i < obstacle_contours.size(); i++)
        {
            cv::approxPolyDP(cv::Mat(obstacle_contours[i]), processed_obstacle_contours[i], 1, true);
        }

        wall_contours = {processed_wall_contour};
        obstacle_contours = processed_obstacle_contours;
    }
}