平滑改進(jìn)A*算法在無(wú)人機(jī)航跡規(guī)劃中的應(yīng)用

儲(chǔ)澤楠 趙凱 宋倍倍

摘? 要： 傳統(tǒng)的A*算法易于陷入局部極小，搜索速度較慢，規(guī)劃的路徑有較多折線。文章對(duì)傳統(tǒng)A*算法進(jìn)行了兩個(gè)改進(jìn)，并將其應(yīng)用于無(wú)人機(jī)航跡規(guī)劃。一是在代價(jià)函數(shù)中加入無(wú)人機(jī)的轉(zhuǎn)向代價(jià)估計(jì)，穩(wěn)定其飛行方向;二是采用后處理方法對(duì)航跡點(diǎn)進(jìn)行擬合，獲得平滑的最短路徑曲線。為保證擬合后的路徑不會(huì)出現(xiàn)較大波動(dòng)，擬合過(guò)程沒(méi)有使用所有的航跡點(diǎn)，而是在一定的誤差允許范圍內(nèi)選取特征點(diǎn)進(jìn)行擬合。改進(jìn)算法的有效性通過(guò)仿真和實(shí)驗(yàn)予以驗(yàn)證。

關(guān)鍵詞： A*算法; 曲線擬合; 無(wú)人機(jī); 后處理

中圖分類(lèi)號(hào)：TP399? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A? ? 文章編號(hào)：1006-8228（2020）02-54-04

Application of smooth improved A-star algorithm in UAV flight path planning

Chu Zenan1， Zhao Kai1， Song Beibei2

（1.Anyang Institute of Technology， 2.Anyang Quality and Technical Supervision， Inspection and Testing Center， Anyang， Henan 455000， China）

Abstract： Traditional A-star algorithm spends much time on searching optimal solution， and is easy to fall into local minima. Moreover， the planned path contains many polygonal lines. In this paper， two improvements are made to the traditional A-star algorithm， and it is applied to the path planning of UAV. One is to add the UAV steering cost estimation to the distance cost function to stabilize its flight direction; the other is to use the post processing method to fit the track points to obtain the smooth shortest path curve. In order to ensure that the fitted path will not fluctuate greatly， the fitting process does not use all the track points， but selects the feature points for fitting within a certain range of allowable error. The effectiveness of the improved algorithm is verified by simulation and experiment.

Key words： A-star algorithm; curve fitting; unmanned aerial vehicle; post processing method

0 引言

A*算法結(jié)構(gòu)簡(jiǎn)單，易于執(zhí)行，受到了人們的廣泛關(guān)注。唐平[1]等使用動(dòng)態(tài)二叉樹(shù)表示二維空間，對(duì)全局路徑規(guī)劃和局部路徑規(guī)劃進(jìn)行綜合考慮，設(shè)計(jì)了移動(dòng)機(jī)器人在復(fù)雜環(huán)境下對(duì)動(dòng)態(tài)障礙物進(jìn)行避障的A*算法。賈慶軒等[2]針對(duì)空間機(jī)械臂在軌操作任務(wù)需求，利用A*算法在空間機(jī)械臂的自由工作空間進(jìn)行無(wú)碰撞路徑搜索，實(shí)現(xiàn)了空間機(jī)械臂的避障路徑規(guī)劃。姚遠(yuǎn)等[3]提出了一種基于稀疏A*搜索算法預(yù)規(guī)劃和改進(jìn)人工勢(shì)場(chǎng)相結(jié)合的無(wú)人機(jī)動(dòng)態(tài)避障算法。該算法根據(jù)障礙物分布建立柵格化模型，采用稀疏A*搜索算法進(jìn)行全局航跡規(guī)劃。王殿軍[4]闡述了全局地圖構(gòu)建方法，根據(jù)移動(dòng)機(jī)器人的實(shí)際運(yùn)行環(huán)境，采用柵格法構(gòu)建了環(huán)境地圖，利用A*算法做初步路徑規(guī)劃。文中也指出了A*算法的不足，路徑規(guī)劃數(shù)據(jù)中包含了所有規(guī)劃點(diǎn)的坐標(biāo)，冗余點(diǎn)較多，且移動(dòng)機(jī)器人無(wú)法在拐點(diǎn)處調(diào)整自身姿態(tài)。

