陳朋 江勇奇 俞天緯 黨源杰 宦若虹

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

近年來(lái)，隨著無(wú)人機(jī)(UAV)廣泛地應(yīng)用于空中拍攝、災(zāi)后救援和城市安防等各種場(chǎng)景，人們對(duì)無(wú)人機(jī)自主導(dǎo)航能力的要求也逐漸提高，尤其是在長(zhǎng)距離導(dǎo)航方面。航跡規(guī)劃是實(shí)現(xiàn)無(wú)人機(jī)自主導(dǎo)航的關(guān)鍵技術(shù)之一，其可分為前端路徑搜索和后端軌跡優(yōu)化[1]。前端通過(guò)路徑搜索算法獲得空間上無(wú)碰撞的初始路徑，但缺少在時(shí)域上對(duì)無(wú)人機(jī)速度和加速度的設(shè)定；后端則在前端基礎(chǔ)上進(jìn)行優(yōu)化，生成更加平滑、安全且適合無(wú)人機(jī)動(dòng)態(tài)飛行的軌跡。

前端路徑搜索主要分為基于采樣的和基于圖搜索的兩類(lèi)方法?；诓蓸拥拇硭惴焖偬剿麟S機(jī)樹(shù)(RRT)[2]從配置空間中隨機(jī)地抽取樣本，并引導(dǎo)樹(shù)向目標(biāo)生長(zhǎng)。雖然RRT算法能夠有效地找到可行的路徑，然而沒(méi)有漸近最優(yōu)性。Karaman等[3]提出了基于采樣的漸進(jìn)最優(yōu)方法RRT*，隨著樣本數(shù)的增加，該算法最終會(huì)收斂到全局最優(yōu)解。基于采樣的方法雖能有效地解決了復(fù)雜約束的路徑規(guī)劃問(wèn)題，但算法效率不穩(wěn)定?；趫D搜索的方法雖未必能找到最優(yōu)路徑，但效率極高且穩(wěn)定，更符合無(wú)人機(jī)的實(shí)時(shí)要求?；趫D搜索的代表算法A*將配置空間離散化，把路徑搜索問(wèn)題轉(zhuǎn)化為圖搜索問(wèn)題，根據(jù)圖中節(jié)點(diǎn)代價(jià)尋找最優(yōu)路徑[4]。近年來(lái)人們通過(guò)改進(jìn)算法A*來(lái)解決相應(yīng)的實(shí)際問(wèn)題。Dolgov等[5]提出了基于物體實(shí)際運(yùn)動(dòng)約束的混合A*算法，生成符合車(chē)輛運(yùn)動(dòng)規(guī)律的軌跡。張慶等[6]通過(guò)融合跳點(diǎn)搜索(JPS)策略來(lái)改進(jìn)A*算法，大大減少了拓展的節(jié)點(diǎn)和尋路算法的內(nèi)存消耗。但上述對(duì)A*算法的改進(jìn)較為復(fù)雜，目前還只應(yīng)用于二維地圖。而Theta*算法[7]利用視線檢查改進(jìn)A*算法并且適用于三維環(huán)境。該算法在遍歷節(jié)點(diǎn)時(shí)檢測(cè)當(dāng)前節(jié)點(diǎn)和上個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)之間的線空間中是否存在視線，若存在，則刪除中間節(jié)點(diǎn)以獲得比A*算法更短的路徑。但Theta*算法僅通過(guò)簡(jiǎn)單的避障檢測(cè)并沒(méi)有解決A*算法遺留的安全問(wèn)題，且節(jié)點(diǎn)代價(jià)計(jì)算方式尚未考慮到無(wú)人機(jī)轉(zhuǎn)彎情況。

