如何利用人工勢(shì)場(chǎng)進(jìn)行運(yùn)動(dòng)規(guī)劃？

1.1 引力勢(shì)場(chǎng)(Attractive Potential Field)

人工勢(shì)場(chǎng)這個(gè)特殊的勢(shì)場(chǎng)并不是一個(gè)單一的場(chǎng)，其實(shí)它是由兩個(gè)場(chǎng)疊加組合而成的，一個(gè)是引力場(chǎng)，一個(gè)是斥力場(chǎng)。

顧名思義引力勢(shì)場(chǎng)是具有吸引的性質(zhì)，會(huì)將機(jī)器人從起點(diǎn)處朝著終點(diǎn)處吸引，所以引力場(chǎng)的存在使得機(jī)器人獲得了運(yùn)動(dòng)的大方向。

在實(shí)際的工程中，其實(shí)有很多種方法可以構(gòu)建這樣的一個(gè)引力勢(shì)場(chǎng)，最簡(jiǎn)單而且也是最常用的方式就是直接對(duì)地圖自由空間的每個(gè)點(diǎn)都相對(duì)終點(diǎn)計(jì)算出歐氏距離的平方，并乘上一個(gè)縮放系數(shù)ε

fa(x, y)便是所謂的引力勢(shì)場(chǎng)函數(shù)，它會(huì)構(gòu)建一個(gè)離終點(diǎn)越遠(yuǎn)引力越大的特殊勢(shì)場(chǎng)，在Matlab中可以非常直觀地看到這個(gè)引力勢(shì)場(chǎng)。

1.2 斥力勢(shì)場(chǎng)(Repulsive Potential Field)

當(dāng)然僅僅只有引力勢(shì)場(chǎng)是不夠的，我們還需要讓機(jī)器人懂得避開(kāi)地圖中的障礙物，這時(shí)斥力勢(shì)場(chǎng)便有用武之地了，斥力勢(shì)場(chǎng)的會(huì)構(gòu)建一個(gè)距離障礙物越近，斥力越大的特殊勢(shì)場(chǎng)。

這個(gè)過(guò)程其實(shí)非常好理解，引力勢(shì)場(chǎng)負(fù)責(zé)吸引機(jī)器人從起點(diǎn)朝著終點(diǎn)運(yùn)動(dòng)，斥力勢(shì)場(chǎng)負(fù)責(zé)規(guī)避地圖中的障礙。

通常我們會(huì)用下面這個(gè)函數(shù)來(lái)構(gòu)建斥力勢(shì)場(chǎng)：

其中ρ(x, y)是一個(gè)特殊的函數(shù)，它會(huì)計(jì)算出離當(dāng)前點(diǎn)(x,y)最近的障礙物的距離，而d0是一個(gè)距離閾值，當(dāng)當(dāng)前點(diǎn)到最近障礙物的距離大于d0時(shí)不產(chǎn)生斥力。η同樣是一個(gè)簡(jiǎn)單的縮放系數(shù)。

在Matlab中可以直觀地看到這個(gè)斥力勢(shì)場(chǎng)的樣子，離障礙物越近的點(diǎn)所具備的斥力值越大，離障礙物越遠(yuǎn)斥力值越小，甚至為0。

我們生成了引力勢(shì)場(chǎng)和斥力勢(shì)場(chǎng)這兩個(gè)特殊勢(shì)場(chǎng)之后，只要將兩個(gè)勢(shì)場(chǎng)進(jìn)行簡(jiǎn)單的疊加組合之后就得到了最終的人工勢(shì)場(chǎng)。

在Matlab中可以直觀地看到這個(gè)疊加的結(jié)果如下，其實(shí)相信看到這里小伙伴們應(yīng)該有點(diǎn)感覺(jué)了，當(dāng)我們每個(gè)人工勢(shì)場(chǎng)都是根據(jù)特定的終點(diǎn)而特定生成的，在地圖不變的情況下，改變終點(diǎn)的位置便會(huì)改變?nèi)斯?shì)場(chǎng)。終點(diǎn)的位置總是人工勢(shì)場(chǎng)中勢(shì)能最低的地方，想象一下，現(xiàn)實(shí)中存在一個(gè)長(zhǎng)得和人工勢(shì)場(chǎng)一樣的曲面，現(xiàn)在我們拿著一個(gè)小球放在曲面的高處，那么在重力的作用下，小球自然而然地就會(huì)順著曲面滾到最低點(diǎn)。如果你成功地想象出這個(gè)畫(huà)面，那么恭喜你，你已經(jīng)領(lǐng)悟了人工勢(shì)場(chǎng)的核心思想了。

1.3 梯度下降

相信只要是學(xué)過(guò)高數(shù)的小伙伴一定知道梯度的概念，如果你忘記了，那么趕緊回去復(fù)習(xí)。梯度下降法在尋找最優(yōu)點(diǎn)的問(wèn)題上被十分廣泛地運(yùn)用，但其實(shí)這也是一個(gè)聽(tīng)名字很高大上，說(shuō)穿了很直觀的方法。

我還是和各路前輩一樣使用山坡為例，來(lái)說(shuō)說(shuō)明梯度下降法的核心思想。假設(shè)你一覺(jué)醒來(lái)被空投到一個(gè)未知的山坡上，現(xiàn)在要求你盡全力下山，也就是說(shuō)要你找到最低的點(diǎn)。那么這時(shí)候你該怎么辦呢？非常簡(jiǎn)單，第一步：環(huán)顧你的四周找到一個(gè)坡度最陡峭的地方；第二步：朝著最陡峭的方向邁出一步；就這樣重復(fù)執(zhí)行第一步、第二步，直到你到達(dá)一個(gè)位置，在這個(gè)位置的四周都比它高，也就是說(shuō)這是一個(gè)你能找到的最低的點(diǎn)。

