本發(fā)明涉及一種無人機(jī)路徑動態(tài)規(guī)劃方法，特別涉及一種基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法。

背景技術(shù)：

無人機(jī)路徑規(guī)劃是在規(guī)劃區(qū)域內(nèi)，在給定的約束條件下為無人機(jī)尋找出一條從起點到終點的最優(yōu)或可行的路徑。無人機(jī)飛行過程中當(dāng)突然出現(xiàn)危險/威脅需要規(guī)避或發(fā)生任務(wù)變更情形時，需要動態(tài)生成一條安全可行的路徑，路徑動態(tài)規(guī)劃問題本質(zhì)上是一個約束眾多的多目標(biāo)優(yōu)化問題。

文獻(xiàn)“基于Laguerre圖的自優(yōu)化A*無人機(jī)航路規(guī)劃算法，系統(tǒng)工程與電子技術(shù)，2015,Vol.37(3),p577-582”公開了一種LA-Star算法用于無人機(jī)航路規(guī)劃。該算法首先將多邊形威脅區(qū)域和多邊形禁飛區(qū)采用“外切圓法”簡化為二維平面的一組圓；然后采用Laguerre圖算法進(jìn)行航路預(yù)規(guī)劃，在此基礎(chǔ)上簡化二次規(guī)劃空間的范圍，該簡化的規(guī)劃空間區(qū)域的頂點由預(yù)規(guī)劃航路點組成；之后恢復(fù)威脅區(qū)域和禁飛區(qū)的真實形狀并使用A*算法在簡化的規(guī)劃空間區(qū)域內(nèi)實施二次航路規(guī)劃，最后對生成的航路進(jìn)行自優(yōu)化處理。

針對復(fù)雜環(huán)境下存在長條形狀的多邊形威脅區(qū)域或禁飛區(qū)情形，上述算法用圓形替代多邊形威脅區(qū)域或禁飛區(qū)與實際形狀會存在較大差異，用圓形替代威脅區(qū)域生成的預(yù)規(guī)劃航路點構(gòu)成簡化的二次規(guī)劃空間搜索區(qū)域，在此搜索空間基于A*算法進(jìn)行二次航路規(guī)劃，該算法二次規(guī)劃時恢復(fù)威脅區(qū)域和禁飛區(qū)的真實形狀，也無法消除一次規(guī)劃用圓形替代長條形狀的多邊形威脅區(qū)域或禁飛區(qū)帶來的顯著差異，上述算法不適應(yīng)于存在長條多邊形威脅區(qū)或禁飛區(qū)復(fù)雜環(huán)境下的路徑規(guī)劃。

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

為了克服現(xiàn)有方法對復(fù)雜環(huán)境中存在長條形狀的多邊形威脅區(qū)域或禁飛區(qū)情形適應(yīng)性差的不足，本發(fā)明提供一種基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法。該方法首先構(gòu)造多邊形威脅區(qū)域或禁飛區(qū)的外包圓，判斷起始點和目標(biāo)點構(gòu)成的線段與各多邊形外包圓是否相交。如果該線段與外包圓有相交則進(jìn)一步判斷該線段與多邊形威脅區(qū)域或禁飛區(qū)是否有交點，如果有交點，則基于A*搜索算法規(guī)劃生成直接規(guī)避多邊形威脅區(qū)域或禁飛區(qū)的路徑，在該路徑規(guī)劃過程中采用了死區(qū)逃離以及兩步尋優(yōu)的策略逐次規(guī)劃生成各路徑點。本發(fā)明能夠適應(yīng)于復(fù)雜環(huán)境中存在長條形狀的多邊形威脅區(qū)或禁飛區(qū)情形，同時還能夠適應(yīng)存在圓形或者多邊形威脅區(qū)域或禁飛區(qū)相互重疊的情形。

本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案：一種基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法，其特點是包括以下步驟：

步驟1：無人機(jī)動態(tài)路徑規(guī)劃初始數(shù)據(jù)獲取與算法參數(shù)設(shè)置。