后端軌跡優(yōu)化一般先把軌跡表示成以時(shí)間為變量的分段多項(xiàng)式形式[8]，然后通過(guò)設(shè)計(jì)目標(biāo)函數(shù)和添加相應(yīng)的約束條件進(jìn)行優(yōu)化。目前許多軌跡優(yōu)化方法被提出來(lái)，并逐漸形成兩種流派：硬約束法[9-12]和軟約束法[13-14]。硬約束法主要是將軌跡表達(dá)式設(shè)為目標(biāo)函數(shù)，并通過(guò)構(gòu)建飛行走廊來(lái)添加硬性的約束條件，即目標(biāo)函數(shù)中的解必須要滿(mǎn)足這些條件。根據(jù)飛行走廊的形狀，硬約束法可分為長(zhǎng)方體法[9-10]、球體法[11]和多面體法[12]。以長(zhǎng)方體法為例，首先將前端算法獲得的每個(gè)航路點(diǎn)構(gòu)造為小立方體，然后延伸這些立方體的面，直到它們接近障礙物，形成一條走廊，再設(shè)計(jì)最小變加速度目標(biāo)函數(shù)，結(jié)合走廊約束條件，形成凸二次規(guī)劃問(wèn)題進(jìn)行求解。軟約束法則直接將約束條件設(shè)計(jì)成目標(biāo)函數(shù)，因此這些約束條件只需要傾向于滿(mǎn)足即可，但為了生成可靠的軌跡，需利用環(huán)境信息構(gòu)建歐氏符號(hào)距離場(chǎng)(ESDF)以提取梯度信息來(lái)加強(qiáng)約束。Zhou等[13]利用B樣條的凸包性質(zhì)，將梯度信息和動(dòng)力學(xué)約束結(jié)合為目標(biāo)函數(shù)，提高了軌跡的平滑度和安全性。Lin等[14]和筆者前期工作[1]中利用環(huán)境梯度信息最小化碰撞代價(jià)、平滑度和動(dòng)力學(xué)可行性的損失函數(shù)，以獲得最佳軌跡。硬約束法雖然可以利用飛行走廊避免碰撞，但依舊容易生成靠近障礙物的軌跡，而軟約束法在一定導(dǎo)航距離內(nèi)可100%生成安全性更高的軌跡。但若目標(biāo)點(diǎn)距起點(diǎn)較遠(yuǎn)，環(huán)境更復(fù)雜，建立ESDF所要處理的信息量更大，軟約束法效率和成功率也會(huì)明顯下降[15]。另外，近年來(lái)不少優(yōu)化無(wú)人機(jī)飛行速度的時(shí)間分配方案被提出。Gao等[16]提出了一種基于離散變量的時(shí)間分配方法，將優(yōu)化問(wèn)題轉(zhuǎn)化為凸二次錐規(guī)劃(SOCP)并求解。Zhang等[17]首次針對(duì)非凸輸入約束提出了基于四旋翼微分平坦度特性的時(shí)間最優(yōu)規(guī)劃方法。

為了提高無(wú)人機(jī)在長(zhǎng)距離導(dǎo)航中的規(guī)劃效率和飛行效率，本文提出了基于局部軟約束優(yōu)化的無(wú)人機(jī)航跡規(guī)劃方法。首先基于改進(jìn)的Theta*算法獲取初始軌跡，并針對(duì)原算法的安全問(wèn)題加入了安全距離約束，同時(shí)增加對(duì)擴(kuò)展節(jié)點(diǎn)轉(zhuǎn)彎代價(jià)的計(jì)算；然后提出了高效局部?jī)?yōu)化策略以限制ESDF構(gòu)建范圍和時(shí)間，采用軟約束法對(duì)初始軌跡中低質(zhì)量片段進(jìn)行優(yōu)化，而對(duì)高質(zhì)量片段僅做簡(jiǎn)單的平滑處理，同時(shí)分別針對(duì)軌跡中的直線段和曲線段設(shè)計(jì)了低階和高階的時(shí)間分配來(lái)提升無(wú)人機(jī)的飛行速度；最后通過(guò)仿真和實(shí)際飛行來(lái)測(cè)試本文方法的性能。

1 基于改進(jìn)Theta*的路徑搜索算法

本文提出的無(wú)人機(jī)航跡規(guī)劃方法框架見(jiàn)圖1，包括前端路徑搜索和后端軌跡優(yōu)化。改進(jìn)Theta*算法的流程圖如圖2所示，相比于前期工作[1]，Theta*算法利用視線檢查機(jī)制能刪減更多的冗余節(jié)點(diǎn)，如圖3(b)和3(c)所示，因而本文算法沿用了該機(jī)制。另外，受文獻(xiàn)[18]的啟發(fā)，本文將視線檢查提前而非放在最后進(jìn)行，這樣只需檢查上一個(gè)節(jié)點(diǎn)的最優(yōu)鄰節(jié)點(diǎn)而不必檢查每個(gè)鄰節(jié)點(diǎn)，從而減少多余的檢查工作。同時(shí)，為了獲取高質(zhì)量的初始軌跡，加入了安全距離約束和轉(zhuǎn)彎代價(jià)函數(shù)。

圖1 本文航跡規(guī)劃方法的框架Fig.1 Framework of the proposed trajectory planning method

1.1 安全距離約束

