基于改進(jìn)A星算法的無人機(jī)自主避障路徑規(guī)劃

高九州,徐威峰

吉林建筑大學(xué),吉林 長春 130118

0 引言

近年來,無人機(jī)技術(shù)發(fā)展越來越成熟,無人機(jī)具有使用方便、能夠執(zhí)行復(fù)雜且危險的任務(wù)、生存能力較強(qiáng)的特點(diǎn)。工業(yè)、物流和救援等領(lǐng)域,因越來越多的無人機(jī)加入,其工作效率得到顯著提高。無人機(jī)的使用涉及很多技術(shù),其中關(guān)鍵的一個技術(shù)問題就是路徑規(guī)劃。在任務(wù)空間的大環(huán)境不變的情況下,當(dāng)確定無人機(jī)起始點(diǎn)和目標(biāo)點(diǎn)之后,無人機(jī)避障路徑規(guī)劃問題的研究主要集中于路徑最短、耗能最少、路徑規(guī)劃時間最小等。通過對任務(wù)環(huán)境數(shù)據(jù)采集、分析、計算與比較,使用相關(guān)的避障算法規(guī)劃出安全、最優(yōu)和無碰撞的路徑[1]。

路徑規(guī)劃問題是國內(nèi)外學(xué)者研究的熱點(diǎn),無人機(jī)自主避障路徑規(guī)劃分為二維和三維2種情況[2],本文主要研究二維平面情況下的路徑規(guī)劃問題。避障路徑的規(guī)劃算法分為局部動態(tài)路徑規(guī)劃算法和全局靜態(tài)路徑規(guī)劃算法[3]。局部動態(tài)路徑規(guī)劃算法是指無人機(jī)在飛行過程中不知道周邊環(huán)境信息[4],在飛行中可能遇到變化的環(huán)境信息,只能依靠無人機(jī)上自帶的一些傳感器來感知飛行環(huán)境周邊信息,根據(jù)周邊環(huán)境的實(shí)時情況建立動態(tài)模型,再通過CPU進(jìn)行分析、計算規(guī)劃出一條避開障礙物的最優(yōu)路徑,是一種邊飛邊設(shè)計的航線。全局靜態(tài)路徑規(guī)劃算法是指無人機(jī)飛行前已知周邊環(huán)境,通過對該飛行范圍環(huán)境的建模,然后以一些約束和標(biāo)準(zhǔn),規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)繞過障礙物的最優(yōu)路徑[5]。

在全局靜態(tài)路徑規(guī)劃算法中A星算法是現(xiàn)在使用最多的路徑規(guī)劃算法之一。A星算法是對Dijkstra算法和貪心算法的借鑒與改進(jìn),在1968年由美國研究者提出,是一種基于啟發(fā)式搜索策略兼顧搜索效率和最優(yōu)航跡的算法,該算法改變了Dijkstra算法中無序搜索的情況,因此該算法成為柵格法建模路徑規(guī)劃的主要算法之一[6]。但A星算法存在搜索節(jié)點(diǎn)過多、搜索效率低、規(guī)劃出來的路徑轉(zhuǎn)折點(diǎn)和冗余點(diǎn)較多的問題。

本文提出一種改進(jìn)型A星算法。首先,該改進(jìn)型A星算法充分考慮無人機(jī)本身的物理信息問題,對傳統(tǒng)A星算法的子節(jié)點(diǎn)擴(kuò)展規(guī)則進(jìn)行改進(jìn),保證無人機(jī)在飛行過程中與障礙物始終保持一種安全距離,防止撞機(jī)。其次,對傳統(tǒng)A星算法的評價函數(shù)進(jìn)行改進(jìn),提出了一種定義障礙率加權(quán)的啟發(fā)函數(shù),從而減少搜索節(jié)點(diǎn)和搜索區(qū)域,節(jié)約搜索時間,提高搜索效率。最后,結(jié)合Floyd算法原理,刪除了一些多余的節(jié)點(diǎn),航跡中的轉(zhuǎn)折變少,使轉(zhuǎn)折角變得平緩,使得航跡得到改善,保證無人機(jī)行駛安全、平穩(wěn)、省時。

1 傳統(tǒng)A星算法

1.1 建模處理

地圖建模的方法主要包括柵格法、拓?fù)浞ā⒖梢晥D法等[7-9],其中,將環(huán)境二維或三維地圖根據(jù)柵格法進(jìn)行分割,單位長度的柵格由實(shí)際的環(huán)境情況決定,比如無人機(jī)自身的一些物理信息。二維或三維靜態(tài)環(huán)境中的障礙物信息用柵格法能被有效展示,因此本文使用MATLAB 2020b中的柵格法構(gòu)建二維環(huán)境地圖。

1.2 A星算法

