|
1、CCPP基础介绍 全路径覆盖算法（CCPP：Complete Coverage Path Planning）作为扫地机器人较为关键的组成部分，其问题的本质是：在栅格地图中，全覆盖路径规划问题就演变为寻找机器人的下一个移动位置，只有准确找出了该位置，才能使机器人自主规划出一条切实可行的无碰撞且重复率低的移动路径。 CCPP需解决的关键问题： 遍历工作区域内除障碍物以外的全部区域 在遍历过程中有效避开所有障碍物 在遍历过程中要尽量避免路径重复，缩短移动距离
路径规划方法分类（根据实现方法) 3、CCPP所用代码库 目前CCPP环境搭建是基于ros环境的，同时基于Opencv去进行分析。 该部分主要是对上文中的binn.cpp以及AStar.cpp的代码思路解析： 1、获取以pgm格式保存的场景地图 2、根据机器人当前的位置获取自身所在地图的位置信息 3、将地图信息已数组的形式进行保存 4、更新当前栅格的浓度信息，并从八个方向进行浓度探测 5、判断当前位姿是否为可记录回溯点，如果是，则保存并等待回溯 6、选择最优方向前进，并重复第四、五步 7、如果八个方向均走过，则进入回溯，并寻找下一个需要清扫的区域 8、A*算法求得最优的路径点集 9、继续执行上述4-8步，直到回溯点集全部清空 CCPP使用教程： 目前CCPP仍处于调试阶段，并未生成动态链接库嵌入到整体行为树中。所以，目前在system_tests中开辟一个ccpp_test.launch来方便调试。只需要运行： roslaunch system_tests ccpp_test.launch
这里给大家展示一下之前我在编写CCPP当中写的核心代码，其效果比传统的遍历算法强上不少，非常适合扫地机的使用。这里我就讲最核心的代码部分贴出一部分吧！ bool CBinnMethod::Choose_optimal_path(vector<vector<float>> &neuronal_vec, Point &start_point, float &pre_direct){ int r = start_point.x + defRAD; int c = start_point.y + defRAD; vector<float> _f_cell_concentration; _f_cell_concentration.clear(); if (msg_vec[r][c] == -100) { neuronal_vec = Cal_cell_value(-E, neuronal_vec, r, c); } else if (msg_vec[r][c] == 100) { neuronal_vec = Cal_cell_value(E, neuronal_vec, r, c); } else if (msg_vec[r][c] == 0) { neuronal_vec = Cal_cell_value(0, neuronal_vec, r, c); } //这块主要是为了提前感知八个方向的情况 _f_cell_concentration.push_back(cell_concentration(r, c + 1, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r, c - 1, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r + 1, c, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r - 1, c, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r + 1, c + 1, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r - 1, c + 1, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r + 1, c - 1, neuronal_vec)); _f_cell_concentration.push_back(cell_concentration(r - 1, c - 1, neuronal_vec));
m_p_next_point = make_pair(-FLT_MAX, 0); for (size_t i = 0; i < _f_driect.size(); i++) { float theta = abs(_f_driect[i] - pre_direct) <= PI ? abs(_f_driect[i] - pre_direct) : 2 * PI - abs(_f_driect[i] - pre_direct); float y = 1 - theta / PI; int c_plus_rl = (sin(_f_driect[i]) > 0.01) ? 1 : (sin(_f_driect[i]) < -0.01) ? -1 : 0; float rl = c_plus_rl != 0 ? 1 : 0; float p_n = _f_cell_concentration[i] + C * y - rl * 0.005; //计算最优方向，同时加入rl权重来保证优先走弓字形 if (p_n > m_p_next_point.first) //找到最合适方向，浓度最大为下一个前进方向 { m_p_next_point = make_pair(p_n, _f_driect[i]); //cout << p_n << endl; } }
int r_plus = (cos(m_p_next_point.second) > 0.01) ? 1 : (cos(m_p_next_point.second) < -0.01) ? -1 : 0; int c_plus = (sin(m_p_next_point.second) > 0.01) ? 1 : (sin(m_p_next_point.second) < -0.01) ? -1 : 0;
Point pre_point = Point(r + r_plus, c + c_plus);
if (abs(m_p_next_point.second - pre_direct) > 0.01) //避免因为浓度问题导致无法保存点集 && neuronal_vec[pre_point.x][pre_point.y] >= 0.1) //代表存在方向变化,并且下一个时刻还未被占用建立回溯列表 { m_vp_recall.push_back(Point(r, c)); //记录当前点 next_point.push_back(start_point); //方向变化位置，记录该点 } else if ((neuronal_vec[pre_point.x][pre_point.y + 1] <= -0.1 && neuronal_vec[r][c + 1] >= 0) || (neuronal_vec[pre_point.x][pre_point.y - 1] <= -0.1 && neuronal_vec[r][c - 1] >= 0)) //如果未来的左边或者右边是障碍物，并且当前没有障碍物 { m_vp_recall.push_back(Point(r, c)); //记录当前点 } else if ((neuronal_vec[pre_point.x][pre_point.y + 1] >= 0 && neuronal_vec[r][c + 1] <= -0.1) || (neuronal_vec[pre_point.x][pre_point.y - 1] >= 0 && neuronal_vec[r][c - 1] <= -0.1)) //如果当前的左边或者右边是障碍物，并且当前存在障碍物 { m_vp_recall.push_back(Point(pre_point.x, pre_point.y)); //记录当前点 } else if (neuronal_vec[pre_point.x][pre_point.y] < 0.1) //已经被占用 { bool _occupy_cell_state = g_Recall_cloud(pre_point); if (_occupy_cell_state == false) { cout << 'end now!!!' << endl; return false; } }
start_point = Point(pre_point.x - defRAD, pre_point.y - defRAD);
cell_img.at<uchar>(start_point.x, start_point.y) = (uchar)128; origin_img.at<uchar>(start_point.x * CELL_SIZE + 3, start_point.y * CELL_SIZE + 3) = (uchar)128; pre_direct = m_p_next_point.second; Mat resize_img; resize(cell_img, resize_img, Size(400, 400)); imshow('output_cell', resize_img); imshow('output', origin_img); waitKey(1); return true; }
后续改进（TODO）： 1、目前使用的是二维vector的形式来分析地图信息，后续可以考虑换为Eigen库来执行矩阵操作。 2、目前仍然是单独开一个launch去处理，未能融合到行为树中。 3、代码可读性仍需要提高，目前的代码缺乏扩展性。
|
