基于螺旋樣條的旋翼無人機區(qū)域軌跡規(guī)劃

陳 杰 董 偉 盛鑫軍 朱向陽

上海交通大學機械系統(tǒng)與振動國家重點實驗室，上海，200240

0 引言

多旋翼無人機具有體積小、結(jié)構(gòu)簡單、能夠垂直起降與懸停等特點，在民用航拍、工業(yè)巡檢[1]、農(nóng)業(yè)作業(yè)[2]、遙感測繪[3]、公共安防、軍事偵察[4]等領(lǐng)域具有廣闊的應用前景。將旋翼無人機應用于區(qū)域作業(yè)時，一種普遍的做法就是先提取作業(yè)區(qū)域的范圍，規(guī)劃無人機的飛行軌跡，之后，無人機按照規(guī)劃的軌跡自動完成作業(yè)區(qū)域的巡檢。相比于傳統(tǒng)的人工檢測和手動操控無人機完成作業(yè)任務，這種方式能極大提高效率。

無人機飛行軌跡規(guī)劃的傳統(tǒng)做法是，先在作業(yè)區(qū)域范圍內(nèi)生成一系列的航跡點，然后以Z字形的折線連接生成覆蓋區(qū)域的軌跡[5-6]。這種方法簡單高效，在植保噴灑農(nóng)藥作業(yè)、地理信息航拍等領(lǐng)域廣泛應用。Z字形的軌跡規(guī)劃方法生成的軌跡與無人機的動力學性能不相符，引起了飛行姿態(tài)的不平滑。CHEN等[6]對每段直線進行速度規(guī)劃來保證無人機加減速過程的姿態(tài)平滑，該方法需要無人機在拐點處頻繁進行加速與減速，作業(yè)效率低，且頻繁的加速與減速增加了無人機的能耗，縮短了無人機的續(xù)航時間。加工制造領(lǐng)域中，刀具的軌跡規(guī)劃及優(yōu)化已有成熟的基礎，文獻[7-8]將螺旋加工軌跡的方法用于型腔銑，在保證刀具磨損度的前提下，提高了加工效率。這種加工軌跡的規(guī)劃方式也同樣適用于無人機區(qū)域作業(yè)的軌跡規(guī)劃。

除了折線型的直線連接，另外一種生成軌跡的方法為樣條擬合，通過控制樣條擬合的階數(shù)來保證曲線在連接點處連續(xù)的導數(shù)階數(shù)，從而規(guī)劃得到無人機姿態(tài)平滑變化的軌跡，這更符合旋翼無人機飛行特性。數(shù)控加工領(lǐng)域，樣條擬合是一種較為常用的軌跡生成方法[9-10]，擬合的曲線通過約束速度、加速度，對加工過程的加減速進行規(guī)劃，生成平滑的加工軌跡。在無人機的軌跡規(guī)劃研究中，PHANG等[11]基于B樣條擬合文字曲線，采用二次規(guī)劃的方法迭代獲得時間最優(yōu)的軌跡，控制無人機完成給定文字的繪制。為了提高軌跡生成效率與實時性，董偉[12]基于非均勻有理樣條，采用線性規(guī)劃的方法來求解最優(yōu)飛行時間問題。上述研究中，無人機飛行軌跡的擬合多基于樣條曲線，生成的軌跡為單參數(shù)的方程，便于進一步的軌跡優(yōu)化與插補。

本文提出一種用于旋翼無人機區(qū)域作業(yè)的螺旋軌跡生成方法，基于非均勻有理B樣條(non-uniform rational B-spline，NURBS)曲線擬合螺線曲線，在曲線離散化后，采用時間最優(yōu)的優(yōu)化模型，將速度、加速度和躍度作為約束條件，求解最優(yōu)飛行速度序列，并根據(jù)無人機的控制周期進行插補，基于軌跡追蹤控制器實現(xiàn)螺旋軌跡的追蹤控制。

1 區(qū)域螺旋軌跡的生成

