基于改進(jìn)人工勢場法的無人機(jī)三維動態(tài)環(huán)境避障航跡規(guī)劃

李克玉， 陸永耕， 鮑世通， 徐培真

(上海電機(jī)學(xué)院 電氣學(xué)院， 上海 201306)

避障航跡規(guī)劃是無人機(jī)巡檢電力線路的一個重要組成部分，該問題是指在未知障礙物的環(huán)境中，無人機(jī)能夠借助自身的傳感器及其給予的反饋，在一些約束條件下，規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)并成功繞過之間所有障礙物的無碰撞巡線路徑[1-2]。常用的避障算法有A*算法[3]、人工勢場法[4]、快速擴(kuò)展隨機(jī)樹法(RRT)[5]、遺傳算法[6]等。

傳統(tǒng)人工勢場法具有結(jié)構(gòu)簡單，易實時控制的優(yōu)點(diǎn)，規(guī)劃出的路徑一般比較平滑且安全；缺點(diǎn)是只考慮了躲避靜態(tài)障礙物的問題，對于動態(tài)障礙物避障則不適用，且存在局部最小點(diǎn)造成目標(biāo)不可達(dá)的問題。因此，國內(nèi)外很多學(xué)者對此缺點(diǎn)進(jìn)行了大量研究和改進(jìn)。貝前程等[7]在斥力勢場函數(shù)中加入機(jī)器人到目標(biāo)點(diǎn)的距離約束，隨機(jī)器人駛向目標(biāo)點(diǎn)時，斥力將減少，為解決局部最小點(diǎn)的問題提供了新方法。徐飛[8]提出一種引入相對速度的人工勢場法,有效解決了動態(tài)障礙物的避障問題。Yang等[9]引入機(jī)器人與障礙物的相對角度、速度和加速度作為約束條件對勢場函數(shù)進(jìn)行改進(jìn)，有效地解決因陷入局部最優(yōu)造成的目標(biāo)不可達(dá)問題。Geva等[10]通過在斥力勢場函數(shù)中添加動態(tài)斥力增益因子來解決目標(biāo)不可達(dá)問題。段建民等[11]除了改進(jìn)人工勢場模型之外，還融合遺傳算法的并行搜索方法，可實現(xiàn)在復(fù)雜的靜態(tài)和動態(tài)環(huán)境中的避障。

目前人工勢場法的改進(jìn)算法大多是基于二維動態(tài)環(huán)境實現(xiàn)，對目前日益多變的三維動態(tài)環(huán)境研究較少，且存在因陷入局部最小值陷阱而導(dǎo)致目標(biāo)不可達(dá)的問題，因此，在傳統(tǒng)的人工勢場法上提出一種新的改進(jìn)方法，該方法的主要思想是根據(jù)傳統(tǒng)人工勢場法的原理，重新定義引力和斥力的勢場函數(shù)，結(jié)合模糊算法，借助模糊規(guī)則計算出的模糊力，有效解決局部極小值的問題；引入無人機(jī)與障礙物的相對速度斥力勢場，推出總勢場函數(shù)，提高無人機(jī)在三維動態(tài)環(huán)境下的避障安全性和高效性。

1 傳統(tǒng)人工勢場法的基本原理

人工勢場法最先是由Khatib[12]于1986年提出，其基本思想是構(gòu)建一個虛擬的人工勢場，由目標(biāo)點(diǎn)周圍產(chǎn)生引力場，方向由無人機(jī)指向目標(biāo)點(diǎn)；障礙物周圍產(chǎn)生斥力場，方向由障礙物指向無人機(jī)，最終無人機(jī)在引力場和斥力場的疊加下向著勢場下降的方向來搜索一條無碰撞航跡[13]，如圖1所示。

圖1 人工勢場法無人機(jī)受力圖

傳統(tǒng)的人工勢場法的引力函數(shù)和斥力函數(shù)都與距離有一定的關(guān)系[14]。常用的引力場函數(shù)為

Uatt(x)=katt(Xu-Xg)2

(1)

相應(yīng)的無人機(jī)受到引力為

Fatt(x)=-ΔUatt(x)

(2)

