[發明專利]動態障礙物下的無人巡航船多目標路徑規劃方法

有效

• 申請號: 202010340555.6
• 申請日: 2020-04-26
• 公開（公告）號: CN111457927B
• 公開（公告）日: 2021-03-05
• 發明（設計）人: 于家斌;劉冠東;王小藝;許繼平;趙峙堯;王立;白玉廷;孫茜;張慧妍
• 申請（專利權）人: 北京工商大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京永創新實專利事務所 11121
• 代理人: 冀學軍

1.動態障礙物下的無人巡航船多目標路徑規劃方法，其特征在于，具體步驟如下：

步驟一：無人機對湖面的環境采集圖像，采用柵格法進行分割，得到柵格地圖并設置起始點和d個采樣點；

具體為：首先，將湖面圖像分割成大小相等的柵格，組成一個柵格地圖；并將環境中的障礙物設置為黑色柵格，將不規則障礙物進行填充處理，將可行區域設置為白色柵格，在柵格地圖中運用二維坐標設置一個起始點和d個采樣點；

d為正整數；

步驟二：采用改進的灰狼優化算法對d個采樣點進行順序優化，得到最優順序并保存；

所述采用改進的灰狼優化算法得到最優順序的步驟如下：

步驟201：初始化灰狼優化算法的參數；

所述的參數包括：

灰狼種群數量n，最大迭代次數t，將采樣點個數定義為搜索空間的維數d，灰狼種群的初始位置，以及每只灰狼各自一條隨機路徑；

步驟202：每只灰狼在d個采樣點的空間中搜索獵物，構建搜索空間與灰狼位置關系的空間域矩陣P；

首先，定義第i只灰狼的位置X_i為一組互不相同的正整數序列，公式如下：

則代表灰狼種群中的第i只灰狼在第d個采樣點上的值；

然后，n只灰狼的位置組成空間域矩陣P，公式如下：

式中，表示第i只灰狼在第m個采樣點的位置，空間域矩陣P中每一行代表一只狼的路徑；

步驟203：構建距離矩陣W來表示每個采樣點與其它采樣點之間的距離；

公式如下：

s_(n,m)為第n個采樣點與第m個采樣點之間的距離；

步驟204：利用空間域矩陣P和距離矩陣W構建適應度函數f；

表達式為：

P_i是指空間域矩陣P中的第i行只狼的路徑；∑s_i(n,m)是指第i行只狼的路徑中，對每兩個采樣點之間的距離相加求和；

步驟205：運用灰狼優化算法求解適應度函數，得到最優的遍歷順序；

具體過程如下：

首先是搜尋過程：

在t代搜索過程中，灰狼的位置為X(t)，獵物的位置為X_P(t)，灰狼與獵物的距離D表示為：

t表示當前迭代次數，r₂為從0到1的隨機數；C是一個隨r₂變化的系數；

然后是包圍過程：

在灰狼群體包圍獵物的過程中，構造灰狼與獵物的關系模型，公式如下：

式中，A·D為包圍步長，a為改進后的收斂因子，r₁為從0到1的隨機數，t_max表示最大迭代次數，μ₁和μ₂為常數，λ₁和λ₂為調節系數，B為非線性的連續函數Beta函數；

改進后的收斂因子a的曲線會隨著迭代次數的增加而非線性減少，在剛開始迭代時，收斂因子a會非線性的迅速下降，而隨著迭代次數的增加，收斂因子a的下降曲線近似于線性；

緊接著，位置更新過程：

根據灰狼與獵物的關系模型，分別得到α，β和δ這三類狼的最終位置X₁，X₂和X₃，在α，β和δ的指引下，整個狼群逐步向獵物靠近，通過α，β和δ狼的位置更新，不斷確定目標的位置，從而實現灰狼對獵物最終的攻擊位置；

式中，X_α，X_β，X_δ表示α，β和δ這三類狼的更新位置，A₁，A₂，A₃表示三個隨機數，X則表示灰狼對獵物最終的攻擊位置；A₁·D_α，A₂·D_β，A₃·D_δ表示α，β和δ這三類狼的包圍步長；

