基于改進(jìn)人工勢場-模糊算法的路徑規(guī)劃算法研究

汪四新, 譚功全, 羅玉豐, 樊 石

(四川輕化工大學(xué)自動化與信息工程學(xué)院, 四川 自貢 643000)

引 言

移動機(jī)器人面臨的首要問題就是如何有效快速地到達(dá)目標(biāo)點，并且能躲避路途中所遇障礙物。路徑規(guī)劃是機(jī)器人導(dǎo)航中最基本的問題[1-2]，指的是如何規(guī)劃出一條由出發(fā)位置到目標(biāo)位置的無碰撞適宜軌跡。路徑規(guī)劃分為局部路徑規(guī)劃和全局路徑規(guī)劃[3]，局部路徑規(guī)劃指的是環(huán)境未知情況下的避障運動規(guī)劃，全局路徑規(guī)劃則是在環(huán)境已知的情況下搜索全局最優(yōu)的路徑規(guī)劃。常用的全局路徑算法主要有柵格法[4]、A＊算法[5]、RRT[6]以及仿生群搜索算法[7-8]等。常用的局部路徑規(guī)劃算法有動態(tài)窗口法DWA[9]、人工勢場法等。

人工勢場法利用障礙物和目標(biāo)形成的勢場來牽引機(jī)器人運動，因此擁有非線性特點，使得運動軌跡平滑，但容易出現(xiàn)目標(biāo)不可達(dá)和陷入局部極小值問題。劉俊雅[10]在斥力中加入歐幾里德距離函數(shù)并提出了一種基于情景行為的障礙物連接法，有效解決了目標(biāo)不可達(dá)與局部極小值問題，但在凹型障礙物中，機(jī)器人還會產(chǎn)生徘徊。針對復(fù)雜環(huán)境中機(jī)器人容易產(chǎn)生劇烈震蕩及局部最優(yōu)，有學(xué)者采用模糊人工勢場對勢場參數(shù)進(jìn)行模糊決策處理[11-12]，但未能完全克服機(jī)器人劇烈震蕩問題。韓偉[13]在此基礎(chǔ)上提出了一種基于模糊人工勢場法的動態(tài)路徑規(guī)劃，避免了復(fù)雜環(huán)境中的劇烈震蕩，但在凹型障礙物中仍存在明顯徘徊、波動?？紤]以上問題，本文在改進(jìn)斥力基礎(chǔ)上，利用模糊輸出力調(diào)整改善運動軌跡。仿真結(jié)果表明，本文方法避免了目標(biāo)不可達(dá)，跳出了局部極小值點，在凹型槽中無徘徊且震蕩較小。

1 人工勢場法

1.1 傳統(tǒng)人工勢場模型

傳統(tǒng)人工勢場法是由Khatbi[14]于1985年提出的一種路徑運動算法。人工勢場是將路徑規(guī)劃中的機(jī)器人運動模式與自然界存在的各種勢場對應(yīng)起來，機(jī)器人運動目的就是到目標(biāo)點，中途要躲避障礙物，因此讓目標(biāo)點對機(jī)器人提供吸引勢場，障礙物對機(jī)器人提供排斥勢場，在兩種勢場相互作用下，機(jī)器人就能比較平滑穩(wěn)定地到達(dá)目標(biāo)點。因此可以定義出機(jī)器人的所受總勢場為：

U(X)=Uatt(X)+Urep(X)

(1)

式中：Uatt為目標(biāo)點提供的吸引勢，Urep為障礙物提供的排斥勢。

機(jī)器人受到引力的牽引而向目標(biāo)點移動，因此機(jī)器人與目標(biāo)的距離與引力應(yīng)該成正比關(guān)系，距離目標(biāo)點越遠(yuǎn)所受的引力應(yīng)該越大，目標(biāo)點處引力為0，因此定義引力勢場Uatt(X)如下：

(2)

式中：Katt為引力常數(shù)，ρg表示機(jī)器人矢量位置與目標(biāo)點矢量位置的歐幾里德距離。