1)獲取初始數(shù)據(jù)，包括獲取無人機(jī)當(dāng)前動態(tài)路徑規(guī)劃的起始點和目標(biāo)點坐標(biāo)以及威脅區(qū)域的參數(shù)數(shù)據(jù)。

①獲取無人機(jī)初始位置坐標(biāo)WP₁(x_S,y_S)和目標(biāo)位置坐標(biāo)WP_E(x_G,y_G)。

②威脅區(qū)域的參數(shù)數(shù)據(jù)包括：建模為圓形的威脅區(qū)域個數(shù)M，每個圓形威脅區(qū)域C_i的圓心和半徑(x_i,y_i,r_i)，其中i＝1,2,…,M；建模為多邊形的威脅區(qū)域個數(shù)N,各個多邊形威脅區(qū)域頂點個數(shù)(n₁,n₂,…,n_N)，每個多邊形威脅區(qū)域P_j頂點坐標(biāo)集其中j＝1,2,…,N。

2)設(shè)置算法參數(shù)。

①圓形威脅區(qū)域外延安全間距d₁，多邊形威脅區(qū)域外延安全間距d₂。圓形威脅區(qū)域的外延距離，也即在威脅圓(x_i,y_i,r_i)的基礎(chǔ)上生成(x_i,y_i,(r_i+d₁))的輔助圓，進(jìn)而在圓(x_i,y_i,(r_i+d₁))上生成具有緩沖安全距離的點；多邊形威脅區(qū)域的外延距離d₂，也即在某個多邊形的頂點處生成距離該點為d₂的點以備后續(xù)算法使用。外延安全間距的大小依據(jù)飛機(jī)的機(jī)動性能、飛行速度條件約束加以選擇設(shè)置。

②設(shè)置代價函數(shù)權(quán)值參數(shù)。

搜索算法代價函數(shù)為f(i)＝w₁·g(i)+w₂·h(i)。

其中，f(i)為節(jié)點i的代價函數(shù)，g(i)為子起始點到預(yù)生成的兩步航點總的距離代價，h(i)為預(yù)生成的第二步航點位置到目標(biāo)點距離的代價。為了得到全局較優(yōu)的路徑，使w₁<w₂，選取w₁＝0.3，w₂＝0.7。

步驟2：分別求各個多邊形威脅區(qū)域?qū)?yīng)的外包圓。

多邊形的外包圓也即包含該多邊形所有頂點的最小圓。一個多邊形外包圓的求取過程如下：

1)計算該多邊形所有頂點兩兩之間的距離，取距離最大值；

2)以該距離最大值線段的中點作為圓心；

3)以該圓心到該多邊形各個頂點距離的最大值為半徑；

4)以步驟2.2)確定的圓心和步驟2.3)確定的半徑形成該多邊形的外包圓。

步驟3：連接當(dāng)前子起始點WP_i1(x_i1,y_i1)和子目標(biāo)點GP_j1(x_j1,y_j1)構(gòu)成線段L_i1,j1，判斷該線段是否與各個威脅區(qū)域有交點。如果該線段和威脅區(qū)域無交點，置C_Flag為0且P_Flag為0，如果子目標(biāo)點GP_j1(x_j1,y_j1)為最終目標(biāo)點，則轉(zhuǎn)入步驟11。如果該線段和威脅區(qū)域有交點，求取距離子起始點WP_i1最近的交點CP_k1，標(biāo)識交點CP_k1所對應(yīng)的威脅區(qū)域，如果交點CP_k1落在多邊形的邊上，將交點所在的多邊形對應(yīng)邊的兩個頂點進(jìn)行標(biāo)識。具體步驟如下：

1)將線段L_i1,j1分別與各個圓形威脅區(qū)域進(jìn)行是否有交點的判斷，

①如果L_i1,j1只與所有圓形威脅區(qū)域中的一個威脅區(qū)域有交點，計算交點與子起始點WP_i1之間的距離，求最短距離d_ci1，將該威脅區(qū)域標(biāo)識為C_i,置C_Flag為1；

