陳 卓 茅云生 宋利飛 董早鵬 楊 鑫 房新楠
(武漢理工大學高性能船舶技術教育部重點實驗室1) 武漢 430063) (武漢理工大學交通學院2) 武漢 430063)
水面無人艇(unmanned surface vehicle, USV)智能系統(tǒng)包括運動控制系統(tǒng)、傳感器系統(tǒng)和通信系統(tǒng),其中運動控制系統(tǒng)下的路徑規(guī)劃子系統(tǒng)則是無人艇實現(xiàn)自主航行的核心,在某種程度上代表了無人艇智能化水平的高低,同時也是無人艇安全航行和執(zhí)行任務的基本保障[1-2],因此,尋找一種合理高效的路徑規(guī)劃技術是無人艇研究的重要方向.
人工勢場法作為一種廣泛使用的路徑規(guī)劃算法,具有模型簡潔、計算快速、路徑光滑等優(yōu)點[3],是水面無人艇最常用的路徑規(guī)劃技術之一.但是勢場模型本身具有局限性,在實際應用時往往需要加以改進.操文芷等[4-6]針對傳統(tǒng)勢場法中無人艇易陷入局部最小點的問題,分別提出了基于不同逃脫策略的改進勢場法;劉建等[7-8]將勢場法與柵格模型進行了結合,使得路徑安全且較短,但是路徑不光滑;Li 等[9]提出了切向勢場法,克服了路徑的局部抖動問題.但是,以上這些方法都是基于傳統(tǒng)勢場模型,沒有考慮無人艇路徑的最優(yōu)化問題和合理性問題,導致路徑只在某方面較優(yōu),而且沒有考慮無人艇的運動能力約束,因而無法滿足無人艇的實際規(guī)劃要求.
微分進化算法(differential evolution algorithm,DE)是一種基于群體差異的啟發(fā)式全局搜索算法,由Storn等[10-12]為求解Chebyshev多項式而提出.相比于傳統(tǒng)的進化算法,DE算法具有模型簡單、受控參數(shù)少、魯棒性強的特點,近年來在優(yōu)化計算等方面得到了廣泛應用.本文將微分進化算法和人工勢場模型進行結合,把DE算法的優(yōu)化特性引入到傳統(tǒng)勢場模型中,提出了勢場路徑評價方程和基于進化策略的改進勢場模型,實現(xiàn)了對勢場路徑的初步優(yōu)化;并且在路徑規(guī)劃中考慮了以無人艇最大轉向角為代表的無人艇運動能力約束,提出了平滑算法對局部路徑進行二次優(yōu)化.通過不同環(huán)境下的仿真試驗對算法效果進行了驗證.
路徑規(guī)劃問題可以描述為這樣的數(shù)學模型:在給定空間內(nèi),尋找出一條連接起始點與目標點并且滿足一定優(yōu)化條件的無碰曲線.實現(xiàn)對環(huán)境中的障礙物的避碰以及對目標點的追蹤是路徑規(guī)劃中的兩個關鍵點,直接決定了路徑規(guī)劃的結果.
人工勢場法借鑒了物理學上的勢場概念,將障礙物和目標點對無人艇運動軌跡的影響以虛擬勢場的方式進行表征.在勢場法中,目標點產(chǎn)生引力場,障礙物附近產(chǎn)生斥力場,環(huán)境中各點所受到的勢場力與無人艇到各障礙物及目標點的距離有關,可由勢場方程計算得到.無人艇在規(guī)劃環(huán)境中沿著勢場合力的方向從起始點向目標點運動,見圖1.
圖1 勢場法路徑規(guī)劃
采用一個標準引力方程和帶有可調(diào)參數(shù)的斥力方程作為基本模型.其中無人艇在每一位置處的運動方向由目標點的引力和障礙物的斥力的矢量和所決定,引力和斥力的大小與無人艇的位置相關.
無人艇在環(huán)境中受到的引力為
(1)
式中:Fatt為目標點引力,方向由USV位置指向目標點;Dug為無人艇當前位置到目標點的歐式距離.
同時,無人艇在障礙物附近會受到斥力作用,斥力方程為
(2)
式中:Frel為障礙物斥力,方向由障礙物位置指向無人艇;Duo為無人艇當前位置到障礙物的最小距離;Dinf為斥力影響半徑,即無人艇在障礙物的影響范圍內(nèi)時,才會受到斥力,否則斥力為零;a和b為斥力場形狀系數(shù),改變這兩個系數(shù)的取值可以構造不同的勢場模型.在傳統(tǒng)勢場模型中,a和b是固定的,一般取1和2,這導致其無法適應于較為復雜的環(huán)境.本文采用DE算法對a和b進行了實值尋優(yōu),對于不同的規(guī)劃環(huán)境,可以得到不同的最優(yōu)勢場系數(shù),從而構造不同的勢場方程.
在確定勢場系數(shù)后,可利用式(1)~(2)計算出無人艇在每一點處受到的勢場合力及其方向:
F=Fatt+∑Frel
(3)
A=∠F
(4)
式中:F為無人艇在勢場中所受的引力和斥力之和;A為合力F的方向角,直接決定無人艇下一步長內(nèi)的航行方向.
路徑規(guī)劃的目的是在充分考慮路徑代價的基礎上生成最優(yōu)或滿意路徑,傳統(tǒng)勢場模型缺乏路徑評價機制,雖然能生成一條路徑,卻未必最優(yōu).因此,文中提出了勢場路徑評價方程,用于評估不同勢場模型下的路徑性能.在無人艇路徑規(guī)劃中,避碰系數(shù)、長度系數(shù)和光滑度系數(shù)是需要考慮的三個主要因素.避碰系數(shù)是路徑安全性的保障,長度系數(shù)和光滑度系數(shù)影響無人艇的航行效率.
避碰系數(shù)由在斥力影響范圍內(nèi)的所有路徑點到相應障礙物的距離之和表示,見圖2.為環(huán)境中第i個障礙物;Dinf為障礙物的影響半徑;Pj,…,Pj+m為落在該障礙物斥力圈內(nèi)的路徑點.
圖2 路徑避碰系數(shù)
路徑的整體避碰系數(shù)為
(5)
式中:M為障礙物總數(shù);m為該障礙物斥力影響范圍內(nèi)的路徑點總數(shù);d(Oi,Pj)為第i個障礙物Oi到第j個路徑點Pj的最小距離.fa越大,表示路徑整體到障礙物的距離越大,避碰程度越高,路徑越安全.
路徑光滑度由路徑上所有的相隔路徑點之間的距離和來表示,見圖3.其中Pi,Pi+1,Pi+2為三個連續(xù)的路徑點,d為PiPi+2間的直線距離,由幾何關系可知,d值越大,Pi,Pi+1,Pi+2三點所成的夾角越大,路徑在該局部就越光滑.
圖3 路徑光滑度系數(shù)
路徑的整體光滑度為
(6)
式中:N為路徑點總數(shù);d(Pi,Pi+2)為相隔路徑點(Pi,Pi+2)之間的直線距離.fs越大,路徑整體曲率變化越小,減少了不必要的轉向次數(shù),因而操縱系統(tǒng)越高效.
路徑長度由各相鄰路徑點之間的距離和來近似表示,路徑的長度系數(shù)為
fl=Nl
(7)
式中:N為路徑點總數(shù);l為無人艇運動步長.fl越小,路徑總長度最短,無人艇執(zhí)行任務過程中所消耗的源和時間就越少,效率越高.
結合以上路徑性能系數(shù),本文構造了如下的勢場路徑評價方程.
f=αfa+βfs-γfl
(8)
式中:α,β,γ對應系數(shù)的權重,滿足α+β+γ=1.在應用時,可根據(jù)不同的規(guī)劃要求為三個系數(shù)設置不同的權重.
DE算法采用了與遺傳算法相似的的進化流程,但是在變異操作時使用了差分策略,即利用種群中個體的差分向量對個體實現(xiàn)擾動,實現(xiàn)個體變異.這種變異方式有效地利用了種群分布特征,提高了算法的搜索能力,彌補了遺傳算法中變異方式的不足.
DE算法采用實數(shù)編碼方式,在實值參數(shù)尋優(yōu)上具有優(yōu)勢,因此本文采用DE算法來對勢場系數(shù)進行優(yōu)化.
步驟1種群初始化 選擇勢場形狀參數(shù)a和b組成實數(shù)串:xi=(xi,1,xi,2),作為種群中的“染色體”,xi,1,xi,2代表這個二維“染色體”上的基因.然后以隨機方式產(chǎn)生初始種群:
i=1,2,…,NP;j=1,2,…,D
(9)
步驟2變異操作 DE算法利用差分策略來實現(xiàn)染色體變異:從種群中隨機選取兩個不同的個體,求出其向量差,將向量差按一定比例縮放后與待變異個體進行相加,即得到變異個體:
vi(g+1)=xr1(g)+F·(xr2(g)-xr3(g)),
i≠r1≠r2≠r3,
(10)
式中:F為縮放因子;xi(g)為第g代種群中的第i個個體;vi(g+1)為第g代個體xr1(g)變異后產(chǎn)生的中間體.
步驟3交叉操作 對第g代種群{xi(g)}及其變異中間體{vi(g)}進行個體間的交叉操作:
(11)
式中:xi,j(g)為第g代種群中第i個個體上的第j個基因值;vi,j(g+1)為其對應的變異中間體上的等位基因值;CR為交叉概率,jrand為1,2,…,D的隨機整數(shù),j=jrand保證了變異中間體上至少有一個基因遺傳到了下一代.
在進化過程中,為保證解的有效性,需要判斷個體中的各“基因”是否滿足邊界條件.如果不滿足,則取為相應的邊界值.
步驟4選擇操作 DE算法采用貪婪算法來選擇下一代種群的個體.
(12)
式中:適應度函數(shù)f取為勢場路徑評價方程,即以每個個體所對應的勢場路徑的整體性能來評價該個體的適應度.適應度值越大,該個體上的信息被遺傳到下一代的可能性越大.
通過DE算法對勢場形狀系數(shù)進行優(yōu)化后,可以得到無人艇在環(huán)境下的最優(yōu)勢場系數(shù),再次利用勢場法原理即可獲得其對應的勢場路徑.
圖4 無人艇相鄰步長內(nèi)的最大轉向角
由圖4中幾何關系,可求得兩步長間的最大轉向角為
(13)
式中:路徑步長l在勢場法中設定;最小回轉半徑R可由無人艇的回轉實驗測得.
基于進化勢場模型所產(chǎn)生的路徑在避碰系數(shù)、光滑度和長度等性能上得到了改善,但是由于勢場法本身的局限性,無人艇的最大轉向角約束無法被兼顧,因而可能會出現(xiàn)局部路徑轉角過大、出現(xiàn)多余弧度的問題,無法保證所規(guī)劃路徑的合理性和可行性.為此提出了基于無人艇最大轉向角的簡單平滑算法來對勢場路徑進行二次優(yōu)化.算法的思路是:從初始點開始,到最后一個路徑點為止,依次從路徑中取出三個連續(xù)路徑點Pi、Pi+1和Pi+2,并判斷該三點所形成的角度φ是否滿足最大轉向角要求.如果夾角φ大于最大轉向角φ,則刪除中間路徑點Pi+1,并更新路徑,直到路徑上的所有點都滿足最大轉向角約束,見圖5.
圖5 基于無人艇最大轉向角的路徑平滑算法
基于進化勢場模型的無人艇路徑規(guī)劃算法分別從勢場模型層面和路徑點層面實現(xiàn)了對無人艇路徑的優(yōu)化.在勢場模型層面,在給定環(huán)境條件的前提下,以勢場路徑評價方程的形式評估不同勢場模型下的路徑,并使用DE算對勢場模型系數(shù)進行進化尋優(yōu),以得到勢場模型下的最優(yōu)路徑;考慮到勢場模型本身的局限性,對勢場路徑的局部路徑點進行二次平滑處理,以使其滿足無人艇的最大轉向角要求.算法的具體流程圖見圖6.
圖6 基于進化勢場模型的無人艇路徑規(guī)劃算法流程圖
圖7為基于傳統(tǒng)勢場模型和進化勢場模型所得到的路徑.兩圖中環(huán)境信息完全相同,環(huán)境參數(shù)設置如下:無人艇從航行起始點(0,0)駛向任務目標點(10,10),其中,障礙物半徑robs和影響半徑Dinf均取0.5,無人艇運動步長l設為0.2,兩步長內(nèi)最大轉向角φ=30°.表1為此相同環(huán)境下基于這兩種勢場模型的無人艇路徑的性能對比.
圖7 基于傳統(tǒng)勢場模型和進化勢場模型的無人艇路徑規(guī)劃
從仿真結果可以看出,基于進化勢場模型的無人艇路徑規(guī)劃算法所得到的路徑在整體性能上明顯優(yōu)于基于傳統(tǒng)勢場模型所得到的路徑.在路徑長度上,基于進化勢場模型的路徑長度為14.319 7,相比于傳統(tǒng)勢場模型,縮短了約9.75%;在操縱性方面,傳統(tǒng)勢場模型的路徑在經(jīng)過平滑算法處理后,仍需要以較大角度轉舵5次來實現(xiàn)對障礙物的避碰,而基于進化勢場模型的路徑只進行了兩次舵角微調(diào)就有效完成了避碰,而且路徑平滑前后的差別不大,這表明了在勢場模型層面對路徑進行優(yōu)化的有效性.
表1 基于傳統(tǒng)勢場模型與進化勢場模型的無人艇路徑性能對比
同時,為了驗證本文算法在實際環(huán)境下的適應性,對復雜海事環(huán)境下的無人艇路徑規(guī)劃也進行了仿真.仿真結果見圖8.圖中的障礙物分布是基于真實海事地圖的模擬,其中海島以多個圓形的組合體來近似表示.從仿真結果可以看到,在復雜海事環(huán)境下,采用本文算法后的無人艇能夠有效地避開各個島型障礙,到達目標點,而且路徑長度較短.圖9為該路徑下無人艇航向角改變量隨運動步長數(shù)變化的曲線圖,由圖9可知,無人艇的整體航向角改變很小,穩(wěn)定在-10°~15°;在第5步、35步及40步附近,出現(xiàn)了較大的航向改變,但是也遠小于無人艇的最大轉向角φ=30°,這表明路徑在操縱性和跟隨性方面也能很好地滿足無人艇的實際運動要求.
圖8 復雜海事環(huán)境下基于進化勢場模型的無人艇路徑規(guī)劃
圖9 無人艇單位步長內(nèi)的航向角改變量
無人艇的路徑規(guī)劃問題是無人艇領域最基本的問題之一,針對傳統(tǒng)勢場模型在路徑最優(yōu)性上的不足,文中提出了一種基于進化勢場模型的無人艇路徑規(guī)劃算法.通過在勢場模型中引入DE算法和路徑平滑算法,分別從模型層面和路徑點層面實現(xiàn)了對路徑的優(yōu)化,有效地提升了無人艇的航行效率.仿真結果表明,本文算法繼承了勢場法計算簡單、路徑安全可靠的優(yōu)點,同時兼顧了無人艇對路徑的優(yōu)化性要求和無人艇的運動約束條件,所規(guī)劃出的路徑短且平滑, 很好地滿足了無人艇的實際規(guī)劃要求.