陳 卓 茅云生 宋利飛 董早鵬 楊 鑫 房新楠

(武漢理工大學(xué)高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室1) 武漢 430063) (武漢理工大學(xué)交通學(xué)院2) 武漢 430063)

0 引 言

水面無人艇(unmanned surface vehicle, USV)智能系統(tǒng)包括運(yùn)動(dòng)控制系統(tǒng)、傳感器系統(tǒng)和通信系統(tǒng)，其中運(yùn)動(dòng)控制系統(tǒng)下的路徑規(guī)劃子系統(tǒng)則是無人艇實(shí)現(xiàn)自主航行的核心，在某種程度上代表了無人艇智能化水平的高低，同時(shí)也是無人艇安全航行和執(zhí)行任務(wù)的基本保障[1-2]，因此，尋找一種合理高效的路徑規(guī)劃技術(shù)是無人艇研究的重要方向.

人工勢(shì)場(chǎng)法作為一種廣泛使用的路徑規(guī)劃算法，具有模型簡(jiǎn)潔、計(jì)算快速、路徑光滑等優(yōu)點(diǎn)[3]，是水面無人艇最常用的路徑規(guī)劃技術(shù)之一.但是勢(shì)場(chǎng)模型本身具有局限性，在實(shí)際應(yīng)用時(shí)往往需要加以改進(jìn).操文芷等[4-6]針對(duì)傳統(tǒng)勢(shì)場(chǎng)法中無人艇易陷入局部最小點(diǎn)的問題，分別提出了基于不同逃脫策略的改進(jìn)勢(shì)場(chǎng)法；劉建等[7-8]將勢(shì)場(chǎng)法與柵格模型進(jìn)行了結(jié)合，使得路徑安全且較短，但是路徑不光滑；Li 等[9]提出了切向勢(shì)場(chǎng)法，克服了路徑的局部抖動(dòng)問題.但是，以上這些方法都是基于傳統(tǒng)勢(shì)場(chǎng)模型，沒有考慮無人艇路徑的最優(yōu)化問題和合理性問題，導(dǎo)致路徑只在某方面較優(yōu)，而且沒有考慮無人艇的運(yùn)動(dòng)能力約束，因而無法滿足無人艇的實(shí)際規(guī)劃要求.

微分進(jìn)化算法(differential evolution algorithm，DE)是一種基于群體差異的啟發(fā)式全局搜索算法，由Storn等[10-12]為求解Chebyshev多項(xiàng)式而提出.相比于傳統(tǒng)的進(jìn)化算法，DE算法具有模型簡(jiǎn)單、受控參數(shù)少、魯棒性強(qiáng)的特點(diǎn)，近年來在優(yōu)化計(jì)算等方面得到了廣泛應(yīng)用.本文將微分進(jìn)化算法和人工勢(shì)場(chǎng)模型進(jìn)行結(jié)合，把DE算法的優(yōu)化特性引入到傳統(tǒng)勢(shì)場(chǎng)模型中，提出了勢(shì)場(chǎng)路徑評(píng)價(jià)方程和基于進(jìn)化策略的改進(jìn)勢(shì)場(chǎng)模型，實(shí)現(xiàn)了對(duì)勢(shì)場(chǎng)路徑的初步優(yōu)化；并且在路徑規(guī)劃中考慮了以無人艇最大轉(zhuǎn)向角為代表的無人艇運(yùn)動(dòng)能力約束，提出了平滑算法對(duì)局部路徑進(jìn)行二次優(yōu)化.通過不同環(huán)境下的仿真試驗(yàn)對(duì)算法效果進(jìn)行了驗(yàn)證.

1 勢(shì)場(chǎng)模型

1.1 勢(shì)場(chǎng)法路徑規(guī)劃原理

路徑規(guī)劃問題可以描述為這樣的數(shù)學(xué)模型：在給定空間內(nèi)，尋找出一條連接起始點(diǎn)與目標(biāo)點(diǎn)并且滿足一定優(yōu)化條件的無碰曲線.實(shí)現(xiàn)對(duì)環(huán)境中的障礙物的避碰以及對(duì)目標(biāo)點(diǎn)的追蹤是路徑規(guī)劃中的兩個(gè)關(guān)鍵點(diǎn)，直接決定了路徑規(guī)劃的結(jié)果.

