技術(shù)特征：

1.一種基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法，其特征是包括如下步驟：

步驟1.以起點(diǎn)S和終點(diǎn)D為基準(zhǔn)建立航行空間，以水下潛器的最小安全潛行深度z_safemin形成的水平面z為基準(zhǔn)，建立Lazy Theta星算法二維障礙物柵格地圖，z＝z_safemin；

步驟2.建立Lazy Theta星算法代價(jià)函數(shù)；

步驟3.以z＝z_safemin水平面為L(zhǎng)azy Theta星算法二維路徑規(guī)劃平面，以路徑起點(diǎn)S的X、Y軸坐標(biāo)(x_s,y_s)和路徑終點(diǎn)D的X、Y軸坐標(biāo)(x_d,y_d)為二維路徑的起點(diǎn)和終點(diǎn)，在水平面內(nèi)進(jìn)行二維路徑規(guī)劃；

步驟4.根據(jù)三維路徑規(guī)劃尋找一條無(wú)碰長(zhǎng)度最短路徑的優(yōu)化目標(biāo)，設(shè)計(jì)深度規(guī)劃評(píng)價(jià)函數(shù)；

步驟5.利用粒子群算法進(jìn)行深度規(guī)劃；

步驟6.輸出最優(yōu)三維路徑。

2.根據(jù)權(quán)利要求1所述的基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法，其特征是步驟3具體包括：柵格地圖中的網(wǎng)格點(diǎn)總數(shù)為n，具體步驟如下，

步驟3.1.定義OPEN和CLOSED兩個(gè)表；

步驟3.2.初始化OPEN表和CLOSED表；

步驟3.3.判斷OPEN表是否為空，為空則搜索路徑失敗，若不為空，執(zhí)行步驟3.4；

步驟3.4.把OPEN表中表頭網(wǎng)格點(diǎn)P_i移入CLOSED表中，i<n；

步驟3.5.判斷節(jié)點(diǎn)P_i是否是終點(diǎn)D，若是，則路徑搜索成功，轉(zhuǎn)至步驟3.9，若不是，執(zhí)行步驟3.6；

步驟3.6.對(duì)節(jié)點(diǎn)P_i和其父節(jié)點(diǎn)P_iparent進(jìn)行障礙物檢測(cè)，若P_i和P_iparent連線之間沒(méi)有障礙物，保留P_iparent為P_i的父節(jié)點(diǎn)，否則，更新節(jié)點(diǎn)P_i的適應(yīng)值和父節(jié)點(diǎn)；

步驟3.7.擴(kuò)展P_i的東、西、南、北、東南、東北、西南、西北八個(gè)方向地理位置相鄰的子節(jié)點(diǎn)P_j，j＜n，j≠i；

步驟3.8.對(duì)OPEN表中各個(gè)網(wǎng)格點(diǎn)按照適應(yīng)值的大小進(jìn)行升序排序，返回步驟3.3；

步驟3.9.算法終止，輸出包含路徑起點(diǎn)和終點(diǎn)的二維路徑序列P(P₁,P₂,…P_k)，k為最終的路徑點(diǎn)的個(gè)數(shù)。

3.根據(jù)權(quán)利要求2所述的基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法，其特征是所述擴(kuò)展P_i的東、西、南、北、東南、東北、西南、西北八個(gè)方向地理位置相鄰的子節(jié)點(diǎn)P_j，具體包括以下幾種情況：

步驟3.7.1.如果P_j不在OPEN表或者CLOSED表中，將其存放在OPEN表中，設(shè)P_i的父節(jié)點(diǎn)為P_iparent，設(shè)置P_j父節(jié)點(diǎn)為P_iparent；

步驟3.7.2.如果P_j在OPEN表中，重新計(jì)算在OPEN表中的適應(yīng)值F(P_j)，若新適應(yīng)值小于原有適應(yīng)值，更新節(jié)點(diǎn)P_j的適應(yīng)值和父節(jié)點(diǎn)，若新適應(yīng)值大于原有適應(yīng)值，保持原有適應(yīng)值和父節(jié)點(diǎn)的設(shè)置；

步驟3.7.3.如果P_j已經(jīng)存在于CLODED表中或者是障礙物點(diǎn)，忽略此節(jié)點(diǎn)。

4.根據(jù)權(quán)利要求1、2或3所述的基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法，其特征是所述利用粒子群算法進(jìn)行深度規(guī)劃的具體步驟為：

步驟5.1.初始化粒子群算法參數(shù)；

步驟5.2.隨機(jī)產(chǎn)生一組初始解；

步驟5.3.根據(jù)步驟4確定的深度規(guī)劃路徑評(píng)價(jià)函數(shù)計(jì)算各個(gè)粒子的適應(yīng)度值，更新粒子的個(gè)體最優(yōu)值和種群最優(yōu)值；

步驟5.4.計(jì)算當(dāng)前迭代中粒子群的慣性權(quán)重和學(xué)習(xí)因子；

步驟5.5.根據(jù)粒子更新公式更新粒子；

步驟5.6.重復(fù)步驟5.3～步驟5.5直到迭代次數(shù)達(dá)到最大迭代次數(shù)N；

步驟5.7.輸出最優(yōu)路徑節(jié)點(diǎn)深度坐標(biāo)，粒子群算法路徑規(guī)劃結(jié)束，得到一條從起點(diǎn)到終點(diǎn)的最優(yōu)三維路徑。