三維場(chǎng)景的實(shí)時(shí)無(wú)人機(jī)航跡規(guī)劃方法

陳 朋，湯粵生，俞天緯，江勇奇

1(浙江工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，杭州 310023)2(浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023)

E-mail：ystang@zjut.edu.cn

1 引 言

近些年來(lái)，隨著相關(guān)技術(shù)的不斷發(fā)展，無(wú)人機(jī)越來(lái)越廣泛地應(yīng)用于物流、設(shè)施巡檢、災(zāi)后救援和城市安防等各種場(chǎng)景.其中，多數(shù)應(yīng)用場(chǎng)景要求無(wú)人機(jī)具備自主導(dǎo)航能力，并且在飛行過(guò)程中能夠規(guī)避障礙以完成給定任務(wù).航跡規(guī)劃技術(shù)是實(shí)現(xiàn)無(wú)人機(jī)自主導(dǎo)航的關(guān)鍵技術(shù)之一，其可分為前端路徑搜索和后端航跡優(yōu)化[1]，區(qū)別在于前者獲得的路徑通常存在不自然轉(zhuǎn)向并且不包含無(wú)人機(jī)的動(dòng)力可行約束，不適合直接用于無(wú)人機(jī)飛行，需要通過(guò)后者進(jìn)一步優(yōu)化，生成適用于無(wú)人機(jī)飛行的航跡.

在前端路徑搜索方面已經(jīng)有一些不錯(cuò)的工作.LaValle[2]提出了快速探索隨機(jī)樹(Rapidly-exploring Random Tree，RRT)算法，從配置空間中隨機(jī)地抽取樣本，并引導(dǎo)樹向目標(biāo)生長(zhǎng).RRT算法雖然能夠高效地找到可行的路徑，但不具有漸進(jìn)最優(yōu)性.此后，Karaman等人[3]對(duì)其進(jìn)行改進(jìn)并提出了RRT*(RRT算法的變體)、概率路圖法(Probabilistic Roadmap*，PRM*)等一系列基于采樣的漸進(jìn)最優(yōu)方法，在這些方法中，隨著樣本數(shù)的增加，最終趨近于全局最優(yōu)解.郗楓飛等人[4]針對(duì)RRT系列算法面對(duì)復(fù)雜地圖時(shí)隨機(jī)采樣效率低和路徑重復(fù)性差的問(wèn)題，提出一種基于模擬植物生長(zhǎng)引導(dǎo)的RRT移動(dòng)機(jī)器人路徑規(guī)劃算法，提升了路徑尋優(yōu)的穩(wěn)定性和效率.另一方面，一些研究人員提出了基于節(jié)點(diǎn)的路徑搜索方法，將配置空間離散化成一系列的節(jié)點(diǎn)，然后定義一個(gè)成本函數(shù)，搜索每個(gè)節(jié)點(diǎn)以找到成本最小的路徑.該類方法可以與其他方法相結(jié)合，實(shí)現(xiàn)全局最優(yōu)，適合實(shí)時(shí)應(yīng)用場(chǎng)景，典型的方法有ARA*[5]、JPS[6]和Hybrid-state A*[7]等.