人工勢(shì)場(chǎng)法借鑒了物理學(xué)上的勢(shì)場(chǎng)概念，將障礙物和目標(biāo)點(diǎn)對(duì)無人艇運(yùn)動(dòng)軌跡的影響以虛擬勢(shì)場(chǎng)的方式進(jìn)行表征.在勢(shì)場(chǎng)法中，目標(biāo)點(diǎn)產(chǎn)生引力場(chǎng)，障礙物附近產(chǎn)生斥力場(chǎng)，環(huán)境中各點(diǎn)所受到的勢(shì)場(chǎng)力與無人艇到各障礙物及目標(biāo)點(diǎn)的距離有關(guān)，可由勢(shì)場(chǎng)方程計(jì)算得到.無人艇在規(guī)劃環(huán)境中沿著勢(shì)場(chǎng)合力的方向從起始點(diǎn)向目標(biāo)點(diǎn)運(yùn)動(dòng)，見圖1.

圖1 勢(shì)場(chǎng)法路徑規(guī)劃

1.2 勢(shì)場(chǎng)方程

采用一個(gè)標(biāo)準(zhǔn)引力方程和帶有可調(diào)參數(shù)的斥力方程作為基本模型.其中無人艇在每一位置處的運(yùn)動(dòng)方向由目標(biāo)點(diǎn)的引力和障礙物的斥力的矢量和所決定，引力和斥力的大小與無人艇的位置相關(guān).

無人艇在環(huán)境中受到的引力為

(1)

式中：Fatt為目標(biāo)點(diǎn)引力，方向由USV位置指向目標(biāo)點(diǎn)；Dug為無人艇當(dāng)前位置到目標(biāo)點(diǎn)的歐式距離.

同時(shí)，無人艇在障礙物附近會(huì)受到斥力作用，斥力方程為

(2)

式中：Frel為障礙物斥力，方向由障礙物位置指向無人艇；Duo為無人艇當(dāng)前位置到障礙物的最小距離；Dinf為斥力影響半徑，即無人艇在障礙物的影響范圍內(nèi)時(shí)，才會(huì)受到斥力，否則斥力為零；a和b為斥力場(chǎng)形狀系數(shù)，改變這兩個(gè)系數(shù)的取值可以構(gòu)造不同的勢(shì)場(chǎng)模型.在傳統(tǒng)勢(shì)場(chǎng)模型中，a和b是固定的，一般取1和2，這導(dǎo)致其無法適應(yīng)于較為復(fù)雜的環(huán)境.本文采用DE算法對(duì)a和b進(jìn)行了實(shí)值尋優(yōu)，對(duì)于不同的規(guī)劃環(huán)境，可以得到不同的最優(yōu)勢(shì)場(chǎng)系數(shù)，從而構(gòu)造不同的勢(shì)場(chǎng)方程.

在確定勢(shì)場(chǎng)系數(shù)后，可利用式(1)～(2)計(jì)算出無人艇在每一點(diǎn)處受到的勢(shì)場(chǎng)合力及其方向：

F=Fatt+∑Frel

(3)

A=∠F

(4)

式中：F為無人艇在勢(shì)場(chǎng)中所受的引力和斥力之和；A為合力F的方向角，直接決定無人艇下一步長(zhǎng)內(nèi)的航行方向.

2 基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃算法

2.1 勢(shì)場(chǎng)路徑評(píng)價(jià)方程

路徑規(guī)劃的目的是在充分考慮路徑代價(jià)的基礎(chǔ)上生成最優(yōu)或滿意路徑，傳統(tǒng)勢(shì)場(chǎng)模型缺乏路徑評(píng)價(jià)機(jī)制，雖然能生成一條路徑，卻未必最優(yōu).因此，文中提出了勢(shì)場(chǎng)路徑評(píng)價(jià)方程，用于評(píng)估不同勢(shì)場(chǎng)模型下的路徑性能.在無人艇路徑規(guī)劃中，避碰系數(shù)、長(zhǎng)度系數(shù)和光滑度系數(shù)是需要考慮的三個(gè)主要因素.避碰系數(shù)是路徑安全性的保障，長(zhǎng)度系數(shù)和光滑度系數(shù)影響無人艇的航行效率.

避碰系數(shù)由在斥力影響范圍內(nèi)的所有路徑點(diǎn)到相應(yīng)障礙物的距離之和表示，見圖2.為環(huán)境中第i個(gè)障礙物；Dinf為障礙物的影響半徑；Pj,…,Pj+m為落在該障礙物斥力圈內(nèi)的路徑點(diǎn).