如圖3(c)所示，關(guān)鍵節(jié)點(diǎn)仍然靠近障礙物，但實(shí)際中無(wú)人機(jī)并不是節(jié)點(diǎn)，存在一定體積，很可能會(huì)與障礙物相撞。為了解決這個(gè)問(wèn)題，在視線檢查和安全檢查中的節(jié)點(diǎn)檢查部分用安全距離檢測(cè)代替障礙物檢測(cè)，如圖2所示。具體檢測(cè)步驟如下：首先提取離被檢測(cè)節(jié)點(diǎn)在dc以?xún)?nèi)的節(jié)點(diǎn)，dc即為安全距離；然后檢查這些節(jié)點(diǎn)是否為障礙物，其中屬于集合P和C中的節(jié)點(diǎn)不需要再次檢查，因?yàn)檫@些節(jié)點(diǎn)已經(jīng)在之前的循環(huán)中檢查過(guò)了。在安全檢查中，通過(guò)安全距離檢測(cè)來(lái)檢查當(dāng)前節(jié)點(diǎn)及其相鄰節(jié)點(diǎn)是否與障礙物保持安全距離，以確保由Theta*算法獲得的關(guān)鍵節(jié)點(diǎn)的安全；在視線檢查中，通過(guò)安全距離檢測(cè)檢查連接相鄰關(guān)鍵節(jié)點(diǎn)的線路上的節(jié)點(diǎn)是否安全，以確保連接線路也安全。圖3(d)中的虛線區(qū)即為安全距離約束下獲得的安全區(qū)域，整個(gè)路徑都能與障礙物保持一定的距離，使無(wú)人機(jī)有足夠的飛行空間。但是安全距離r的值要相對(duì)較小，若超過(guò)一定閾值，會(huì)使算法的搜索效率大大下降，所以需要根據(jù)實(shí)際飛行要求設(shè)定r為相對(duì)較小且能保證安全的值。

圖2 改進(jìn)Theta*算法的流程圖Fig.2 Flow chart of the improved Theta* algorithm

圖3 不同算法的路徑Fig.3 Paths of different algorithms

1.2 代價(jià)函數(shù)的改進(jìn)

Theta*算法遵循A*算法的總代價(jià)函數(shù)f(ni)，為

f(ni)=g(ni)+h(ni)

(1)

式中，g(ni)為節(jié)點(diǎn)ni和起點(diǎn)之間的代價(jià)函數(shù)，h(ni)為ni和終點(diǎn)ngoal之間的代價(jià)函數(shù)(即啟發(fā)式函數(shù))。文中g(shù)(ni)和h(ni)的代價(jià)計(jì)算是相同的：

g(ni)=g(ni-1)+H(ni-1,ni)

(2)

h(ni)=H(ni,ngoal)

(3)

(4)

為了便于算法的實(shí)現(xiàn)，將環(huán)境視為三維網(wǎng)格地圖[19]。因此，采用對(duì)角距離來(lái)計(jì)算成本，比曼哈頓距離更精確，而且避免了歐氏距離中的平方根運(yùn)算。設(shè)dx、dy、dz是節(jié)點(diǎn)ni和nj的三維坐標(biāo)之差，dmin、dmed、dmax是它們的最小值、中值和最大值，則hd(ni,nj)定義為

|dmax-dmed|

(5)

此外，考慮飛行中轉(zhuǎn)彎帶來(lái)的能量消耗和時(shí)間消耗，本文設(shè)計(jì)了轉(zhuǎn)彎代價(jià)函數(shù)ht(ni,nj)。計(jì)算出節(jié)點(diǎn)ni和其父節(jié)點(diǎn)ni.p構(gòu)成的向量vni.p→ni與節(jié)點(diǎn)ni和nj構(gòu)成的向量vni→nj之間的夾角，即得到無(wú)人機(jī)在當(dāng)前節(jié)點(diǎn)ni的真實(shí)轉(zhuǎn)彎角度，并表示為弧度作為轉(zhuǎn)彎代價(jià)的值，即

(6)

2 高效的軌跡優(yōu)化方法

2.1 局部?jī)?yōu)化策略

前端算法中有安全距離的約束，使初始路徑存在一定的安全保障，故不需要對(duì)整個(gè)路徑進(jìn)行優(yōu)化，即可以采取局部?jī)?yōu)化策略來(lái)減少優(yōu)化工作和提升方法效率。

首先，本文用s段貝塞爾曲線來(lái)表示軌跡：

(7)

(8)

初始路徑由若干個(gè)關(guān)鍵點(diǎn)(起點(diǎn)、終點(diǎn)和拐點(diǎn))組成，因此先采用1階貝塞爾曲線(即直線)連接相鄰關(guān)鍵點(diǎn)形成初始軌跡。但這樣的軌跡存在如下問(wèn)題：①拐點(diǎn)處不平滑；②在拐點(diǎn)密集處(如圖4中虛線圓內(nèi))，障礙物也密集，無(wú)人機(jī)發(fā)生碰撞的概率較大，存在安全隱患；③采用低階貝塞爾曲線對(duì)拐點(diǎn)密集處同時(shí)進(jìn)行時(shí)間分配和平滑處理是困難的。因此，本文考慮對(duì)超過(guò)一個(gè)拐點(diǎn)的局部進(jìn)行進(jìn)一步優(yōu)化，并用高階貝塞爾曲線表示，而其他部分(如局部單個(gè)拐點(diǎn))只用低階貝塞爾曲線進(jìn)行簡(jiǎn)單的平滑處理，未優(yōu)化的直線部分則保留。整個(gè)局部?jī)?yōu)化策略算法描述如下：