在后端航跡優(yōu)化方面，前人也提出了多種不同的方法.王玉寶等人[8]根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)特征，提出一種時(shí)間最優(yōu)軌跡規(guī)劃算法，采用簡(jiǎn)化粒子群模型和高項(xiàng)式插值模型對(duì)粒子群算法學(xué)習(xí)因子進(jìn)行動(dòng)態(tài)調(diào)節(jié)，在保證運(yùn)動(dòng)軌跡時(shí)間最優(yōu)的同時(shí)，彌補(bǔ)了基本粒子群算法局部收斂的不足.鄭紅波等人[9]提出了一種基于層次包圍盒碰撞檢測(cè)的路徑規(guī)劃優(yōu)化算法，在原始A*算法基礎(chǔ)上，采用粗試探和精搜索的策略進(jìn)行路徑規(guī)劃，可以規(guī)避無(wú)法通過(guò)的道路，提高了時(shí)間效率.Mellinger等人[10]最先提出了Minimum snap軌跡生成方法，由分段多項(xiàng)式函數(shù)表示軌跡，并將軌跡生成問(wèn)題擬合成二次規(guī)劃問(wèn)題，但該方法直接對(duì)多項(xiàng)式系數(shù)進(jìn)行求解，容易陷入數(shù)值不穩(wěn)定問(wèn)題.對(duì)此，Richter等人[11]利用了代入置換技術(shù)，將問(wèn)題轉(zhuǎn)化成無(wú)約束的二次規(guī)劃，間接求解多項(xiàng)式系數(shù)，解決了文獻(xiàn)[10]容易出現(xiàn)的數(shù)值問(wèn)題.為了能適應(yīng)障礙物密集的復(fù)雜環(huán)境，許多研究人員[12-14]提出了構(gòu)建安全飛行走廊，該飛行走廊由若干凸區(qū)域構(gòu)成，可將軌跡限制在該安全區(qū)域內(nèi)，但該類方法會(huì)引入大量的不等式約束，導(dǎo)致二次規(guī)劃問(wèn)題的求解難度增加.此外，Zucker等人[15]把軌跡規(guī)劃問(wèn)題表示為最小化兩個(gè)成本項(xiàng)：一個(gè)是對(duì)軌跡與障礙物碰撞的懲罰，另一個(gè)是軌跡本身的平滑性.但該方法是針對(duì)離散的軌跡點(diǎn)進(jìn)行優(yōu)化的，相對(duì)來(lái)說(shuō)更適合用于機(jī)械臂的控制.在此基礎(chǔ)上，Oleynikova等人[16]采用了連續(xù)時(shí)間分段多項(xiàng)式來(lái)表示軌跡，即整條軌跡由若干段以時(shí)間為自變量的多項(xiàng)式組成，無(wú)需對(duì)狀態(tài)空間或時(shí)間進(jìn)行離散操作，同時(shí)對(duì)于任意給定的時(shí)間能夠快速評(píng)估高階導(dǎo)數(shù)的連續(xù)性，有利于無(wú)人機(jī)的飛行控制，但該方法生成安全軌跡的成功率并不高，只有0.6左右的成功率，無(wú)法保證實(shí)際應(yīng)用的安全性.受文獻(xiàn)[16]的啟發(fā)，本文對(duì)碰撞代價(jià)函數(shù)進(jìn)行優(yōu)化，并顯式地引入了無(wú)人機(jī)的動(dòng)力可行約束，在保證實(shí)時(shí)性的同時(shí)大幅度地提高了其在復(fù)雜環(huán)境中生成安全軌跡的成功率.本文的主要工作可歸結(jié)如下：1)改進(jìn)A*算法的啟發(fā)函數(shù)，使其更適應(yīng)無(wú)人機(jī)的三維運(yùn)動(dòng)特征，并利用梯度信息篩選關(guān)鍵航點(diǎn)，有利于后端航跡優(yōu)化的進(jìn)行；2)優(yōu)化碰撞代價(jià)函數(shù)，并顯式地引入無(wú)人機(jī)的動(dòng)力可行約束，構(gòu)建二次規(guī)劃模型，使優(yōu)化后的航跡更適合無(wú)人機(jī)飛行；3)與現(xiàn)有航跡規(guī)劃方法對(duì)比，實(shí)驗(yàn)結(jié)果表明所提出的方法在保證實(shí)時(shí)性的同時(shí)具有更短的航跡長(zhǎng)度和更高的成功率.

2 前端路徑搜索方法

2.1 改進(jìn)啟發(fā)函數(shù)的A*算法

A*算法是一種啟發(fā)式搜索算法，通過(guò)使用啟發(fā)式信息引導(dǎo)搜索決策提高搜索效率，同時(shí)保證路徑的優(yōu)越性.A*算法的成本函數(shù)由實(shí)際成本和估計(jì)成本兩部分組成，如式(1)所示.

f(n)=g(n)+h(n)

(1)

其中，n是當(dāng)前的擴(kuò)展節(jié)點(diǎn)，f(n)是從起始節(jié)點(diǎn)經(jīng)過(guò)當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最佳路徑成本估計(jì)，g(n)是從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際成本，h(n)是從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)成本，通常也稱為啟發(fā)函數(shù)，它在成本函數(shù)中起著主導(dǎo)性的作用[17].如果h(n)總是小于或等于從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的實(shí)際成本，那么A*算法就能確保找到最短路徑，最好的情況當(dāng)然是h(n)正好等于實(shí)際成本，此時(shí)在保證得到最優(yōu)解的同時(shí)搜索效率最高.如果h(n)遠(yuǎn)小于實(shí)際成本，就會(huì)導(dǎo)致需要搜索的節(jié)點(diǎn)過(guò)多，搜索效率低下，而如果遠(yuǎn)大于實(shí)際成本，則可能得不到最短路徑.因此，設(shè)計(jì)一個(gè)合適的啟發(fā)函數(shù)就顯得很關(guān)鍵.通常A*算法的啟發(fā)函數(shù)多采用歐氏距離或曼哈頓距離，如式(2)和式(3)所示.

