改進(jìn)人工勢(shì)場(chǎng)法的室內(nèi)移動(dòng)機(jī)器人的局部規(guī)劃研究*

張鎧翔 姜文剛 薛 慧

（1.江蘇科技大學(xué)電子信息學(xué)院 鎮(zhèn)江 212000）（2.江蘇科技大學(xué)計(jì)算機(jī)學(xué)院 鎮(zhèn)江 212000）

1 引言

隨著機(jī)器人控制技術(shù)不斷發(fā)展，智能機(jī)器人被應(yīng)用到各行各業(yè)，并發(fā)揮著重要的作用，因此機(jī)器人的自主導(dǎo)航成為研究的熱點(diǎn)。而路徑規(guī)劃作為自主導(dǎo)航的核心技術(shù)主要解決機(jī)器人在靜態(tài)和動(dòng)態(tài)環(huán)境中如何行走問題，主要完成從起始點(diǎn)到目標(biāo)點(diǎn)搜索一條最優(yōu)的無障礙安全路徑［1］。路徑規(guī)劃可以分為完全已知環(huán)境下的全局規(guī)劃和部分已知環(huán)境下的的局部規(guī)劃。全局規(guī)劃是在完全已知的環(huán)境下進(jìn)行的，因此缺乏實(shí)時(shí)性與實(shí)用性。針對(duì)局部路徑規(guī)劃問題，目前方法有人工勢(shì)場(chǎng)法［2］、神經(jīng)網(wǎng)絡(luò)算法［3］、蟻群算法［4］、模糊控制［5］、RRT 算法［6］等。在這些局部規(guī)劃的方法中人工勢(shì)場(chǎng)法由于計(jì)算方便，實(shí)時(shí)性好，因此被研究者廣泛用于存在較多障礙物的未知環(huán)境下的路徑規(guī)劃。

目前移動(dòng)機(jī)器人應(yīng)用較多的環(huán)境是室內(nèi)，而針對(duì)室內(nèi)多障礙物，更加容易導(dǎo)致傳統(tǒng)人工勢(shì)場(chǎng)法的目標(biāo)不可達(dá)和局部最優(yōu)的問題，為解決這一問題，眾多學(xué)者對(duì)人工勢(shì)場(chǎng)法進(jìn)行深入研究，總結(jié)了四個(gè)改進(jìn)方向。一是基于傳統(tǒng)勢(shì)場(chǎng)模型，消除或逃拖局部極小值；二是改進(jìn)或使用新的勢(shì)場(chǎng)函數(shù)；三是融合其他算法結(jié)合；四是分步控制［7］。文獻(xiàn)［8］設(shè)置虛擬障礙物在極值點(diǎn)附近，解決傳統(tǒng)勢(shì)場(chǎng)力出現(xiàn)的局部最優(yōu)的問題。文獻(xiàn)［9］提出了一種基于柵格地圖并在蟻群算法中利用人工勢(shì)場(chǎng)重構(gòu)啟發(fā)函數(shù)去尋找最優(yōu)路徑的算法。文獻(xiàn)［10］通過改進(jìn)傳統(tǒng)人工勢(shì)場(chǎng)的斥力場(chǎng)結(jié)構(gòu)，解決目標(biāo)不可達(dá)問題。但由于室內(nèi)環(huán)境較為復(fù)雜，存在不同材質(zhì)與形狀的障礙物，這些方法都在遇到特殊障礙物的時(shí)候都無法起到作用。

針對(duì)室內(nèi)出現(xiàn)的V 型障礙物以及玻璃障礙物會(huì)導(dǎo)致應(yīng)用勢(shì)場(chǎng)法后的移動(dòng)機(jī)器人陷入局部極小值的問題，本文在加入機(jī)器人安全半徑的改進(jìn)斥場(chǎng)力函數(shù)的基礎(chǔ)上，設(shè)計(jì)判斷陷入局部極小值閾值因子，建立邊界虛擬目標(biāo)點(diǎn)的方法，用來解決局部極小值問題；同時(shí)加入自適應(yīng)逃脫步長因子，使機(jī)器人在建立虛擬拉點(diǎn)后讓機(jī)器人更快地逃脫極小值點(diǎn)。經(jīng)仿真實(shí)驗(yàn)得出改進(jìn)后的人工勢(shì)場(chǎng)法可以有效地解決了傳統(tǒng)人工勢(shì)場(chǎng)的極值點(diǎn)和目標(biāo)不可達(dá)問題。

