飛行器棲落機動的軌跡跟蹤控制及吸引域優化計算

王無天，何真，岳珵

（南京航空航天大學 自動化學院，南京211106）

在自然界中，大型鳥類通過拉大飛行迎角來實現快速、準確的降落，將這種降落方式稱為棲落機動。如果固定翼飛行器可以模仿大型鳥類進行棲落機動，即拉大飛行迎角、快速降低飛行速度并最終棲落在目標區域，那么將極大地擴展其應用場合［1-3］。棲落機動不但能保留固定翼飛行器在續航時間、飛行范圍和速度等方面的優勢，還能在一定程度上彌補其通常不能在小場地降落的缺點。

由于棲落機動給固定翼飛行器帶來的好處，棲落機動的研究得到了越來越多的關注。文獻［4］為了驗證棲落的可行性，進行了小型滑翔機棲落在電線上的實驗。文獻［5］研究了變體部件對無人機棲落的影響。文獻［6］通過配點法生成了棲落機動的標稱軌跡。文獻［7］對軌跡的穩定性研究發現，在終端階段會發生發散，對棲落機動設計了滑模軌跡跟蹤控制器。文獻［8］對側滑棲落機動提出了一種自適應增益滑模控制技術。文獻［9］采用開放時間最優化方法來研究棲落軌跡優化問題。

目前，棲落機動的研究主要驗證了棲落機動的可行性，以及如何設計控制器提高棲落機動的穩定性，沒有考慮所設計的棲落機動系統的吸引域問題。吸引域是指非線性系統局部穩定的區域，為了保證飛行器能棲落在目標區域，需要計算棲落機動的吸引域。文獻［10］提出了用平方和（Sum-of-Squares，SOS）算法計算動力學系統的吸引域。文獻［11］針對棲落機動設計了誤差反饋控制律，并將平方和算法用于計算棲落軌跡控制系統的吸引域。受文獻［11］啟發，本文針對棲落機動設計了更易于求解的分段線性控制律，并對平方和算法進行優化設計，以擴大吸引域。

本文采用廣義偽譜法對棲落機動軌跡進行優化并生成標稱軌跡，將動力學模型沿標稱軌跡線性化，設計了分段線性的軌跡跟蹤控制器；采用平方和算法計算系統的吸引域，并且優化吸引域算法以尋找出更大的棲落機動吸引域。

1 飛行器棲落機動的動力學模型

動力學模型的狀態變量選取飛行速度V、航跡角μ、迎角α、俯仰角速率q、俯仰角θ、水平位移x和高度h，控制輸入選取推力T和升降舵的偏轉角度δe。假設推力經過重心且指向機頭方向，圖1為飛行器的縱向受力分析圖。圖中：Xa為速度坐標系的橫向位移；Xb為機體坐標系的橫向位移；Xg為地面坐標系的橫向位移；Za為速度坐標系的縱向位移；Zb為機體坐標系的縱向位移；Zg為地面坐標系的縱向位移；g為重力系數。

圖1 飛行器縱向受力分析圖Fig.1 Longitudinal force analysis diagram of UAV

根據圖1建立速度坐標系下的飛行器縱向動力學方程：

式中：m為飛行器的質量；M 為空氣動力和力矩；L為升力；D為阻力；Iy為飛行器俯仰轉動慣量。

L、D、M 的表達式如下：

式中：CL、CD和CM分別為升力、阻力和力矩系數；Sa為飛行器的空氣動力表面積；ρa為空氣密度。

飛行器棲落機動的空氣動力學系數由平板模型方法［12-14］得到，可表達為

式中：le為升降舵空氣動力重心到飛行器質心距離；Se為升降舵的空氣動力表面積。

棲落機動的動力學模型與常規飛行器的區別主要在于棲落機動具有大迎角。棲落屬于一類復雜的機動，是非線性領域控制的一個挑戰。

2 棲落機動軌跡跟蹤控制律設計