{輸入：初始軌跡。

輸出：最終軌跡。

初始化：從初始軌跡中提取包括關(guān)鍵點(diǎn)在內(nèi)的等間距航點(diǎn)(如圖4所示，航點(diǎn)個(gè)數(shù)根據(jù)軌跡長(zhǎng)度和航點(diǎn)間距大小而定)，并從起點(diǎn)到終點(diǎn)依次存入W數(shù)組。W數(shù)組中關(guān)鍵點(diǎn)的索引按順序存儲(chǔ)在K數(shù)組中。這樣關(guān)鍵點(diǎn)的間距

圖4 局部?jī)?yōu)化策略Fig.4 Local optimization strategy

d可以通過(guò)索引相減獲得的航點(diǎn)個(gè)數(shù)來(lái)表示，相比歐氏距離的計(jì)算，此方法減少了計(jì)算量，同時(shí)調(diào)參更方便。

局部?jī)?yōu)化檢測(cè)(省略對(duì)起點(diǎn)和終點(diǎn)的處理)

fori=1 ton-1 do ∥n為關(guān)鍵點(diǎn)個(gè)數(shù)且大于3

forj=i+1 ton-1 do

if (K[j]-K[j-1])

else break;

end for

if(j==i+1)

此時(shí)拐點(diǎn)W[K[i]]與下一個(gè)拐點(diǎn)W[K[i+1]]的間距大于a個(gè)航點(diǎn)間距，W[K[i]-b]、W[K[i]]、W[K[i]+b]被作為2階貝塞爾曲線的控制點(diǎn)，對(duì)相應(yīng)的軌跡片段進(jìn)行平滑處理;

else

此時(shí)拐點(diǎn)W[K[i]],W[K[i+1]],…,W[K[j]]的間距d都小于a個(gè)航點(diǎn)間距，這些拐點(diǎn)與W[K[i]-b]、W[K[j]+b]被作為局部軟約束優(yōu)化的關(guān)鍵點(diǎn)，對(duì)相應(yīng)的軌跡片段進(jìn)行優(yōu)化;

/*提取W[K[i]-b]和W[K[j]+b]兩個(gè)航點(diǎn)是為了保證未優(yōu)化的直線段和優(yōu)化后的曲線段連接處的平滑性，具體優(yōu)化方法詳見(jiàn)2.2節(jié)*/

i=j;∥從j開(kāi)始繼續(xù)循環(huán)

end for}

a值決定了局部?jī)?yōu)化范圍，b值決定了平滑范圍。平滑一般只處理直線、曲線的連接處，范圍較小，實(shí)驗(yàn)中b的范圍為[2,4]。a越大，軌跡中局部?jī)?yōu)化范圍越大，直線段數(shù)量越少，算法效率越低，因此a不宜過(guò)大；a越小，軌跡優(yōu)化效果越不明顯，會(huì)影響安全性。另外，為了讓無(wú)人機(jī)在直線段有足夠的距離加減速，以保證直線、曲線之間的速度連續(xù)性，直線段長(zhǎng)度ds(如圖4所示)需滿(mǎn)足

(9)

式中：dl為航點(diǎn)間距，實(shí)驗(yàn)中設(shè)為0.1 m；vmax、amax分別為無(wú)人機(jī)最大的速度和加速度。因此a也與無(wú)人機(jī)設(shè)定有關(guān)，實(shí)驗(yàn)中a的范圍為[10,30]。

2.2 局部軟約束優(yōu)化

在優(yōu)化前需先構(gòu)建歐氏符號(hào)距離場(chǎng)(ESDF)，它能為其所在范圍內(nèi)每一個(gè)體素提供該體素與最近障礙物的歐氏距離，一般采用截?cái)嗑嚯x法[20]構(gòu)建。因?yàn)榫植績(jī)?yōu)化只需要局部的障礙物信息，所以只需構(gòu)建局部ESDF而非全局，這樣也能減少相應(yīng)的規(guī)劃時(shí)間。首先采用局部?jī)?yōu)化策略得到某個(gè)軌跡段的所有關(guān)鍵點(diǎn)坐標(biāo)(xi,yi,zi)|i=1,2,…,n，分別比較它們的x、y、z坐標(biāo)值，得到x、y、z坐標(biāo)的最小值和最大值(xmin、xmax、ymin、ymax、zmin、zmax)。然后根據(jù)坐標(biāo)軸上這6個(gè)值處的垂直平面，構(gòu)建一個(gè)包含該軌跡片段的長(zhǎng)方體，再對(duì)長(zhǎng)方體膨脹一定體積，將周?chē)^近的障礙物囊括進(jìn)來(lái)。這樣根據(jù)該長(zhǎng)方體及其所包含的障礙物信息就可以構(gòu)建局部的ESDF。

