本發(fā)明屬于多無(wú)人機(jī)自主飛行與協(xié)同避障，具體涉及一種針對(duì)結(jié)構(gòu)化空域中的多無(wú)人機(jī)自主飛行與協(xié)同避障方法。

背景技術(shù)：

1、隨著無(wú)人機(jī)技術(shù)的飛速進(jìn)步，其在多個(gè)關(guān)鍵領(lǐng)域如物流、救援及災(zāi)害響應(yīng)中的應(yīng)用日益深化，極大地推動(dòng)了無(wú)人機(jī)產(chǎn)業(yè)的繁榮與飛行需求的多元化。然而，無(wú)人機(jī)數(shù)量的激增與有限空域資源之間的矛盾愈發(fā)凸顯，在城市低空空域管理中，傳統(tǒng)的點(diǎn)對(duì)點(diǎn)自由飛行模式已無(wú)法有效應(yīng)對(duì)日益復(fù)雜的安全與效率挑戰(zhàn)。為此，無(wú)人機(jī)航跡規(guī)劃與協(xié)同避障技術(shù)成為破解難題的關(guān)鍵。

2、然而，現(xiàn)有的無(wú)人機(jī)航跡規(guī)劃與避障方法大多存在局限性，如只考慮無(wú)人機(jī)點(diǎn)對(duì)點(diǎn)飛行、依賴于軌跡預(yù)測(cè)結(jié)果孤立地處理無(wú)人機(jī)間的沖突、計(jì)算資源消耗大、探測(cè)靈敏度不足、軌跡抖動(dòng)以及避障行為解釋性不強(qiáng)等，并且在在無(wú)人機(jī)自主航跡規(guī)劃、協(xié)同避障、低空空域結(jié)構(gòu)的協(xié)同問(wèn)題上考慮較少。這些問(wèn)題限制了無(wú)人機(jī)在障礙物密度高、環(huán)境復(fù)雜多變的城市低空區(qū)域中的高效、安全運(yùn)行。

技術(shù)實(shí)現(xiàn)思路

1、針對(duì)上述問(wèn)題，本發(fā)明提出一種基于強(qiáng)化學(xué)習(xí)的多無(wú)人機(jī)航跡動(dòng)態(tài)規(guī)劃與協(xié)同避障方法，旨在解決低空復(fù)雜運(yùn)行環(huán)境中大規(guī)模無(wú)人機(jī)運(yùn)行時(shí)航跡動(dòng)態(tài)規(guī)劃、協(xié)同運(yùn)行、沖突探測(cè)與協(xié)同避障問(wèn)題。該方法通過(guò)構(gòu)建三維柵格地圖、生成四維航跡、實(shí)時(shí)障礙物檢測(cè)與互惠速度障礙物計(jì)算、構(gòu)建時(shí)間序列深度神經(jīng)網(wǎng)絡(luò)模型、協(xié)同避障策略執(zhí)行與調(diào)整以及初始航跡動(dòng)態(tài)調(diào)整與優(yōu)化等步驟，實(shí)現(xiàn)了無(wú)人機(jī)在低空航路網(wǎng)中的高效航跡規(guī)劃與自主避障。

2、為了實(shí)現(xiàn)上述技術(shù)目的，達(dá)到上述技術(shù)效果，本發(fā)明通過(guò)以下技術(shù)方案實(shí)現(xiàn)：

3、一種基于強(qiáng)化學(xué)習(xí)的多無(wú)人機(jī)航跡動(dòng)態(tài)規(guī)劃與協(xié)同避障方法，其特征在于，包括如下步驟：

4、s1、構(gòu)建三維柵格地圖：對(duì)城市區(qū)域進(jìn)行三維建模，將建筑物、樹(shù)木障礙物建模為其最小包絡(luò)圓柱，構(gòu)建城市低空三維柵格地圖，包含：可飛柵格與障礙物柵格；

5、s2、生成初始四維航跡：在構(gòu)建的城市低空三維柵格地圖中，基于無(wú)人機(jī)飛行任務(wù)需求，使用路徑規(guī)劃算法生成無(wú)人機(jī)的初始四維航跡，該航跡包含無(wú)人機(jī)在三維空間中的位置隨時(shí)間變化的序列，即考慮了空間和時(shí)間兩個(gè)維度；

