王 慶, 徐海明, 呂 品, 蔣 銳, 苗東東

(1.合肥工業(yè)大學(xué) 工業(yè)與裝備技術(shù)研究院,安徽 合肥 230009; 2.中國科學(xué)院 合肥智能機(jī)械研究所,安徽 合肥 230031; 3.中國科學(xué)院 合肥技術(shù)創(chuàng)新工程院,安徽 合肥 230088)

隨著無人機(jī)技術(shù)的不斷發(fā)展成熟,特別是體積小且廉價的微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)慣性導(dǎo)航系統(tǒng)的成功研制,無人機(jī)正逐步從軍工領(lǐng)域向民用領(lǐng)域過渡。其中多旋翼無人機(jī)僅通過改變螺旋槳轉(zhuǎn)速就能實現(xiàn)上下、前后、左右、偏航、橫滾、俯仰運(yùn)動,同時結(jié)構(gòu)簡單、安全可靠,現(xiàn)已成為當(dāng)下最主流的飛行器,被廣泛應(yīng)用于航拍、電力巡檢、警用安防、農(nóng)業(yè)植保、地圖測繪等領(lǐng)域。

航跡規(guī)劃是無人機(jī)實現(xiàn)自主飛行的關(guān)鍵技術(shù)之一,國內(nèi)外學(xué)者已經(jīng)對其做了大量的研究工作,也取得了很多成果。航跡規(guī)劃實際上就是在滿足一定的約束條件下,找到一條從初始點到目標(biāo)點最優(yōu)無碰撞的航行路線。常見的航跡規(guī)劃方法包括A*算法[1-3]、人工勢場法[4-5]、最優(yōu)控制法、可視圖法、Voronoi圖法[6]等傳統(tǒng)算法以及遺傳算法[7]、蟻群算法、粒子群算法[8]等智能算法。

傳統(tǒng)的航跡規(guī)劃算法以A*算法和人工勢場法最為典型。由于借助了啟發(fā)式函數(shù)的引導(dǎo),A*算法通常能擁有更好的性能,但是該算法的運(yùn)算難度會隨著維數(shù)的增加呈指數(shù)型的增長,因此一般只適用于簡單的二維空間的路徑規(guī)劃;人工勢場法在復(fù)雜環(huán)境中容易發(fā)生震蕩且易陷入局部最優(yōu)解;其他傳統(tǒng)算法針對三維的路徑規(guī)劃都有一定的局限性。隨著人工智能算法的發(fā)展,蟻群算法、粒子群算法、遺傳算法、神經(jīng)網(wǎng)絡(luò)等方法因具有很強(qiáng)的魯棒性、正反饋機(jī)制和自組織特點,更適用于三維路徑規(guī)劃計算。文獻(xiàn)[9]將最短距離信息發(fā)送給系統(tǒng)進(jìn)行控制,并通過改進(jìn)狀態(tài)轉(zhuǎn)移概率的計算來優(yōu)化節(jié)點選擇方法;文獻(xiàn)[10]提出了一種信息素調(diào)節(jié)的方法,將航路上殘留信息素限制在一定范圍內(nèi),從而避免因信息素過于集中導(dǎo)致的蟻群算法早熟;文獻(xiàn)[11]將三維規(guī)劃分解成二維的平面規(guī)劃和高度規(guī)劃,采用自適應(yīng)調(diào)整系數(shù)和幾何優(yōu)化的方法來提升螞蟻的個體交互能力和相互引導(dǎo)性,但其規(guī)劃空間仍然是準(zhǔn)三維環(huán)境;文獻(xiàn)[12]將當(dāng)前的最優(yōu)與最差路徑引入信息素的獎罰機(jī)制來更新全局信息素,但只是將航跡距離作為影響因子,最終規(guī)劃的路徑折線偏多;文獻(xiàn)[13]采用當(dāng)前最優(yōu)航跡信息素和歷史最優(yōu)航跡更新相融合的方法,引入信息素回退清零機(jī)制并進(jìn)行離線計算,雖然提高了搜索效率,但是迭代次數(shù)沒有顯著降低。

