本發(fā)明涉及一種基于導航誤差約束的水面無人艇局部分層路徑規(guī)劃方法，屬于水面無人艇路徑規(guī)劃技術(shù)領(lǐng)域。

背景技術(shù)：

隨著科技的飛速發(fā)展，海上智能交通作為我國科技戰(zhàn)略的重要組成部分，主要用于實現(xiàn)船舶航行的自動化和海上交通管理的智能化。因此，水面無人艇作為海上交通系統(tǒng)的個體船舶，對其航行路徑的研究是很有必要的。路徑規(guī)劃是水面無人艇的一個重要研究領(lǐng)域，是水面無人艇能夠安全完成巡航任務(wù)的前提。近年來，水面無人艇路徑規(guī)劃已經(jīng)成為海上智能化的研究熱點。

路徑規(guī)劃的方法有很多，因自身的優(yōu)缺點而適用于各種不同的情況，大致可分為傳統(tǒng)算法、圖形法、智能仿生學算法以及其他算法等。傳統(tǒng)算法有人工勢場法、模糊邏輯算法和模擬退火算法等；圖形法有可視圖空間法、柵格法和voronoi圖法等；智能仿生學算法有蟻群算法、神經(jīng)網(wǎng)絡(luò)算法、遺傳算法和粒子群算法等；其他算法有A*算法、Dijkstra算法和Floyd算法等。

目前，絕大多數(shù)路徑規(guī)劃只劃分為全局路徑規(guī)劃和局部路徑規(guī)劃兩個層面，未考慮局部路徑規(guī)劃的分層問題，具有一定的局限性。此外，絕大多數(shù)靜態(tài)路徑規(guī)劃均是假設(shè)在導航信息精確的前提下，水面無人艇能夠精準地跟蹤規(guī)劃好的路徑。但實際導航系統(tǒng)中存在誤差，將會導致水面無人艇與實際障礙物之間的安全區(qū)域減少，會造成初始點到目標點的自由路徑不存在。即使自由路徑仍存在，也很有可能給水面無人艇帶來如碰撞等不可預(yù)知的風險。因此，在路徑規(guī)劃過程中，導航誤差的影響是不可忽視的問題，對水面無人艇的實際工程應(yīng)用具有重要意義。

目前也有部分與本發(fā)明有關(guān)的研究報告，1、復雜海況下水面無人艇分層危險規(guī)避方法研究，哈爾濱工程大學，2014，該博士論文中雖然考慮了三個分層層面，但未考慮導航誤差對路徑規(guī)劃的影響。2、一種基于導航誤差空間的無人水下航行器路徑規(guī)劃方法，兵工學報，2014，35(8)：1243-1250，該文獻中雖然考慮了導航誤差，但只是針對水下航行器進行了全局路徑規(guī)劃研究，且并未結(jié)合遺傳算法。

技術(shù)實現(xiàn)要素：

技術(shù)問題：本發(fā)明提供一種減少導航誤差對路徑選擇造成的不利影響的基于導航誤差約束的水面無人艇局部分層路徑規(guī)劃方法，該方法將局部路徑規(guī)劃劃分為基于導航誤差約束并結(jié)合遺傳算法的局部靜態(tài)路徑規(guī)劃以及融合海事規(guī)則的局部動態(tài)路徑規(guī)劃兩個層面，綜合考慮導航誤差約束、水面無人艇自身動力學約束等限制條件，可以提高規(guī)劃路徑的安全性和可行性。

技術(shù)方案：本發(fā)明的基于導航誤差約束的水面無人艇局部分層路徑規(guī)劃方法，包括以下步驟：

步驟1)根據(jù)作業(yè)任務(wù)、電子海圖和傳感器得到的靜態(tài)障礙物信息，采用直角坐標系和柵格法，將環(huán)境離散化為二維的基本單元柵格，通過對這些柵格的標示進行環(huán)境建模，初始化起點位置Start和目標點位置Goal；

