基于粒子群算法和航行規(guī)則的分步多船避碰路徑規(guī)劃

杲飛 顏德文

摘 ?要： 針對寬水域多船避碰過程中路徑規(guī)劃難與航行規(guī)則結(jié)合的問題，提出一種分步多船避碰路徑規(guī)劃算法。首先將障礙船視為靜態(tài)障礙物，利用極坐標(biāo)空間的粒子群路徑規(guī)劃算法結(jié)合船舶安全領(lǐng)域進(jìn)行靜態(tài)路徑規(guī)劃，得到船舶避碰轉(zhuǎn)向點;然后利用航行規(guī)則對轉(zhuǎn)向點的轉(zhuǎn)向角度進(jìn)行動態(tài)修正。通過對算法進(jìn)行仿真驗證，能夠很好地解決多船避碰路徑規(guī)劃的問題，為解決多船避碰問題提供一種新的思路。

關(guān)鍵詞： 多船避碰; 粒子群; 安全領(lǐng)域; 路徑規(guī)劃; 航行規(guī)則; 仿真驗證

中圖分類號： TN850.6?34; U664 ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼： A ? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2020）02?0106?04

Stepwise multi?ship collision avoidance path planning based on particle swarm optimization and navigation rules

GAO Fei， YAN Dewen

Abstract： A stepwise multi?ship collision avoidance path planning algorithm is proposed to solve the problem that the route planning is difficult to combine with the navigation rules in the process of multi?ship collision avoidance in wide water area. Firstly， the obstacle ship is regarded as the static obstacle， and the static path planning is performed in combination with the field of ship safety and by means of the particle swarm route planning algorithm in polar coordinate space， so as to obtain the collision avoidance steering point. The turning angle of the steering point is modified dynamically by the navigation rules. The simulation results show that this algorithm can conduct the path planning of multi?ship collision avoidance， and provide a new idea for the solution of multi?ship collision avoidance.

Keywords： multi?ship collision avoidance; particle swarm optimization; security area; path planning; navigation rules; simulation verification

0 ?引 ?言

船舶避碰路徑規(guī)劃問題一直是船舶操縱和航運安全領(lǐng)域的亟需解決的課題之一[1]。其主要目的是在船舶會遇過程中找到一條從出發(fā)點到目標(biāo)點最安全、最高效的路徑。其難點在于規(guī)劃的路徑要符合船舶航行的規(guī)則約束。然而多船避碰問題更是其中的難點，更好地解決多船避碰路徑規(guī)劃問題才能夠真正實現(xiàn)船舶自動避碰。

考慮到智能優(yōu)化算法對于處理抽象和定性的影響因素具有較好的效果，更加適用于船舶避碰路徑規(guī)劃[2]。且智能優(yōu)化算法與規(guī)則結(jié)合應(yīng)用于機(jī)器人[3]和無人艇[4]的路徑規(guī)劃已經(jīng)取得了不錯的效果。粒子群算法作為一種智能優(yōu)化算法機(jī)理簡單，收斂速度快，在路徑規(guī)劃方面有其特有的優(yōu)勢。將智能優(yōu)化算法和航行規(guī)則結(jié)合的應(yīng)用在避碰領(lǐng)域的研究也有很多，主要是對避碰角度和單船避碰過程的尋優(yōu)[5?7]。所以本文提出了采用粒子群算法與航行規(guī)則結(jié)合的分步多船避碰路徑規(guī)劃算法。首先將多條障礙船視為靜態(tài)障礙物，利用規(guī)則對障礙船建模，并利用極坐標(biāo)空間的粒子群路徑規(guī)劃算法進(jìn)行靜態(tài)路徑規(guī)劃，得到船舶避碰的全局路徑，同時得到避碰轉(zhuǎn)向點和轉(zhuǎn)向角度。然后設(shè)計了利用航行規(guī)則推理的轉(zhuǎn)向點和轉(zhuǎn)向角度動態(tài)修正算法，在船舶航行過程中對初始路徑進(jìn)行動態(tài)修正。此過程中結(jié)合船員實際避碰的操縱習(xí)慣，解決了多船避碰路徑規(guī)劃過程中難與規(guī)則相結(jié)合的問題，使規(guī)劃的路徑符合航行規(guī)則的要求，提高了航行的效率和安全性。

1 ?問題描述與建模

