牛耕分解+形态学分割全覆盖路径规划（一）

Arik (IoT)

已于 2024-09-13 17:37:24 修改

阅读量419

点赞数 12

文章标签：路径规划

于 2024-09-13 15:29:02 首次发布

本文链接：https://blog.csdn.net/qq_45770364/article/details/142211785

版权

先放个最终的效果图：

在这里插入图片描述

原文链接：牛耕分解+形态学分割全覆盖路径规划（一）

源码是开源ipa覆盖算法：ipa_coverage_planning
调试过程参考：ipa 覆盖算法测试

下文是我在该项目下的调试，主要用到了ipa_coverage_planning中的牛耕覆盖和形态学分割两个算法，ipa中还给出了很多其他的算法供大家参考

2024/9/11

由于对整个房间直接进行牛耕覆盖看起来效果不太好，而且也不符合扫地机器人的实际清扫过程，考虑先进行分区，然后对于单独房间分别进行覆盖；

由于采用先分区后覆盖的思路，所以我们需要考虑两个算法的输入输出条件，以及满足我们需求的逻辑：

从牛耕的输入出发，需要的重要的参数是：

room_map ：地图
starting_position_ ：起始坐标

针对地图，如果我们想一个房间一个房间进行规划的话，那么最好的就是输入的地图为单独的房间？单独的房间？如何处理？回想分区的结果，我们可以得到些什么：

① 带有标签的地图，所谓的标签，就是在原地图的基础上，房间被赋值为顺序的标号，也就是indexed_map
② 房间的中心点坐标（room_centers_x_values，room_centers_y_values），以及房间的四个顶点的坐标（xmin,xmax,ymin,ymax）

如果有这些数据的话，是不是可以考虑每遍历一个房间，将其余的房间全部填充0（不可到达），然后规划路径；ummmmmm显然不太好，不去考虑填充时计算量的庞大，最后生成的各个房间的路径该如何连接在一起？

有了上面的思考，思路变得清晰了，这里我们需要考虑两个问题：

如何单个房间进行牛耕覆盖，输入的地图应该是什么样的
两个房间的路径该如何连接起来

继续思考，先不去想输入的地图的格式，把问题转到starting_position_ 也就是起始坐标上，因为输入地图有笨办法（对标签地图进行处理，非当前房间置0）只需考虑优化，那么房间的起点该如何确定？
（为什么不用考虑房间的终点呢？因为两个房间连接的话只需要两个点就好，新房间的起点和旧房间的终点，终点就是覆盖路径的最后一个点可以由算法直接给出。有这两个点之后来一个A就可以连接两个房间）【补充：在做完之后才发现房间的终点也是十分重要的】
由于比较容易得到每个房间的边界点的坐标，同时也可以根据这个坐标得到房间的中心点（代码中考虑了机器人能否到达）那么我们是否可以以设定的机器人的初始点到第一个房间的中心点生成一条路径（A），然后随着路径寻找，属于房间范围的第一个点即为房间的初始点，理论可行！开始实践！

Step 1 ： 确定机器人初始坐标点，由于不知道放哪比较好，毕竟实际的初始坐标是根据基站或者说开机的位置确定的，所以这里设定为整个地图的最中心：

starting_position_.x = msg.info.origin.position.x +  msg.info.width * 0.5;
starting_position_.y = msg.info.origin.position.y +  msg.info.height * 0.5;

Step2 ： 了解牛耕覆盖调用的A库（因为A都差不多，不想去修改牛耕的代码，就直接拿它的A了），并使用A库去完成两点生成路径；

int index_room = 7;

cv::Point start_point = starting_position_;
cv::Point end_point(room_centers_x_values[index_room], room_centers_y_values[index_room]);
std::vector astar_path;

cv::Point start_point2;
if (start_point.x >= min_x_value_of_the_room[index_room] &&
    start_point.x <= max_x_value_of_the_room[index_room] && start_point.y >= min_y_value_of_the_room[index_room] &&
    start_point.y <= max_y_value_of_the_room[index_room])
{
    start_point2 = start_point;
}
else
{
    double path_length = 0.0;
    path_length = path_planner_.planPath(original_map, start_point, end_point, 1.0, 0.0, map_resolution_, robot_radius_, &astar_path);
    for (size_t i = 0; i < astar_path.size(); ++i) { // 检查点是否在房间的边界内 if (astar_path[i].x >= min_x_value_of_the_room[index_room] &&
            astar_path[i].x <= max_x_value_of_the_room[index_room] && astar_path[i].y >= min_y_value_of_the_room[index_room] &&
            astar_path[i].y <= max_y_value_of_the_room[index_room])
        {
            start_point2 = astar_path[i];
            astar_path.resize(i + 1);
            break;
        }
    }
}