2.1 非線性參考軌跡

將動力學模型（1）記為

式中：X為非線性模型的狀態向量；u為非線性模型的輸入量；f（X，u）表示非線性函數。

針對軌跡優化和非線性軌跡，采用廣義偽譜法為基礎的數值非線性優化程序生成［15-16］。目前的廣義偽譜法主要有4種類型，分別為Guass偽譜法、Legendra偽譜法、Rudau偽譜法和Chebyshev偽譜法。本文采用Rudau偽譜法進行軌跡優化，主要原因是：Rudau偽譜法相對其他3種廣義偽譜法有著較高的精度，在其他3種廣義偽譜法的基礎上增加了初始點或者著重點。

動力學模型含有6個狀態量，在進行優化計算時，將升降舵的偏轉角度δe作為新的狀態量，則狀態向量可表示為X=［V，α，μ，q，x，h，δe］T，并且設定參考的推力T為常值3.768 N。廣義偽譜法是根據整個過程對軌跡狀態量的約束和狀態方程生成標稱軌跡，所以需要對7個狀態量根據棲落機動軌跡的要求進行約束。在執行棲落機動的過程中設置參數：將初始點位置設置為坐標原點，并且根據實際情況設置各個狀態的約束范圍。升降舵的偏轉角度設為狀態量，所以將其導數作為輸入量u，給定u的約束條件為：τ（u）=（-1，1）。設定初始時刻上限值和下限值為：Xh（t0）=［9.984 m/s，0 rad，0.25 rad，0 rad/s，1 m，0 m，-0.15］T，Xl（t0）=［9.984 m/s，0 rad，0.25 rad，0 rad/s，0 m，-1 m，-0.15］T。設置狀態量過程中的范圍如表1所示。設定終點時刻上限值和下限值為Xh（tf）=［4m/s，π/2 rad，π/4 rad，3.5 rad/s，15m，2 m，π/3］T，Xl（tf）=［3 m/s，-π/2 rad，-π/4 rad，-3.5 rad/s，14m，1m，-π/3］T。棲落終點的位置要根據實際的棲落點確定，如棲落在電線桿上，所以設置棲落終點時給定了一定的約束范圍，如表1所示。

選取的二次型優化指標J為式中：第1項為積分型，是對整個過程中產生的狀態誤差和輸入誤差進行的補償；第2項為終點型，是對終點產生的誤差進行補償。二次性能指標（5）體現為對棲落機動的綜合性能優化。式（5）中：Qf、Q、R分別為對應項所占的權重；x（tf）為棲落終點的目標狀態，所以對Qf的選擇越大，越要求棲落終點位置越接近指定的位置；u為整個過程的輸入，R越大則對輸入整體的操控需要的能量越小；x為整個過程的狀態變量，Q越大則對整個狀態變量要求越平穩。棲落機動狀態量變化的幅度較大，所以對狀態變量不設限制，整個優化可以理解為以最小的輸入達到最優的最終棲落狀態。其中，Qf=diag［10，30，0，0，10，10，0］，R=90，Q=diag［0，0，0，0，0，0，0］。

飛行器的物理參數來源于文獻［14］，具體參數如表2所示。

表1 狀態變量過程約束Table 1 Process constraints of state variables

表2 飛行器物理參數Table 2 Physical parameters of UAV

根據以上約束條件用廣義偽譜法即對應MATLAB的GPOPS工具包針對非線性模型生成的標稱軌跡如圖2和圖3所示。

從圖2中看出，經過軌跡優化設計出棲落機動軌跡的高度和水平位置都可以很好地落在終點約束的目標范圍內。

圖2 狀態變量標稱曲線Fig.2 Nominal curves of state variables

圖3 推力和升降舵的偏轉角度標稱曲線Fig.3 Nominal curves of thrust and rudder angle

