雷鋒網(wǎng)新智造按：運動規(guī)劃 (Motion Planning) ，有別于軌跡規(guī)劃 (Path Planning)。一般來說，軌跡規(guī)劃用于無人車/無人機領(lǐng)域，而運動規(guī)劃主要用于機械臂，類人機器人領(lǐng)域。關(guān)于運動規(guī)劃的具體定義，雷鋒網(wǎng)新智造本期公開課特邀請到上海交通大學機械與動力工程學院博士生邱強為我們做詳細的講解。邱強目前研究方向為機械臂運動規(guī)劃，除了講解什么是運動規(guī)劃，還會介紹他做的機器人規(guī)劃實例。同時，他還會講解目前關(guān)于運動規(guī)劃都有哪些前沿研究方向。

嘉賓介紹：

邱強，上海交通大學機械與動力工程學院博士生，本科畢業(yè)于清華大學機械工程系，目前研究方向為機械臂運動規(guī)劃。運營個人微信公眾號Nao (ID: qRobotics)，以qqfly（或fly qq）之名撰寫過多篇技術(shù)文章。

公開課完整視頻：

以下內(nèi)容整理自邱強在雷鋒網(wǎng)硬創(chuàng)公開課的分享，由于內(nèi)容有很多針對圖或視頻的細節(jié)講解，文中做了刪減，完整內(nèi)容可觀看視頻。關(guān)注雷鋒網(wǎng)旗下微信公眾號「新智造」，回復(fù)「PPT」可獲取嘉賓完整PPT。

什么是運動規(guī)劃（Motion Planning）

我們先來看這四個視頻。左上角是機器人在抓取桌上的東西，這是我們實驗室之前一個博士師兄的課題，主要就是機械臂通過軌跡規(guī)劃抓取識別到的物體而不碰到障礙物。右上角是蛋白質(zhì)折疊過程，使用的是我們之后會提到的算法去規(guī)劃它空間變化。左下角是《帝國時代2》的場景，我們在玩這類游戲的時候只需要點擊一個目標點，游戲人物就會自行找到可行的路徑。最后一個是我們之前做過的一個機器人導(dǎo)航項目，通過激光雷達和算法機器人可在室內(nèi)找到路徑。從這四個視頻我們可以從中給運動規(guī)劃得出一個定義：

在給定環(huán)境中，指定機器人起點與終點，計算出連接起點與終點，并滿足一定約束條件（如避障）的軌跡。

從數(shù)學角度上看，移動機器人的路徑規(guī)劃（ Path Planning ）也屬于運動規(guī)劃的范疇。但由于問題的維度不同，所以使用的算法也不同，大家習慣上將兩者區(qū)分開。

為什么研究路徑規(guī)劃

社會老齡化

這是世界銀行發(fā)布的關(guān)于中日兩國國內(nèi)生產(chǎn)總值（GDP）變化曲線圖，小圖是中日兩國的人口結(jié)構(gòu)，可以看到2000年日本和中國2015年的人口結(jié)構(gòu)已經(jīng)很接近了，所以未來中國勞動力數(shù)量會減少，我們必須提高平均勞動生產(chǎn)力，這樣才能防止GDP的增速減緩。機器人是可以解決這些問題的。

市場轉(zhuǎn)變

傳統(tǒng)工業(yè)機器人主要應(yīng)用在汽車行業(yè)，而這個行業(yè)的特點是一個車型可以生產(chǎn)很多年，同時每臺車的利潤也會相較較高，但是從目前來看機器人在汽車行業(yè)已經(jīng)基本飽和，所以大家的關(guān)注點開始轉(zhuǎn)向3C（Computer、Communication、ConsumerElectronics）行業(yè)。

3C產(chǎn)品具備這些特點：更新周期短、款式種類多、單件利潤低、整體市場大、勞動力成本增加、對自動化需要加大。

示教

現(xiàn)在我們工業(yè)機器人的使用方法通常是示教，即使像右圖采用拖動示教這種比較便利的方式，效率還是很低，因為每一臺機器人的示教都需要人參與進來，而且示教的路徑?jīng)]辦法應(yīng)對其他一些環(huán)境的變化，尤其在3C行業(yè)你每次更新一次機型，我們就必須對流水線上所有的機器人重新示教，這樣的效率肯定是不夠高的。