步驟2)按照如下方式得到M條染色體：采用隨機法在柵格地圖上隨機選取除起點位置Start和目標點位置Goal外的a個自由柵格點，把起點和目標點分為a+1段，然后采用A*算法連接起點、目標點和a個自由柵格點，使之成為一個可行的初始解，即得到一條有效的染色體；

將所述M條染色體作為初始種群，即為第一代種群，其中M為第一代種群的規(guī)模；

步驟3)確定每條染色體的每個基因點的導航誤差，具體為：利用每條染色體中第k個基因點處的期望導航位置μ_k與第k+1個基因點處的期望導航位置μ_k+1之間的航行距離D(μ_k，μ_k+1)和導航誤差變化率Δ_ε，進行乘積運算，再加上第k個基因點的導航誤差ε_k，得到每條染色體第k+1個基因點的導航誤差ε_k+1；然后根據(jù)北斗導航誤差ε_BD，若第k+1個基因點的導航誤差ε_k+1＞ε_BD，則將ε_BD賦值ε_k+1；否則，第k+1個基因點的導航誤差ε_k+1為原值；

步驟4)根據(jù)所述步驟3)得到每條染色體的每個基因點的導航誤差，采用高斯模型作為導航誤差模型，根據(jù)該導航誤差模型的導航位置分布密度函數(shù)采用蒙特卡洛采樣法計算得到每條染色體的路徑代價L(s₀，s_g)，其中所有染色體路徑代價中的最小值為該代種群的最優(yōu)解；

步驟5)將有障礙物的危險區(qū)域作為碰撞區(qū)域A，根據(jù)導航誤差模型，在碰撞區(qū)域A內(nèi)對導航誤差概率密度函數(shù)進行積分，求出每條染色體的每個基因點處的碰撞概率，得到每條染色體的起點到目標點，即第0個基因點到第g個基因點之間的全部基因點處的碰撞概率集合{P_c(X₀)，P_c(X₁)...P_c(X_g)}；

步驟6)根據(jù)所述步驟5)得到的每條染色體的全部基因點的碰撞概率集合，針對每條染色體，查找該染色體在碰撞概率集合{P_c(X₀)，P_c(X₁)...P_c(X_g)}中的碰撞概率最大值，然后將其作為該染色體的碰撞概率P_c(X)；

步驟7)根據(jù)所述步驟4)得到的每條染色體的路徑代價和所述步驟6)得到的每條染色體的碰撞概率P_c(X)，按照以下方式計算每條染色體的適應(yīng)度；

判斷所述步驟6)得到的每條染色體的碰撞概率P_c(X)是否小于或等于設(shè)定值，如是，則根據(jù)下式計算每條染色體的適應(yīng)度E：

否則根據(jù)下式計算每條染色體的適應(yīng)度E：

其中m為權(quán)重值設(shè)定值，m＞＞L(s₀，s_g)；

將所有染色體的適應(yīng)度相加求平均，作為該代種群的平均適應(yīng)值；

步驟8)假設(shè)第n代種群代數(shù)為x(n)，若種群代數(shù)x(n)＜設(shè)定值n_setting，則進入步驟9)；否則，按照如下方式處理：

若在第n-(n_setting-1)代種群到當前第n代種群的連續(xù)n_setting代進化中，種群的最優(yōu)解均未發(fā)生變化，且相鄰兩代的種群平均適應(yīng)值變化沒有超過1％，則將當前第n代中具有最優(yōu)解的染色體作為所求解的一條最優(yōu)無碰路徑，并根據(jù)組成該染色體的基因點的坐標值得到一系列從起點到目標點的路徑節(jié)點序列，將該路徑節(jié)點序列作為局部航路點序列后，進入步驟10)，否則進入步驟9)；