為了將非線性模型分段線性化，定義飛行器棲落機動的時間范圍為［t0，tf］，在時間范圍上均勻地選取n個時間點｛t0，t1，…，tn-1｝，且有tn-1=tf。在每個時刻對標稱軌跡線性化，所以任意時刻點tq的線性化模型為

整個時間內分段線性模型為

式中：χq（t）相當于隨著時間變換的函數，可描述為

2.2 控制律設計

本節針對棲落機動分段線性模型（9）設計采用分段線性最優軌跡跟蹤控制器。針對飛行器棲落機動動力學系統，尋找一個最優控制uc（t）使得X能跟蹤上Xr。選取的二次型性能指標J1如下：

式中：Q≥0，R＞0為正定的對角陣常值矩陣；ΔXq為實際軌跡和標稱軌跡在q時刻點的狀態差值；Δuq為q時刻點的最優控制，需要彌補到實際輸入上。

根據線性二次型調節器（Linear Quadratic Regulator，LQR）可得到控制律kq為

式中：Sq為滿足式（14）的正定對稱矩陣，即黎卡提方程：

所以，系統在q時刻的閉環狀態方程為

式（15）設置了分段線性系統的每個子系統的最優控制律，可以使子系統穩定地收斂至標稱軌跡附近。

雖然上述每一個分段線性系統可以保證穩定性，但是不能保證整個棲落系統的穩定性。為了保證整個棲落系統的穩定，需要使得每一個子系統的Lyapunov函數值大于或等于下一個子系統的Lyapunov函數值，即滿足：

式中：VL為Lyapunov函數；Pq和Qq為選取的正定對稱矩陣。

下面對式（16）進行證明。

引理1［17］時變系統˙=C（t）x，x（t0）=x0，且t≥t0，C（t）為以時間t的連續和分段連續函數為元的矩陣。則系統一致漸近穩定的充要條件是：存在N＞0，c＞0，使得對于任意的t0和t≥t0，狀態轉移矩陣Φ（t，t0）滿足：

證明構造Lyapunov函數：

選取時間為（tq-Δt，tq+Δt］的線性子系統。因為Pq為正定對稱矩陣，所以將其分解為Pq=UTdiag（λ1，λ2，…，λn）U，λq為Pq的特征值，U為酉矩陣。因此，子系統滿足：

由式（15）閉環子系統可推導出

再由式（16）可得

因為

由式（19）得

聯立式（21）～式（23）得到

引入非線性系統比較原理，可以證明Ve滿足：

棲落機動的整個過程分為N個子空間且經歷的時間為［t0，tf］，所以由引理1可以得到Ve在（tq-Δt，tq+Δt］時間按指數收斂。

又由式（16）可知，在2個子系統切換時刻ts=tq-Δt滿足：

所以，綜合可知Ve按照指數形式收斂且在切換時刻后非增，整個系統漸近穩定。由式（13）計算控制律kq后要代到式（16）驗算是否滿足整個系統的漸近穩定要求，如果不滿足，要調整式（11）中Q、R的權值再次驗算。這樣，滿足條件（16）的控制律（13）能保證整個棲落分段線性系統（9）的穩定。證畢

軌跡跟蹤控制系統的結構如圖4所示。

圖4 軌跡跟蹤控制框圖Fig.4 Trajectory tracking control block diagram

3 棲落機動軌跡跟蹤控制吸引域的計算

吸引域是指非線性系統局部穩定的區域。棲落機動軌跡的吸引域能保證其內的飛行器能夠在規定時間內棲落在指定目標區域。2.2節中設計的棲落機動控制律是針對分段線性化的模型設計的。而棲落機動的實際模型是非線性動力學系統（1）。針對非線性系統（1）計算其在控制律（13）下的局部穩定范圍即棲落機動軌跡的吸引域是一個很重要的問題。