對于船舶來講，路徑規(guī)劃就是尋找其在海洋中航行的轉(zhuǎn)向點的集合，并且由于船舶航行中主要的避碰操作多為轉(zhuǎn)向操作，因此采用文獻(xiàn)[8]基于極坐標(biāo)空間的粒子群路徑規(guī)劃建模方法：其中S為出發(fā)點，G為目標(biāo)點，以S為原點，以S與G的連線作為極坐標(biāo)軸建立極坐標(biāo)系。將SG進(jìn)行m+1等分，得到間隔長度為[SG（m+1）]的m+1段線段，以各個等分點到S的連線為半徑作圓，可以得到一系列的同心圓弧[li]。圓弧[li]與路徑的交點即為路徑轉(zhuǎn)向點。路徑規(guī)劃即為尋找轉(zhuǎn)向點的集合[pi={p1，p2，…，pm}]。其中轉(zhuǎn)向點和相鄰轉(zhuǎn)向點之間不能存在障礙船的船舶領(lǐng)域。船舶領(lǐng)域（燈號的能見距離）所適用的最大距離為6 n mile，當(dāng)兩船距離大于6 n mile時，船舶采取任何行動都不違反規(guī)則約束，船舶避碰條款此時不適用，無需進(jìn)行避碰操作。所以將目標(biāo)船膨脹成半徑為6 n mile的圓形領(lǐng)域，于6 n mile以外對船舶的航行路徑進(jìn)行靜態(tài)規(guī)劃。極坐標(biāo)下路徑規(guī)劃示意圖如圖1所示。

第i個轉(zhuǎn)向點[pi]的坐標(biāo)就可以用[Rpi，θpi]表示，同時通過圖1發(fā)現(xiàn)極徑[Rpi]和極角[θpi]存在的關(guān)系：

[θpi=i=0mΔθpiRpi=i·SGm+1] ? ? ? ? ? ? ? ? （1）

式中，[Δθpi]為第i個轉(zhuǎn)向點的轉(zhuǎn)向角度。通過式（1）可以看出路徑上轉(zhuǎn)向點可以通過本身序號和轉(zhuǎn)向角度唯一確定?？紤]到船員在船舶航行過程中的主要避碰行動為轉(zhuǎn)向避碰，因此一條候選路徑可以用此路徑上轉(zhuǎn)向點的轉(zhuǎn)向角度來表示：

[pi=Δθp1，Δθp2，…，Δθpm]

這樣路徑規(guī)劃就轉(zhuǎn)換成轉(zhuǎn)向角度的規(guī)劃，這對于避碰路徑的動態(tài)修正是有利的。

2 ?基于粒子群的路徑規(guī)劃

2.1 ?粒子群優(yōu)化算法的基本原理

粒子群優(yōu)化算法是一種群體智能算法。模擬鳥類的捕食行為，采用速度、位置搜索模型，每一個粒子都是一個備選解，解的優(yōu)劣程度由適應(yīng)度決定。粒子群算法隨機(jī)初始化n個粒子，每個粒子m維。第i（i=1，2，…，n）個粒子在m維解空間的位置為[xi=（x（i，1），x（i，2），…，x（i，m））]，每一次迭代粒子根據(jù)自身的飛行經(jīng)驗得到個體最優(yōu)值[Pbesti]和群體最優(yōu)值[Gbest]來確定自己的速度，調(diào)整自己的位置，逐漸向最優(yōu)粒子靠攏。第i個粒子的速度為：

[vi=v（i，1），v（i，2），…，v（i，m）]

粒子的速度和位置更新公式如下[9]：

[vt+1i，j=ω·vti，j+c1r1pti，j-xti，jΔt+c2r2ptg，j-xti，jΔtxt+1i，j=xti，j+vt+1i，jΔt] （2）

式中： [vt+1i，j]，[xt+1i，j]分別表示粒子i第j維（j=1，2，…，m）分量在t時刻的速度和位置;[pti，j]是粒子i第j維分量在t時刻搜索到的最優(yōu)位置;[ptg，j]表示所有粒子第j維分量到t時刻搜索到的最優(yōu)位置;[Δt]為單位時間;[c1，c2]為加速常數(shù)，代表粒子向自身最優(yōu)位置和全局最優(yōu)位置的推進(jìn)加速權(quán)值;[r1]，[r2]為隨機(jī)數(shù)一般取值在（0，1）之間;[ω]為慣性權(quán)重，[ω]較大則全局搜索能力強，[ω]較小一般局部搜索能力強，且[ω]隨迭代次數(shù)的線性減小，[ω]取值一般在0.4～0.9之間。