針對(duì)傳統(tǒng)A*算法的不足，很多研究人員提出了改進(jìn)型的A*算法。針對(duì)A*算法時(shí)間性能較差的問(wèn)題，熊壬浩等[5]提出了一種并行搜索和快速插入的算法。改進(jìn)的算法與原算法相比，大大節(jié)省了的搜索時(shí)間。Fu等[6]提出了一種改進(jìn)的A*算法，在進(jìn)行下一步搜索之前，該算法會(huì)直接使用當(dāng)前規(guī)劃出的無(wú)碰撞局部路徑。同時(shí)，在算法中引入了后處理模塊，通過(guò)把局部路徑取直的方法提高規(guī)劃路徑的光滑性。

在本文中，針對(duì)傳統(tǒng)A*算法搜索時(shí)間長(zhǎng)和容易出現(xiàn)局部冗余點(diǎn)較多的問(wèn)題，提出了一種平滑改進(jìn)的實(shí)時(shí)A*算法。首先對(duì)傳統(tǒng)的A*算法的代價(jià)函數(shù)作了改進(jìn)，加入了轉(zhuǎn)向的代價(jià)函數(shù)估計(jì)。采用后處理的方式對(duì)算法規(guī)劃出的最短航跡進(jìn)行擬合，擬合后的B樣條航跡具有三階連續(xù)導(dǎo)數(shù)，能夠保證航跡的光滑性，有利于后續(xù)的速度規(guī)劃和飛行控制。最后在二維和三維地圖上對(duì)提出的方案仿真，基于自制的無(wú)人機(jī)平臺(tái)對(duì)該方案進(jìn)行了實(shí)驗(yàn)測(cè)試。

1 A*算法回顧

A*算法是一種基于柵格的算法。自主運(yùn)動(dòng)設(shè)備被放置在均勻柵格的地圖中。設(shè)備需要自行規(guī)劃路徑，從給定的起點(diǎn)到達(dá)目標(biāo)地點(diǎn)。在這個(gè)過(guò)程中，通常要求路徑長(zhǎng)度是最短的。而且，規(guī)劃的路徑必須避開(kāi)地圖中的障礙物。為了簡(jiǎn)單起見(jiàn)，我們對(duì)地圖環(huán)境有如下假設(shè)。

假設(shè)1：在建立的坐標(biāo)系中，無(wú)人機(jī)所處的起始點(diǎn)的坐標(biāo)是已知的。

假設(shè)2：目標(biāo)地點(diǎn)可以是除去障礙物內(nèi)的任意位置，坐標(biāo)是已知的。

假設(shè)3：障礙物可以是柵格地圖上的任意位置，位置是已知的。

基于以上假設(shè)，典型的A*算法要在一個(gè)樹(shù)或圖表示的特定搜索空間中，找到最短路徑。這條路徑是由一系列的節(jié)點(diǎn)組成，連接這些節(jié)點(diǎn)即可找到最優(yōu)的路徑。首先，A*算法需要將所在的地圖劃分柵格，有障礙物的柵格用“滿(mǎn)”表示，無(wú)障礙的柵格用“空”表示。最優(yōu)路徑連接的節(jié)點(diǎn)就是某些空的柵格中心。算法需要維護(hù)Openlist和Closelist兩張表，分別用來(lái)保存待檢驗(yàn)的節(jié)點(diǎn)和已經(jīng)檢驗(yàn)過(guò)的節(jié)點(diǎn)。路徑是否最短是通過(guò)如下代價(jià)函數(shù)來(lái)判斷。

[F=G+H] ⑴

其中，[F]為代價(jià)函數(shù)的值，[G]代表從起點(diǎn)到中間節(jié)點(diǎn)代價(jià)的估計(jì)，[H]代表從中間節(jié)點(diǎn)到最終節(jié)點(diǎn)的代價(jià)的估計(jì)。注意，在估計(jì)[H]時(shí)一般忽略障礙物的影響。通常，[G]是歐拉距離，而[H]是曼哈頓距離。典型的A*算法可以通過(guò)如下步驟來(lái)描述。

輸入：地圖，起點(diǎn)，終點(diǎn)，障礙物。

輸出：最短路徑。

步驟1：將起點(diǎn)移動(dòng)到Closelist，并將與起點(diǎn)相鄰的26個(gè)節(jié)點(diǎn)加入Openlist中，將起點(diǎn)設(shè)置為這26個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)。

步驟2：遍歷整個(gè)Openlist，查找出代價(jià)函數(shù)最小的節(jié)點(diǎn)，將該節(jié)點(diǎn)移動(dòng)到Closelist中（此時(shí)Openlist中會(huì)減少一個(gè)節(jié)點(diǎn)，而Closelist中會(huì)增加一個(gè)節(jié)點(diǎn)），并將該節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)。

