基于嵌套飽和的四旋翼無人機吊掛負載控制器設計*

余志凱，黃子豪，傅瑾瑜，蔣憲鑫，辛 穎

(1. 中國人民解放軍93145部隊·上?！?01109；2. 上海航天控制技術研究所·上?！?01109)

0 引 言

四旋翼無人機因為其低廉的成本、豐富的擴展、靈活的機動能力，以及典型的欠驅動特性，一方面在工業檢測、農業植保、商業娛樂、軍事攻防等領域應用廣泛，另一方面可作為各種算法以及傳感器的理想驗證平臺[1]。四旋翼吊掛負載的研究既有切實的運輸、消防、救援等實際應用，又有探索針對高耦合和強欠驅模型控制理論的價值。

Guerrero等提出了具有較好魯棒性的互聯和阻力分配的無源控制器，分別設計了依賴與不依賴于吊掛角度的控制律，實現了點到點機動下擺動的抑制[2-4]。范云生等人利用擴張狀態觀測器對擾動及不確定性等優秀的估計能力設計非線性控制器，并通過QBall2四旋翼無人機攜帶吊掛模型的三維螺旋軌跡進行仿真驗證，有效實現了對吊掛擺動的快速抑制與軌跡的精確跟蹤[5]。Lyu等人在四旋翼吊掛負載系統建模中考慮了吊掛風擾，設計了具有指數收斂穩定性的非線性級聯控制器[6]，之后又采用有限時間控制方法，進一步提高了系統的收斂速度與魯棒性[7]。鮮斌課題組是國內對該系統研究較多的團隊，其文章主要基于能量函數開展，利用自適應控制或神經網絡等方法估計吊掛變化或外界擾動，各控制器的有效性都經過了理論與試驗的雙重驗證[8-10]。梁曉等人提出一種簡單判別系統微分平坦的方法，并以此為基礎通過動態反饋線性化實現軌跡跟蹤控制[11]。焦海林等人則分析了吊掛系統本身的擺動特性，借用抑制移動容器內液體晃動的加速度補償技術，有效減少了機體移動引起的副作用實現負載擺動的抑制[12]。Omar等人根據吊掛單擺的周期，設計相關延時反饋的抑擺控制器，只需修飾設計軌跡即可實現擺角抑制，便于直接移植應用于已有成熟算法平臺(例如PX4固件)，并利用Gazebo搭建了硬件在回路仿真平臺[13-14]，之后針對延時又進一步設計了模糊控制器，增強了抑擺的有效性[15]。

上述方法主要解決了四旋翼無人機吊掛負載控制在兩個方面的問題：1)四旋翼機體抗吊掛干擾的穩定控制；2)吊掛擺角的最終穩定收斂。但是四旋翼無人機在大幅度位置機動過程中吊掛擺角的抑制不夠突出，機動過程中的最大擺角容易隨著機動的增大而明顯增大。受文獻[16]的啟發，可以通過合理設置飽和區間從而抑制擺角，但因文獻[16]的控制效果不夠明顯，飽和控制方法有進一步提升空間。

本文通過一種嵌套飽和控制方法，設置虛擬輸入飽和來限制實際最大過載，有效限制機動加速度，從而減小擺動。并考慮到嵌套飽和在抑制干擾方面沒有明顯優勢，所以針對吊掛變化影響不敏感的垂直位置，設計基于擴張狀態觀測器的控制器，通過結合不同算法的優勢得到本文所提控制器。

1 四旋翼吊掛系統建模

四旋翼無人機本身為四輸入六自由度的典型欠驅動系統，攜帶吊掛負載后(假設飛行中吊繩始終處于拉伸狀態)額外增加兩個自由度，使得系統的欠驅動特性更加顯著，系統復雜度進一步增加，其三維示意圖如圖1所示。

圖1 四旋翼吊掛系統三維示意圖Fig.1 Three-dimensional schematic diagram of quadrotor suspended payload system

圖1中的四旋翼機體為X形，在四旋翼中心建立固連的機體坐標系obxbybzb，并以地面起飛位置為原點建立慣性坐標系oixiyizi，φ，?，ψ為四旋翼機體分別繞xb，yb，zb軸旋轉的姿態角，并定義慣性系到機體系的歐拉角旋轉順序為z(ψ)→y(?)→x(φ)。吊掛擺角α，β為吊繩在平面obybzb上的投影，分別與obzb軸、吊繩的夾角。定義單機質量為M，吊掛質量為m，吊掛繩長為l，慣性系下旋翼升力在各軸的分力為Fx，Fy，Fz，機體系下沿各軸的力矩為τx，τy，τz，轉動慣量為Jxx，Jyy，Jzz。

