機械臂的運動規劃

若是你想要讓機器人能幫你拿瓶子、作飯、收拾屋子等，就必須賦予機器人快速生成無碰撞、最優運動軌跡的能力，這就須要靠運動規劃了。有人以爲運動規劃已經很成熟了，無需再研究，但實際上，機械臂運動規劃很是難……

如 果你想要讓機器人能幫你拿瓶子、作飯、收拾屋子等，就必須賦予機器人快速生成無碰撞、最優運動軌跡的能力，這就須要靠運動規劃了。有人以爲運動規劃已經很 成熟了，無需再研究，但實際上，機械臂運動規劃很是難，之因此這麼難，主要是由於規劃問題的維度過高（具體後面分析），目前暫無兼顧實時性與最優性的規劃 算法。

什麼是運動規劃（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 等方式創建表格，這個表格紀錄的是從狀態到動做的映射。不過運動規劃的維度這麼高不可能用表格來存，因此能夠經過神經網來解決這個映射問題。