如何將MATLAB運(yùn)動(dòng)規(guī)劃應(yīng)用于各種自主系統(tǒng)

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

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

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

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

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

什么是運(yùn)動(dòng)規(guī)劃？

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

“運(yùn)動(dòng)規(guī)劃”和“路徑規(guī)劃”這兩個(gè)詞經(jīng)?；煊?，但兩者有一個(gè)關(guān)鍵區(qū)別。

運(yùn)動(dòng)規(guī)劃在車輛位置隨時(shí)間變化時(shí)生成車輛的運(yùn)動(dòng)，而路徑規(guī)劃只生成車輛的路徑。

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

場景 1：紅燈亮起時(shí)，汽車減速后停止；綠燈亮起時(shí)，汽車?yán)^續(xù)行駛，這是運(yùn)動(dòng)變化，不是規(guī)劃路徑變化。

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

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

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

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

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

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

路徑代價(jià)

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

最優(yōu)性

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

完備性

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

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

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

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

運(yùn)動(dòng)規(guī)劃的常見類型

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

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

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

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

基于搜索的規(guī)劃

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

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

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

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

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

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

基于采樣的規(guī)劃

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

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

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

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

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

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

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

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

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

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

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

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

使用 MATLAB 進(jìn)行運(yùn)動(dòng)規(guī)劃的四步工作流

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

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

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

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

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

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

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

表示狀態(tài)空間

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

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

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

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

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

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

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

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

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

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

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

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

選擇運(yùn)動(dòng)規(guī)劃算法

Navigation Toolbox 中提供了以下運(yùn)動(dòng)規(guī)劃算法。

進(jìn)一

責(zé)任編輯：haq