式中：katt為引力增益系數(shù)；Xu(xu,yu,zu)為無人機(jī)的當(dāng)前位置；Xg(xg,yg,zg)為目標(biāo)點(diǎn)的位置；Xu-Xg為無人機(jī)與目標(biāo)點(diǎn)之間的歐幾里得距離。

常用的斥力場函數(shù)為

(3)

相應(yīng)的無人機(jī)受到斥力為

Frep(x)=-ΔUrep(x)

(4)

式中：krep為斥力增益系數(shù)；Xob(xob,yob,zob)為障礙物的位置；Xu-Xob為無人機(jī)與障礙物之間的歐幾里得距離；X0為障礙物的影響半徑。

2 人工勢場的改進(jìn)

由上述傳統(tǒng)人工勢場法的定義可知，斥力函數(shù)與無人機(jī)到障礙物的歐氏距離有關(guān)，且它們距離越近，無人機(jī)所受斥力勢能越大，且當(dāng)無人機(jī)所受勢能為零時，說明無人機(jī)已經(jīng)脫離了障礙物的影響。因此，改進(jìn)的斥力場函數(shù)為

(5)

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

(6)

考慮到無人機(jī)應(yīng)對碰撞時會設(shè)定安全距離d，如果無人機(jī)到障礙物的歐氏距離Xuob→d，則根據(jù)式(6)可知，斥力Frep將極大，為了使Frep連續(xù)，可將其修改為

(7)

由引力和斥力定義，可以得到無人機(jī)受到的合力為

Fsum(x)=[(Fatt(x)+Frep(x))2+(Fatt(y)+

Frep(y))2+(Fatt(z)+Frep(z))2]1/2

(8)

在實際中，對不同尺寸的無人機(jī)和不同的飛行速度，d以及X0的值需要進(jìn)行修正。

2.1 解決局部極小值問題

無人機(jī)在飛行時可能遇到多障礙物的影響，可能導(dǎo)致無人機(jī)飛行至某點(diǎn)時，無人機(jī)受到的吸引力和斥力相等，方向相反，導(dǎo)致無人機(jī)在此點(diǎn)所受到的合力為零，使得無人機(jī)誤以為到達(dá)目標(biāo)點(diǎn)而停滯不前，不知道下一步飛行方向，這就出現(xiàn)了無人機(jī)不能到達(dá)目標(biāo)點(diǎn)的局部極小值的現(xiàn)象[15]，如圖2所示，無人機(jī)在第3、4個障礙物處達(dá)到二力平衡，陷入局部極小值陷阱。由圖3可以看出，大概在58步左右，無人機(jī)所受合力急劇下降，最終消耗所有規(guī)劃步數(shù)也沒有逃離局部極小值陷阱。

2.1.1 引入模糊算法 為了解決上述問題，對于上述改進(jìn)的人工勢場法來說，可通過引入TS模糊算法來調(diào)節(jié)相關(guān)參數(shù)，使用模糊規(guī)則計算打破陷入局部極小值的局面。其中，模糊控制器的輸入需要包含d、Frep和Fatt這些可以影響無人機(jī)飛行狀態(tài)的參數(shù)，輸出應(yīng)該是更好地逃離局部極小值的方案。由式(4)和(5)可知，在陷入局部極小值前，無人機(jī)受到的引力和斥力存在差值Fd，合力Fsum也不為零。因此，選擇Fd的絕對值與合力的比值Fb

圖2 傳統(tǒng)人工勢場法

圖3 陷入極小值陷阱的合力大小

作為一個輸入量，論域控制在[-1,1]之間；另外將引力和斥力的角度差ξ作為另一個輸入量，論域在[-π,π]之間；定義模糊輸出量為模糊力Fz，將其分解到x軸和y軸方向，然后再與沒有加模糊之前的各軸分力相疊加，用來調(diào)整無人機(jī)合力的大小和方向，進(jìn)而幫助無人機(jī)逃脫局部極小值。