為了使生成的軌跡能夠覆蓋整個作業(yè)區(qū)域，選擇距離中心點最遠的點，根據(jù)無人機作業(yè)覆蓋區(qū)域得到螺旋線的圈數(shù)：

(1)

輪廓線向中心方向偏置σ(根據(jù)實際作業(yè)需求確定)，得到第一層曲線。以第一層曲線為基準，以每兩層向內(nèi)偏置2Rε，生成m層輪廓曲線，如圖1b所示。輪廓集表示為

Q={Q1，Q2，…，Qm}

(2)

qi,j=q1,j+i(o-q1,j)/m

(3)

其中，Q為由所有輪廓組成的總輪廓點集；Qi為i層輪廓組成的點集，i=1，2，…，m；j為該層輪廓點數(shù)。

圖1 螺旋曲線的控制點生成Fig.1 Generation of spiral control points

中心點和所有的輪廓點將m層輪廓曲線分成n份。對每兩層輪廓曲線進行計算，得到曲線在折線處的控制點集

D0={di，j|di，j=qi，j+(j-1)(qi+1,j-qi，j)/n}

(4)

選定最外層的一個點為起點，按照順時針或逆時針方向逐層向內(nèi)將控制點集組成有序的點集序列：

D={dI|I=1，2，…，mn}

(5)

式中，dI為點I的坐標。

定義參數(shù)δ來控制折線處生成曲線的曲率。在點集序列D的每兩個相鄰控制點之間生成控制點，如圖1b所示，得到曲線的控制點集P={pi}，pi為控制點坐標。生成控制點方法如圖2所示。

圖2 控制點生成流程圖Fig.2 Flow diagram of control points generation

本文采用NURBS曲線擬合無人機的飛行軌跡，得到軌跡相對于單個參數(shù)的函數(shù)，以便于進一步的軌跡優(yōu)化。

一條k(k=0，1，2，…)階的NURBS曲線定義如下[11]：

(6)

其中，wj為權(quán)因子；u為曲線參數(shù)，0≤u≤1；Nj，k(u)為NURBS曲線的基函數(shù)，通過遞推公式獲得，k=0時

(7)

k>0時

(8)

曲線的節(jié)點向量為

U=(u0，u1，…，un+k+1)

(9)

對于NURBS開曲線，兩端節(jié)點的重復度取k+1，即u0=u1=…=uk=0，un+1=un+2=…=un+k+1=1。節(jié)點向量U根據(jù)哈特利-賈德方法[11]確定。

這里令wj=1，可以簡化得到

(10)

為了得到無人機平滑飛行的軌跡，即保證無人機飛行過程中姿態(tài)的平滑變化，無人機姿態(tài)的變化率需要是可導的，其中，姿態(tài)對應于加速度，姿態(tài)的變化率對應于躍度，所以躍度是可導的，飛行曲線需4階可導。本文采用4次NURBS曲線進行擬合。

根據(jù)得到的控制點集P、節(jié)點向量U，可獲得擬合的螺旋曲線，圖3所示為由4階NURBS樣條擬合控制點得到的螺旋樣條曲線。

圖3 4階NURBS樣條擬合的螺旋樣條曲線Fig.3 Fitting of spiral curve by 4-order NURBS

對于一般具有較為復雜凸邊界的區(qū)域，通過提取邊界輪廓的點集，應用以上軌跡生成算法，均可生成覆蓋整個區(qū)域的螺旋作業(yè)軌跡，圖4所示為不規(guī)則區(qū)域的螺旋軌跡。

圖4 不規(guī)則區(qū)域螺旋軌跡生成Fig.4 Trajectory generation on an irregular region

如圖5所示，區(qū)域中心不在區(qū)域范圍內(nèi)的凹邊界無法直接采用上述的算法生成軌跡。針對這種情況，將區(qū)域進行分割，得到2個凸邊界組成的區(qū)域，分別對2個區(qū)域生成軌跡控制點，連接兩個區(qū)域的控制點，然后進行樣條擬合，就可得到連續(xù)的覆蓋區(qū)域的雙螺旋飛行軌跡。對于更加復雜的邊界，同樣可采用這種分割擬合的方法，生成多螺旋區(qū)域覆蓋軌跡。

