基于改進(jìn)人工勢(shì)場(chǎng)法的四旋翼無(wú)人機(jī)航跡規(guī)劃算法

鄧 葉, 姜香菊

(蘭州交通大學(xué) 自動(dòng)化與電氣工程學(xué)院,甘肅 蘭州 730070)

0 引 言

航跡規(guī)劃在四旋翼無(wú)人機(jī)高效與安全飛行中具有很重要的作用,它是指四旋翼無(wú)人機(jī)按照某種要求規(guī)劃出一條從起始點(diǎn)到終止點(diǎn)最優(yōu)或者次優(yōu)的航跡,是四旋翼無(wú)人機(jī)研究領(lǐng)域的一個(gè)重要分支。目前常用的航跡規(guī)劃方法包括：RRT算法[1]、粒子群算法[2]、蟻群算法[3,4]、人工蜂群算法[5]和人工勢(shì)場(chǎng)法[6～9]等。其中人工勢(shì)場(chǎng)法在航跡規(guī)劃中具有比較成熟的應(yīng)用,因其具有計(jì)算量小,規(guī)劃速度快的特點(diǎn)。雖然人工勢(shì)場(chǎng)法算法簡(jiǎn)便,但是也存在一些需要解決的問(wèn)題,如目標(biāo)不可達(dá)、振蕩等問(wèn)題。

文獻(xiàn)[10]提出了增加垂直引導(dǎo)斥力解決了傳統(tǒng)人工勢(shì)場(chǎng)法的缺陷,并且比較和分析了人工勢(shì)場(chǎng)、模糊邏輯和蟻群算法三種算法的性能,對(duì)于研究局部路徑規(guī)劃有很重要的參考價(jià)值。文獻(xiàn)[11]為了解決極小值問(wèn)題提出了“選擇穿越法”,當(dāng)四旋翼無(wú)人機(jī)陷入極小值狀態(tài)時(shí)迅速激活該行為,使四旋翼無(wú)人機(jī)有選擇性的穿越過(guò)障礙物,從而順利的到達(dá)目標(biāo)點(diǎn)。文獻(xiàn)[12]對(duì)于復(fù)雜環(huán)境下的航跡規(guī)劃問(wèn)題,首先建立了柵格化環(huán)境模型,然后將蟻群算法和人工勢(shì)場(chǎng)法結(jié)合,設(shè)計(jì)了環(huán)境感知因子,仿真結(jié)果表明了所提方法的有效性。文獻(xiàn)[13]將稀疏A*搜索算法和改進(jìn)的人工勢(shì)場(chǎng)法相結(jié)合,應(yīng)用在不同的障礙物所構(gòu)成的威脅分布中,規(guī)劃出了給定威脅指標(biāo)下的最優(yōu)路徑,結(jié)果證明了其具有良好的動(dòng)態(tài)規(guī)避性能。

四旋翼無(wú)人機(jī)在飛行過(guò)程中會(huì)受到很多因素的約束,本文在考慮了這些約束的條件下,首先以安全性為前提,提出了一種改進(jìn)的人工勢(shì)場(chǎng)法,即在斥力勢(shì)場(chǎng)函數(shù)中加入了四旋翼無(wú)人機(jī)當(dāng)下航跡點(diǎn)和目標(biāo)點(diǎn)之間的距離,并且引入了協(xié)調(diào)力,在隨機(jī)分布的障礙物環(huán)境和U型槽型障礙物環(huán)境下進(jìn)行了仿真驗(yàn)證。

1 環(huán)境建模與約束條件

1.1 環(huán)境建模

假設(shè)四旋翼無(wú)人機(jī)的任務(wù)區(qū)域?yàn)檎叫?并且以恒定的速度從起始點(diǎn)飛往目標(biāo)點(diǎn)。不考慮任務(wù)區(qū)域的高度,將任務(wù)區(qū)域分為X和Y方向,進(jìn)行m等分,則任務(wù)區(qū)域可以表示成由(m+1)個(gè)節(jié)點(diǎn)構(gòu)成的柵格。在這些柵格內(nèi)存在n個(gè)障礙物,四旋翼無(wú)人機(jī)在飛行的過(guò)程中需要避開這些障礙物,安全的到達(dá)目標(biāo)點(diǎn)。