本節運用平方和算法［18］求出閉環非線性棲落系統標稱軌跡的吸引域區域Ω［19］。在求出吸引域后，通過優化平方和算法［20］中求解的變量，對原有吸引域的半徑進行了很大程度的擴大，使得能尋找出更大的吸引域。3.1節將介紹在一般求解吸引域方法的基礎上求解棲落非線性系統的吸引域。3.2節在3.1節吸引域的基礎上通過優化平方和算法的求解變量擴大吸引域。

3.1 軌跡吸引域算法

在非線性時變的情況下，將系統寫成如下形式：

式中：f為關于x和t的多項式函數。設Γ?［t0，tf］×Rn為系統解的集合，給定的目標區域記為g?Rn。當集合Γ終點時刻的狀態收斂到g，也就是對于任何的x∈Rn，當（tf，x）∈Γ時，x∈g，即為系統整個運動過程的吸引域。

將棲落機動容許的落點范圍定義為目標區域g，定義棲落機動標稱軌跡xr的候選吸引域區域為

根據引理2，要使式（28）為吸引域，要滿足ρ（t）≤1和式（29）：

即當t=tq時狀態量仍在吸引域內。

選擇適當的ρ（t）使得Ω（ρ，t）滿足：

將上述求吸引域的過程整理成最優求解的形式如下：

將式（32）表示的條件轉化為

最終將條件轉化為

2）存在多項式f∈p｛f1…fs｝，g∈M｛g1…gl｝，h∈I｛h1…hu｝滿足f+g2+h=0，Rn表示所有實數域上n元多項式的集合。

再根據引理3，將條件（34）轉化為

設定格式如下：

式中：O為6×6的實對稱矩陣。

將棲落的時間［t0，tf］分為N段，計算棲落機動標稱軌跡的吸引域。根據棲落終點的誤差范圍給定終點吸引域半徑ρf的值。假設在［tf-1+Δt，tf］這段時間內的吸引半徑都是ρf，計算tf-1時刻的吸引域半徑ρf-1，這時用ρf-1按照線性的模式去估計時間［tf-1-Δt，tf-1+Δt］內的半徑。

3.2 擴大吸引域半徑的優化算法

將式（12）代入模型系統（6），得到的系統閉環狀態方程為

由式（14）可知

因為Q≥0，S＞0，R＞0，所以

直到式（45）左邊不屬于平方和時，選取無解時前一時刻有解的S矩陣。

改變S矩陣的方法是給S矩陣乘上一個常數k（0＜k＜1），k按照一定步長從1往下遞減直到k S矩陣使得式（45）不成立。這樣選取改變S矩陣的原因有：

證明如下：

因為只是選取的S矩陣發生了變化，而由LQR控制器得到的狀態方程沒有變化，所以

2）選擇一定步長從1往下遞減的原因是：在最優化S矩陣的過程中，搜索出的最優S矩陣可以變相地擴大吸引域半徑。

推導過程如下：

當S矩陣乘上k

兩邊同時除以k

又因為0＜k＜1，所以ρ（t）/k比原來的ρ（t）大，所以減小S矩陣可以擴大吸引域半徑。

4 仿真結果與分析

4.1 棲落機動軌跡跟蹤控制仿真

固定翼飛行器的幾何參數如表2所示，仿真采用棲落機動非線性模型（1），非線性模型的標稱輸入為由GPOPS得到的標稱軌跡的推力和升降舵的偏轉角度。氣動參數由式（2）、式（3）計算。

LQR控制器采用分段線性化的形式設計，整個棲落的時間設定為2 s，每隔0.05 s選取出參考點進行控制律設計。R=diag［9，45］，Q =diag［18，100，16，100，20，50］；聯立式（16）求得控制律。將求得的控制律加入到非線性模型的輸入上，即可得到經LQR控制器的跟蹤軌跡。飛行器初始 理 想 狀 態 X*（t0）=［9.984 m/s，0 rad，0.25 rad，0 rad/s，0m，0m］，第2節的標稱軌跡的初始狀態也是按照理想狀態設定，得到了理想的標稱軌跡。為了檢驗跟蹤控制器的效果，在設計實際的初始狀態上加上了偏差。加上偏差的初始狀態為［11m/s，0.1 rad，0.4 rad，0 rad/s，-1 m，-0.7m］。仿真結果如圖5和圖6所示。