引力函數(shù)即為引力勢函數(shù)的負(fù)梯度，方向是由機(jī)器人指向目標(biāo)點，引力函數(shù)如下：

Fatt(X)=-▽Uatt(X)

(3)

(4)

機(jī)器人在往目標(biāo)點運動過程中要躲避障礙物，障礙物給予機(jī)器人的就是斥力作用，因此機(jī)器人與障礙物的距離與斥力為反比關(guān)系，距離障礙物越近障排斥力越大，斥力勢場Urep(X)如下：

(5)

式中：Krep為斥力常數(shù)，ρr為障礙物矢量位置與機(jī)器人矢量位置的歐幾里德距離，ρ0為障礙物最大影響距離。

同樣斥力勢場的負(fù)梯度即為斥力函數(shù)，方向由障礙物指向機(jī)器人，斥力函數(shù)如下：

Frep(X)=-▽Urep(X)

(6)

(7)

1.2 改進(jìn)斥力人工勢場模型

針對傳統(tǒng)人工勢場法容易陷入目標(biāo)不可達(dá)的問題[15-16]，對斥力場進(jìn)行改進(jìn)，加入機(jī)器人與目標(biāo)點歐幾里德距離的對數(shù)，并將對數(shù)左移一個單位，使得機(jī)器人在目標(biāo)點對數(shù)函數(shù)值為0。改進(jìn)的斥力勢場函數(shù)如下：

(8)

在新排斥勢場中，機(jī)器人受力如圖1所示，對排斥勢場求負(fù)梯度可得排斥力，將排斥力Frep分解為兩個分量，F(xiàn)rep1分量由障礙物指向機(jī)器人，F(xiàn)rep2分量由機(jī)器人指向目標(biāo)點。

圖1 機(jī)器人在新勢場中受力分析圖

斥力函數(shù)公式如下：

(9)

由兩分量的方向，F(xiàn)rep1為Frep對ρr求導(dǎo)，F(xiàn)rep2為Frep對ρg求導(dǎo)，公式如下：

(10)

(11)

對改進(jìn)斥力的函數(shù)加入一個基于機(jī)器人、障礙物、目標(biāo)點3個坐標(biāo)計算的判據(jù)，定義機(jī)器人位置坐標(biāo)(X,Y)，障礙物位置坐標(biāo)(Xr,Yr)，目標(biāo)點坐標(biāo)(Xg,Yg)，判據(jù)公式如下：

M=XYr+XrYg+XgY-XYg-XrY-XgYr

(12)

當(dāng)M≤1時，代表機(jī)器人陷入了局部最優(yōu)或者死循環(huán)，此時通過加大沿障礙物到機(jī)器人方向斥力分量Frep1的Y坐標(biāo)軸分量到原來的10倍，使之有效逃離局部最優(yōu)。

2 改進(jìn)人工勢場-模糊算法

在人工勢場中加入模糊算法，采用TS(Takagi-Sugeno)模糊模型。通過計算出引力與斥力的角度差以及斥力與引力的合力差，將這兩個差量模糊化，作為模糊決策的兩個輸入量，通過模糊規(guī)則得到一個TS模糊輸出量，定義這個輸出量為模糊力Ffu，分解Ffu到X軸方向和Y軸方向，再將分解的兩個分力與未加模糊控制前的分力相疊加，進(jìn)而調(diào)整機(jī)器人合力大小與方向，使得機(jī)器人能協(xié)助逃離局部最小點并且軌跡更加平滑穩(wěn)定。

2.1 模糊輸入

分別計算出引力、斥力夾角，根據(jù)夾角的定義，其為最小正角，所以要將計算出的夾角還原成真實的象限角。判斷出夾角處于的象限，若夾角處于一象限則不處理，若夾角處于二、三、四象限則象限角分別如下：

θ1=π-θ

(13)

θ1=π+θ

(14)

θ1=2π-θ

(15)

式中：θ1為象限角，θ為引力或斥力夾角。

