論文：《Efficient Trajectory Planning for MultipleNon-holonomic Mobile Robots via PrioritizedTrajectory 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 trajectoryplanning 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 [email protected] 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 軌跡跟蹤控制方法來執行軌跡。