2 傳統(tǒng)人工勢(shì)場(chǎng)法

2.1 傳統(tǒng)人工勢(shì)場(chǎng)法基本模型

傳統(tǒng)的人工勢(shì)場(chǎng)法因?yàn)槠洫?dú)特的優(yōu)點(diǎn)而被廣泛應(yīng)用，該算法起初是由Khatib.O［11］提出的。其主要思想是在將機(jī)器人的移動(dòng)環(huán)境虛擬成具有斥力與引力的人工勢(shì)場(chǎng)，將機(jī)器人所要到達(dá)的目標(biāo)點(diǎn)施加引力場(chǎng)，可以來牽引機(jī)器人，而在移動(dòng)的過程中遇到的障礙物則對(duì)機(jī)器人具有斥力作用，因此機(jī)器人在引力與斥力的共同作用下，躲避障礙物，一步一步地控制機(jī)器人到達(dá)目標(biāo)點(diǎn)。移動(dòng)機(jī)器人在運(yùn)動(dòng)過程的受力分析如圖1所示。

圖1 運(yùn)動(dòng)受力模型

設(shè)機(jī)器人的移動(dòng)空間大小為一個(gè)二維空間W=[X Y]T，機(jī)器人在空間的位置為p=(x，y)，目標(biāo)點(diǎn)的位置是pgoal=(xgoal，ygoal) ，障礙物的位置pobs=(xo，yo)，在勢(shì)場(chǎng)中機(jī)器人受到目標(biāo)點(diǎn)的牽引會(huì)生成引力場(chǎng)Uatt，障礙物對(duì)機(jī)器人的斥力作用則會(huì)使機(jī)器人產(chǎn)生斥力場(chǎng)為Urep。

2.2 引力函數(shù)與斥力函數(shù)

引力函數(shù)主要是根據(jù)人工勢(shì)場(chǎng)中引力與目標(biāo)和機(jī)器人距離成正比［12］。

引力勢(shì)場(chǎng)函數(shù)公式表示為

式中ε為引力勢(shì)能的增益系數(shù)；ρ(p，pgoal)為機(jī)器人與目標(biāo)點(diǎn)的相對(duì)距離。

根據(jù)引力場(chǎng)的負(fù)梯度得出引力Fatt為

斥力函數(shù)則是通過障礙物使機(jī)器人周圍產(chǎn)生一定的斥力場(chǎng)，對(duì)機(jī)器人具有斥力，斥力與機(jī)器人具障礙物的距離成反比。

斥力勢(shì)場(chǎng)函數(shù)表示為

式中，η為斥力勢(shì)能增益系數(shù)；ρ0為障礙物斥力場(chǎng)對(duì)機(jī)器人的影響距離，則表示當(dāng)機(jī)器人在影響距離內(nèi)收到斥力，影響距離外受到斥力為0；d=ρ(p，pobs)為障礙物與機(jī)器人的相對(duì)距離。

則通過斥力場(chǎng)的負(fù)梯度得出斥力公式為

由引力函數(shù)與斥力函數(shù)得出機(jī)器人受到的合力為

2.3 傳統(tǒng)人工勢(shì)場(chǎng)法存在的問題

通過進(jìn)行機(jī)器人路徑規(guī)劃和避障時(shí)，傳統(tǒng)人工勢(shì)場(chǎng)法一般存這三點(diǎn)問題。

1）當(dāng)機(jī)器人周圍的障礙物產(chǎn)生的斥力總和等于引力大小，并且方向是相反時(shí)，則機(jī)器人會(huì)陷入鎖死狀態(tài)無法移動(dòng)［13］。

2）在所要到達(dá)目標(biāo)點(diǎn)附近有一定數(shù)量的障礙物存在時(shí)，使得機(jī)器人目標(biāo)點(diǎn)的牽引力小于障礙物對(duì)機(jī)器人產(chǎn)生的斥力的總和，那么有可能使機(jī)器人無法到達(dá)目標(biāo)點(diǎn)［14］。