計算出引力與斥力的合力差，并分別對角度差限幅(-π,π)，合力差限幅(-1,1)，超出部分等于邊界值。

將兩個模糊輸入分為五個模糊語言變量{負(fù)大(NB)，負(fù)中(NM)，零(Z)，正中(PM)，正大(PB)}，角度差論域為(-π,π)，合力差論域為(-1,1)，隸屬度函數(shù)采用三角形隸屬度函數(shù)，角度差θ與合力差e的隸屬度函數(shù)如圖2所示。

圖2 模糊輸入隸屬度函數(shù)

2.2 模糊決策

將設(shè)定好的模糊輸入通過表1給出的模糊規(guī)則決策，得到模糊輸出。因為使用的是TS模糊模型，模糊輸出值為確定常數(shù)值，不需要進(jìn)行解模糊化，且模糊輸出Ffu對應(yīng)的5個常數(shù)值分別為mf1(-1)，mf2(-0.5),mf3(0),mf4(0.5),mf5(1)。

表1 引力與斥力角度、合力差值模糊規(guī)則

2.3 合力重計算

為了使模糊輸出力對機(jī)器人有良好的效果，將其擴(kuò)大到50倍，并設(shè)定模糊輸出夾角為原合力夾角加0.5π：

θfu=θt+0.5π

(16)

式中：θfu為模糊輸出夾角，θt為原合力夾角。

將模糊輸出力分解到X軸分量Ffux和Y軸分量Ffuy，再與模糊控制前的分力相疊加，進(jìn)而形成新的合力F與合力夾角αt，公式如下：

Fx=Fattx+Frep1x+Frep2x+Ffux

(17)

Fy=Fatty+Frep1y+Frep2y+Ffuy

(18)

(19)

3 仿真實驗及分析

在Matlab軟件中進(jìn)行仿真實驗，并利用其中Fuzzy工具箱完成模糊控制系統(tǒng)設(shè)計。通過搭建一個10×10的地圖，添加好機(jī)器人坐標(biāo)、目標(biāo)點坐標(biāo)以及若干障礙物坐標(biāo)。對比以往勢場法與逐次改進(jìn)的方法，通過仿真得到路徑、合力、斥力圖，分析機(jī)器人陷入目標(biāo)不可達(dá)和局部極小值問題，以此證明本文方法的有效性。

首先進(jìn)行參數(shù)設(shè)置，為保證各方法具有可比性，初始參數(shù)設(shè)置保持一致。地圖均為10×10大小，因此起點位置和終點位置分別均為(0,0)、(10,10)，引力增益常數(shù)Katt為15，斥力增益常數(shù)Krep為5，障礙物最大影響距離P0為1，對數(shù)函數(shù)冪a取為0.5，機(jī)器人步長L為0.2，以K表示機(jī)器人到達(dá)目標(biāo)點的迭代次數(shù)。

3.1 散亂障礙物環(huán)境仿真

隨機(jī)添加散亂分布的障礙物，障礙物個數(shù)為15，對比傳統(tǒng)人工勢場法、改進(jìn)斥力人工勢場法、改進(jìn)人工勢場-模糊算法三種逐次改進(jìn)的仿真效果，如圖3所示。為便于觀察機(jī)器人軌跡及仿真效果，傳統(tǒng)人工勢場軌跡采用的是“.”表示方式，其他兩種仿真軌跡采用的是“.-”表示方式，即在“.-”處才是機(jī)器人真實軌跡。

圖3 散亂障礙物路徑規(guī)劃

傳統(tǒng)人工勢場法在目標(biāo)點前面臨較多密集障礙物時出現(xiàn)了容易極小問題，從而不能到達(dá)終點，如圖3(a)所示，并在中途出現(xiàn)了循環(huán)軌跡，其合力分析如圖4所示。由圖4可以看出，機(jī)器人在x=8 cm處合力非常小，使得機(jī)器人陷入了局部極小問題，以致于機(jī)器人不能達(dá)到目標(biāo)點。加入了對數(shù)函數(shù)及極小值判據(jù)的改進(jìn)算法，能有效避免目標(biāo)不可達(dá)，在局部極小值點時，通過改變斥力Y軸分量大小，有效逃離局部極小值點，順利到達(dá)目標(biāo)點，如圖3(b)所示。改進(jìn)人工勢場-模糊方法能夠自適應(yīng)調(diào)節(jié)受力大小和方向，路徑更加平滑，波動更少，穩(wěn)定性更強(qiáng)，如圖3(c)所示。三種遞增式方法的仿真效果對比見表2。