②如果L_i1,j1和多個圓形威脅區(qū)域有交點，計算各交點與子起始點WP_i1之間的距離，求最短距離d_ci1，并將最短距離對應(yīng)交點所在的威脅區(qū)域標(biāo)識為C_i,置C_Flag為1；

③如果L_i1,j1和所有圓形威脅區(qū)域均無交點，置C_Flag為0。

2)將線段L_i1,j1和所有的多邊形威脅區(qū)域進(jìn)行是否有交點的判斷，

①判斷線段L_i1,j1與所有多邊形的外包圓是否有交點，如果該線段和所有多邊形的外包圓都沒有交點，置P_Flag為0；

②如果線段L_i1,j1與外包圓有交點，進(jìn)一步判斷如下：

a)如果L_i1,j1與一個外包圓有交點，進(jìn)一步判斷線段L_i1,j1與該外包圓對應(yīng)的多邊形威脅區(qū)域是否相交。

如果線段L_i1,j1與該外包圓對應(yīng)的多邊形威脅區(qū)域沒有相交，則置P_Flag為0；否則，計算交點與子起始點WP_i1之間的距離，求最短距離d_Pi1，將該多邊形威脅區(qū)域標(biāo)識為P_j，同時將該最短距離對應(yīng)交點所在的多邊形邊的兩個頂點分別標(biāo)識為置P_Flag為1。

b)如果L_i1,j1與多個外包圓有交點，進(jìn)一步判斷線段L_i1,j1與各個相交的外包圓對應(yīng)的多邊形威脅區(qū)域是否相交。具體如下：

如果線段L_i1,j1與所有相交的外包圓對應(yīng)的多邊形均沒有交點，置P_Flag為0；

如果線段L_i1,j1與所有相交的外包圓對應(yīng)的多邊形中的一個多邊形有交點，計算交點與子起始點WP_i1之間的距離，求最短距離d_Pi1，將該多邊形威脅區(qū)域標(biāo)識為P_j，同時將該最短距離對應(yīng)交點所在的多邊形邊的兩個頂點分別標(biāo)識為置P_Flag為1；

如果線段L_i1,j1與所有相交的外包圓對應(yīng)的多邊形中的多個多邊形有交點，分別計算各個交點與子起始點WP_i1之間的距離，求最短距離d_Pi1，標(biāo)識該最短距離對應(yīng)的交點所在的多邊形區(qū)域P_j以及對應(yīng)交點所在邊的兩個頂點為置P_Flag為1。

3)如果C_Flag為1且P_Flag為1，則表明該線段與圓形威脅區(qū)域以及多邊形威脅區(qū)域都有交點，如果d_ci1小于等于d_Pi1，則置P_Flag為0，否則置C_Flag為0。

步驟4：基于步驟3標(biāo)識的威脅圓或多邊形進(jìn)一步生成具有安全緩沖距離的兩個外延點。

1)如果C_Flag為1，則基于標(biāo)識的圓形威脅區(qū)域C_i和子起始點WP_i1生成具有安全緩沖距離為d₁的兩個外延點，具體步驟如下：

①構(gòu)造輔助圓C_i2，該輔助圓的圓心為半徑為

②通過輔助圓C_i2與圓形威脅區(qū)域C_i的兩個標(biāo)準(zhǔn)方程作差，求得子起始點WP_i1向標(biāo)識的圓形威脅區(qū)域C_i作切線時，兩個切點CO1'、CO2'所在的直線L_co1,co2；

③求直線L_co1,co2和基于圓形威脅區(qū)域C_i生成的外延圓C_i3的兩個交點WP_i1 O1、WP_i1O2，WP_i1O1即為CO1'點的外延點，WP_i1O2即為CO2'點的外延點，上述外延圓C_i3的圓心和半徑為(x_i,y_i,(r_i+d₁))。

2)如果P_Flag為1，則基于標(biāo)識的多邊形威脅區(qū)域P_j以及頂點生成具有安全緩沖距離為d₂的兩個外延點WP_i1O1、WP_i1O2。具體如下：

