基于A星算法的無人機(jī)高速公路巡檢應(yīng)用研究

靳 朝,林富生,宋志峰,余聯(lián)慶

(武漢紡織大學(xué) 機(jī)械工程與自動(dòng)化學(xué)院,湖北 武漢 430200)

0 引言

目前,國(guó)內(nèi)高速公路巡檢主要是人工巡檢和固定式路測(cè)監(jiān)控設(shè)備巡檢兩種巡檢方式:人工巡檢費(fèi)時(shí)費(fèi)力,效率低下且巡檢工作人員有安全風(fēng)險(xiǎn);固定式路測(cè)監(jiān)控設(shè)備僅分布在個(gè)別重要路段,數(shù)量有限[1-3]。此兩種巡檢方法的低效率、高支出,難以應(yīng)對(duì)復(fù)雜地形及惡劣環(huán)境,缺乏即時(shí)反饋手段,已經(jīng)難以滿足日益加重的高速公路巡檢任務(wù)。對(duì)此,無人機(jī)技術(shù)作為一種新型技術(shù)可以擺脫上述困難,降低環(huán)境因素所造成的影響,保證巡查的效率與便捷。

1 無人機(jī)航跡規(guī)劃的實(shí)現(xiàn)原理

無人機(jī)的航跡規(guī)劃主要分為4個(gè)部分:初始化參數(shù)、構(gòu)建地圖、路徑搜索、路徑優(yōu)化,如圖1所示。

圖1 無人機(jī)航跡規(guī)劃技術(shù)流程

1.1 參數(shù)初始化

在進(jìn)行無人機(jī)自主飛行航跡規(guī)劃之前需要對(duì)無人機(jī)進(jìn)行結(jié)構(gòu)分析和數(shù)據(jù)估算來確定無人機(jī)的約束條件,其約束條件包括最大爬升角、最小步長(zhǎng)等。無人機(jī)的最大爬升角指無人機(jī)的航跡坐標(biāo)系X軸與水平面之間所能形成的最大夾角,最大爬升角影響的是無人機(jī)三維空間的節(jié)點(diǎn)劃分距離,每一個(gè)三維立體節(jié)點(diǎn)之間所形成的爬升角必須要小于無人機(jī)的最大爬升角。無人機(jī)的最小步長(zhǎng)是指在當(dāng)前飛行環(huán)境中無人機(jī)改變飛行狀態(tài)所需要飛行的最小直線距離,無人機(jī)的最小步長(zhǎng)影響的是無人機(jī)二維空間的節(jié)點(diǎn)劃分距離,二維節(jié)點(diǎn)間的最短距離不得小于無人機(jī)的最小步長(zhǎng)。為了簡(jiǎn)化空間模型,將三維的空間轉(zhuǎn)化為二維空間,消除無人機(jī)最大爬升角的約束,所有避障方式全部選擇繞飛。

1.2 構(gòu)建地圖

因?yàn)椴捎美@飛的避障策略,所有Z軸方向上的障礙物都可以投影在俯視的XY平面上,在進(jìn)行空間模型簡(jiǎn)化后,得到的就是一張二維的空間分布圖。為了更加精確地模擬無人機(jī)的飛行,使用柵格法構(gòu)建了一個(gè)30 m×30 m的二維空間地圖;再根據(jù)無人機(jī)本身的俯視投影面積大小,來確定無人機(jī)的工作區(qū)間大小,以確保無人機(jī)有足夠的飛行空間。

1.3 路徑搜索

