本發(fā)明涉及一種基于多距離約束的無人機路徑規(guī)劃方法，屬于城市環(huán)境下無人機低空飛行的自動路徑規(guī)劃技術(shù)。

背景技術(shù)：

1、a星算法是典型的啟發(fā)式路徑規(guī)劃方法，標(biāo)準(zhǔn)a星算法的評價函數(shù)可以表示為f(n)＝g(n)+h(n)，其中f(n)表示由起始節(jié)點途徑當(dāng)前節(jié)點并到達(dá)目標(biāo)節(jié)點的總代價值，g(n)為代價函數(shù)，表示由起始節(jié)點到達(dá)當(dāng)前節(jié)點的代價值，h(n)為啟發(fā)式函數(shù)，表示由當(dāng)前節(jié)點到達(dá)目標(biāo)節(jié)點的代價值。a星算法的路徑搜索過程如圖1所示，a星算法的每個節(jié)點都對應(yīng)一個總代價值，節(jié)點的總代價值越小，則表明該節(jié)點越被優(yōu)先考慮；a星算法通過不斷檢測最小的f(n)來獲得一條總代價值最小的路徑。

2、傳統(tǒng)a星算法通過實際路徑長度代價來計算g(n)，用單一距離表達(dá)式來計算h(n)，如式(1)的曼哈頓距離、式(2)的歐氏距離和式(3)的切比雪夫距離。

3、h(n)＝|xe-xn|+|ye-yn|?????????????????????????(1)

4、

5、h(n)＝max(xe-xn|,|ye-yn|)???????????????????(3)

6、式中：(xn,yn)表示當(dāng)前節(jié)點的坐標(biāo)位置，(xe,ye)表示目標(biāo)節(jié)點的坐標(biāo)位置。

7、申請?zhí)枮閏n202210684359x、名稱為一種改進(jìn)a*算法的適合雜亂環(huán)境的無人機路徑規(guī)劃方法的中國專利申請，在傳統(tǒng)a星算法的基礎(chǔ)上，改進(jìn)了啟發(fā)式函數(shù)，通過計算當(dāng)前節(jié)點距離最近障礙物的距離，來綜合評判當(dāng)前節(jié)點的總代價值，使得規(guī)劃出的路徑遠(yuǎn)離障礙物，以保證無人機在飛行過程中更安全。

8、此外，申請?zhí)枮閏n202211534316x、名稱為一種基于astar算法的跨海物流無人機路徑規(guī)劃方法的中國專利申請，除了計算當(dāng)前節(jié)點的航程距離代價外，還充分考慮不同地圖區(qū)域的人口危險度代價、高度變化代價等，通過加權(quán)融合不同的代價來計算總代價值，以解決無人機運輸過程中的飛行安全問題。

9、還有一些方法，通過當(dāng)前節(jié)點與目標(biāo)節(jié)點的距離，動態(tài)調(diào)整代價函數(shù)和啟發(fā)式函數(shù)的權(quán)值，來加速收斂，減少不必要的往返搜索。

10、可以看出，現(xiàn)有的a星算法，都只用單一距離式來計算啟發(fā)式函數(shù)，導(dǎo)致搜索節(jié)點的效率低，整個路徑規(guī)劃過程收斂速度慢、計算耗時高；另外，不同的距離具有不同的特點和作用，例如曼哈頓距離可以加速垂直和水平方向的收斂速度、歐氏距離可以獲得最短直線飛行距離、切比雪夫距離則對斜對角方向的路徑規(guī)劃友好。但是，無人機作業(yè)場景是多變和和復(fù)雜性，采用單一距離式來計算啟發(fā)式函數(shù)難以滿足路徑規(guī)劃的需求。

11、此外，現(xiàn)有的a星算法獲得的路徑雖然在物理距離上是最短路徑，但是路徑轉(zhuǎn)折點多，不符合無人機飛行的運動學(xué)特征，過多的轉(zhuǎn)折點極大地增加無人機飛行能耗，對無人機的續(xù)航能力造成較大影響。同時，基于二維柵格地圖的環(huán)境建模，a星算法只考慮基于距離計算的啟發(fā)式函數(shù)，導(dǎo)致路徑存在緊貼障礙物和路徑經(jīng)過障礙物拐角等現(xiàn)象，這對無人機的飛行安全是極大的挑戰(zhàn)，主要表現(xiàn)在兩個方面：一是沒有考慮無人機本身的物理大小，將無人機作為無尺寸的粒子；二是沒有考慮定位誤差問題，無人機在開放環(huán)境作業(yè)易受到電磁干擾，需留有一定的定位誤差冗余。

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

1、發(fā)明目的：為了克服現(xiàn)有技術(shù)中存在的不足，本發(fā)明提供一種基于多距離約束的無人機路徑規(guī)劃方法，用以解決在復(fù)雜的城市環(huán)境下無人機飛行路徑規(guī)劃問題，該方法基于城市的三維地理信息數(shù)據(jù)，能夠快速規(guī)劃出最優(yōu)飛行路徑，同時能夠有效避開城市高樓等障礙物，實現(xiàn)無人機的安全飛行。

