論文：《Efficient Trajectory Planning for MultipleNon-holonomic Mobile Robots via PrioritizedTrajectory 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那部分內容。

傳送門：