(2)

hm(n)=D·(dx+dy)

(3)

其中，he(n)和hm(n)分別表示采用歐氏距離和曼哈頓距離的啟發(fā)函數(shù)，D′和D為節(jié)點(diǎn)間的移動(dòng)代價(jià)，dx和dy分別為當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的橫坐標(biāo)之間和縱坐標(biāo)之間的差的絕對(duì)值.歐式距離可以表示任意方向的運(yùn)動(dòng)，但其不能體現(xiàn)柵格間的路徑距離，并且由于平方根的存在，相對(duì)難以計(jì)算.而曼哈頓距離雖然計(jì)算簡(jiǎn)單，但是只能表示前后左右四個(gè)方向的運(yùn)動(dòng)，無(wú)法體現(xiàn)對(duì)角運(yùn)動(dòng)，通常得到的距離估計(jì)值會(huì)大于真實(shí)值.因此，為了能實(shí)現(xiàn)對(duì)角運(yùn)動(dòng)、距離估計(jì)值更貼近真實(shí)值，同時(shí)保證計(jì)算的相對(duì)簡(jiǎn)單高效，本文采用了對(duì)角距離.

hd(n)=D·(dx+dy)+(D2-2·D)·min(dx，dy)

(4)

其中，hd(n)表示采用對(duì)角距離的啟發(fā)函數(shù)，D和D2分別表示兩個(gè)相鄰節(jié)點(diǎn)(有公共邊)和面對(duì)角相鄰節(jié)點(diǎn)(無(wú)公共邊)之間的移動(dòng)代價(jià).如圖1所示，為在二維柵格地圖中采用曼哈頓距離和對(duì)角距離的方式進(jìn)行路徑搜索的區(qū)別示意圖.

圖1 采用兩種距離進(jìn)行路徑搜索的區(qū)別Fig.1 Difference between two distances for path finding

其中，字母S、G分別表示起點(diǎn)和目標(biāo)點(diǎn)，有陰影填充的柵格表示已搜索的節(jié)點(diǎn)，空白柵格表示未知節(jié)點(diǎn)，虛線段表示路徑，橢圓狀虛線圈出來(lái)的區(qū)域則表示兩者區(qū)別所在，采用對(duì)角距離的方式需要進(jìn)行min(dx，dy)次對(duì)角移動(dòng)，但比采用曼哈頓距離的方式要節(jié)省2×min(dx，dy)次直線移動(dòng)(即橫向和縱向的移動(dòng)).對(duì)于經(jīng)常需要進(jìn)行三維多自由度運(yùn)動(dòng)的無(wú)人機(jī)來(lái)說(shuō)，顯然二維的搜索算法是不能完全適應(yīng)的.因此，沿著上面的思路將其推廣到三維情況，即可表示面對(duì)角方向上的運(yùn)動(dòng)又可表示體對(duì)角方向上的運(yùn)動(dòng)，更加適應(yīng)無(wú)人機(jī)的三維運(yùn)動(dòng)特征，本文提出適應(yīng)三維多自由度運(yùn)動(dòng)的啟發(fā)函數(shù)，如式(5)所示.

hd(n)=

(5)

其中，dmin=min(dx，dy，dz)，D3為體對(duì)角相鄰節(jié)點(diǎn)(無(wú)公共邊)的移動(dòng)代價(jià).相對(duì)于采用曼哈頓距離和歐式距離的方式，我們改進(jìn)的啟發(fā)函數(shù)hd(n)不但能表示對(duì)角運(yùn)動(dòng)，使當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的成本估計(jì)值更貼近實(shí)際成本，而且避免了直接計(jì)算平方根，保證了較高的計(jì)算效率，適合于類似無(wú)人機(jī)這樣的機(jī)動(dòng)性強(qiáng)、機(jī)載計(jì)算資源有限的平臺(tái)上.

2.2 關(guān)鍵航點(diǎn)的篩選