3）當(dāng)機(jī)器人在空間的某一點(diǎn)所受到的合力為零或者合力方向與引力和斥力共線時(shí)，機(jī)器人將會(huì)陷入局部極值點(diǎn)，將會(huì)停滯或在此處發(fā)生振蕩［15］。

4）由于每次規(guī)劃路徑的時(shí)候都要計(jì)算與障礙物的距離，因此隨著障礙物的增多，則會(huì)影響計(jì)算效率。

3 改進(jìn)傳統(tǒng)人工勢(shì)場(chǎng)法

3.1 改進(jìn)斥力函數(shù)

針對(duì)目標(biāo)不可達(dá)的問題，文獻(xiàn)［10］將目標(biāo)與機(jī)器人的相對(duì)距離引入到斥力函數(shù)中，可以很好地解決目標(biāo)達(dá)不到問題，本文在此基礎(chǔ)上給機(jī)器人加入安全半徑，其中障礙物只與加入安全半徑后機(jī)器人的邊界圈發(fā)生斥力場(chǎng)，可以避免引力過大造成機(jī)器人與障礙物碰撞，可以更好地遠(yuǎn)離障礙物，為機(jī)器人的路徑規(guī)劃提供一個(gè)更安全的路徑。

改進(jìn)后斥力場(chǎng)函數(shù)為

式中，η為斥力勢(shì)能增益系數(shù)；ρ0為障礙物斥力場(chǎng)對(duì)機(jī)器人的影響距離；dR=ρ(pR，pobs)為障礙物與機(jī)器人安全邊界的相對(duì)距離；n為大于零的調(diào)節(jié)參數(shù)。

得到改進(jìn)后的斥場(chǎng)力為

式中

其中Frep1力的方向是由障礙物指向移動(dòng)機(jī)器人，F(xiàn)rep2力的方向是由機(jī)器人指向目標(biāo)點(diǎn)。

3.2 局部最優(yōu)問題的改進(jìn)

3.2.1 局部極值點(diǎn)檢測(cè)

在判斷機(jī)器人是否陷入局部最優(yōu)造成停止或者徘徊的問題上，文獻(xiàn)［10］采用單位時(shí)間內(nèi)檢測(cè)運(yùn)動(dòng)距離，由于陷入局部震蕩時(shí)也存在距離，所以此方法存在一定的不合理性，因此本文采用機(jī)器人到目標(biāo)點(diǎn)的距離作為檢測(cè)的標(biāo)準(zhǔn)，只要機(jī)器人單位時(shí)間內(nèi)的，機(jī)器人距離目標(biāo)點(diǎn)的距離在這個(gè)閾值內(nèi)，則可判點(diǎn)是否陷入局部最優(yōu)。同時(shí)此方法在面對(duì)室內(nèi)的玻璃障礙物時(shí)也可以很好地判斷，由于激光測(cè)距儀是很難檢測(cè)到玻璃障礙物的，所以當(dāng)遇到玻璃障礙物的時(shí)候，移動(dòng)機(jī)器人也會(huì)陷入局部最優(yōu)，可以根據(jù)此方法來判定。

單位時(shí)間閾值表達(dá)式為

式中dg表示機(jī)器人與目標(biāo)點(diǎn)的距離；α表示單位時(shí)間內(nèi)機(jī)器人與目標(biāo)點(diǎn)距離徘徊的閾值。

3.2.2 局部最優(yōu)問題解決方案

在斥力函數(shù)中加入了目標(biāo)點(diǎn)與機(jī)器人的距離因子后雖然可以解決目標(biāo)不可達(dá)問題，在斥力勢(shì)場(chǎng)中引入距離因子可以解決目標(biāo)點(diǎn)附近有障礙物導(dǎo)致的目標(biāo)不可達(dá)問題，但是對(duì)于局部最優(yōu)的兩種情況是不適用的，需要進(jìn)一步改進(jìn)人工勢(shì)場(chǎng)模型。文獻(xiàn)［10］只是針對(duì)合力為零的情況分析，其模型具有局限性，因此本文將對(duì)兩種情況分析，并采用改進(jìn)后的人工勢(shì)場(chǎng)模型。

對(duì)偶存在模態(tài)算子