圖5 特殊區(qū)域分割多螺旋軌跡生成Fig.5 Trajectory with multi-spiral curve for special region

2 軌跡規(guī)劃

2.1 速度規(guī)劃

得到生成的軌跡后需要進行速度規(guī)劃。在無人機滿足自身動力學性能的條件下以最短的時間完成作業(yè)任務，需要建立有速度約束、加速度約束及躍度約束，以最短的時間為目標的優(yōu)化問題，完成軌跡的速度規(guī)劃。

引用文獻[10]的最優(yōu)速度規(guī)劃方法，將飛行時間最短問題等效為每點速度最大的優(yōu)化問題，將曲線離散化為N個區(qū)間，可以得到如下最優(yōu)化問題：

(11)

式中，v(uJ)為軌跡離散點uJ的速度；(·)μ表示向量在μ軸上的分量，μ∈{x,y,z}；vmax為最大飛行速度約束；aμmax為μ軸的最大飛行加速度約束；jμmax為μ軸的最大躍度約束；τ為軌跡追蹤精度約束；ρJ為uJ處的曲率半徑；εmax為最大弓高誤差約束；T為插補周期。

令β=‖dC(u)/du‖，γ=v2(u)/β2，則加速度表示為

進而通過差分以離散形式在uJ得到加速度

對于A(u)的μ軸分量

即

aμ(uJ)=C″μ(uJ)γ(uJ)+C′μ(uJ)γ′(uJ)/2

躍度可表示為

式中，F(xiàn)′(uJ)、F″(uJ)、F?(uJ)分別為函數(shù)F(uJ)對uJ的1階導數(shù)、2階導數(shù)、3階導數(shù)。

以v2(uJ)為變量，將式(11)的優(yōu)化問題轉(zhuǎn)換為一個線性規(guī)劃問題，確定軌跡的速度、加速度的邊界約束，利用最優(yōu)理論求解得到最優(yōu)速度序列。圖6所示為圖3中的矩形區(qū)域螺旋軌跡在速度約束為3 m/s、加速度約束為5 m/s2、躍度約束為3 m/s3時的規(guī)劃結(jié)果。軌跡的速度、加速度基本達到約束要求，且曲線曲率較大位置的速度較小，符合旋翼無人機的動力學特性。有躍度約束的情況下，無人機的加速度在約束范圍內(nèi)受到了一定的限制。

圖6 螺旋軌跡速度規(guī)劃結(jié)果Fig.6 Results of spiral trajectory planning

2.2 曲線插值

若需無人機完成生成軌跡飛行的任務，則需要將軌跡按控制周期向時域內(nèi)映射，即需要對軌跡按照飛行器控制周期進行插值。綜合考慮精度與計算量，本文采用二階泰勒展開法[12]進行插補參數(shù)計算。

曲線參數(shù)的時間函數(shù)記為u(t)，ti為第i個控制周期的時刻，ti+1為第i+1個控制周期的時刻，記u(ti)=ui，u(ti+1)=ui+1，對ui+1進行二階泰勒展開可得

由于初始點的速度為0，無法計算得到下一個插值點，因此需要給初始點一個小的速度。計算得到新的插值點ui+1后，基于每兩個離散點之間速度的變化是線性的假設，離線點之間的新插值點的速度由相鄰兩個離散點已知的速度，通過線性擬合得到，從而通過插值得到間隔為控制周期的位置序列與速度序列。圖7a所示為曲線插值的結(jié)果，由圖7b可以看到，在速度較小的位置，插值的軌跡點更加密集。

圖7 螺旋曲線插值Fig.7 Curvilinear interpolation of spiral trajectory

3 軌跡追蹤控制