步驟9)根據(jù)交叉率p_c，即設(shè)定的交叉操作的概率值，隨機選擇p_c*M條染色體進行交叉操作，對交叉的個體兩兩隨機地交換基因點形成新的染色體，替換適應(yīng)度最大的p_c*M條染色體，然后再根據(jù)變異率p_m，即設(shè)定的變異操作的概率值，隨機選擇p_m*M條染色體進行變異操作，對進行變異的個體隨機選擇基因點進行實值變異，替換適應(yīng)度最大的p_m*M條染色體；將交叉變異操作獲得的種群作為新的種群，即下一代(n+1代)種群代數(shù)為x(n+1)＝x(n)+1，返回步驟3)；

步驟10)根據(jù)所述步驟8)得到的一系列局部航路序列點，將第一個局部航路點作為動態(tài)規(guī)劃起始點，將第二個局部航路點作為動態(tài)規(guī)劃目標點；

步驟11)根據(jù)傳感器獲得水面無人艇航行過程中的動態(tài)障礙物位置信息及速度信息，求解水面無人艇與障礙物的最近會遇距離DCPA和到達最近會遇點的時間TCPA，然后根據(jù)下式，采用DCPA與TCPA加權(quán)得到碰撞危險度ρ：

ρ＝(ω_DDCPA)²+(ω_TTCPA)²

其中，ω_D、ω_T分別為加權(quán)值；

步驟12)判斷所述步驟11)得到的碰撞危險度ρ是否小于或等于經(jīng)驗值，如是，則根據(jù)融合海事規(guī)則的避障策略及水面無人艇的動力學約束條件，調(diào)整水面無人艇的航向角或速度大小，否則，保持水面無人艇原有的航向角和速度大小不變；

步驟13)根據(jù)步驟12)得到的航向角改變量Δα、速度大小改變量ΔV_USV和設(shè)定的規(guī)劃時間ΔT，計算水面無人艇規(guī)劃一個ΔT時間后的當前位置(X，Y)為：

式中，X、Y分別為規(guī)劃一個ΔT時間后水面無人艇在直角坐標系中的橫坐標值和縱坐標值，X_prev、Y_prev分別為此次規(guī)劃前水面無人艇在直角坐標系中的橫坐標值和縱坐標值，α、V_USV分別表示水面無人艇的航向角和速度大小。

判斷水面無人艇的當前位置是否為當前動態(tài)規(guī)劃目標點，如是，則進入步驟14)，否則返回步驟11)；

步驟14)根據(jù)所述步驟1)得到的目標點位置Goal，判斷水面無人艇的當前位置是否為目標點位置Goal，如是，則結(jié)束本方法流程，否則將當前動態(tài)規(guī)劃目標點作為下一個動態(tài)規(guī)劃起始點，將下一個局部航路點作為下一個動態(tài)規(guī)劃目標點，返回步驟11)。

進一步的，本發(fā)明方法中，所述步驟4)中結(jié)合導航誤差模型求解每條染色體的路徑代價的具體求解過程包括：

導航誤差模型為：

X_k～N(μ_k，Σ_k)

式中，為水面無人艇的期望導航位置，X_k＝(x_k，y_k)為第k個基因點處水面無人艇的導航位置，∑_k為第k個基因點的協(xié)方差矩陣。

則該導航誤差模型的導航位置分布密度函數(shù)為：

所述協(xié)方差矩陣∑_k根據(jù)下式確定：

其中，ε_k＝3σ_k

式中，ε_k為第k個基因點的導航誤差；σ_k為第k個基因點的標準差；

所述每條染色體的路徑代價L(s₀，s_g)的具體計算過程包括：

根據(jù)概率模型中的3σ準則，在第k個基因點處的狀態(tài)s_k的導航不確定區(qū)域內(nèi)，即以第k個基因點處的期望導航位置μ_k為圓心的3σ置信區(qū)域內(nèi)，采用蒙特卡洛法采樣N個樣本點，獲得第k個基因點處的狀態(tài)s_k的導航不確定區(qū)域內(nèi)的采樣集合Θ_k：