2.2 ?算法實現(xiàn)

在第i條圓弧[li]上選取一個轉(zhuǎn)向點[pi] （[pi]必須是自由點），為了保證航行的安全性，其中[pi]的選取必須滿足幾個條件：

1） 在[li]的取值范圍內(nèi);

2） 不在任何障礙船的領(lǐng)域內(nèi);

3） [pi]與其相鄰的圓弧[li-1]上所選點的連線不能與任何障礙船的領(lǐng)域相交。

為滿足這三個條件將[pi]在[li]上的取值范圍設(shè)置為圓弧與海圖的交點到圓弧與障礙船領(lǐng)域交點之間，粒子初始化時應(yīng)事先設(shè)定取值范圍;其次相鄰兩點之間的連線到圓心的距離應(yīng)該大于障礙船的領(lǐng)域半徑。從起始點S出發(fā)，依次連接圓弧上的各點到達(dá)目標(biāo)點G，即得到一條規(guī)劃的路徑，接下來對路徑進(jìn)行尋優(yōu)，即得到全局最優(yōu)路徑。

考慮到航行的經(jīng)濟(jì)性，路徑規(guī)劃的結(jié)果是期望得到航行的距離盡可能短，所以適應(yīng)度函數(shù)選取為航行的路徑和：

[Fi（x）=lp=lsp1+i=1m-1lpipi+1+lpmG=i=0mlpipi+1] ? （3）

具體步驟：

Step1：初始化粒子數(shù)n、粒子的緯度m。初始化粒子[pi]，每個粒子的歷史最優(yōu)值即為其本身[Pbesti]。計算每個粒子的適應(yīng)度值，選取適應(yīng)度值最小的粒子為全局最優(yōu)值[Gbest]。

Step2：由式（2）更新粒子的速度和位置。

Step3：對每個粒子[pi]，計算適應(yīng)度值，若適應(yīng)度值小值[Pbesti]的適應(yīng)度值，則[Pbesti]=[pi]。

Step4：轉(zhuǎn)到Step2進(jìn)行迭代，直到滿足精度或迭代次數(shù)要求為止。

3 ?航行規(guī)則推理的避碰路徑動態(tài)修正算法

3.1 ?基本思想

根據(jù)研究船舶避碰采取最多的操縱行為是轉(zhuǎn)向避碰，本文正是采取轉(zhuǎn)向避碰的方式。首先本船把障礙船視為靜態(tài)障礙并進(jìn)行靜態(tài)環(huán)境下的路徑規(guī)劃，得到最優(yōu)轉(zhuǎn)向點集合[pi]，當(dāng)規(guī)劃完初始路徑以后，船舶沿著初始路徑規(guī)劃轉(zhuǎn)向點航行，每走一點會判斷下一轉(zhuǎn)向點[pi+1]與最近障礙船的距離[DT]，判斷本船在[pi+1]是否有碰撞危險，若[DT]大于船舶安全會遇距離，則無碰撞危險，無需對轉(zhuǎn)向點進(jìn)行修正，繼續(xù)沿著初始規(guī)劃轉(zhuǎn)向點航行即可。當(dāng)[DT]小于船舶安全會遇距離時，根據(jù)避碰規(guī)則確定會遇狀態(tài)、避碰方式、轉(zhuǎn)向角度[Δφ]，以調(diào)整去往下一轉(zhuǎn)向點[pi+1]的轉(zhuǎn)向角度[Δθ]，得到新的下一轉(zhuǎn)向點[pnewi+1]，并更新最新的轉(zhuǎn)向點到動態(tài)修正路徑集合[pnewi]中。[pnewi]即船舶的動態(tài)避碰路徑。

3.2 ?規(guī)則設(shè)計

動態(tài)避碰規(guī)則的設(shè)計考慮到船舶避碰場景的突發(fā)性，采用正向推理的辦法，將規(guī)則庫設(shè)計為五輸入兩輸出的模型，減少了思考過程，增強了時效性。其中，輸入包括[θT]表示下一轉(zhuǎn)向點障礙船相對本船的舷角;[∠A]為本船與障礙船的速度夾角; [DT]為下一轉(zhuǎn)向點本船與最近障礙船的距離;[v0]，[vt]為本船與障礙船的速度，輸出[O]為本船在全局路徑上根據(jù)動態(tài)障礙物調(diào)整的類型;[Δφ]為船舶在下一航跡點航向的調(diào)整量?？捎墒剑?）更新轉(zhuǎn)向點的極角：

