基于改進(jìn)人工勢(shì)場(chǎng)法的小車避障路徑規(guī)劃研究

范玉龍，黃 波，周 仟，陳賀軍

（上海工程技術(shù)大學(xué)機(jī)械與汽車工程學(xué)院，上海 201620）

0 引言

路徑規(guī)劃任務(wù)是規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的安全無(wú)碰撞的最優(yōu)路徑。近幾年研究者不斷提出新方法應(yīng)用于各類路徑規(guī)劃，主要有柵格法、離散人工勢(shì)場(chǎng)法、模糊算法、模型預(yù)測(cè)算法和遺傳算法等。人工勢(shì)場(chǎng)法模型設(shè)計(jì)簡(jiǎn)單、易于理解，能夠規(guī)劃出較為平滑的路徑，同時(shí)具有很強(qiáng)的實(shí)時(shí)性，因此被廣泛應(yīng)用于機(jī)器人和智能車路徑規(guī)劃中。文獻(xiàn)［1］首次提出人工勢(shì)場(chǎng)法理論，其原理是將運(yùn)動(dòng)物體的運(yùn)動(dòng)空間看作虛擬力場(chǎng)空間，由目標(biāo)點(diǎn)吸引力和障礙物斥力的共同作用規(guī)劃物體運(yùn)動(dòng)路徑。文獻(xiàn)［2］分析了傳統(tǒng)人工勢(shì)場(chǎng)法存在的一些缺陷：當(dāng)目標(biāo)點(diǎn)附近有障礙物或障礙物處于運(yùn)動(dòng)物體與目標(biāo)點(diǎn)之間時(shí)會(huì)存在目標(biāo)不可達(dá)問(wèn)題；當(dāng)環(huán)境中某點(diǎn)的斥力與引力相等且方向相反時(shí)，物體會(huì)出現(xiàn)停擺不前而無(wú)法逃出局部極小值點(diǎn)的問(wèn)題。

對(duì)于上述傳統(tǒng)人工勢(shì)場(chǎng)法存在的問(wèn)題，很多學(xué)者都提出了改進(jìn)方法。如文獻(xiàn)［3］改進(jìn)了函數(shù)形式，通過(guò)選取適當(dāng)?shù)脑鲆嫦禂?shù)解決了目標(biāo)不可達(dá)問(wèn)題；文獻(xiàn)［4］在斥力勢(shì)場(chǎng)函數(shù)中加入目標(biāo)與障礙物距離調(diào)節(jié)因子，使得在目標(biāo)點(diǎn)附近斥力為零；文獻(xiàn)［5］提出在局部極小點(diǎn)改變斥力角度和設(shè)定虛擬最小區(qū)域的方案；文獻(xiàn)［6］針對(duì)機(jī)器人無(wú)法到達(dá)目標(biāo)點(diǎn)的問(wèn)題，提出改進(jìn)勢(shì)場(chǎng)模型中的斥力構(gòu)成以及采用虛擬水流法解決局部最小陷阱問(wèn)題；文獻(xiàn)［7］通過(guò)“沿邊規(guī)劃”解決局部極小值問(wèn)題；文獻(xiàn)［8］將模糊控制與人工勢(shì)場(chǎng)法相結(jié)合，在相應(yīng)時(shí)刻改變運(yùn)動(dòng)物體的合力大小及方向，從而避免在局部極小點(diǎn)出現(xiàn)震蕩；文獻(xiàn)［9］提出設(shè)置中間目標(biāo)點(diǎn)的方法，給機(jī)器人一個(gè)外力以確保機(jī)器人能夠逃出最小值陷阱；文獻(xiàn)［10］、［11］通過(guò)改變斥力解決合力為零的問(wèn)題，通過(guò)將斥力旋轉(zhuǎn)一定角度，依據(jù)障礙物距離大小采用相應(yīng)的斥力和引力，并設(shè)置障礙物影響范圍以解決極小值問(wèn)題；文獻(xiàn)［12］基于改進(jìn)人工勢(shì)場(chǎng)法提出一種救災(zāi)機(jī)器人路徑規(guī)劃方法，將障礙填充法與擾動(dòng)場(chǎng)法相結(jié)合以解決局部最小點(diǎn)問(wèn)題；文獻(xiàn)［13］重新定義了引力公式，以提高機(jī)器人避障和動(dòng)態(tài)目標(biāo)追蹤的靈活性；文獻(xiàn)［14］提出在動(dòng)態(tài)不確定環(huán)境下快速調(diào)整移動(dòng)路徑的方法，以調(diào)整當(dāng)前狀態(tài)與目標(biāo)狀態(tài)之間的移動(dòng)路徑；文獻(xiàn)［15］、［16］考慮機(jī)器人與目標(biāo)點(diǎn)的相對(duì)位置及相對(duì)速度因素，構(gòu)建引力勢(shì)場(chǎng)和引力函數(shù)，以提高動(dòng)態(tài)環(huán)境下機(jī)器人的避障能力；文獻(xiàn)［17］通過(guò)改變斥力方向和添加距離因子避免了傳統(tǒng)人工勢(shì)場(chǎng)法的目標(biāo)不可達(dá)問(wèn)題；文獻(xiàn)［18］基于最小轉(zhuǎn)向半徑對(duì)算法作進(jìn)一步改進(jìn)，以滿足小車的轉(zhuǎn)向行駛要求；文獻(xiàn)［19］引入斥力偏轉(zhuǎn)模型，增加了斥力增益系數(shù)函數(shù)，以優(yōu)化路徑規(guī)劃中航向改變過(guò)大的問(wèn)題；文獻(xiàn)［20］在斥力函數(shù)中引入距離影響因子，以解決目標(biāo)點(diǎn)旁存在障礙物時(shí)的目標(biāo)不可達(dá)問(wèn)題，通過(guò)引入動(dòng)態(tài)法向力以消除由單個(gè)障礙物形成的局部極值點(diǎn)振蕩。