在優(yōu)化中實(shí)施了兩步優(yōu)化機(jī)制，即將軌跡優(yōu)化問(wèn)題分解成空間和時(shí)間上的優(yōu)化，該機(jī)制的效果已在文獻(xiàn)[16]中得到驗(yàn)證。時(shí)間上的優(yōu)化就是通過(guò)對(duì)時(shí)間參數(shù)的設(shè)定來(lái)限制無(wú)人機(jī)的速度和加速度，也就是本文所謂的時(shí)間分配。為了減少重復(fù)的工作，在優(yōu)化中只需要對(duì)軌跡在空間上的位置進(jìn)行限制，時(shí)間參數(shù)和無(wú)人機(jī)速度則被設(shè)置為常量，構(gòu)建如下目標(biāo)函數(shù)：

ming=β1gs+β2gc

(10)

式中：gs為加強(qiáng)軌跡平滑性的平滑項(xiàng)；gc為限制軌跡碰撞保證安全性的碰撞項(xiàng)；β1和β2為權(quán)重，實(shí)驗(yàn)中分別設(shè)置為2.0和1.5。

本文將平滑項(xiàng)gs表示成加速度二階導(dǎo)數(shù)的形式，即軌跡fμ(t)的4階導(dǎo)數(shù)的平方對(duì)時(shí)間t的積分，具體形式如下：

(11)

式中：[ti-1,ti]為局部軌跡片段的時(shí)間范圍；fμ(t)為m段n階多項(xiàng)式(實(shí)驗(yàn)中n=5)，m為該局部的關(guān)鍵點(diǎn)個(gè)數(shù)減1，即每段多項(xiàng)式表示相鄰關(guān)鍵點(diǎn)間的軌跡段。

碰撞項(xiàng)采用可微函數(shù)c(·)沿航跡弧長(zhǎng)的線性積分來(lái)表示，同時(shí)本文將c(·)設(shè)計(jì)成指數(shù)函數(shù)，即

c(dp(t))=αe-γ(dp(t)-d0)

(12)

式中：dp(t)為t時(shí)刻無(wú)人機(jī)的位置p(t)與最近障礙物的距離，由所構(gòu)建的ESDF直接獲得；d0為安全距離閾值；α為函數(shù)的幅值；γ為變化率。根據(jù)該函數(shù)c(·)，當(dāng)無(wú)人機(jī)與最近障礙物的距離小于安全距離d0時(shí)，函數(shù)值能迅速變得很大，使碰撞項(xiàng)成為整個(gè)目標(biāo)函數(shù)的主導(dǎo)項(xiàng);而當(dāng)距離大于d0時(shí)，函數(shù)值趨于平緩且相對(duì)較小，這樣就能使軌跡與障礙物保持安全距離。因此gc的具體形式如下(為了方便計(jì)算，積分最終離散化為求和形式)：

(13)

其中，l為該軌跡段的長(zhǎng)度，Ti=ti-ti-1為該軌跡段的時(shí)間參數(shù)，Γk=ti-1+kδt為離散的時(shí)間變量，v(·)為無(wú)人機(jī)某時(shí)刻的速度。

上述平滑項(xiàng)和碰撞項(xiàng)的具體二次型形式和它們的雅可比矩陣推導(dǎo)可參考文獻(xiàn)[1]，這里不再贅述。而對(duì)于該優(yōu)化問(wèn)題的求解，本文采用凸可分近似算法(CCSA)[21]。該算法不僅適用于大量變量的優(yōu)化，而且能保證從任何起始點(diǎn)開(kāi)始都能收斂到某個(gè)局部最小值。

2.3 時(shí)間分配

(14)

因此第j段的速度貝塞爾曲線可由式(8)推得

(15)

同理，第j段的加速度貝塞爾曲線可由速度貝塞爾曲線的速度特性推得，這里不再贅述。根據(jù)速度和加速度貝塞爾曲線可知，時(shí)間參數(shù)越小，無(wú)人機(jī)速度和加速度越大，完成相應(yīng)軌跡的飛行所需時(shí)間越短，但由于無(wú)人機(jī)有最大速度和加速度的限制，每個(gè)時(shí)間參數(shù)理論上都有一個(gè)最小值。

經(jīng)過(guò)簡(jiǎn)單平滑處理和局部?jī)?yōu)化后，本文方法獲得的軌跡是由低階多項(xiàng)式(1階、2階)和高階多項(xiàng)式(5階)組合表示的，因此設(shè)計(jì)了包含低階和高階時(shí)間分配的方法。本文先實(shí)現(xiàn)高階時(shí)間分配再實(shí)現(xiàn)低階時(shí)間分配，這樣約束較多的高階時(shí)間分配可以避免考慮相鄰軌跡段間速度連續(xù)的問(wèn)題。在高階時(shí)間分配中，本文直接采用文獻(xiàn)[16]的時(shí)間分配框架，調(diào)整了該框架的輸入輸出，其中以軌跡段的控制點(diǎn)位置、初始時(shí)間參數(shù)、最大速度和最大加速度為輸入，以最優(yōu)時(shí)間參數(shù)、該軌跡段起點(diǎn)初速度和終點(diǎn)末速度為輸出。