圖2 路徑避碰系數(shù)

路徑的整體避碰系數(shù)為

(5)

式中：M為障礙物總數(shù)；m為該障礙物斥力影響范圍內(nèi)的路徑點(diǎn)總數(shù)；d(Oi,Pj)為第i個(gè)障礙物Oi到第j個(gè)路徑點(diǎn)Pj的最小距離.fa越大，表示路徑整體到障礙物的距離越大，避碰程度越高，路徑越安全.

路徑光滑度由路徑上所有的相隔路徑點(diǎn)之間的距離和來表示，見圖3.其中Pi，Pi+1，Pi+2為三個(gè)連續(xù)的路徑點(diǎn)，d為PiPi+2間的直線距離，由幾何關(guān)系可知，d值越大，Pi，Pi+1，Pi+2三點(diǎn)所成的夾角越大，路徑在該局部就越光滑.

圖3 路徑光滑度系數(shù)

路徑的整體光滑度為

(6)

式中：N為路徑點(diǎn)總數(shù)；d(Pi,Pi+2)為相隔路徑點(diǎn)(Pi,Pi+2)之間的直線距離.fs越大，路徑整體曲率變化越小，減少了不必要的轉(zhuǎn)向次數(shù)，因而操縱系統(tǒng)越高效.

路徑長(zhǎng)度由各相鄰路徑點(diǎn)之間的距離和來近似表示，路徑的長(zhǎng)度系數(shù)為

fl=Nl

(7)

式中：N為路徑點(diǎn)總數(shù)；l為無人艇運(yùn)動(dòng)步長(zhǎng).fl越小，路徑總長(zhǎng)度最短，無人艇執(zhí)行任務(wù)過程中所消耗的源和時(shí)間就越少，效率越高.

結(jié)合以上路徑性能系數(shù)，本文構(gòu)造了如下的勢(shì)場(chǎng)路徑評(píng)價(jià)方程.

f=αfa+βfs-γfl

(8)

式中：α，β，γ對(duì)應(yīng)系數(shù)的權(quán)重，滿足α+β+γ=1.在應(yīng)用時(shí)，可根據(jù)不同的規(guī)劃要求為三個(gè)系數(shù)設(shè)置不同的權(quán)重.

2.2 基于DE算法的進(jìn)化勢(shì)場(chǎng)模型

DE算法采用了與遺傳算法相似的的進(jìn)化流程，但是在變異操作時(shí)使用了差分策略，即利用種群中個(gè)體的差分向量對(duì)個(gè)體實(shí)現(xiàn)擾動(dòng)，實(shí)現(xiàn)個(gè)體變異.這種變異方式有效地利用了種群分布特征，提高了算法的搜索能力，彌補(bǔ)了遺傳算法中變異方式的不足.

DE算法采用實(shí)數(shù)編碼方式，在實(shí)值參數(shù)尋優(yōu)上具有優(yōu)勢(shì)，因此本文采用DE算法來對(duì)勢(shì)場(chǎng)系數(shù)進(jìn)行優(yōu)化.

步驟1種群初始化 選擇勢(shì)場(chǎng)形狀參數(shù)a和b組成實(shí)數(shù)串：xi=(xi,1,xi,2)，作為種群中的“染色體”，xi,1,xi,2代表這個(gè)二維“染色體”上的基因.然后以隨機(jī)方式產(chǎn)生初始種群：

i=1,2,…,NP;j=1,2，…,D

(9)

步驟2變異操作 DE算法利用差分策略來實(shí)現(xiàn)染色體變異：從種群中隨機(jī)選取兩個(gè)不同的個(gè)體，求出其向量差，將向量差按一定比例縮放后與待變異個(gè)體進(jìn)行相加，即得到變異個(gè)體：

vi(g+1)=xr1(g)+F·(xr2(g)-xr3(g)),

i≠r1≠r2≠r3,

(10)

式中：F為縮放因子；xi(g)為第g代種群中的第i個(gè)個(gè)體；vi(g+1)為第g代個(gè)體xr1(g)變異后產(chǎn)生的中間體.

步驟3交叉操作 對(duì)第g代種群{xi(g)}及其變異中間體{vi(g)}進(jìn)行個(gè)體間的交叉操作：

(11)

