系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理
文章目录
- 系列文章目录
- 前言
- 算法原理
- 方法一：全地图进行牛耕覆盖步骤
- 方法二：区域分解地图进行牛耕覆盖步骤
- 凸多边形基于栅格地图的算法实现
- 优缺点
前言
认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！本文先对Boustrophedon Cellular Decomposition Path Planning用珊格地图生成每个cell的覆盖路径做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章
提示：以下是本篇文章正文内容
算法原理
单元内的覆盖路径通过牛耕法来规划。当遇到障碍物的时候按照“左、下、上、右”4个固定的优先级顺序，使机器人在室内进行全覆盖的移动。
沿着一条直线穿过整个田地,然后转身,沿着与前一条路径相邻的新直线路径前进。通过重复这个过程,保证覆盖整个田地。
基于图形地图牛耕覆盖实现与基于栅格地图牛耕覆盖实现的核心原理都是一样的
全地图牛耕覆盖VS区域分解牛耕覆盖
全地图进行牛耕覆盖会出现死点，当进行了区域单圈分解后，cell是一个凸多边形，一般不会到达一个死点位置
方法一：全地图进行牛耕覆盖步骤
单元内的覆盖路径通过牛耕法来规划。当遇到障碍物的时候按照“左、下、上、右”4个固定的优先级顺序，使机器人在室内进行全覆盖的移动。直至到达死点位置。重新选择新的角点直到把非占据栅格都填满
回溯机制：
（a）栅格回溯列表建立：关键角点
（b）回溯点的选择：欧式距离、最优路径距离，其它方案
方法二：区域分解地图进行牛耕覆盖步骤
凸多边形基于栅格地图的算法实现
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;
//计算连接每个cell中的路径local_path
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);
if(visualize_cells||visualize_path)
{
cv::namedWindow("map", cv::WINDOW_NORMAL);
cv::imshow("map", vis_map);
}
if(visualize_cells)
{
std::cout<<"cell graph has "<<cell_graph.size()<<" cells."<<std::endl;
for(int i = 0; i < cell_graph.size(); i++)
{
for(int j = 0; j < cell_graph[i].neighbor_indices.size(); j++)
{
std::cout<<"cell "<< i << "'s neighbor: cell "<<cell_graph[cell_graph[i].neighbor_indices[j]].cellIndex<<std::endl;
}
}
for(const auto& cell : cell_graph)
{
DrawCells(vis_map, cell);
cv::imshow("map", vis_map);
cv::waitKey(500);
}
}
std::deque<cv::Scalar> JetColorMap;
InitializeColorMap(JetColorMap, color_repeats);
if(visualize_path)
{
cv::circle(vis_map, cv::Point(start_point.x, start_point.y), 1, cv::Scalar(0, 0, 255), -1);
for(const auto& point : init_path)
{
vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(uchar(JetColorMap.front()[0]),uchar(JetColorMap.front()[1]),uchar(JetColorMap.front()[2]));
UpdateColorMap(JetColorMap);
cv::imshow("map", vis_map);
cv::waitKey(1);
}
}
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++)
{
计算单个cell中的覆盖路径
inner_path = GetBoustrophedonPath(cell_graph, cell_path[i], corner_indicator, robot_radius);
local_path.insert(local_path.end(), inner_path.begin(), inner_path.end());
if(visualize_path)
{
for(const auto& point : inner_path)
{
vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(uchar(JetColorMap.front()[0]),uchar(JetColorMap.front()[1]),uchar(JetColorMap.front()[2]));
UpdateColorMap(JetColorMap);
cv::imshow("map", vis_map);
cv::waitKey(1);
}
}
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]);
// for debugging
// std::cout<<std::endl;
// for(int i = 0; i < link_path.front().size(); i++)
// {
// int idx = DetermineCellIndex(cell_graph, link_path.front()[i]).front();
// std::cout<<"point lies in curr cell "<<idx<<std::endl;
// }
//
// for(int i = 0; i < link_path.back().size(); i++)
// {
// int idx = DetermineCellIndex(cell_graph, link_path.back()[i]).front();
// std::cout<<"point lies in next cell "<<idx<<std::endl;
// }
// std::cout<<std::endl;
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());
if(visualize_path)
{
for(const auto& point : link_path.front())
{
// vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(255, 255, 255);
vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(uchar(JetColorMap.front()[0]),uchar(JetColorMap.front()[1]),uchar(JetColorMap.front()[2]));
UpdateColorMap(JetColorMap);
cv::imshow("map", vis_map);
cv::waitKey(1);
}
for(const auto& point: link_path.back())
{
// vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(255, 255, 255);
vis_map.at<cv::Vec3b>(point.y, point.x)=cv::Vec3b(uchar(JetColorMap.front()[0]),uchar(JetColorMap.front()[1]),uchar(JetColorMap.front()[2]));
UpdateColorMap(JetColorMap);
cv::imshow("map", vis_map);
cv::waitKey(1);
}
}
}
}
global_path.emplace_back(local_path);
if(visualize_cells||visualize_path)
{
cv::waitKey(0);
}
return global_path;
}
std::vector<int> DetermineCellIndex(std::vector<CellNode>& cell_graph, const Point2D& point)
{
std::vector<int> cell_index;
for(int i = 0; i < cell_graph.size(); i++)
{
for(int j = 0; j < cell_graph[i].ceiling.size(); j++)
{
if(point.x == cell_graph[i].ceiling[j].x && point.y >= cell_graph[i].ceiling[j].y && point.y <= cell_graph[i].floor[j].y)
{
cell_index.emplace_back(int(i));
}
}
}
return cell_index;
}
std::deque<Point2D> WalkInsideCell(CellNode cell, const Point2D& start, const Point2D& end)
{
std::deque<Point2D> inner_path = {start};
int start_ceiling_index_offset = start.x - cell.ceiling.front().x;
int first_ceiling_delta_y = cell.ceiling[start_ceiling_index_offset].y - start.y;
int end_ceiling_index_offset = end.x - cell.ceiling.front().x;
int second_ceiling_delta_y = end.y - cell.ceiling[end_ceiling_index_offset].y;
int start_floor_index_offset = start.x - cell.floor.front().x;
int first_floor_delta_y = cell.floor[start_floor_index_offset].y - start.y;
int end_floor_index_offset = end.x - cell.floor.front().x;
int second_floor_delta_y = end.y - cell.floor[end_floor_index_offset].y;
if((abs(first_ceiling_delta_y)+abs(second_ceiling_delta_y)) < (abs(first_floor_delta_y)+abs(second_floor_delta_y))) //to ceiling
{
int first_increment_y = 0;
if(first_ceiling_delta_y != 0)
{
first_increment_y = first_ceiling_delta_y / abs(first_ceiling_delta_y);
for(int i = 1; i <= abs(first_ceiling_delta_y); i++)
{
inner_path.emplace_back(Point2D(start.x, start.y+(first_increment_y*i)));
}
}
int delta_x = cell.ceiling[end_ceiling_index_offset].x - cell.ceiling[start_ceiling_index_offset].x;
int increment_x = 0;
if(delta_x != 0)
{
increment_x = delta_x / abs(delta_x);
}
for(int i = 0; i < abs(delta_x); i++)
{
// 提前转
if((cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].y-cell.ceiling[start_ceiling_index_offset+increment_x*(i)].y>=2)
&&(i+1 <= abs(delta_x))
&&(i <= abs(delta_x)))
{
int delta = cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].y-cell.ceiling[start_ceiling_index_offset+increment_x*(i)].y;
int increment = delta/abs(delta);
for(int j = 0; j <= abs(delta); j++)
{
inner_path.emplace_back(Point2D(cell.ceiling[start_ceiling_index_offset+increment_x*i].x, cell.ceiling[start_ceiling_index_offset+increment_x*i].y+increment*(j)));
}
}
// 滞后转
else if((cell.ceiling[start_ceiling_index_offset+increment_x*(i)].y-cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].y>=2)
&&(i<=abs(delta_x))
&&(i+1<=abs(delta_x)))
{
inner_path.emplace_back(cell.ceiling[start_ceiling_index_offset+increment_x*(i)]);
int delta = cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].y-cell.ceiling[start_ceiling_index_offset+increment_x*(i)].y;
int increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
inner_path.emplace_back(Point2D(cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].x, cell.ceiling[start_ceiling_index_offset+increment_x*(i+1)].y+abs(delta)+increment*(k)));
}
}
else
{
inner_path.emplace_back(cell.ceiling[start_ceiling_index_offset+(increment_x*i)]);
}
}
int second_increment_y = 0;
if(second_ceiling_delta_y!=0)
{
second_increment_y = second_ceiling_delta_y/abs(second_ceiling_delta_y);
for(int i = 1; i <= abs(second_ceiling_delta_y); i++)
{
inner_path.emplace_back(Point2D(cell.ceiling[end_ceiling_index_offset].x, cell.ceiling[end_ceiling_index_offset].y+(second_increment_y*i)));
}
}
}
else // to floor
{
int first_increment_y = 0;
if(first_floor_delta_y != 0)
{
first_increment_y = first_floor_delta_y / abs(first_floor_delta_y);
for(int i = 1; i <= abs(first_floor_delta_y); i++)
{
inner_path.emplace_back(Point2D(start.x, start.y+(first_increment_y*i)));
}
}
int delta_x = cell.floor[end_floor_index_offset].x - cell.floor[start_floor_index_offset].x;
int increment_x = 0;
if(delta_x != 0)
{
increment_x = delta_x / abs(delta_x);
}
for(int i = 0; i < abs(delta_x); i++)
{
//提前转
if((cell.floor[start_floor_index_offset+increment_x*(i)].y-cell.floor[start_floor_index_offset+increment_x*(i+1)].y>=2)
&&(i<=abs(delta_x))
&&(i+1<=abs(delta_x)))
{
int delta = cell.floor[start_floor_index_offset+increment_x*(i+1)].y-cell.floor[start_floor_index_offset+increment_x*(i)].y;
int increment = delta/abs(delta);
for(int j = 0; j <= abs(delta); j++)
{
inner_path.emplace_back(Point2D(cell.floor[start_floor_index_offset+increment_x*(i)].x, cell.floor[start_floor_index_offset+increment_x*(i)].y+increment*(j)));
}
}
//滞后转
else if((cell.floor[start_floor_index_offset+increment_x*(i+1)].y-cell.floor[start_floor_index_offset+increment_x*(i)].y>=2)
&&(i+1<=abs(delta_x))
&&(i<=abs(delta_x)))
{
inner_path.emplace_back(Point2D(cell.floor[start_floor_index_offset+increment_x*(i)].x, cell.floor[start_floor_index_offset+increment_x*(i)].y));
int delta = cell.floor[start_floor_index_offset+increment_x*(i+1)].y-cell.floor[start_floor_index_offset+increment_x*(i)].y;
int increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
inner_path.emplace_back(Point2D(cell.floor[start_floor_index_offset+increment_x*(i+1)].x, cell.floor[start_floor_index_offset+increment_x*(i+1)].y-abs(delta) +increment*(k)));
}
}
else
{
inner_path.emplace_back(cell.floor[start_floor_index_offset+(increment_x*i)]);
}
}
int second_increment_y = 0;
if(second_floor_delta_y!=0)
{
second_increment_y = second_floor_delta_y/abs(second_floor_delta_y);
for(int i = 1; i <= abs(second_floor_delta_y); i++)
{
inner_path.emplace_back(Point2D(cell.floor[end_floor_index_offset].x, cell.floor[end_floor_index_offset].y+(second_increment_y*i)));
}
}
}
return inner_path;
}
std::vector<Point2D> ComputeCellCornerPoints(const CellNode& cell)
{
Point2D topleft = cell.ceiling.front();
Point2D bottomleft = cell.floor.front();
Point2D bottomright = cell.floor.back();
Point2D topright = cell.ceiling.back();
// 按照TOPLEFT、BOTTOMLEFT、BOTTOMRIGHT、TOPRIGHT的顺序储存corner points（逆时针）
std::vector<Point2D> corner_points = {topleft, bottomleft, bottomright, topright};
return corner_points;
}
void InitializeColorMap(std::deque<cv::Scalar>& JetColorMap, int repeat_times)
{
for(int i = 0; i <= 255; i++)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(0, i, 255));
}
}
for(int i = 254; i >= 0; i--)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(0, 255, i));
}
}
for(int i = 1; i <= 255; i++)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(i, 255, 0));
}
}
for(int i = 254; i >= 0; i--)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(255, i, 0));
}
}
for(int i = 1; i <= 255; i++)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(255, 0, i));
}
}
for(int i = 254; i >= 1; i--)
{
for(int j = 0; j < repeat_times; j++)
{
JetColorMap.emplace_back(cv::Scalar(i, 0, 255));
}
}
}
//单个cell,boustrophedon生成覆盖路径
std::deque<Point2D> GetBoustrophedonPath(std::vector<CellNode>& cell_graph, CellNode cell, int corner_indicator, int robot_radius)
{
int delta, increment;
std::deque<Point2D> path;
std::vector<Point2D> corner_points = ComputeCellCornerPoints(cell);
std::vector<Point2D> ceiling, floor;
ceiling.assign(cell.ceiling.begin(), cell.ceiling.end());
floor.assign(cell.floor.begin(), cell.floor.end());
if(cell_graph[cell.cellIndex].isCleaned)
{
if(corner_indicator == TOPLEFT)
{
path.emplace_back(corner_points[TOPLEFT]);
}
if(corner_indicator == TOPRIGHT)
{
path.emplace_back(corner_points[TOPRIGHT]);
}
if(corner_indicator == BOTTOMLEFT)
{
path.emplace_back(corner_points[BOTTOMLEFT]);
}
if(corner_indicator == BOTTOMRIGHT)
{
path.emplace_back(corner_points[BOTTOMRIGHT]);
}
}
else
{
if(corner_indicator == TOPLEFT)
{
int x, y, y_start, y_end;
bool reverse = false;
for(int i = 0; i < ceiling.size(); i = i + (robot_radius+1))
{
x = ceiling[i].x;
if(!reverse)
{
y_start = ceiling[i].y;
y_end = floor[i].y;
for(y = y_start; y <= y_end; y++)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(floor[i+1].y-floor[i].y)>=2)&&(i+1<floor.size()))
{
delta = floor[i+1].y-floor[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i].x, floor[i].y + increment * (k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着floor从左往右
if( x+j >= floor.back().x)
{
i = i - (robot_radius - (j - 1));
break;
}
//提前转
else if((floor[i+(j)].y-floor[i+(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
delta = floor[i+(j+1)].y-floor[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i+(j)].x, floor[i+(j)].y+increment*(k)));
}
}
//滞后转
else if((floor[i+(j+1)].y-floor[i+(j)].y>=2)
&&(j+1<=robot_radius+1)
&&(j<=robot_radius+1))
{
path.emplace_back(Point2D(floor[i+(j)].x, floor[i+(j)].y));
delta = floor[i+(j+1)].y-floor[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i+(j+1)].x, cell.floor[i+(j+1)].y-abs(delta) +increment*(k)));
}
}
else
{
path.emplace_back(floor[i+(j)]);
}
}
}
reverse = !reverse;
}
else
{
y_start = floor[i].y;
y_end = ceiling[i].y;
for (y = y_start; y >= y_end; y--)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(ceiling[i+1].y-ceiling[i].y)>=2)&&(i+1<ceiling.size()))
{
delta = ceiling[i+1].y-ceiling[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i].x, ceiling[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着ceiling从左往右
if(x+j >= ceiling.back().x)
{
i = i - (robot_radius - (j - 1));
break;
}
// 提前转
else if((ceiling[i+(j+1)].y-ceiling[i+(j)].y>=2)
&&(j+1 <= robot_radius+1)
&&(j <= robot_radius+1))
{
delta = ceiling[i+(j+1)].y-ceiling[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i+j].x, ceiling[i+j].y+increment*(k)));
}
}
// 滞后转
else if((ceiling[i+(j)].y-ceiling[i+(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
path.emplace_back(ceiling[i+(j)]);
delta = ceiling[i+(j+1)].y-ceiling[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i+(j+1)].x, ceiling[i+(j+1)].y+abs(delta)+increment*(k)));
}
}
else
{
path.emplace_back(ceiling[i+j]);
}
}
}
reverse = !reverse;
}
}
}
if(corner_indicator == TOPRIGHT)
{
int x=0, y=0, y_start=0, y_end=0;
bool reverse = false;
for(int i = ceiling.size()-1; i >= 0; i=i-(robot_radius+1))
{
x = ceiling[i].x;
if(!reverse)
{
y_start = ceiling[i].y;
y_end = floor[i].y;
for(y = y_start; y <= y_end; y++)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(floor[i-1].y-floor[i].y)>=2)&&(i-1>=0))
{
delta = floor[i-1].y-floor[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i].x, floor[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着floor从右往左
if(x-j <= floor.front().x)
{
i = i + (robot_radius - (j - 1));
break;
}
//提前转
else if((floor[i-(j)].y-floor[i-(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
delta = floor[i-(j+1)].y-floor[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i-(j)].x, floor[i-(j)].y+increment*(k)));
}
}
//滞后转
else if((floor[i-(j+1)].y-floor[i-(j)].y>=2)
&&(j+1<=robot_radius+1)
&&(j<=robot_radius+1))
{
path.emplace_back(Point2D(floor[i-(j)].x, floor[i-(j)].y));
delta = floor[i-(j+1)].y-floor[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i-(j+1)].x, cell.floor[i-(j+1)].y-abs(delta) +increment*(k)));
}
}
else
{
path.emplace_back(floor[i-(j)]);
}
}
}
reverse = !reverse;
}
else
{
y_start = floor[i].y;
y_end = ceiling[i].y;
for (y = y_start; y >= y_end; y--)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(ceiling[i-1].y-ceiling[i].y)>=2)&&(i-1>=0))
{
delta = ceiling[i-1].y-ceiling[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i].x, ceiling[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着ceiling从右往左
if( x-j <= ceiling.front().x)
{
i = i + (robot_radius - (j - 1));
break;
}
// 提前转
else if((ceiling[i-(j+1)].y-ceiling[i-(j)].y>=2)
&&(j+1 <= robot_radius+1)
&&(j <= robot_radius+1))
{
delta = ceiling[i-(j+1)].y-ceiling[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i-j].x, ceiling[i-j].y+increment*(k)));
}
}
// 滞后转
else if((ceiling[i-(j)].y-ceiling[i-(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
path.emplace_back(ceiling[i-(j)]);
delta = ceiling[i-(j+1)].y-ceiling[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i-(j+1)].x, ceiling[i-(j+1)].y+abs(delta)+increment*(k)));
}
}
else
{
path.emplace_back(ceiling[i-j]);
}
}
}
reverse = !reverse;
}
}
}
if(corner_indicator == BOTTOMLEFT)
{
int x=0, y=0, y_start=0, y_end=0;
bool reverse = false;
for(int i = 0; i < ceiling.size(); i=i+(robot_radius+1))
{
x = ceiling[i].x;
if(!reverse)
{
y_start = floor[i].y;
y_end = ceiling[i].y;
for(y = y_start; y >= y_end; y--)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(ceiling[i+1].y-ceiling[i].y)>=2)&&(i+1<ceiling.size()))
{
delta = ceiling[i+1].y-ceiling[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i].x, ceiling[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着ceiling从左往右
if(x+j >= ceiling.back().x)
{
i = i - (robot_radius - (j - 1));
break;
}
// 提前转
else if((ceiling[i+(j+1)].y-ceiling[i+(j)].y>=2)
&&(j+1 <= robot_radius+1)
&&(j <= robot_radius+1))
{
delta = ceiling[i+(j+1)].y-ceiling[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i+j].x, ceiling[i+j].y+increment*(k)));
}
}
// 滞后转
else if((ceiling[i+(j)].y-ceiling[i+(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
path.emplace_back(ceiling[i+(j)]);
delta = ceiling[i+(j+1)].y-ceiling[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i+(j+1)].x, ceiling[i+(j+1)].y+abs(delta)+increment*(k)));
}
}
else
{
path.emplace_back(ceiling[i+j]);
}
}
}
reverse = !reverse;
}
else
{
y_start = ceiling[i].y;
y_end = floor[i].y;
for (y = y_start; y <= y_end; y++)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(floor[i+1].y-floor[i].y)>=2)&&(i+1<floor.size()))
{
delta = floor[i+1].y-floor[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i].x, floor[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着floor从左往右
if(x+j >= floor.back().x)
{
i = i - (robot_radius - (j - 1));
break;
}
//提前转
else if((floor[i+(j)].y-floor[i+(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
delta = floor[i+(j+1)].y-floor[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i+(j)].x, floor[i+(j)].y+increment*(k)));
}
}
//滞后转
else if((floor[i+(j+1)].y-floor[i+(j)].y>=2)
&&(j+1<=robot_radius+1)
&&(j<=robot_radius+1))
{
path.emplace_back(Point2D(floor[i+(j)].x, floor[i+(j)].y));
delta = floor[i+(j+1)].y-floor[i+(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i+(j+1)].x, cell.floor[i+(j+1)].y-abs(delta) +increment*(k)));
}
}
else
{
path.emplace_back(floor[i+(j)]);
}
}
}
reverse = !reverse;
}
}
}
if(corner_indicator == BOTTOMRIGHT)
{
int x=0, y=0, y_start=0, y_end=0;
bool reverse = false;
for(int i = ceiling.size()-1; i >= 0; i=i-(robot_radius+1))
{
x = ceiling[i].x;
if(!reverse)
{
y_start = floor[i].y;
y_end = ceiling[i].y;
for(y = y_start; y >= y_end; y--)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(ceiling[i-1].y-ceiling[i].y)>=2)&&(i-1>=0))
{
delta = ceiling[i-1].y-ceiling[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i].x, ceiling[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着ceiling从右往左
if(x-j <= ceiling.front().x)
{
i = i + (robot_radius - (j - 1));
break;
}
// 提前转
else if((ceiling[i-(j+1)].y-ceiling[i-(j)].y>=2)
&&(j+1 <= robot_radius+1)
&&(j <= robot_radius+1))
{
delta = ceiling[i-(j+1)].y-ceiling[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i-j].x, ceiling[i-j].y+increment*(k)));
}
}
// 滞后转
else if((ceiling[i-(j)].y-ceiling[i-(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
path.emplace_back(ceiling[i-(j)]);
delta = ceiling[i-(j+1)].y-ceiling[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(ceiling[i-(j+1)].x, ceiling[i-(j+1)].y+abs(delta)+increment*(k)));
}
}
else
{
path.emplace_back(ceiling[i-j]);
}
}
}
reverse = !reverse;
}
else
{
y_start = ceiling[i].y;
y_end = floor[i].y;
for (y = y_start; y <= y_end; y++)
{
path.emplace_back(Point2D(x, y));
}
if((std::abs(floor[i-1].y-floor[i].y)>=2)&&(i-1>=0))
{
delta = floor[i-1].y-floor[i].y;
increment = delta/abs(delta);
for(int k = 1; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i].x, floor[i].y+increment*(k)));
}
}
if(robot_radius != 0)
{
for(int j = 1; j <= robot_radius+1; j++)
{
// 沿着floor从右往左
if(x-j <= floor.front().x)
{
i = i + (robot_radius - (j - 1));
break;
}
//提前转
else if((floor[i-(j)].y-floor[i-(j+1)].y>=2)
&&(j<=robot_radius+1)
&&(j+1<=robot_radius+1))
{
delta = floor[i-(j+1)].y-floor[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i-(j)].x, floor[i-(j)].y+increment*(k)));
}
}
//滞后转
else if((floor[i-(j+1)].y-floor[i-(j)].y>=2)
&&(j+1<=robot_radius+1)
&&(j<=robot_radius+1))
{
path.emplace_back(Point2D(floor[i-(j)].x, floor[i-(j)].y));
delta = floor[i-(j+1)].y-floor[i-(j)].y;
increment = delta/abs(delta);
for(int k = 0; k <= abs(delta); k++)
{
path.emplace_back(Point2D(floor[i-(j+1)].x, cell.floor[i-(j+1)].y-abs(delta) +increment*(k)));
}
}
else
{
path.emplace_back(floor[i-(j)]);
}
}
}
reverse = !reverse;
}
}
}
}
return path;
}
优缺点
牛耕覆盖算法在空旷环境能够提供漂亮的覆盖路径，但在复杂，混乱的环境中规划路径有些混乱。覆盖率偏高。