加中間點

當然，目前有些機器人應(yīng)用是加入了機器視覺等技術(shù)，就是在檢測之后讓機器人應(yīng)對一些變化情況。左邊碼垛機器人就是通過視覺可以抓取東西，但它的路徑是人工指定中間點。右邊是我做過的類似插秧機器人，原理與前面碼垛機器人類似。這類機器人想要在3C行業(yè)被靈活運用肯定是不行的，所以如果運動規(guī)劃研究成熟算法比較穩(wěn)定的話，就可以用高級編程語言去編程，比如我們的指令讓它抓取零件A然后加工零件B的某一面，這種下達指令的方式就不需要每一步都示教了。

怎么做運動規(guī)劃

對于規(guī)劃器的評價標準，我們現(xiàn)在有兩個準則：

Optimality（最優(yōu)性）: 路徑最短、規(guī)劃速度最快等。

Complete（完備性）：在有限時間內(nèi)解決所有有解問題。

然后，我們從最基礎(chǔ)的問題入手，也就是2維環(huán)境中的點狀機器人（point agent），點狀機器人是沒有實體的。接下來介紹下點狀機器人的路徑規(guī)劃算法都有哪些。

Walk To

直接朝著目標走，直到到達目標點為止。

很多 RPG 游戲就采用了這種簡單的算法

最優(yōu)性，但不完備

優(yōu)化算法（蟻群等）

類似最優(yōu)控制

大部分情況下效果不錯，但復(fù)雜問題很容易陷入局部極值

不完備也不最優(yōu)

人工勢場

在障礙物周圍建立排斥勢場

從起點到終點構(gòu)建吸引勢場

采用梯度下降等方式求解

容易實現(xiàn)、效果很好

可以與控制結(jié)合

可能陷入局部極值

不完備且不最優(yōu)

圖搜索算法

將問題描述成圖（節(jié)點+邊）

用圖搜索算法解決問題

Dijkstra、A*

在給定的圖中完備且最優(yōu)

可視圖（Visibility Graph）

用封閉多面體描述障礙物

利用障礙物頂點間的連線構(gòu)建一個圖（graph），之后用圖搜索算法求解

站在某個頂點上，環(huán)繞四周，把你能看到（無障礙物）的頂點連接起來

完備且最

柵格化（Cell Decomposition）

按一定分辨率將地圖進行網(wǎng)格劃分

用四連通或八連通規(guī)則建立網(wǎng)格圖

分辨率完備（Resolution Complete）且最優(yōu)

隨機路圖法

PRM（Probabilistic Road Maps）

通過隨機采樣選取不碰撞的點

兩點連接采用簡單的局部規(guī)劃器如 Walk to 算法

將起止點連入路圖

用圖搜索求解

概率完備且不最優(yōu)

快速擴展隨機樹法

RRT（Randomly Exploring Randomized Trees）

基于樹狀結(jié)構(gòu)的搜索算法

概率完備且不最優(yōu)

前面我們講的都是2D點狀機器人的情況，現(xiàn)在我們想怎么把這些問題推廣到實際機器人上。實際機器人有兩個問題，一個是機器人不再是一個點，需要將機器人的體積考慮在內(nèi)，另外，機器人的自由度更高，原本的算法是否都還可用？

C空間（理論基礎(chǔ)）

構(gòu)形空間，Configuration Space

用向量描述機器人的構(gòu)形

在C空間內(nèi)，機器人是一個點

C 空間拓撲性質(zhì)與笛卡爾坐標系下的情況不同——二自由度機械臂的C空間是一個圓環(huán)面

大部分機構(gòu)（連續(xù)旋轉(zhuǎn)關(guān)節(jié)、平動關(guān)節(jié)等）形成的構(gòu)形空間均是微分流形，任一點的鄰域均與歐式空間同態(tài)

微分流形：大部分算法效果與在笛卡爾坐標下效果相同

高維度

蟻群等優(yōu)化算法：收斂慢，更多局部極值點

可視圖法：在高維空間中，算法不成立

