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

背景技術(shù)：

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

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

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

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

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

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

步驟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_i1 O1、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。

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

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

附圖說(shuō)明

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

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

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

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

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

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

具體實(shí)施方式

參照?qǐng)D1-6。本發(fā)明基于A*搜索的無(wú)人機(jī)路徑動(dòng)態(tài)規(guī)劃方法具體步驟如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

步驟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í)。

步驟4：基于步驟3標(biāo)識(shí)的威脅區(qū)域生成具有一定安全緩沖距離的兩個(gè)外延點(diǎn)WP_i1O1、WP_i1O2。參照?qǐng)D2。

步驟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ū)的判斷與逃離處理，具體說(shuō)明如下：

1)對(duì)外延點(diǎn)WP_i1OK(即WP_i1O1或WP_i1O2)與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)行是否相交的判斷。參照?qǐng)D3。

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}(k＝1,2,…,NUM_1)作為子起始點(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}(k＝1,2,…,NUM_1)作為子起始點(diǎn)，以系統(tǒng)的目標(biāo)點(diǎn)為子目標(biāo)點(diǎn)，依次執(zhí)行步驟3～步驟7，生成的可行外延點(diǎn)個(gè)數(shù)都為零，則置step2_fail_flag為零。

步驟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。

步驟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。參照?qǐng)D4。

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

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

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