扫地机/商用清洁-全覆盖路径规划-弓字/回字路径原理、开源项目解读、技巧、跑机效果

视频讲解

全覆盖路径规划-弓字回字路径原理、开源项目解读、技巧、跑机效果-扫地机商用清洁_哔哩哔哩_bilibili全覆盖路径规划-弓字回字路径原理、开源项目解读、技巧、跑机效果-扫地机、商用清洁机器算法;笔记文档：https://www.yuque.com/tmjtyj/ni9h6a/bfi5643274ewvlbu, 视频播放量 3130、弹幕量 0、点赞数 107、投硬币枚数 59、收藏人数 344、转发人数 42, 视频作者 tmjtyj, 作者简介 持续输入，持续输出，相关视频：HybridAstar原理、改进、技巧、代码实现、开源项目推荐和解读、应用案例，路径规划matlab全集，ROS小车 SLAM+导航+全覆盖路径规划，花了8888买的path planning教程！收藏，速，扫地机弓形全覆盖算法，全覆盖路径规划——应用于扫地机器人（C++版本），MATLAB实现全覆盖路径规划算法，路径规划与轨迹跟踪系列算法学习_第1讲_Dijkstra算法，全中国搞路径规划的研究生都进来看！不仅对路径规划，我们也要对心规划！，【转载】将牛耕式全覆盖路径规划作为广义旅行商问题重新审视https://www.bilibili.com/video/BV11H4y1B7PM/?spm_id_from=333.999.0.0【视频note】全覆盖路径规划-弓字/回字路径原理、开源项目解读、技巧、跑机效果-扫地机/商用清洁 · 语雀

1. 示例

弓字全覆盖示例（扫地机）

---

回字全覆盖示例（商用清洁）

---

2. 弓字路径规划

2.1. 原理

E:\[openFromGit]\Chap6-CellDecomp_howie(牛耕法原理).pdf

📎Chap6-CellDecomp_howie(牛耕法原理).pdf

牛耕弓字路径；

单调多边形;

牛耕区域的特点；

Boustrophedon cell decomposition

【【UIUC CS598】【计算拓扑学】001 Introduction, Jordan polygon theorem-哔哩哔哩】 【UIUC CS598】【计算拓扑学】001 Introduction, Jordan polygon theorem_哔哩哔哩_bilibili

2.1.1. 轮廓区域处理、轮廓树

pri_轮廓区域处理、轮廓树

对融合地图的轮廓查找和处理；

找到机器所在连通域的顶级轮廓和次级轮廓（孔洞），在进行牛耕分解；

2.1.2. 多边形的分解-开源项目推荐

TCD :trapezoid cell decomposition梯形分解

BCD：boustrophedon cell decomposition牛耕分解

几何算法：cgal有实现；

下面这个文件里面有提到这两种分解的方法实现；

polygon_coverage_planning/polygon_coverage_geometry/include/polygon_coverage_geometry/decomposition.h at master · ethz-asl/polygon_coverage_planning · GitHub

批量计算地图弓字路径

2.2. 开源项目推荐

---

D:\[openFromGit]\CoveragePlanner-master

这个repo使用了cgal的几何算法；

---

ipa全覆盖规划开源项目中的技术要点总结

ipa coverage path planning;

CCPP(complete coverage path planning)全覆盖路径规划；

ECPP(edge coverage path planning)边缘覆盖路径规划;(沿边，清洁，割草机)；

使用比较多的，ros package;

原理和实现步骤同上；

见批量计算地图弓字路径

3. 回字折线路径规划

3.1. 八点法边缘路径搜索

实现方法，参考xju _robot,对应有b站视频；

GitHub - Mr-Tony921/xju-robot: xju robot project for algorithm teaching.

实现步骤：

根据路径间距对地图做降采样；

改进：

对地图做分区；最好是凸分解；

