1.CCPP整体算法文档

1.1 ccpp基础介绍

全路径覆盖算法（CCPP: Complete Coverage Path Planning）作为扫地机器人较为关键的组成部分，其问题的本质是：在栅格地图中,全覆盖路径规划问题就演变为寻找机器人的下一个移动位置,只有准确找出了该位置,才能使机器人自主规划出一条切实可行的无碰撞且重复率低的移动路径。

CCPP需解决的关键问题：

• 遍历工作区域内除障碍物以外的全部区域
• 在遍历过程中有效避开所有障碍物
• 在遍历过程中要尽量避免路径重复,缩短移动距离

CCPP技术指标：

• 区域覆盖率
• 路径重复率
• 总行程

路径规划方法分类（根据实现方法)

• 传统经典算法
  • 基于图的方法
  • 基于栅格的方法
  • 势场法
  • 数学编程法
• 智能方法
  • 横糊方法
  • 神经网络方法
  • 遗传算法

路径搜索要求

• 路径表达：以环境模型中的结点序列组成或由直线段序列组成
• 路径平滑：根据机器人运动学或动力学约束，形成机器人可跟踪执行的运动轨迹
  • 运动学约束：路径轨迹的一阶导数应连续
  • 动力学约束：路径轨迹的二阶导数应连续

1.2 ccpp输入输出

ccpp

1.3 ccpp所用代码库

目前ccpp环境搭建是基于ros环境的，同时基于Opencv去进行分析

1.3ccpp处理流程

1.4 ccpp代码思路

​ 该部分主要是对上文中的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处理结果

全路径覆盖结果图

全路径覆盖关键路径图

纯路径规划示意图

2. 核心代码

这里作者展示一下之前在编写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;
    }

2.1 后续改进(TODO)

1. 目前使用的是二维vector的形式来分析地图信息，后续可以考虑换为Eigen库来执行矩阵操作
2. 目前仍然是单独开一个launch去处理，未能融合到行为树中
3. 代码可读性仍需要提高，目前的代码缺乏扩展性

3. 参考文章

全覆盖路径规划算法(CCPP)

回溯算法

移动机器人全覆盖路径规划及仿真

BCD分割算法

基于生物激励神经网络方法的清扫机器人路径规划研究