前端用改進(jìn)后的A*算法在柵格地圖中找到的初始路徑除了不包含無(wú)人機(jī)動(dòng)力可行約束外，通常會(huì)存在一些不自然轉(zhuǎn)向和許多冗余的航點(diǎn)，不適合直接用于無(wú)人機(jī)的飛行控制，而且大量的冗余航點(diǎn)也會(huì)加大后端航跡優(yōu)化工作的難度.針對(duì)上述問(wèn)題，本文利用航點(diǎn)間的梯度信息，濾除冗余航點(diǎn)，保留起點(diǎn)、拐點(diǎn)和目標(biāo)點(diǎn)這些關(guān)鍵航點(diǎn).如圖2所示，有幾何圖形占據(jù)的區(qū)域表示障礙物，空白柵格則代表未探索(或無(wú)障礙)的區(qū)域，深色柵格表示路徑上的航點(diǎn)，一系列的深色柵格構(gòu)成了柵格路徑，路徑周圍有淺色陰影填充的柵格表示已探索的節(jié)點(diǎn)，字母S、G分別代表起點(diǎn)和目標(biāo)點(diǎn)，虛線段表示由關(guān)鍵航點(diǎn)構(gòu)成的初始路徑.而圖2(c)中圓點(diǎn)等同于深色柵格也代表路徑上的航點(diǎn)，箭頭則表示相鄰航點(diǎn)構(gòu)成的向量.根據(jù)航點(diǎn)間的梯度信息，這里即是向量間的方向變化，就可以剔除掉冗余航點(diǎn)并篩選出關(guān)鍵航點(diǎn)，作為后端航跡優(yōu)化的初始條件、約束條件等信息，在盡可能地保留初始路徑信息的基礎(chǔ)上，最大限度地減輕后端軌跡優(yōu)化的難度.以上為了方便說(shuō)明，先在二維情況下進(jìn)行方法的描述，后將其推廣到三維情況.如在圖1和圖2表示的二維柵格地圖中，只采用式(4)作為啟發(fā)函數(shù)并進(jìn)行八鄰域搜索即可，而對(duì)于三維柵格地圖則是采用所提出的式(5)作為啟發(fā)函數(shù)在當(dāng)前節(jié)點(diǎn)周圍相鄰的26個(gè)節(jié)點(diǎn)進(jìn)行搜索.對(duì)于關(guān)鍵航點(diǎn)的篩選，在二維和三維情況下都是通過(guò)判斷向量間的方向變化來(lái)實(shí)現(xiàn)的.

圖2 關(guān)鍵航點(diǎn)篩選示意圖Fig.2 Diagram of key waypoints screening

3 后端航跡優(yōu)化方法

有了前端路徑搜索得到的初始路徑和關(guān)鍵航點(diǎn)信息，本文在后端就可以對(duì)初始路徑進(jìn)行優(yōu)化，解決初始路徑存在的不自然轉(zhuǎn)向、不夠平滑和缺少動(dòng)力可行約束等問(wèn)題，生成適合于無(wú)人機(jī)自主導(dǎo)航的可行航跡.

3.1 問(wèn)題描述

通常希望航跡能夠滿足一系列的約束條件，比如：起點(diǎn)或目標(biāo)點(diǎn)的位置、速度和加速度，相鄰航跡段連接處平滑，優(yōu)化后的航跡無(wú)碰撞，滿足無(wú)人機(jī)的動(dòng)力可行約束(這里的動(dòng)力可行約束是指無(wú)人機(jī)的最大速度、加速度、加加速度(Jerk)等限制)等等.而一般情況下滿足約束條件的航跡可能存在無(wú)數(shù)條，實(shí)際問(wèn)題中往往需要一條特定的航跡，因此需要構(gòu)建一個(gè)最優(yōu)函數(shù)，在可行航跡中找出特定的那條軌跡.

3.2 目標(biāo)函數(shù)的構(gòu)建

本文采用以時(shí)間t為變量的分段多項(xiàng)式來(lái)表示航跡，N階M段的航跡如式(6)所示.

(6)

其中，μ表示x，y，z中的任意一個(gè)維度，pi，j表示第i段航跡的第j次項(xiàng)的系數(shù)，T1，T2，…，TM表示每段航跡終點(diǎn)的時(shí)間，而整條航跡的時(shí)間為T=TM-T0.

由文獻(xiàn)[16]得到啟發(fā)，本文顯式地引入了無(wú)人機(jī)的動(dòng)力可行約束，構(gòu)建出如式(7)所示的目標(biāo)函數(shù).

ming=ω1·gs+ω2·gc+ω3·gd

(7)