A星算法汲取了Dijkstra算法與貪心算法的優(yōu)點(diǎn)[9],核心是計算其擴(kuò)展節(jié)點(diǎn)的代價函數(shù),在擴(kuò)展過程中,不斷用代價值最小的節(jié)點(diǎn)作為子節(jié)點(diǎn)[10],并且以該子節(jié)點(diǎn)作為當(dāng)前的父節(jié)點(diǎn)進(jìn)行擴(kuò)展與搜尋下個過程的子節(jié)點(diǎn),將代價函數(shù)最小的子節(jié)點(diǎn)依次尋找出來,形成優(yōu)化航跡。

F(n)評價函數(shù)的公式為:

F(n)=G(n)+H(n)

(1)

式中:G(n)為實(shí)際路徑代價值從起點(diǎn)到當(dāng)前點(diǎn);H(n)為估計路徑代價值從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)。

歐幾里得距離公式、曼哈頓距離公式和切比雪夫距離公式為計算G(n)、H(n)的3種計算方法。該文章采用歐幾里得距離公式,其表達(dá)式為:

(2)

(3)

式中:(xi,yi)為當(dāng)前節(jié)點(diǎn);(xs,ys)為起點(diǎn);(xt,yt)為目標(biāo)點(diǎn)。

A星算法實(shí)質(zhì)是擴(kuò)展搜索過程中不斷更新開和閉列表中的節(jié)點(diǎn)及其相關(guān)信息,通過比較計算每個擴(kuò)展節(jié)點(diǎn)的評價函數(shù)值,選擇代價函數(shù)值最小的一個節(jié)點(diǎn)作為下個步驟搜索的父節(jié)點(diǎn),依次查找直至找到目標(biāo)點(diǎn),形成搜索路徑。路徑的優(yōu)劣則主要依賴于評價函數(shù)的設(shè)計,因?yàn)槿绻阉鞔鷥r函數(shù)值每一步都是最小的,那么就能確保搜尋出來的路徑也是最優(yōu)的。但是,A星算法在運(yùn)行中也存在一些未考慮的問題。

1)在柵格地圖的二維空間中搜索方法為擴(kuò)展8節(jié)點(diǎn)的,在路徑搜索過程中算法搜索節(jié)點(diǎn)數(shù)量太多,因?yàn)槊總€節(jié)點(diǎn)的評價函數(shù)值都需要計算,這樣每次計算將導(dǎo)致計算量過大,需要較長的計算時間,使其搜索效率低下。

2)該算法在進(jìn)行航跡規(guī)劃時,會產(chǎn)生多余點(diǎn)、轉(zhuǎn)折點(diǎn)過多的情況,導(dǎo)致飛行路徑有較多的轉(zhuǎn)折,不能保證無人機(jī)的平穩(wěn)飛行。

3)進(jìn)行航跡規(guī)劃時,該算法僅僅將障礙物節(jié)點(diǎn)信息考慮在內(nèi),并沒有將無人機(jī)自身物理信息考慮在內(nèi),未考慮可能發(fā)生撞機(jī)時,無人機(jī)路徑不能以柵格對角線的形式斜穿障礙物頂點(diǎn)的情況。

2 改進(jìn)型A星算法

2.1 路徑安全設(shè)計

本文對傳統(tǒng)A星算法進(jìn)行了路徑安全改進(jìn)設(shè)計,以二維空間環(huán)境為例,為防止撞機(jī)設(shè)置安全距離為1個柵格單位,如果存在障礙物節(jié)點(diǎn)信息在當(dāng)前節(jié)點(diǎn)的左/右、上/下方向上,則不使用以對角線形式生成路徑上的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)的擴(kuò)展子節(jié)點(diǎn),所以路徑距離障礙物小于1個柵格單位安全距離的對角線不能生成航跡。

如圖1所示,假設(shè)航點(diǎn)為A—B—C—D—E—F—G—H,B點(diǎn)上側(cè)和E點(diǎn)左側(cè)存在障礙節(jié)點(diǎn),傳統(tǒng)A星算法的最優(yōu)擴(kuò)展節(jié)點(diǎn)為A—B—D—E—G—H,但是,該路徑距障礙點(diǎn)(3,7)和(3,3)的距離小于1個柵格單位,故B擴(kuò)展子節(jié)點(diǎn)時不允許其擴(kuò)展子節(jié)點(diǎn)C,E不允許其擴(kuò)展子節(jié)點(diǎn)G,最佳子節(jié)點(diǎn)依次應(yīng)為A—B—C—D—E—F—G—H。

圖1 優(yōu)化擴(kuò)展子節(jié)點(diǎn)方式