step3： 将生成的start_point2点作为牛耕的起点，输入地图修改为除了该房间其余都为不可到达区域的地图，下图为单个房间的路径输出

可以看到效果并不好，因为我在从index_map转为room_map的时候直接将original_map的数据传入room_map中，实际效果并不好，应该先进行二值处理
在这里插入图片描述

step4： 将A*的路径插入到该路径中尝试连续两个房间的覆盖

下图是将房间进行二值处理后规划的路径；根据生成中的效果可以发现两个问题：

1、相邻两个房间连接的时候回穿墙，在单个房间规划的时候也存在穿墙现象；分析为牛耕带的A库并不好用，下一步想办法替换为自己的A库
2、房间0也就是右上角的房间会出问题，当我尝试访问房间0的时候就会出错，目前没找到导致问题出现的地点

2024/9/13

解决bug2 ，由于index_map的标签从1开始，但是room_centers_x_values从0开始，所以在room_map生成的时候应该判断 indexed_map.at(x, y) == index_room + 1 如下：

cv::Mat room_map = original_map.clone();
for (int x = 0; x < indexed_map.rows; ++x)
{
    for (int y = 0; y < indexed_map.cols; ++y)
    {
        if (indexed_map.at(x, y) == index_room + 1)
            room_map.at(x, y) = 255;
        else
            room_map.at(x, y) = 0;
    }
}

在这里插入图片描述

bug1问题： A改了半天的结果，用自己的A视觉效果比较差（但实际路径还可以）

自己的A*库的结果:
在这里插入图片描述

牛耕自带的A*库的结果：
在这里插入图片描述

！！！补充bug1： 找到问题所在了，① 我用A*去找下一个区域的中点，然后到达该区域的第一个点作为区域的起点这里，如何判定到达该区域出现了问题，我直接设定路径上的点满足区域范围是不对的，应该还包括该区域的标签值是该区域的标签（说的比较绕）这个条件；② 由于index_map与astar输出坐标相反，所以在判断是否满足区域的时候应该选取index_map(j,i)修改代码：

if (astar_path[i].x >= min_x_value_of_the_room[index_room] &&
    astar_path[i].x <= max_x_value_of_the_room[index_room] && astar_path[i].y >= min_y_value_of_the_room[index_room] &&
    astar_path[i].y <= max_y_value_of_the_room[index_room] &&
    indexed_map.at(astar_path[i].y, astar_path[i].x) == (index_room + 1))
{
    room_begin_point = astar_path[i];
    astar_path.resize(i + 1);
    break;
}

实现效果：可以看出不同房间之间的穿墙变得少了。但在单个房间内覆盖还是存在穿墙现象
在这里插入图片描述

使用改进的牛耕分解：

//boustrophedon_explorer_.getExplorationPath(room_map, oneRoom_exploration_path, map_resolution_, room_begin_point, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);
boustrophedon_variant_explorer_.getExplorationPath(room_map, oneRoom_exploration_path, map_resolution_, room_begin_point, map_origin_, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);

效果：（有优化吗，好像看不太出来）
在这里插入图片描述

设置 min_cell_area = 2，grid_obstacle_offset = 0.1，首先，这里的前提是我在进行牛耕前并没有对地图进行腐蚀和膨胀，所以并没有对安全距离进行考究；所以设置grid_obstacle_offset = 0.1 保持边界的安全距离，这样也为后续延边的路径留出空间，防止重复规划，效果：
在这里插入图片描述

2024/9/13 待解决问题：

牛耕覆盖存在的路径穿墙问题，可能是：

① ros发布的是path，所以两个点之间直接用直线连接了起来，并不是实际规划的路径
② 牛耕使用的A*存在问题，可能地图传入的时候出现了问题

验证情况①：

利用ros中的visualization_msgs::Marker将exploration_path发布为点集：

visualization_msgs::Marker points;
points.header.frame_id = "map"; // 使用你所在的坐标系
points.header.stamp = ros::Time::now();
points.ns = "exploration_points";
points.id = 0;
points.type = visualization_msgs::Marker::POINTS;
points.action = visualization_msgs::Marker::ADD;

// 设置点的尺寸
points.scale.x = 0.05; // 设置点的宽度
points.scale.y = 0.05; // 设置点的高度
points.color.r = 1.0f;
points.color.g = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0f;

// 将路径上的点添加到Marker中
for (const auto &pose : exploration_path)
{
    geometry_msgs::Point p;
    p.x = pose.x;
    p.y = pose.y;
    p.z = 0.0; // z 轴可以保持为 0，如果你的路径在平面上
    points.points.push_back(p);
}

// 发布Marker
marker_pub.publish(points);