其中，gs為平滑項(xiàng)保證航跡的平滑，gc為碰撞項(xiàng)使航跡與障礙物保持一定的安全距離，gd為動(dòng)力可行約束項(xiàng)保證速度、加速度維持在可行的范圍內(nèi).而ω1，ω2和ω3為對(duì)應(yīng)項(xiàng)的權(quán)重，權(quán)衡三者在目標(biāo)函數(shù)中所占的比重，任何一項(xiàng)系數(shù)設(shè)置過(guò)大都會(huì)導(dǎo)致該項(xiàng)占據(jù)主導(dǎo)地位、其他項(xiàng)作用微弱，設(shè)置過(guò)小則會(huì)導(dǎo)致該項(xiàng)所起的作用微弱，相應(yīng)的結(jié)果也不盡相同.本文實(shí)驗(yàn)過(guò)程中將ω1，ω2和ω3分別設(shè)置為2.0，1.5和1.0，但系數(shù)設(shè)置并非絕對(duì)，應(yīng)根據(jù)實(shí)際情況做出相應(yīng)調(diào)整.

目標(biāo)函數(shù)的平滑項(xiàng)可以表示成軌跡fu(t)的k階導(dǎo)數(shù)的平方對(duì)時(shí)間t的積分，具體如式(8)所示.

(8)

進(jìn)一步可以將其寫成二次型形式pTQp，其中p為所有航跡段的多項(xiàng)式系數(shù)構(gòu)成的向量，Q為海森矩陣.如文獻(xiàn)[11]所提到的閉式求解法，本文引入一個(gè)映射矩陣A和置換矩陣C將多項(xiàng)式系數(shù)映射到多項(xiàng)式航跡段端點(diǎn)的導(dǎo)數(shù)，如式(9)所示.

(9)

這里矩陣A的作用是將多項(xiàng)式系數(shù)p映射為端點(diǎn)導(dǎo)數(shù)d，矩陣C則是將d中的固定導(dǎo)數(shù)dF(已知變量)和自由導(dǎo)數(shù)dP(未知變量)分離出來(lái).因此，平滑項(xiàng)可以寫成式(10)形式.

(10)

令CTA-TQA-1C為矩陣R，并根據(jù)固定導(dǎo)數(shù)和自由導(dǎo)數(shù)對(duì)其進(jìn)行劃分，可寫成分塊矩陣形式如式(11).

(11)

將式(11)展開，并關(guān)于自由導(dǎo)數(shù)dP進(jìn)行求導(dǎo)，可得到平滑項(xiàng)相應(yīng)的雅可比矩陣Js如式(12)、式(13).

(12)

(13)

其中，μ∈{x，y，z}代表x，y，z三個(gè)維度.令式(13)等于零，便可求得自由導(dǎo)數(shù)dP，再由式(9)就可間接得到多項(xiàng)式系數(shù)p.

對(duì)于目標(biāo)函數(shù)碰撞項(xiàng)的構(gòu)造，與文獻(xiàn)[16]類似，本文也采用了可微函數(shù)c(f(t))沿航跡弧長(zhǎng)的線性積分來(lái)表示，但不同的是本文采用了指數(shù)函數(shù)來(lái)表示可微函數(shù)c，如式(14)所示.

c(σ)=α·e-β(σ-σ0)

(14)

其中，σ為到障礙物的距離，這里可以通過(guò)建立一個(gè)歐式有向距離場(chǎng)來(lái)獲取σ值，σ0為安全距離閾值，α為函數(shù)的幅值，β則代表變化率.從形式上來(lái)看，指數(shù)函數(shù)相比文獻(xiàn)[16]中采用的分段函數(shù)具有形式簡(jiǎn)單，容易求導(dǎo)的特點(diǎn).從實(shí)際物理意義上來(lái)看，如圖3所示，本文希望當(dāng)距離障礙物小于安全距離σ0時(shí)，c(σ)的值能迅速變得很大，使碰撞項(xiàng)成為整個(gè)目標(biāo)函數(shù)的主導(dǎo)者，而當(dāng)距離大于σ0時(shí)，c(σ)的值趨于平緩并且值相對(duì)較小，這樣就能使航跡與障礙物保持著安全距離.因此，碰撞項(xiàng)gc可以寫成式(15)的形式.

圖3 碰撞代價(jià)函數(shù)示意圖Fig.3 Diagram of collision cost function

(15)

其中，s表示弧長(zhǎng)，f(·)和v(·)分別代表航跡上的點(diǎn)的位置和速度，Γk=T0+kδt.為了計(jì)算簡(jiǎn)單，這里將積分離散化變成求和的形式.因此，關(guān)于自由導(dǎo)數(shù)dPμ的雅可比矩陣Jc可寫成如式(16)、式(17).

(16)

(17)

(18)