由圖5和圖6可知，在LQR控制器的作用下，由非線性模型生成閉環曲線的狀態量均能跟上參考軌跡。所以，設計的LQR控制器是有效的。控制輸入范圍分別在T∈［2.7，4.4］N，δe∈［-0.7，0.3］rad，是合理的輸入量范圍。棲落的最終位置精度要求很高。在飛行器棲落機動的跟蹤曲線中，由GPOPS生成的參考軌跡的棲落位置是（14.80，1.162）m，在控制器下的飛行器實際棲落位置是（14.79，1.160）m。由此可見，即使初始位置偏差較大，經過LQR控制器的作用，飛行器能準確地降落在參考軌跡棲落位置附近。

4.2 棲落機動軌跡吸引域和擴大吸引域仿真

基于非線性模型（1），在棲落的時間2 s內每隔0.05 s選取一個時間點聯立式（36），計算每個時間段的吸引域。棲落機動吸引域是從棲落終點時刻開始倒推計算的。因為飛行器棲落機動要求降落在參考軌跡終點的附近，需要設定棲落位置允許的誤差。棲落終點的容許位置誤差設為Δx＜0.15m，Δh＜0.15m。根據棲落終點位置的容許誤差范圍，設定終點2 s的棲落收斂半徑為0.15m。已知2 s的半徑，即可根據式（36）推出1.95 s時刻的吸引域半徑，如此倒推出整個過程的吸引域半徑。飛行器棲落機動的整個過程共計算了41個吸引域半徑。

根據式（36）得到的吸引域比較小。根據3.2節擴大吸引域的算法，通過減小S矩陣來擴大吸引域。圖7為根據計算的41個吸引域半徑在xh平面的投影，其中綠色是沒有擴大前的吸引域，黑色是擴大后的吸引域，紅色是由GPOPS根據約束條件生成的參考軌跡水平位置和高度的曲線。

圖7 吸引域擴大對比Fig.7 Comparison of expanded attraction domain

由圖7可知，通過優化計算吸引域的方法，可以找出飛行器棲落機動更大的吸引域，這樣使得飛行器能夠在更大的初始狀態偏差下保證棲落在預定目標范圍。

為了驗證吸引域的可靠性，選取了4個在0 s吸引域的不同初始狀態，仿真結果如圖8所示，棲落軌跡全部在所求吸引域內，并且終點范圍收斂到目標區域驗證了吸引域的有效性。

圖8 棲落軌跡及吸引域Fig.8 Perching trajectory and attraction domain

5 結 論

1）廣義偽譜法可以根據對飛行狀態的約束生成棲落機動標稱軌跡。

2）棲落軌跡跟蹤控制仿真的結果表明，采用LQR設計的軌跡跟蹤控制器可以很好地跟蹤上由GPOPS生成的標稱軌跡，驗證了LQR控制器的有效性。

3）吸引域的仿真結果表明，通過優化求解吸引域的過程可以尋找出更大的棲落機動軌跡的吸引域。對擴大的吸引域進行的棲落機動控制仿真表明，起始于初始吸引域內的軌跡均保留在吸引域內，驗證了其有效性。

本文是針對一條棲落軌跡進行控制器及吸引域的設計。后續工作中將設計多條軌跡形成棲落軌跡庫，以此為基礎研究吸引域計算，擴大飛行器實現定點棲落的容許初始范圍，并逐步展開棲落機動實驗驗證的研究。后續科研工作中，也將進一步考慮參數不確定性和干擾條件對控制器的影響，以及如何提高控制器的魯棒性。