本文提出的三維航跡規(guī)劃算法充分考慮了起始點到目標(biāo)點的直線因素,優(yōu)化初始信息素濃度作為蟻群搜索的先驗知識;提出了基于時間與空間的自適應(yīng)揮發(fā)系數(shù),加快了收斂速度并保證了搜索范圍;在分析多旋翼無人機(jī)的飛行約束條件后,將前、后節(jié)點的轉(zhuǎn)角值和節(jié)點對于“理想路徑”的偏移值加入啟發(fā)函數(shù)計算,在縮短航跡長度的同時,盡可能減少各段航跡轉(zhuǎn)角和。最后通過仿真實驗驗證了本文算法的有效性。

1 航跡規(guī)劃問題建模

1.1 航跡的優(yōu)化問題描述

本文采用的航跡規(guī)劃策略是從起始點到目標(biāo)點順序生成若干節(jié)點,在節(jié)點處航跡可能存在轉(zhuǎn)角,若轉(zhuǎn)角過大,則允許無人機(jī)懸停并調(diào)整航向至下一節(jié)點,再啟動飛行,因此多旋翼無人機(jī)可以很好地跟隨任意軌跡,而忽略轉(zhuǎn)彎角和爬升角的限制。

與固定翼相比,多旋翼最大的缺點是續(xù)航時間短,并且多旋翼通常在低空的復(fù)雜環(huán)境下低速飛行,飛行距離有限,航跡較為復(fù)雜。因為軌跡上的折點會造成運(yùn)動的不連續(xù),導(dǎo)致無人機(jī)頻繁地降速至懸停再啟動加速,所以相同長度的2段軌跡折點較多的花費(fèi)的時間會更長,能耗更高。對于多旋翼無人機(jī)的航跡規(guī)劃,轉(zhuǎn)彎角和爬升角只作為航線優(yōu)化參數(shù),不同于固定翼那樣有數(shù)值大小的限制。

1.2 三維空間環(huán)境建模

本文建立的無人機(jī)飛行的三維空間模型如下:

z1(x,y)=sin(y+a)+bsinx+ecosy+

(1)

(2)

z(x,y)=max(z1(x,y),z2(x,y))

(3)

其中:x、y分別為數(shù)字地圖模型中點在水平面投影的坐標(biāo);z為地圖坐標(biāo)點(x,y)點的高度;a、b、c、d、e、f、g均為常數(shù);n為環(huán)境中山峰的個數(shù);(xi,yi)為第i個山峰的峰頂坐標(biāo);hi為峰頂?shù)母叨?xis,yis分別為第i個山峰沿x軸和y軸方向上的衰減量,用來控制山體的坡度。

采用(1)式建立基準(zhǔn)地形模型來模擬較平坦的地面,采用(2)式指數(shù)函數(shù)模擬無人機(jī)飛行中遇到的較高山體,再將(1)式、(2)式的數(shù)據(jù)融合得到(3)式,即形成了最終無人機(jī)飛行的較為復(fù)雜的三維空間模型。利用(1)～(3)式在Matlab中建立三維仿真地圖,用于無人機(jī)規(guī)劃航跡的模擬實驗。

1.3 面向蟻群算法的空間離散化

參考文獻(xiàn)[14]，將復(fù)雜的連續(xù)空間離散為若干個空間點。假設(shè)飛行的起始點坐標(biāo)為S(x0,y0,z0),目標(biāo)點坐標(biāo)為T(xd,yd,zd),可以得知起始點到目標(biāo)點在x軸方向、y軸方向和z軸方向的距離分別為|xd-x0|、|yd-y0|、|zd-z0|;取這三者最大值作為運(yùn)動的主方向(本文假設(shè)為x軸正方向),即螞蟻移動的主方向,沿此方向?qū)⒁?guī)劃空間進(jìn)行n等分,得到n+1個平面Mi:x=xi(i=0,1,2,…,n),再分別沿y軸方向和z軸方向?qū)⑵矫鍹i(i=0,1,2,…,n)進(jìn)行m等分和l等分;最終得到三維離散化的規(guī)劃空間圖形。該空間是由(n+1)×(m+1)×(l+1)個空間點組成的集合,這些空間點可以表示為p(i,j,k) (i=0,1,…,n;j=0,1,…,m;k=0,1,…,l),從而得到任意空間點的坐標(biāo)為(ixgrid,jygrid,kzgrid),其中,xgrid、ygrid、zgrid為各個方向等分后的間距。

1.4 螞蟻移動規(guī)則

