轉載：https://www.cnblogs.com/lvchaoshun/p/6681541.html

1.引言

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

2.什麼是運動規劃（Motion Planning）

2.1 定義

我們可以從中給運動規劃得出一個定義：

在給定環境中，指定機器人起點與終點，計算出連線起點與終點，並滿足一定約束條件（如避障）的軌跡。

從數學角度上看，移動機器人的路徑規劃（ Path Planning ）也屬於運動規劃的範疇。但由於問題的維度不同，所以使用的演算法也不同，大家習慣上將兩者區分開。

2.2 為什麼研究路徑規劃

（1）社會老齡化

　　這是世界銀行釋出的關於中日兩國國內生產總值（GDP）變化曲線圖，小圖是中日兩國的人口結構，可以看到2000年日本和中國2015年的人口結構 已經很接近了，所以未來中國勞動力數量會減少，我們必須提高平均勞動生產力，這樣才能防止GDP的增速減緩。機器人是可以解決這些問題的。

（2）市場轉變

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

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

（3）示教

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

（4）加中間點

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

2.3 怎麼做運動規劃

對於規劃器的評價標準，我們現在有兩個準則：

• Optimality（最優性）: 路徑最短、規劃速度最快等。
• Complete（完備性）：在有限時間內解決所有有解問題。

3.運動規劃演算法

3.1 二維運動規劃

（1）Walk To

• 直接朝著目標走，直到到達目標點為止。
• 很多 RPG 遊戲就採用了這種簡單的演算法
• 最優性，但不完備

（2）優化演算法（蟻群等）

• 類似最優控制
• 大部分情況下效果不錯，但複雜問題很容易陷入區域性極值
• 不完備也不最優

（3）人工勢場

• 在障礙物周圍建立排斥勢場
• 從起點到終點構建吸引勢場
• 採用梯度下降等方式求解
• 容易實現、效果很好
• 可以與控制結合
• 可能陷入區域性極值
• 不完備且不最優

（4）圖搜尋演算法

• 將問題描述成圖（節點+邊）
• 用圖搜尋演算法解決問題
• Dijkstra、A*
• 在給定的圖中完備且最優

（5）可檢視（Visibility Graph）

• 用封閉多面體描述障礙物
• 利用障礙物頂點間的連線構建一個圖（graph），之後用圖搜尋演算法求解
• 站在某個頂點上，環繞四周，把你能看到（無障礙物）的頂點連線起來
• 完備且最優

（6）柵格化（Cell Decomposition）

• 按一定解析度將地圖進行網格劃分
• 用四連通或八連通規則建立網格圖
• 解析度完備（Resolution Complete）且最優

（7）隨機路圖法

• PRM（Probabilistic Road Maps）
• 通過隨機取樣選取不碰撞的點
• 兩點連線採用簡單的區域性規劃器如 Walk to 演算法
• 將起止點連入路圖
• 用圖搜尋求解
• 概率完備且不最優

（8）快速擴充套件隨機樹法

• RRT（Randomly Exploring Randomized Trees）
• 基於樹狀結構的搜尋演算法
• 概率完備且不最優

3.2 三維路徑規劃

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

（1）C空間（理論基礎）

• 構形空間，Configuration Space
• 用向量描述機器人的構形
• 在C空間內，機器人是一個點
• C 空間拓撲性質與笛卡爾座標系下的情況不同——二自由度機械臂的C空間是一個圓環面
• 大部分機構（連續旋轉關節、平動關節等）形成的構形空間均是微分流形，任一點的鄰域均與歐式空間同態
• 微分流形：大部分演算法效果與在笛卡爾座標下效果相同

（2）高維度*

• 蟻群等優化演算法：收斂慢，更多區域性極值點
• 可檢視法：在高維空間中，演算法不成立
• 柵格法：理論上可行；但會計算量太大；對於一個六自由度機械臂，我們按照6°解析度（已經是很低的解析度了）劃分網格，那麼將會產生606 = 4.67 × 1010 個網格，單是對每個網格進行碰撞檢測（如果碰撞檢測速度為0.1ms），就需要1296小時。
• 一般在高於三維的問題上不使用該方法。

（3）人工勢場

• 在 C 空間內建立勢場不方便
• 只對個別控制點進行計算，折算到每個關節上
• 不完備且不最優，但對於簡單的問題很實用

（4）PRM 和 RRT

• 不需要知道 C 空間的具體情況，只對隨機取樣點進行碰撞檢測（判斷是否在 C 空間的可行區域內）
• 兩點之間採用簡單的區域性規劃器（如 Walk to）進行連線
• PRM：獲得一個圖，採用圖搜尋演算法求解
• RRT：獲得一個連線到終點的樹，反向搜尋即可
• 在高維空間內可行，概率完備且不最優

現狀：主要使用 RRT 和 PRM 等 Sampling-based methods；這些演算法計算的結果一般需要進行後處理（smoothing等）。

（5）RRT 和 PRM 變種

• C 空間
• 隨機取樣（各種取樣演算法 T-RRT）
• 有效性判斷（如碰撞檢測演算法 AABB、減少碰撞檢測 Lazy-RRT）
• 區域性規劃器連線（各種連線方法、重新連線 RRT，PRM）

（6）RRT*

• 漸進最優

（7）Informed RRT*

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

4.其他

4.1 理論學習

　　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/

    容易上手+容易修改

4.2 前沿研究方向

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

（1）新問題

• 重規劃 re-plan

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

• 考慮系統動力學

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

• 考慮接觸動力學

　　因為我沒有做這塊東西，所以不太清楚它是怎麼運作的，但是這個問題是存在的，因為在規劃的時候會跟環境接觸，例如這個機器人攀爬杆子然後落地，涉及 到整個身體動力學跟你身體運動的協調過程，這個工作是MIT電腦科學與人工智慧實驗室在2014年的實驗。接觸動力學比傳統的單體動力學複雜很多，因為 我們不知道它接觸的碰撞摩擦力這些不好建模。

• 運動規劃+任務規劃

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

（2）實用化

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

• 軌跡複用（相對固定的動作）

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

• 舊軌跡資訊（相對固定的環境）

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

• 加約束

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

• 深度強化學習 DRL

　　我個人現在現在最關注的一塊，目前還沒有實質性的東西出來，在這裡就和大家討論下，我覺得這一塊未來會出來不少的研究成果。

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

　　我為什麼對這方面很感興趣呢？首先，CNN已經具備強大的環境理解能力，很容易從觀測估計狀態，觀測是圖片這類，而狀態，如果是物體識別，就是是什 麼物體，如果是定位，那就是物體在什麼地方。也就是說，在給定資訊滿足系統狀態可觀性的前提下，CNN環境理解能力是非常強大的。

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

5.參考網址

V-rep學習筆記：機器人路徑規劃1