[發明專利]一種分級模糊-人工勢場路徑規劃的方法

有效

• 申請號: 202010538510.X
• 申請日: 2020-06-13
• 公開（公告）號: CN111610788B
• 公開（公告）日: 2023-04-18
• 發明（設計）人: 王寧;黎承忠;徐宏威;樊宇;陳浩華;陳廷凱
• 申請（專利權）人: 大連海事大學
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01C21/20
• 代理公司: 大連東方專利代理有限責任公司 21212
• 代理人: 姜玉蓉;李洪福

1.一種分級模糊-人工勢場路徑規劃的方法，其特征在于，包括如下步驟：

S1、環境建模；

S2、執行改進的精英遺傳算法，規劃無人船全局路徑；所述步驟S2中執行改進的精英遺傳算法，規劃無人船全局路徑的方法包括如下子步驟：

S21、計算個體的自適應度；所述步驟S21中的計算個體的自適應度的方法包括：

采用包含B樣條的適應度函數，適應度函數設計為：

其中，l,Δ是常量且Δ＞0；是B樣條中基函數的控制點，是第i次迭代中的第n個航路點；

L_p是路徑長度：

l_p是某個路徑點與障礙物的最近距離，l_p通過歐式距離表示為：

其中，是距離路徑點的最近障礙物位置；分別表示第n個障礙物的橫、縱坐標；

其中，表示正整數集，表示第i次迭代中路徑點的集合，p_d,i(u)＝[x(u),y(u)]^T,表示實數集，且由節點向量分割而成；表示第N_w+k+1個節點，Ω_θ是一個不遞減的節點序列；N_i,k(u)是被Boor-Cox遞歸函數定義的i次k階的B樣條基函數，其表達式如下：

其中，表示不遞減的節點序列，且

S22、采用十進制式的編碼方式來簡化通過遺傳算法進行全局路徑規劃的復雜度；

S23、執行精英保留操作；所述步驟S23中的執行精英保留操作的方法包括：

通過路徑長度公式對所有個體進行適應度計算，并按照0＜P_e:＝n_e/N_p＜1的精英保留率，將所有個體按照適應度排序并進行精英保留，其中，P_e:表示精英保留率，n_e表示保留的精英個體數目，N_p表示個體總數目；

S24、執行多樣化增量操作；所述步驟S24中的執行多樣化增量操作的方法包括：

采取0＜P_d:＝n_d/N_p＜1的多樣化增量比率，從可行區域中隨機生成n_d·N_w個候選航路點有序的組合成n_d個個體，其中，P_d:表示多樣化增量比率，n_d表示增加的個體數目；

S25、執行選擇、交叉、自適應變異操作；所述步驟S25中的執行選擇、交叉、自適應變異操作的方法包括：

S251、執行選擇操作，即

通過輪盤概率選擇的方式將適應度值高的個體進行保留，適應度值高的個體有更高的選擇概率進入下一代，因此，種群中的第i個體被選擇的概率為：

其中，ε(p_d,i)按照適應度函數的公式進行計算；

S252、執行交叉操作，即

對不同個體之間染色體進行基因重組，即生成新的染色體的過程，交叉操作表示為：

a_qj＝a_qj(1-b)a_ljb

a_lj＝a_lj(1-b)a_kjb

其中，a_qj,a_lj分別表示第q個和第l個染色體中第j個基因的值；b∈[0,1]的隨機數；

S253、執行自適應變異操作，即

采用自適應變異操作，通過對個體的適應值來決定個體的變異概率：

其中，P_m,max和P_m,min分別表示個體最大和最小的變異概率；ε(p_d,max)和ε(p_d,avg)分別表示群體中最大和平均適應值；

S3、執行具有模糊決策的模糊-人工勢場算法，規劃無人船局部路徑；所述步驟S3中執行的具有模糊決策的模糊-人工勢場算法，其函數表達式如下：