為了簡化空間規(guī)劃的復(fù)雜程度,規(guī)定螞蟻沿著主方向進(jìn)行的移動必須順序經(jīng)過每個平面Mi(i=1,2,…,n),并且限制了另2個方向的移動量(ymax≤p=1,zmax≤q=1),從而簡化了蟻群的搜索空間,減少了計算機(jī)進(jìn)行數(shù)據(jù)處理的運(yùn)算量和儲存量,進(jìn)而提高蟻群算法的搜索效率。

無人機(jī)在實際環(huán)境中飛行時,起始點到目標(biāo)點在高度方向上的距離比經(jīng)緯度方向上的距離短?，F(xiàn)假設(shè)在仿真中將x軸方向作為運(yùn)動的主方向,起始點與目標(biāo)點在x軸方向上的距離與在y軸方向上的距離相差不大,且都大于z軸方向上的位移,則螞蟻在z軸方向移動的過程中會有更多的靈活性;相反,在y軸方向的移動則需要更多地考慮始終向目標(biāo)點靠近,否則螞蟻在移動到最后階段,在y方向上距離目標(biāo)點有較大差距,最終導(dǎo)致螞蟻會在每次搜索的最后一步,即由平面Mn-1到平面Mn移動時會橫跨多個柵格,雖然這2個節(jié)點都屬于無障礙點,但是這2點的連線可能會穿過障礙點,導(dǎo)致航跡規(guī)劃失敗。文獻(xiàn)[15]將2個副運(yùn)動方向上每步可移動的最大移動量定為p=q=2,該方法雖然增加了螞蟻移動的靈活性,在復(fù)雜環(huán)境中有更好的適應(yīng)能力,但是可能會造成航跡轉(zhuǎn)彎過大、運(yùn)動不連續(xù)等問題。

為了解決這些問題,本文提出將起始點到目標(biāo)點連線在水平面上的投影作為主方向,因此要對坐標(biāo)系進(jìn)行旋轉(zhuǎn),為了保證旋轉(zhuǎn)后起始點仍位于新坐標(biāo)系下第1個平面M0′內(nèi),選擇繞起始點S旋轉(zhuǎn)坐標(biāo)系O-XYZ至x軸與起始點和目標(biāo)點連線平行,旋轉(zhuǎn)后的x軸即為運(yùn)動的主方向,旋轉(zhuǎn)得到新的坐標(biāo)系記作O-X′Y′Z′。規(guī)劃空間中離散點在新的坐標(biāo)系下的坐標(biāo)值為:

(4)

其中:(x,y,z)為任意空間點在原坐標(biāo)系下的坐標(biāo);(x′,y′,z′)為該點在新坐標(biāo)系下的坐標(biāo);(x0,y0)為起始點在水平面的坐標(biāo);θ為順時針旋轉(zhuǎn)的角度。

最后,將O-X′Y′Z′下規(guī)劃的航跡點通過逆運(yùn)算還原成O-XYZ下的坐標(biāo),并形成最終規(guī)劃航跡。

2 基于改進(jìn)蟻群算法的航跡規(guī)劃

2.1 蟻群算法的理論基礎(chǔ)

蟻群在移動并選擇下一節(jié)點時會遵循一定的概率,此概率可以表示為:

(5)

其中:τ為節(jié)點的信息素濃度;η為啟發(fā)信息。

將N只螞蟻置于起始點,通過(5)式分別計算每只螞蟻下一個可行節(jié)點的概率,通過輪賭法按概率隨機(jī)選擇下一節(jié)點,直至螞蟻最終到達(dá)目標(biāo)點。當(dāng)N只螞蟻全都到達(dá)目標(biāo)點以后,根據(jù)每只螞蟻運(yùn)動的路線長度更新節(jié)點的信息素濃度值,重新計算轉(zhuǎn)移概率值P,重復(fù)上述步驟Nmax次,最終得到最優(yōu)路徑bestpath。

2.2 初始信息素的高斯分布

