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那部分内容。

传送门：