其中，ζ是一個自修正模糊因子，ρ₀為障礙物斥力勢場作用范圍的最大半徑；其中為斥力修正系數，隨著斥力的兩個分量的作用，障礙物對無人船的斥力逐漸減小，目標點對無人船的引力逐漸增大，故λ∈(0,1)，k_r和k_a是推力和引力系數，ρ^λ(p_x,p_E)表示USV到目標點距離的λ次方，ρ(p_b,p_E)和ρ(p_b,p_obs)分別是障礙物和目標點到當前位置的距離，即：

ρ(p_x,p_*)＝||p_x,p_*||₂

為了保證無人船能夠安全完成局部避碰，并快速回到預設的全局航線，以下從三個角度分析：

1)當障礙物不在全局路徑上

當無人船處于障礙物斥力勢場作用范圍內時，修正斥力勢場分別對ρ(p_x,p_obs)和ρ^λ(p_x,p_E)求導，得到修正斥力的兩個分量和其表達形式為：

其中，ρ(p_x,p_obs)為障礙物到USV的距離，ρ(p_x,p_E)是目標點到USV的距離，矢量分量的方向為障礙物指向無人船，矢量分量的方向為無人船運動方向與安全區域d_s的切線方向；

引力分量就是其勢場的負梯度，其表達形式如下：

由此可得，合力F_h為斥力與引力F_a的和：

2)當障礙物在全局路徑上

通過斥力分量和斥力分量的修正作用，改變勢場作用力的合力F_h的方向，使USV能夠成功繞開障礙物，完成局部路徑規劃，此時USV與障礙物之間的距離逐漸減小，自校正模糊因子ζ會增加，增大障礙物的排斥力，并保證USV的航行安全；

3)避碰后快速返回全局路徑

當無人船安全范圍d_s內檢測無障礙物時，證明已完成動態避碰，此時將障礙物與目標點的勢場關閉，引入航向決策角γ，并通過設置引力點Q和K使無人船快速返回預設置的全局路徑，具體的：

從無人船當前位置向全局路徑做一條夾角為γ的射線，交全局路徑于點Q，并在Q前方距離η處的全局路徑上取點K，此時分別在Q點和K點處引入引力勢場函數，其定義為：

式中，k_a為引力勢場增益因子，Ψ∈{Q,K}，將引力場負梯度處理，得出引力F_t(Ψ)為：

由于吸引力與USV與參考點Ψ之間的距離成正比，因此可以保證USV安全，平穩地返回全局路徑，由于USV與K點之間的距離始終大于Q點，此外，合力的方向始終偏向K點并指向全局路徑，因此USV可以通過Q點和K點之間的某個點，以便可以限制返回角度符合USV的運動特性；

S4、在所述無人船全局路徑上插入虛擬返回點，將所述無人船全局路徑和無人船局部路徑進行融合。

2.根據權利要求1所述的分級模糊-人工勢場路徑規劃的方法，其特征在于，所述步驟S1環境建模，即對無人船的檢測區域進行建模，通過定義無人船的檢測區域，檢測障礙物的位置，使無人船的路徑遠離障礙物；具體的：

S11、定義無人船的檢測區域，如下：

其中，d_s為無人船的檢測半徑，p_b＝[x_b,y_b]^T是無人船所處位置，p(t)＝[x,y]^T為環境中被檢測點的位置；x_b,y_b分別表示當前無人船的橫、縱坐標，x,y分別表示被檢測點的橫、縱坐標；

S12、設定航行區域：其中，為可行區域，為不可行區域；為無人船的檢測區域，為航行區域；

S13、在所述設定航行區域中獲取一條從起點p_S＝[x_s,y_s]^T到終點p_E＝[x_E,y_E]^T的無碰撞路徑；x_S,y_S分別表示起點的橫、縱坐標，x_E,y_E分別表示橫、縱坐標；

S14、利用公式h(p)＝0.5R(p)+0.5G(p)+0B(p)對原始的彩色地圖進行柵格二值化的方式來區分禁止通行的區域和自由行駛的區域，從而得到相應的二值化地圖

其中，R(p)、G(p)和B(p)分別為p點對應的紅、綠、藍三色數值，H(p)是周圍10個像素內灰度值的平均值，g(p_s)＝0代表該區域為可行區域，g(p_s)＝255代表該區域為不可行區域；最終得到可行區域和不可行區域