式中：xi,j(g)為第g代種群中第i個(gè)個(gè)體上的第j個(gè)基因值；vi,j(g+1)為其對(duì)應(yīng)的變異中間體上的等位基因值；CR為交叉概率，jrand為1，2，…,D的隨機(jī)整數(shù)，j=jrand保證了變異中間體上至少有一個(gè)基因遺傳到了下一代.

在進(jìn)化過程中，為保證解的有效性，需要判斷個(gè)體中的各“基因”是否滿足邊界條件.如果不滿足，則取為相應(yīng)的邊界值.

步驟4選擇操作 DE算法采用貪婪算法來選擇下一代種群的個(gè)體.

(12)

式中：適應(yīng)度函數(shù)f取為勢(shì)場(chǎng)路徑評(píng)價(jià)方程，即以每個(gè)個(gè)體所對(duì)應(yīng)的勢(shì)場(chǎng)路徑的整體性能來評(píng)價(jià)該個(gè)體的適應(yīng)度.適應(yīng)度值越大，該個(gè)體上的信息被遺傳到下一代的可能性越大.

通過DE算法對(duì)勢(shì)場(chǎng)形狀系數(shù)進(jìn)行優(yōu)化后，可以得到無人艇在環(huán)境下的最優(yōu)勢(shì)場(chǎng)系數(shù)，再次利用勢(shì)場(chǎng)法原理即可獲得其對(duì)應(yīng)的勢(shì)場(chǎng)路徑.

2.3 基于最大轉(zhuǎn)向角的路徑平滑處理

圖4 無人艇相鄰步長(zhǎng)內(nèi)的最大轉(zhuǎn)向角

由圖4中幾何關(guān)系，可求得兩步長(zhǎng)間的最大轉(zhuǎn)向角為

(13)

式中：路徑步長(zhǎng)l在勢(shì)場(chǎng)法中設(shè)定；最小回轉(zhuǎn)半徑R可由無人艇的回轉(zhuǎn)實(shí)驗(yàn)測(cè)得.

基于進(jìn)化勢(shì)場(chǎng)模型所產(chǎn)生的路徑在避碰系數(shù)、光滑度和長(zhǎng)度等性能上得到了改善，但是由于勢(shì)場(chǎng)法本身的局限性，無人艇的最大轉(zhuǎn)向角約束無法被兼顧，因而可能會(huì)出現(xiàn)局部路徑轉(zhuǎn)角過大、出現(xiàn)多余弧度的問題，無法保證所規(guī)劃路徑的合理性和可行性.為此提出了基于無人艇最大轉(zhuǎn)向角的簡(jiǎn)單平滑算法來對(duì)勢(shì)場(chǎng)路徑進(jìn)行二次優(yōu)化.算法的思路是：從初始點(diǎn)開始，到最后一個(gè)路徑點(diǎn)為止，依次從路徑中取出三個(gè)連續(xù)路徑點(diǎn)Pi、Pi+1和Pi+2，并判斷該三點(diǎn)所形成的角度φ是否滿足最大轉(zhuǎn)向角要求.如果夾角φ大于最大轉(zhuǎn)向角φ，則刪除中間路徑點(diǎn)Pi+1，并更新路徑，直到路徑上的所有點(diǎn)都滿足最大轉(zhuǎn)向角約束，見圖5.

圖5 基于無人艇最大轉(zhuǎn)向角的路徑平滑算法

2.4 算法流程圖

基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃算法分別從勢(shì)場(chǎng)模型層面和路徑點(diǎn)層面實(shí)現(xiàn)了對(duì)無人艇路徑的優(yōu)化.在勢(shì)場(chǎng)模型層面，在給定環(huán)境條件的前提下，以勢(shì)場(chǎng)路徑評(píng)價(jià)方程的形式評(píng)估不同勢(shì)場(chǎng)模型下的路徑，并使用DE算對(duì)勢(shì)場(chǎng)模型系數(shù)進(jìn)行進(jìn)化尋優(yōu)，以得到勢(shì)場(chǎng)模型下的最優(yōu)路徑；考慮到勢(shì)場(chǎng)模型本身的局限性，對(duì)勢(shì)場(chǎng)路徑的局部路徑點(diǎn)進(jìn)行二次平滑處理，以使其滿足無人艇的最大轉(zhuǎn)向角要求.算法的具體流程圖見圖6.

圖6 基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃算法流程圖

3 仿真實(shí)驗(yàn)及結(jié)果分析

