zoukankan      html  css  js  c++  java
  • 2021.07.17-多车轨迹优化-实践

    论文:《Efficient Trajectory Planning for Multiple Non-holonomic Mobile Robots via Prioritized Trajectory Optimization》 Juncheng Li

    开源项目:https://github.com/gaows123/multi_robot_traj_planner

    主要步骤:

    需要的几个对象:

    //地图,这里是欧式距离地图
    std::shared_ptr<DynamicEDTOctomap> distmap_obj;
    //规划器 初始解规划器(作为基类,子类通过ECBS实现)
    std::shared_ptr<InitTrajPlanner> initTrajPlanner_obj;
    //走廊
    std::shared_ptr<Corridor> corridor_obj;
    //轨迹优化(这里起名MPC?)
    std::shared_ptr<MPCPlanner> MPCPlanner_obj;

    0. Build 3D Euclidean Distance Field

    用octomap构建地图,这里倒不是需要用到距离地图上的距离信息,还是作为栅格地图使用的。

    1. Plan Initial Trajectory

    涉及到3个.cpp

    • init_traj_planner.hpp(基类)
    • ecbs_planner.hpp(子类,处理地图等,使得满足ecbs的使用条件)
    • ecbs.cpp(在子类中直接调用本处的搜路算法)

    并没有用到作者论文中提到的那种前端可以考虑动力学的planner,而是直接用经典的ECBS在栅格地图上搜索无冲突路径。

    ECBS在栅格地图上求解的路径,可以看出,栅格尺寸就是右边的栅格尺寸,离散点也比较少;

    //构建初始解规划器,需要将距离地图和任务传入
    //注意这里的写法:基类.reset(new 子类)  ,这样的好处是,可以方便的用其他子类规划器 
    //基类.reset(new 子类2),  基类.reset(new 子类3), 基类.reset(new 子类4)
    initTrajPlanner_obj.reset(new ECBSPlanner(distmap_obj, mission, param));
    //这个是基类,其中update是纯虚函数
    initTrajPlanner_obj.get()->update(param.log)

    子类ECBSPlnanner,主要实现的功能有:

    1. 设置障碍物(相当于生成栅格地图)
    2. 设置起点终点信息
    3. 调用ECBS求解器
    class ECBSPlanner : public InitTrajPlanner{
    public:
        //构造函数
        ECBSPlanner(.......)
        {
            setObstacles();
            setWaypoints();
        }
    
        //子类重写基类函数
        bool update(bool log) override {
            //内部具体搜路直接用开源的ECBS
            bool success = ecbs.search(ecbs_startStates, solution, param.log);
        }    
    
    };

    可以看到,初始解的栅格地图,尺寸是非常大的,这个尺寸,对于仓库场景,只要能够覆盖到所有的库位,就可以了,不用想洗地车那样,为了覆盖率,设置栅格尺寸很小。

                       

    2.  Generate Safe Corridor

    这个走廊的生成方法,很有用,除了这里,感觉有可能用到洗地车全覆盖路径规划上(探索式的)。

    有时间可以跟高飞、李柏、刘思康等 几种走廊生成方法做个对比。

    //new一个指针
    corridor_obj.reset(new Corridor(initTrajPlanner_obj, distmap_obj, mission, param));
    //生成走廊
    corridor_obj.get()->update(param.log)

     主函数:

    bool update(bool log){
        return updateObsBox(log);
    }
    //走廊存储在vector中
    typedef std::vector<std::vector<std::pair<std::vector<double>, double>>> SFC_t;
    
    //遍历每个任务
    for (size_t qi = 0; qi < mission.qn; ++qi) {
        //上一个box,用于判断当前box是否完全在上一个box中
        std::vector<double> box_prev{0,0,0,0,0,0};
                double height=param.height;
                for (int i = 0; i < N-1; i++) {
                    //当前waypoint
                    double x = plan[qi][i][0];
                    double y = plan[qi][i][1];
                    double z = height;
                    //当前box
                    std::vector<double> box;
                    //下一个waypoint
                    x_next = plan[qi][i+1][0];
                    y_next = plan[qi][i+1][1];
                    z_next = height;
                    //判断当前两个waypoint是否在上一个box中
                    if(isPointInBox(octomap::point3d(x_next, y_next, z_next), box_prev)){
                        continue;
                    }
                    //初始化box,大小固定的
                    //检查障碍物是否在box中
                    //expand_box(box, mission.quad_size[qi]+0.1);
                    //box加入到走廊中
                    SFC[qi].emplace_back(std::make_pair(box, i+1));
    
                    box_prev = box;
    
                }
    
    }        

    3.  Formulate NLP problem and solving it to generate trajectory for the robot team

    这部分是核心,如何形成NLP问题,如何用Ipopt求解。

    MPCPlanner_obj.reset(new MPCPlanner(corridor_obj, initTrajPlanner_obj, mission, param));
    if (!MPCPlanner_obj.get()->update(param.log)) {
          return -1;
    }

    模型:

     本文是差速底盘,选择unicycle model。

    状态:

    控制输入:

    运动方程:

     约束:

     以及不与障碍物和其他agent碰撞

    先看一下构造函数里做了什么

    MPCPlanner() {
      //太多东西了,没有注释,感觉不可能看懂了。。。
    }
    bool update() {
        //一大堆计算,大概是在分组吧
        //分完组后,依次求解
        bool group_ok =Solve(i);
    }

    Ipopt求解部分还是很清晰的,步骤也不多:

    bool Solve(int index) { 
         //首先是一堆赋值
         //变量
         //约束
        //变量上下界限
        //约束上下界限
        //CppAD的求解步骤
    }

    4.  结论

    看了个寂寞,还是取复习一下优达学城里MPC那部分内容。

    传送门:

  • 相关阅读:
    大道至简——一个例子讲清楚观察者模式
    博客园界面代码风格自定义设置
    前端资源网址大集合
    如何检测浏览器是否安装了Adblock,uBlock Origin,Adguard,uBlock等广告屏蔽插件
    WebUploader 上传插件结合bootstrap的模态框使用时选择上传文件按钮无效问题的解决方法
    js中+号的另外一种用法
    C++ 最简单的日志类
    树莓派入门(3)—— 基本配置
    树莓派入门(2)——安装系统
    树莓派入门(1)——准备工作
  • 原文地址:https://www.cnblogs.com/gaowensheng/p/15023657.html
Copyright © 2011-2022 走看看