2、技術(shù)方案：為實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案為：

3、一種基于多距離約束的無人機路徑規(guī)劃方法，在a星算法的基礎(chǔ)上引入多距離約束方法進(jìn)行無人機路徑規(guī)劃；所述多距離約束方法用于計算當(dāng)前節(jié)點與鄰域節(jié)點的總代價函數(shù)，將總代價函數(shù)最小的鄰域節(jié)點作為規(guī)劃路徑，多距離約束方法采用的總代價函數(shù)為：

4、f(n)＝g(n)+h(n)+d(n)

5、其中：起始節(jié)點坐標(biāo)為(xs,ys)，當(dāng)前節(jié)點坐標(biāo)為(xn,yn)，目標(biāo)節(jié)點坐標(biāo)為(xe,ye)；f(n)為總代價函數(shù)，表示由起始節(jié)點途徑當(dāng)前節(jié)點并到達(dá)目標(biāo)節(jié)點的總代價值；g(n)為代價函數(shù)，表示由起始節(jié)點到達(dá)當(dāng)前節(jié)點的代價值；h(n)為啟發(fā)式函數(shù)，表示由當(dāng)前節(jié)點到達(dá)目標(biāo)節(jié)點的代價值；d(n)為懲罰項，表示當(dāng)前節(jié)點的鄰域占用懲罰；

6、使用起始節(jié)點到當(dāng)前節(jié)點的最短移動步長來計算代價函數(shù)g(n)，移動一個柵格表示距離增減數(shù)值1，不考慮不同方向的影響；

7、通過融合加權(quán)曼哈頓距離dist1和歐氏距離dist2的方式計算啟發(fā)式函數(shù)h(n)：

8、h(n)＝ω1×dist1+ω2×dist2

9、dist1＝|xe-xn|+|ye-yn|

10、

11、其中：ω1和ω2分別為曼哈頓距離和歐氏距離的權(quán)重系數(shù)，本案設(shè)置ω1＝4、ω2＝1；

12、懲罰項d(n)通過下式計算：

13、

14、其中：inf表示取數(shù)值上的極大值；r表示當(dāng)前節(jié)點的鄰域范圍，若選擇領(lǐng)域范圍為1個步長時，r的取值為-1和1，也可以根據(jù)需要設(shè)置更大的步長；表示當(dāng)前節(jié)點(xn,yn)的鄰域范圍中某個鄰域節(jié)點(xn+i,yn+j)的占用狀態(tài)，若為占用狀態(tài)，則若為空閑狀態(tài)，則

15、綜合以上，本案中的多距離約束評價函數(shù)可表示為：

16、

17、本發(fā)明在a星算法的基礎(chǔ)上引入多距離約束方法，能夠提高路徑搜索效率、減少路徑中的轉(zhuǎn)折點數(shù)量，同時解決無人機貼近障礙物飛行增加飛行風(fēng)險問題，實現(xiàn)高效且安全的無人機路徑規(guī)劃。

18、具體的，將無人機任務(wù)區(qū)域的三維物理空間切分為一個個緊密排列的三維柵格，每個柵格作為一個節(jié)點，每個柵格的邊長為1個步長。

19、具體的，在無人機路徑規(guī)劃的一開始，先將無人機的起始節(jié)點提升至目標(biāo)節(jié)點高度，選擇該高度的柵格層作為無人機路徑規(guī)劃的二維柵格地圖，無人機采用多距離約束方法選擇下一個基準(zhǔn)節(jié)點，直至到達(dá)目標(biāo)節(jié)點；最后，保留同一直線上的首尾兩個基準(zhǔn)節(jié)點作為關(guān)鍵節(jié)點，去除中間的冗余節(jié)點；調(diào)整起始節(jié)點高度過程中，默認(rèn)無人機路徑為直線。

20、具體的，通過相鄰基準(zhǔn)節(jié)點之間的斜率來去除冗余節(jié)點，在無人機選中的所有基準(zhǔn)節(jié)點中，從起始節(jié)點開始依次向后遍歷，將先后的三個相鄰基準(zhǔn)節(jié)點分別記為節(jié)點a、節(jié)點b和節(jié)點c，三個基準(zhǔn)節(jié)點的坐標(biāo)分別為(xa,ya)、(xb,yb)和(xc,yc)，分別計算相鄰基準(zhǔn)節(jié)點之間的斜率和若kab＝kbc，則節(jié)點b為冗余節(jié)點，舍去節(jié)點b，將節(jié)點c作為新的節(jié)點b，再向后遍歷一個基準(zhǔn)節(jié)點作為新的節(jié)點c，重復(fù)斜率計算和判斷，直至目標(biāo)節(jié)點；若kab≠kbc，則將節(jié)點b作為新的節(jié)點a，再向后遍歷兩個基準(zhǔn)節(jié)點作為新的節(jié)點b和新的節(jié)點c，重復(fù)斜率計算和判斷，直至目標(biāo)節(jié)點。