因為傳統(tǒng)蟻群算法在處理旅行商問題(traveling salesman problem,TSP)時每個城市都必須經(jīng)過,所以任意一條邊的初始信息素取值都相同;而三維環(huán)境中空間點的數(shù)量要遠(yuǎn)遠(yuǎn)大于螞蟻路過的節(jié)點數(shù),如果信息素在空間上分布均勻,那么螞蟻在初始階段搜索的范圍太廣且無目標(biāo)指向,會導(dǎo)致迷失方向的螞蟻數(shù)量增加,影響航跡規(guī)劃效果。文獻(xiàn)[12]采用的初始信息素會使在直線上的節(jié)點濃度過大,而離直線ST距離遠(yuǎn)的節(jié)點信息素濃度又偏小,影響了螞蟻的搜索范圍。

因此本文考慮了空間節(jié)點“理想最優(yōu)路徑”,即初始點到目標(biāo)點連線ST的距離因素,設(shè)計空間節(jié)點的初始信息素τ表達(dá)式如下:

τ=τ0exp(-l2)

(6)

其中:τ0為一常數(shù),代表直線ST上節(jié)點的初始信息素,同時也是初始信息素最大值;l為空間上任一節(jié)點P到直線ST的距離,即

初始信息素濃度呈高斯分布的設(shè)計增強(qiáng)了蟻群在靠近直線ST附近節(jié)點的搜索力度,有利于減少螞蟻搜索的盲目性,大幅度提高了第1代螞蟻搜索的質(zhì)量,能夠集中螞蟻沿直線搜索快速收斂到最短路徑;同時也保證了蟻群的搜索范圍,不至于過早地陷入局部最優(yōu)。

2.3 信息素的更新策略

蟻群在覓食過程中需要信息素的引導(dǎo),而信息素的更新策略很大程度上影響著路徑規(guī)劃的收斂速度。信息素更新包括局部信息素更新和全局信息素更新,其中局部信息素更新是指螞蟻每經(jīng)過一個節(jié)點即對其進(jìn)行更新,目的是降低其他螞蟻重復(fù)經(jīng)過該節(jié)點的概率。局部信息素的表達(dá)式為:

τijk=(1-α)τijk

(7)

當(dāng)所有螞蟻完成一輪路徑搜索后,結(jié)合每只螞蟻所走的路徑長度,更新全局信息素,表達(dá)式如下:

(8)

其中:ρ為全局信息素?fù)]發(fā)系數(shù);Fm為第m只螞蟻走的路徑綜合評價值;Q為常數(shù)。

三維空間規(guī)劃問題規(guī)模較大,由于揮發(fā)系數(shù)ρ的作用使得未被搜索的節(jié)點上的信息素減小過快,降低了全局的搜索能力;相反,揮發(fā)系數(shù)ρ過小,會使螞蟻已搜索的節(jié)點信息素過大而易陷入局部最優(yōu)。文獻(xiàn)[15]提出了二維空間下的基于時間和空間的自適應(yīng)揮發(fā)系數(shù),基于時間的揮發(fā)系數(shù)ρ(t)是為了避免算法陷入局部最優(yōu),而基于空間的揮發(fā)系數(shù)ρ(l)將搜索區(qū)域劃分為重要區(qū)域、核心區(qū)域和非核心區(qū)域。本文將該方法向三維空間進(jìn)行擴(kuò)展,使其既能進(jìn)一步加快算法的收斂速度,又能提高蟻群算法的全局搜索能力。其表達(dá)式如下:

ρ(t)=1.05ρ(t)

(9)

(10)

其中:F(x,y,z)=0為空間直線ST的方程;Mmid為螞蟻主運(yùn)動方向的中間分割面;δ為常數(shù),用于平衡ρ(t)和ρ(l)的大小。

將基于時間和基于空間上的信息素?fù)]發(fā)系數(shù)相融合,同時為其設(shè)定了上、下限,得到的揮發(fā)系數(shù)表達(dá)式如下:

(11)

2.4 啟發(fā)函數(shù)的設(shè)計

螞蟻選擇節(jié)點概率的另一個影響因素為啟發(fā)值,啟發(fā)函數(shù)反應(yīng)當(dāng)前節(jié)點到下個可行節(jié)點的能見度和期望程度,直接決定了螞蟻尋找航跡的快慢和準(zhǔn)確度。在TSP問題中啟發(fā)值一般取為1/dij,dij表示從i點到j(luò)點的距離。但是三維空間中的航跡規(guī)劃區(qū)別于TSP問題,航線不需要經(jīng)過每個節(jié)點,航跡規(guī)劃的線路更加趨向于尋找最佳方向,方向優(yōu)則線路短,并且柵格中可通行的節(jié)點間的距離比較固定,用1/dij表示啟發(fā)值區(qū)分度不大。文獻(xiàn)[16]將節(jié)點到目標(biāo)點的距離作為啟發(fā)值,但是忽略了相鄰節(jié)點的距離,實驗證明此方法規(guī)劃的路徑折線較多,且容易陷入局部最優(yōu)。