6、s3、實(shí)時(shí)障礙物檢測(cè)與互惠速度障礙物計(jì)算：在無(wú)人機(jī)運(yùn)行過(guò)程中，實(shí)時(shí)獲取每架無(wú)人機(jī)周圍一定范圍內(nèi)的障礙物信息，并計(jì)算無(wú)人機(jī)之間的互惠速度障礙物，以預(yù)測(cè)潛在的碰撞風(fēng)險(xiǎn)；同時(shí)，將存在沖突的建筑物與互惠速度障礙物按照預(yù)計(jì)碰撞時(shí)間進(jìn)行優(yōu)先度排列，以指導(dǎo)后續(xù)的避障決策；

7、s4、時(shí)間序列神經(jīng)網(wǎng)絡(luò)模型構(gòu)建與訓(xùn)練：構(gòu)建時(shí)間序列神經(jīng)網(wǎng)絡(luò)模型，將無(wú)人機(jī)的當(dāng)前狀態(tài)向量(包括位置、速度、加速度等)、障礙物向量以及所檢測(cè)到的互惠速度障礙物向量作為輸入；通過(guò)多層神經(jīng)網(wǎng)絡(luò)處理，輸出無(wú)人機(jī)下一時(shí)間步的最優(yōu)控制指令(如速度、航向角等)，以確保無(wú)人機(jī)與障礙物、其他無(wú)人機(jī)間的安全間隔滿足要求；采用強(qiáng)化學(xué)習(xí)算法對(duì)模型進(jìn)行訓(xùn)練，通過(guò)定義一系列獎(jiǎng)懲函數(shù)(如到達(dá)目標(biāo)點(diǎn)的獎(jiǎng)勵(lì)、偏離航線的懲罰、空中碰撞的嚴(yán)重懲罰以及軌跡抖動(dòng)的輕微懲罰)來(lái)引導(dǎo)模型學(xué)習(xí)最優(yōu)的避障策略；

8、s5、協(xié)同避障策略執(zhí)行與調(diào)整：使用訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)模型，根據(jù)各無(wú)人機(jī)的當(dāng)前狀態(tài)、優(yōu)先級(jí)、及相對(duì)位置關(guān)系調(diào)整避障策略、協(xié)調(diào)避障動(dòng)作，確保無(wú)人機(jī)之間無(wú)碰撞；

9、s6、初始航跡動(dòng)態(tài)調(diào)整與優(yōu)化：無(wú)人機(jī)執(zhí)行避障操作后，根據(jù)時(shí)間序列神經(jīng)網(wǎng)絡(luò)的輸出指令回溯無(wú)人機(jī)在每個(gè)時(shí)間步的實(shí)際動(dòng)作，包括位置、速度、航向等的調(diào)整。通過(guò)記錄調(diào)整后的狀態(tài)，重新構(gòu)建無(wú)人機(jī)四維航跡，即無(wú)人機(jī)位置隨時(shí)間變化的完整序列；最終得到在滿足無(wú)人機(jī)任務(wù)需求的同時(shí)，各無(wú)人機(jī)間安全間隔滿足要求的航跡集合。

10、可選地，步驟s1中，城市低空三維柵格地圖的構(gòu)建方法包括：

11、s11、基于激光雷達(dá)、地理信息系統(tǒng)gis數(shù)據(jù)、gps數(shù)據(jù)，采集空域內(nèi)三維物體坐標(biāo)信息，并將采集到的數(shù)據(jù)進(jìn)行坐標(biāo)系轉(zhuǎn)換；

12、s12、將空域內(nèi)的障礙物建模為其最小外形包絡(luò)圓柱，方便后續(xù)計(jì)算，減小沖突探測(cè)復(fù)雜度；

13、s13、確定柵格大小，將三維空間劃分為若干個(gè)大小相等的立方體柵格；每個(gè)柵格代表一個(gè)空間單元，用于存儲(chǔ)該單元內(nèi)的環(huán)境信息；

14、s14、對(duì)柵格進(jìn)行賦值，其中，0代表可飛空域，1代表障礙物區(qū)域，0-1間的值代表該柵格的風(fēng)險(xiǎn)值。

15、可選地，所述步驟s2中，無(wú)人機(jī)初始四維航跡的構(gòu)建方法包括：

