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

高九州,徐威峰

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

0 引言

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

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

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

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

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

1.1 建模處理

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

1.2 A星算法

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

F(n)評(píng)價(jià)函數(shù)的公式為:

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

(1)

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

歐幾里得距離公式、曼哈頓距離公式和切比雪夫距離公式為計(jì)算G(n)、H(n)的3種計(jì)算方法。該文章采用歐幾里得距離公式,其表達(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)信息,通過比較計(jì)算每個(gè)擴(kuò)展節(jié)點(diǎn)的評(píng)價(jià)函數(shù)值,選擇代價(jià)函數(shù)值最小的一個(gè)節(jié)點(diǎn)作為下個(gè)步驟搜索的父節(jié)點(diǎn),依次查找直至找到目標(biāo)點(diǎn),形成搜索路徑。路徑的優(yōu)劣則主要依賴于評(píng)價(jià)函數(shù)的設(shè)計(jì),因?yàn)槿绻阉鞔鷥r(jià)函數(shù)值每一步都是最小的,那么就能確保搜尋出來的路徑也是最優(yōu)的。但是,A星算法在運(yùn)行中也存在一些未考慮的問題。

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

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

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

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

2.1 路徑安全設(shè)計(jì)

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

如圖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個(gè)柵格單位,故B擴(kuò)展子節(jié)點(diǎn)時(shí)不允許其擴(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四個(gè)方向中任意節(jié)點(diǎn)為障礙點(diǎn)時(shí),依次不允許生成0、2節(jié)點(diǎn);2、4節(jié)點(diǎn);4、6節(jié)點(diǎn);6、5節(jié)點(diǎn)。通過此方法設(shè)計(jì),防止無人機(jī)撞機(jī)。

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

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

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

2.2 評(píng)價(jià)函數(shù)優(yōu)化

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

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

對(duì)障礙率Q求對(duì)數(shù),當(dāng)柵格地圖中該環(huán)境中的障礙率比較小時(shí),可以直接向目標(biāo)點(diǎn)所在方向進(jìn)行搜索,讓該算法的評(píng)價(jià)函數(shù)H(n)適當(dāng)增大,提高該空間地圖環(huán)境中目標(biāo)點(diǎn)的方向性;當(dāng)該柵格地圖環(huán)境中障礙率比較大時(shí),不能再進(jìn)行增大評(píng)價(jià)函數(shù)H(n)的做法,如果增大太多讓其只向目標(biāo)點(diǎn)方向搜尋,會(huì)讓算法陷入局部最優(yōu),同時(shí)使其距離代價(jià)變大,導(dǎo)致最優(yōu)路徑變的難以搜索。因此,當(dāng)柵格地圖環(huán)境中障礙物比較多,障礙率比較大時(shí),應(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在這個(gè)建模環(huán)境內(nèi)所占比重。

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

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

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

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

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

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

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

2.3 路徑簡(jiǎn)化處理

當(dāng)使用傳統(tǒng)A星算法時(shí)生成的路徑為A—B—D—F—G,由于只能沿著柵格點(diǎn)依次形成避障路徑,因此會(huì)存在大量的冗余點(diǎn)和轉(zhuǎn)折點(diǎn)。本文結(jié)合Floyd算法將傳統(tǒng)A星算法中一些不必要的多余點(diǎn)和轉(zhuǎn)折點(diǎn)進(jìn)行簡(jiǎn)化刪除,縮短路徑的長(zhǎng)度,對(duì)其生成的航跡進(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)折且長(zhǎng)?？蓪⑵浜?jiǎ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則更加簡(jiǎn)短,更合理,符合航跡安全規(guī)則。故舍棄航跡A—C—D—G,選擇較合理的航跡A—E—D—G。

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

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

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

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

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

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

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

4 結(jié)束語

本文在傳統(tǒng)A星算法的基礎(chǔ)上,通過對(duì)無人機(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è)計(jì)的方法,保證障礙物坐標(biāo)點(diǎn)與生成的航跡距離不小于設(shè)定的安全界限。

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

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