本發(fā)明涉及的是一種水下潛器路徑規(guī)劃方法。具體地說是一種水下潛器的三維路徑規(guī)劃方法。

背景技術：

三維航路規(guī)劃是水下潛器等具有空間移動能力的智能機器執(zhí)行水下任務的關鍵技術之一，越來越受到人們的重視?，F(xiàn)有的三維路徑規(guī)劃算法大部分由二維路徑規(guī)劃算法發(fā)展而來，在二維路徑規(guī)劃中，A星算法應用廣泛。例如申請?zhí)枮?01110172301.9的專利文件中，提出了一種游戲路徑搜索的簡化方法；申請?zhí)枮?01410531309.3的專利文件中，他提出了一種基于二叉堆節(jié)點排序的A星尋路方法系統(tǒng)；申請?zhí)枮?01410010003.3的專利文件中，提出了一種基于自適應A星算法的飛行器最優(yōu)路徑確定方法。上述三個技術方案中利用A星算法，能夠快速搜索得到二維空間中一條無碰最短路徑。然而A星算法搜索得到的路徑有兩個明顯的缺點，一是路徑長度比實際最短路徑長，二是存在許多折角需要進行后期加工平滑。Lazy Theta星算法是解決A星算法不足的一種變體。Lazy Theta星算法不僅路徑不受地圖網(wǎng)格邊緣限制，而且路徑節(jié)點少，路徑長度短，但是在三維問題求解上適應性不高，高維大范圍問題的求解計算復雜度仍很大。

三維環(huán)境相比于二維環(huán)境無論在地形障礙物建?；蚴锹窂剿阉魃隙几鼮閺碗s。目前備受關注的三維路徑規(guī)劃的算法是啟發(fā)式智能優(yōu)化算法，如粒子群算法、蟻群算法、螢火蟲算法等。例如申請?zhí)枮?01210178003.5的專利文件中，公開了一種基于粒子群算法的自適應三維空間路徑規(guī)劃方法；申請?zhí)枮?01310744400.9的專利文件中，公開了一種基于布谷鳥搜索算法的水下潛器三維路徑規(guī)劃方法；申請?zhí)枮?01310235123.9的專利文件中，公開了一種基于高程圖和蟻群尋食的三維路徑規(guī)劃方法。這些方法都在三維路徑規(guī)劃問題的求解上有較強的智能性和適應性，然而在大范圍路徑搜索上依然存在計算復雜度大、盲目性大等問題。

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

本發(fā)明的目的在于提供一種能降低計算復雜度，提高三維路徑規(guī)劃的快速性和可靠性的基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法。

本發(fā)明的目的是這樣實現(xiàn)的：

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

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

步驟3.以z＝z_safemin水平面為Lazy Theta星算法二維路徑規(guī)劃平面，以路徑起點S的X、Y軸坐標(x_s,y_s)和路徑終點D的X、Y軸坐標(x_d,y_d)為二維路徑的起點和終點，在水平面內進行二維路徑規(guī)劃；

步驟4.根據(jù)三維路徑規(guī)劃尋找一條無碰長度最短路徑的優(yōu)化目標，設計深度規(guī)劃評價函數(shù)；

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

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

本發(fā)明還可以包括：

1、步驟3具體包括：柵格地圖中的網(wǎng)格點總數(shù)為n，具體步驟如下，

步驟3.1.定義OPEN和CLOSED兩個表；

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

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

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

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

步驟3.6.對節(jié)點P_i和其父節(jié)點進行障礙物檢測，若P_i和連線之間沒有障礙物，保留為P_i的父節(jié)點，否則，更新節(jié)點P_i的適應值和父節(jié)點；

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

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

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

2、所述擴展P_i的東、西、南、北、東南、東北、西南、西北八個方向地理位置相鄰的子節(jié)點P_j，具體包括以下幾種情況：

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

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

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

3、所述利用粒子群算法進行深度規(guī)劃的具體步驟為：

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

步驟5.2.隨機產生一組初始解；

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

步驟5.4.計算當前迭代中粒子群的慣性權重和學習因子；

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

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

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