步驟3：對(duì)當(dāng)前節(jié)點(diǎn)的26個(gè)相鄰的節(jié)點(diǎn)進(jìn)行檢查，并對(duì)每個(gè)節(jié)點(diǎn)完成如下步驟。

① 判斷該節(jié)點(diǎn)是否為障礙物以及該節(jié)點(diǎn)是否已經(jīng)在Closelist中。如果至少有一項(xiàng)條件滿(mǎn)足，則忽略該相鄰節(jié)點(diǎn);如果均不滿(mǎn)足，轉(zhuǎn)步驟3.2。

② 判斷該相鄰節(jié)點(diǎn)是否在Openlist中。如果不在，則將該相鄰節(jié)點(diǎn)加入Openlist中，把A節(jié)點(diǎn)設(shè)置為該相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)，計(jì)算該相鄰節(jié)點(diǎn)的代價(jià)函數(shù)[F，G]和[H]。如果該節(jié)點(diǎn)已經(jīng)在Openlist中，轉(zhuǎn)③。

③ 檢查經(jīng)由A到該相鄰節(jié)點(diǎn)是否比其他路徑好，用代價(jià)函數(shù)[G]來(lái)衡量，即該相鄰節(jié)點(diǎn)的[G]值（注意：此時(shí)的[G]值是以起始節(jié)點(diǎn)作為父節(jié)點(diǎn)計(jì)算得到的）是否比通過(guò)A節(jié)點(diǎn)到達(dá)更小。如果通過(guò)A節(jié)點(diǎn)到達(dá)是比其他路徑好，則將該相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)更改為A節(jié)點(diǎn)，并重新計(jì)算代價(jià)函數(shù)的值[F，G]和[H]。如果經(jīng)由A節(jié)點(diǎn)到達(dá)該相鄰節(jié)點(diǎn)不是最好的，則返回步驟2。

步驟4：當(dāng)步驟2和步驟3執(zhí)行完畢后已經(jīng)找到了一個(gè)當(dāng)前節(jié)點(diǎn)，符合最小路徑的條件，則繼續(xù)下一個(gè)當(dāng)前節(jié)點(diǎn)的尋找。

步驟5：重復(fù)步驟2，3，4，直到目標(biāo)節(jié)點(diǎn)被加入Openlist中，說(shuō)明已經(jīng)找到了最短路徑。此時(shí)停止搜索。

除起始節(jié)點(diǎn)外，所有曾經(jīng)或仍然在Openlist中的節(jié)點(diǎn)都有一個(gè)父節(jié)點(diǎn)。通過(guò)父節(jié)點(diǎn)即可以找到最短路徑。注意，如果Openlist為空，則說(shuō)明沒(méi)有找到最短路徑，搜索失敗。

2 改進(jìn)的A*算法

為了能夠加速收斂，我們對(duì)代價(jià)函數(shù)進(jìn)行修改，在代價(jià)函數(shù)的基礎(chǔ)上增加了方向和轉(zhuǎn)向時(shí)間的代價(jià)。

假設(shè)向量[a=[xa，ya]T]表示父節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)構(gòu)成的向量，向量[b=[xb，yb]T]表示父節(jié)點(diǎn)的相鄰節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)構(gòu)成的向量，則兩者夾角的余弦表示為：

[cosθ=x1x2+y1y2x12+x22y12+y22] ⑵

式⑵實(shí)際上是對(duì)方向的估計(jì)。另外，在無(wú)人機(jī)實(shí)際運(yùn)行的過(guò)程中，我們總是希望其能夠沿直線前進(jìn)，減少不必要的轉(zhuǎn)向。因此，在代價(jià)函數(shù)中考慮轉(zhuǎn)向時(shí)間的影響是有必要的[7]。轉(zhuǎn)向時(shí)間用[K]來(lái)表示：

[K=1? ? ?轉(zhuǎn)向≥450? ? ?其他] ⑶

其中，轉(zhuǎn)向角度總是以角度變化的絕對(duì)值衡量。[K]值的引入是為了能夠?qū)⑥D(zhuǎn)向的時(shí)間成本計(jì)算到代價(jià)函數(shù)里，以兼顧時(shí)間成本。

綜上，經(jīng)過(guò)改進(jìn)的代價(jià)函數(shù)可以寫(xiě)成：

[F=G+H+Length*（cosθ+K）] ⑷

其中，[Length]表示柵格化地圖時(shí)使用的柵格的邊長(zhǎng)（2D）或棱長(zhǎng)（3D）。加入[Length]因子是為了能夠使所有的代價(jià)函數(shù)估計(jì)能夠在同一數(shù)量級(jí)下。另外，在使用自適應(yīng)柵格的改進(jìn)A*算法中，由于[Length]因子的存在，代價(jià)函數(shù)仍然能夠正常的發(fā)揮作用。