本文基于改進(jìn)的人工勢(shì)場(chǎng)法，通過(guò)動(dòng)態(tài)改變障礙物影響范圍來(lái)解決目標(biāo)點(diǎn)附近存在障礙物時(shí)的目標(biāo)不可達(dá)問(wèn)題。對(duì)于局部極小值，當(dāng)小車陷入局部極小點(diǎn)時(shí)，通過(guò)設(shè)置虛擬障礙物，由虛擬障礙物提供額外的逃逸力以引導(dǎo)小車逃離局部極值點(diǎn)。通過(guò)設(shè)置合適的對(duì)比實(shí)驗(yàn)，在單個(gè)和多個(gè)障礙物的條件下，比較傳統(tǒng)算法和改進(jìn)算法的實(shí)際規(guī)劃效果，最后在MATLAB 仿真環(huán)境中對(duì)規(guī)劃算法進(jìn)行驗(yàn)證。

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

小車在虛擬勢(shì)場(chǎng)中運(yùn)動(dòng)時(shí)受到兩種勢(shì)場(chǎng)力影響：障礙物的斥力Fr，方向由障礙物指向小車；目標(biāo)點(diǎn)的引力Fa，方向由小車指向目標(biāo)點(diǎn)。其中，L0表示靜態(tài)障礙物作用范圍，受力情況如圖1 所示。

在二維空間中，忽略小車自身幾何大小，即把小車簡(jiǎn)化為質(zhì)點(diǎn)模型?？紤]到所有研究對(duì)象的平面坐標(biāo)，假設(shè)小車坐標(biāo)為L(zhǎng)=（xv，yv），目標(biāo)點(diǎn)位置坐標(biāo)為L(zhǎng)g=（xg，yg），定義目標(biāo)點(diǎn)對(duì)小車的引力勢(shì)場(chǎng)為兩者歐式距離的函數(shù)：

Fig.1 Schematic diagram of traditional artificial potential field method圖1 傳統(tǒng)人工勢(shì)場(chǎng)法原理示意圖

