天天看點

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

人工勢場法是局部路徑規劃的一種比較常用的方法。這種方法假設機器人在一種虛拟力場下運動。

如圖所示,機器人在一個二維環境下運動,圖中指出了機器人,障礙和目标之間的相對位置。

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

這個圖比較清晰的說明了人工勢場法的作用,物體的初始點在一個較高的“山頭”上,要到達的目标點在“山腳”下,這就形成了一種勢場,物體在這種勢的引導下,避開障礙物,到達目标點。

人工勢場包括引力場合斥力場,其中目标點對物體産生引力,引導物體朝向其運動(這一點有點類似于A*算法中的啟發函數h)。障礙物對物體産生斥力,避免物體與之發生碰撞。物體在路徑上每一點所受的合力等于這一點所有斥力和引力的和。這裡的關鍵是如何建構引力場和斥力場。下面我們分别讨論一下:

引力場:

常用的引力函數:

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

這裡的ε是尺度因子.ρ(q,q_goal)表示物體目前狀态與目标的距離。引力場有了,那麼引力就是引力場對距離的導數(類比實體裡面W=FX):、

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

關于梯度的算法可以參考相關資料,簡單提一下,二進制函數梯度是醬紫的[δx,δy],這個符号是偏導數,不太對,見諒。

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

Fig .引力場模型

斥力場:

公式(3)是傳統的斥力場公式,現在還沒有搞清楚是怎麼推導出來的。公式中η是斥力尺度因子,ρ(q,q_obs)代表物體和障礙物之間的距離。ρ_0代表每個障礙物的影響半徑。換言之,離開一定的距離,障礙物就對物體沒有斥力影響。

斥力就是斥力場的梯度

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】
【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

Fig 斥力場模型

總的場就是斥力場合引力場的疊加,也就是U=U_att+U_rep,總的力也是對對應的分力的疊加,如下圖所示:

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

二、存在的問題

(a) 當物體離目标點比較遠時,引力将變的特别大,相對較小的斥力在甚至可以忽略的情況下,物體路徑上可能會碰到障礙物

(b)當目标點附近有障礙物時,斥力将非常大,引力相對較小,物體很難到達目标點

(c)在某個點,引力和斥力剛好大小相等,方向想反,則物體容易陷入局部最優解或震蕩

三、各種改進版本的人工勢場法

(a)對于可能會碰到障礙物的問題,可以通過修正引力函數來解決,避免由于離目标點太遠導緻引力過大

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

和(1)式相比,(5)式增加了範圍限定。d*_goal 給定了一個門檻值限定了目标和物體之間的距離。對應的梯度也就是引力相應變成:

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

(b)目标點附近有障礙物導緻目标不可達的問題,引入一種新的斥力函數

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

這裡在原有斥力場的基礎上,加上了目标和物體距離的影響,(n是正數,我看到有篇文獻上n=2)。直覺上來說,物體靠近目标時,雖然斥力場要增大,但是距離在減少,是以在一定程度上可以起到對斥力場的拖拽作用

相應斥力變成:

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

是以可以看到這裡引力分為兩個部分,程式設計時要格外注意

(c)局部最優問題是一個人工勢場法的一個大問題,這裡可以通過加一個随機擾動,讓物體跳出局部最優值。類似于梯度下降法局部最優值的解決方案。

【路徑規劃】基于matlab改進的人工勢場算法機器人避障路徑規劃【含Matlab源碼 1151期】

版本:2014a