針對(duì)兩種情況本文將采用設(shè)置虛擬目標(biāo)點(diǎn)的方式，在虛擬目標(biāo)點(diǎn)作用的過程中，真正的目標(biāo)點(diǎn)不提供引力，障礙物也不會(huì)提供斥力，并且將機(jī)器人牽引出局部最優(yōu)區(qū)域后，恢復(fù)真正目標(biāo)點(diǎn)作用，虛擬目標(biāo)點(diǎn)消失。

1）目標(biāo)點(diǎn)、障礙物、機(jī)器人三個(gè)點(diǎn)共線。這種情況會(huì)導(dǎo)致機(jī)器人在局部極值點(diǎn)周圍徘徊。國內(nèi)外學(xué)者通過旋轉(zhuǎn)斥力法或增加逃逸力的方法解決局部最優(yōu)，但是具體的逃逸力參數(shù)沒有具體說明［7］。改進(jìn)后的人工勢(shì)場(chǎng)模型如圖2所示。

圖2 共線人工勢(shì)場(chǎng)模型

假設(shè)機(jī)器人的坐標(biāo)為A=(x0，y0)，且設(shè)機(jī)器人的初始方向是指向目標(biāo)點(diǎn)，障礙物的坐標(biāo)為B=（xobs，yobs），機(jī)器人的安全半徑為R，障礙物的似然半徑Robs。虛擬目標(biāo)點(diǎn)為機(jī)器人可到達(dá)的點(diǎn)C=(x，y)，與障礙物相切，Q1，Q2為切點(diǎn)，O為交點(diǎn)，E為圓心C投影到AB上的點(diǎn)，這樣可以使機(jī)器人以最短路徑到達(dá)虛擬目標(biāo)點(diǎn)。

則虛擬牽引點(diǎn)的坐標(biāo)函數(shù)求解如下：

虛擬目標(biāo)點(diǎn)與障礙物的距離為

由相似定理可得：

由勾股定理得

則

相應(yīng)的引力函數(shù)為

其中Fvir_att為虛擬目標(biāo)點(diǎn)的引力；pvir為虛擬目標(biāo)點(diǎn)；ρ(p，pvir)為機(jī)器人與虛擬目標(biāo)點(diǎn)的距離。

障礙物與機(jī)器人的內(nèi)切線還有與圖中對(duì)稱的一條，同理可求得另一個(gè)虛擬目標(biāo)點(diǎn)，人工勢(shì)場(chǎng)法過程選其中一個(gè)即可。

2）由于室內(nèi)障礙物較多經(jīng)常會(huì)堆積出各種形狀阻礙移動(dòng)機(jī)器人，不難發(fā)現(xiàn)大多數(shù)障礙物的堆積都會(huì)成V 型區(qū)域，且V 型區(qū)域中障礙物之間的距離是機(jī)器人無法通過的，而這類障礙物往往會(huì)導(dǎo)致機(jī)器人引力與斥力相抵消陷入局部極小值，因此加入虛擬目標(biāo)點(diǎn)，建立改進(jìn)的人工勢(shì)場(chǎng)模型。改進(jìn)的人工勢(shì)場(chǎng)模型如圖3所示。

圖3 V型區(qū)域人工勢(shì)場(chǎng)模型

針對(duì)V 型區(qū)域虛擬目標(biāo)點(diǎn)的設(shè)置需要先進(jìn)行最優(yōu)障礙物的檢索，本文提出一種最優(yōu)虛擬目標(biāo)點(diǎn)的搜索法。具體步驟如下：