式中，η是引力勢(shì)場(chǎng)的系數(shù)，令上式中的相對(duì)距離為D：

對(duì)引力勢(shì)場(chǎng)Ua求解偏導(dǎo)數(shù)，即為小車所受目標(biāo)點(diǎn)的引力：

假設(shè)運(yùn)動(dòng)空間中障礙物坐標(biāo)為L(zhǎng)b=（xb，yb），則小車所受障礙物斥力的勢(shì)場(chǎng)函數(shù)為：

其中，μ是斥力勢(shì)場(chǎng)的系數(shù)，L0是斥力勢(shì)場(chǎng)的最大作用距離，Lvb是障礙物到小車的距離，即認(rèn)為在L0范圍內(nèi)時(shí)小車才會(huì)受到障礙物的斥力作用。Lvb可采用以下公式計(jì)算：

根據(jù)式（5）計(jì)算出運(yùn)動(dòng)小車受到的障礙物斥力為：

通過(guò)以上分析，小車在虛擬勢(shì)場(chǎng)中運(yùn)動(dòng)時(shí)假設(shè)受到n個(gè)障礙物的斥力作用，其所受合力為：

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

2.1 目標(biāo)不可達(dá)問(wèn)題

當(dāng)小車行駛的目標(biāo)位置附近存在障礙物時(shí)，小車受到的引力變得越來(lái)越小，障礙物的斥力變得越來(lái)越大。因此，小車可能會(huì)處于停止或振蕩狀態(tài)而不能到達(dá)目標(biāo)點(diǎn)（見(jiàn)圖2）。

Fig.2 Target unreachable diagram圖2 目標(biāo)不可達(dá)示意圖

在原斥力勢(shì)場(chǎng)函數(shù)中，通過(guò)改變障礙物影響范圍，使得目標(biāo)點(diǎn)不再受到障礙物斥力的作用，從而使小車?yán)^續(xù)向目標(biāo)點(diǎn)行進(jìn)。改進(jìn)后的斥力勢(shì)場(chǎng)函數(shù)如下：

式中，用新的范圍s 替代原先的障礙物影響范圍，使小車順利到達(dá)目標(biāo)。

2.2 局部極小點(diǎn)問(wèn)題

當(dāng)小車、障礙物、目標(biāo)點(diǎn)在同一條直線上，且障礙物處于中間位置時(shí)，小車會(huì)處于一個(gè)臨界狀態(tài)，此時(shí)小車會(huì)出現(xiàn)振蕩或停滯。當(dāng)小車陷入局部極小點(diǎn)時(shí)，將障礙物當(dāng)作質(zhì)點(diǎn)，以障礙物影響的最大距離為半徑畫圓。以小車質(zhì)心與障礙物質(zhì)心的連線作為參考線，從小車質(zhì)心作圓的切線，以與參考線夾角較小的切線為參考方向。然后在參考方向上設(shè)置虛擬障礙物，由虛擬障礙物提供額外的逃逸力引導(dǎo)小車逃離局部極小值點(diǎn)（見(jiàn)圖3）。

Fig.3 Schematic diagram of virtual obstacle method圖3 虛擬障礙物法示意圖

假設(shè)虛擬障礙物的斥力勢(shì)場(chǎng)函數(shù)為：

其中，μ′為大于零的虛擬斥力勢(shì)場(chǎng)常數(shù)，Lvb′為小車與虛擬障礙物之間距離，Lv是虛擬障礙物對(duì)小車的影響距離。只有小車與虛擬障礙物之間距離小于Lv時(shí)，虛擬障礙物才會(huì)對(duì)小車產(chǎn)生斥力，則虛擬斥力為：

為實(shí)現(xiàn)上述算法，設(shè)計(jì)算法流程如圖4 所示。

Fig.4 Algorithm flow圖4 算法流程

3 仿真模擬