在第k+1個基因點處的狀態(tài)s_k+1的導航不確定區(qū)域內(nèi)采樣N個樣本點，得到第k+1個基因點處的狀態(tài)s_k+1的導航不確定區(qū)域內(nèi)的采樣集合Θ_k+1：

根據(jù)第k個基因點的采樣集合Θ_k與第k+1個基因點的采樣集合Θ_k+1，計算第k個基因點到第k+1個基因點的N對采樣點之間的歐式距離，然后將N對采樣點之間的歐式距離求和取平均，得到第k個基因點到第k+1個基因點的轉(zhuǎn)移代價L(s_k，s_k+1)：

根據(jù)下式計算起始基因點處的狀態(tài)s₀到目標基因點處的狀態(tài)s_g的每條染色體的路徑代價L(s₀，s_g)：

進一步的，本發(fā)明方法中，所述步驟12)中的融合海事規(guī)則的避障策略為：

式中：e_i+f_i＝1，其中e_i和f_i為二進制變量，取0或1，ξ為正實數(shù)且無窮大，當水面無人艇和障礙物右側(cè)交叉相遇局面或正面相遇局面時，靠右側(cè)避障，此時e_i＝1，f_i＝0，當水面無人艇和障礙物左側(cè)交叉相遇局面或追越局面時，靠左側(cè)避障，此時e_i＝0，f_i＝1；ΔV為水面無人艇相對障礙物的速度矢量V_UO大?。沪う?sub>UO^L為水面無人艇向左轉(zhuǎn)時，水面無人艇能夠逃離碰撞區(qū)域前提下，相對速度ΔV的方向改變量的最小值；Δγ_UO^R為水面無人艇向右轉(zhuǎn)時，水面無人艇能夠逃離碰撞區(qū)域前提下，相對速度ΔV的方向改變量的最大值；為水面無人艇相對障礙物的速度矢量V_UO到水面無人艇速度矢量V_USV的角度；ΔV_USV為水面無人艇速度大小的改變量；Δα為水面無人艇航向角的改變量；

所述步驟12)中，根據(jù)上述避碰策略及水面無人艇的動力學約束，采用混合整數(shù)線性規(guī)劃法，得到水面無人艇的航向角和速度大小的調(diào)整量。

進一步的，本發(fā)明方法中，所述步驟11)中，對于右舷目標ω_D＝5、ω_T＝0.5，對于左舷目標ω_D＝5、ω_T＝1。

進一步的，本發(fā)明方法中，所述步驟12)中，水面無人艇的動力學約束條件包括最大速度、最大加速度、最小轉(zhuǎn)彎半徑。本發(fā)明方法中，路徑規(guī)劃主要為了實現(xiàn)避障，障礙物可劃分動態(tài)和靜態(tài)兩種形式，前者(...局部靜態(tài)路徑規(guī)劃)主要避開靜態(tài)障礙物，后者(...局部動態(tài)路徑規(guī)劃)主要避開動態(tài)障礙物。但兩者并不相互獨立，前者靜態(tài)規(guī)劃后得到一系列局部航路點，可以作為后者動態(tài)規(guī)劃的起點和目標點。假設(shè)靜態(tài)規(guī)劃獲得一系列包含起始點Start的位置點S₀和目標點Goal的位置點Sn的局部航路點序列S₀、S₁、S₂...Sn，則進行動態(tài)規(guī)劃時，首先以S₀作為動態(tài)規(guī)劃起點，S₁作為動態(tài)規(guī)劃目標點，進行動態(tài)路徑規(guī)劃到達動態(tài)規(guī)劃目標點后，然后將S₁作為動態(tài)規(guī)劃起點，S₂作為動態(tài)規(guī)劃目標點，進行動態(tài)規(guī)劃，不斷重復，直到到達目標點Sn停止。

有益效果：本發(fā)明與現(xiàn)有技術(shù)相比，具有以下優(yōu)點：