圖7為基于傳統(tǒng)勢(shì)場(chǎng)模型和進(jìn)化勢(shì)場(chǎng)模型所得到的路徑.兩圖中環(huán)境信息完全相同，環(huán)境參數(shù)設(shè)置如下：無人艇從航行起始點(diǎn)(0，0)駛向任務(wù)目標(biāo)點(diǎn)(10，10)，其中，障礙物半徑robs和影響半徑Dinf均取0.5，無人艇運(yùn)動(dòng)步長(zhǎng)l設(shè)為0.2，兩步長(zhǎng)內(nèi)最大轉(zhuǎn)向角φ=30°.表1為此相同環(huán)境下基于這兩種勢(shì)場(chǎng)模型的無人艇路徑的性能對(duì)比.

圖7 基于傳統(tǒng)勢(shì)場(chǎng)模型和進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃

從仿真結(jié)果可以看出，基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃算法所得到的路徑在整體性能上明顯優(yōu)于基于傳統(tǒng)勢(shì)場(chǎng)模型所得到的路徑.在路徑長(zhǎng)度上，基于進(jìn)化勢(shì)場(chǎng)模型的路徑長(zhǎng)度為14.319 7，相比于傳統(tǒng)勢(shì)場(chǎng)模型，縮短了約9.75%；在操縱性方面，傳統(tǒng)勢(shì)場(chǎng)模型的路徑在經(jīng)過平滑算法處理后，仍需要以較大角度轉(zhuǎn)舵5次來實(shí)現(xiàn)對(duì)障礙物的避碰，而基于進(jìn)化勢(shì)場(chǎng)模型的路徑只進(jìn)行了兩次舵角微調(diào)就有效完成了避碰，而且路徑平滑前后的差別不大，這表明了在勢(shì)場(chǎng)模型層面對(duì)路徑進(jìn)行優(yōu)化的有效性.

表1 基于傳統(tǒng)勢(shì)場(chǎng)模型與進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑性能對(duì)比

同時(shí)，為了驗(yàn)證本文算法在實(shí)際環(huán)境下的適應(yīng)性，對(duì)復(fù)雜海事環(huán)境下的無人艇路徑規(guī)劃也進(jìn)行了仿真.仿真結(jié)果見圖8.圖中的障礙物分布是基于真實(shí)海事地圖的模擬，其中海島以多個(gè)圓形的組合體來近似表示.從仿真結(jié)果可以看到，在復(fù)雜海事環(huán)境下，采用本文算法后的無人艇能夠有效地避開各個(gè)島型障礙，到達(dá)目標(biāo)點(diǎn)，而且路徑長(zhǎng)度較短.圖9為該路徑下無人艇航向角改變量隨運(yùn)動(dòng)步長(zhǎng)數(shù)變化的曲線圖，由圖9可知，無人艇的整體航向角改變很小，穩(wěn)定在-10°～15°；在第5步、35步及40步附近，出現(xiàn)了較大的航向改變，但是也遠(yuǎn)小于無人艇的最大轉(zhuǎn)向角φ=30°，這表明路徑在操縱性和跟隨性方面也能很好地滿足無人艇的實(shí)際運(yùn)動(dòng)要求.

圖8 復(fù)雜海事環(huán)境下基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃

圖9 無人艇單位步長(zhǎng)內(nèi)的航向角改變量

4 結(jié) 束 語

無人艇的路徑規(guī)劃問題是無人艇領(lǐng)域最基本的問題之一，針對(duì)傳統(tǒng)勢(shì)場(chǎng)模型在路徑最優(yōu)性上的不足，文中提出了一種基于進(jìn)化勢(shì)場(chǎng)模型的無人艇路徑規(guī)劃算法.通過在勢(shì)場(chǎng)模型中引入DE算法和路徑平滑算法，分別從模型層面和路徑點(diǎn)層面實(shí)現(xiàn)了對(duì)路徑的優(yōu)化，有效地提升了無人艇的航行效率.仿真結(jié)果表明，本文算法繼承了勢(shì)場(chǎng)法計(jì)算簡(jiǎn)單、路徑安全可靠的優(yōu)點(diǎn)，同時(shí)兼顧了無人艇對(duì)路徑的優(yōu)化性要求和無人艇的運(yùn)動(dòng)約束條件，所規(guī)劃出的路徑短且平滑， 很好地滿足了無人艇的實(shí)際規(guī)劃要求.