其中，aμ(t)為加速度函數(shù).相應(yīng)地，gv的雅可比矩陣Jv也可得到，與式(17)類似.此外，加速度約束項(xiàng)ga的構(gòu)造與速度約束項(xiàng)gv的構(gòu)造也類似，因?yàn)榧铀俣群瘮?shù)aμ(t)是航跡多項(xiàng)式函數(shù)的二階導(dǎo)數(shù)，所以這里不再贅述.因此，動(dòng)力約束項(xiàng)gd=gv+ga，相應(yīng)的雅可比矩陣則為Jd=Jv+Ja.

3.3 優(yōu)化問(wèn)題的求解

由3.2節(jié)的構(gòu)造過(guò)程，可以得到整個(gè)目標(biāo)函數(shù)g的雅可比矩陣J=ω1·Js+ω2·Jc+ω3·Jd，一般的非線性優(yōu)化方法都可以用來(lái)最小化目標(biāo)函數(shù)，比如梯度下降法、文獻(xiàn)[16]里采用的擬牛頓法等.雖然目標(biāo)函數(shù)是非凸的，可能存在多個(gè)局部最優(yōu)解，但是由于目標(biāo)函數(shù)具有在障礙物附近迅速趨于無(wú)窮大的特性，以及目標(biāo)函數(shù)的每一項(xiàng)都可導(dǎo)、至少存在二階導(dǎo)數(shù)，容易求得對(duì)應(yīng)的海森矩陣，具有更好的收斂特性，因此即使得到的是局部最優(yōu)解，也能保證其是安全可行的.本文這里采用了保守的凸可分近似算法(Conservative Convex Separable Approximation，CCSA)[18]來(lái)求解問(wèn)題，其適用于具有大量變量的優(yōu)化問(wèn)題，即使目標(biāo)函數(shù)的海森矩陣是密集型的，而且它能夠保證從任何可行的起始點(diǎn)收斂到某個(gè)局部最小值.在求解過(guò)程中，固定變量dF為起點(diǎn)和目標(biāo)點(diǎn)的導(dǎo)數(shù)，包括位置、速度和加速度，自由變量dP(即待求解變量)則為中間關(guān)鍵航點(diǎn)的相應(yīng)導(dǎo)數(shù).根據(jù)初始關(guān)鍵航點(diǎn)到最近障礙物的距離，設(shè)定自由變量dP的邊界約束，保證其在適當(dāng)?shù)娜≈捣秶鷥?nèi).當(dāng)滿足設(shè)定的終止條件或者達(dá)到預(yù)設(shè)的最大優(yōu)化時(shí)間限制時(shí)，整個(gè)優(yōu)化過(guò)程結(jié)束，當(dāng)前的解即為優(yōu)化后的結(jié)果.實(shí)際求解結(jié)果表明，采用CCSA方法相比其他兩種方法對(duì)本文優(yōu)化問(wèn)題能得到更好的局部最優(yōu)解.

4 實(shí)驗(yàn)結(jié)果與分析

本文所提出的無(wú)人機(jī)航跡規(guī)劃方法分為兩個(gè)階段：前端路徑搜索獲取初始路徑、關(guān)鍵航點(diǎn)和后端航跡優(yōu)化得到平滑、安全、滿足無(wú)人機(jī)動(dòng)力可行約束的航跡.下面章節(jié)先對(duì)實(shí)驗(yàn)平臺(tái)進(jìn)行簡(jiǎn)單的介紹，然后通過(guò)相關(guān)的實(shí)驗(yàn)數(shù)據(jù)分別對(duì)兩個(gè)階段進(jìn)行定量和定性評(píng)估，并與現(xiàn)有航跡規(guī)劃方法進(jìn)行對(duì)比，驗(yàn)證所提方法的有效性.

4.1 實(shí)驗(yàn)平臺(tái)介紹

三維場(chǎng)景中的無(wú)人機(jī)航跡規(guī)劃仿真實(shí)驗(yàn)在Ubuntu系統(tǒng)下進(jìn)行，基于ROS(Robot Operating System)框架采用C/C++語(yǔ)言進(jìn)行算法軟件設(shè)計(jì)，并利用ROS自帶的可視化工具Rviz完成算法在三維環(huán)境下的可視化演示.所用實(shí)驗(yàn)平臺(tái)的相關(guān)配置如下：處理器為Intel(R) Core(TM) i5-6500 3.20GHz× 4，內(nèi)存大小15.6GB，系統(tǒng)版本為Ubuntu 16.04 LTS，ROS版本為Kinetic 1.12.14.

4.2 前端路徑搜索方法