21、具體的，首先，設(shè)置開放列表openlist和封閉列表closedlist，將起始節(jié)點作為當(dāng)前節(jié)點加入到openlist中；然后，逐個判斷當(dāng)前節(jié)點步長為1的8個鄰域節(jié)點是否在closedlist中，若不在則判斷鄰域節(jié)點是否在openlist中，若不在則計算鄰域節(jié)點的總代價值并將鄰域節(jié)點加入到openlist中；接著，將當(dāng)前節(jié)點從openlist中移除，并作為新的基準(zhǔn)節(jié)點加入到closedlist中，判斷新的基準(zhǔn)節(jié)點是否與目標(biāo)節(jié)點重合；接著，從openlist中選取總代價值最小的節(jié)點作為新的當(dāng)前節(jié)點；重復(fù)至新的新基準(zhǔn)節(jié)點與目標(biāo)節(jié)點重合。

22、具體的，利用激光雷達(dá)點云數(shù)據(jù)，將城市場景構(gòu)建為三維柵格地圖；將無人機任務(wù)區(qū)域的三維物理空間切分為一個個緊密排列的三維柵格，將每個柵格作為一個節(jié)點；若有激光點落在某一柵格中，則將該柵格標(biāo)記為占用狀態(tài)，表示有障礙物，不能飛行；反之，則將該柵格標(biāo)記為空閑狀態(tài)，表示沒有障礙物，可以安全飛行。

23、具體的，該方法包括如下步驟：

24、step1、利用激光雷達(dá)點云數(shù)據(jù)，將城市場景構(gòu)建為三維柵格地圖；將無人機任務(wù)區(qū)域的三維物理空間切分為一個個緊密排列的三維柵格，將每個柵格作為一個節(jié)點；若有激光點落在某一柵格中，則將該柵格標(biāo)記為占用狀態(tài)，表示有障礙物，不能飛行；反之，則將該柵格標(biāo)記為空閑狀態(tài)，表示沒有障礙物，可以安全飛行；

25、step2、將三維物理空間中的無人機路徑規(guī)劃問題簡化到二維平面；對于起始節(jié)點和目標(biāo)節(jié)點在不同高度的情況，先將起始節(jié)點的高度調(diào)整至與目標(biāo)節(jié)點的高度一致，然后在該高度對應(yīng)的二維柵格地圖上進(jìn)行無人機路徑規(guī)劃，將三維物理空間中每個節(jié)點的27個搜索方向縮減至8個搜索方向，減少路徑規(guī)劃的計算復(fù)雜度；

26、step3、初始化開放列表openlist和封閉列表closedlist；

27、step4、將起始節(jié)點作為當(dāng)前節(jié)點加入到openlist中；

28、step5、逐個判斷當(dāng)前節(jié)點步長為1的8個鄰域節(jié)點是否在closedlist中，若不在，則判斷鄰域節(jié)點是否在openlist中，若不在則計算鄰域節(jié)點的總代價值并將鄰域節(jié)點加入到openlist中；

29、step6、將當(dāng)前節(jié)點從openlist中移除，并作為新的基準(zhǔn)節(jié)點加入到closedlist中，判斷新的基準(zhǔn)節(jié)點是否與目標(biāo)節(jié)點重合：若重合，則進(jìn)入步驟step8，否則，進(jìn)入步驟step7；

30、step7、從openlist中選取總代價值最小的節(jié)點作為新的當(dāng)前節(jié)點，返回step5；

31、step8、通過相鄰基準(zhǔn)節(jié)點之間的斜率來去除closedlist中的冗余節(jié)點，保留同一直線上的首尾兩個基準(zhǔn)節(jié)點作為關(guān)鍵節(jié)點，得到最終的無人機路徑規(guī)劃結(jié)果。

32、有益效果：本發(fā)明提供的基于多距離約束的無人機路徑規(guī)劃方法，相對于現(xiàn)有技術(shù)，具有如下優(yōu)勢：1、本發(fā)明通過設(shè)計加權(quán)融合不同距離式改進(jìn)a星路徑規(guī)劃方法的啟發(fā)函數(shù)，充分利用曼哈頓距離在水平和垂直方向的收斂速度和歐式距離在路徑最短約束上的優(yōu)勢，保證了規(guī)劃路徑最短的同時，提升了路徑規(guī)劃的計算速度，并同步減少路徑的轉(zhuǎn)折點；2、本發(fā)明通過對評價函數(shù)引入鄰域占用懲罰項，使得一定鄰域范圍出現(xiàn)障礙物的節(jié)點的總代價值變得很大，a星路徑規(guī)劃方法每次尋路迭代都是選擇總代價值最小的節(jié)點，這樣就有效抑制了路徑通過障礙物附近區(qū)域的可能性，進(jìn)一步保障了無人機飛行安全；3、本發(fā)明是以a星路徑規(guī)劃方法為基礎(chǔ)，提出的多距離加權(quán)融合的啟發(fā)函數(shù)，本發(fā)明選擇了曼哈頓距離和歐氏距離，若選擇切比雪夫距離和歐氏距離也有類似的提升效果。