①分別求得頂點對應(yīng)多邊形頂角的角平分線；

②分別在對應(yīng)角平分線所在直線上選取遠(yuǎn)離對應(yīng)頂點距離為d₂的外延點WP_i1O1、WP_i1O2。

步驟5：如果C_Flag為1或P_Flag為1，則進(jìn)行外延點是否落入其他威脅區(qū)域內(nèi)部的判斷。如果生成的外延點WP_i1O1、WP_i1O2都處于威脅區(qū)域內(nèi)部，則置IN_threat_Flag為1，否則置IN_threat_Flag為0。

步驟6：如果IN_threat_Flag為0，進(jìn)行外延點是否進(jìn)入死區(qū)的判斷與逃離處理，即將生成的外延點與存放在pathclose表中已規(guī)劃的路徑點進(jìn)行是否重合的判斷。若二者重合，生成新的外延點來避免和pathclose中的路徑點重合。具體如下：

1)對某一外延點WP_i1OK與pathclose表中已規(guī)劃的路徑點進(jìn)行是否存在重合的判斷，具體步驟如下：

①如果WP_i1OK不在pathclose中，即沒有重合，表明該點沒有進(jìn)入威脅死區(qū)，置flag_WP_i1為1，否則轉(zhuǎn)入步驟6.1).②；

②如果WP_i1OK為WP_i1O1，則將WP_i1O1所對應(yīng)的多邊形的頂點序號減1對應(yīng)的頂點為基礎(chǔ)生成相應(yīng)的外延點WP_i1O0；如果WP_i1OK為WP_i1O2，則將WP_i1O2所對應(yīng)的多邊形的頂點序號加1對應(yīng)的頂點為基礎(chǔ)生成相應(yīng)的外延點WP_i1O3，轉(zhuǎn)入步驟6.1).③；

③判斷新生成外延點WP_i1O0或WP_i1O3是否在其他威脅區(qū)域內(nèi)部，同時判斷其是否在pathclose中。如果新生成外延點不在其他威脅區(qū)域內(nèi)部同時也不在pathclose中，則置flag_WP_i1為1，并標(biāo)識外延點WP_i1O0或WP_i1O3，否則置flag_WP_i1為0。

2)經(jīng)過步驟5的判斷，如果只有一個外延點不在威脅區(qū)域內(nèi)部，將該外延點執(zhí)行步驟6.1)的判斷，此時如果flag_WP_i1為0，則置dead_flag_WP_i1為1。

3)經(jīng)過步驟5的判斷，如果兩個外延點WP_i1O1、WP_i1O2都不在威脅區(qū)域內(nèi)部，依次對這兩個外延點執(zhí)行步驟6.1)的判斷，并且得到每個外延點對應(yīng)的flag_WP_i1。如果兩個外延點WP_i1O1、WP_i1O2得到的flag_WP_i1都是0，則置dead_flag_WP_i1為1。

步驟7：如果dead_flag_WP_i1不為1，將步驟6的所有可行外延點依次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域進(jìn)行是否相交的判斷，具體如下：

1)如果可行外延點依次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域都不存在相交，置local_check_flag_WP_i1為1。

2)如果存在相交，依次將與威脅區(qū)域相交的外延點作為子目標(biāo)點，執(zhí)行步驟3～步驟6。

3)若新生成的外延點個數(shù)大于0，則對新的外延點再次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域進(jìn)行是否相交的判斷，即執(zhí)行步驟7.1)～7.2)。

4)如果子起始點WP_i1對應(yīng)的可行外延點個數(shù)為0，則置local_check_flag_WP_i1為0。

5)如果當(dāng)前所有與威脅區(qū)域不相交的子起始點WP_i1對應(yīng)的外延點的個數(shù)大于等于N_i1，終止步驟3～步驟6的執(zhí)行，置local_check_flag_WP_i1為1。

