曾幾何時，汽車離不開駕駛員，機器也離不開實體控制器。但現在不一樣了：如今，高度智能的自動駕駛汽車已經能夠自己變道、禮讓行人甚至側方停車。

自動駕駛汽車、機器人操作臂、無人地面交通工具 (UGV) 和無人機等系統要實現自主，離不開三大支柱，運動規劃就是其中之一。另外兩大支柱則是感知和控制。

與人類非常相似，自主系統也是通過掃描環境來探索新環境，以了解自己所在的位置和周圍環境。

一旦得出環境地圖，運動規劃算法就會規劃一條通往指定目的地的無障礙路徑。

算法會決定沿著路徑要采取的下一步，控制器根據這一決定向作動器發送命令，使得系統移動。

什么是運動規劃？

運動規劃是一種計算問題，旨在尋找將機器人或車輛從初始狀態移動至目標狀態的動作序列。

“運動規劃”和“路徑規劃”這兩個詞經常混用，但兩者有一個關鍵區別。

運動規劃在車輛位置隨時間變化時生成車輛的運動，而路徑規劃只生成車輛的路徑。

通過運動規劃，車輛可以在遵循現有路徑的同時改變運動，如以下兩個自動駕駛汽車場景所示：

場景 1：紅燈亮起時，汽車減速后停止；綠燈亮起時，汽車繼續行駛，這是運動變化，不是規劃路徑變化。

狀態空間和其他運動規劃關鍵概念

在實際應用中，運動規劃的實現有賴于多個功能部件。其中包括使用同步定位與地圖構建 (SLAM) 算法生成的環境地圖，以及機器人或車輛的狀態（位置和方向）。

機器人的狀態間變換定義了其運動。可以應用于機器人的變換集合稱為狀態空間或配置空間(Cspace)。配置空間可以包括自由空間（其中的機器人狀態被視為有效）和障礙空間（其中的機器人狀態被視為無效）。

例如，在自動駕駛汽車中，汽車的位置及其駛向或方向共同代表了它的狀態。

對于自動駕駛汽車的自動泊車，停車場的地圖標識了自由空間和障礙空間，狀態空間表示使用運動模型定義的所有可能的前進和后退機動的集合。

路徑代價、最優性和完備性

路徑代價

當機器人或車輛在尋找路徑時，它所采取的每一步都與代價相關聯。穿越自由空間的代價通常設為零，穿越包含障礙物的空間的代價設為無窮大。

最優性

如果路徑規劃算法總能找到最優路徑，則稱其為最優算法。為了使路徑最優，其轉換代價（邊緣代價）之和在從初始位置到目標位置的所有可能路徑中必須是最低的。

完備性

在有限的時間內，當路徑存在時，路徑規劃算法能找出路徑，當路徑不存在時，算法能報告路徑不存在，則稱該算法為完備的。

最優且完備的路徑規劃算法所提供的路徑不一定是最短的，但代價會是最小的。

在某些特定的情況下（例如，讓室內機器人沿著走廊移動），可以將機器人沿走廊中心移動的代價定義為低于靠近墻壁移動的代價。

在這種情況下，最優路徑是讓機器人沿著走廊中心移動，減少與墻壁碰撞的機會。

運動規劃的常見類型

運動規劃有許多不同類型的方法。最常見的方法如下：

基于搜索的規劃和基于采樣的規劃方法，取決于搜索樹或圖的創建方式

全局和局部路徑規劃方法，取決于規劃是在整個地圖中還是在某一子集中完成

接下來我們將逐一探討每種方法。

基于搜索的規劃

基于搜索的規劃創建一個可搜索的圖，將每個車輛狀態或配置標識為一個節點。該圖從起始節點擴展到目標節點，使用基于代價和啟發式的方法來尋找最短路徑。

基于搜索的規劃通常在離散化地圖上執行，其中地圖被細分為柵格單元，狀態數是有限的或可數無限的（可以為每個狀態分配一個唯一的整數）。

離散狀態空間通常用二維柵格地圖表示，其中各個網格的中心是要搜索的狀態。一種常見的地圖表示方法是占據柵格地圖。

A* 算法是一種常用的基于搜索的方法，用于在離散柵格地圖中尋找路徑。

當車輛或機器人可被視為一個點且規劃階段不涉及運動模型或運動學方程時，柵格地圖上基于搜索的規劃通常適用。