柵格法：理論上可行；但會計算量太大；對于一個六自由度機械臂，我們按照6°分辨率（已經(jīng)是很低的分辨率了）劃分網(wǎng)格，那么將會產(chǎn)生606 = 4.67 × 1010 個網(wǎng)格，單是對每個網(wǎng)格進行碰撞檢測（如果碰撞檢測速度為0.1ms），就需要1296小時。

一般在高于三維的問題上不使用該方法。

人工勢場

在 C 空間內(nèi)建立勢場不方便

只對個別控制點進行計算，折算到每個關(guān)節(jié)上

不完備且不最優(yōu)，但對于簡單的問題很實用

PRM 和 RRT

不需要知道 C 空間的具體情況，只對隨機采樣點進行碰撞檢測（判斷是否在 C 空間的可行區(qū)域內(nèi)）

兩點之間采用簡單的局部規(guī)劃器（如 Walk to）進行連接

PRM：獲得一個圖，采用圖搜索算法求解

RRT：獲得一個連接到終點的樹，反向搜索即可

在高維空間內(nèi)可行，概率完備且不最優(yōu)

現(xiàn)狀：主要使用 RRT 和 PRM 等 Sampling-based methods；這些算法計算的結(jié)果一般需要進行后處理（smoothing等）。

RRT 和 PRM 變種

C 空間

隨機采樣（各種采樣算法 T-RRT）

有效性判斷（如碰撞檢測算法 AABB、減少碰撞檢測 Lazy-RRT）

局部規(guī)劃器連接（各種連接方法、重新連接 RRT*，PRM*）

…

RRT*

漸進最優(yōu)

Informed RRT*

先驗知識——只在sub-problem下采樣

理論學習

Coursera: 賓大 Robotics: Computational Motion Planning （簡單編程）

Choset, Howie M. Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

經(jīng)典論文+編程實現(xiàn)經(jīng)典算法。

實踐

ROS MoveIt!：http://moveit.ros.org/

容易上手+容易修改

前沿研究方向

理論現(xiàn)狀是，從運動學規(guī)劃角度，給定足夠多的時間一定能夠最優(yōu)且完備地求解到軌跡。從理論的角度而言，這個問題已經(jīng)解決了?，F(xiàn)在研究方向主要在這兩個方面，探索新問題和做一些實用化工作。

新問題

重規(guī)劃 re-plan

這個算是蠻實用的，因為每次規(guī)劃完執(zhí)行的過程中會遇到環(huán)境變化的問題，這就需要在執(zhí)行過程中重新規(guī)劃。重新規(guī)劃的路線與之前的路線是連接的，而不是中間停下來重新走。上面是RSS在2016年的研究，感興趣的可以了解下。

考慮系統(tǒng)動力學

理想狀態(tài)下機器人在運動規(guī)劃下直接端一杯水到一個地方就行了，但實際情況下這個過程是有動力學在里面，如果不做任何處理，這個杯子會掉。所以，在考慮了動力學之后，重新進行運動規(guī)劃，這時候杯子才不會掉。這個問題還是比較簡單的，因為你只需要把它變成一個約束就好了。

考慮接觸動力學

因為我沒有做這塊東西，所以不太清楚它是怎么運作的，但是這個問題是存在的，因為在規(guī)劃的時候會跟環(huán)境接觸，例如這個機器人攀爬桿子然后落地，涉及到整個身體動力學跟你身體運動的協(xié)調(diào)過程，這個工作是MIT計算機科學與人工智能實驗室在2014年的實驗。接觸動力學比傳統(tǒng)的單體動力學復(fù)雜很多，因為我們不知道它接觸的碰撞摩擦力這些不好建模。

運動規(guī)劃+任務(wù)規(guī)劃

運動規(guī)劃是指我給你一個大任務(wù)，你自動生成一些小任務(wù)。這是IROS在2016年的一個工作，它的目標是讓機器人到達對面這個點，而它的路徑被障礙物擋住了，這個時候把運動規(guī)劃加進來，從更高一個空間維度去求解這個問題。第一步，它把這個桌子往前推，發(fā)現(xiàn)桌子推不動的時候?qū)θ蝿?wù)進行重規(guī)劃，然后規(guī)劃到去推這個桌子，然后發(fā)現(xiàn)執(zhí)行的效果與預(yù)計的不一樣，所以它又生成新的任務(wù)，然后它拉開桌子之后就走到了對面實現(xiàn)了工作。這只是一個很簡單的demo，但實際上生活中會遇到很多這樣的問題，比如我想從這個房間到另一個房間，而門是關(guān)著的，這個時候就需要把門打開。所以說，不是要給機器人生成很多子任務(wù)，而是一個大任務(wù)，未來服務(wù)機器人想要做好這塊是必須要做的。