6)如果local_check_flag_WP_i1為1，則將子起始點WP_i1對應(yīng)的所有可行外延點存入openpath表中。

步驟8：如果local_check_flag_WP_i1為1，則將存入openpath表中的所有外延點轉(zhuǎn)存至openpath1表中生成子起始點WP_i1的一步外延點集，其外延點個數(shù)為NUM_1。依次以存儲在openpath1表中的每一個外延點P_step1__k作為子起始點，以系統(tǒng)的目標(biāo)點為子目標(biāo)點，依次執(zhí)行步驟3～步驟7，如果生成的可行外延點個數(shù)不為零，則依次將步驟7生成openpath表中的所有外延點轉(zhuǎn)存至openpath2_k表中，生成子起始點WP_i1一步外延點對應(yīng)的二步外延點集。

如果openpath1表中的每一個外延點P_step1__k作為子起始點，以系統(tǒng)的目標(biāo)點為子目標(biāo)點，依次執(zhí)行步驟3～步驟7，生成的可行外延點個數(shù)都為零，則置step2_fail_flag為零。

其中，k＝1,2,…,NUM_1。

步驟9：如果step2_fail_flag不為零，以子起始點WP_i1的存入openpath1表中的一步外延點P_step1__k、相應(yīng)的openpath2_k表中第二步外延點以及最終目標(biāo)點為基礎(chǔ)，依據(jù)代價函數(shù)最小生成子起始點WP_i1的下一個路徑點WP_i1+1(x_i1+1,y_i1+1)，存入closepath表中，轉(zhuǎn)入步驟11。

代價函數(shù)為f(i)＝w₁·g(i)+w₂·h(i)。

其中g(shù)(i)為子起始點WP_i1到一步外延點的距離與該一步外延點到相應(yīng)第二步外延點距離之和，h(i)為估計代價，即相應(yīng)的第二步外延點位置到目標(biāo)點距離。

步驟10：針對從當(dāng)前子起始點WP_i1生成一步外延點個數(shù)為零或者一步外延點不為零但第二步外延點總數(shù)為零的情形，選用相應(yīng)的備用點，以該備用點作為子起始點，以目標(biāo)點作為子目標(biāo)點，再轉(zhuǎn)入步驟3。備用點選擇步驟如下：

1)如果當(dāng)前子起始點WP_i1是外界輸入的系統(tǒng)起始點WP₁(x_S,y_S)，生成備用點的步驟如下：

①連接起始點WP₁(x_S,y_S)和目標(biāo)點WP_E(x_G,y_G)構(gòu)成線段L_S,G；

②在線段L_S,G上，生成距離起始點WP₁為R_1,E的點如果點不在威脅區(qū)域內(nèi)部且起始點與該點的連線與所有各威脅區(qū)域沒有交點，則點即為備用點；否則，以起始點WP₁為圓心，以點WP₁與點為半徑左右旋轉(zhuǎn)各60°形成的圓弧段上選擇滿足不在威脅區(qū)域內(nèi)部且起始點與該點的連線與所有各威脅區(qū)域沒有交點的點作為備用點。

2)如果當(dāng)前子起始點WP_i1不是系統(tǒng)起始點WP₁(x_S,y_S)，執(zhí)行如下步驟：

①將當(dāng)前子起始點WP_i1從closepath表中刪除；

②將WP_i1-1作為當(dāng)前子起始點，從存儲該點的一步外延點的表openpath1中刪除WP_i1點，然后依據(jù)每一個一步外延點生成對應(yīng)的第二步外延點集，通過求取代價函數(shù)最小值選取相應(yīng)的新的路徑點WP_i1'；

③將WP_i1'作為新的子起始點，如果依據(jù)航點WP_i1'生成的第二步外延點總個數(shù)為零且openpath1不為空，則轉(zhuǎn)入步驟10.2).①；

④如果openpath1為空，則子起始點進(jìn)一步回撤，直至回撤到起始點WP₁(x_S,y_S)，執(zhí)行步驟10.1)。

