如果你想要讓機器人能幫你拿瓶子、做飯、收拾屋子等，就必須賦予機器人快速生成無碰撞、最優運動軌跡的能力，這就需要靠運動規划了。有人覺得運動規划已經很成熟了，無需再研究，但實際上，機械臂運動規划非常難……
如 果你想要讓機器人能幫你拿瓶子、做飯、收拾屋子等，就必須賦予機器人快速生成無碰撞、最優運動軌跡的能力，這就需要靠運動規划了。有人覺得運動規划已經很 成熟了，無需再研究，但實際上，機械臂運動規划非常難，之所以這么難，主要是因為規划問題的維度太高（具體后面分析），目前暫無兼顧實時性與最優性的規划 算法。
什么是運動規划（Motion Planning）
在上面四張圖片中，左上角是機器人在抓取桌上的東西，這是我們實驗室之前一個博士師兄的課題，主要就是機械臂通過軌跡規划抓取識別到的物體而不碰到 障礙物。右上角是蛋白質折疊過程，使用的是我們之后會提到的算法去規划它空間變化。左下角是《帝國時代2》的場景，我們在玩這類游戲的時候只需要點擊一個 目標點，游戲人物就會自行找到可行的路徑。最后一個是我們之前做過的一個機器人導航項目，通過激光雷達和算法機器人可在室內找到路徑。從這四張圖片我們可 以從中給運動規划得出一個定義：
-
在給定環境中，指定機器人起點與終點，計算出連接起點與終點，並滿足一定約束條件（如避障）的軌跡。
-
從數學角度上看，移動機器人的路徑規划（ Path Planning ）也屬於運動規划的范疇。但由於問題的維度不同，所以使用的算法也不同，大家習慣上將兩者區分開。
為什么研究路徑規划
- 社會老齡化
這是世界銀行發布的關於中日兩國國內生產總值（GDP）變化曲線圖，小圖是中日兩國的人口結構，可以看到2000年日本和中國2015年的人口結構 已經很接近了，所以未來中國勞動力數量會減少，我們必須提高平均勞動生產力，這樣才能防止GDP的增速減緩。機器人是可以解決這些問題的。
- 市場轉變
傳統工業機器人主要應用在汽車行業，而這個行業的特點是一個車型可以生產很多年，同時每台車的利潤也會相較較高，但是從目前來看機器人在汽車行業已 經基本飽和，所以大家的關注點開始轉向3C（Computer、Communication、ConsumerElectronics）行業。
3C產品具備這些特點：更新周期短、款式種類多、單件利潤低、整體市場大、勞動力成本增加、對自動化需要加大。
- 示教
現在我們工業機器人的使用方法通常是示教，即使像右圖采用拖動示教這種比較便利的方式，效率還是很低，因為每一台機器人的示教都需要人參與進來，而 且示教的路徑沒辦法應對其他一些環境的變化，尤其在3C行業你每次更新一次機型，我們就必須對流水線上所有的機器人重新示教，這樣的效率肯定是不夠高的。
- 加中間點
當然，目前有些機器人應用是加入了機器視覺等技術，就是在檢測之后讓機器人應對一些變化情況。左邊碼垛機器人就是通過視覺可以抓取東西，但它的路徑 是人工指定中間點。右邊是我做過的類似插秧機器人，原理與前面碼垛機器人類似。這類機器人想要在3C行業被靈活運用肯定是不行的，所以如果運動規划研究成 熟算法比較穩定的話，就可以用高級編程語言去編程，比如我們的指令讓它抓取零件A然后加工零件B的某一面，這種下達指令的方式就不需要每一步都示教了。
怎么做運動規划
對於規划器的評價標准，我們現在有兩個准則：
Optimality（最優性）: 路徑最短、規划速度最快等。
Complete（完備性）：在有限時間內解決所有有解問題。
然后，我們從最基礎的問題入手，也就是2維環境中的點狀機器人（point agent），點狀機器人是沒有實體的。接下來介紹下點狀機器人的路徑規划算法都有哪些。
-
Walk To
直接朝着目標走，直到到達目標點為止。
很多 RPG 游戲就采用了這種簡單的算法
最優性，但不完備
-
優化算法（蟻群等）
類似最優控制
大部分情況下效果不錯，但復雜問題很容易陷入局部極值
不完備也不最優
-
人工勢場
在障礙物周圍建立排斥勢場
從起點到終點構建吸引勢場
采用梯度下降等方式求解
容易實現、效果很好
可以與控制結合
可能陷入局部極值
不完備且不最優
-
圖搜索算法
將問題描述成圖（節點+邊）
用圖搜索算法解決問題
Dijkstra、A*
在給定的圖中完備且最優
-
可視圖（Visibility Graph）
用封閉多面體描述障礙物
利用障礙物頂點間的連線構建一個圖（graph），之后用圖搜索算法求解
站在某個頂點上，環繞四周，把你能看到（無障礙物）的頂點連接起來
完備且最優
-
柵格化（Cell Decomposition）
按一定分辨率將地圖進行網格划分
用四連通或八連通規則建立網格圖
分辨率完備（Resolution Complete）且最優
-
隨機路圖法
PRM（Probabilistic Road Maps）
通過隨機采樣選取不碰撞的點
兩點連接采用簡單的局部規划器如 Walk to 算法
將起止點連入路圖
用圖搜索求解
概率完備且不最優
-
快速擴展隨機樹法
RRT（Randomly Exploring Randomized Trees）
基於樹狀結構的搜索算法
概率完備且不最優
前面我們講的都是2D點狀機器人的情況，現在我們想怎么把這些問題推廣到實際機器人上。實際機器人有兩個問題，一個是機器人不再是一個點，需要將機器人的體積考慮在內，另外，機器人的自由度更高，原本的算法是否都還可用？
-
C空間（理論基礎）
構形空間，Configuration Space
用向量描述機器人的構形
在C空間內，機器人是一個點
C 空間拓撲性質與笛卡爾坐標系下的情況不同——二自由度機械臂的C空間是一個圓環面
大部分機構（連續旋轉關節、平動關節等）形成的構形空間均是微分流形，任一點的鄰域均與歐式空間同態
微分流形：大部分算法效果與在笛卡爾坐標下效果相同
-
高維度*
蟻群等優化算法：收斂慢，更多局部極值點
可視圖法：在高維空間中，算法不成立
柵格法：理論上可行；但會計算量太大；對於一個六自由度機械臂，我們按照6°分辨率（已經是很低的分辨率了）划分網格，那么將會產生606 = 4.67 × 1010 個網格，單是對每個網格進行碰撞檢測（如果碰撞檢測速度為0.1ms），就需要1296小時。
一般在高於三維的問題上不使用該方法。
-
人工勢場
在 C 空間內建立勢場不方便
只對個別控制點進行計算，折算到每個關節上
不完備且不最優，但對於簡單的問題很實用
-
PRM 和 RRT
不需要知道 C 空間的具體情況，只對隨機采樣點進行碰撞檢測（判斷是否在 C 空間的可行區域內）
兩點之間采用簡單的局部規划器（如 Walk to）進行連接
PRM：獲得一個圖，采用圖搜索算法求解
RRT：獲得一個連接到終點的樹，反向搜索即可
在高維空間內可行，概率完備且不最優
現狀：主要使用 RRT 和 PRM 等 Sampling-based methods；這些算法計算的結果一般需要進行后處理（smoothing等）。
-
RRT 和 PRM 變種
C 空間
隨機采樣（各種采樣算法 T-RRT）
有效性判斷（如碰撞檢測算法 AABB、減少碰撞檢測 Lazy-RRT）
局部規划器連接（各種連接方法、重新連接 RRT，PRM）
…
-
RRT*
漸進最優
-
Informed RRT*
先驗知識——只在sub-problem下采樣
-
理論學習
Coursera: 賓大 Robotics: Computational Motion Planning （簡單編程）
Choset, Howie M. Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.
經典論文+編程實現經典算法。
-
實踐
ROS MoveIt!：http://moveit.ros.org/
容易上手+容易修改
前沿研究方向
理論現狀是，從運動學規划角度，給定足夠多的時間一定能夠最優且完備地求解到軌跡。從理論的角度而言，這個問題已經解決了。現在研究方向主要在這兩個方面，探索新問題和做一些實用化工作。
新問題
- 重規划 re-plan
這個算是蠻實用的，因為每次規划完執行的過程中會遇到環境變化的問題，這就需要在執行過程中重新規划。重新規划的路線與之前的路線是連接的，而不是中間停下來重新走。上面是RSS在2016年的研究，感興趣的可以了解下。
- 考慮系統動力學
理想狀態下機器人在運動規划下直接端一杯水到一個地方就行了，但實際情況下這個過程是有動力學在里面，如果不做任何處理，這個杯子會掉。所以，在考慮了動力學之后，重新進行運動規划，這時候杯子才不會掉。這個問題還是比較簡單的，因為你只需要把它變成一個約束就好了。
- 考慮接觸動力學
因為我沒有做這塊東西，所以不太清楚它是怎么運作的，但是這個問題是存在的，因為在規划的時候會跟環境接觸，例如這個機器人攀爬桿子然后落地，涉及 到整個身體動力學跟你身體運動的協調過程，這個工作是MIT計算機科學與人工智能實驗室在2014年的實驗。接觸動力學比傳統的單體動力學復雜很多，因為 我們不知道它接觸的碰撞摩擦力這些不好建模。
- 運動規划+任務規划
運動規划是指我給你一個大任務，你自動生成一些小任務。這是IROS在2016年的一個工作，它的目標是讓機器人到達對面這個點，而它的路徑被障礙 物擋住了，這個時候把運動規划加進來，從更高一個空間維度去求解這個問題。第一步，它把這個桌子往前推，發現桌子推不動的時候對任務進行重規划，然后規划 到去推這個桌子，然后發現執行的效果與預計的不一樣，所以它又生成新的任務，然后它拉開桌子之后就走到了對面實現了工作。這只是一個很簡單的demo，但 實際上生活中會遇到很多這樣的問題，比如我想從這個房間到另一個房間，而門是關着的，這個時候就需要把門打開。所以說，不是要給機器人生成很多子任務，而 是一個大任務，未來服務機器人想要做好這塊是必須要做的。
實用化
另外大部分時間大家都用在了實用化上，雖然說只要有足夠時間它一定能求解出來，但實際情況下我們不可能給它無限的時間。另外RRT這些算法生成軌跡 很奇怪， 你可以看右邊這個視頻，只是讓它敲這個東西它要畫一大圈，所以這也是一個問題，就是怎么優化它的軌跡。所以需要將研究領域好的算法往工業領域推，目前兩者 之間是存在很大缺口的。
- 軌跡復用（相對固定的動作）
這個工作是想辦法把舊的軌跡給用起來，通過人工的方式指定一個運動微元，也就是原始軌跡，等到了新的環境后再進行改變。當然，這個爬樓梯的過程，環 境和動作基本上都相同，所以可以在這個微元的基礎上進行改變。首先，通過變形的工作拉到現在起始點位置，部分起始點會重合，然后對這些新起始點進行重復利 用，它會形成一個好的軌跡。這個工作是Hauser et al在2008年發布的論文。現在存在的問題是運動微元必須由人工來指定，所以研究方向是由系統自動生成運動微元。
- 舊軌跡信息（相對固定的環境）
這是之前做的一個內容，比較簡單但在相對固定的環境比較好用。大概原理就是根據人工示教的路徑，通過高斯混合模型（GMM）對可行C空間進行建模， 之后在這個GMM-C空間內進行規划。這個方法有點類似Learning From Demonstration 的工作，但我只用了它們前面一半的步驟，后面一半還是采用采樣的方法。
- 加約束
這個是我針對加工過程做的另一個工作。我們在工業領域用機器人往往期望的不是整個機械臂的動作，而只是末端的動作。假設我要拋光一個面，首先我要對 末端進行規划，用CAD模型就可以計算實現；得到路徑后發給機器人，之后直接求逆解或者用雅克比迭代過去。當然，這種方法大部分時候夠用，但有時候也會遇 到奇異點或者碰到障礙物。我就是針對這個七軸的機械臂，利用它的一個冗余自由度進行規划。因為末端是固定的軌跡，這個時候，只要找到冗余自由度對應的C空 間流形，我們就可以在這么一個低維（2維）流形內進行很快速的規划，實現末端固定軌跡，且關節避障避奇異。
- 深度強化學習 DRL
我個人現在現在最關注的一塊，目前還沒有實質性的東西出來，在這里就和大家討論下，我覺得這一塊未來會出來不少的研究成果。
假設深度學習做運動規划，那么它進行一次運動規划的時間就是一次網絡正向傳播時間，這個時間非常短的，所以只要網絡訓練好后，運動規划需要耗費很長 時間的問題就沒有了。目前這塊也有一些這方面的研究，上面左邊圖是用深度學習玩游戲，Nature上的一篇論文，效果比人還厲害；右上角是谷歌用深度學習 來開門；右下角就是AlphaGo下圍棋了。這個是很有意思的，它也是運動規划和控制的問題，但它是用網絡來做的映射。
我為什么對這方面很感興趣呢？首先，CNN已經具備強大的環境理解能力，很容易從觀測估計狀態，觀測是圖片這類，而狀態，如果是物體識別，就是是什 么物體，如果是定位，那就是物體在什么地方。也就是說，在給定信息滿足系統狀態可觀性的前提下，CNN環境理解能力是非常強大的。
第二個就是RL（強化學習）可以進行路徑規划，通過 value iteration 等方式建立表格，這個表格紀錄的是從狀態到動作的映射。不過運動規划的維度這么高不可能用表格來存，所以可以通過神經網來解決這個映射問題。