1.2 約束條件

1)最大曲率約束:由于四旋翼無(wú)人機(jī)自身機(jī)動(dòng)性能的約束,不能在過(guò)小的轉(zhuǎn)彎半徑內(nèi)改變航向,為了能使航跡滿足無(wú)人機(jī)的各種條件要求,對(duì)得到的航跡需要進(jìn)行曲線擬合,然后計(jì)算擬合曲線上各點(diǎn)是否滿足四旋翼無(wú)人機(jī)的曲率要求,在不滿足條件的情況下對(duì)規(guī)劃的航跡進(jìn)行平滑處理,直到滿足需求為止[14]。假設(shè)四旋翼無(wú)人機(jī)飛行速度為v,最大曲率為kmax,四旋翼無(wú)人機(jī)的過(guò)載限制為n,則曲率與最小轉(zhuǎn)彎半徑Rmin之間滿足的關(guān)系為

(1)

2)最大航程約束:受限于四旋翼無(wú)人機(jī)電量的約束,無(wú)人機(jī)必須在允許的最大航程和最長(zhǎng)時(shí)間范圍內(nèi)完成任務(wù),這就使得所規(guī)劃的航跡要盡可能的短,該約束可以表示為L(zhǎng)

1.3 航跡代價(jià)

航跡代價(jià)是用來(lái)評(píng)價(jià)四旋翼無(wú)人機(jī)航跡規(guī)劃的優(yōu)劣,在無(wú)人機(jī)實(shí)際飛行過(guò)程中面臨的環(huán)境是復(fù)雜多樣的,所以,應(yīng)該多方面考慮各種因素對(duì)四旋翼無(wú)人機(jī)航跡的影響。假設(shè)不考慮四旋翼無(wú)人機(jī)飛行高度且在恒速的狀態(tài)下飛行,為了能夠縮短四旋翼無(wú)人機(jī)的飛行時(shí)間進(jìn)而減少能量的消耗,航跡長(zhǎng)度和避障是本文評(píng)價(jià)航跡優(yōu)劣的兩個(gè)考慮指標(biāo)。為了方便分析不同航跡規(guī)劃方法的優(yōu)劣,這里引入代價(jià)函數(shù)

J=∑(w1Lij+w2fij)

(2)

式中w1和w2分別為航跡代價(jià)和威脅代價(jià)的權(quán)重,fij為安全代價(jià),用航跡在障礙物影響區(qū)域內(nèi)的長(zhǎng)度表示；Lij為航線段的歐氏距離,總和為航跡的總長(zhǎng)度。

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

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

人工勢(shì)場(chǎng)法是由Khatib提出的一種虛擬力法,該方法在目標(biāo)點(diǎn)位置構(gòu)造了虛擬的引力場(chǎng),在障礙物位置構(gòu)造了虛擬的斥力場(chǎng)。目標(biāo)點(diǎn)對(duì)被控對(duì)象產(chǎn)生引力,障礙物對(duì)被控對(duì)象產(chǎn)生斥力。

假設(shè)四旋翼無(wú)人機(jī)的當(dāng)前位置為X=(x,y),目標(biāo)點(diǎn)的位置為Xg=(xg,yg)。則四旋翼無(wú)人機(jī)受到的引力場(chǎng)函數(shù)Uatt為

(3)

式中k為引力場(chǎng)增益。

四旋翼無(wú)人機(jī)受到的引力Fatt(X)為引力勢(shì)場(chǎng)函數(shù)的負(fù)梯度

(4)

假設(shè)障礙物的位置坐標(biāo)為Xo=(xo,yo),無(wú)人機(jī)到障礙物的距離為

ρ(X,Xo)=‖X-Xo‖

(5)

定義斥力勢(shì)場(chǎng)函數(shù)為

(6)

式中m為斥力系數(shù),ρo為障礙物的影響范圍。由此斥力可定義為

(7)

人工勢(shì)場(chǎng)法算法簡(jiǎn)單、實(shí)時(shí)性好,但是容易使四旋翼無(wú)人機(jī)陷入局部最小點(diǎn)的狀態(tài)無(wú)法出來(lái),難以到達(dá)目標(biāo)點(diǎn)。

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