步驟11：判斷生成的路徑點WP_i1+1是否與目標(biāo)點WP_E(x_G,y_G)重合，如果重合，則規(guī)劃路徑搜索結(jié)束，輸出規(guī)劃路徑點集closepath表；如果不重合，以生成的路徑點WP_i1+1為子起始點，以目標(biāo)點WP_E(x_G,y_G)作為子目標(biāo)點，轉(zhuǎn)入步驟3。

本發(fā)明的有益效果是：該方法首先構(gòu)造多邊形威脅區(qū)域或禁飛區(qū)的外包圓，判斷起始點和目標(biāo)點構(gòu)成的線段與各多邊形外包圓是否相交。如果該線段與外包圓有相交則進(jìn)一步判斷該線段與多邊形威脅區(qū)域或禁飛區(qū)是否有交點，如果有交點，則基于A*搜索算法規(guī)劃生成直接規(guī)避多邊形威脅區(qū)域或禁飛區(qū)的路徑，在該路徑規(guī)劃過程中采用了死區(qū)逃離以及兩步尋優(yōu)的策略逐次規(guī)劃生成各路徑點。本發(fā)明能夠適應(yīng)于復(fù)雜環(huán)境中存在長條形狀的多邊形威脅區(qū)或禁飛區(qū)情形，同時還能夠適應(yīng)存在圓形或者多邊形威脅區(qū)域或禁飛區(qū)相互重疊的情形。由于采用了基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法，使得無人機(jī)飛行過程中突然出現(xiàn)危險/威脅或發(fā)生任務(wù)變更需要執(zhí)行動態(tài)路徑規(guī)劃時，能夠有效的適應(yīng)復(fù)雜環(huán)境中存在長條形狀的多邊形威脅區(qū)或禁飛區(qū)以及存在圓形或者多邊形威脅區(qū)域或禁飛區(qū)相互重疊的情形。

下面結(jié)合附圖和具體實施方式對本發(fā)明作詳細(xì)說明。

附圖說明

圖1是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法的流程圖。

圖2是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法中線段求交算法原理圖。

圖3是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法中路徑點局部檢測判斷原理圖。

圖4是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法中回撤至起始點時輔助離散點生成圖。

圖5是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法在典型場景下規(guī)劃生成的路徑圖。

圖6是本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法在典型場景下預(yù)判兩步規(guī)劃一步策略效果圖。

具體實施方式

參照圖1-6。本發(fā)明基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃方法具體步驟如下：

仿真是在{(x,y)|-110km≤x≤110km,-90km≤y≤90km}的二維平面矩形規(guī)劃空間進(jìn)行，規(guī)劃空間中包括圓形、多邊形以及相互疊加的威脅區(qū)域。

步驟1：無人機(jī)動態(tài)路徑規(guī)劃初始數(shù)據(jù)獲取與算法參數(shù)設(shè)置。

1)初始數(shù)據(jù)獲取。

①獲取無人機(jī)初始位置坐標(biāo)WP₁(50,80)和目標(biāo)位置坐標(biāo)WP_E(-110,-90)；

②威脅區(qū)域的參數(shù)數(shù)據(jù)包括：建模為圓形的威脅區(qū)域個數(shù)M＝13，每個圓形威脅區(qū)域C_i的圓心和半徑(x_i,y_i,r_i)，其中i＝1,2,…,M；建模為多邊形的威脅區(qū)域個數(shù)N＝10,各個多邊形威脅區(qū)域頂點個數(shù)(n₁,n₂,…,n_N)，每個多邊形威脅區(qū)域P_j頂點坐標(biāo)集其中j＝1,2,…,N。

表1圓形威脅區(qū)域參數(shù)(單位：km)

表2各多邊形威脅區(qū)域頂點個數(shù)

表3各多邊形威脅區(qū)域頂點坐標(biāo)集

2)設(shè)置算法參數(shù)。

①圓形威脅區(qū)域外延安全間距d₁＝2km,多邊形威脅區(qū)域外延安全間距d₂＝2km；

