如何利用人工勢場進行運動規(guī)劃？

1.1 引力勢場(Attractive Potential Field)

人工勢場這個特殊的勢場并不是一個單一的場，其實它是由兩個場疊加組合而成的，一個是引力場，一個是斥力場。

顧名思義引力勢場是具有吸引的性質(zhì)，會將機器人從起點處朝著終點處吸引，所以引力場的存在使得機器人獲得了運動的大方向。

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

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

1.2 斥力勢場(Repulsive Potential Field)

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

這個過程其實非常好理解，引力勢場負責(zé)吸引機器人從起點朝著終點運動，斥力勢場負責(zé)規(guī)避地圖中的障礙。

通常我們會用下面這個函數(shù)來構(gòu)建斥力勢場：

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

在Matlab中可以直觀地看到這個斥力勢場的樣子，離障礙物越近的點所具備的斥力值越大，離障礙物越遠斥力值越小，甚至為0。

我們生成了引力勢場和斥力勢場這兩個特殊勢場之后，只要將兩個勢場進行簡單的疊加組合之后就得到了最終的人工勢場。

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

1.3 梯度下降

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

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

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

所以這便回答了方才提出的“如何利用人工勢場進行運動規(guī)劃”這一問題，我們采用梯度下降法配合人工勢場進行運動規(guī)劃。

2. 人工勢場法的Matlab實現(xiàn)

2.1 引力勢場函數(shù)

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

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

2.2 斥力勢場函數(shù)

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

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

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

最終將兩個生成的勢場簡單地相加就可以得到最終的人工勢場了。

2.3 梯度規(guī)劃器

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

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

2.4 最終運行效果

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

為了更加直觀地看到效果，我另外用一個小球代表機器人制作了下面這個動畫，這回是不是就特別好懂了。這就是為什么我在開頭的時候說人工勢場是這個系列五個算法中最好理解，最符合直覺的。

3. 人工勢場的不足

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

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

再比如，我們將起點和終點設(shè)置如下，讓我們來看看會怎么樣？

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

總結(jié)

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

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