1)與現(xiàn)有技術(shù)相比，本發(fā)明采用了局部分層思想，將局部路徑規(guī)劃劃分為靜態(tài)路徑規(guī)劃和動態(tài)路徑規(guī)劃兩個層面，靜態(tài)局部路徑規(guī)劃以路徑最短為優(yōu)化目標，可獲得最短的無碰路徑，動態(tài)局部路徑規(guī)劃以盡快逃離障礙物的運動區(qū)域為優(yōu)化目標，使得水面無人艇遠離運動障礙物以確保實時航行的安全性。此外，在算法實現(xiàn)上，將電子海圖及傳感器獲得的靜態(tài)障礙物信息傳遞到局部靜態(tài)路徑規(guī)劃中，在動態(tài)局部規(guī)劃中不需要實時對靜態(tài)障礙物進行重復規(guī)劃，減少了路徑規(guī)劃時間以及內(nèi)存占用。

2)與現(xiàn)有技術(shù)相比，本發(fā)明采用了基于導航誤差約束的改進遺傳路徑規(guī)劃方法，將非精確的導航信息轉(zhuǎn)化為環(huán)境模型中的未知環(huán)境信息，融合到路徑規(guī)劃中，不再以精確導航信息為假設(shè)前提，在環(huán)境模型中充分考慮導航誤差的影響，減少了導航誤差對路徑選擇造成的不利影響，提高了規(guī)劃路徑的安全性。與枚舉、啟發(fā)式等傳統(tǒng)搜索方法相比較，改進遺傳算法具有良好的全局搜索能力，可以從路徑規(guī)劃空間中快速搜索出全體滿足約束條件的無碰路徑，而不會陷入局部最優(yōu)解；搜索從所有可行路徑構(gòu)成的種群出發(fā)，具有潛在的并行性，可以方便地進行分布式計算，加快求解速度；對于輸入規(guī)模很大時，不同于傳統(tǒng)方法的無解情況，仍可以有效地求解出最優(yōu)或近似最優(yōu)解。

本發(fā)明在VC仿真條件下，對該方法考慮導航誤差影響基于遺傳算法的局部靜態(tài)路徑規(guī)劃進行仿真實驗：

遺傳算法參數(shù)設(shè)置：種群大小M＝50，交叉率p_c＝0.8，變異率p_m＝0.1；

導航誤差初始值：ε₀＝0.1m；

導航誤差變化率：Δ_ε＝0.05；

利用發(fā)明所述方法得到的，分別如圖3所示。結(jié)果表明在考慮導航誤差影響下，采用本發(fā)明方法可以在路徑規(guī)劃過程中不再選擇狹小區(qū)域而選擇繞行到自由區(qū)域，以犧牲路徑長度的代價來保證水面無人艇航行安全性和可行性。

附圖說明

圖1為本發(fā)明的流程框圖；

圖2為本發(fā)明中海事規(guī)則沖突情景定義以及不同會遇情景下所需采取的避讓措施示意圖；

圖3為本發(fā)明基于導航誤差的水面無人艇路徑規(guī)劃示意圖；

圖4為本發(fā)明有無考慮導航誤差影響的水面無人艇局部靜態(tài)路徑規(guī)劃對比曲線圖。

圖5為本發(fā)明的結(jié)構(gòu)框圖。

具體實施方式

下面結(jié)合實施例和說明書附圖對本發(fā)明作進一步的說明。

本發(fā)明的基于導航誤差約束的水面無人艇局部分層路徑規(guī)劃方法包括以下步驟：

步驟1)根據(jù)作業(yè)任務(wù)、電子海圖和傳感器得到的靜態(tài)障礙物信息，采用直角坐標系和柵格法，將環(huán)境離散化為二維的基本單元柵格，通過對這些柵格的標示進行環(huán)境建模，初始化起點位置Start和目標點位置Goal；其中有障礙物的區(qū)域標記為1，即表示障礙柵格，無障礙物的區(qū)域標記為0，即表示自由柵格；直角坐標系以起始點為原點，x、y、z分別指向起始點的東、北、天；