在低階時(shí)間分配中，軌跡段主要包含表示直線的1階貝塞爾曲線和表示直線間小段彎弧的2階貝塞爾曲線，其中2階曲線較短，只需為其設(shè)置相對(duì)合理的時(shí)間參數(shù)、初速度和末速度。而對(duì)于直線段軌跡，它的長(zhǎng)度ds如圖4所示，往往接近或大于a個(gè)航點(diǎn)間距，足以為其劃分出3個(gè)區(qū)域：加速區(qū)、勻速區(qū)和減速區(qū)。因此本文重新表示該軌跡段，其中加速區(qū)和減速區(qū)用2階貝塞爾曲線表示(有加速度，階數(shù)必須大于1)，勻速區(qū)用1階貝塞爾曲線表示，而且它們的控制點(diǎn)都在該直線上。這樣無(wú)人機(jī)在該直線段軌跡上先從初速度vs加速到最大速度vm進(jìn)行勻速飛行，再減速到末速度ve進(jìn)入下一段軌跡，其中加減速時(shí)的加速度為最大加速度am,vs、ve由與該直線段相鄰的曲線段在前面所述的時(shí)間分配后提供。然后計(jì)算得到加速區(qū)、勻速區(qū)和減速區(qū)的長(zhǎng)度d1、d2、d3及對(duì)應(yīng)的時(shí)間參數(shù)t1、t2、t3(d為該直線段總長(zhǎng))：

(16)

最后根據(jù)這些已知量和式(15)，可以求得表示這3個(gè)區(qū)域的多項(xiàng)式中控制點(diǎn)的具體位置。另外，若無(wú)人機(jī)飛行中發(fā)生重規(guī)劃，那起點(diǎn)處是有初速度的。大部分情況下只需在重規(guī)劃的時(shí)間分配中加入初速度限制即可，但若初速度較大且方向與重規(guī)劃后的軌跡方向誤差較大，本文考慮讓無(wú)人機(jī)降速至0再進(jìn)行重規(guī)劃。

3 實(shí)驗(yàn)及結(jié)果分析

3.1 仿真實(shí)驗(yàn)

本文基于機(jī)器人操作系統(tǒng)(ROS)框架和C/C++語(yǔ)言實(shí)現(xiàn)規(guī)劃方法。實(shí)驗(yàn)平臺(tái)配置如下：處理器為Intel(R) Core(TM) i5-6500(3.20 GHz×4)，內(nèi)存為16 GB，系統(tǒng)版本為Ubuntu 18.04。實(shí)驗(yàn)中采用了文獻(xiàn)[9]開(kāi)源的四旋翼模型，其輸入為當(dāng)前時(shí)刻t的位置、速度和加速度向量，這些輸入量可從規(guī)劃方法所生成的軌跡及其相應(yīng)的表達(dá)式獲得。模型感知范圍是以模型為中心的圓，感知半徑可人為設(shè)置。地圖是障礙物隨機(jī)分布的三維場(chǎng)景(50 m×50 m×6 m)。實(shí)驗(yàn)從單次規(guī)劃和多次規(guī)劃兩種情況對(duì)本文方法進(jìn)行測(cè)試，最大速度和加速度分別為2 m/s和2 m/s2，數(shù)據(jù)都是30次實(shí)驗(yàn)的平均值，其中關(guān)鍵點(diǎn)數(shù)為Nk，軌跡長(zhǎng)度為L(zhǎng)，飛行時(shí)間為tf，軌跡合格率為R，規(guī)劃時(shí)間為tg(包括了ESDF時(shí)間tESDF和前后端時(shí)間tba)，障礙物數(shù)為No，飛行距離為df，規(guī)劃次數(shù)為Ng，規(guī)劃總時(shí)間為tg,t。

3.1.1 單次規(guī)劃的性能測(cè)試

表1 單次航跡規(guī)劃的性能測(cè)試結(jié)果Table 1 Performance test results of single trajectory planning

表2 不同轉(zhuǎn)彎代價(jià)權(quán)值下的測(cè)試結(jié)果Table 2 Test results under different turn cost weights

圖5 單次航跡規(guī)劃效果Fig.5 Effect of single trajectory planning

3.1.2 多次規(guī)劃的性能測(cè)試