(1)

(2)

通過計算可得8個方程構成的方程組作為三維數學模型，由于結果較為繁瑣冗長，且僅在仿真中使用，下文控制器設計中沒有全部使用，這里不再給出具體形式。

考慮四旋翼無人機小角度飛行時，吊掛系統水平運動在正交方向耦合較小，為了便于控制器設計，此處給出oiyizi二維平面的簡化形式

(3)

2 控制器設計

若控制器設計合理，則能保證擺角始終處于小角度狀態，結合式(3)的第三條方程可知，此時擺角的余弦值接近1而正弦值接近0，易得擺角變化主要取決于y向變化，與z向相關性較小。因此，本文通過y向控制同時實現擺角控制，z向獨立控制，其變化視為對擺角的小幅度擾動，反之亦然，分別設計水平、垂直位置控制器。

2.1 垂直位置控制器

考慮到四旋翼垂直位置與擺角相互耦合，垂直位置控制器又不直接作用于擺角控制，為了提高z向抗擾能力，引入自抗擾控制中的擴張狀態觀測器估計z向總擾動，抑制后續的干擾，以提升控制性能。

為了適應三維情況，利用式(2)計算zi軸結果，并定義zi軸的總擾動為Δdz，經整理得到如下表達式

(4)

其中

(5)

(6)

式中，κ1，κ2，κ3，a1，a2，a3，δ為觀測器參數，sign(·)為符號函數。

在得到擾動估計的前提下，得到z軸垂直位置控制器

(7)

式中，zc為期望位置指令，Kp，Kd為非負的控制參數。

2.2 擺角抑制控制器

四旋翼吊掛系統作為高度欠驅動系統，在z軸方向獨立控制前提下，本文所設計的控制器通過擺角抑制控制力uα與水平y軸的控制力Fy疊加的方式實現擺角α與y軸的完全耦合。

四旋翼機體在空中穩定飛行(勻速直線或懸停狀態)時，吊掛在水平位置的擺動可簡化為吊繩上端的水平移動與吊掛負載單擺運動的疊加，從而得到如下擺角控制器

(8)

其中kα為大于零的控制參數。

下面進行穩定性證明。構造如下李雅普諾夫函數

(9)

使用“do/does/did+v.”構式既能表達變化，也能表達移動。變化是抽象的移動，移動是具體的變化。造成變化和移動的可以是物理力，也可以是心理力，甚至是社會力。例7中，他們受到心理感受的無形的力而改變態度，例8是物理力的作用，是施事者就具體物體發出的力量，例9則是來自社會的約束力。使用目標構式表達變化或移動，起到凸顯力作用的效果。

(10)

將式(8)的控制器代入上式，易得

(11)

擺角理論上漸近收斂。

2.3 水平位置控制器

水平位置控制器主體采用嵌套飽和控制器，存在如下引理[17-18]：

對于如下形式的n階多次積分系統

(12)

其中umax為正數，表示執行器的最大輸出大小，令

(13)

(14)

原狀態空間經過形式如p=Tx的狀態方程變換為Teel標準型

(15)

其中

等價變化矩陣為

(16)

當n為偶數有

(17)

當n為奇數有

(18)

已知ε為正數，定義飽和函數σε

(19)

若給定如下控制律，能使系統鎮定

(20)

其中

根據上述引理進行控制器設計，對模型進行小角度線性化，并將四旋翼無人機視為無吊掛的系統進行推導設計?？紤]到嵌套飽和控制器無法直接控制水平分力，而需姿態角變化實現水平位置控制。所以本文將抑擺控制器的輸出轉化為姿態角零位補償，從而引入到嵌套飽和控制器中，得到如下模型

(21)

其中φε=φ-φb，φb=-uα/(Mg)為角度零位補償，用于近似水平分力抑制吊掛擺動。

對模型進行簡單整理，易得狀態空間表達式

(22)

(23)

利用式(16)可反推變換矩陣，易得變換后的系統狀態變量

(24)

得到水平位置控制器

(25)

3 仿真驗證及分析

為了驗證本文設計控制器的控制效果，盡可能接近實物試驗便于后續飛行驗證，本文基于已有四旋翼無人機吊掛系統實物的模型參數，考慮了傳感器、電機等多種影響因素，采用連續模型和離散控制器進行三通道仿真，計算多組典型工況并加以分析。

四旋翼吊掛模型的主要參數：M=1.5kg，m=0.1kg，l=1.0m，Jxx=0.022kg·m2。本文控制器主要參數：κ1=100，κ2=300，κ3=1000，a1=1，a2=0.5，a3=0.25，δ=0.02，Kp=2，Kd=2.8，kα=2，ε1=0.11，ε2=0.266。