3 航跡光滑化

盡管A*算法規(guī)劃出來(lái)的路徑是最短路徑，但是由于柵格化地圖的影響，這些路徑是以節(jié)點(diǎn)的形式存在的。直接連接這些節(jié)點(diǎn)構(gòu)成的最短路徑并不光滑，會(huì)影響到后續(xù)的速度規(guī)劃和速度控制。為此，F(xiàn)owler[8]等提出了使用后處理的方法對(duì)路徑進(jìn)行光滑化的處理。本文中，借鑒逆向工程里的思想，將A*算法規(guī)劃的路徑擬合為光滑的B樣條曲線，方便后續(xù)的速度規(guī)劃和飛行控制。我們改進(jìn)了基于主點(diǎn)選擇的自適應(yīng)擬合方法[9]，并將其用于擬合A*算法規(guī)劃出的最短路徑。算法的描述如下。

輸入：構(gòu)成最短路徑的一系列節(jié)點(diǎn)[{N1，N2，…，Nn}]。

輸出：光滑的B樣條曲線

步驟1：采用局部曲率法計(jì)算各節(jié)點(diǎn)處的曲率。

步驟2：根據(jù)局部曲率尋找曲率極值點(diǎn)作為初始擬合點(diǎn)，這些初始擬合點(diǎn)稱(chēng)為特征點(diǎn)。

步驟3：基于最小二乘法將初始的特征點(diǎn)擬合為初始曲線[α（s）]，其中，[s]為3階B樣條曲線的參數(shù)。

步驟4：使用自適應(yīng)弦長(zhǎng)法將所有節(jié)點(diǎn)進(jìn)行參數(shù)化。

步驟5：用式計(jì)算除選中的特征點(diǎn)外的其他節(jié)點(diǎn)偏離初始擬合曲線[α（s）]的距離

[di=α（si）-Ni] ? ⑸

步驟6：將最大偏離距離對(duì)應(yīng)的節(jié)點(diǎn)加入到初始擬合點(diǎn)，并重復(fù)步驟3-5，直到滿(mǎn)足如下條件：

[j=1nα（sj）-Nj≤δ] ? ?⑹

其中，[δ]為給定的誤差上限。光滑后的曲線為3階的B樣條曲線，其具有連續(xù)的3階導(dǎo)數(shù)信息，完全能夠滿(mǎn)足后續(xù)速度規(guī)劃的要求。

4 仿真與實(shí)驗(yàn)

本文使用仿真和實(shí)驗(yàn)的方法來(lái)驗(yàn)證提出算法的有效性。仿真軟件使用Matlab，運(yùn)行在裝有win7的PC機(jī)上，內(nèi)存8G，處理器為AMD-6500?？偣策M(jìn)行了兩組仿真，一組是在二維空間中進(jìn)行;另一組是在三維空間進(jìn)行。

測(cè)試1：

為了測(cè)試本文提出的方法的有效性，測(cè)試1在30m×30m柵格地圖上進(jìn)行。如圖1（a）所示，其中黑點(diǎn)表示障礙物，藍(lán)色加號(hào)表示改進(jìn)A*算法規(guī)劃出的航跡點(diǎn)。其中，綠色圓圈表示起始點(diǎn)，紅色圓圈表示目標(biāo)地點(diǎn)。從圖中可以看出，這些航跡點(diǎn)能夠避開(kāi)障礙物，但是由于柵格化地圖的緣故，得到的航跡點(diǎn)是節(jié)點(diǎn)不是曲線。用線段連接這些航跡點(diǎn)可以得到最簡(jiǎn)單的航跡，然而光滑性很不好。采用我們提出的B樣條擬合的方法對(duì)航跡點(diǎn)擬合可以得到光滑的航跡，如圖1（b）所示。

測(cè)試2：

二維環(huán)境下沒(méi)有考慮無(wú)人機(jī)在[Z]軸方向上的運(yùn)動(dòng)，但真實(shí)的無(wú)人機(jī)是在三維環(huán)境下運(yùn)動(dòng)的。測(cè)試2即在三維環(huán)境下對(duì)提出的規(guī)劃算法進(jìn)行了驗(yàn)證。該三維地圖如圖2（a）所示，大小為10m×30m×6m，起點(diǎn)設(shè)置為[（0，-4.9，0.2）]，終點(diǎn)設(shè)置為[（6.0，18.0，5.1）]。圖2（a）中紅色方塊為障礙物，紅色實(shí)線為無(wú)人運(yùn)動(dòng)的軌跡。圖2（b）給出了無(wú)人機(jī)在三個(gè)坐標(biāo)軸方向上的速度曲線。從中可以看出速度曲線均較為光滑，說(shuō)明我們提出的算法成功地光滑化了無(wú)人機(jī)的飛行路徑。