未知環(huán)境是指無(wú)人機(jī)還未感知的環(huán)境。當(dāng)目標(biāo)點(diǎn)在較遠(yuǎn)的未知環(huán)境中，無(wú)人機(jī)飛行時(shí)感知的環(huán)境會(huì)不斷更新(仿真中更新頻率為50 Hz)，若已規(guī)劃出來(lái)的航跡碰到更新后的障礙物就會(huì)進(jìn)行重規(guī)劃，因此需要多次規(guī)劃才能到達(dá)目標(biāo)點(diǎn)。在實(shí)際應(yīng)用中，無(wú)人機(jī)感知范圍有限(一般在20 m以?xún)?nèi)，實(shí)驗(yàn)中設(shè)置為10 m)，目標(biāo)點(diǎn)距離往往較遠(yuǎn)，故對(duì)規(guī)劃方法在長(zhǎng)距離中實(shí)現(xiàn)多次規(guī)劃的性能測(cè)試是有必要的。文獻(xiàn)[1]方法由于需要構(gòu)建全局ESDF，在更長(zhǎng)距離的規(guī)劃中效率極低，很難達(dá)到實(shí)時(shí)要求，因此采用本文方法與當(dāng)前經(jīng)典且開(kāi)源的硬約束法[9](兩者單次規(guī)劃效果差距不明顯)分別做了障礙物數(shù)為250、500和750的3組實(shí)驗(yàn)(規(guī)劃距離約為40 m)，結(jié)果如表3、圖6和圖7所示。從表3中可知，無(wú)論哪組實(shí)驗(yàn)，本文方法的飛行距離、規(guī)劃效率(規(guī)劃總時(shí)間和次數(shù))和飛行效率(飛行時(shí)間)均優(yōu)于硬約束法。在飛行距離方面，因?yàn)橛布s束法的前后端都沒(méi)有對(duì)軌跡長(zhǎng)度加以限制，其后端中也是全局構(gòu)建飛行走廊，在未知環(huán)境中容易生成冗余的軌跡(如圖6(a)所示)，在重規(guī)劃前，無(wú)人機(jī)會(huì)按該冗余軌跡飛行，而本文方法的前端能搜索出最短路徑，且后端只進(jìn)行局部?jī)?yōu)化，未知環(huán)境中基本上是直線軌跡(如圖6(b)所示)。從圖7(a)可知，本文方法的整個(gè)飛行航跡明顯比硬約束法簡(jiǎn)短，轉(zhuǎn)彎幅度也小很多。在規(guī)劃效率方面，因?yàn)槲粗h(huán)境中軌跡越長(zhǎng)，撞到更新后的障礙物概率越大，規(guī)劃次數(shù)也越多，同時(shí)，軌跡合格率較低也會(huì)使規(guī)劃次數(shù)增加。雖然硬約束法的單次規(guī)劃效率可能更高，但由于其規(guī)劃次數(shù)多，故規(guī)劃總時(shí)間要多于本文方法。在飛行效率方面，除了飛行距離的優(yōu)勢(shì)外，本文方法的最大優(yōu)勢(shì)是時(shí)間分配的合理性，曲線段和直線段軌跡都采用了高效的時(shí)間分配，如圖7(b)所示，本文方法盡可能利用最大加速度進(jìn)行加減速，讓無(wú)人機(jī)在直線段軌跡中更多地進(jìn)行最大速度的勻速飛行，而硬約束法利用速度場(chǎng)獲取相對(duì)合理的時(shí)間參數(shù)，過(guò)于保守。在軌跡合格率方面，因障礙物較少，兩種方法的效果都很好，但隨著障礙物的增加，硬約束法的合格率會(huì)下降得更快，因?yàn)轱w行走廊只是邊界約束，允許軌跡貼近該邊界(即貼近障礙物)，而本文方法前端加入安全距離約束，后端對(duì)障礙物密集的地方進(jìn)行軟約束優(yōu)化進(jìn)一步加強(qiáng)安全性，軟約束通過(guò)構(gòu)建ESDF所帶來(lái)的安全效益要優(yōu)于飛行走廊。不過(guò)如果障礙物個(gè)數(shù)超過(guò)1 000，關(guān)鍵點(diǎn)數(shù)增加，局部構(gòu)建ESDF的范圍也會(huì)變大，軟約束優(yōu)化的效率和精度也會(huì)明顯下降。

表3 多次航跡規(guī)劃的性能測(cè)試結(jié)果Table 3 Performance test results of multiple trajectory planning

圖6 不同規(guī)劃方法生成的航跡對(duì)比Fig.6 Comparison of tracks generated by different planning methods

圖7 兩種方法的無(wú)人機(jī)航跡、速度對(duì)比Fig.7 UAV track and speed comparisons of two methods

3.2 室內(nèi)飛行實(shí)驗(yàn)