在數(shù)學(xué)中，梯度的方向就是函數(shù)位于當(dāng)前某一點(diǎn)(x0，y0)時(shí)，增長(zhǎng)速度最快的方向。那么反過(guò)來(lái)，將函數(shù)添加上負(fù)號(hào)，再求梯度，便可以找到函數(shù)減少最快的方向。

所以這便回答了方才提出的“如何利用人工勢(shì)場(chǎng)進(jìn)行運(yùn)動(dòng)規(guī)劃”這一問(wèn)題，我們采用梯度下降法配合人工勢(shì)場(chǎng)進(jìn)行運(yùn)動(dòng)規(guī)劃。

2. 人工勢(shì)場(chǎng)法的Matlab實(shí)現(xiàn)

2.1 引力勢(shì)場(chǎng)函數(shù)

首先我們需要生成一個(gè)和地圖同樣大小的meshgrid

然后引力勢(shì)場(chǎng)函數(shù)的實(shí)現(xiàn)相對(duì)比較簡(jiǎn)單，就是計(jì)算meshgrid中每個(gè)點(diǎn)相對(duì)于終點(diǎn)的歐氏距離的平方，乘以一個(gè)縮放系數(shù)。對(duì)，就是這么簡(jiǎn)單。

2.2 斥力勢(shì)場(chǎng)函數(shù)

斥力勢(shì)場(chǎng)函數(shù)在實(shí)現(xiàn)中比較麻煩的點(diǎn)在于ρ(x,y)函數(shù)，但是各位不要擔(dān)心，萬(wàn)能的Matlab已經(jīng)有現(xiàn)成的函數(shù)可以實(shí)現(xiàn)這個(gè)功能，這個(gè)函數(shù)就是bwdist()

在bwdist()的描述中我們可以得知，這個(gè)函數(shù)其實(shí)是一個(gè)圖像處理函數(shù)，他會(huì)計(jì)算出一張二值圖片中所有像素點(diǎn)相對(duì)其最近的非零像素點(diǎn)的距離。這就正合我們意了，我們的障礙物地圖恰好就是一張二值圖片，0表示自由空間，1表示障礙物。那么這個(gè)函數(shù)就會(huì)計(jì)算出地圖中每個(gè)點(diǎn)相對(duì)最近障礙物的距離值，當(dāng)然如果當(dāng)前點(diǎn)恰好是個(gè)障礙物，那么最近的障礙物就是它自己，所以得到的距離結(jié)果就是0。

如下圖的代碼所示，influence就是表達(dá)式中的d0即距離閾值，這里將bwdist()的結(jié)果除以100再加1，首先是除以100是為了將距離值縮放到一個(gè)合理的范圍，其次加1是為了規(guī)避前面說(shuō)過(guò)障礙物結(jié)果為0的情況。

最終將兩個(gè)生成的勢(shì)場(chǎng)簡(jiǎn)單地相加就可以得到最終的人工勢(shì)場(chǎng)了。

2.3 梯度規(guī)劃器

Matlab將梯度顯示出來(lái)以后我們可以看到如下圖所示，圖中各個(gè)小箭頭就是代表當(dāng)位置的梯度大小和方向，其中遠(yuǎn)離障礙物的位置的梯度明顯指向黃色的終點(diǎn)，越靠近終點(diǎn)處的梯度值越小；靠近障礙物的地方的梯度明顯呈現(xiàn)排斥的趨勢(shì)，而且越靠近障礙物排斥力越大。

梯度規(guī)劃器核心代碼如下所示，可以看到在每輪迭代中都會(huì)沿著當(dāng)前梯度的方向前進(jìn)單位距離，直到到達(dá)終點(diǎn)，即梯度下降收斂。

2.4 最終運(yùn)行效果

最終的運(yùn)行結(jié)果如下，可以看到最終規(guī)劃出的是一條沿著梯度方向走的光滑路徑

為了更加直觀地看到效果，我另外用一個(gè)小球代表機(jī)器人制作了下面這個(gè)動(dòng)畫(huà)，這回是不是就特別好懂了。這就是為什么我在開(kāi)頭的時(shí)候說(shuō)人工勢(shì)場(chǎng)是這個(gè)系列五個(gè)算法中最好理解，最符合直覺(jué)的。

3. 人工勢(shì)場(chǎng)的不足

人工勢(shì)場(chǎng)法其實(shí)也不是完美無(wú)缺的規(guī)劃算法，它也是有缺點(diǎn)的，比如在某些情況下會(huì)陷入局部最優(yōu)解，而導(dǎo)致規(guī)劃失敗，比如當(dāng)我們?cè)O(shè)置起點(diǎn)和終點(diǎn)如下。

那么這種情況是會(huì)陷入局部最優(yōu)解的，具體情況如下圖所示，我們可以看到路徑在梯度下降的過(guò)程中卡在了死胡同里，最終導(dǎo)致規(guī)劃失敗。

再比如，我們將起點(diǎn)和終點(diǎn)設(shè)置如下，讓我們來(lái)看看會(huì)怎么樣？

從下圖的結(jié)果可以看出，這會(huì)導(dǎo)致另一種情況：終點(diǎn)的位置距離障礙物太近，規(guī)劃路徑難以接近終點(diǎn)的位置，只能停在終點(diǎn)附近，最終規(guī)劃失敗。

總結(jié)

以上是生活随笔為你收集整理的matlab人工势场法三维演示图,运动规划入门 | 5. 白话人工势场法，从原理到Matlab实现...的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。