實(shí)驗(yàn)在自制的無(wú)人機(jī)平臺(tái)上進(jìn)行，如圖3所示。自制的無(wú)人機(jī)平臺(tái)包括無(wú)人機(jī)機(jī)體和遙控器兩部分，無(wú)人機(jī)飛行控制部分采用STM32為核心處理器，配合MPU6050作為飛行姿態(tài)傳感器，HMC5883L作為電子羅盤(pán)，GPS作為定位系統(tǒng)。STM32以72MHz的主控頻率能夠順利完成A*算法的實(shí)時(shí)規(guī)劃。在飛行過(guò)程中，無(wú)人機(jī)通過(guò)飛行姿態(tài)傳感器和電子羅盤(pán)獲得姿態(tài)數(shù)據(jù)，通過(guò)PD控制器實(shí)現(xiàn)無(wú)人機(jī)的姿態(tài)控制。配合GPS定位系統(tǒng)，氣壓計(jì)等，無(wú)人機(jī)可以實(shí)現(xiàn)機(jī)體在空中的懸停。在實(shí)驗(yàn)中，我們給出起點(diǎn)和目標(biāo)點(diǎn)的GPS信息，無(wú)人機(jī)順利的完成了自主航跡規(guī)劃，飛行到達(dá)目的地。

5 結(jié)論

本文對(duì)基本的A*算法進(jìn)行了改進(jìn)，在代價(jià)函數(shù)中引入了轉(zhuǎn)向和時(shí)間控制。采用B樣條擬合的方法對(duì)航跡點(diǎn)進(jìn)行了擬合，該擬合方法能夠減少擬合曲線的尖點(diǎn)，最大程度的實(shí)現(xiàn)航跡的光滑性。在二維和三維空間中對(duì)提出的方案進(jìn)行了仿真，仿真結(jié)果表明，提出的方案能夠有效的實(shí)現(xiàn)最短路徑規(guī)劃和路徑光滑化。三維空間中，在各個(gè)坐標(biāo)軸的速度的仿真曲線表明航跡充分光滑，沒(méi)有出現(xiàn)突變。實(shí)驗(yàn)在自制的無(wú)人機(jī)平臺(tái)上運(yùn)行，也證實(shí)了方案的有效性。

參考文獻(xiàn)（References）：

[1] 唐平， 楊宜民. 動(dòng)態(tài)二叉樹(shù)表示環(huán)境的A*算法及其在足球機(jī)器人路徑規(guī)劃中的實(shí)現(xiàn)[J].中國(guó)工程科學(xué)，2002.4（9）.

[2] 賈慶軒，陳鋼，孫漢旭等.基于A*算法的空間機(jī)械臂避障路徑規(guī)劃[J].機(jī)械工程學(xué)報(bào)，2010.46（13）：109-115

[3] 姚遠(yuǎn)，周興社，張凱龍等.基于稀疏A*搜索和改進(jìn)人工勢(shì)場(chǎng)的無(wú)人機(jī)動(dòng)態(tài)航跡規(guī)劃[J].控制理論與應(yīng)用，2010.27（7）：953-959

[4] 王殿君.基于改進(jìn)A*算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃[J]. 清華大學(xué)學(xué)報(bào)：自然科學(xué)版，2012.8：1085-1089

[5] 熊壬浩，劉羽.A*算法的改進(jìn)及并行化[J].計(jì)算機(jī)應(yīng)用， 2015.35（7）：1843-1848

[6] Fu B，Chen L，Zhou Y，et al. An improved A* algorithm for the industrial robot path planning with high success rate and short length[J]. Robotics & Autonomous Systems，2018：106.

[7] 劉晨曦. 基于改進(jìn)A*算法的仿人機(jī)器人路徑規(guī)劃研究[D].北京建筑大學(xué)，2018.

[8] Fowler L，Rogers J.Bézier Curve Path Planning for Parafoil Terminal Guidance[J]. Journal of Aerospace Information Systems，2014.11：300-315

[9] Park H.，Lee JH.B-spline curve fitting based on adaptive curve refinement using dominant points[J].Computer-Aided Design，2007.39（6）：439-451