本文提出了一種改進(jìn)的人工勢(shì)場(chǎng)法,解決了四旋翼無(wú)人機(jī)無(wú)法到達(dá)目標(biāo)點(diǎn)的問(wèn)題[15]。在斥力勢(shì)場(chǎng)函數(shù)中引入四旋翼無(wú)人機(jī)和目標(biāo)點(diǎn)之間的距離,改進(jìn)后的斥力勢(shì)場(chǎng)函數(shù)如下

(8)

式中 (X-Xg)t=|(x-xg)t|+|(y-yg)t|為四旋翼無(wú)人機(jī)和目標(biāo)點(diǎn)之間的相對(duì)距離,t為大于零的可變常數(shù),一般的取值為2。則對(duì)應(yīng)得到的斥力為

(9)

式中

(10)

(11)

在實(shí)際的應(yīng)用中通常采用它們的簡(jiǎn)化形式

(12)

(13)

此外引入?yún)f(xié)調(diào)力

(14)

式中g(shù)為協(xié)調(diào)增益系數(shù)。

由此可以得到總的合力F(X)為

F(X)=Fatt(X)+Fre(X)+Fco(X)

(15)

3 航跡修正

3.1 航跡簡(jiǎn)化

在利用改進(jìn)的人工勢(shì)場(chǎng)法得到航跡后,發(fā)現(xiàn)航跡存在一些不必要的航跡點(diǎn),從而使得規(guī)劃出來(lái)的航跡并不是最短的航跡。因此,可以進(jìn)一步對(duì)航跡點(diǎn)進(jìn)行裁減處理。航跡點(diǎn)裁減流程圖如圖1所示。

圖1 航跡點(diǎn)裁減流程圖

3.2 航跡平滑

本文采用最小二乘法消除離散點(diǎn),對(duì)采用改進(jìn)人工勢(shì)場(chǎng)法得到的航跡進(jìn)行修正,主要針對(duì)航跡轉(zhuǎn)彎角度較大和振蕩的問(wèn)題,通常四旋翼無(wú)人機(jī)的轉(zhuǎn)彎角度限制在±90°范圍內(nèi)。對(duì)于擬合得到的新航跡需要分析是否符合航跡約束條件,若滿足則采用修正好的航跡,若不滿足則需要重新進(jìn)行修正,直至滿足航跡規(guī)劃的約束要求。

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

為了驗(yàn)證所述改進(jìn)算法的合理性,首先在MATLAB 2014b平臺(tái)上進(jìn)行了仿真,任務(wù)區(qū)域?yàn)?0×20 的柵格環(huán)境,在不同的障礙物條件下的仿真結(jié)果如圖2～圖5所示。

4.1 隨機(jī)分布的障礙物

圖2是利用傳統(tǒng)人工勢(shì)場(chǎng)法和改進(jìn)的人工勢(shì)場(chǎng)法在兩種隨機(jī)分布的障礙物情況下規(guī)劃的四旋翼無(wú)人機(jī)航跡,實(shí)線為傳統(tǒng)的人工勢(shì)場(chǎng)法得到的航跡,從圖2中可以看出,在點(diǎn)(1300,1300)處四旋翼無(wú)人機(jī)陷入了局部最小,無(wú)法按計(jì)劃到達(dá)目標(biāo)點(diǎn)；虛線為采用了在斥力勢(shì)場(chǎng)函數(shù)中加入了四旋翼無(wú)人機(jī)航跡點(diǎn)和目標(biāo)點(diǎn)之間的距離同時(shí)引入?yún)f(xié)調(diào)力的方法所得到的航跡,改進(jìn)后的算法使四旋翼無(wú)人機(jī)到達(dá)了設(shè)定的目標(biāo)點(diǎn)。

圖2 隨機(jī)分布的障礙物

改進(jìn)后的人工勢(shì)場(chǎng)法參數(shù)設(shè)定為(a)k=215,m=400,t=20,總的航跡長(zhǎng)度為2.950 8 km; (b)k=390,m=1 000,t=1.3,總的航跡長(zhǎng)度為2.958 5 km。

4.2 三種不同的U型槽障礙物

1)U型槽向左