首先為了驗證抑擺控制器的效果，對比了嵌套飽和控制有無引入抑擺角度零位補償兩種情況，在z軸方向上，0s時輸入幅值為8m的階躍信號，在y軸方向上，10s時輸入幅值為8m的階躍信號，仿真結果如圖2所示。引入擴張狀態觀測器的z軸方向位置響應抗擾能力較強，幾乎不受抑擺控制器補償的影響；y軸方向位置響應在姿態角有無抑擺補償情況下總體變化趨勢差別不大，但有抑擺補償時超調有少許增加；抑擺補償下姿態角變化有些許增大，用以補償擺動；引入抑擺補償后，吊掛擺角的全過程變化趨于平穩，當位置到達目標點附近后，擺角會快速收斂并趨近于零，不再有明顯振蕩。

目前已有的四旋翼吊掛系統控制器幾乎都能保證位置機動到位后的擺角快速收斂，本文通過引入控制效果較好的自適應控制器進行對比，自適應控制器設計可見文獻[10]，但不包括姿態控制，仿真時使用串級PID進行姿態控制。利用同一組控制參數，分別進行了z軸和y軸方向上，輸入信號為幅值2m的階躍信號的小范圍位置機動工況仿真，結果如圖3所示，以及z軸和y軸方向上，輸入信號為幅值6m的階躍信號的大范圍機動工況仿真，結果如圖4所示。

圖3 小范圍位置機動控制器效果對比Fig.3 Effect comparison of small range position maneuvering controllers

圖4 大范圍位置機動控制器效果對比Fig.4 Effect comparison of large range position maneuvering controllers

首先從z軸控制器進行對比，兩種工況下本文所提控制器與自適應控制器均無產生超調，但本文控制響應明顯收斂更快，在幅值較大的階躍指令作用下尤為明顯。不同工況下，兩種控制器在y軸方向上的位置響應快慢各有優勢，但本文控制器作用下響應的上升時間、調節時間以及超調量等指標波動幅度更小，響應趨勢較為一致；而自適應控制器的參數在小范圍位置機動工況下控制性能較好，但在階躍指令變化較大后響應曲線變化明顯，超調量及調節時間顯著增大。y軸方向的控制間接作用于姿態控制，在幅值較大的階躍輸入信號作用下自適應控制器更容易產生過大的姿態指令以提高速度，從而更容易產生姿態角的飽和；本文所采用的嵌套飽和控制方法，對位置、姿態分級產生飽和控制效果，姿態角反而不易飽和。

自適應控制器的最大擺角在圖3中約為2.3°，圖4中約為9.0°，明顯隨指令信號的增大而增大。本文所提控制器的最大擺角在圖3中約為2.8°，圖4中約為3.8°，擺角抑制效果受指令信號影響較小。顯然在幅值較小的階躍信號作用下自適應控制抑制效果略優，但本文所提控制器更能顯現出對最大擺角的抑制，實際應用更有優勢。

對本文所提控制器進行y軸方向上不同階躍輸入信號情況下的仿真，結果如圖5所示。y軸方向上的控制響應速度近似且較快，超調量始終保持在10%以內。y軸在1m，3m，6m，10m階躍信號下對應的最大擺角大小分別約為1.7°，2.8°，3.8°，3.6°，即使在水平方向上做大范圍機動，擺角大小都控制在4°之內。除單位階躍信號外，其余信號響應下擺角與姿態角變化趨勢近似，兩者角度響應在縱軸正負半軸的占比接近。體現了嵌套飽和控制器通過較為穩定均衡地分配位置機動的加減速階段，減小加速度凸變引起吊掛劇烈擺動的特點。

圖5 多范圍位置機動控制器響應對比Fig.5 Response comparison of multi-range position maneuvering controllers

4 結 論

本文研究了四旋翼無人機吊掛負載控制系統，針對多數控制方法忽略的大機動過程中吊掛最大擺角抑制問題，提出了一種綜合控制方法，其借助擴張狀態觀測器獨立控制z軸方向運動，以抑制吊掛干擾，減小耦合。該方法采用嵌套飽和控制方法抑制加速度劇烈變化，以及通過抑擺控制器以姿態角零位補償的形式引入水平控制器。通過與自適應控制器進行仿真對比，驗證了機動過程抑擺的有效性以及多工況適應性。但本文未考慮未知吊掛質量與繩長的情況，后續將探索飽和控制與自適應控制相結合的方法。