本文搭建圖8所示的四旋翼平臺(tái)。該平臺(tái)配備了一個(gè)英偉達(dá)Jetson TX2，它包含一個(gè)由256個(gè)CUDA內(nèi)核組成的圖形處理器，有8 GB的隨機(jī)存儲(chǔ)內(nèi)存和32 GB的eMMC存儲(chǔ)空間。本文將PX4的開(kāi)源硬件Pixhawk2用作飛行控制器，并通過(guò)UART接口將TX2的無(wú)人機(jī)控制命令發(fā)送給Pixhawk2，實(shí)現(xiàn)飛行控制。本文在TX2中構(gòu)建了Ubuntu 16.04操作系統(tǒng)并基于ROS框架實(shí)現(xiàn)圖1所示的航跡規(guī)劃框架。本文還搭建了圖9所示的室內(nèi)場(chǎng)景，障礙物大小相同且都是長(zhǎng)方體。本文只測(cè)試航跡規(guī)劃的效果，所以每次飛行前地圖通過(guò)1∶1人工構(gòu)建，四旋翼起點(diǎn)和終點(diǎn)在地圖中固定，相距約8 m，只需進(jìn)行單次規(guī)劃。對(duì)于規(guī)劃方法生成的航跡，本文采用幾何控制器[22]進(jìn)行跟蹤。另外，四旋翼飛行中可通過(guò)雙目相機(jī)獲取其與障礙物的距離來(lái)檢測(cè)航跡規(guī)劃算法的安全性。光流模塊則用于室內(nèi)定位。

圖8 四旋翼平臺(tái)Fig.8 Quadrotor platform

飛行中設(shè)定最大速度和加速度分別為0.5 m/s和0.5 m/s2，本文方法的室內(nèi)航跡規(guī)劃效果見(jiàn)圖9，虛線框內(nèi)為局部?jī)?yōu)化后實(shí)現(xiàn)的曲線飛行，實(shí)驗(yàn)中四旋翼無(wú)碰撞到達(dá)終點(diǎn)，且始終與障礙物保持一定的距離，其tESDF和tba分別為0.095 s和0.150 s。另外，本文統(tǒng)計(jì)了不同障礙物數(shù)下的飛行結(jié)果，如表4所示，每個(gè)數(shù)據(jù)都是進(jìn)行20次飛行的平均值，其中r為曲線占比。從表中可知，隨著障礙物的增加，后端優(yōu)化后的曲線占比也增加，這保證了軌跡合格率始終在0.9及以上，并且四旋翼按合格軌跡飛行時(shí)無(wú)碰撞發(fā)生，但軌跡長(zhǎng)度和飛行時(shí)間也有所增加。該結(jié)果證明了本文方法可根據(jù)障礙物疏密程度切換直線和曲線兩種飛行模式，其中直線飛行速度接近0.4 m/s，而曲線飛行速度低于0.3 m/s，這樣能同時(shí)保證較高的飛行安全性和飛行效率。

表4 四旋翼室內(nèi)飛行結(jié)果Table 4 Results of quadrotor indoor flight

4 總結(jié)與展望

針對(duì)無(wú)人機(jī)長(zhǎng)距離航跡規(guī)劃效率不高的問(wèn)題，本文提出了一種基于局部軟約束優(yōu)化的實(shí)時(shí)無(wú)人機(jī)航跡規(guī)劃方法。該方法分為前端和后端兩個(gè)階段：前端通過(guò)安全距離約束和轉(zhuǎn)彎代價(jià)函數(shù)改進(jìn)Theta*算法，獲取高質(zhì)量的初始路徑；后端設(shè)計(jì)局部?jī)?yōu)化策略減少優(yōu)化工作，并對(duì)局部質(zhì)量不高的軌跡片段進(jìn)行軟約束優(yōu)化加強(qiáng)安全性和平滑性，然后根據(jù)貝塞爾曲線的速度特性實(shí)現(xiàn)高效的時(shí)間分配，以提升飛行效率。實(shí)驗(yàn)結(jié)果表明：本文方法克服了前期工作中無(wú)法實(shí)現(xiàn)高效的長(zhǎng)距離規(guī)劃問(wèn)題；與現(xiàn)有的經(jīng)典硬約束法相比，在約40 m距離的飛行中，本文方法的無(wú)人機(jī)飛行距離要短3 m左右，飛行時(shí)間少7 s多，規(guī)劃總時(shí)間少0.5 s以上。實(shí)際的室內(nèi)飛行實(shí)驗(yàn)也驗(yàn)證了本文方法在真實(shí)系統(tǒng)中的適用性。

本文方法提升了無(wú)人機(jī)的整體飛行速度，但其安全性只在靜態(tài)環(huán)境中有保障，對(duì)于動(dòng)態(tài)環(huán)境中高速飛行的緊急避障性能還有待提高。在今后工作中，可以考慮采用精度更高的B樣條曲線來(lái)表示航跡，利用其局部修改性質(zhì)來(lái)避免全局重規(guī)劃，以提升整體規(guī)劃效率，完善緊急避障功能。