void GetSpiral(cv::Mat neural_mat_, int start_x, int start_y, double angle, const char *name, cv::Mat &mapdata)
{
	std::vector<CellIndex> indexed_path;
	CellIndex init_point, next_point, cur_point;
	using dir_t = std::tuple<int, int, int>;
	std::vector<dir_t> directs{
		std::make_tuple(0, 0, 1),
		std::make_tuple(1, -1, 1),
		std::make_tuple(2, -1, 0),
		std::make_tuple(3, -1, -1),
		std::make_tuple(4, 0, -1),
		std::make_tuple(5, 1, -1),
		std::make_tuple(6, 1, 0),
		std::make_tuple(7, 1, 1)};
	// 初始点选择
	init_point.row = start_y;
	init_point.col = start_x;
	//
	init_point.theta = angle;
	indexed_path.emplace_back(init_point);
	cur_point = init_point;
	auto current_theta = static_cast<int>(init_point.theta / 45);
	//
	while (1)
	{
		neural_mat_.at<uint8_t>(cur_point.row, cur_point.col) = 0;
		auto rotate_dist = std::distance(directs.begin(),
										 std::find_if(directs.begin(), directs.end(),
													  [&](dir_t const &a)
													  {
														  return current_theta == std::get<0>(a);
													  })) -
						   static_cast<int>(directs.size() / 2);
		rotate_dist = (rotate_dist + static_cast<int>(directs.size())) % static_cast<int>(directs.size());
		std::rotate(directs.begin(), directs.begin() + rotate_dist, directs.end());

		auto find_near = false;
		for (auto const &dir : directs)
		{
			auto r = cur_point.row + std::get<1>(dir);
			auto c = cur_point.col + std::get<2>(dir);
			if (std::max(0, r) == std::min(r, neural_mat_.rows - 1) &&
				std::max(0, c) == std::min(c, neural_mat_.cols - 1) &&
				neural_mat_.at<uint8_t>(r, c) > 0)
			{
				find_near = true;
				next_point.row = r;
				next_point.col = c;
				next_point.theta = std::get<0>(dir) * 45;
				indexed_path.emplace_back(next_point);
				cur_point = next_point;
				current_theta = std::get<0>(dir);
				break;
			}
		}
		if (find_near)
			continue;

		// float min_dist = FLT_MAX;
		// float dist;
		// int ii = 0;
		// int min_index = -1;
		// for (auto const& fs : free_space_) {
		//   if (neural_mat_.at<float>(fs.row, fs.col) > 0) {
		//     dist = static_cast<float>((cur_point.row - fs.row) * (cur_point.row - fs.row)
		//                               + (cur_point.col - fs.col) * (cur_point.col - fs.col));
		//     if (dist < min_dist) {
		//       min_dist = dist;
		//       min_index = ii;
		//     }
		//   }

		//   ++ii;
		// }

		//
		bool find_min_cell = false;
		int min_dist = neural_mat_.rows * neural_mat_.rows + neural_mat_.cols * neural_mat_.cols;
		// int min_index = -1;
		CellIndex min_cell;
		{
			for (int i = 0; i < neural_mat_.rows; i++)
			{
				for (int j = 0; j < neural_mat_.cols; j++)
				{
					if (neural_mat_.at<uint8_t>(i, j) > 0)
					{
						auto dist = static_cast<int>((cur_point.row - i) * (cur_point.row - i) + (cur_point.col - j) * (cur_point.col - j));
						if (dist < min_dist)
						{
							find_min_cell = true;
							min_dist = dist;
							min_cell.col = j;
							min_cell.row = i;
						}
					}
				}
			}
		}
		if (find_min_cell == false)
		{
			//   ROS_WARN("The program has gone through %lu steps", indexed_path.size());
			std::cout << "The program has gone through" << indexed_path.size() << "steps" << std::endl;
			break;
		}

		next_point = min_cell;
		next_point.theta = atan2(next_point.col - cur_point.col, next_point.row - cur_point.row) / M_PI * 180;
		if (next_point.theta < 0)
		{
			next_point.theta = 360 + next_point.theta;
		}

		// ROS_DEBUG_STREAM("next point index: " << min_index << " "
		// 									  << "distance: " << std::sqrt(min_dist));
		// ROS_DEBUG_STREAM("current point: " << cur_point.row << ", " << cur_point.col);
		// ROS_DEBUG_STREAM("next point: " << next_point.row << ", " << next_point.col);
		cur_point = next_point;
		indexed_path.emplace_back(next_point);
		current_theta = 4;
	}
	// show path
	{
		{
			// 过滤indexed_path;
		}
		// 生成随机数
		RNG rng(time(0));
		using namespace cv;
		// 显示一下
		//        Mat trace_edge = Mat::zeros(neural_mat_.rows, neural_mat_.cols, CV_8UC1);
		Mat trace_edge = neural_mat_.clone();
		cv::imwrite("path_spiral_original.png", trace_edge);
		Mat trace_edge_color;
		cvtColor(trace_edge, trace_edge_color, cv::COLOR_GRAY2BGR);
		// 扩大四倍
		cv::resize(trace_edge_color, trace_edge_color, cv::Size(trace_edge_color.cols * 4, trace_edge_color.rows * 4), 0, 0, 1);
		uint8_t color_increase = 0;
		int dir = 1;
		CellIndex last_index = indexed_path.front();
		uint32_t index_num = 0;
		for (auto &item : indexed_path)
		{
			index_num++;
			//
			if (color_increase + dir > (int)255)
			{
				dir = -dir;
			}
			else if (color_increase + dir < (int)0)
			{
				dir = -dir;
			}
			color_increase = color_increase + dir;
			// Scalar color = Scalar(rng.uniform(0, 255 + color_increase), rng.uniform(0, 255), rng.uniform(0, 255));
			Scalar color = Scalar(color_increase, 255 - color_increase * 0.8, color_increase * 0.9);
			//            trace_edge_color.at<Vec3b>( item.row*4, item.col*4)[0] = color[0];
			//            trace_edge_color.at<Vec3b>(item.row*4, item.col*4)[1] = color[1];
			//            trace_edge_color.at<Vec3b>(item.row*4, item.col*4)[2] = color[2];
			cv::line(trace_edge_color, cv::Point(item.col * 4, item.row * 4), cv::Point(last_index.col * 4, last_index.row * 4), color);
			{
				//                auto pic_show = trace_edge_color.clone();
				//                cv::resize(pic_show, pic_show, cv::Size(trace_edge_color.cols * 4, trace_edge_color.rows * 4), 0, 0, 1);
				// imshow("path_spiral_colored", trace_edge_color);
			}
			last_index = item;
			// cv::waitKey(2);
		}
		{
			std::string savename(name);
			savename = savename + std::string("-spiral.png");
			cv::imwrite(savename.c_str(), trace_edge_color);
			imshow("path_spiral_colored", trace_edge_color);
		}
		// cv::imwrite("path_spiral_colored.png", trace_edge_color);
	}
	// show path 
}

见 批量计算回字折线路径

3.2. 改进，凸分解

4. 回字曲线路径规划

4.1. 使用回字折线加bezier曲线平滑

对外边缘不能保证无碰撞；

改进：

先沿墙；

将bezier控制点取密集；

4.2. 使用contourline

对白色区域腐蚀，提轮廓

5. 跑机效果

【扫地机】沿边与弓字全覆盖清扫-对接coppeliasim仿真环境呈现