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

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

算法步骤

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;
    }
}