步驟2)按照如下方式得到M條染色體：采用隨機法在柵格地圖上隨機選取除起點位置Start和目標點位置Goal外的a個自由柵格點，把起點和目標點分為a+1段，然后采用A*算法連接起點、目標點和a個自由柵格點，使之成為一個可行的初始解，即得到一條有效的染色體；

將所述M條染色體作為初始種群，即為第一代種群，其中M為第一代種群的規(guī)模；

步驟3)確定每條染色體的每個基因點的導航誤差，具體為：利用每條染色體中第k個基因點處的期望導航位置μ_k與第k+1個基因點處的期望導航位置μ_k+1之間的航行距離D(μ_k，μ_k+1)和導航誤差變化率Δ_ε，進行乘積運算，再加上第k個基因點的導航誤差ε_k，得到每條染色體第k+1個基因點的導航誤差ε_k+1：

f_ε：ε_k+1＝ε_k+Δ_εD(μ_k，μ_k+1)

然后根據(jù)北斗導航誤差ε_BD，若第k+1個基因點的導航誤差ε_k+1＞ε_BD，則將ε_BD賦值ε_k+1；否則，第k+1個基因點的導航誤差ε_k+1為原值；

步驟4)根據(jù)所述步驟3)得到每條染色體的每個基因點的導航誤差，采用高斯模型作為導航誤差模型，根據(jù)該導航誤差模型的導航位置分布密度函數(shù)采用蒙特卡洛采樣法計算得到每條染色體的路徑代價L(s₀，s_g)，其中所有染色體路徑代價中的最小值為該代種群的最優(yōu)解；

4.1)導航誤差模型為：

X_k～N(μ_k，∑_k)

式中，為水面無人艇的期望導航位置，X_k＝(x_k，y_k)為第k個基因點處水面無人艇的導航位置，∑_k為第k個基因點的協(xié)方差矩陣。

則該導航誤差模型的導航位置分布密度函數(shù)為：

所述協(xié)方差矩陣∑_k根據(jù)下式確定：

其中，ε_k＝3σ_k

式中，ε_k為第k個基因點的導航誤差；σ_k為第k個基因點的標準差；

4.2)每條染色體的路徑代價L(s₀，s_g)的具體計算過程包括：

根據(jù)概率模型中的3σ準則，在第k個基因點處的狀態(tài)s_k的導航不確定區(qū)域內(nèi)，即以第k個基因點處的期望導航位置μ_k為圓心的3σ置信區(qū)域內(nèi)，采用蒙特卡洛法采樣N個樣本點，獲得第k個基因點處的狀態(tài)s_k的導航不確定區(qū)域內(nèi)的采樣集合Θ_k：

在第k+1個基因點處的狀態(tài)s_k+1的導航不確定區(qū)域內(nèi)采樣N個樣本點，得到第k+1個基因點處的狀態(tài)s_k+1的導航不確定區(qū)域內(nèi)的采樣集合Θ_k+1：

根據(jù)第k個基因點的采樣集合Θ_k與第k+1個基因點的采樣集合Θ_k+1，計算第k個基因點到第k+1個基因點的N對采樣點之間的歐式距離，然后將N對采樣點之間的歐式距離求和取平均，得到第k個基因點到第k+1個基因點的轉(zhuǎn)移代價L(s_k，s_k+1)：

根據(jù)下式計算起始基因點處的狀態(tài)s₀到目標基因點處的狀態(tài)s_g的每條染色體的路徑代價L(s₀，s_g)：

步驟5)將有障礙物的危險區(qū)域作為碰撞區(qū)域A，根據(jù)導航誤差模型，在碰撞區(qū)域A內(nèi)對導航誤差概率密度函數(shù)進行積分，求出每條染色體的每個基因點處的碰撞概率：

式中，P_c(X_k)為每條染色體第k個基因點處的碰撞概率；

根據(jù)上式得到每條染色體的第0個基因點(即起點)到第g個基因點(即目標點)之間的全部基因點處的碰撞概率集合{P_c(X₀)，P_c(X₁)...P_c(X_g)}；

