本發(fā)明屬于無人潛航器控制方法，尤其涉及一種用于破冰潛航器路徑規(guī)劃粒子群優(yōu)化算法。

背景技術(shù)：

1、路徑規(guī)劃對于破冰潛航器執(zhí)行破冰任務(wù)至關(guān)重要，合理的路徑規(guī)劃不僅可以減少破冰潛航器的行進距離，還能避免破冰潛航器遇上厚冰層，甚至冰山等潛在的威脅。其核心在于利用路徑規(guī)劃算法來確定破冰潛航器從起點到終點的最優(yōu)航線。此過程應(yīng)當(dāng)綜合考慮冰層厚度、物理限制以及其他潛在的環(huán)境因素，確保破冰潛航器能夠安全有效地完成破冰任務(wù)。

技術(shù)實現(xiàn)思路

1、本發(fā)明的目的在于，基于破冰潛航器的任務(wù)需求，提供一種能夠在充分考慮潛航器運行成本的前提下高效完成破冰任務(wù)的路徑規(guī)劃粒子群優(yōu)化算法。

2、為實現(xiàn)上述目的，本發(fā)明采用如下技術(shù)方案。

3、一種用于破冰潛航器路徑規(guī)劃粒子群優(yōu)化算法，包括如下步驟：

4、步驟一、構(gòu)建成本函數(shù)，具體是指：

5、a1、確定深度成本：粒子i規(guī)劃的航行路徑pi包括有n個航路點，每個航路點對應(yīng)于搜索地圖中的一個路徑結(jié)點pij＝(xij，yij，zij)，其中j表示航路點編號；水下破冰潛航器下潛深度限制為極值最小深度hmin和最大深度hmax之間，針對某一確定的航路點計算出的深度成本f1(pi)為：

6、

7、

8、其中，hij表示水下破冰潛航器經(jīng)過pij時的絕對深度，hij表示當(dāng)下水下破冰潛航器未按照平均深度航行時所需的成本；

9、a2、確定平滑航行成本：平滑航行成本的變量為水下破冰潛航器執(zhí)行任務(wù)時路徑的水平角和俯仰角ψij，定義為兩個連續(xù)路徑在水平方向上的夾角，ψij定義為路徑和其在水平方向上投影之間的夾角，定義航路點pij＝(xij，yij，zij)在xy平面上的投影為p′ij＝(x′ij，y′ij，0)，則水平角和俯仰角ψij的表達式分別為：

10、

11、平滑航行成本f2(pi)為：

12、其中a1和a2分別是轉(zhuǎn)彎角度和爬升角度的成本系數(shù)；由于船舶的航行路徑是水下破冰潛航器的規(guī)劃路徑在xy平面上的投影，并且水下破冰潛航器設(shè)置為定深模式，所以參數(shù)a2為0。

13、a3、確定路徑成本：考慮到水下破冰潛航器航行距離的限制，選擇最小化路徑長度以提供更遠距離的路徑規(guī)劃；航路點pij＝(xij，yij，zij)在xy平面上的投影為p′ij＝(x′ij，y′ij，0)；由于破冰潛航器在z軸方向上的位移相較于整個航行路徑而言過小，因此忽略不計，所以兩個航路點之間的歐式距離路徑成本f3(pi)可表達為：

14、a4、確定威脅成本：規(guī)劃路徑需要引導(dǎo)船舶避免水下障礙物帶來的威脅；定義z是所有威脅的集合；將威脅以一個圓柱體進行建模，其在xy平面上投影的中心坐標為cz，半徑為rz；對于給定的路徑段威脅成本與cz到的距離dz成正比；則考慮水下破冰潛航器的直徑d和危險距離s確定的破冰潛航器跨過所有航路點pij的威脅成本f4(pi)為：

15、

16、是指通過給定的路徑段的威脅成本

17、dz是指水下障礙物中心在xy平面上投影的中心坐標為cz，對于給定的路徑段dz即為cz到的距離；z∈z是指第z個障礙物；

18、a5、確定連續(xù)破冰成本：連續(xù)破冰成本j大小主要取決于海冰厚度t，其表達式可用分段函數(shù)來描述：

19、

20、將水下破冰潛航器的路徑采樣后投影到海冰底部得到npts個海冰底部的高程數(shù)據(jù)zi″，其絕對值為海冰的厚度ti，對應(yīng)的破冰成本為ji；對于規(guī)劃的航行路徑pi，其連續(xù)破冰成本可被表示為：