基于上述原因,本文設(shè)計了基于方向最優(yōu)的啟發(fā)函數(shù)η,綜合考慮了上述2種距離,并引入了方向啟發(fā)信息Φijk,減少了螞蟻轉(zhuǎn)彎的概率,具體表達(dá)式如下:

ηijk=DijkΦijkHijk

(12)

(13)

dijk=

(14)

Φijk=exp(cosθijk-1)

(15)

(16)

其中:下標(biāo)a表示當(dāng)前節(jié)點A;下標(biāo)b表示下一個節(jié)點B;r為常數(shù);dijk為B點到直線AT的垂直距離,用于檢測螞蟻對理想路線的偏移量;θijk為航跡在該點的轉(zhuǎn)角;Φijk為角度啟發(fā)信息,引入該項可以減少螞蟻的階梯式搜索;Hijk為保證螞蟻僅在可行節(jié)點上移動。

2.5 三維航跡綜合評定函數(shù)

為了簡化計算,這里不區(qū)分轉(zhuǎn)彎角和爬升角,結(jié)合1.4節(jié)的內(nèi)容,通過對航跡在XY和XZ面上投影的各節(jié)點轉(zhuǎn)角進(jìn)行編碼,計算出節(jié)點間的三維真實轉(zhuǎn)角。

當(dāng)xgrid=ygrid=zgrid時,當(dāng)前節(jié)點在XY(XZ)面上的Y軸(Z軸)坐標(biāo)值為c0,前一節(jié)點坐標(biāo)值為c1,下一節(jié)點坐標(biāo)值為c2,則有:

(1) 若{c1,c0,c2}={C,C,C}∪{C-1,C,C+1}∪ {C-1,C,C+1},則航跡上當(dāng)前節(jié)點在該投影面上的轉(zhuǎn)角記為0(0°)。其中，C為任一正整數(shù)。

(2) 若{c1,c0,c2}={C,C,C+1}∪{C-1,C,C}∪ {C+1,C,C}∪{C,C,C-1},則航跡上當(dāng)前節(jié)點在該投影面上的轉(zhuǎn)角記為1(45°)。

(3) 若{c1,c0,c2}={C,C+1,C}∪{C+1,C,C+1},則航跡上當(dāng)前節(jié)點在該投影面上的轉(zhuǎn)角記為2(90°)。

三維軌跡各線段在空間的真實轉(zhuǎn)角見表1所列。

表1 三維航跡空間轉(zhuǎn)角編碼計算

本文將轉(zhuǎn)角和航程作為衡量航跡優(yōu)劣的參數(shù),并提出了航跡的綜合評價函數(shù)如下:

(17)

其中,Li、θi分別為第i段的長度和夾角。航路評價表達(dá)式(17)兼顧了飛行長度和轉(zhuǎn)折量,能夠選出相對最優(yōu)航跡。

2.6 三維航跡的平滑處理

上述改進(jìn)算法規(guī)劃的航跡是由若干條線段組成的,線段之間相交且不連續(xù)。為了進(jìn)一步優(yōu)化航線,應(yīng)將規(guī)劃航跡作平滑處理。

B樣條曲線因為凸包性、易于局部修改、更逼近原路徑等優(yōu)點,在工程中被廣泛運(yùn)用。此外,B樣條曲線在折線連接處二階連續(xù),速度與加速度都是連續(xù)的,可以減少多旋翼無人機(jī)的懸停,提高飛行效率。

本文采用三階B樣條對航跡進(jìn)行曲線擬合[17-18],曲線方程表達(dá)式為:

(18)

其中,t∈[0,1]。

事實證明,三次B樣條曲線能呈現(xiàn)出較好的平滑效果。

3 仿真實驗與結(jié)果分析

為了驗證所提出的改進(jìn)蟻群算法的航跡規(guī)劃性能,本文進(jìn)行了仿真實驗。