[θpnewi+1=θpi+Δθpi+Δφ] ? （4）

首先根據(jù)[θT]結(jié)合避碰規(guī)則對避碰區(qū)域進(jìn)行劃分。根據(jù)規(guī)則互見中的兩船會遇，將行動局面劃分為A，B，C，D，E，F(xiàn)六種區(qū)域，如圖2所示。A區(qū)來船，本船應(yīng)采取向右轉(zhuǎn)向的避碰行動，B區(qū)來船，本船為讓路船，且相對方位大，采去向左轉(zhuǎn)向，在C，D，E區(qū)本船為直航船，本船不必采取行動;F區(qū)來船本船應(yīng)采取向右轉(zhuǎn)向。

讓路船最佳轉(zhuǎn)向角度可由式（5）得到[10]：

[Δφ=maxθT，arcsin 0.25-∠S′+θT] ?（5）

式中，

[∠S′=arcsinvTsin（∠A+Δφ）v20+v2T-2v0vT（∠A+Δφ）]

安全通過距離根據(jù)不同的會遇局面有不同的劃分，通過確定不同的會遇局面，確定相應(yīng)的安全通過距離。安全距離通過文獻(xiàn)[11]得到在開闊水域中船舶的安全通過距離，其與各分區(qū)對應(yīng)表如表1所示。

通過對會遇局面、轉(zhuǎn)向避碰角度、安全距離的研究得到了如表2所示的船舶動態(tài)避碰規(guī)則庫。

以上規(guī)則根據(jù)船舶航行規(guī)則結(jié)合船舶避碰決策分析整理，將輸入與輸出一一對應(yīng)，用于對初始路徑轉(zhuǎn)向點進(jìn)行動態(tài)的修正，能夠解決船舶相遇過程中的各種局面，得到新的動態(tài)避碰路徑。

3.3 ?具體步驟

Step1：用粒子群算法求出初始靜態(tài)狀態(tài)下的最優(yōu)路徑轉(zhuǎn)向點集合[pi]，初始化動態(tài)避碰路徑轉(zhuǎn)向點集合[pnewi（i=1，2，…，m）]，m為轉(zhuǎn)向點數(shù)量和障礙船[Tj（j=1，2，…，n）]，[n]為障礙船數(shù)量。令i=1，用i來遍歷最優(yōu)的路徑轉(zhuǎn)向點[pi]，并指向下一轉(zhuǎn)向點;令j=1，用j來遍歷障礙船的數(shù)量。

Step2：若i>m，轉(zhuǎn)Step6。獲取初始靜態(tài)路徑的下一轉(zhuǎn)向點[pi+1]，設(shè)在動態(tài)環(huán)境下的船舶航行的下一轉(zhuǎn)向點為[pnewi+1]，初始化[pnewi+1=pi+1]。

Step3：若j>n，轉(zhuǎn)Step5。獲取障礙船的航行信息。若在轉(zhuǎn)向點[pi+1]距離[DTj]（表示距第j條障礙船的距離）大于安全距離，則令j=j+1，轉(zhuǎn)Step3。

Step4：船舶當(dāng)前航跡點為[pi]，下一航跡點[pi+1]與[Tj]障礙船有碰撞危險，確定規(guī)則庫各輸入的值，得到正確的避碰決策輸出[O]，得到新的[pnewi+1]。

Step5：令[i=i+1]，將[pnewi+1]加入到動態(tài)轉(zhuǎn)向點集合[pnewi]中，轉(zhuǎn)到Step2。

Step6：算法結(jié)束，將船舶航行的路徑點保存的[pnewi]中。

4 ?仿真實驗

為了驗證算法的效果，在計算機(jī)上進(jìn)行了仿真實驗。本船當(dāng)前位置點為S點，目標(biāo)航跡點為正東方向的G點，兩點相聚36 n mile，令正東方向航向為0°，順時針為正方向，各障礙船相對本船的位置坐標(biāo)和航向如表3所示。本船和障礙船的航速均為12 n，各障礙船均按航行規(guī)則行駛，粒子群算法參數(shù)設(shè)置為[c1=c2=1.5]，[ω]=0.9，種群數(shù)n=15，最大迭代次數(shù)設(shè)置為50。