2.1.2 模糊計算 將Fb經(jīng)過模糊化后得M={NB,NM,NS,ZO,PS,PM,PB},NB、NM、NS、ZO、PS、PM、PB分別表示負(fù)大、負(fù)中、負(fù)小、零、正大、正中、正小，隸屬度函數(shù)如圖4所示。ξ經(jīng)過模糊化后得N={NB,NM,NS,PM,PS,PB},隸屬度函數(shù)如圖5所示。

圖4 Fb隸屬度函數(shù)

圖5 ξ隸屬度函數(shù)

其模糊規(guī)則如表1所示。由于人工勢場法在接近極小值陷阱時，引力和斥力的差值以及它們點(diǎn)的角度差較小，再乘以隸屬度就更小，這時得到的Fz在x軸和y軸的分力相對沒有加入模糊之前的x軸和y軸上的力小，導(dǎo)致Fz對幫助無人機(jī)逃離極小值陷阱起不到很大的作用，所以為了使模糊規(guī)則計算出的模糊力Fz發(fā)揮更好的效果，可將其適當(dāng)?shù)臄U(kuò)大倍數(shù)即nFz(n為常數(shù)，一般設(shè)置為20左右)，然后用于幫助無人機(jī)逃脫局部極小值陷阱。引入模糊算法的人工勢場法在局部極小值的受力情況如圖6所示。

表1 Fd、ξ模糊規(guī)則表

圖6 改進(jìn)算法后的無人機(jī)受力

圖7所示為改進(jìn)后的人工勢場-模糊算法成功繞開障礙物并最終到達(dá)目標(biāo)點(diǎn)，圖8給出了逃離極小值陷阱的合力大小。由圖8可知，當(dāng)在58步左右達(dá)到二力平衡，合力急劇下降，此時通過模糊力Fz的“拉拽”，使無人機(jī)在規(guī)定的規(guī)劃步數(shù)內(nèi)成功擺脫了極小值陷阱，并最終到達(dá)目標(biāo)點(diǎn)，從而驗證了改進(jìn)算法對解決局部極小值陷阱的有效性。

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

圖8 逃離極小值陷阱的合力大小

2.2 解決動態(tài)障礙物避障問題

為了解決無人機(jī)飛行環(huán)境中的動態(tài)障礙物避障的問題，在斥力場函數(shù)中引入無人機(jī)與動態(tài)障礙物的相對速度勢場，這樣使得無人機(jī)在運(yùn)動的過程中受到相對距離和速度的共同影響。無人機(jī)與動態(tài)障礙物的相對速度勢場為

Urep(v)=

(9)

相應(yīng)的速度斥力為

Frep(v)=-ΔUrep(v)=

(10)

式中：kv為速度斥力增益；vuob為無人機(jī)與動態(tài)障礙物的相對速度；Φ為無人機(jī)與障礙物相對速度和相對距離的夾角，當(dāng)Φ不在范圍內(nèi)時，表明動態(tài)障礙物朝遠(yuǎn)離無人機(jī)的方向運(yùn)動，此時無需考慮此障礙物的影響。

則加入速度斥力后的無人機(jī)總斥力為

Frep=Frep(x)+Frep(v)=

(11)

其總的受力如圖9所示。

圖9 無人機(jī)總受力

由傳統(tǒng)人工勢場法對障礙物產(chǎn)生斥力場的定義可知，其作用范圍是局部的。在相應(yīng)局部范圍內(nèi)，相對位置斥力方向由障礙物指向無人機(jī)，可使無人機(jī)遠(yuǎn)離障礙物；而相對速度斥力垂直于無人機(jī)與障礙物相對位置方向，當(dāng)Φ較小時，速度斥力幾乎垂直于無人機(jī)與障礙物的相對速度方向，能夠使無人機(jī)快速脫離動態(tài)障礙物威脅，提高避障航跡規(guī)劃效率。

3 實驗仿真結(jié)果及分析

針對局部極小值問題，相關(guān)改進(jìn)方法及仿真實驗已得到了驗證。針對動態(tài)障礙物避障的問題，在上述研究基礎(chǔ)上引入無人機(jī)與障礙物的相對速度斥力勢場，結(jié)合相對距離斥力勢場得到總的斥力勢場函數(shù)，可使無人機(jī)在飛行中更高效地脫離動態(tài)障礙物的影響。仿真環(huán)境設(shè)置為100 m×100 m×100 m的區(qū)域，在空間中隨機(jī)分布6個障礙物，相關(guān)信息如表2所示。

