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

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

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

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

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

什么是運動規(guī)劃？

運動規(guī)劃是一種計算問題，旨在尋找將機器人或車輛從初始狀態(tài)移動至目標(biāo)狀態(tài)的動作序列。

“運動規(guī)劃”和“路徑規(guī)劃”這兩個詞經(jīng)常混用，但兩者有一個關(guān)鍵區(qū)別。

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

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

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

狀態(tài)空間和其他運動規(guī)劃關(guān)鍵概念

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

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

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

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

路徑代價、最優(yōu)性和完備性

路徑代價

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

最優(yōu)性

如果路徑規(guī)劃算法總能找到最優(yōu)路徑，則稱其為最優(yōu)算法。為了使路徑最優(yōu)，其轉(zhuǎn)換代價（邊緣代價）之和在從初始位置到目標(biāo)位置的所有可能路徑中必須是最低的。

完備性

在有限的時間內(nèi)，當(dāng)路徑存在時，路徑規(guī)劃算法能找出路徑，當(dāng)路徑不存在時，算法能報告路徑不存在，則稱該算法為完備的。

最優(yōu)且完備的路徑規(guī)劃算法所提供的路徑不一定是最短的，但代價會是最小的。

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

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

運動規(guī)劃的常見類型

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

基于搜索的規(guī)劃和基于采樣的規(guī)劃方法，取決于搜索樹或圖的創(chuàng)建方式

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

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

基于搜索的規(guī)劃

基于搜索的規(guī)劃創(chuàng)建一個可搜索的圖，將每個車輛狀態(tài)或配置標(biāo)識為一個節(jié)點。該圖從起始節(jié)點擴展到目標(biāo)節(jié)點，使用基于代價和啟發(fā)式的方法來尋找最短路徑。

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

離散狀態(tài)空間通常用二維柵格地圖表示，其中各個網(wǎng)格的中心是要搜索的狀態(tài)。一種常見的地圖表示方法是占據(jù)柵格地圖。

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

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

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

基于采樣的規(guī)劃

在基于采樣的規(guī)劃中，搜索樹或路線圖是通過在狀態(tài)空間中隨機添加節(jié)點來創(chuàng)建的。使用連續(xù)運動模型，可以找到可能的無碰撞路徑。

基于采樣的規(guī)劃通常使用啟發(fā)式方法來探索搜索空間并偏轉(zhuǎn)搜索方向。創(chuàng)建后，樹或路線圖使用碰撞檢查或搜索方法來尋找到達目標(biāo)的最短路徑。

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

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

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

全局和局部路徑規(guī)劃

全局路徑規(guī)劃又稱基于地圖的規(guī)劃，它根據(jù)有關(guān)環(huán)境的先驗知識尋找最優(yōu)路徑。

全局規(guī)劃算法規(guī)劃初始路徑，以避開環(huán)境中已知的靜態(tài)障礙。

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

局部路徑規(guī)劃又稱動態(tài)重規(guī)劃，它重新計算路徑，以避開未知的動態(tài)障礙。

局部規(guī)劃算法跟蹤全局規(guī)劃并創(chuàng)建局部軌跡，同時避開新引入的障礙。

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

使用 MATLAB 進行運動規(guī)劃的四步工作流

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

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

此外，Navigation Toolbox 提供了一個接口，可讓您在系統(tǒng)化的四步工作流中實現(xiàn)基于采樣的運動規(guī)劃算法：

表示狀態(tài)空間。

定義狀態(tài)校驗器。

對新狀態(tài)進行采樣并檢查有效性。

將一組有效狀態(tài)表示為路徑。

表示狀態(tài)空間

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

Navigation Toolbox 提供以下現(xiàn)成的狀態(tài)空間。

定義狀態(tài)校驗器

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

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

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

對新狀態(tài)進行采樣并檢查有效性

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

對于通過 SLAM 算法獲得的地圖，為對地圖內(nèi)的狀態(tài)進行采樣，會應(yīng)用與地圖外側(cè)界限相對應(yīng)的狀態(tài)空間邊界。

表示采樣狀態(tài)的集合

您可以使用 Navigation Toolbox 中的 plan 函數(shù)將規(guī)劃算法的輸出整理成樹狀數(shù)據(jù)結(jié)構(gòu)。您可以使用 navPath 類存儲給定狀態(tài)空間中的狀態(tài)集合，并對它們進行插值以獲得路徑。

運動規(guī)劃函數(shù)：https://ww2.mathworks.cn/help/nav/motion-planning.html

選擇運動規(guī)劃算法

Navigation Toolbox 中提供了以下運動規(guī)劃算法。

進一

責(zé)任編輯：haq