如圖2所示,節(jié)點(diǎn)A的1、3、5、7四個方向中任意節(jié)點(diǎn)為障礙點(diǎn)時,依次不允許生成0、2節(jié)點(diǎn);2、4節(jié)點(diǎn);4、6節(jié)點(diǎn);6、5節(jié)點(diǎn)。通過此方法設(shè)計,防止無人機(jī)撞機(jī)。

圖2 優(yōu)化擴(kuò)展子節(jié)點(diǎn)示意

圖3為路徑安全設(shè)計后的航跡。該路徑安全設(shè)計對路徑長度有略微增加,但能保證其形成航跡的安全性,防止撞機(jī)。

圖3 優(yōu)化擴(kuò)展子節(jié)點(diǎn)生成的航跡

2.2 評價函數(shù)優(yōu)化

傳統(tǒng)A星算法以子節(jié)點(diǎn)的評價函數(shù)值為核心進(jìn)行啟發(fā)式搜索時,會在OPEN列表中不斷計算搜尋總代價值最小的節(jié)點(diǎn),這將造成傳統(tǒng)算法不斷進(jìn)行往返搜索,大大增加了算法的計算時間,使其效率低下。

定義障礙率Q為在起點(diǎn)與目標(biāo)點(diǎn)組成的局部環(huán)境中,障礙物柵格頂點(diǎn)坐標(biāo)的個數(shù)與該范圍內(nèi)局部柵格地圖坐標(biāo)點(diǎn)個數(shù)之比。起點(diǎn)與目標(biāo)點(diǎn)組成的局部柵格地圖內(nèi)的障礙物頂點(diǎn)個數(shù)為M,起點(diǎn)坐標(biāo)(xs,ys)、目標(biāo)點(diǎn)坐標(biāo)(xt,yt),表達(dá)式如下所示。

對障礙率Q求對數(shù),當(dāng)柵格地圖中該環(huán)境中的障礙率比較小時,可以直接向目標(biāo)點(diǎn)所在方向進(jìn)行搜索,讓該算法的評價函數(shù)H(n)適當(dāng)增大,提高該空間地圖環(huán)境中目標(biāo)點(diǎn)的方向性;當(dāng)該柵格地圖環(huán)境中障礙率比較大時,不能再進(jìn)行增大評價函數(shù)H(n)的做法,如果增大太多讓其只向目標(biāo)點(diǎn)方向搜尋,會讓算法陷入局部最優(yōu),同時使其距離代價變大,導(dǎo)致最優(yōu)路徑變的難以搜索。因此,當(dāng)柵格地圖環(huán)境中障礙物比較多,障礙率比較大時,應(yīng)適當(dāng)調(diào)整啟發(fā)函數(shù)的值,讓其適當(dāng)減小,從而增大搜尋的范圍,通過適當(dāng)降低該算法的搜尋速度,提高搜尋的精度,以獲得最優(yōu)的航線。

F(n)=G(n)+βH(n)

β=1-lnP

式中:β為權(quán)重值;P代表障礙點(diǎn)Q在這個建模環(huán)境內(nèi)所占比重。

與傳統(tǒng)A星算法相比,改進(jìn)后的算法其路徑長度、搜尋節(jié)點(diǎn)數(shù)比較如表1所示。改進(jìn)評價函數(shù)A星算法的對比仿真如圖4～7所示。

表1 不同障礙率節(jié)點(diǎn)數(shù)與路徑長度對比

圖4 障礙物多時啟發(fā)函數(shù)未加權(quán)前A星算法路徑

圖5 障礙物多時改進(jìn)啟發(fā)函數(shù)加權(quán)為β1后A星算法路徑

圖6 障礙物少時啟發(fā)函數(shù)未加權(quán)前A星算法路徑

圖7 障礙物少時改進(jìn)啟發(fā)函數(shù)加權(quán)為β2后A星算法路徑

由表1可知,對該評價函數(shù)進(jìn)行加權(quán)后,當(dāng)障礙物比較少時往返搜索的節(jié)點(diǎn)少了近65%,閉環(huán)節(jié)點(diǎn)少了52%,顯著減少了該算法往返一些節(jié)點(diǎn)無用搜尋的次數(shù),顯著提高算法的搜尋效率。當(dāng)障礙物比較多時往返搜索的節(jié)點(diǎn)少了近40%,閉環(huán)節(jié)點(diǎn)少了28%,在保證其搜索精度的前提下,即減少了往返無用搜索的次數(shù),也提高了算法的計算效率。

2.3 路徑簡化處理

