(19)國家知識產(chǎn)權(quán)局事務(wù)所(普通合伙)51422專利代理師賴將軍本發(fā)明提供了一種直升機避障路徑規(guī)劃方向的總代價對三維空間中直升機的避障路徑進S21.一種直升機避障路徑規(guī)劃方法，其特征在于，包S2、基于填充結(jié)果，獲取滿足約束條件的終點，并針對起點或終點為填充障礙點時，從起點或終點擴散取消填充障礙點；S3、基于擴散取消結(jié)果，采用雙向A*算法同步擴展，對直升機避障路徑進行搜索，并通過設(shè)計起點方向和終點方向的總代價中的實際代價和啟發(fā)代價對三維空間中直升機的避S4、基于生成的相鄰節(jié)點列表，利用繞墻法生成新的相鄰節(jié)點，并將新的相鄰節(jié)點列表中的最優(yōu)相鄰節(jié)點作為下一節(jié)點，并返回S3;S5、從相遇節(jié)點開始，分別沿著兩個方向的父節(jié)點回溯，直至起點和終點，并合并兩個方向的回溯路徑，得到從起點到終點完整路徑，完成對直升機避障路徑的規(guī)劃。2.根據(jù)權(quán)利要求1所述的直升機避障路徑規(guī)劃方法，其特征在于，所述S1具體為：對高程柵格地圖進行預(yù)處理，將4800m以上區(qū)域標(biāo)記為障礙點，采用水杯填充法進行填4800m以上區(qū)域指高程柵格地圖中當(dāng)前柵格高度大于4800m,標(biāo)記其為障礙區(qū)域，直升機無法飛過；水杯填充法指通過填充凹形區(qū)域降低直升機避障路徑規(guī)劃中的搜索空間，包含水平填充和垂直填充，掃描策略指逐行橫向遍歷地圖，檢查通行點；水平填充時，若當(dāng)前點左側(cè)為障礙點，當(dāng)前點上側(cè)或下側(cè)存在障礙點，以該點為起始點向右擴散，并將擴散點添加進擴散列表中，所有擴散點保持與起始點同方向的支撐點，若擴散中出現(xiàn)異側(cè)支撐或無支撐，則判定為漏口并終止當(dāng)前點填充；若擴散遇右側(cè)為障礙點，則將擴散列表里的點標(biāo)記為填充障礙點；垂直填充時，若當(dāng)前點上側(cè)為障礙點，當(dāng)前點左側(cè)或右側(cè)存在障礙點，以該點為起始點向下擴散，并將擴散點添加進擴散列表中，所有擴散點保持與起始點同方向的支撐點，若擴散中出現(xiàn)異側(cè)支撐或無支撐，則判定為漏口并終止當(dāng)前點填充；若擴散遇下側(cè)障礙點，則將擴散列表里的點標(biāo)記為填充障礙點；持續(xù)遍歷地圖，直至擴散列表為空的遍歷完一次時，完成填充。3.根據(jù)權(quán)利要求1所述的直升機避障路徑規(guī)劃方法，其特征在于，所述S2具體為：其中，終點不能為障礙點，即高度不超過4800m,同時終點的坡度不高于30度；S203、針對起點或終點為填充障礙點時，從起點或終點擴散取消填充障礙點。4.根據(jù)權(quán)利要求3所述的直升機避障路徑規(guī)劃方法，其特征在于，所述擴散查找滿足條從終點向周圍八個方向擴散，檢查是否滿足包括坡度和高度的約束條件，若不滿足，則在八個鄰居點的基礎(chǔ)上繼續(xù)擴散八個方向，直至查找到滿足約束條件的新的終點；坡度的表達式如下：3其中，slpoe表示坡度，dz表示鄰居點與中心點高度差的絕對值，ds表示鄰居點與中心點水平距離的絕對值，heightneighbor表示鄰居點的高度，heightcener表示中心點的高度，5.根據(jù)權(quán)利要求3所述的直升機避障路徑規(guī)劃方法，其特征在于，所述從起點或終點擴針對起點或終點為填充障礙點時，以起點或終點為中心向八個方向擴散，將擴散路徑中所有標(biāo)記為填充障礙的點恢復(fù)為可通行狀態(tài)；若擴散過程中遇到障礙點或可通行點，則6.根據(jù)權(quán)利要求3所述的直升機避障路徑規(guī)劃方法，其特征在于，所述S3具體為：S301、基于擴散取消結(jié)果，初始化雙向A*算法，分別創(chuàng)建從起點出發(fā)和從終點出發(fā)的開放列表Open_Listst、Open_Listena和關(guān)閉列表Closed_Listst、Closed_Listemd;S302、初始化起點方向和終點方向的總代價，并將起點和終點分別加入至開放列表Open_Lists,m和Open_Listea中，其中，起點方向和終點方向的總代價的表達式分別如下：f(E)=g(E)+h(E→S);h(S→E)=|xs-xg|+|ys-yE|+|zs其中，f(S)表示起點方向的總代價，f(E)表示終點方向的總示終點，g(S)表示起點的實際代價，起點為0,g(Nsa)表示當(dāng)前節(jié)點Nsua的實際代價，c(Nsa?,S)表示從當(dāng)前節(jié)點Nsa到達起點節(jié)點S的移動代價，由于是起點方向，起點為第一個節(jié)點，并無從其他節(jié)點到達起點的情況，因此都為0,h(S→E)表示起點到達終點的啟發(fā)式代價，g(E)表示終點的實際代價，終點為0,g(Nena)表示當(dāng)前節(jié)點Na的實際代價，c(Nena,S)表示從當(dāng)前節(jié)點Na到達起點節(jié)點E的移動代價，h(E→S)表示從終點到達起點的啟發(fā)式代價，(xg,ys,zs)表示起點的位置坐標(biāo)，(xE,YE,zE)表示終點的位置坐標(biāo)；4f(Nsram)值最小的節(jié)點Nstu和開放列表Open_Listma中f(Nena)值最小的節(jié)點Nma;S306、將當(dāng)前節(jié)點N從開放列表Open_List中移除，并將當(dāng)前節(jié)點N添加至關(guān)閉列表Closed_List中，同時根據(jù)當(dāng)前節(jié)點與目標(biāo)節(jié)點的方向角度生成相鄰節(jié)點列表將當(dāng)前節(jié)點N從開放列表Open_List中移除，并將當(dāng)前節(jié)點N添加至關(guān)閉列表示開放列表Open_Listea中f值最小的節(jié)點，arg表示取開放列表5針對方向角度為0°≤θ<45°時，其水平擴展方向為A1,三維擴展方向上的鄰節(jié)點列表標(biāo)為(0,0,0),其相鄰節(jié)點列表Neighbor_List坐標(biāo)如下：的位置，若方向角度為45°≤θ<90°時，其水平擴展方向為A2時，將相鄰節(jié)點列表Neighbor_List中節(jié)點f的x方向和y方向換為A2,其中，相鄰節(jié)點列表Neighbor_List中除節(jié)點f外的節(jié)點，需在節(jié)點f基礎(chǔ)上進行更改，具體如下：8.根據(jù)權(quán)利要求6所述的直升機避障路徑規(guī)劃方法，其特征在于，所述S4具體為：S401、判斷相鄰節(jié)點列表Neighbor_List里面的節(jié)點是否符合條件，若是，則進入S402,否則，利用繞墻法生成新的相鄰節(jié)點M,并進入S402;S402、判斷相鄰節(jié)點M是否已在當(dāng)前方向的開放列表Open_List中，若是，則進入S404,S403、判斷相鄰節(jié)點M是否已在當(dāng)前方向的關(guān)閉列表Closed_List中，若是，則進入6S406、設(shè)置相鄰節(jié)點M的父節(jié)點為節(jié)點N、總代價f(M),將相鄰節(jié)點M添加至開放列表7技術(shù)領(lǐng)域[0001]本發(fā)明屬于航空路徑規(guī)劃技術(shù)領(lǐng)域，尤其涉及一種直升機避障路徑規(guī)劃方法。背景技術(shù)[0002]近年來，伴隨自然災(zāi)害和各種突發(fā)事件的發(fā)生，導(dǎo)致對應(yīng)急救援服務(wù)需求的急速增長。而直升機以其具有的救援速度快、救援效率高、救援專業(yè)性強等特點，在應(yīng)急救援中得到廣泛應(yīng)用。[0003]但是在直升機應(yīng)急救援的工程中，缺少高效的路徑規(guī)劃，容易造成直升機應(yīng)急救援效率的不足，并且往往會忽略限制因素的影響，造成救援任務(wù)的失敗甚至是直升機事故。[0004]目前常用的算法主要有Dijkstra算法、快速擴展隨機樹算法(Rapidly-exploringRandomTrees,RRT)、A*算法、蟻群算法等。在實際應(yīng)用過程中，直升機路徑規(guī)劃問題面臨著許多挑戰(zhàn)和限制，首先飛行過程中需要考慮地形、氣象條件、障礙物等因素，其次還需要考慮到飛行時間、能耗、飛行安全等因素。因此，提出一種有效的路徑規(guī)劃算法就顯得尤為重要。[0005]A*算法是一種在圖形的路徑規(guī)劃和圖搜索中廣泛應(yīng)用的啟發(fā)式搜索算法。它是一種綜合了最佳優(yōu)先搜索和Dijkstra算法的特性。A*算法通過估計從起點到目標(biāo)的最短距離來引導(dǎo)搜索過程，同時考慮已經(jīng)走過的路徑代價和搜索代價。發(fā)明內(nèi)容[0006]針對現(xiàn)有技術(shù)中的上述不足，本發(fā)明提供的一種直升機避障路徑規(guī)劃方法，本發(fā)明解決了復(fù)雜三維空間下直升機避障路徑搜索算法效率低的問題。[0007]為了達到以上目的，本發(fā)明采用的技術(shù)方案為：一種直升機避障路徑規(guī)劃方法，包括以下步驟：[0009]S2、基于填充結(jié)果，獲取滿足約束條件的終點，并針對起點或終點為填充障礙點時，從起點或終點擴散取消填充障礙點；[0010]S3、基于擴散取消結(jié)果，采用雙向A*算法同步擴展，對直升機避障路徑進行搜索，并通過設(shè)計起點方向和終點方向的總代價中的實際代價和啟發(fā)代價對三維空間中直升機[0011]S4、基于生成的相鄰節(jié)點列表，利用繞墻法生成新的相鄰節(jié)點，并將新的相鄰節(jié)點列表中的最優(yōu)相鄰節(jié)點作為下一節(jié)點，并返回S3;[0012]S5、從相遇節(jié)點開始，分別沿著兩個方向的父節(jié)點回溯，直至起點和終點，并合并兩個方向的回溯路徑，得到從起點到終點完整路徑，完成對直升機避障路徑的規(guī)劃。[0013]本發(fā)明的有益效果是：本發(fā)明提供一種基于A*算法的改進直升機高效避障路徑規(guī)劃方法，主要包括對地圖進行水杯填充法預(yù)處理，減少搜索空間；采取雙向A*算法同步擴展，加快搜索效率；合理設(shè)計總代價中的實際代價和啟發(fā)代價提升三維空間中路徑規(guī)劃的8效率；減少不必要節(jié)點動作來提升搜索效率；通過繞墻法避免陷入死循環(huán)，有效提高復(fù)雜環(huán)境中的實用性。[0015]對高程柵格地圖進行預(yù)處理，將4800m以上區(qū)域標(biāo)記為障礙點，采用水杯填充法進[0016]4800m以上區(qū)域指高程柵格地圖中當(dāng)前柵格高度大于4800m,標(biāo)記其為障礙區(qū)域，直升機無法飛過；水杯填充法指通過填充凹形區(qū)域降低直升機避障路徑規(guī)劃中的搜索空[0017]水平填充時，若當(dāng)前點左側(cè)為障礙點，當(dāng)前點上側(cè)或下側(cè)存在障礙點，以該點為起始點向右擴散，并將擴散點添加進擴散列表中，所有擴散點保持與起始點同方向的支撐點，若擴散中出現(xiàn)異側(cè)支撐或無支撐，則判定為漏口并終止當(dāng)前點填充；若擴散遇右側(cè)為障礙點，則將擴散列表里的點標(biāo)記為填充障礙點；[0018]垂直填充時，若當(dāng)前點上側(cè)為障礙點，當(dāng)前點左側(cè)或右側(cè)存在障礙點，以該點為起始點向下擴散，并將擴散點添加進擴散列表中，所有擴散點保持與起始點同方向的支撐點，若擴散中出現(xiàn)異側(cè)支撐或無支撐，則判定為漏口并終止當(dāng)前點填充；若擴散遇下側(cè)障礙點，則將擴散列表里的點標(biāo)記為填充障礙點；[0019]持續(xù)遍歷地圖，直至擴散列表為空的遍歷完一次時，完成填充。[0020]上述進一步方案的有益效果是：本發(fā)明提供了水杯填充法預(yù)處理方案，可以在三維空間內(nèi)減少直升機的搜索空間，大大的提高搜索效率。S202,其中，終點不能為障礙點，即高度不超過4800m,同時終點的坡度不高于30度；[0024]S203、針對起點或終點為填充障礙點時，從起點或終點擴散取消填充障[0025]上述進一步方案的有益效果是：避免因水杯填充法預(yù)處理方案，將起點或終點填充為被填充障礙包圍的情況，從而路徑規(guī)劃失敗。[0027]從終點向周圍八個方向擴散，檢查是否滿足包括坡度和高度的約束條件，若不滿足，則在八個鄰居點的基礎(chǔ)上繼續(xù)擴散八個方向，直至查找到滿足約束條件的新的終點；[0028]坡度的表達式如下：9[0033]上述進一步方案的有益效果是：避免終點設(shè)置到4800m高度上，從而導(dǎo)致直升機無法到達終點，路徑規(guī)劃失敗，而采用坡度計算，則是為了避免選點到坡度過陡，直升機無法安全穩(wěn)定降落到該點。[0034]再進一步地，所述從起點或終點擴散取消填充障礙點，其具體為：[0035]針對起點或終點為填充障礙點時，以起點或終點為中心向八個方向擴散，將擴散路徑中所有標(biāo)記為填充障礙的點恢復(fù)為可通行狀態(tài)；若擴散過程中遇到障礙點或可通行點，則當(dāng)前點不進行擴散，直到無擴散點。[0036]再進一步地，所述S3具體為：[0037]S301、基于擴散取消結(jié)果，初始化雙向A*算法，分別創(chuàng)建從起點出發(fā)和從終點出發(fā)[0038]S302、初始化起點方向和終點方向的總代價，并將起點和終點分別加入至開放列表Open_Listsm和Open_Listena中，其中，起點方向和終點方向的總代價的表達式分別如[0043]h(S→E)=|xs-xg|+|ys-yB|+|zs-zB|[0044]h(E→S)=|xg-xs|+|yE-ys|+|zB-zs|第一個節(jié)點，并無從其他節(jié)點到達起點的情況，因此都為0,h(S→E)表示起點到達終點的啟發(fā)式代價，g(E)表示終點的實際代價，終點為0,g(Nen)表示當(dāng)前節(jié)點Nem的實際代價，c(Nena,S)表示從當(dāng)前節(jié)點Na到達起點節(jié)點E的移動代價，h(E→S)表示從終點到達起點的啟發(fā)式代價，(xs,ys,Zs)表示起點的位置坐標(biāo)，(xE,yE,zE)表示終點的位置坐標(biāo)；[0046]S303、判斷開放列表Open_Lists,m和Open_Listema是否存在空列表，若是，則不能規(guī)劃出三維空間中直升機的避障路徑，結(jié)束流程，否則，進入S304;Open_Listsu中f(Nstam)值最小的節(jié)點Nsa和開放列表Open_Listm中f(Nma)值最小的[0048]S305、判斷從兩個開放方方向的關(guān)閉列表Closed_List中，若是，則判斷為表Closed_List中，同時根據(jù)當(dāng)前節(jié)點與目標(biāo)節(jié)點的方向角度生成相鄰節(jié)點列表[0051]將當(dāng)前節(jié)點N從開放列表Open_List中移除，并將當(dāng)前節(jié)點N添加至關(guān)閉列表[0054]其中，N表示開放列表Open_List中的節(jié)點，f(N)表示開放列表Open_List中所a表示取開放列表Open_List中f值a示開放列表Open_Listea中f值最小的節(jié)點，表示取開放列表A7和A8:11點N坐標(biāo)為(0,0,0),其相鄰節(jié)點列表Neighbor_List坐標(biāo)如下：G、H、I的位置，若方向角度為45°≤θ<90°時，其水平擴展方向為Neighbor_List中節(jié)點fe的x方向和y方向換為A2,其中，相鄰節(jié)點列表Neighbor_List中除節(jié)點fe外的節(jié)點，需在節(jié)點f基礎(chǔ)上進行更改，具體如下：[0061]上述進一步方案的有益效果是：在三維柵格空間的節(jié)點搜索設(shè)計中，每個節(jié)點具在正常飛行時應(yīng)保持前進方向的運動，因此依據(jù)當(dāng)前節(jié)點與目標(biāo)節(jié)點的方向角度來確定水平拓展方向，這樣直升機三維方向能夠選擇拓展的節(jié)點被限定于前進方向的9個柵格動作，這一優(yōu)化策略大幅削減了不必要的搜索節(jié)點，顯著提升了搜索效率。S402,否則，利用繞墻法生成新的相鄰節(jié)點M,并進入S402;[0064]S402、判斷相鄰節(jié)點M是否已在當(dāng)前方向的開放列表Open_List中，若是，則進入[0066]S404、判斷8ne(M)是否小于g(M),若是，則經(jīng)當(dāng)前節(jié)點N到相鄰節(jié)點M的實際代價最小，并更新相鄰節(jié)點M的父節(jié)點為節(jié)點N,重新計算總代價f(M),并返回S303,否則，不示開放列表Open_List中記錄曾經(jīng)從除當(dāng)前節(jié)點N外的節(jié)點到達相鄰節(jié)點M的實際代價；[0067]S405、判斷8nen(M)是否小于g(M),若是，則相鄰節(jié)點M從關(guān)閉列表Closed_List[0068]S406、設(shè)置相鄰節(jié)點M的父節(jié)點為節(jié)點N、總代價f(M),將相鄰節(jié)點M添加至開放列[0069]上述進一步方案的有益效果是：檢查相鄰節(jié)點M是否在開放列表Open_List,若在關(guān)閉列表Closed_List,若是，則決定是否重新啟用，若否，就在設(shè)置屬性后加入開放列表Open_List,合理管理搜索空間，讓搜索更有序。而其中對比8nen(M)和g(M),有助于找到全局更優(yōu)路徑，提升路徑規(guī)劃代價最小結(jié)果。[0070]再進一步地，所述總代價f(M)的表達式如下：[0073]上述進一步方案的有益效果是：總代價f(M)內(nèi)包括實際代價8neun(M)與啟發(fā)代價h(N→G),其中8new(M)確保算法不會偏離已探索的較優(yōu)實際代價，而三維曼哈頓距離作為啟發(fā)代價，指明趨近目標(biāo)節(jié)點的方向，算法借二者綜合考量，精準(zhǔn)評估節(jié)點，從而找到更優(yōu)路徑。附圖說明[0074]圖1為本發(fā)明的方法流程圖。[0075]圖2為根據(jù)當(dāng)前節(jié)點與目標(biāo)節(jié)點角度確定的前進方向圖。具體實施方式[0077]下面對本發(fā)明的具體實施方式進行描述，以便于本技術(shù)領(lǐng)域的技術(shù)人員理解本發(fā)明，但應(yīng)該清楚，本發(fā)明不限于具體實施方式的范圍，對本技術(shù)領(lǐng)域的普通技術(shù)人員來講，只要各種變化在所附的權(quán)利要求限定和確定的本發(fā)明的精神和范圍內(nèi)，這些變化是顯而易見的，一切利用本發(fā)明構(gòu)思的發(fā)明創(chuàng)造均在保護之列。單元格?，F(xiàn)方法如下：直升機無法飛過；水杯填充法指通過填充凹形區(qū)域降低直升機避障路徑規(guī)劃中的搜索空點與起點的距離。[0097]S203、針對起點和終點為填充障礙點時，從起點或終點擴散取消填充障礙點，其具體為：[0098]針對起點或終點為填充障礙點時，以起點或終點為中心向八個方向擴散，將擴散路徑中所有標(biāo)記為填充障礙的點恢復(fù)為可通行狀態(tài)；若擴散過程中遇到障礙點或可通行點，則當(dāng)前點不進行擴散，直到無擴散點。[0099]S3、基于擴散取消結(jié)果，采用雙向A*算法同步擴展，對直升機避障路徑進行搜索，并通過設(shè)計起點方向和終點方向的總代價中的實際代價和啟發(fā)代價對三維空間中直升機的避障路徑進行規(guī)劃，生成相鄰節(jié)點列表，其中，當(dāng)為雙向搜索相遇時，進入S5,其實現(xiàn)方法如下：[0100]S301、基于擴散取消結(jié)果，初始化雙向A*算法，分別創(chuàng)建從起點出發(fā)和從終點出發(fā)存儲已擴展節(jié)點。[0102]S302、初始化起點方向和終點方向的總代價，并將起點和終點分別加入至開放列表Open_Listsam和Open_Listema中，其中，起點和終點的總代價的表達式分別如下：[0107]h(S→E)=|xs-xg|第一個節(jié)點，并無從其他節(jié)點到達起點的情況，因此都為0,h(S→E)表示起點到達終點的啟發(fā)式代價，g(E)表示終點的實際代價，終點為0,g(Nema)表示當(dāng)前節(jié)點Nema的實際代價，c(Nena,S)表示從當(dāng)前節(jié)點Na到達起點節(jié)點E的移動代價，由于是終點方向，終點為第一個節(jié)點，并無從其他節(jié)點到達終點的情況，因此都為0,h(E→S)表示從終點到達起點的啟發(fā)式代價，(xg,ys,zs)表示起點的位置坐標(biāo)，(xg,yE,zE)表示終點的位置坐標(biāo)；啟發(fā)式代價均通過三維曼哈頓距離來計算，移動代價c(A,B)指從A點到B點的移動代價，實際代價[0111]S304、若兩個方向的開放列表Open_List均非空，則分別選擇開放列表節(jié)點Nena;表Closed_List中，同時根據(jù)當(dāng)前節(jié)點與目標(biāo)節(jié)點的方向角度生成相鄰節(jié)點列表[0114]將當(dāng)前節(jié)點N從開放列表Open_List中移除，并將當(dāng)前節(jié)點N添加至關(guān)閉列表有節(jié)點的f值構(gòu)成的集合，Nskur表示開放列表Open_Listsur中f值最小的節(jié)點，aa示開放列表Open_Listea中f值最小的節(jié)點，表示取開放列表A7和A8:[0120]針對方向角度為0°≤θ<45°時，其水平擴展方向為A1,三維擴展方向上的鄰節(jié)點列表Neighbor_List分布于前方整個面的前左上節(jié)點A、前左節(jié)點B、前左下節(jié)點C、前上節(jié)點D、前節(jié)點E、前下節(jié)點F、前右上節(jié)點G、前右節(jié)點H和前右下節(jié)點九個點上，其中，若當(dāng)前節(jié)點N坐標(biāo)為(0,0,0),其相鄰節(jié)點列表Neighbor_List坐標(biāo)如下：[0122]其中，0表示方向角度，fa、fB、fc、fp、fE、f、fg、fa和f1分別表示當(dāng)前節(jié)點NG、H、I的位置，若方向角度為45°≤θ<90°時，其水平擴展方向為A2時，將相鄰節(jié)點列表Neighbor_List中節(jié)點fe的x方向和y方向換為A2,其中，相鄰節(jié)點列表Neighbor_List中除節(jié)點f外的節(jié)點，需在節(jié)點f基礎(chǔ)上進行更改，具體如下：[0124]本實施例中，考慮到直升機在正常飛行時應(yīng)保持前進方向的運動，其中前進方向只分析水平方向，而其中前進的運行方向根據(jù)終點與起點的方向生成角度。[0125]本實施例中，如圖2所示，方向圖描述如下：其中當(dāng)前節(jié)點指的是直升機的當(dāng)前位置，而目標(biāo)節(jié)點指的是要當(dāng)前方向到達的目標(biāo)節(jié)點，從起點出發(fā)時，目標(biāo)節(jié)點指的是終點，從終點出發(fā)時，目標(biāo)節(jié)點指的是起點，而前進方向僅指代水平方向，因此柵格方向是一個二維的方向，只有x和y坐標(biāo)的變化，因此當(dāng)前節(jié)點為N時，他一共有8個方向可以選擇，需要根據(jù)方向角度來確定前進方向。[0126]本實施例中，如圖3所示，節(jié)點搜索范圍圖描述為：當(dāng)確定前進方向后，當(dāng)前節(jié)點為這些則是相鄰節(jié)點列表里的相鄰節(jié)[0127]本實施例中，水杯填充法為現(xiàn)有技術(shù)，流程描述如下：[0128]1.初始化：初始化當(dāng)前柵格點；[0129]2.主循環(huán)：[0130]a.逐行遍歷地圖柵格點；[0131]b.判斷地圖是否遍歷完成?[0132]Y:地圖遍歷完成，判斷擴散列表是否為空?[0133]Y:擴散列表為空，無需填充，結(jié)束；[0134]N:擴散列表不為空，仍需填充，初始化當(dāng)前柵格點，從頭開始；[0135]N:地圖未遍歷完成，向下進行，取下一個柵格點；[0136]c.判斷當(dāng)前點是否是可通行點?[0137]Y:是可通行點，向下進行，判斷填充類型；[0138]N:不是可通行點，返回遍歷地圖；[0139]d.判斷當(dāng)前點左側(cè)是否為障礙點?[0140]Y:是障礙點，水平填充觸發(fā)；[0141]N:不是障礙點，下面判斷當(dāng)前點上側(cè)是否為障礙點；[0142]e.判斷當(dāng)前點上側(cè)是否為障礙點?[0143]Y:是障礙點，垂直填充觸發(fā)；[0144]N:不是障礙點，返回遍歷地圖；[0145]3.擴散填充：[0146]a.沿觸發(fā)方向逐點檢測，水平填充方向為向右，垂直填充方向為向下；[0147]遇同側(cè)支撐則持續(xù)擴散，并將擴散點添加至擴散列表；[0148]遇異側(cè)支撐或無支撐則判定漏口，終止擴散，清空擴散列表；[0149]遇障礙點則將擴散列表里的擴散點標(biāo)記為填充障礙；[0150]4.循環(huán)終止條件：完成全圖遍歷且擴散列表為空時結(jié)束。[0151]本實施例中，中心擴散法為現(xiàn)有技術(shù)，流程描述如下：[0152]1.初始化列表：初始化待檢查點列表，已檢查列表；[0153]2.主循環(huán)[0154]a.令待檢查點為終點；[0155]b.將待檢查點添加進待檢查點列表；[0156]c.判斷是否遍歷完待檢查點列表?[0157]Y:遍歷完，沒有滿足約束條件的點，擴散結(jié)束；[0158]N:沒有遍歷完，正常往下進行；[0159]d.獲取遍歷的待檢查點，并將其從待檢查點列表中移除；[0160]e.判斷待檢查點是否存在于已檢查點列表?[0161]Y:存在，則返回繼續(xù)遍歷；[0162]N:不存在，則正常向下進行；[0163]f.獲取待檢查點與其8個鄰居點之間的角度，并將低于4800m的鄰居點添加進待檢查點列表；[0164]g.將鄰居點添加進待檢查列表，將待檢查點添加進已檢查列表；[0165]h.判斷待檢查點與鄰居點的角度是否小于30度且高度小于4800m?[0166]Y:待檢查點滿足約束條件，則待檢查點為新終點，結(jié)束；[0167]N:待檢查點不滿足約束條件，則返回繼續(xù)遍歷待檢查點列表；[0168]3.循環(huán)終止條件：找到滿足約束條件的待檢查點或遍歷完待檢查點列表。[0169]本實施例中，取消水杯填充法為現(xiàn)有技術(shù)，流程描述如下：[0170]1.初始化列表：初始化待檢查點列表，已檢查列表[0171]2.主循環(huán)[0172]a.判斷當(dāng)前點是否為填充障礙點?[0173]Y:是填充障礙點，正常向下進行；[0174]N:不是填充障礙點，無需取消擴散的點，流程結(jié)束；[0175]b.令待檢查點為當(dāng)前點；[0176]c.將待檢查點添加進待檢查點列表，并取消待檢查點的填充障礙標(biāo)記；[0177]d.判斷是否遍歷完待檢查點列表?[0178]Y:遍歷完成，無需取消擴散的點，流程結(jié)束；[0179]N:沒有遍歷完，正常往下進行；[0180]e.獲取遍歷的待檢查點，并將其從待檢查點列表中移除；[0181]f.判斷待檢查點是否存在于已檢查點列表?[0182]Y:存在，則返回繼續(xù)遍歷待檢查點列表[0183]N:不存在，則正常向下進行；[0184]g.獲取待檢查點的8個鄰居點；[0185]h.判斷8個鄰居點是否為填充障礙點?[0186]Y:是填充障礙點的鄰居點，將該鄰居點添加進待檢查列表，并取消該鄰居點的填充障礙標(biāo)記；[0187]N:不是填充障礙點的鄰居點，跳過處理，返回繼續(xù)遍歷待檢查點列表；[0188]3.循環(huán)終止條件：遍歷完待檢查點列表。[0189]本實施例中，左手繞墻法為現(xiàn)有技術(shù)，流程描述如下：[0190]1.初始化：初始化節(jié)點通行列表，待繞墻列表，繞墻計數(shù)為0;[0192]a.判斷相鄰節(jié)點列表中是否存在非通行點?[0193]Y:存在的，將非通行點添加進待繞墻列表；[0194]N:不存在的，則無需繞墻，直接結(jié)束；[0195]b.判斷是否遍歷完相鄰節(jié)點列表?[0196]Y:遍歷完，獲得待繞墻列表；[0197]N:未遍歷完，返回繼續(xù)判斷相鄰節(jié)點列表中是否存在非通行點；[0198]c.判斷是否遍歷完待繞墻列表?[0199]Y:遍歷完，待繞墻列表中不存在需繞墻節(jié)點，結(jié)束；[0200]N:未遍歷完，非通行點與當(dāng)前點x、y坐標(biāo)差值獲取水平動作direction;[0201]d.右繞墻法，右繞墻動作directions序列：(1,0)→(1,-1)→(0,-1)→(-1,-1)[0202]e.按照右繞墻動作directions序列進行繞墻，這是一個循環(huán)，根據(jù)水平動作direction選擇從哪里開始遍歷，每繞過一次繞墻計數(shù)+1;[0203]f.判斷繞墻計數(shù)是否大于8?[0204]Y.繞墻計數(shù)大于8,返回判斷是否遍歷完待繞墻列表；[0205]N:繞墻計數(shù)未大于8,獲取繞墻后的位置，也就是從當(dāng)前位置進行左繞墻動作directions序列中的下一個動作；[0206]g.判斷繞墻后的位置是否存在可通行點?[0207]Y:存在可通行點，將可通行點添加進節(jié)點通行列表；[0208]N:返回繞墻循環(huán)；[0209]3.循環(huán)終止條件：相鄰節(jié)點列表不存在非通行點，或遍歷完待繞墻列表。[0210]S4、基于生成的相鄰節(jié)點列表，利用繞墻法生成新的相鄰節(jié)點，并將新的相鄰節(jié)點列表中的最優(yōu)相鄰節(jié)點作為下一節(jié)點，并返回S3,其實現(xiàn)方法如下：[0211]S401、判斷相鄰節(jié)點列表Neighbor_List里面的節(jié)點是否符合條件，若是，則進入S402,否則，利用繞墻法生成新的相鄰節(jié)點M,并進入S402;[0212]本實施例中，繞墻法是一種沿障礙物邊緣繞行的避障策略，包含左手摸墻法和右手摸墻法。遇障時，左手法優(yōu)先向左轉(zhuǎn)，若左方的上、中、下三節(jié)點仍為障礙，則繼續(xù)左轉(zhuǎn)，直至不遇見障礙或轉(zhuǎn)向次數(shù)超過8次；右手法反之，優(yōu)先向右轉(zhuǎn)，若右方三節(jié)點仍為障礙，則繼續(xù)右轉(zhuǎn)，直至滿足相同終止條件。[0213]S402、判斷相鄰節(jié)點M是否已在當(dāng)前方向的開放列表Open_List中，若是，則進入[0214]本實施例中，該S402是這是為了確保開放列表Open_List中始終保存到達個節(jié)點的最小代價路徑，避免因早期記錄的高代價路徑而錯過更優(yōu)解。[0216]本實施例中，S403是為了發(fā)現(xiàn)更優(yōu)路徑時重新激活節(jié)點，確保全局最優(yōu)解的可能性。[0217]S404、判斷8new(M)是否小于g(M),若是，則經(jīng)當(dāng)前節(jié)點N到相鄰節(jié)點M的實際代價最小，并更新相鄰節(jié)點M的父節(jié)點為節(jié)點N,重新計算總代價f(M),這樣到達相鄰節(jié)點M當(dāng)前節(jié)點N到達相鄰節(jié)點M的實際代價，g(M)表示開放列表Open_List中記錄曾經(jīng)從除當(dāng)前節(jié)點N外的節(jié)點到達相鄰節(jié)點M的實際代價；[0218]S405、判斷8nen(M)是否小于g(M),若是，則相鄰節(jié)點M從關(guān)閉列表Closed_List[0219]S406、設(shè)置相鄰節(jié)點M的父節(jié)點為節(jié)點N、總代價f(M),將相鄰節(jié)點M添加至開放列[0220]S5、從相遇節(jié)點開始，分別沿著兩個方向的父節(jié)點回溯，直至起點和終點，并合并兩個方向的回溯路徑，得到從起點到終點完整路徑，完成對直升機避障路徑的規(guī)劃。[0223]2.主循環(huán)[0224]a.判斷相鄰節(jié)點列表中是否存在非通行點?[0225]Y:存在的，將非通行點添加進待繞墻列表；[0227]b.判斷是否遍歷完相鄰節(jié)點列表?[0229]N:未遍歷完，返回繼續(xù)判