多旋翼無人飛行器的控制系統(tǒng)一般由4個級聯(lián)的PID控制器組成，包括位置環(huán)、速度環(huán)、姿態(tài)環(huán)和角速度環(huán)，如圖8所示，通過設定目標位置實現(xiàn)位置閉環(huán)。

圖8 飛行控制器的級聯(lián)控制框圖Fig.8 Cascade control block diagram of flight controller

由前一節(jié)的插值得到對應控制周期的位置序列，將這些位置作為每個控制周期的目標位置，在每個控制周期向無人機發(fā)送位置設定指令，實現(xiàn)軌跡的追蹤，但采用這種追蹤方式時，無人機的軌跡追蹤性能不佳，故基于軌跡的速度序列，加入速度前饋，從而獲得較好的軌跡追蹤性能，如圖9所示。速度控制量可表示為

v=kpep+vff

式中，ep為位置誤差；kp為位置追蹤器比例系數(shù)；vff為速度前饋。

圖9 軌跡追蹤控制框圖Fig.9 Control block diagram of trajectory tracking

本文基于開源飛行控制器Pixhawk搭建了飛行平臺 ，以實現(xiàn)基本飛行姿態(tài)的控制，將Raspberry Pi 2作為上層控制器，以實現(xiàn)軌跡追蹤控制。

4 實驗與結(jié)果

針對一塊特定的10 m×10 m的矩形區(qū)域，在相同的動力學約束以及作業(yè)覆蓋要求的條件下，分別采用本文的螺旋軌跡生成算法、折線型軌跡規(guī)劃算法[6]生成并規(guī)劃軌跡。實際作業(yè)過程中，會根據(jù)無人機的動力、作業(yè)需求等實際情況，限制無人機飛行的最大速度、最大的飛行姿態(tài)傾角、最大的姿態(tài)角速度，對應于無人機的速度、加速度以及躍度，本文設定無人機的速度約束為3 m/s，加速度約束為5 m/s2，躍度約束為3 m/s3，無人機作業(yè)覆蓋直徑為2 m。兩個算法生成的覆蓋軌跡如圖10所示。

圖10 10 m×10 m的正方形區(qū)域軌跡規(guī)劃Fig.10 Trajectory planning of a square with 10 m×10 m

Pixhawk平臺能實時記錄飛行的位置、速度、姿態(tài)等無人機的狀態(tài)信息并將其保存到SD卡。測試完成后，讀取記錄的數(shù)據(jù)，并畫出無人機的位置、速度、姿態(tài)信息。圖11、圖12所示為兩種區(qū)域軌跡規(guī)劃的實驗結(jié)果。

在相同的動力學約束條件下，兩次實驗過程中，無人機的速度、姿態(tài)均在給定的約束范圍內(nèi)，本文方法的作業(yè)時間29.3 s比折線型區(qū)域軌跡規(guī)劃方法的作業(yè)時間34.7 s縮短了15.5%，有效地提高了區(qū)域飛行的效率。

5 結(jié)論

針對旋翼無人飛行器在區(qū)域作業(yè)中的應用需求，提出了一種基于非均勻有理B樣條擬合螺旋曲線的軌跡生成方法，并針對特殊區(qū)域提出先分割區(qū)域，再擬合的多螺旋軌跡生成方法。根據(jù)旋翼無人機的動力學特性，基于速度、加速度、躍度約束條件進行軌跡規(guī)劃，獲得了無人機在最短時間完成作業(yè)軌跡的位置速度序列，采用二階泰勒展開法進行曲線的插值，得到用于控制的軌跡序列，設計了軌跡追蹤控制器，基于開源飛行平臺完相比較于傳統(tǒng)折線區(qū)域軌跡規(guī)劃方法，螺旋成了飛行對比實驗。軌跡規(guī)劃方法能夠在保證無人機飛行姿態(tài)平滑且完成相同作業(yè)任務的前提下，提高無人機區(qū)域作業(yè)的效率，縮短作業(yè)時間。

圖12 折線軌跡規(guī)劃實驗結(jié)果Fig.12 Flight test result of zigzag trajectory