[發明專利]雙重改進A星最短航路規劃方法

有效

• 申請號: 202010552031.3
• 申請日: 2020-06-17
• 公開（公告）號: CN111561933B
• 公開（公告）日: 2023-03-31
• 發明（設計）人: 王晶;劉洋;牛元龍;周利軍
• 申請（專利權）人: 西安愛生技術集團有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 西安凱多思知識產權代理事務所(普通合伙) 61290
• 代理人: 劉新瓊

1.一種雙重改進A星最短航路規劃方法，其特征在于包括下述步驟：

步驟1：計算最小安全距離：將飛機轉彎半徑和飛機大小看成質點，所需要的最小安全距離L_s為最大速度的最小轉彎半徑R_min與飛機翼展L_A之和，即：

L_s＝R_min+L_A

步驟2：安全區和禁飛區重生成：為了將飛機和飛機運動特性看成質點，將對禁飛區區域S_f按照最小安全距離L_s擴大得到新禁飛區S_fL，安全區區域S_s按照最小安全距離L_s縮小得到新安全區S_sL；

所述擴大的方法分為兩種情況：

1)當禁飛區區域為凸多邊形，將每個頂點轉換為直角坐標(P_xi,P_yi)，求凸多邊形的中心點(P_x,P_y)，計算凸多邊形的每個頂點i到中心點的距離L_i和凸多邊形的每個頂點和中心點的連線與正北方向的夾角形成的角度A_i，在距離L_i加上最小安全距離L_s作為新距離L_is，L_is＝L_i+L_s，通過新距離L_is和初始角度A_i，計算每個新頂點位置(P_nxi,P_nyi)，將每個新頂點位置(P_nxi,P_nyi)轉換為地理坐標并依次連接，即可完成了區域擴大；

n為凸多邊形總個數；

P_nxi＝P_x+sin(A_i)*L_is

P_nyi＝P_y+cos(A_i)*L_is

2)當禁飛區區域為凹多邊形，先將凹多邊形拆分成多個凸多邊形，再按照凸多邊形擴大的步驟完成每個凸多邊形的區域擴大，多個被擴大后的凸多邊形的構成區域的并集，形成擴大后的凹多邊形；

所述縮小的方法分為兩種情況：

1)當安全區區域為凸多邊形，將每個頂點轉換為直角坐標(P_xi,P_yi),求凸多邊形的中心點(P_x,P_y)，計算凸多邊形的每個頂點i到中心點的距離L_i和凸多邊形的每個頂點和中心點的連線與正北方向的夾角形成的角度A_i，在距離L_i減去最小安全距離L_s作為新距離L_is，L_is＝L_i-L_s，通過新距離L_is和初始角度A_i，計算每個新頂點位置(P_nxi,P_nyi)，將每個新頂點位置(P_nxi,P_nyi)轉換為地理坐標并依次連接，即可完成區域縮小；

n為凸多邊形總個數

P_nxi＝P_x+sin(A_i)*L_is

P_nyi＝P_y+cos(A_i)*L_is

2)當安全區區域為凹多邊形，先將凹多邊形拆分成多個凸多邊形，再按照凸多邊形縮小的步驟完成每個凸多邊形的區域縮小，多個被縮小后的凸多邊形的構成區域的并集，形成縮小后的凹多邊形；

步驟3：確定A星算法搜索方向數目N_d和搜索步長D_s：

A星搜索方向數目N_d為4-360，每個方向間隔45度，搜索步長D_s取最小轉彎半徑R_min；

步驟4：確定重復點的判斷依據：

歷史搜索節點node_i包括自身位置，父節點指針，花費代價，歷史代價，估計代價；

新加入節點node_n包括自身位置，父節點指針，花費代價，歷史代價，估計代價；

依次通過歷史搜索過的節點node_i和新節點node_n的自身位置，計算節點node_i和新節點node_n的之間距離D_(i,n)，當距離D_(i,n)小于步長D_s的一半，即D_(i,n)D_s/2，則認為新節點node_n和歷史節點node_i為重復點，將不再作為新節點參與計算；

步驟5：確定禁飛區相交依據：新點node_n在新禁飛區S_fL內，或者新點node_n與新點的父節點node_p的連線與新禁飛區S_fL存在交點，兩個條件滿足其一，則認為與禁飛區相交；

步驟6：確定安全區相交依據：新點node_n在新安全區S_sL外，或者新點node_n與新點的父節點node_p的連線和新安全區S_sL存在交點；