本發(fā)明充分利用Lazy Theta星算法和粒子群算法的優(yōu)點，提出了一種基于Lazy Theta星和粒子群混合算法的水下潛器三維路徑規(guī)劃方法。本發(fā)明克服了傳統(tǒng)路徑規(guī)劃算法在三維路徑規(guī)劃問題的求解上適應性不強的缺陷。利用降維的思想，將三維問題轉化為一個二維平面路徑規(guī)劃和一個深度規(guī)劃，二維路徑規(guī)劃問題利用Lazy Theta星算法進行求解，得到具有二維信息的路徑節(jié)點。在得到的二維路徑節(jié)點的基礎上，利用粒子群算法對二維路徑點進行深度規(guī)劃，得到路徑點的深度信息。通過對三維問題的簡化，結合兩種不同算法的優(yōu)點，降低算法的計算復雜度，提高三維路徑規(guī)劃的快速性和可靠性。

附圖說明

圖1：本發(fā)明中基于Lazy Theta星和粒子群混合算法的三維路徑規(guī)劃的流程圖；

圖2：本發(fā)明中基于Lazy Theta星算法的二維路徑規(guī)劃流程圖；

圖3：本發(fā)明中基于粒子群算法的深度規(guī)劃流程圖。

具體實施方式

本發(fā)明先用Lazy Theta星進行二維路徑規(guī)劃，再利用粒子群算法對路徑在深度方向上進行深度規(guī)劃，得到水下潛器的三維路徑，具體包括以下幾個步驟：

步驟1.航行空間的建模

步驟1.1.航行空間的建立

在水下潛器三維路徑規(guī)劃范圍內建立全局坐標系Oxyz，以坐標為(x_s,y_s,z_s)的起點S和坐標為(x_d,y_d,z_d)的終點D建立航行空間。

步驟1.2.建立Lazy Theta星算法二維地圖

在步驟1.1建立的航行空間內，以水下潛器潛行的最小安全潛行深度z_safemin為標準作水平面，形成z＝z_safemin的水平面。以z＝z_safemin水平面上的障礙物為基準，建立障礙物柵格地圖。沿X、Y方向，即沿著經(jīng)度、緯度方向分別以10′為單位劃分，形成10′×10′的柵格區(qū)域。檢測每個網(wǎng)格的中心點的X、Y坐標位置是否處于障礙物內，處于障礙物內的網(wǎng)格值置為1，處于障礙物外的網(wǎng)格值置為0。遍歷所有的網(wǎng)格點，得到具有障礙物信息的0-1柵格地圖。

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

為了達到Lazy Theta星算法搜索一條無碰的最短二維路徑的目的，設計代價函數(shù)F(P)為：

其中，F(xiàn)(P)是適應值函數(shù)，G(P)是自起始節(jié)點S到節(jié)點P所走過的實際路徑長度，其值等于起始節(jié)點S到節(jié)點P的父節(jié)點P_parent走過的實際路徑長度加上父節(jié)點P_parent到節(jié)點P的歐式距離H(P)是路徑的啟發(fā)值，是節(jié)點P到終點D的歐式距離。二維歐式距離計算公式如下：

其中，P.x，P.y為節(jié)點P的X、Y軸坐標，P_parent.x，P_parent.y為節(jié)點P_parent的X、Y軸坐標，D.x，D.y為終點D的X、Y軸坐標。

步驟3.利用Lazy Theta星算法在水平面內進行二維路徑規(guī)劃

以z＝z_safemin水平面為Lazy Theta星算法二維路徑規(guī)劃平面，以路徑起點S的X、Y軸坐標(x_s,y_s)和路徑終點D的X、Y軸坐標(x_d,y_d)為二維路徑的起點和終點，進行路徑規(guī)劃。設柵格地圖中一共有n個網(wǎng)格點，Lazy Theta星算法二維路徑規(guī)劃流程如圖2，具體步驟如下：

步驟3.1.定義OPEN和CLOSED兩個表，OPEN表用于存放未檢測過的網(wǎng)格點，CLOSED表存放已檢測過的網(wǎng)格點。

步驟3.2.將起始點S放入OPEN表中，初始化起始點S的父節(jié)點為其自身，適應值F(S)＝0。CLOSED表初始化為空，第i個節(jié)點定義為P_i，其中i為網(wǎng)格點的索引(i<n)。

步驟3.3.判斷OPEN表是否為空，為空則搜索路徑失敗，若不為空，轉至步驟3.4。

步驟3.4.把OPEN表中表頭網(wǎng)格點P_i移入CLOSED表中。