②代價函數(shù)權(quán)值參數(shù)設(shè)置：w₁＝0.3，w₂＝0.7。

步驟2：分別求所有多邊形威脅區(qū)域?qū)?yīng)的外包圓。

表4外包圓參數(shù)(單位：km)

步驟3：連接當(dāng)前子起始點WP_i1(x_i1,y_i1)和子目標(biāo)點GP_j1(x_j1,y_j1)構(gòu)成線段L_i1,j1，判斷該線段是否與各個威脅區(qū)域有交點。如果該線段和威脅區(qū)域無交點，置C_Flag為0且P_Flag為0，如果子目標(biāo)點GP_j1(x_j1,y_j1)為最終目標(biāo)點，則轉(zhuǎn)入步驟11。如果該線段和威脅區(qū)域有交點，求取距離子起始點WP_i1最近的交點CP_k1，標(biāo)識交點CP_k1所對應(yīng)的威脅區(qū)域，如果交點CP_k1落在多邊形的邊上，需要將交點所在的多邊形對應(yīng)邊的兩個頂點進(jìn)行標(biāo)識。

步驟4：基于步驟3標(biāo)識的威脅區(qū)域生成具有一定安全緩沖距離的兩個外延點WP_i1O1、WP_i1O2。參照圖2。

步驟5：如果C_Flag為1或P_Flag為1，則進(jìn)行外延點是否落入其他威脅區(qū)域內(nèi)部的判斷。如果生成的外延點WP_i1O1、WP_i1O2都處于威脅區(qū)域內(nèi)部，則置IN_threat_Flag為1，否則置IN_threat_Flag為0。

步驟6：如果IN_threat_Flag為0，進(jìn)行外延點是否進(jìn)入死區(qū)的判斷與逃離處理，具體說明如下：

1)對外延點WP_i1OK(即WP_i1O1或WP_i1O2)與pathclose表中已規(guī)劃的路徑點進(jìn)行是否存在重合的判斷，具體步驟如下：

①如果WP_i1OK不在pathclose中，即沒有重合，表明該點沒有進(jìn)入威脅死區(qū)，置flag_WP_i1為1，否則轉(zhuǎn)入6.1).②；

②如果WP_i1OK為WP_i1O1，則將WP_i1O1所對應(yīng)的多邊形的頂點序號減1對應(yīng)的頂點為基礎(chǔ)生成相應(yīng)的外延點WP_i1O0；如果WP_i1OK為WP_i1O2，則將WP_i1O2所對應(yīng)的多邊形的頂點序號加1對應(yīng)的頂點為基礎(chǔ)生成相應(yīng)的外延點WP_i1O3，轉(zhuǎn)入6.1).③；

③判斷新生成外延點WP_i1O0或WP_i1O3是否在其他威脅區(qū)域內(nèi)部，同時判斷其是否在pathclose中。如果新生成外延點不在其他威脅區(qū)域內(nèi)部同時也不在pathclose中，則置flag_WP_i1為1，并標(biāo)識外延點WP_i1O0或WP_i1O3，否則置flag_WP_i1為0。

2)經(jīng)過步驟5的判斷，如果只有一個外延點不在威脅區(qū)域內(nèi)部，將該外延點執(zhí)行步驟6.1)的判斷，此時如果flag_WP_i1為0，則置dead_flag_WP_i1為1。

3)經(jīng)過步驟5的判斷，如果兩個外延點WP_i1O1、WP_i1O2都不在威脅區(qū)域內(nèi)部，依次對這兩個外延點執(zhí)行步驟6.1)的判斷，并且得到每個外延點對應(yīng)的flag_WP_i1。如果兩個外延點WP_i1O1、WP_i1O2得到的flag_WP_i1都是0，則置dead_flag_WP_i1為1。

步驟7：如果dead_flag_WP_i1不為1，將步驟6的所有可行外延點依次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域進(jìn)行是否相交的判斷。參照圖3。

1)如果可行外延點依次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域都不存在相交，置local_check_flag_WP_i1為1。