步驟6)根據(jù)所述步驟5)得到的每條染色體的全部基因點的碰撞概率集合，針對每條染色體，查找該染色體在碰撞概率集合{P_c(X₀)，P_c(X₁)...P_c(X_g)}中的碰撞概率最大值，然后將其作為該染色體的碰撞概率P_c(X)：

P_c(X)＝max{P_c(X₀)，P_c(X₁)...P_c(X_g)}

步驟7)根據(jù)所述步驟4)得到的每條染色體的路徑代價和所述步驟6)得到的每條染色體的碰撞概率P_c(X)，按照以下方式計算每條染色體的適應(yīng)度；

判斷所述步驟6)得到的每條染色體的碰撞概率P_c(X)是否小于或等于設(shè)定值，如是，則根據(jù)下式計算每條染色體的適應(yīng)度E：

否則根據(jù)下式計算每條染色體的適應(yīng)度E：

其中m為權(quán)重值設(shè)定值，m＞＞L(s₀，s_g)；

將所有染色體的適應(yīng)度相加求平均，作為該代種群的平均適應(yīng)值；

步驟8)假設(shè)第n代種群代數(shù)為x(n)，若種群代數(shù)x(n)＜設(shè)定值n_setting，則進入步驟9)；否則，按照如下方式處理：

若在第n-(n_setting-1)代種群到當前第n代種群的連續(xù)n_setting代進化中，種群的最優(yōu)解均未發(fā)生變化，且相鄰兩代的種群平均適應(yīng)值變化沒有超過1％，則將當前第n代中具有最優(yōu)解的染色體作為所求解的一條最優(yōu)無碰路徑，并根據(jù)組成該染色體的基因點的坐標值得到一系列從起點到目標點的路徑節(jié)點序列，將該路徑節(jié)點序列作為局部航路點序列后，進入步驟10)，否則進入步驟9)；

步驟9)根據(jù)交叉率p_c，即設(shè)定的交叉操作的概率值，隨機選擇p_c*M條染色體進行交叉操作，對交叉的個體兩兩隨機地交換基因點形成新的染色體，替換適應(yīng)度最大的p_c*M條染色體，然后再根據(jù)變異率p_m，即設(shè)定的變異操作的概率值，隨機選擇p_m*M條染色體進行變異操作，對進行變異的個體隨機選擇基因點進行實值變異，替換適應(yīng)度最大的p_m*M條染色體；將交叉變異操作獲得的種群作為新的種群，即下一代(n+1代)種群代數(shù)為x(n+1)＝x(n)+1，返回步驟3)；

步驟10)根據(jù)所述步驟8)得到的一系列局部航路序列點，將第一個局部航路點作為動態(tài)規(guī)劃起始點，將第二個局部航路點作為動態(tài)規(guī)劃目標點；

步驟11)根據(jù)傳感器獲得水面無人艇航行過程中的動態(tài)障礙物位置信息及速度信息，求解水面無人艇與障礙物的最近會遇距離DCPA和到達最近會遇點的時間TCPA，然后根據(jù)下式，采用DCPA與TCPA加權(quán)得到碰撞危險度ρ：

ρ＝(ω_DDCPA)²+(ω_TTCPA)²

其中，ω_D、ω_T分別為加權(quán)值；一般情況下，對于右舷目標ω_D＝5、ω_T＝0.5，對于左舷目標ω_D＝5、ω_T＝1；

步驟12)判斷所述步驟11)得到的碰撞危險度ρ是否小于或等于經(jīng)驗值，如是，則根據(jù)融合海事規(guī)則的避障策略及水面無人艇的動力學約束條件，調(diào)整水面無人艇的航向角或速度大小，否則，保持水面無人艇原有的航向角和速度大小不變；