針對(duì)圖3(a)所示的U型槽環(huán)境,改進(jìn)的人工勢(shì)場(chǎng)法設(shè)定的參數(shù)為k=390,m=800,t=9.17。設(shè)置蟻群算法參數(shù)為α=1,β=7,ρ=0.3,Q=1,M=50,K=100。

圖3 U型槽向左時(shí)

2)U型槽向右

針對(duì)圖4(a)所示的U型槽環(huán)境,改進(jìn)的人工勢(shì)場(chǎng)法設(shè)定的參數(shù)為k=500,m=1 000,t=11。設(shè)置蟻群算法參數(shù)為α=1,β=7,ρ=0.3,Q=1,M=50,K=100。

圖4 U型槽向右時(shí)

3)U型槽向下

針對(duì)圖5(a)所示的U型槽環(huán)境,改進(jìn)的人工勢(shì)場(chǎng)法設(shè)定的參數(shù)為k=200,m=500,t=10。設(shè)置蟻群算法參數(shù)為α=1,β=7,ρ=0.3,Q=1,M=50,K=100。

圖5 U型槽向下時(shí)

圖3(a)，圖4(a)和圖5(a)得到的是分別采用改進(jìn)的人工勢(shì)場(chǎng)法和蟻群算法在三種不同的U型槽下的航跡,由于采用改進(jìn)的人工勢(shì)場(chǎng)法得到的航跡不夠平滑,存在振蕩和轉(zhuǎn)彎角過(guò)大的情況,以上三幅圖都是用了最小二乘法進(jìn)行曲線平滑處理后得到的航跡。表1為采用不同方法得到的航跡長(zhǎng)度對(duì)比。

表1 航跡長(zhǎng)度對(duì)比 km

分析蟻群算法的收斂曲線圖可以看出,當(dāng)U型槽向右時(shí)即圖4(b),算法具有很好的收斂性,在迭代到第22次時(shí),螞蟻就找到了最短路徑。但如圖3(b)所示,算法的收斂性不是很好,在找到最短路徑后又出現(xiàn)了振蕩,在迭代到第181次時(shí),規(guī)劃的路徑變成了4.169 8 km,這與最開始收斂穩(wěn)定規(guī)劃好的航跡相比存在很大的差距,無(wú)法滿足四旋翼無(wú)人機(jī)的最大航跡約束要求。從圖5(b)可以看出在迭代到第52次時(shí)螞蟻已經(jīng)找到了最短路徑為2.997 9 km,但是隨著迭代次數(shù)的增加最小路徑的長(zhǎng)度不斷變化,收斂曲線存在很嚴(yán)重的不穩(wěn)定問(wèn)題,雖然最終在第154次的時(shí)候趨于穩(wěn)定,最小路徑為4.028 4 km,但并不是最優(yōu)路徑。

比較兩種算法能夠看出，改進(jìn)的人工勢(shì)場(chǎng)法相比蟻群算法更具有穩(wěn)定性,而且實(shí)時(shí)性更好,能夠快速準(zhǔn)確地規(guī)劃出最優(yōu)航跡,而蟻群算法耗時(shí)久、收斂性差難以在較短的時(shí)間內(nèi)規(guī)劃出一條最優(yōu)航跡。

5 結(jié) 論

四旋翼無(wú)人機(jī)航跡規(guī)劃在四旋翼無(wú)人機(jī)研究領(lǐng)域是一個(gè)重要的分支,本文針對(duì)四旋翼無(wú)人機(jī)航跡規(guī)劃問(wèn)題,提出了改進(jìn)的人工勢(shì)場(chǎng)法,解決了傳統(tǒng)人工勢(shì)場(chǎng)法出現(xiàn)的局部最小值問(wèn)題。并且在柵格化的任務(wù)區(qū)域環(huán)境中,將改進(jìn)的人工勢(shì)場(chǎng)法和蟻群算法進(jìn)行了對(duì)比,結(jié)果表明：改進(jìn)的人工勢(shì)場(chǎng)法能夠快速高效地規(guī)劃出最優(yōu)航跡,使四旋翼無(wú)人機(jī)能夠在最短的時(shí)間內(nèi)避開障礙物,安全的到達(dá)目標(biāo)點(diǎn)。