實用化

另外大部分時間大家都用在了實用化上，雖然說只要有足夠時間它一定能求解出來，但實際情況下我們不可能給它無限的時間。另外RRT這些算法生成軌跡很奇怪， 你可以看右邊這個視頻，只是讓它敲這個東西它要畫一大圈，所以這也是一個問題，就是怎么優(yōu)化它的軌跡。所以需要將研究領(lǐng)域好的算法往工業(yè)領(lǐng)域推，目前兩者之間是存在很大缺口的。

軌跡復(fù)用（相對固定的動作）

這個工作是想辦法把舊的軌跡給用起來，通過人工的方式指定一個運動微元，也就是原始軌跡，等到了新的環(huán)境后再進行改變。當然，這個爬樓梯的過程，環(huán)境和動作基本上都相同，所以可以在這個微元的基礎(chǔ)上進行改變。首先，通過變形的工作拉到現(xiàn)在起始點位置，部分起始點會重合，然后對這些新起始點進行重復(fù)利用，它會形成一個好的軌跡。這個工作是Hauser et al在2008年發(fā)布的論文?，F(xiàn)在存在的問題是運動微元必須由人工來指定，所以研究方向是由系統(tǒng)自動生成運動微元。

舊軌跡信息（相對固定的環(huán)境）

這是之前做的一個內(nèi)容，比較簡單但在相對固定的環(huán)境比較好用。大概原理就是根據(jù)人工示教的路徑，通過高斯混合模型（GMM）對可行C空間進行建模，之后在這個GMM-C空間內(nèi)進行規(guī)劃。這個方法有點類似Learning From Demonstration 的工作，但我只用了它們前面一半的步驟，后面一半還是采用采樣的方法。

加約束

這個是我針對加工過程做的另一個工作。我們在工業(yè)領(lǐng)域用機器人往往期望的不是整個機械臂的動作，而只是末端的動作。假設(shè)我要拋光一個面，首先我要對末端進行規(guī)劃，用CAD模型就可以計算實現(xiàn)；得到路徑后發(fā)給機器人，之后直接求逆解或者用雅克比迭代過去。當然，這種方法大部分時候夠用，但有時候也會遇到奇異點或者碰到障礙物。我就是針對這個七軸的機械臂，利用它的一個冗余自由度進行規(guī)劃。因為末端是固定的軌跡，這個時候，只要找到冗余自由度對應(yīng)的C空間流形，我們就可以在這么一個低維（2維）流形內(nèi)進行很快速的規(guī)劃，實現(xiàn)末端固定軌跡，且關(guān)節(jié)避障避奇異。

深度強化學習 DRL

我個人現(xiàn)在現(xiàn)在最關(guān)注的一塊，目前還沒有實質(zhì)性的東西出來，在這里就和大家討論下，我覺得這一塊未來會出來不少的研究成果。

假設(shè)深度學習做運動規(guī)劃，那么它進行一次運動規(guī)劃的時間就是一次網(wǎng)絡(luò)正向傳播時間，這個時間非常短的，所以只要網(wǎng)絡(luò)訓(xùn)練好后，運動規(guī)劃需要耗費很長時間的問題就沒有了。目前這塊也有一些這方面的研究，上面左邊圖是用深度學習玩游戲，Nature上的一篇論文，效果比人還厲害；右上角是谷歌用深度學習來開門；右下角就是AlphaGo下圍棋了。這個是很有意思的，它也是運動規(guī)劃和控制的問題，但它是用網(wǎng)絡(luò)來做的映射。

我為什么對這方面很感興趣呢？首先，CNN已經(jīng)具備強大的環(huán)境理解能力，很容易從觀測估計狀態(tài)，觀測是圖片這類，而狀態(tài)，如果是物體識別，就是是什么物體，如果是定位，那就是物體在什么地方。也就是說，在給定信息滿足系統(tǒng)狀態(tài)可觀性的前提下，CNN環(huán)境理解能力是非常強大的。

