论文：《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，主要实现的功能有：
- 设置障碍物（相当于生成栅格地图）
- 设置起点终点信息
- 调用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那部分内容。
传送门：