對初始狀態(tài)的路徑進(jìn)行規(guī)劃，得到初始路徑轉(zhuǎn)向點如表4所示。船舶沿轉(zhuǎn)向點航行，進(jìn)行動態(tài)避碰，當(dāng)兩船間航行距離小于安全避碰距離時，即根據(jù)避碰規(guī)則庫進(jìn)行轉(zhuǎn)向，從而改變下一轉(zhuǎn)向點得到新的轉(zhuǎn)向點坐標(biāo)如表5所示。

為了便于路徑在海圖中顯示，利用式（6）將極坐標(biāo)下的轉(zhuǎn)向點轉(zhuǎn)換成直角坐標(biāo)。

[xy=cos α-sin α ?sin αcos α·Rpcos θpRpsin θp] ? ? ? （6）

式中，[α]為極軸與直角坐標(biāo)x軸的夾角，這里為0°。

在Matlab上的仿真結(jié)果如圖3所示。其中，虛線為靜態(tài)狀態(tài)下規(guī)劃的路徑;實線為動態(tài)調(diào)整的路徑;圓圈為初始狀態(tài)下的船舶領(lǐng)域;箭頭方向為障礙船運動方向。

粒子群算法迭代圖如圖4所示，可見在初始狀態(tài)下粒子群算法能夠較快地找到一條最優(yōu)路徑，最短路徑為38.4 n mile，在迭代35次左右取得。

5 ?結(jié) ?語

利用粒子群算法結(jié)合船舶領(lǐng)域與船舶航行規(guī)則，進(jìn)行分步多船避碰路徑規(guī)劃為多船避碰輔助決策提供了新的思路。此思路解決了避碰算法難與航行規(guī)則結(jié)合的問題。經(jīng)過仿真發(fā)現(xiàn)，該算法具有很好的避碰效果，能夠真正地實現(xiàn)多船動態(tài)避碰。

參考文獻(xiàn)

[1] TAM C K， BUCKNALL R， GREIG A. Review of collision avoidance and path planning methods for ships in close range encounters [J]. Journal of navigation， 2009， 62（3）： 455.

[2] 康與濤，朱大奇，陳偉炯.船舶避碰路徑規(guī)劃綜述[J].船海工程，2013，42（5）：141?145.

[3] 徐曉睛，朱慶保.動態(tài)環(huán)境下基于多人工魚群算法和避碰規(guī)則庫的機(jī)器人路徑規(guī)劃[J].電子學(xué)報，2012，40（8）：1694?1699.

[4] 向祖權(quán)，靳超，杜開君，等.基于粒子群優(yōu)化算法的水面無人艇分層局部路徑規(guī)劃[J].武漢理工大學(xué)學(xué)報，2015，37（7）：38?45.

[5] 馬文耀，吳兆麟，楊家軒，等.人工魚群算法的避碰路徑規(guī)劃決策支持[J].中國航海，2014，37（3）：63?67.

[6] 胥文，胡江強，尹建川，等.基于克隆選擇優(yōu)化的多船轉(zhuǎn)向避碰決策[J].艦船科學(xué)技術(shù)，2018，40（1）：52?56.

[7] 田雨波，潘朋朋.免疫粒子群算法在船舶避碰上的應(yīng)用[J].中國航海，2011，34（1）：48?53.

[8] 祖?zhèn)?，李剛，齊正霞.基于改進(jìn)粒子群優(yōu)化算法的路徑規(guī)劃方法研究[J].彈射與制導(dǎo)學(xué)報，2008，28（4）：68?82.

[9] EBERHART， SHI Y. Particle swarm optimization： developments， applications and resources [C]// Proceedings of the 2001 Congress on Evolutionary Computation. Seoul： IEEE， 2002： 81?86.

[10] 鄭中義，吳兆麟.船舶最佳轉(zhuǎn)向避碰幅度決策模型[J].大連海事大學(xué)學(xué)報，2000，26（4）：5?8.

[11] 齊樂，鄭中義，李國平.互見中基于AIS數(shù)據(jù)的船舶領(lǐng)域[J].大連海事大學(xué)學(xué)報，2011，37（1）：48?50.

作者簡介：杲 ?飛（1994—），男，山東濟(jì)南人，碩士，研究方向為船舶自主控制與自動化裝置。

顏德文（1969—），男，浙江溫嶺人，博士，副教授，研究方向為船舶自主控制與自動化裝置。