其中，根據(jù)融合海事規(guī)則的避障策略，調(diào)整水面無人艇的航向角或速度大小的具體過程包括：步驟12.1)以水面無人艇的航行方向為基準，求出水面無人艇和障礙物之間的航向差Δθ，再根據(jù)國際海事規(guī)則的沖突情景定義判斷相遇局面類型：

如果Δθ∈[0°，45°]∪[315°，360°)，則為追越局面；

如果Δθ∈[165°，195°]，則為正面相遇局面；

如果Δθ∈(45°，165°)，則為右側(cè)交叉相遇局面；

如果Δθ∈(195°，315°)，則為左側(cè)交叉相遇局面；

步驟12.2)根據(jù)當前水面無人艇的位置矢量X_UsV和速度矢量V_USV、障礙物的位置矢量X_Obs和速度矢量V_Obs，結(jié)合極坐標系，以北向為極軸方向，逆時針為正，通過速度避障法求得水面無人艇的避碰模型為：

式中，ΔV為水面無人艇相對障礙物的速度矢量V_UO大小；Δγ_UO^L為水面無人艇向左轉(zhuǎn)時，水面無人艇能夠逃離碰撞區(qū)域前提下，相對速度ΔV的方向改變量的最小值；Δγ_UO^R為水面無人艇向右轉(zhuǎn)時，水面無人艇能夠逃離碰撞區(qū)域前提下，相對速度ΔV的方向改變量的最大值；為水面無人艇相對障礙物的速度矢量V_UO到水面無人艇速度矢量V_USV的角度；ΔV_USV為水面無人艇速度大小的改變量；Δα為水面無人艇航向角的改變量；

步驟12.3)根據(jù)所述步驟12.1)得到的相遇局面類型和所述步驟12.3)得到的避障模型，得到避碰策略為：

式中：e_i+f_i＝1，其中e_i和f_i為二進制變量，取0或1，ξ為正實數(shù)且無窮大，當水面無人艇和障礙物右側(cè)交叉相遇局面或正面相遇局面時，靠右側(cè)避障，此時e_i＝1，f_i＝0，當水面無人艇和障礙物左側(cè)交叉相遇局面或追越局面時，靠左側(cè)避障，此時e_i＝0，f_i＝1；

步驟12.4)根據(jù)所述步驟12.3)得到的避碰策略及水面無人艇的最大速度、最大加速度、最小轉(zhuǎn)彎半徑等動力學約束，采用混合整數(shù)線性規(guī)劃法，得到水面無人艇的航向角和速度大小的調(diào)整量。

步驟13)根據(jù)水面無人艇的性能和實際需求自行設(shè)置規(guī)劃時間ΔT，根據(jù)步驟12)得到的航向角改變量Δα、速度大小改變量ΔV_USV和設(shè)定的規(guī)劃時間ΔT，計算水面無人艇規(guī)劃一個ΔT時間后的當前位置(X，Y)為：

式中，X、Y分別為規(guī)劃一個ΔT時間后水面無人艇在直角坐標系中的橫坐標值和縱坐標值，X_prev、Y_prev分別為此次規(guī)劃前水面無人艇在直角坐標系中的橫坐標值和縱坐標值，α、V_USV分別表示水面無人艇的航向角和速度大小。

判斷水面無人艇的當前位置是否為當前動態(tài)規(guī)劃目標點，如是，則進入步驟14)，否則返回步驟11)；

步驟14)根據(jù)所述步驟1)得到的目標點位置Goal，判斷水面無人艇的當前位置是否為目標點位置Goal，如是，則結(jié)束本方法流程，否則將當前動態(tài)規(guī)劃目標點作為下一個動態(tài)規(guī)劃起始點，將下一個局部航路點作為下一個動態(tài)規(guī)劃目標點，返回步驟11)。

上述實施例僅是本發(fā)明的優(yōu)選實施方式，應(yīng)當指出：對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進和等同替換，這些對本發(fā)明權(quán)利要求進行改進和等同替換后的技術(shù)方案，均落入本發(fā)明的保護范圍。