16、s21、獲取無(wú)人機(jī)運(yùn)行計(jì)劃，包括起止點(diǎn)、期望起飛時(shí)間、期望到達(dá)時(shí)間、風(fēng)險(xiǎn)偏好(路徑最短、風(fēng)險(xiǎn)最低、延誤最小等)等；

17、s22、選取不同航跡規(guī)劃算法(a*、theta*等)，考慮無(wú)人機(jī)飛行性能約束，根據(jù)風(fēng)險(xiǎn)偏好設(shè)置不同權(quán)重啟發(fā)函數(shù)，在三維柵格地圖中對(duì)航線起止點(diǎn)進(jìn)行鏈接；

18、s23、根據(jù)無(wú)人機(jī)期望運(yùn)行速度v_del，計(jì)算無(wú)人機(jī)到達(dá)航路上航路點(diǎn)時(shí)間t，生成航路點(diǎn)坐標(biāo)-到點(diǎn)時(shí)間序列(x,y,z,t)。

19、可選地，所述步驟s3中，實(shí)時(shí)障礙物檢測(cè)與互惠速度障礙物計(jì)算方法具體包括：

20、s31、選取當(dāng)前無(wú)人機(jī)一定范圍內(nèi)的障礙物與該范圍內(nèi)的所有無(wú)人機(jī)；

21、s32、分別計(jì)算當(dāng)前無(wú)人機(jī)與范圍內(nèi)所有無(wú)人機(jī)的速度障礙物向量；

22、其中，無(wú)人機(jī)在保持當(dāng)前速度、或在一定速度范圍內(nèi)進(jìn)行移動(dòng)時(shí)，可能會(huì)與其他無(wú)人機(jī)發(fā)生碰撞的區(qū)域，該區(qū)域被定義為一系列速度向量集合，當(dāng)無(wú)人機(jī)采取該集合內(nèi)的速度向量時(shí)，將導(dǎo)致碰撞；具體為：

23、選取與當(dāng)前無(wú)人機(jī)運(yùn)行方向與(x,y)平面投影向量左右45°，距離50m，高度差10m范圍內(nèi)障礙物信息；

24、選取與當(dāng)前無(wú)人機(jī)距離50m范圍內(nèi)無(wú)人機(jī)，計(jì)算其速度障礙向量：

25、

26、其中，pa、pb表示當(dāng)前無(wú)人機(jī)與其他無(wú)人機(jī)的位置向量，pra、prb代表兩架飛機(jī)的優(yōu)先級(jí)，va、vb代表兩架飛機(jī)速度向量，t為時(shí)間步長(zhǎng)，a為時(shí)間縮放因子，ra、rb代表兩架飛機(jī)碰撞區(qū)域半徑，pvo為速度障礙圓錐起點(diǎn)，為速度障礙圓錐中心向量，α為速度障礙圓錐張角，則無(wú)人機(jī)速度障礙向量可表示為：

27、

28、障礙物向量可表示為：其中，(x,y)為障礙物坐標(biāo)，h為障礙物高度，r為圓柱化建模后的障礙物半徑；

29、s33、計(jì)算當(dāng)前無(wú)人機(jī)與障礙物、所有速度障礙物的預(yù)計(jì)碰撞時(shí)間，將障礙物按照避撞緊迫性進(jìn)行排序。

30、其中，無(wú)人機(jī)間預(yù)計(jì)碰撞時(shí)間計(jì)算方法可計(jì)算如下：

31、設(shè)兩無(wú)人機(jī)初始距離相對(duì)位置為(relx,rely,relz)，初始相對(duì)速度為(relvx,relvy,relvz)，半徑分別為ra、rb，在t時(shí)刻相對(duì)距離為d(t)，則有：

32、

33、在碰撞瞬間，d(t)＝r＝ra+rb，則有：

34、

35、兩邊平方得：

36、r2＝(relx+relvx*t)2+(rely+relvy*t)2+(relz+relvz*t)2

37、整理得關(guān)于t的二次方程：

38、at2+bt+c＝0

39、其中，a＝relvx2+relvy2+relvz2，b＝2(relx*relvx+rely*relvy+relz*relvz)，c＝relx2+rely2+relz2-r2。