這里分別采用曼哈頓距離、歐式距離以及對(duì)角距離三種方式來(lái)表示A*算法的啟發(fā)函數(shù)，在三維復(fù)雜仿真空間場(chǎng)景(20m×20m×8m大小，隨機(jī)分布稠密障礙物，如圖4所示)中，隨機(jī)生成起始點(diǎn)和目標(biāo)點(diǎn)(實(shí)際應(yīng)用中諸如Intel Realsense深度相機(jī)、ZED雙目相機(jī)等有效測(cè)量范圍在0.1～20m之間，因此，實(shí)驗(yàn)設(shè)置取中值10m作為起始點(diǎn)和目標(biāo)點(diǎn)的直線距離，隨機(jī)生成的起始點(diǎn)和目標(biāo)點(diǎn)滿足該條件)，然后分別進(jìn)行3組實(shí)驗(yàn)，每組實(shí)驗(yàn)進(jìn)行10次，最終結(jié)果為10次實(shí)驗(yàn)數(shù)據(jù)取平均值，評(píng)估結(jié)果如表1所示.圖4為三維路徑搜索實(shí)驗(yàn)示意圖，圖4(a)為關(guān)鍵航點(diǎn)篩選前的路徑搜索結(jié)果，圖4(b)為利用梯度信息篩選出關(guān)鍵航點(diǎn).其中，由一系列淺色柵格組成的柱狀體為障礙物，空白區(qū)域?yàn)樽杂煽臻g(無(wú)障礙物)，深色柵格代表搜索到的路徑上的航點(diǎn)，字母S、G分別代表起始點(diǎn)和目標(biāo)點(diǎn).

圖4 三維路徑搜索示意圖Fig.4 Diagram of 3D path finding

由表1可知，與采用曼哈頓距離表示啟發(fā)函數(shù)的方法對(duì)比，本文改進(jìn)的采用對(duì)角距離表示啟發(fā)函數(shù)的方法在平均標(biāo)準(zhǔn)化長(zhǎng)度(實(shí)際路徑長(zhǎng)度除以起始點(diǎn)到終點(diǎn)間的歐式距離)、航點(diǎn)總數(shù)以及關(guān)鍵航點(diǎn)數(shù)三方面都有所減少，雖然在平均時(shí)間上不占有優(yōu)勢(shì)，但是可以克服采用曼哈頓距離表示啟發(fā)函數(shù)的方式無(wú)法實(shí)現(xiàn)對(duì)角方向上運(yùn)動(dòng)的缺點(diǎn)；與采用歐式距離作為啟發(fā)函數(shù)的方法相比，本文改進(jìn)的采用對(duì)角距離表示啟發(fā)函數(shù)的方法在平均標(biāo)準(zhǔn)化長(zhǎng)度和航點(diǎn)總數(shù)上與其相當(dāng)，但在平均時(shí)間和關(guān)鍵航點(diǎn)數(shù)上占有比較大的優(yōu)勢(shì).因此，相比于其他兩種方法，本文優(yōu)化的三維路徑搜索方法在保證具有較快速度的基礎(chǔ)上，能夠保證找到最短的初始路徑、最少的關(guān)鍵航點(diǎn)數(shù)，同時(shí)又能表示對(duì)角方向上的運(yùn)動(dòng).

表1 采用不同啟發(fā)函數(shù)的路徑搜索對(duì)比Table 1 Comparison of path finding via different heuristic functions

4.3 后端航跡優(yōu)化方法

本文分別與文獻(xiàn)[11]和文獻(xiàn)[16]所提出的方法進(jìn)行對(duì)比，值得注意的是，這里只針對(duì)后端航跡優(yōu)化方法進(jìn)行評(píng)估，不包括前端的路徑搜索.同前面一樣也是在20m×20m×8m大小的三維仿真環(huán)境中，隨機(jī)生成起始點(diǎn)和目標(biāo)點(diǎn)，根據(jù)前端路徑搜索環(huán)節(jié)得到的關(guān)鍵航點(diǎn)，并設(shè)置不同的障礙物密度，然后分別進(jìn)行50次的航跡優(yōu)化對(duì)比實(shí)驗(yàn)，最終結(jié)果為50次實(shí)驗(yàn)數(shù)據(jù)的平均值.表2表示在障礙物密度為0.2個(gè)/m2情況下的定量評(píng)估結(jié)果.其中，與文獻(xiàn)[11]相比，本文方法在成功率、平均標(biāo)準(zhǔn)化長(zhǎng)度和平均時(shí)間三個(gè)方面都有不錯(cuò)的表現(xiàn)；與文獻(xiàn)[16]相比，本文方法在平均時(shí)間上略微高于文獻(xiàn)[16]的方法，原因在于本文引入了無(wú)人機(jī)動(dòng)力可行約束項(xiàng)，由此帶來(lái)計(jì)算量的略微增加.而在成功率和平均標(biāo)準(zhǔn)化長(zhǎng)度兩方面，本文方法都具有一定的優(yōu)勢(shì)，特別是在場(chǎng)景障礙物密度不是很高的情況下，本文方法可以達(dá)到100%的成功率.

