論文：《Efficient Trajectory Planning for Multiple Non-holonomic Mobile Robots via Prioritized Trajectory Optimization》 Juncheng Li
最近看到了好多用這種套路作軌跡優化的，無人機也有，無人車也有，單車也有，多車也有：
初始路徑-->安全走廊-->非線性/凸優化問題-->求解器求解
Introduction
本文提出了一種創新的方法，在富含障礙物的環境，為non-holonomic移動機器人多車生成避障的最優軌跡。
為了提高計算效率，decoupled planning方法被提出[6]，就是序列優先級。每個agent的軌跡需要避免與之前的agent的軌跡發生沖突。
文獻[7][8]提出的軌跡優化方法，利用sequential planning，顯著提高了計算效率。這些decoupled planning methods都很快，但是存在
的問題是--不完備。有些場景下的可行解，可能發現不了。
在富含障礙物的環境，單機軌跡規划有大量的研究，套路分為前端路徑規划和后端軌跡優化。（這里提到了很多高飛老師的研究。）
多機軌跡規划，也采用兩段的套路（可以保證完備），（這里提到的研究還是無人機的，無人車的研究這么少么？）
多無人機可以用quadratic program (QP) problem表示，然后優化。
無人機的多車軌跡規划，是作用在線性動力學linear dynamics的，因此軌跡優化可以表示為QP問題，這個是凸優化，容易求解。
但是在現代工業，很多差速驅動的機器人，是非線性動力學nonlinear dynamics，軌跡優化只能表示為非凸的非線性問題（NLP）。
分布式的規划方法，計算效率高，但是不能保證無死鎖並且在迷宮形的環境性能很差。基於上述問題的考慮，我們的方法可以生成安全，可行，接近最優的多機器人軌跡。
在前端和后端兩個階段，機器人的非線性運動模型都被考慮進去，以保證軌跡的可行。我們提出了一種優先級軌跡優化方法，使得計算效率提高，可以用於大規模的機器人群體。
本文是在連續空間集中式多non-holonomic機器人軌跡規划的第一次嘗試，主要貢獻如下：
- 大規划無滑移多機器人軌跡規划。
- 優先級優化方法提高了計算效率。（這個在李柏的書里也有專門的介紹）
問題描述：
0.碰撞模型
每個robot定義為一個半徑為R的圓形，根據圓形占據的柵格是否碰撞來確定。
如果一個圓形不夠精確，那可以用兩個圓形，這些是無人車里常用的方法。
1.初始路徑構成waypoints
這里初始路徑是（x, y, theta）的集合。
2.kinematic model
本文采用差速輪的unicycle model，狀態為(x, y, theta)，機器人通過線性速度和角速度控制。
3.最大速度，最大角速度
看到這里，真正的差速輪是沒有轉彎半徑限制的，如果想限制，那只能在這里加了。v / w > R。
具體方法：
A. Discrete Path Planning
將占據柵格地圖轉化為一種2-D grid graph。相比於傳統的柵格地圖，我們考慮了位置和朝向。x,y變成了x, y, theta。
首先就要構建這個圖了。假設圖的柵格尺寸為D，dT表示機器人移動一個邊需要的時間。考慮到機器人的運動學約束：
確定了D和dT，就可以構建這個圖。
離散路徑的沖突處理，采用ECBS來處理。這里沖突定義兩類，一個是節點之間，一個是邊之間。（不是優先級嗎？怎么初始路徑還需要ECBS呢？可能優先級是針對軌跡優化的）
采用ECBS獲取無沖突的離散路徑（那就是說，作者把ECBS的下層搜索修改了？）
在軌跡優化過程，兩個robots之間每個時間步的路徑點距離都會被檢測，所以每一個robot的離散路徑長度都相等（這里想表達的應該是路徑短的robot到達終點后，持續存在！持續到跟最長的那個路徑相等）。
B.安全走廊構建：
這個安全走廊構建方法有很多，劉思康，高飛，李柏，港科大，等等，有很多構建方法。
每個robot的離散路徑，里邊即waypoints rk，每兩個waypoints之間的線段用Ik = < rk-1----->rk >表示。在每個線段附近生成無碰撞的凸多面體Sk。
需要滿足以下條件：
凸多面體跟障礙物沒有交集
相鄰多面體之間必須有交集
本文參考了《Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial》中關於走廊生成的方法。
分別在x軸和y軸方向擴張到最大空間。但是對角線的線段，是有可能沒有凸多面體的，需要加密waypoints，滿足一下條件即可確保走廊連續（即確定最大waypoints間隔）：
每一段重新分割成h小段, 路徑長度為M, 新的采樣包含waypoints個數H = 1 + h * ( M - 1 )
Ri是機器人碰撞半徑。
如果下一段線段被上一個凸多面體完全包圍，那就不用再求了。
C.軌跡優化問題：
問題描述：
一些參數：
簡化問題：
目標函數分為兩部分：
1.連個連續的控制輸入之間的差值需要懲罰；應該是這里是差分驅動，控制輸入為速度，所以沒有用比較常用的minimum snap。
2.最優軌跡與參考軌跡之間的差值。這里作者說的是因為初始的參考軌跡已經是可行路徑啦，所以要懲罰這個偏離。額，感覺這里有問題，沒必要貼近初始軌跡。
4.Prioritized Trajectory Optimization
對於大規模的機器人隊伍，上邊的問題求解是不夠有效的。我們提出了優先級的軌跡優化方法。
robots分為一些groups,優先級低的求解時，把高優先級的軌跡作為障礙物要避免碰撞。
如果優先級排的不好，可能導致低級的robot軌跡優化失敗，我們提出了分組和優先級排序的方法：
略
仿真：
A.實施細節
Ubuntu 16.04 with Intel i5-6300HQ @2.30GHz CPU and 12GB of RAM
地圖：OctoMap地圖表示占據柵格地圖
求解器：內點法非線性求解器IPOPT
參數：碰撞模型半徑0.15m，柵格圖尺寸D=1m（在這么大的柵格上是必須的嗎），移動一個edge耗時∆T=1.6s，最大線速度1m/s，最大角速度1rad/s。構建安全走廊時，每一段線分為h=5個小段。
B.計算效率和解質量
環境大下：10 m X 12 m,包含6個3 X 0.6m的貨架
ECBS次優系數：1.5
與coupled方法（不分優先級）作對比，可以看出，求解時間減少，但是最優性依然保證！！
C.成功率
實驗：
碰撞模型半徑：R=0.3m
最大線速度：0.6m/s
最大角速度：0.6m/s
移動一個edge的時間間隔：∆T=2.65s
其他參數同仿真
將優化的軌跡上傳，使用MPC-based 軌跡跟蹤控制方法來執行軌跡。