路徑搜索的基本原理就是通過將所需飛行區(qū)域簡(jiǎn)化為方形柵格,方形柵格也就成為了一個(gè)二維數(shù)組,每一個(gè)柵格就是二維數(shù)組的一個(gè)元素,而這些元素都帶有標(biāo)記,每一個(gè)元素都是一個(gè)節(jié)點(diǎn),每一個(gè)節(jié)點(diǎn)都帶有可到達(dá)或不可到達(dá)的標(biāo)識(shí)。在設(shè)定好起始點(diǎn)和目標(biāo)點(diǎn)后,無人機(jī)會(huì)根據(jù)自身所帶的攝像模塊識(shí)別每一個(gè)節(jié)點(diǎn)所攜帶的標(biāo)識(shí),最終再串聯(lián)帶有可到達(dá)標(biāo)識(shí)的節(jié)點(diǎn),形成一條路徑最終到達(dá)目標(biāo)點(diǎn)。

2 基于A?(A星)算法的航跡路徑規(guī)劃無人機(jī)

A?航跡搜尋算法是一種作用于搜索區(qū)域的啟發(fā)式算法[4],A?航跡搜索算法結(jié)合于Dijkstra算法和BFS算法,所以它兼具兩種算法的優(yōu)點(diǎn),不僅能夠搜索到最短路徑,而且可以使用啟發(fā)函數(shù)。A?航跡搜尋算法在規(guī)劃航跡時(shí)會(huì)對(duì)每一個(gè)節(jié)點(diǎn)進(jìn)行估值,即:

式中:F(n)指最后經(jīng)過n個(gè)節(jié)點(diǎn)所得到的路徑;G(n)指從起點(diǎn)出發(fā)到達(dá)第n個(gè)節(jié)點(diǎn)所付出的移動(dòng)代價(jià);H(n)指從第n個(gè)節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)的估算成本。

從起點(diǎn)開始A?算法會(huì)建立一個(gè)庫(kù),這個(gè)庫(kù)被稱為open list。open list作為一個(gè)待檢查的清單,從起點(diǎn)開始每一個(gè)經(jīng)過的節(jié)點(diǎn)周圍的所有可到達(dá)節(jié)點(diǎn)都會(huì)進(jìn)入這個(gè)庫(kù),反復(fù)遍歷open list查找F(n)最小值的點(diǎn),再把它當(dāng)做處理點(diǎn),將周圍的可到達(dá)節(jié)點(diǎn)納入open list,這個(gè)處理點(diǎn)會(huì)被納入另一個(gè)庫(kù)close list,當(dāng)節(jié)點(diǎn)進(jìn)入close list后就不會(huì)再被當(dāng)做處理點(diǎn),重復(fù)上述過程,一直到把目標(biāo)點(diǎn)納入open list,即得到一條最優(yōu)的路徑,如圖2所示。

圖2 A?算法示意圖

圖2中,灰色圓點(diǎn)代表起點(diǎn),灰色三角形代表終點(diǎn),黑色填充方格代表障礙物,小方塊填充方格代表待搜索的open list,交叉線填充方格代表已經(jīng)搜索過的close list,灰色填充方格代表當(dāng)前搜索過的路徑。

得到路徑的過程即估算成本取決于H(n)所使用的啟發(fā)函數(shù),本文使用的是曼哈頓算法和歐幾里得啟發(fā)算式。

曼哈頓啟發(fā)函數(shù)為:

式中:D表示一個(gè)柵格的曼哈頓距離;c代表當(dāng)前節(jié)點(diǎn);goal代表目標(biāo)節(jié)點(diǎn)。

歐幾里得啟發(fā)函數(shù)為:

式中:D表示一個(gè)柵格的歐幾里得距離;c代表當(dāng)前節(jié)點(diǎn);goal代表目標(biāo)節(jié)點(diǎn)。

因?yàn)闅W幾里得距離接近兩點(diǎn)最短距離,所以得到的路徑為最短距離,同時(shí)結(jié)合曼哈頓啟發(fā)算法,可以保證路徑的穩(wěn)定性,計(jì)算后得到最優(yōu)航跡路線。

3 基于A?算法的無人機(jī)仿真實(shí)驗(yàn)

3.1 環(huán)境構(gòu)建與運(yùn)行仿真

