5
雷鋒網新智造按：運動規劃 (Motion Planning) ，有別于軌跡規劃 (Path Planning)。一般來說，軌跡規劃用于無人車/無人機領域，而運動規劃主要用于機械臂，類人機器人領域。關于運動規劃的具體定義，雷鋒網新智造本期公開課特邀請到上海交通大學機械與動力工程學院博士生邱強為我們做詳細的講解。邱強目前研究方向為機械臂運動規劃，除了講解什么是運動規劃，還會介紹他做的機器人規劃實例。同時，他還會講解目前關于運動規劃都有哪些前沿研究方向。
嘉賓介紹：
邱強，上海交通大學機械與動力工程學院博士生，本科畢業于清華大學機械工程系，目前研究方向為機械臂運動規劃。運營個人微信公眾號Nao (ID: qRobotics)，以qqfly（或fly qq）之名撰寫過多篇技術文章。
公開課完整視頻：
以下內容整理自邱強在雷鋒網硬創公開課的分享，由于內容有很多針對圖或視頻的細節講解，文中做了刪減，完整內容可觀看視頻。關注雷鋒網旗下微信公眾號「新智造」，回復「PPT」可獲取嘉賓完整PPT。
我們先來看這四個視頻。左上角是機器人在抓取桌上的東西，這是我們實驗室之前一個博士師兄的課題，主要就是機械臂通過軌跡規劃抓取識別到的物體而不碰到障礙物。右上角是蛋白質折疊過程，使用的是我們之后會提到的算法去規劃它空間變化。左下角是《帝國時代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 等方式建立表格，這個表格紀錄的是從狀態到動作的映射。不過運動規劃的維度這么高不可能用表格來存，所以可以通過神經網來解決這個映射問題。
Q：運動規劃、軌跡規劃、路徑規劃的區別與聯系？
A：這幾個概念在國內的教材里確實比較混亂，所以我按照我的理解大概說一下：①運動規劃（Motion Planning）就是我這次說的主要內容；②路徑規劃（Path Planning）跟運動規劃概念是一樣的，只是我們習慣上把它放在平面機器人上；③軌跡規劃（Trajectory Planning），主要是涉及速度、加速度插值，包括梯形速度曲線等底層的軌跡插值算法。
一般運動規劃后的結果是離散的路徑點，也會需要進行軌跡規劃后發給機器人。
Q：運動規劃中如何實現姿態轉接，角速度，角加速度的連續。
A：首先是姿態的連續變化問題：因為我們是在C空間（也就是關節空間進行的），所以不存在這個問題；如果是對無人機進行規劃（在SE(3)里），那么可以考慮看看四元數插值的算法。
第二個是角速度、角加速度連續；這個有兩種方法，一個是我只規劃角度，然后用軌跡規劃進行插值；另一個是我同時規劃速度和加速度（如優化算法），直接得到一個軌跡。
Q：移動機器人（例如AGV）的路徑規劃有什么推薦的軟件或者平臺嗎？
A：AGV的路徑規劃，我這邊做的不多，而且會根據你具體做的內容而不同（如SLAM、定位、導航等）。就我實驗室來說的話，在ROS下效果就不錯，可以用gmapping建圖、然后navigation導航。當然，如果ROS還是比較適合實驗室，商用的話我就不太了解了。
Q：學習機器人，一定要從運動學動力學軌跡規劃一步一步走過來嘛？
A：因為機器人涉及的內容真的很多，如果是做機械臂的話，我還是建議把基礎的東西完全弄清楚，包括空間運動、正逆解、動力學等。因為這些基礎知識里很多細節，如果直接跳過，后面會遇到很多問題。這方面打好基礎，后面就會輕松很多的。
Q：機械臂逆運動學推導和雅可比矩陣求解怎么在Ros上結合規劃算法來做出仿真？
A：其實我們規劃的過程中需要不斷進行運動學求解等。當然，ROS里面的話，我們可以先不關心這個。因為ROS里的KDL、ikFast等都可以幫我們做這些工作。當然了，由于MoveIt采用的是Plugin的形式，如果是你自己算的運動學，那可以做成Plugin放進去。之后MoveIt的規劃就是基于你的運動學進行計算了。
Q：請問動態環境下的運動規劃有什么研究熱點和方向？
A：這個有兩點，一個是剛才提到的重規劃re-plan問題，當環境變化或者任務變化的時候能在執行的過程中重新規劃出一個新的軌跡；另一個則是在規劃的過程中考慮動態情況：例如我們對無人車進行規劃的時候，就應該同時估計出周圍人、車的速度和運動趨勢，之后在此基礎上進行規劃。這樣可以避開運動的障礙物等。
雷峰網原創文章，未經授權禁止轉載。詳情見轉載須知。
專題