實(shí)驗(yàn)步驟如下：①在MATLAB2018 中建立工作空間坐標(biāo)系，設(shè)置合理的汽車起點(diǎn)、目標(biāo)點(diǎn)、引力常數(shù)、斥力常數(shù)、障礙物個(gè)數(shù)、步長(zhǎng)、迭代次數(shù)等參數(shù)；②根據(jù)改進(jìn)的人工勢(shì)場(chǎng)法進(jìn)行引力、斥力與合力計(jì)算；③判斷是否存在局部極小點(diǎn)，若存在，則利用作切線的方法設(shè)置虛擬目標(biāo)點(diǎn)，通過(guò)虛擬斥力使得車輛逃離平衡點(diǎn)；④根據(jù)改進(jìn)的人工勢(shì)場(chǎng)法計(jì)算下一步要到達(dá)的坐標(biāo)，判斷目標(biāo)點(diǎn)附近是否存在影響車輛到達(dá)目標(biāo)的障礙物，若存在，動(dòng)態(tài)改變障礙物影響距離以消除斥力的作用，使得車輛可以順利抵達(dá)目標(biāo)點(diǎn)；⑤判斷計(jì)算出的坐標(biāo)點(diǎn)是否與目標(biāo)點(diǎn)重合，若重合，則結(jié)束算法流程，否則重新回到步驟③繼續(xù)計(jì)算，直到抵達(dá)目標(biāo)點(diǎn)為止。

為了驗(yàn)證上述算法在實(shí)際應(yīng)用中的有效性與可行性，在MATLAB 仿真環(huán)境下設(shè)計(jì)實(shí)驗(yàn)，分別用來(lái)說(shuō)明傳統(tǒng)人工勢(shì)場(chǎng)法與改進(jìn)后的人工勢(shì)場(chǎng)法在目標(biāo)不可達(dá)、局部極小點(diǎn)問(wèn)題上的對(duì)比。首先對(duì)原算法進(jìn)行仿真，可看出傳統(tǒng)人工勢(shì)場(chǎng)法存在目標(biāo)不可達(dá)問(wèn)題，這是因?yàn)槟繕?biāo)點(diǎn)附近存在障礙物，障礙物的斥力作用使得小車出現(xiàn)振蕩或停滯，從而導(dǎo)致小車不能順利到達(dá)目標(biāo)點(diǎn)?？紤]在單個(gè)障礙物情況下，傳統(tǒng)人工勢(shì)場(chǎng)法與改進(jìn)后的人工勢(shì)場(chǎng)法在目標(biāo)不可達(dá)問(wèn)題上的表現(xiàn)，仿真結(jié)果如圖5 所示。

Fig.5 Single obstacle target unreachable圖5 單個(gè)障礙物目標(biāo)不可達(dá)

通過(guò)分析可知，由于單個(gè)障礙物在目標(biāo)點(diǎn)附近對(duì)小車的斥力，導(dǎo)致其不能順利抵達(dá)目標(biāo)點(diǎn)，動(dòng)態(tài)改變障礙物影響距離后，小車可按照規(guī)劃出的平滑路徑到達(dá)目標(biāo)點(diǎn)。進(jìn)一步考慮在多個(gè)障礙物的情況下，改進(jìn)后的人工勢(shì)場(chǎng)法能否保證小車到達(dá)目標(biāo)點(diǎn)。為此，在單個(gè)障礙物基礎(chǔ)上添加多個(gè)障礙物，以研究在多個(gè)障礙物綜合影響下的小車運(yùn)動(dòng)情況。仿真結(jié)果如圖6 所示。

Fig.6 Multiple obstacles target unreachable圖6 多個(gè)障礙物目標(biāo)不可達(dá)