實驗參數(shù)如下:蟻群個數(shù)為30;起始點、目標(biāo)點分別為(0,10,50)、(50,40,20);ρ=α=0.2,ρmin=0.1,ρmax=0.3;τ0=1;Q=50;迭代次數(shù)Nmax=200次。環(huán)境地圖建模所使用的參數(shù)見表2所列。

表2 數(shù)字地圖參數(shù)

蟻群算法航跡規(guī)劃的三維空間軌跡如圖1所示。

從圖1可以看出:傳統(tǒng)的蟻群算法規(guī)劃的三維航跡比較“蜿蜒崎嶇”,在無障礙處多次出現(xiàn)U或V字型折線,增加了航程和轉(zhuǎn)角,影響了無人機(jī)飛行的速度,并且目標(biāo)點T并不是倒數(shù)第2個節(jié)點的可視節(jié)點,最后一步縱向移動了6個單位,從而忽略了中間柵格的障礙物;本文提出的改進(jìn)蟻群算法航跡點分布更加合理,在最后一步?jīng)]有出現(xiàn)穿過多個柵格現(xiàn)象，并且對初始信息素分布、信息素更新、啟發(fā)函數(shù)設(shè)計的改進(jìn),使得規(guī)劃的航跡趨于平緩,來回反復(fù)的折線較少。

圖1 傳統(tǒng)蟻群算法和改進(jìn)蟻群算法的規(guī)劃路徑

傳統(tǒng)蟻群算法和改進(jìn)蟻群算法的每代螞蟻綜合評價值的最優(yōu)解收斂曲線如圖2所示。

圖2 最優(yōu)綜合評價值變化趨勢

2種蟻群算法在航跡長度、收斂速度、最優(yōu)航跡的轉(zhuǎn)角代數(shù)和等方面仿真的具體結(jié)果見表3所列。

表3 2種蟻群算法性能比較

為了驗證本文的改進(jìn)蟻群算法能適用于不同環(huán)境,在文獻(xiàn)[19]建立的仿真環(huán)境中進(jìn)行了另外3組補(bǔ)充實驗。實驗中將螞蟻的數(shù)量改為20個,迭代50次,其他參數(shù)不變。

文獻(xiàn)[19]中傳統(tǒng)蟻群算法與本文改進(jìn)蟻群算法規(guī)劃的三維航跡仿真效果如圖3所示。2種算法在圖3環(huán)境下進(jìn)行的3組仿真實驗數(shù)據(jù)結(jié)果見表4所列。

圖3 文獻(xiàn)[19]算法與本文改進(jìn)蟻群算法規(guī)劃航跡的對比

表4 2種蟻群算法的性能比較

由表4可知,改進(jìn)蟻群算法規(guī)劃的平均航跡長度縮短了14%,平均迭代次數(shù)由34次降到了5.3次,整個航程轉(zhuǎn)角代數(shù)和降低了46.2%。這充分說明了本文改進(jìn)蟻群算法的性能均優(yōu)于傳統(tǒng)的蟻群算法。

將規(guī)劃出的三維航跡經(jīng)過三次B樣條曲線平滑,得到的結(jié)果如圖4所示。從圖4可以看出,優(yōu)化后的航跡過渡更加平緩,更易滿足無人機(jī)飛行中的動力學(xué)要求。

圖4 航跡平滑處理效果圖

4 結(jié) 論

(1) 本文航跡規(guī)劃根據(jù)起始點到目標(biāo)點進(jìn)行坐標(biāo)系旋轉(zhuǎn)計算,避免了最后一步縱向橫跨多個單元的現(xiàn)象。

(2) 對初始信息素沿“最優(yōu)路徑”在空間高斯分布,克服螞蟻搜索的盲目性,極大優(yōu)化了初代蟻群的搜索質(zhì)量。

(3) 基于空間和時間的信息素?fù)]發(fā)系數(shù)既加大了重點區(qū)域的搜索力度,又保證了搜索范圍,加快了算法的收斂速度,避免蟻群陷入局部最優(yōu)。

(4) 將方向和角度信息融入啟發(fā)值,使得搜索的線路航程更短,總轉(zhuǎn)角更小。

(5) 對蟻群算法規(guī)劃的最優(yōu)航跡作三階B樣條曲線平滑,有利于多旋翼無人機(jī)快速跟隨航跡。