最后判斷是否達到最大迭代次數，如果是，則輸出迭代過程中最優順序；否則，依次保存適應度函數最小時得到的三個最優解，再次對α，β和δ這三類狼的位置進行更新；

步驟三：按照灰狼算法得到的最優順序，將各采樣點一一標記到柵格地圖上；

步驟四：利用D*Lite算法計算柵格地圖中標記的每兩個采樣點之間的最優柵格路徑，得到一條從起始點到最終采樣點之間的最優路徑；

具體過程為：

步驟401：初始化各柵格點的路徑代價估計值rhs(s)值、g(s)值和k_m值；

根據獲取的湖面二維坐標圖，以及由灰狼優化算法計算出的最優順序，設定D*Lite算法的相關參數，其中，每個柵格點分別作為一個節點；S為地形圖中的路徑節點的集合，s屬于集合S；

步驟402：從柵格地圖的當前節點s_goal開始，自當前路徑點向周圍8個方向鄰格擴展，通過比較各擴展相鄰節點的估值函數k(s)，選取k(s)值最小點為下一個擴展節點；

當前節點s_goal初始值為第一個采樣點；由于水面里大中型生物的移動，水面高度的變化導致巖石的出現，在地圖中設置動態障礙物，設水面出現障礙物的概率和障礙物消失的概率都為3％，即每個空白柵格變成障礙物柵格和障礙物柵格變為空白柵格的概率為3％；在探測的過程中，巡航船采用激光探測儀，探測距離為5格*5格；

其中，估值函數k(s)包括兩個值：k₁(s)和k₂(s)，優先比較k₁(s)大小，當k₁(s)值相等時，比較k₂(s)大小，公式如下：

k₁(s)＝min(g(s),rhs(s))+h(s_start,S)+k_m

k₂(s)＝min(g(s),rhs(s))

式中，h(s_start,S)為啟發函數，表示起始點到當前節點的路徑代價；k_m為環境變化時更新的第m次變量；

步驟403：將下一個擴展節點作為當前節點，繼續擴展相鄰節點，直至達到節點s_start，得到一條從節點s_goal到節點s_start之間的路徑；

節點s_start初始值為柵格地圖的起始點；

步驟404：針對當前節點s_start，計算該節點所有后繼節點的rhs(s)值，選取rhs(s)最小的節點作為新起始點s'_start，巡航船移動到該點s'_start；

步驟405：掃描地圖，更新節點信息，判斷周圍環境是否發生變化，如果沒有發生變化，進入步驟406；否則，進入步驟407；

步驟406、將巡航船移動到的新節點s'_start作為當前節點，再次返回步驟402，得到從節點s_goal到該節點之間的路徑，并繼續移動巡航船的位置，直至巡航船到達節點s_goal，進入步驟408；

步驟407、更新估值函數k(s)中的變量k_m值和節點s'_start，返回步驟402；

表達式如下：

k_m＝k_m-1+h(s_last,s'_start)

s_last＝s'_start

式中，k_m-1為環境變化時更新的第m-1次變量，其中k₀＝0；s_last表示巡航船前一個位置節點，h(s_last,s'_start)表示巡航船前一個位置點到當前節點s'_start的路徑代價；

如果航船移動過程中，周圍環境沒有發生變化，則航船移動的路徑與從節點s_goal到節點s_start之間擴展的路徑吻合；

當周圍環境發生變化后，估值函數的值會發生改變，各節點的擴展節點也會發生改變，從而節點s_goal到節點s_start之間擴展的路徑也會發生改變，巡航船的采樣路徑也會動態變化；

步驟408：判斷節點s_goal是否為最終采樣點，如果是，終止算法，得到一條遍歷多個采樣點的最優路徑；否則，將該節點s_goal設置為新起始點s'_start，將下一次采樣點設置為目標點s_goal，返回步驟402；

步驟五：自主巡航船從柵格地圖對應的起始點開始巡航，沿著最優柵格路徑依次經過各采樣點，最終返回起始點完成巡航。