圖4 傳統(tǒng)人工勢場法合力圖

表2 散亂障礙物路徑規(guī)劃仿真參數(shù)統(tǒng)計表

3.2 凹型槽障礙物環(huán)境仿真

傳統(tǒng)人工勢場法、改進(jìn)斥力人工勢場法、改進(jìn)人工勢場-模糊算法在經(jīng)典凹槽障礙物模型下的對比仿真圖形如圖5所示。

圖5 凹型槽障礙物路徑規(guī)劃

在凹型槽障礙物環(huán)境下，傳統(tǒng)人工勢場法由于無法改變機(jī)器人的位姿狀態(tài)，一旦陷入了局部問題，很難逃離，如圖5(a)所示。經(jīng)過改進(jìn)斥力的方法，使機(jī)器人及時逃離困境，其能成功從凹槽中逃離出來并到達(dá)終點，如圖5(b)所示。在此基礎(chǔ)上融合模糊決策后的模糊力能自適應(yīng)調(diào)整機(jī)器人位姿，使得軌跡更加平滑，能更快、更穩(wěn)地到達(dá)目標(biāo)點，如圖5(c)所示。三種遞增式方法的仿真效果對比見表3。由表3可知，改進(jìn)后兩種方法均能逃離到達(dá)目標(biāo)點，但改進(jìn)人工勢場-模糊算法到達(dá)目標(biāo)點所用迭代次數(shù)更少。

表3 凹型槽障礙物路徑規(guī)劃仿真參數(shù)統(tǒng)計表

對能到達(dá)終點方法的斥力與合力分析，分別如圖6與圖7所示。對應(yīng)比較分析圖6與圖7兩方法的斥力與合力可知，在凹槽連續(xù)障礙物的作用下，加入了模糊力自適應(yīng)調(diào)整的機(jī)器人所受斥力和合力波動明顯減少，而且在接近凹槽末端拐角x=7 cm處，由于機(jī)器人需要做出較大角度的調(diào)整，加入模糊方法后可以避免陡然劇增的受力，使得機(jī)器人能夠更加安全、穩(wěn)定、快速地到達(dá)目標(biāo)點。

圖6 斥力圖

圖7 合力圖

4 結(jié)束語

傳統(tǒng)人工勢場法在移動機(jī)器人路徑規(guī)劃中容易出現(xiàn)局部極小值和目標(biāo)不可達(dá)問題，從而無法達(dá)到目標(biāo)點。本文在傳統(tǒng)人工勢場法的基礎(chǔ)上加入歐氏距離對數(shù)改進(jìn)斥力函數(shù)，避免出現(xiàn)目標(biāo)不可達(dá)，并通過機(jī)器人、障礙物、目標(biāo)點位置判據(jù)逃離局部極小值點。在此基礎(chǔ)上結(jié)合模糊算法，使得移動機(jī)器人在完成局部問題逃離的同時路徑更加穩(wěn)定平滑、到達(dá)終點的迭代次數(shù)更少，尤其在凹槽障礙物地圖中，有效解決了機(jī)器人徘徊及震蕩問題。但在極度簡單的障礙物環(huán)境和沒有出現(xiàn)目標(biāo)不可達(dá)、局部極小值問題下傳統(tǒng)人工勢場法仍快速有效。在MATLAB軟件中進(jìn)行了仿真，對傳統(tǒng)方法、改進(jìn)斥力人工勢場法和改進(jìn)人工勢場-模糊算法做了對比試驗，仿真結(jié)果證明了本文改進(jìn)方法的有效性。