步驟3.5.判斷節(jié)點P_i是否是目標終點D，若是，則路徑搜索成功，轉至步驟3.9，若不是，轉至步驟3.6。

步驟3.6.對節(jié)點P_i和其父節(jié)點進行障礙物檢測，等距離取節(jié)點P_i和節(jié)點連線上的q個點，定位這q個點坐標是否有在障礙內的點，q的值可根據(jù)柵格地圖分辨率大小而定，在本發(fā)明中，取q＝10。若P_i和連線之間沒有障礙物，保留為P_i的父節(jié)點，若P_i和連線之間存在障礙物，計算起始點S到節(jié)點P_i已經(jīng)擴展過的所有鄰居節(jié)點P_c(此節(jié)點在CLOSED表中)的路徑長度G(P_c)，并計算節(jié)點P_i到節(jié)點P_c的直線距離選取鄰居節(jié)點中使得值最小的節(jié)點P_c'作為節(jié)點P_i的父節(jié)點，更新節(jié)點P_i的適應值和父節(jié)點。

步驟3.7.擴展P_i的八個方向(東，西，南，北，東南，東北，西南，西北)地理位置相鄰的子節(jié)點P_j(j＜n，j≠i)，考慮以下幾種情況：

(1)如果P_j不在OPEN表或者CLOSED表中，將其存放在OPEN表中，設P_i的父節(jié)點為直接設置P_j父節(jié)點為計算節(jié)點P_j的適應值F(P_j)＝G(P_j)+H(P_j)，G(P_j)是自起始節(jié)點S到節(jié)點P_j所走過的實際路徑長度，其值等于起始節(jié)點S到節(jié)點P_j的父節(jié)點P_iparent走過的實際路徑長度加上父節(jié)點P_iparent到節(jié)點P_j的歐式距離H(P_j)是路徑的啟發(fā)值，是節(jié)點P_j到終點D的歐式距離。

(2)如果P_j在OPEN表中，重新計算在OPEN表中的適應值F(P_j)。若新適應值小于原有適應值，更新節(jié)點P_j的適應值和父節(jié)點，若新適應值大于原有適應值，保持原有適應值和父節(jié)點的設置。

(3)如果P_j已經(jīng)存在于CLODED表中或者是障礙物點，忽略此節(jié)點，返回步驟3.7。

步驟3.8.對OPEN表中各個網(wǎng)格點按照適應值的大小進行升序排序，返回3.3步驟。

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

步驟4.深度規(guī)劃評價函數(shù)的建立

Lazy Theta星算法規(guī)劃出二維路徑點后，三維路徑規(guī)劃中路徑點的X、Y軸坐標確定。問題轉化為:路徑P(P₁,P₂,...P_k)的每個路徑點P_i(i＜k)所在X、Y方向上的坐標位置(P_i.x,P_i.y)確定(在步驟3得到)，利用粒子群算法(PSO)優(yōu)化路徑點Z軸方向上的坐標位置P_i.z，得到完整的三維路徑，k為二維路徑點的個數(shù)。

粒子群中每個粒子代表一條路徑，粒子的維數(shù)即路徑的節(jié)點數(shù)。粒子各維的值是路徑節(jié)點的深度，因此只要以航程最短為目標，搜索合適的節(jié)點深度組合就可以達到三維規(guī)劃的目的。根據(jù)三維路徑規(guī)劃尋找一條無碰長度最短路徑的優(yōu)化目標，設路徑P(P₁,P₂,...P_k)為路徑上的節(jié)點集，其中k為粒子維數(shù)，其值等于步驟3得到的路徑節(jié)點數(shù)目，節(jié)點之間直線連接即成為一條無碰路徑，評價函數(shù)f(P)設置為：

其中，ΔL_i是第i段路徑的直線歐式距離。P_i.x,P_i.y,P_i.z為第i個路徑節(jié)點的X、Y、Z軸方向上的坐標，P_i-1.x,P_i-1.y,P_i-1.z為第i-1個路徑節(jié)點的X、Y、Z軸方向上的坐標，其中i≤k。m₁和m₂分別是進入障礙物的節(jié)點數(shù)和中間節(jié)點數(shù)，M是路徑進入障礙物的懲罰值，取值為起點S與終點D歐式距離的20％～50％。