21、a6、確定總體成本：規(guī)劃路徑需在滿足實際應(yīng)用要求的條件下達到最佳；通過考慮與航路點pi相關(guān)聯(lián)的最優(yōu)性、安全性和可行性約束，求得總體成本函數(shù)可表示為：

22、

23、步驟二、基于粒子群算法的路徑優(yōu)化，具體是指：

24、將水下破冰潛航器的路徑規(guī)劃轉(zhuǎn)化為優(yōu)化問題，其目標是找到合適的航路點列表pi使得f(pi)最?。蝗褐械拿總€粒子i通過位置信息pi＝(pi1，pi2，…，pin)和速度信息vi＝(vi1，vi2，...，vin)來表征；第i個粒子迄今為止搜索到的局部最優(yōu)位置為li＝(li1，li2，…，lin)，整個粒子群搜索到的全局最優(yōu)位置為lg＝(lg1，lg2，...，lgn)；

25、對于總數(shù)為m的粒子群，在找到兩個最優(yōu)位置后，先更新自己的速度：

26、

27、粒子根據(jù)速度更新自己的位置：

28、

29、其中，r為迭代次數(shù)；c1和c2為學(xué)習(xí)因子；d1j和d2j為[0，1]之間的均勻隨機數(shù)；

30、由于所有路徑的起點和終點是固定的，每個粒子的位置編碼中不包括起點和終點；則粒子群對應(yīng)的m條路徑矩陣p＝[p1，p2，...，pm]t每個都包括以下形式的n個路徑點列表：

31、pi＝(xi1，yi1，zi1，xi2，yi2，zi2，...，xi，n，yi,n，zi，n)；

32、基于總體成本函數(shù)給出的評估依據(jù)，不斷迭代計算優(yōu)化收斂，最終給出最優(yōu)路徑，使得總成本函數(shù)最小。

33、對前述用于破冰潛航器路徑規(guī)劃粒子群優(yōu)化算法的進一步改進或者優(yōu)選實施方案，所述步驟二中，基于如下步驟實現(xiàn)最優(yōu)搜索：

34、建立具有n個航路點的水下路徑ui在3n維超球面上的位置向量：

35、ui＝(ρi1，ψi1，φi1，ρi2，ψi2，φi2，…，ρin，ψin，φin)，n＝n-2；

36、其中ρij，φij表示水下破冰潛航器在水下路徑上第j個航路點的姿態(tài)信息

37、，j∈(1，n)；其中ρin∈(0，path_length)表示水下破冰潛航器在水下路徑ui上第n個航路點的路徑長度；path_length是指路徑總長度；表示水下破冰潛航器在水下路徑ui上第j個航路點的俯仰角；φij∈(-π，π)表示水下破冰潛航器在水下路徑ui上第j個航路點的方位角；n是指水下路徑ui的路徑點總數(shù)；

38、建立樹下無人機在前述位置的速度向量δui：

39、δui＝(δρi1，δψi1，δφi1…δρin，δψin，δφin)；

40、其中δρij，δφij表示水下破冰潛航器在水下路徑ui上第j個航路點的速度信息；δρij表示水下破冰潛航器在水下路徑ui上第j個航路點的速度；

41、表示水下破冰潛航器在水下路徑ui上第j個航路點的俯仰角變化率；δφij表示水下破冰潛航器在水下路徑ui上第j個航路點的方位角變化率；

42、定義球面矢量速度矢量得到基于集合的粒子群算法的路徑更新方程為：

43、

44、其中i是指第i個粒子，j是指路徑中的第j個航路點；r是指迭代次數(shù)；w是指慣性權(quán)重，影響粒子搜索速度；m是指粒子群總數(shù)；

45、其中qi＝(qi1，qi2，...，qin)和qg＝(qg1，qg2，...，qgn)分別表示i粒子局部和全局表征最佳位置的向量集；為確定qi和qg，通過向量ui求出對應(yīng)的pi，以便評估相關(guān)的成本；

46、其中向量到航路點pij＝(xij，yij，zij)∈pi映射可被表示為：

47、

48、定義映射ξ：u→p，則局部最優(yōu)位置和全局最優(yōu)位置的計算方法如下：

49、

50、

51、其有益效果在于：

52、本發(fā)明的用于破冰潛航器路徑規(guī)劃粒子群優(yōu)化算法，能夠在充分考慮潛航器使用過程中各種運行成本和風(fēng)險成本的基礎(chǔ)上，提供最合理的導(dǎo)航路徑規(guī)劃數(shù)據(jù)，能夠有效提高破冰潛航器任務(wù)執(zhí)行的效率，降低巡航時的能源消耗進而提高潛航器續(xù)航時間和范圍，擴大其業(yè)務(wù)能力。