如果路徑規劃算法為機器人提供了要遵循的路點，則可以使用控制算法來添加運動學約束。

基于采樣的規劃

在基于采樣的規劃中，搜索樹或路線圖是通過在狀態空間中隨機添加節點來創建的。使用連續運動模型，可以找到可能的無碰撞路徑。

基于采樣的規劃通常使用啟發式方法來探索搜索空間并偏轉搜索方向。創建后，樹或路線圖使用碰撞檢查或搜索方法來尋找到達目標的最短路徑。

RRT 算法是一種常用的基于采樣的方法，用于在連續狀態空間中尋找路徑。

基于采樣的運動規劃適用于高維搜索空間，例如尋找一組有效的配置，使機械臂能夠拾取物體。基于采樣的規劃廣泛適用于多種實際應用，雖然不能提供完備解，但仍廣受歡迎。

如果搜索樹的密度使樣本足夠接近，則當解存在時，找到解的概率會收斂到 1。這使得一些基于采樣的規劃器（例如 RRT 和 RRT*）在概率上是完備的。

全局和局部路徑規劃

全局路徑規劃又稱基于地圖的規劃，它根據有關環境的先驗知識尋找最優路徑。

全局規劃算法規劃初始路徑，以避開環境中已知的靜態障礙。

例如，一個自主移動機器人可以規劃一條全局路徑，在有墻壁等靜態障礙物的走廊上，將一本書從一個辦公室送到另一個辦公室。

局部路徑規劃又稱動態重規劃，它重新計算路徑，以避開未知的動態障礙。

局部規劃算法跟蹤全局規劃并創建局部軌跡，同時避開新引入的障礙。

例如，一輛自動駕駛汽車可能會規劃局部軌跡，變道以避開其他車輛，然后重新匯入全局路徑以抵達目的地。

使用 MATLAB 進行運動規劃的四步工作流

Navigation Toolbox 提供了用于實現各種規劃算法的類，包括常見的基于搜索的規劃器（例如 A*）和基于采樣的規劃器（例如 RRT 和 RRT*）。

該工具箱還提供路徑指標，來評估所規劃路徑的避障間隙和平滑度。

此外，Navigation Toolbox 提供了一個接口，可讓您在系統化的四步工作流中實現基于采樣的運動規劃算法：

表示狀態空間。

定義狀態校驗器。

對新狀態進行采樣并檢查有效性。

將一組有效狀態表示為路徑。

表示狀態空間

自定義狀態空間類 nav.StateSpace 允許您定義一個狀態空間，在其中包含任何應用的可能狀態或配置。例如，stateSpaceDubins 和 stateSpaceReedsShepp 通過連接狀態空間中的任意兩個狀態來支持自動泊車規劃，以便狀態空間模擬汽車類機器人或帶有阿克曼轉向的機器人的運動。

Navigation Toolbox 提供以下現成的狀態空間。

定義狀態校驗器

狀態校驗器基于狀態空間，并與通過 SLAM 算法獲得的地圖相對應。它檢查單個狀態的有效性或兩個采樣狀態之間的運動的有效性。例如，碰撞檢查器是一種狀態校驗器，可指示機器人狀態或配置與障礙物發生碰撞的情況。

Navigation Toolbox 提供以下狀態校驗器，用于校驗二維和三維占據地圖中的狀態和離散化運動。

這些狀態校驗器派生自工具箱中提供的自定義狀態校驗器 nav.StateValidator，可用于確定單個狀態的有效性或任意兩個狀態之間的運動的有效性。

對新狀態進行采樣并檢查有效性

基于采樣的規劃算法在定義的狀態空間中隨機對狀態采樣，并使用狀態校驗器創建從起點到目標的無障礙路徑。RRT 和 PRM 等算法使用不同的采樣方案對狀態進行采樣，并創建搜索樹或路線圖。

對于通過 SLAM 算法獲得的地圖，為對地圖內的狀態進行采樣，會應用與地圖外側界限相對應的狀態空間邊界。

表示采樣狀態的集合

您可以使用 Navigation Toolbox 中的 plan 函數將規劃算法的輸出整理成樹狀數據結構。您可以使用 navPath 類存儲給定狀態空間中的狀態集合，并對它們進行插值以獲得路徑。

運動規劃函數：https://ww2.mathworks.cn/help/nav/motion-planning.html

選擇運動規劃算法

Navigation Toolbox 中提供了以下運動規劃算法。

進一

責任編輯：haq