為了保障算法可以應(yīng)用于大多數(shù)復(fù)雜場(chǎng)景下,在模擬場(chǎng)景中將會(huì)盡量多地構(gòu)建不同類型的障礙物。根據(jù)無人機(jī)的最小步長(zhǎng),將模擬空間平面圖構(gòu)建為一個(gè)30 m×30 m的地圖,每一個(gè)無人機(jī)無法到達(dá)的障礙點(diǎn)都用一個(gè)黑色柵格表示,其他無標(biāo)注區(qū)域均為無人機(jī)可活動(dòng)區(qū)域,灰色圓點(diǎn)代表無人機(jī)所在位置,灰色三角形代表無人機(jī)的目標(biāo)點(diǎn),如圖3所示。通過運(yùn)用卡爾曼數(shù)據(jù)融合技術(shù)對(duì)各傳感器的各項(xiàng)數(shù)據(jù)進(jìn)行整合,然后解算無人機(jī)的姿態(tài)。再運(yùn)用曼哈頓距離的啟發(fā)算法對(duì)G(n)進(jìn)行推算,同時(shí)通過路徑的改變不斷地計(jì)算出各點(diǎn)的F(n),確保無人機(jī)沿G(n)最小值的路徑進(jìn)行移動(dòng),最終得出最短路徑,如圖3所示。

3.2 路徑優(yōu)化

如果通過A?路徑算法搜索所得到的最優(yōu)航跡路線是一條直線,那么就不用對(duì)所得航跡進(jìn)行路徑優(yōu)化處理。但是在現(xiàn)實(shí)環(huán)境中飛行航跡不會(huì)這么理想化,因此需要對(duì)航跡進(jìn)行平滑處理,而要進(jìn)行平滑處理就需要消除無人機(jī)最大升角和最小步長(zhǎng)的影響。本文使用Savitzky-Golay濾波(SG濾波),SG濾波[5]基于局域多項(xiàng)式最小二乘法擬合的濾波方法,其擬合多項(xiàng)式為:

式中:p(n)為擬合多項(xiàng)式;ak為第k個(gè)數(shù)值;k為數(shù)據(jù)項(xiàng)數(shù)。

其最小二乘殘差為:

式中:εN為殘差期望值;x[n]為給定模型函數(shù)。

其運(yùn)算可以通過卷積運(yùn)算的方式實(shí)現(xiàn),算式如下:

式中:y[n]為濾波后的結(jié)果信號(hào);h[n]為濾波系數(shù);x[n]為輸入信號(hào);M表示濾波的抽頭數(shù);濾波階數(shù)為M-1。

再將所得航跡路線進(jìn)行平滑處理后得到的路徑如圖4所示。

圖4 平滑處理后的航跡路線簡(jiǎn)化圖

由圖4可以看出,由于對(duì)飛行航跡路線進(jìn)行了優(yōu)化處理,更加地符合無人機(jī)的飛行運(yùn)動(dòng)規(guī)律,同時(shí)因?yàn)閷?duì)每一個(gè)轉(zhuǎn)角的平滑處理使得無人機(jī)在貼近障礙物飛行時(shí)不會(huì)因?yàn)檗D(zhuǎn)折角度問題貼近障礙物的邊緣,故以此種尋跡方法進(jìn)行飛行會(huì)降低無人機(jī)飛行的碰撞危險(xiǎn)性。

4 結(jié)語

從上述實(shí)驗(yàn)可以看出,基于A?算法的無人機(jī)在較為復(fù)雜的環(huán)境中可以實(shí)現(xiàn)尋跡、避障等操作。針對(duì)基于傳統(tǒng)人工的巡查方式,本文通過分析高速公路的環(huán)境特征、巡查特點(diǎn),結(jié)合無人機(jī)技術(shù),對(duì)無人機(jī)在高速公路巡查的應(yīng)用進(jìn)行研究,提出無人機(jī)自主飛行高速公路巡查方案。