由圖6 可知，傳統(tǒng)人工勢(shì)場(chǎng)法規(guī)劃出的路徑不能到達(dá)目標(biāo)，目標(biāo)點(diǎn)附近存在兩個(gè)障礙物并且對(duì)小車的行進(jìn)造成了影響，因此紅線在兩個(gè)障礙物中間附近不能繼續(xù)前進(jìn)，小車在這里處于受力平衡狀態(tài)而出現(xiàn)停滯。藍(lán)線是改進(jìn)后的人工勢(shì)場(chǎng)法規(guī)劃路線，當(dāng)小車在兩個(gè)障礙物中間停滯不前時(shí)，通過(guò)動(dòng)態(tài)縮小障礙物影響范圍，使得小車能夠在引力作用下繼續(xù)行進(jìn)。由圖6 可以看到，小車能夠避開(kāi)目標(biāo)點(diǎn)附近障礙物，成功到達(dá)目標(biāo)點(diǎn)。

為驗(yàn)證局部極小點(diǎn)的路徑規(guī)劃問(wèn)題，考慮在目標(biāo)、障礙物、小車共線的情況下，單個(gè)與多個(gè)障礙物作用對(duì)最終結(jié)果的影響。首先，當(dāng)小車、障礙物、目標(biāo)點(diǎn)在同一條直線上，且障礙物處于中間位置時(shí)，小車在障礙物影響范圍內(nèi)會(huì)出現(xiàn)振蕩或停滯，因此不能繼續(xù)前進(jìn)。為驗(yàn)證改進(jìn)后的虛擬障礙物可以幫助小車逃離局部極小值點(diǎn)，在MATLAB中進(jìn)行仿真，結(jié)果如圖7 所示。

Fig.7 Local minima of single obstacle圖7 單個(gè)障礙物局部極小點(diǎn)

通過(guò)分析圖7 可知，改進(jìn)后的人工勢(shì)場(chǎng)法可以較為平滑地規(guī)劃出路徑，引導(dǎo)小車順利到達(dá)目標(biāo)點(diǎn)。

進(jìn)一步考慮在多個(gè)障礙物情況下的避障能力，仿真結(jié)果如圖8 所示。

Fig.8 Local minima of multiple obstacles圖8 多個(gè)障礙物局部極小點(diǎn)

通過(guò)進(jìn)一步分析可知，在多個(gè)障礙物存在的情況下，傳統(tǒng)人工勢(shì)場(chǎng)法由于陷入局部極小值點(diǎn)而不能到達(dá)目標(biāo)。改進(jìn)后的人工勢(shì)場(chǎng)法利用虛擬障礙物的逃逸力引導(dǎo)小車?yán)@開(kāi)局部極小點(diǎn)，可在多個(gè)障礙物的間隙中規(guī)劃出一條避開(kāi)局部極小點(diǎn)、整體比較平順、長(zhǎng)度合適的路徑。

4 結(jié)語(yǔ)

為解決傳統(tǒng)人工勢(shì)場(chǎng)法存在的目標(biāo)不可達(dá)、局部極小值等問(wèn)題，本文主要作了以下幾方面改進(jìn)：首先，通過(guò)動(dòng)態(tài)改變障礙物影響范圍，使得目標(biāo)點(diǎn)附近障礙物不影響小車?yán)^續(xù)前進(jìn)，并在具有單個(gè)和多個(gè)障礙物情況下進(jìn)行仿真，結(jié)果表明改進(jìn)后的算法能夠引導(dǎo)小車抵達(dá)目標(biāo)點(diǎn)；其次，當(dāng)小車陷入局部極小點(diǎn)時(shí)，將障礙物當(dāng)作質(zhì)點(diǎn)，以障礙物影響的最大距離為半徑畫圓，以小車質(zhì)心與障礙物質(zhì)心的連線作為參考線，從小車質(zhì)心作圓的切線，以與參考線夾角較小的切線為參考方向，之后在參考方向上設(shè)置虛擬障礙物，由虛擬障礙物提供的額外逃逸力引導(dǎo)小車逃離局部極小點(diǎn)。MATLAB 仿真結(jié)果表明了改進(jìn)后算法的有效性，但小車在實(shí)際復(fù)雜環(huán)境下的避障能力有待后續(xù)進(jìn)一步研究。