以障礙物、機(jī)器人、目標(biāo)點(diǎn)形成的夾角為第一判斷標(biāo)準(zhǔn)，夾角最大的障礙物旁邊設(shè)置虛擬目標(biāo)點(diǎn)，且夾角范圍設(shè)定為[0，π）。

根據(jù)1）的判定方法，可能存在多個(gè)符合1）標(biāo)準(zhǔn)的障礙物，則進(jìn)行第二個(gè)評(píng)判標(biāo)準(zhǔn)，通過判斷這些障礙物與機(jī)器人的距離，找出與機(jī)器距離最短的障礙物。

在此處找出的障礙物周圍設(shè)置虛擬目標(biāo)點(diǎn)，由于按照第一種情況的方法設(shè)定虛擬目標(biāo)點(diǎn)可能會(huì)使機(jī)器人再次陷入局部極值點(diǎn)，本文結(jié)合第一種情況方法進(jìn)行優(yōu)化，將虛擬目標(biāo)點(diǎn)設(shè)在目標(biāo)點(diǎn)與障礙物的連線與機(jī)器人與障礙物的切線的交點(diǎn)上，計(jì)算過程在情況一的基礎(chǔ)上稍作改進(jìn)。這樣可以保證在多障礙物的環(huán)境下，機(jī)器人可以順利地到達(dá)目標(biāo)點(diǎn)。

3.2.3 自適應(yīng)逃脫步長因子

為了提高逃脫局部最優(yōu)點(diǎn)的效率，本文設(shè)計(jì)一種自適應(yīng)步長因子，當(dāng)檢測(cè)到機(jī)器人陷入局部最優(yōu)時(shí)，步長將會(huì)自動(dòng)調(diào)整，增大步長，幫助機(jī)器人快速到達(dá)虛擬目標(biāo)點(diǎn)，逃脫局部最優(yōu)。

改進(jìn)后的步長如：

式中L0正常規(guī)劃的固定步長；ω為開關(guān)值；k任意大于的調(diào)節(jié)參數(shù)；ρ(p，pvir)為機(jī)器人與虛擬目標(biāo)點(diǎn)的距離，加入到指數(shù)函數(shù)中可以形成衰減趨勢(shì)，保證逃脫速率的穩(wěn)定性。

4 仿真結(jié)果與分析

為了驗(yàn)證本文提出的改進(jìn)的人工勢(shì)場(chǎng)法在路徑規(guī)劃的可行性。在Matlab2018a仿真軟件上分別改進(jìn)前后的人工勢(shì)場(chǎng)法進(jìn)行仿真對(duì)比，仿真環(huán)境為14m*14m，起始點(diǎn)為（0，0），目標(biāo)點(diǎn)為（10，10）。

仿真參數(shù)如表1所示。

表1 仿真基本參數(shù)

4.1 目標(biāo)點(diǎn)未到達(dá)

1）在目標(biāo)點(diǎn)（10，10）的附近存在障礙物，進(jìn)行傳統(tǒng)人工勢(shì)場(chǎng)法仿真，得出機(jī)器人在并沒有到達(dá)目標(biāo)點(diǎn)。仿真結(jié)果如圖4所示。

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

2）下面進(jìn)行改進(jìn)后的人工勢(shì)場(chǎng)法的仿真，其中添加了障礙物的似然半徑與機(jī)器人的安全半徑?；疑珗A圈為機(jī)器人的安全半徑，黑色的為障礙物的似然半徑。仿真結(jié)果如圖5所示。

圖5 改進(jìn)人工勢(shì)場(chǎng)法

從改進(jìn)前后的對(duì)比圖可以看出，在斥力函數(shù)中加入了目標(biāo)點(diǎn)與機(jī)器人的相對(duì)距離之后，機(jī)器人可以平穩(wěn)地到達(dá)目標(biāo)點(diǎn)。引入了機(jī)器人的似然半徑和機(jī)器人的安全半徑之后，保證了機(jī)器人與障礙物的距離，大大降低了機(jī)器人的碰撞幾率。

根據(jù)上面實(shí)驗(yàn)對(duì)比可以得出以下數(shù)據(jù)對(duì)比。改進(jìn)前后性能對(duì)比表如表2所示。

表2 改進(jìn)前后性能對(duì)比

由規(guī)劃長度和迭代次數(shù)可知，傳統(tǒng)的人工勢(shì)場(chǎng)在目標(biāo)點(diǎn)附近發(fā)生振蕩導(dǎo)致無法到達(dá)目標(biāo)點(diǎn)。而改進(jìn)后的人工勢(shì)場(chǎng)法規(guī)劃長度為17.6m，迭代次數(shù)44 次，運(yùn)行時(shí)間為1.0545s 成功地使機(jī)器人到達(dá)目標(biāo)點(diǎn)。

4.2 局部極值點(diǎn)

1）局部最優(yōu)情況一仿真如圖6所示。

圖6 情況一

當(dāng)障礙物和機(jī)器人，目標(biāo)點(diǎn)三點(diǎn)共線的時(shí)候，斥力與引力方向在同一線上，這個(gè)時(shí)候機(jī)器人在局部極值點(diǎn)徘徊。

根據(jù)改進(jìn)后的人工勢(shì)場(chǎng)法成功繞過障礙物到達(dá)目標(biāo)點(diǎn)，仿真結(jié)果如圖7所示。

圖7 情況一改進(jìn)方案

可見采用在障礙物與機(jī)器人內(nèi)切線位置設(shè)置虛擬目標(biāo)點(diǎn)改進(jìn)人工勢(shì)場(chǎng)法可以有效逃脫局部極小值。

2）局部最優(yōu)情況二，當(dāng)機(jī)器人身處在V 型區(qū)域的障礙物中，多個(gè)障礙物的合斥力與目標(biāo)點(diǎn)的引力相抵消。機(jī)器會(huì)陷入局部最優(yōu)出現(xiàn)停止與振蕩，無法進(jìn)行下一步運(yùn)動(dòng)。如圖8所示。

圖8 V型區(qū)域局部最優(yōu)

通過本文提出的搜索方案確定最優(yōu)障礙物，并將虛擬目標(biāo)點(diǎn)設(shè)在機(jī)器人與最優(yōu)障礙物的內(nèi)切線與最優(yōu)障礙物和目標(biāo)點(diǎn)的連線的交點(diǎn)上可以有效使機(jī)器人逃脫V型區(qū)域。如圖9所示。

圖9 V型區(qū)域改進(jìn)方案

為了加快逃脫局部最優(yōu)限制，在改進(jìn)的基礎(chǔ)上引入自適應(yīng)逃脫步長因子，提高虛擬目標(biāo)點(diǎn)的作用效率，運(yùn)行軌跡相對(duì)平穩(wěn)。當(dāng)達(dá)到虛擬目標(biāo)點(diǎn)的時(shí)候，步長因子恢復(fù)到固定步長。如圖10所示。

圖10 融入自適應(yīng)逃脫半徑改進(jìn)方案

對(duì)比加入自適應(yīng)逃脫步長因子后，規(guī)劃長度比加入后縮短了0.4813m，迭代次數(shù)少了4 次，運(yùn)行效率相對(duì)提高了一點(diǎn)，可見加入了自適應(yīng)半徑后，整體規(guī)劃效率明顯提高。

表3 改進(jìn)前后數(shù)據(jù)對(duì)比

5 結(jié)語

本文主要針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法存在的目標(biāo)不可達(dá)以及局部極值點(diǎn)的問題提出相應(yīng)的改進(jìn)策略。本文在傳統(tǒng)人工勢(shì)場(chǎng)的模型中給機(jī)器人加入了安全半徑以及障礙物的似然半徑，為了保證機(jī)器人與障礙物的距離，在此基礎(chǔ)上給人工勢(shì)場(chǎng)模型的斥力勢(shì)能函數(shù)中加入目標(biāo)點(diǎn)與機(jī)器人的距離來解決機(jī)目標(biāo)點(diǎn)附近有障礙物的導(dǎo)致目標(biāo)問題不可到達(dá)問題。在解決局部極小值點(diǎn)導(dǎo)致機(jī)器人停止或徘徊無法進(jìn)行下一步規(guī)劃的問題上，本文創(chuàng)新性的設(shè)計(jì)判斷機(jī)器人是否陷入局部極小值的閾值，其中此判斷方法在玻璃障礙物也一樣適用，并提出一種設(shè)置虛擬目標(biāo)點(diǎn)方法，將機(jī)器人拉出局部極小值點(diǎn)，并通過兩種常見的障礙物區(qū)域進(jìn)行具體分析來確定虛擬目標(biāo)點(diǎn)，其中主要采用切線法與搜索法相結(jié)合的方法來確定。并引入自適應(yīng)逃脫步長因子幫助機(jī)器人快速逃脫局部極值。通過仿真驗(yàn)證，結(jié)果表明本算法的可行性和優(yōu)越性。本研究為室內(nèi)移動(dòng)機(jī)器人在實(shí)際環(huán)境中的應(yīng)用提供了一定的理論基礎(chǔ)。