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

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

引 言

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

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

1 人工勢(shì)場(chǎng)法

1.1 傳統(tǒng)人工勢(shì)場(chǎng)模型

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

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

(1)

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

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

(2)

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

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

Fatt(X)=-▽Uatt(X)

(3)

(4)

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

(5)

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

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

Frep(X)=-▽Urep(X)

(6)

(7)

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

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

(8)

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

圖1 機(jī)器人在新勢(shì)場(chǎng)中受力分析圖

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

(9)

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

(10)

(11)

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

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

(12)

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

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

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

2.1 模糊輸入

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

θ1=π-θ

(13)

θ1=π+θ

(14)

θ1=2π-θ

(15)

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

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

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

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

2.2 模糊決策

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

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

2.3 合力重計(jì)算

為了使模糊輸出力對(duì)機(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 仿真實(shí)驗(yàn)及分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖6 斥力圖

圖7 合力圖

4 結(jié)束語

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