本發(fā)明的障礙物檢測中，除了檢測路徑節(jié)點是否進入障礙物外，還考慮連接節(jié)點的直線段是否進入障礙物。具體做法是，在每個節(jié)點直線段中間均勻插入若干點，插入點的數(shù)量q可以根據(jù)規(guī)劃難度而定，本發(fā)明中設置q＝10。定位路徑節(jié)點和路徑節(jié)點間的點的X、Y坐標所在點的障礙物所在深度，若路徑點的Z坐標位置在障礙物深度以下，視為路徑點進入障礙物，在障礙物深度以上，則為沒有進入障礙物。

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

粒子群深度規(guī)劃流程如圖3所示，具體步驟如下：

步驟5.1.初始化粒子群算法的各類參數(shù)，包括粒子數(shù)目n、迭代次數(shù)N、粒子維數(shù)k、更新速度。

步驟5.2.隨機產生一組初始解。

步驟5.3.根據(jù)步驟4確定的路徑評價函數(shù)計算各個粒子的適應值。更新粒子的個體最優(yōu)值和種群最優(yōu)值。

若當前迭代的粒子個體適應度優(yōu)于歷史記錄的個體最優(yōu)值，則以當前粒子的適應度作為該粒子的個體最優(yōu)值。若種群中出現(xiàn)某個體的最優(yōu)值優(yōu)于種群最優(yōu)值，則以此個體的最優(yōu)值作為種群最優(yōu)值。

步驟5.4.計算當前迭代中粒子群的慣性權重和學習因子

(1)慣性權重ω的計算方法

慣性權重ω代表了粒子慣性的大小，ω較大時具有較強的全局收斂能力，ω較小時具有較強的局部搜索能力。在實際問題中，一般希望算法的初始階段有較強的全局搜索能力，到算法的后期則希望算法有較強的局部搜索能力。因此，慣性權重ω隨迭代次數(shù)的增加設計成：

式中，ω_max為最大慣性權重，取值范圍為0.75～1.05，本發(fā)明中取值為0.9，ω_min為最小慣性權重系數(shù)，取值范圍為0.3～0.55，本發(fā)明取值為0.4，i為當前迭代次數(shù)，N為最大迭代次數(shù)。

(2)學習因子的計算方法

兩個學習因子中，c₁越大粒子種群多樣性越好。c₂越大則粒子的收斂速度越快。為了使算法不要過早收斂，提高種群的多樣性，在優(yōu)化初期取較大的c₁使得種群多樣性豐富，在算法的后期提高c₂加快算法收斂。根據(jù)這兩個規(guī)則設計c₁、c₂隨迭代次數(shù)變化的計算公式為:

其中，c_1f,c_1c,c_2f,c_2c為常數(shù)，c_1f,c_2c取值范圍為0.3～0.7，c_1c,c_2f取值范圍為2.2～2.8，本發(fā)明取c_1f＝0.5，c_1c＝2.5，c_2f＝2.5，c_2c＝0.5，i為當前迭代次數(shù)，N為最大迭代次數(shù)。

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

設粒子群的種群粒子數(shù)目為n，k維空間中搜索最優(yōu)解，k的值等于步驟3得到的路徑節(jié)點數(shù)目。第j個粒子位置記為P_j＝(P_j1,P_j2,...,P_jk),j＝1,2,...,n，粒子速度記為V_j＝(V_j1,V_j2,...,V_jk),(j＝1,2,...,n)。第j個粒子至今搜索到的個體最優(yōu)解記為P_jbest，群體至今搜索到的最優(yōu)解記為P_gbest。第j個粒子的位置和速度根據(jù)以下公式進行更新：

V′_j＝ωV_j+c₁rand()(P_jbest-P_j)+c₂rand()(P_gbest-P_j)

P′_j＝P_j+P′_j

式中，V_j'是第j個粒子更新后的運動速度，P_j'是第j個粒子更新后的粒子位置，在本發(fā)明中為粒子各路徑點的深度信息。ω是慣性權重系數(shù)，c₁,c₂是學習因子，ω，c₁,c₂的值由步驟5.4求得。c₁,c₂調節(jié)的是粒子學習種群最優(yōu)解和個體最優(yōu)解的程度。rand()為在0到1之間取的隨機數(shù)。

步驟5.6.重復步驟5.3～步驟5.5直到迭代次數(shù)達到N。

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

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