表2 障礙物相關(guān)信息表

表2中，障礙物3(以下簡稱“Obs3”)為動態(tài)障礙物，假設(shè)沿z軸負(fù)方向做勻速運(yùn)動，速度為2 m/s，各障礙物的影響半徑為(1.5×各障礙物半徑)m，人工勢場法的擴(kuò)展步長為2 m，無人機(jī)從起點(diǎn)(0,0,0)飛行到終點(diǎn)(100,100,100)。根據(jù)文獻(xiàn)[16]對人工勢場法相關(guān)參數(shù)的討論，本文設(shè)置相關(guān)參數(shù)為：引力增益katt取20，斥力增益krep取10，速度斥力增益kv取8，擴(kuò)大倍數(shù)n取20。在上述仿真三維環(huán)境下，無人機(jī)在傳統(tǒng)人工勢場法和改進(jìn)的人工勢場法的避障航跡仿真結(jié)果如圖10和11所示。其中，各球代表三維環(huán)境中分布的障礙物，標(biāo)注的“Obs3”為動態(tài)障礙物，沿z軸負(fù)方向勻速運(yùn)動，其正下方的兩球為運(yùn)動過程產(chǎn)生的球；由點(diǎn)組成的軌跡為各算法規(guī)劃的避障航跡，以及分別采樣生成避障航跡的總長度、節(jié)點(diǎn)數(shù)和無人機(jī)每個軌跡點(diǎn)到目標(biāo)點(diǎn)的距離，分別如圖12和表3所示。

圖10 傳統(tǒng)人工勢場法規(guī)劃航跡

圖11 改進(jìn)的人工勢場法規(guī)劃航跡

圖12 每個軌跡點(diǎn)到目標(biāo)點(diǎn)的距離

表3 避障航跡總長度和節(jié)點(diǎn)數(shù)

從圖10可以明顯地看出，傳統(tǒng)人工勢場法在遇到動態(tài)障礙物時會由于勢場力不足而無法大范圍改變無人機(jī)運(yùn)動方向，導(dǎo)致無人機(jī)只能沿著位置變動的“Obs3”產(chǎn)生的變化斥力和目標(biāo)點(diǎn)對其的引力而運(yùn)動，使無人機(jī)無法高效避障，從而航行到目標(biāo)點(diǎn)的步數(shù)增加；而改進(jìn)的人工勢場法，由于引入無人機(jī)與障礙物的相對速度斥力勢場，速度斥力幾乎垂直于無人機(jī)與障礙物的相對速度方向，使無人機(jī)能夠大范圍改變運(yùn)動方向，進(jìn)而使無人機(jī)快速脫離動態(tài)障礙物威脅，提高避障航跡規(guī)劃效率(見圖11)。由圖12和表3也明顯可以看出，傳統(tǒng)人工勢場法在遇到“Obs3”時，出現(xiàn)一個較長的規(guī)避路徑，最終導(dǎo)致軌跡節(jié)點(diǎn)數(shù)和總長度的增加，而改進(jìn)的人工勢場法有效地解決了這個問題，由此可以看出改進(jìn)的人工勢場法在三維動態(tài)環(huán)境中避障的有效性。

4 結(jié) 論

針對傳統(tǒng)人工勢場法在路徑搜索時容易產(chǎn)生局部極小值，以及在三維動態(tài)環(huán)境中不適用的問題，對傳統(tǒng)人工勢場法的基本勢場函數(shù)進(jìn)行改進(jìn)，結(jié)合模糊算法，借助模糊規(guī)則計算出的模糊力，有效地解決了局部極小值的問題；引入無人機(jī)與障礙物的相對速度勢場，推出總勢場函數(shù)，提高了動態(tài)障礙物避障的效率。在相同環(huán)境下進(jìn)行對比實驗，數(shù)據(jù)對比分析驗證了改進(jìn)的人工勢場法能夠更高效地避障，更平滑地到達(dá)目標(biāo)點(diǎn)。