表2 不同航跡優(yōu)化方法對(duì)比Table 2 Comparison of different trajectory optimization algorithms

圖5展現(xiàn)了三種航跡優(yōu)化方法的定性評(píng)估結(jié)果，其中曲線1代表文獻(xiàn)[11]的方法優(yōu)化的航跡，曲線2為本文方法優(yōu)化后的航跡，曲線3則代表文獻(xiàn)[16]的方法優(yōu)化的航跡，字母S、G分別代表起始點(diǎn)和目標(biāo)點(diǎn)，空白區(qū)域表示自由空間(無(wú)障礙)，若干數(shù)量的柱狀體則代表著障礙物.由圖5可以觀察到，曲線2始終保持著與周圍障礙物適中的安全距離，不像另外兩條曲線在某些地方會(huì)出現(xiàn)離障礙物很近的情況，容易出現(xiàn)碰撞危險(xiǎn).

圖5 不同軌跡優(yōu)化方法對(duì)比圖Fig.5 Diagram of different trajectory optimization algorithms

圖6 不同障礙物密度下的成功率Fig.6 Success rate at different obstacle density

這也就是本文方法能夠獲得較高成功率的原因所在，對(duì)碰撞項(xiàng)的優(yōu)化以及無(wú)人機(jī)動(dòng)力可行約束項(xiàng)的引進(jìn)所致.圖6表示了三種航跡優(yōu)化方法在不同障礙物密度下的成功率變化情況.隨著障礙物密度的增加，三種航跡優(yōu)化方法的成功率都逐漸下降，但相比其他兩種方法，本文航跡優(yōu)化方法下降的幅度最低并且維持著最高的成功率.此外，在圖7中我們還給出了本文方法在不同障礙物密度下的航跡優(yōu)化表現(xiàn)情況.其中，

圖7 不同障礙物密度下的航跡規(guī)劃圖Fig.7 Diagram of trajectory planning at different obstacle density

字母S、G代表起點(diǎn)和目標(biāo)點(diǎn)，兩點(diǎn)間的曲線代表優(yōu)化后的航跡，在曲線上或附近的單個(gè)柵格點(diǎn)則代表前端路徑搜索得到的關(guān)鍵航點(diǎn).實(shí)驗(yàn)過(guò)程中，算上前端路徑搜索和后端航跡優(yōu)化所需的時(shí)間總和基本都維持在50ms內(nèi)，能夠滿足無(wú)人機(jī)實(shí)時(shí)航跡規(guī)劃要求.

5 總 結(jié)

針對(duì)三維場(chǎng)景中無(wú)人機(jī)航跡規(guī)劃的實(shí)時(shí)性和安全性問(wèn)題，本文提出了一種基于關(guān)鍵航點(diǎn)的實(shí)時(shí)無(wú)人機(jī)航跡規(guī)劃方法.該方法分為兩階段：首先，改進(jìn)A*算法的啟發(fā)函數(shù)，使其更適應(yīng)無(wú)人機(jī)的三維運(yùn)動(dòng)特征，在此基礎(chǔ)上利用航點(diǎn)間的梯度信息對(duì)初始路徑進(jìn)行關(guān)鍵航點(diǎn)的篩選，有利于后端航跡優(yōu)化的進(jìn)行；接著，顯式引入無(wú)人機(jī)動(dòng)力可行約束，并改進(jìn)碰撞懲罰項(xiàng)，構(gòu)建二次規(guī)劃模型，對(duì)初始路徑進(jìn)行優(yōu)化進(jìn)而得到一條平滑、安全并且適合無(wú)人機(jī)飛行的航跡.實(shí)驗(yàn)驗(yàn)證表明，本文方法保持了較好的實(shí)時(shí)性，運(yùn)行時(shí)間可達(dá)50ms內(nèi)，同時(shí)一定程度上提高了航跡規(guī)劃的成功率以及航跡的質(zhì)量，在障礙物密度小于0.5個(gè)/m2的情況下，能維持在100%的成功率，并且優(yōu)化后的航跡長(zhǎng)度更短，更符合實(shí)際工程應(yīng)用的要求.