當(dāng)使用傳統(tǒng)A星算法時生成的路徑為A—B—D—F—G,由于只能沿著柵格點(diǎn)依次形成避障路徑,因此會存在大量的冗余點(diǎn)和轉(zhuǎn)折點(diǎn)。本文結(jié)合Floyd算法將傳統(tǒng)A星算法中一些不必要的多余點(diǎn)和轉(zhuǎn)折點(diǎn)進(jìn)行簡化刪除,縮短路徑的長度,對其生成的航跡進(jìn)行優(yōu)化處理,使航跡的轉(zhuǎn)折角變得平滑,更有利于保證無人機(jī)飛行的連續(xù)性和安全性。

Floyd算法的基本原理如圖8所示,傳統(tǒng)A星算法規(guī)劃出的路徑為A—B—D—F—G,即每一步只能沿著柵格的正/負(fù)、上/下或?qū)蔷€方向規(guī)劃,但由圖可知,節(jié)點(diǎn)B顯然是多余的,即A—B—D的航跡不太合理,該航跡有轉(zhuǎn)折且長。可將其簡化為航跡A—C—D—G或航跡A—E—D—G,比較航跡A—C—D—G或A—E—D—G。在Floyd算法下,航跡A—D—G航跡雖然最短,但與障礙點(diǎn)距離小于安全距離,不合理,因此舍棄。航跡A—E—D—G則更加簡短,更合理,符合航跡安全規(guī)則。故舍棄航跡A—C—D—G,選擇較合理的航跡A—E—D—G。

圖8 Floyd簡化航跡規(guī)劃原理

通過Floyd算法優(yōu)化,刪除了一些多余的節(jié)點(diǎn),航跡中的轉(zhuǎn)折變少,使轉(zhuǎn)折角變得緩,使得航跡得到改善,保證無人機(jī)飛行過程比較安全、平穩(wěn)、省時。

A星算法優(yōu)化后的流程如圖9所示。

圖9 改進(jìn)A星算法流程

3 改進(jìn)A星算法的仿真和分析

在傳統(tǒng)A星算法航跡規(guī)劃的基礎(chǔ)上,對其進(jìn)行簡化路徑長度、路徑安全設(shè)計、評價函數(shù)加權(quán)優(yōu)化,然后在MATLAB 2020b中進(jìn)行仿真測試與分析。

當(dāng)起點(diǎn)坐標(biāo)為(11,1)、終點(diǎn)坐標(biāo)為(35,34)時,該柵格地圖環(huán)境中設(shè)置7組障礙物。圖10是未改進(jìn)評價函數(shù)的航跡圖,實(shí)線是在路徑安全設(shè)計下的A星算法航跡,路徑安全設(shè)計和Floyd簡化航跡的設(shè)計下的航跡由虛線表示。對評價函數(shù)進(jìn)行優(yōu)化的航跡圖如圖11所示,在路徑安全設(shè)計和優(yōu)化評價函數(shù)條件下的A星算法航跡由實(shí)線表示,在路徑安全設(shè)計、改進(jìn)評價函數(shù)和結(jié)合Floyd簡化航跡的設(shè)計下A星算法航跡由虛線表示。比較圖10和圖11可知,沒有進(jìn)行評價函數(shù)優(yōu)化的航跡與進(jìn)行評價函數(shù)優(yōu)化的條件下航跡步數(shù)比較都為63,航跡步數(shù)一致。但是,在沒有進(jìn)行評價函數(shù)優(yōu)化的搜索節(jié)點(diǎn)數(shù)量達(dá)到550個,進(jìn)行評價函數(shù)優(yōu)化后的搜索節(jié)點(diǎn)數(shù)量只有239個,其搜尋數(shù)量顯著減少,顯著提高了搜索效率。

4 結(jié)束語

本文在傳統(tǒng)A星算法的基礎(chǔ)上,通過對無人機(jī)避障航跡規(guī)劃的問題進(jìn)行優(yōu)化處理,得出一種改進(jìn)型A星算法。通過仿真驗(yàn)證了此改進(jìn)A星算法的無人機(jī)自主避障航跡規(guī)劃的合理性和有效性。改進(jìn)型A星算法具有以下特點(diǎn)。

1)將無人機(jī)本身的物理信息考慮在內(nèi),進(jìn)行路徑安全設(shè)計的方法,保證障礙物坐標(biāo)點(diǎn)與生成的航跡距離不小于設(shè)定的安全界限。

2)優(yōu)化評價函數(shù),進(jìn)行加權(quán)處理,降低了算法搜尋時間,搜索效率得到提高。

3)結(jié)合Floyd簡化航跡算法,優(yōu)化了改進(jìn)A星算法生成的避障航跡,在確保航跡符合無人機(jī)飛行安全界限時,刪除多余節(jié)點(diǎn),使規(guī)劃出來的航跡比較平緩,減少航跡中的轉(zhuǎn)折角,保證無人機(jī)從起點(diǎn)到終點(diǎn)的飛行安全平穩(wěn)、用時較少。