2)如果存在相交，依次將與威脅區(qū)域相交的外延點作為子目標(biāo)點，執(zhí)行步驟3～步驟6。

3)若新生成的外延點個數(shù)大于0，則對新的外延點再次作為子目標(biāo)點與子起始點WP_i1構(gòu)成的線段與各威脅區(qū)域進(jìn)行是否相交的判斷，即執(zhí)行步驟7.1)～7.2)。

4)如果子起始點WP_i1對應(yīng)的可行外延點個數(shù)為0，則置local_check_flag_WP_i1為0。

5)如果當(dāng)前所有與威脅區(qū)域不相交的子起始點WP_i1對應(yīng)的外延點的個數(shù)大于等于N_i1，終止步驟3～步驟6的執(zhí)行，置local_check_flag_WP_i1為1。

6)如果local_check_flag_WP_i1為1，則將子起始點WP_i1對應(yīng)的所有可行外延點存入openpath表中。

步驟8：如果local_check_flag_WP_i1為1，則將存入openpath表中的所有外延點轉(zhuǎn)存至openpath1表中生成子起始點WP_i1的一步外延點集，其外延點個數(shù)為NUM_1。依次以存儲在openpath1表中的每一個外延點P_{step1_k}(k＝1,2,…,NUM_1)作為子起始點，以系統(tǒng)的目標(biāo)點為子目標(biāo)點，依次執(zhí)行步驟3～步驟7，如果生成的可行外延點個數(shù)不為零，則依次將步驟7生成openpath表中的所有外延點轉(zhuǎn)存至openpath2_k表中，生成子起始點WP_i1一步外延點對應(yīng)的二步外延點集。

如果openpath1表中的每一個外延點P_{step1_k}(k＝1,2,…,NUM_1)作為子起始點，以系統(tǒng)的目標(biāo)點為子目標(biāo)點，依次執(zhí)行步驟3～步驟7，生成的可行外延點個數(shù)都為零，則置step2_fail_flag為零。

步驟9：如果step2_fail_flag不為零，以子起始點WP_i1的存入openpath1表中的一步外延點P_{step1_k}、相應(yīng)的openpath2_k表中第二步外延點以及最終目標(biāo)點為基礎(chǔ)，依據(jù)代價函數(shù)最小生成子起始點WP_i1的下一個路徑點WP_i1+1(x_i1+1,y_i1+1)，存入closepath表中，轉(zhuǎn)入步驟11。

步驟10：針對從當(dāng)前子起始點WP_i1生成一步外延點個數(shù)為零或者一步外延點不為零但第二步外延點總數(shù)為零的情形，選用相應(yīng)的備用點，以該備用點作為子起始點，以目標(biāo)點作為子目標(biāo)點，再轉(zhuǎn)入步驟3。參照圖4。

步驟11：判斷生成的路徑點WP_i1+1是否與目標(biāo)點WP_E(-110,-90)重合，如果重合，則規(guī)劃路徑搜索結(jié)束，輸出規(guī)劃路徑點集closepath表；否則以生成的路徑點WP_i1+1為子起始點，以目標(biāo)點WP_E(-110,-90)作為子目標(biāo)點，轉(zhuǎn)入步驟3。

表5路徑點坐標(biāo)數(shù)據(jù)

仿真結(jié)果如附圖5和附圖6所示。其中，附圖5是基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃算法生成的路徑，威脅區(qū)域及禁飛區(qū)建模為附圖5中的圓形/多邊形以及相互疊加的區(qū)域，通過A*算法規(guī)劃生成從起始點WP₁到目標(biāo)點WP_E的規(guī)避威脅區(qū)域及禁飛區(qū)的路徑；附圖6為基于A*搜索的無人機(jī)路徑動態(tài)規(guī)劃算法預(yù)判兩步規(guī)劃一步策略示意圖，圖中各路徑分支顯示了路徑生成過程中的多種路徑生成可能，最終通過代價函數(shù)規(guī)劃生成附圖5中的路徑。