步驟7：確定有效搜索點判斷依據：根據步驟4判斷不是重復點，步驟5判斷不與禁飛區相交和步驟6判斷不與安全區相交；當三個條件都滿足時，認為當前搜索點為有效點；

步驟8：確定A星算法花費代價F_(i)：

花費代價F_(i)等于從起始點到當前有效搜索點位置的歷史代價G_(i)與該位置到目標位置的估計代價H_(i)之和，即：

F_(i)＝G_(i)+H_(i)；

步驟9：父節點node_p生成：

開始指定起始點為第一個父節點node_p，通過父節點node_p的位置(X_p,Y_p)和當前節點需搜索方向角度D_n，計算新節點node_n位置(X_n,Y_n)，通過步驟7的有效搜索點的判斷依據，判斷新節點node_n是否為有效搜索節點，如果不是有效搜索節點，則改變搜索方向角度，并重新計算；否則是有效搜索節點，將有效搜索新節點node_n加入有效數隊列openlist中，并根據步驟8計算每個有效搜索點node_n的花費代價F_(n)值，查找openlist中所有搜索點，將F_(n)最小的節點返回，當做新的父節點node_p，并從openlist中移除F_(n)最小的節點，將新的父節點node_p加入已搜索隊列closelist中；

新節點node_n位置(X_n,Y_n)的計算公式為：

X_n＝X_p+sin(D_n)*D_s

Y_n＝Y_p+cos(D_n)*D_s

步驟10：新節點node_n生成：

根據父節點node_p位置(X_p,Y_p)、當前節點的搜索方向角度D_n和搜索步長D_s，計算出新節點node_n的位置(X_n,Y_n)，將新節點node_n的父節點指針設置為node_p，方便后期逆向查找；根據步驟8分別計算新節點node_n的花費代價F_(n)，歷史代價G_(n)，估計代價H_(n)；

其中，D_n＝D₀+360/N_d*N_i；

D_n為當前節點的搜索方向角度，即本次新節點和父節點的連線和正北方向的夾角為搜索方向角度；

D₀為本次目標點相對父節點的初始角度；

N_i為當前方向數目；

G_(n)＝G_(p)+C_(n)

G_(n)為新節點歷史代價，取從起始點到新節點總距離；

G_(p)為父節點歷史代價，取從起始點到父節點總距離；

C_(n)從父節點到新節點的花費代價，取父節點到新節點的距離；

F_(n)＝G_(n)+H_(n)

H_(n)為新節點估計代價，取從新節點直接到目標點的距離；

步驟11：確定搜索結束依據：

當目標點變成為有效搜索節點時，則規劃成功，進入步驟12；如果目標點不是有效搜索節點時，進入步驟9，重新搜尋父節點，并改變新父節點，添加新目標點；

當有效隊列openlist中所有有效節點都被移除，搜尋父節點失敗，則規劃失敗，結束本次航線段規劃，并返回空航路；

步驟12：規劃航線初始生成：

當規劃成功后，對終止時父節點node_p反向尋找生成該節點的父節點，根據當前節點存儲的父節點的指針尋找父節點，一直尋找直到父節點為起始節點時，將尋找出來的父節點從起始位置開始依次連接起來，構成的節點集合為生成的初始規劃航路；

步驟13：A星重構優化航路：按照初始規劃航線的節點集合，作為A星搜索的新點集；

步驟14：歷史代價重新計算G_(n)，父節點重新指定：

按照順序取節點node_p，當作父節點，依次取出節點node_p后面的節點node_b當作新節點node_n，根據步驟7判斷新節點node_n的有效性，當node_n為無效節點時，跳過本次節點，取出下個節點node_n進行判斷，當node_n為有效節點時，重新計算父節點node_p直接擴展到node_n的新歷史代價G_(n)，如果新歷史代價G_(n)小于node_n中原始代價G_(n)，則改變節點node_n中原始代價G_(n)為新歷史代價G_(n)，并更新節點node_n中父節點指針為節點node_p，否則node_n中任何值不改變；

G_(n)＝G_(p)+C_(n)G_(n)為新節點歷史代價，取從起始點到新節點總距離；

G_(p)為父節點歷史代價，取從起始點到父節點總距離；

C_(n)從父節點到新節點的花費代價，取父節點到新節點的距離；

步驟15：航路規劃完成：通過對終止時父節點node_p反向尋找生成該節點的父節點，一直尋找直到父節點為起始節點時，將尋找出來的父節點，從起始位置開始依次連接，連接起來構成的位置集合為生成的最短規劃航路。

2.根據權利要求1所述的一種雙重改進A星最短航路規劃方法，其特征在于：

所述步驟3中，A星搜索方向取8個方向。