技術(shù)特征：

1.一種基于A*搜索的無(wú)人機(jī)路徑動(dòng)態(tài)規(guī)劃方法，其特征在于包括以下步驟：

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

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

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

②威脅區(qū)域的參數(shù)數(shù)據(jù)包括：建模為圓形的威脅區(qū)域個(gè)數(shù)M，每個(gè)圓形威脅區(qū)域C_i的圓心和半徑(x_i,y_i,r_i)，其中i＝1,2,…,M；建模為多邊形的威脅區(qū)域個(gè)數(shù)N,各個(gè)多邊形威脅區(qū)域頂點(diǎn)個(gè)數(shù)(n₁,n₂,...,n_N)，每個(gè)多邊形威脅區(qū)域P_j頂點(diǎn)坐標(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₁))上生成具有緩沖安全距離的點(diǎn)；多邊形威脅區(qū)域的外延距離d₂，也即在某個(gè)多邊形的頂點(diǎn)處生成距離該點(diǎn)為d₂的點(diǎn)以備后續(xù)算法使用；外延安全間距的大小依據(jù)飛機(jī)的機(jī)動(dòng)性能、飛行速度條件約束加以選擇設(shè)置；

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

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

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

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

多邊形的外包圓也即包含該多邊形所有頂點(diǎn)的最小圓；一個(gè)多邊形外包圓的求取過(guò)程如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4)如果子起始點(diǎn)WP_i1對(duì)應(yīng)的可行外延點(diǎn)個(gè)數(shù)為0，則置local_check_flag_WP_i1為0；

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

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

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

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

其中，k＝1,2,...,NUM_1；

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

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

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

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

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

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

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

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

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

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

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

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

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