40、若c≤0，則兩機(jī)相背飛行，若判別式b2-4ac≤0，則無(wú)相撞風(fēng)險(xiǎn)，若大于0，則利用求根公式求解，選兩根中大于等于0的最小值為預(yù)計(jì)碰撞時(shí)間。

41、可選地，所述步驟s4中，時(shí)間序列神經(jīng)網(wǎng)絡(luò)模型考慮時(shí)間依賴性和空間相關(guān)性，通過(guò)引入循環(huán)神經(jīng)網(wǎng)絡(luò)rnn或長(zhǎng)短期記憶網(wǎng)絡(luò)lstm等結(jié)構(gòu)來(lái)捕捉無(wú)人機(jī)狀態(tài)、觀測(cè)空間隨時(shí)間的動(dòng)態(tài)變化，并輸出無(wú)人機(jī)控制指令；模型訓(xùn)練過(guò)程采用了模擬環(huán)境與實(shí)際飛行數(shù)據(jù)相結(jié)合的方式，以提高模型的泛化能力和魯棒性；同時(shí)，通過(guò)不斷迭代優(yōu)化，使模型能夠在復(fù)雜多變的城市環(huán)境中實(shí)現(xiàn)高效、安全的自主導(dǎo)航與避障。

42、其中，基于時(shí)間序列神經(jīng)網(wǎng)絡(luò)的深度學(xué)習(xí)模型構(gòu)建方法為：

43、s4.1.1、數(shù)據(jù)獲?。韩@取無(wú)人機(jī)當(dāng)前狀態(tài)觀測(cè)向量obs_self＝[p,v,r,pr,v_des]，其中，p為無(wú)人機(jī)當(dāng)前三維位置坐標(biāo)，v為無(wú)人機(jī)當(dāng)前速度，r為無(wú)人機(jī)碰撞檢測(cè)半徑，pr為無(wú)人機(jī)優(yōu)先級(jí)，v_des為無(wú)人機(jī)期望運(yùn)行速度；

44、獲取無(wú)人機(jī)檢測(cè)范圍內(nèi)所有障礙物觀測(cè)向量其中，為單個(gè)障礙物觀測(cè)向量，n為觀測(cè)區(qū)障礙物總數(shù)；

45、獲取無(wú)人機(jī)周圍其他無(wú)人機(jī)速度障礙物觀測(cè)向量其中，為單個(gè)速度障礙物向量，m為觀測(cè)區(qū)域內(nèi)速度障礙物總數(shù)；

46、將無(wú)人機(jī)當(dāng)前狀態(tài)觀測(cè)向量、障礙物觀測(cè)向量、速度障礙物觀測(cè)向量進(jìn)行鏈接，得到該時(shí)刻無(wú)人機(jī)對(duì)環(huán)境觀測(cè)向量：

47、obs＝[obs_self,obs_barrier,obs_vo]