第二個就是RL（強化學習）可以進行路徑規(guī)劃，通過 value iteration 等方式建立表格，這個表格紀錄的是從狀態(tài)到動作的映射。不過運動規(guī)劃的維度這么高不可能用表格來存，所以可以通過神經(jīng)網(wǎng)來解決這個映射問題。

Q&A 環(huán)節(jié)

Q：運動規(guī)劃、軌跡規(guī)劃、路徑規(guī)劃的區(qū)別與聯(lián)系？ 

A：這幾個概念在國內(nèi)的教材里確實比較混亂，所以我按照我的理解大概說一下：①運動規(guī)劃（Motion Planning）就是我這次說的主要內(nèi)容；②路徑規(guī)劃（Path Planning）跟運動規(guī)劃概念是一樣的，只是我們習慣上把它放在平面機器人上；③軌跡規(guī)劃（Trajectory Planning），主要是涉及速度、加速度插值，包括梯形速度曲線等底層的軌跡插值算法。

一般運動規(guī)劃后的結(jié)果是離散的路徑點，也會需要進行軌跡規(guī)劃后發(fā)給機器人。

Q：運動規(guī)劃中如何實現(xiàn)姿態(tài)轉(zhuǎn)接，角速度，角加速度的連續(xù)。

A：首先是姿態(tài)的連續(xù)變化問題：因為我們是在C空間（也就是關(guān)節(jié)空間進行的），所以不存在這個問題；如果是對無人機進行規(guī)劃（在SE(3)里），那么可以考慮看看四元數(shù)插值的算法。

第二個是角速度、角加速度連續(xù)；這個有兩種方法，一個是我只規(guī)劃角度，然后用軌跡規(guī)劃進行插值；另一個是我同時規(guī)劃速度和加速度（如優(yōu)化算法），直接得到一個軌跡。

Q：移動機器人（例如AGV）的路徑規(guī)劃有什么推薦的軟件或者平臺嗎？

A：AGV的路徑規(guī)劃，我這邊做的不多，而且會根據(jù)你具體做的內(nèi)容而不同（如SLAM、定位、導(dǎo)航等）。就我實驗室來說的話，在ROS下效果就不錯，可以用gmapping建圖、然后navigation導(dǎo)航。當然，如果ROS還是比較適合實驗室，商用的話我就不太了解了。

Q：學習機器人，一定要從運動學動力學軌跡規(guī)劃一步一步走過來嘛？

A：因為機器人涉及的內(nèi)容真的很多，如果是做機械臂的話，我還是建議把基礎(chǔ)的東西完全弄清楚，包括空間運動、正逆解、動力學等。因為這些基礎(chǔ)知識里很多細節(jié)，如果直接跳過，后面會遇到很多問題。這方面打好基礎(chǔ)，后面就會輕松很多的。

Q：機械臂逆運動學推導(dǎo)和雅可比矩陣求解怎么在Ros上結(jié)合規(guī)劃算法來做出仿真？

A：其實我們規(guī)劃的過程中需要不斷進行運動學求解等。當然，ROS里面的話，我們可以先不關(guān)心這個。因為ROS里的KDL、ikFast等都可以幫我們做這些工作。當然了，由于MoveIt采用的是Plugin的形式，如果是你自己算的運動學，那可以做成Plugin放進去。之后MoveIt的規(guī)劃就是基于你的運動學進行計算了。

Q：請問動態(tài)環(huán)境下的運動規(guī)劃有什么研究熱點和方向？

A：這個有兩點，一個是剛才提到的重規(guī)劃re-plan問題，當環(huán)境變化或者任務(wù)變化的時候能在執(zhí)行的過程中重新規(guī)劃出一個新的軌跡；另一個則是在規(guī)劃的過程中考慮動態(tài)情況：例如我們對無人車進行規(guī)劃的時候，就應(yīng)該同時估計出周圍人、車的速度和運動趨勢，之后在此基礎(chǔ)上進行規(guī)劃。這樣可以避開運動的障礙物等。

雷峰網(wǎng)原創(chuàng)文章，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見轉(zhuǎn)載須知。