48、s4.1.2、構(gòu)建時(shí)間序列神經(jīng)網(wǎng)絡(luò)：選取適合處理時(shí)間序列數(shù)據(jù)的神經(jīng)網(wǎng)絡(luò)(如lstm或bigru將環(huán)境觀測(cè)向量作為時(shí)間序列神經(jīng)網(wǎng)絡(luò)輸入，獲取時(shí)間序列神經(jīng)網(wǎng)絡(luò)的前向信息h_forward或反向信息h_back(如果有)，并將其進(jìn)行拼接處理，形成包含完整上下文信息的輸出；

49、s4.1.3、深度神經(jīng)網(wǎng)絡(luò)構(gòu)建：使用拼接后的時(shí)間序列神經(jīng)網(wǎng)絡(luò)輸出作為深度神經(jīng)網(wǎng)絡(luò)的輸入；

50、該深度神經(jīng)網(wǎng)絡(luò)應(yīng)包含：歸一化層、全連接層、以及可選的卷積層；

51、該深度神經(jīng)網(wǎng)絡(luò)網(wǎng)絡(luò)的輸出為無(wú)人機(jī)在三維方向上的加速度(ax,ay,az)。

52、可選地，使用強(qiáng)化學(xué)習(xí)算法訓(xùn)練深度神經(jīng)網(wǎng)絡(luò)模型，具體方法為：

53、s4.2.1、無(wú)人機(jī)與觀測(cè)向量狀態(tài)獲?。簩o(wú)人機(jī)當(dāng)前狀態(tài)觀測(cè)向量obs_self、障礙物觀測(cè)向量obs_barrier和速度障礙物觀測(cè)向量obs_vo作為強(qiáng)化學(xué)習(xí)模型的狀態(tài)輸入；

54、s4.2.2、策略網(wǎng)絡(luò)與價(jià)值網(wǎng)絡(luò)：以上述使用時(shí)間序列神經(jīng)網(wǎng)絡(luò)作為策略網(wǎng)絡(luò)的一部分，以處理時(shí)間序列數(shù)據(jù)并捕捉環(huán)境動(dòng)態(tài)。接收環(huán)境觀測(cè)作為輸入，輸出無(wú)人機(jī)的控制動(dòng)作(如加速度)。若使用基于值的強(qiáng)化學(xué)習(xí)算法，除了策略網(wǎng)絡(luò)外，還需使用價(jià)值網(wǎng)絡(luò)來(lái)評(píng)估給定狀態(tài)下采取某個(gè)動(dòng)作的未來(lái)期望獎(jiǎng)勵(lì)，其也接收環(huán)境觀測(cè)作為輸入，輸出為狀態(tài)-動(dòng)作對(duì)的價(jià)值估計(jì)；

55、s4.2.3、懲罰項(xiàng)：根據(jù)無(wú)人機(jī)運(yùn)行的目標(biāo)，選取入侵速度障礙區(qū)、偏離航線、延誤、發(fā)生碰撞作為懲罰與到達(dá)航路點(diǎn)、到達(dá)目的地作為獎(jiǎng)勵(lì)；由于延誤、碰撞與到達(dá)目標(biāo)位置均可通過(guò)時(shí)間與坐標(biāo)距離差進(jìn)行計(jì)算，僅給出入侵速度障礙區(qū)與偏離航線計(jì)算方法。

56、(1)入侵速度障礙區(qū)計(jì)算方式如下：

57、設(shè)當(dāng)前無(wú)人機(jī)為a，相關(guān)無(wú)人機(jī)為b，b相對(duì)于a速度障礙物定義為：

58、

59、其中，pvo為速度障礙圓錐起點(diǎn)，為速度障礙圓錐中心向量，α為速度障礙圓錐張角；其計(jì)算方式為：

60、

61、其中，pa、pb表示當(dāng)前無(wú)人機(jī)與其他無(wú)人機(jī)的位置向量，pra、prb代表兩架飛機(jī)的優(yōu)先級(jí)，va、vb代表兩架飛機(jī)速度向量，t為時(shí)間步長(zhǎng)，a為時(shí)間縮放因子，ra、rb代表兩架飛機(jī)碰撞區(qū)域半徑。

62、設(shè)當(dāng)前a無(wú)人機(jī)狀態(tài)向量為：state＝[p,v,r,pr,v_des]，其中，p為無(wú)人機(jī)當(dāng)前三維位置坐標(biāo)，v為無(wú)人機(jī)當(dāng)前速度，r為無(wú)人機(jī)碰撞檢測(cè)半徑，pr為無(wú)人機(jī)優(yōu)先級(jí)，v_des為無(wú)人機(jī)期望運(yùn)行速度。

63、則a無(wú)人機(jī)在經(jīng)過(guò)at個(gè)時(shí)間步后的位置為：pnew＝p+v*at，此時(shí)pnew與速度障礙物起點(diǎn)向量為其與速度障礙圓錐中心向量夾角β可計(jì)算為：

64、

65、對(duì)速度障礙圓錐張角α與β大小進(jìn)行判定：若α≥β，則認(rèn)為兩機(jī)有互相侵入速度障礙物風(fēng)險(xiǎn)，若α＜β，則認(rèn)為兩機(jī)無(wú)碰撞風(fēng)險(xiǎn)。

66、(2)偏離航線程度計(jì)算方法如下：

67、設(shè)由一段直飛航路由起點(diǎn)s＝(x1,y1,z1)與終點(diǎn)e＝(x2,y2,z2)定義，該時(shí)刻無(wú)人機(jī)位置p＝(x0,y0,z0)，則可計(jì)算航路方向向量為：

68、d＝e-s＝(x2-x1,y2-y1,z2-z1)＝(dx,dy,dz)

69、將方向向量進(jìn)行歸一化：

70、

71、其中，計(jì)算無(wú)人機(jī)位置到航路起點(diǎn)向量的投影長(zhǎng)度：

72、

73、因此，航線上離無(wú)人機(jī)最近點(diǎn)為：

74、

75、無(wú)人機(jī)偏離程度即為無(wú)人機(jī)到最近點(diǎn)的距離：

76、

77、s4.2.4、策略優(yōu)化：將懲罰項(xiàng)賦予不同權(quán)重并加和(或其他處理方法)作為懲罰函數(shù)，以最小化懲罰函數(shù)、最大化累積獎(jiǎng)勵(lì)為目標(biāo)，使用策略梯度算法來(lái)更新策略。

78、若使用基于值的強(qiáng)化學(xué)習(xí)算法則需要更新價(jià)值網(wǎng)絡(luò)。價(jià)值網(wǎng)絡(luò)的更新依賴于策略網(wǎng)絡(luò)輸出的動(dòng)作以及從環(huán)境中獲得的獎(jiǎng)勵(lì)和下一個(gè)狀態(tài)；

79、s4.2.5、經(jīng)驗(yàn)回放：在無(wú)人機(jī)與環(huán)境交互的過(guò)程中，將每個(gè)時(shí)間步的狀態(tài)、動(dòng)作、獎(jiǎng)勵(lì)和下一個(gè)狀態(tài)存儲(chǔ)在經(jīng)驗(yàn)池中。然后，在訓(xùn)練過(guò)程中隨機(jī)采樣一批經(jīng)驗(yàn)來(lái)更新網(wǎng)絡(luò)；

80、s4.2.6、模型部署：在預(yù)定好的模擬場(chǎng)景中訓(xùn)練模型，并在模型性能滿足要求的條件下在無(wú)人機(jī)上進(jìn)行部署，在實(shí)際環(huán)境中進(jìn)行訓(xùn)練。

81、s4.2.7、軌跡調(diào)整：當(dāng)所有無(wú)人機(jī)完成無(wú)碰撞、無(wú)延誤、偏離預(yù)定航路程度滿足需求的航路后，對(duì)所有無(wú)人機(jī)狀態(tài)進(jìn)行回溯，形成新的位置-時(shí)間四維航跡集合。

82、可選地，所述步驟s6中，初始航跡動(dòng)態(tài)調(diào)整與優(yōu)化的具體方法為：在無(wú)人機(jī)運(yùn)行時(shí)，記錄無(wú)人機(jī)在每個(gè)時(shí)間步的位置，若在所有無(wú)人機(jī)到達(dá)目標(biāo)點(diǎn)后，航路網(wǎng)內(nèi)無(wú)無(wú)人機(jī)相撞或安全間隔不滿足要求的情況發(fā)生，且無(wú)人機(jī)到達(dá)時(shí)刻與偏離初始航跡的程度滿足要求，則將所有無(wú)人機(jī)的位置-時(shí)間序列作為新的四維航跡集合。

83、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果在于：

84、本發(fā)明通過(guò)構(gòu)建城市低空航路網(wǎng)，為無(wú)人機(jī)規(guī)劃最優(yōu)航路，同時(shí)結(jié)合深度神經(jīng)網(wǎng)絡(luò)與速度障礙方法構(gòu)建沖突探測(cè)-解脫模型，充分利用航路網(wǎng)結(jié)構(gòu)和無(wú)人機(jī)運(yùn)行計(jì)劃等信息，旨在提高沖突探測(cè)的靈敏度與效率，降低解脫動(dòng)作的計(jì)算復(fù)雜度，并使用強(qiáng)化學(xué)習(xí)算法對(duì)模型進(jìn)行訓(xùn)練，實(shí)現(xiàn)多無(wú)人機(jī)間的協(xié)同避障與整體飛行路徑的優(yōu)化，并為無(wú)人機(jī)解脫行為提供可解釋性與可評(píng)估性。在提高無(wú)人機(jī)運(yùn)行效率的同時(shí)，實(shí)現(xiàn)更加智能、高效、可理解評(píng)估的協(xié)同避障策略。