高階微分反饋控制及在四旋翼飛行器中的應用

李霞，齊國元，郭曦彤，趙旭

1. 天津工業大學 機械工程學院，天津 300387

2. 天津工業大學 天津市電氣裝備智能控制重點實驗室，天津 300387

四旋翼飛行器以其體積小，靈活度高，易于操控等優點被廣泛應用于軍事和民用等領域[1-2]。近年來，基于飛行器的集群技術也吸引了越來越多研究者的關注[3]。在飛行器實現自主避障及編隊等集群協同任務過程中，單個飛行器的控制精度和響應速度等影響著集群控制的效果，因此單個飛行器的高性能控制尤為重要。

四旋翼飛行器(Quadrotor Unmanned Aerial Vehicle, QUAV)是典型的欠驅動系統，動力學模型具有強耦合性、非線性且開環不穩定特點。尤其是其姿態系統，如果沒有有效的控制，會產生復雜的動力學，甚至混沌振蕩行為[4-5]。飛行器實際的飛行環境復雜多變，系統結構參數和動力系統參數時刻變化，且容易受到陣風等未知擾動的影響，因此很難建立其精確的數學模型[1]。目前市場上的四旋翼飛行器產品大多還是采用經典的PID控制，主要原因在于它只依賴系統的輸入輸出數據，不需要系統內部狀態已知或可測，且不依賴系統精確的數學模型。但是，PID參數整定一直是個問題，另外，系統未知不確定項，外界擾動等信息都沒有在PID控制器中考慮和引入，甚至已知的模型也被忽略，容易產生穩態誤差，出現暫態跟蹤超調過大等問題。因此，PID在系統受未知擾動影響時魯棒性就會變差[6]。針對旋翼飛行器模型不精確且易受未知擾動影響的特點，很多學者提出了不依賴系統精確數學模型，并具有抗干擾能力的控制算法。

鮮斌等[1,7]采用基于侵入-不變集(Immersion and Invariance, I&I)原理的自適應算法估計飛行器模型中的非線性化參數，并設計誤差符號函數積分的魯棒控制方法抑制未知擾動和補償估計偏差，實現飛行器的穩定控制。Zhang等[8]采用基于模糊估計器的自適應模糊量化控制方案，實現四旋翼飛行器的位姿跟蹤控制。僅依賴系統輸入輸出數據的數據驅動控制技術，如自適應動態規劃，強化學習，無模型自適應控制和迭代學習控制等[9-12]也被成功應用于模型具有不確定性和受外部擾動的四旋翼飛行器控制中，提高了系統的魯棒性。此外，基于擾動觀測器的不依賴模型控制策略，通過設計觀測器估計系統的未知擾動，也被廣泛應用于飛行器系統的控制器設計中[13-15]。其中具有代表性的是由韓京清[16]提出的自抗擾控制器(Active Disturbance Rejection Control, ADRC)，高志強[17]將其線性化后促進了自抗擾控制器的推廣應用，目前已逐漸發展成熟為一種可用的工業控制策略。文獻[18-21]采用自抗擾控制器抑制系統所受的內外擾動實現了飛行器位姿的魯棒控制。

作為自抗擾控制器的核心部分，擴張狀態觀測器(Extended State Observer, ESO)的估計精度影響著系統的控制性能。郭寶珠等[22-23]對ESO的收斂性進行了證明，表明當總擾動及總擾動的導數有界時，增加觀測器帶寬能保證估計誤差收斂到有界值。陳增強等在文獻[24]中指出，增加帶寬會引入測量信號的高頻噪聲，實際系統中不適宜選取高增益的觀測器參數。Qi等[25]證明了當總擾動為拋物線及更高階次信號時，ESO的收斂性是不能通過增加帶寬改善的，并指出了其估計精度受限的具體原因。

Qi等[26]提出了高階微分反饋控制器(High Order Differential Feedback Controller, HODFC)，該方案應用設計的高階微分器(High Order Differentiator, HOD)估計參考輸入和輸出的微分以及高階微分信息，提出了應用控制濾波器間接補償系統未知模型的方法，并通過微分反饋控制和控制濾波補償使閉環系統漸近穩定。HODFC是一種新型的不依賴模型控制策略。近年來已經成功應用于機械臂，斯圖爾特(Stewart)平臺，自動電壓調節器等系統的控制[27-29]。文獻[30]基于半自主自駕儀應用高階微分反饋控制器實現了飛行器的位置跟蹤控制及編隊飛行，且控制性能優于傳統的PID控制。迄今為止，采用ESO估計系統總擾動精度有限，HOD僅用于信號的高階微分估計，尚沒有研究將其用于估計系統的總擾動或未知模型函數。為了方便，我們統稱系統模型中所有未知不確定性和未知擾動為系統的未知模型函數。本文的創新和貢獻在于：

1) 研究發現，HOD可以用于估計未知模型函數，在HOD基礎上，引入控制輸入信息，設計了改進的高階微分器(Improved High Order Differentiator, IHOD)。分析了IHOD對未知模型函數估計的收斂性，證明IHOD較ESO高1個類型，仿真驗證了該結果。另外，對具有高階無窮小量一般未知正弦函數進行了測試，表明IHOD估計精度比ESO提高14倍。

2) 針對具有未知擾動和不確定性的四旋翼飛行器姿態系統設計了雙閉環不依賴模型控制策略。內環應用IHOD估計未知模型和輸出微分，設計了帶有模型和擾動補償姿態角速度的跟蹤控制器。外環直接通過比例控制實現姿態角的跟蹤控制。應用Lyapunov穩定性理論，證明了所設計的基于IHOD的雙閉環不依賴模型控制器的閉環系統穩定性。

3) 通過基于Pixhawk的四旋翼飛行器姿態系統控制測試平臺，實驗驗證了所設計的基于IHOD的雙閉環不依賴模型控制策略對不同參考姿態角的跟蹤性能及抗擾性能。同時分別設計了PID，ADRC和傳統HODFC的雙閉環不依賴模型控制器，并與所提出的基于IHOD的雙閉環控制策略進行了性能對比分析，進一步驗證了所提出的改進的高階微分器在控制性能上更具優勢。

1 四旋翼飛行器姿態系統模型

對“×”型結構四旋翼飛行器進行研究，如圖1所示，其中，E={x,y,z}表示慣性坐標系，B={xb,yb,zb}表示機體坐標系，xb指向機頭方向，zb垂直機體平面指向地面，yb與xb、zb滿足右手定則，Fi(i=1,2,3,4)表示電機旋轉產生的升力。

考慮系統的不確定性，四旋翼飛行器的姿態動力學模型描述為

(1)

式中：Θ=[φθψ]T為四旋翼飛行器在慣性坐標系中的歐拉角；ω=[pqr]T為機體角速度；τ=[τxτyτz]T為螺旋槳產生的控制輸入力矩；d=[dxdydz]T為系統的不確定性，它包括兩部分，一部分是未建模動態，如陀螺力矩、機體部分壞損、負載和氣動阻力等，另一部分是可能存在的內部或外部未知時變擾動。矩陣W表示為

(2)

四旋翼飛行器的轉動慣量矩陣為

(3)

實際中，不能保證四旋翼飛行器結構完全對稱，其質心與機體幾何中心可能不完全重合。因此，考慮慣量積不為零，系統式(1)中第2個式子描述的每一個子系統都可以統一表示為如下一階形式：

(4)

(5)

為了方便控制器的設計，省略式(5)中的下標i和j，有:

(6)

式中：f為未知函數，包括系統結構和參數不確定性，已知和未知擾動，未知控制輸入部分等；b是與系統結構參數相關的可調參數。一階系統式(6)所表達的無人機姿態系統具有一定的普遍性。盡管很多系統用二階微分方程表示，但是如果采用雙閉環方案，內環仍然是一個一階系統。關于未知函數f的估計將在第2節給出。

給定參考姿態角Θr=[φreθreψre]T，定義eΘ=Θr-Θ為姿態角跟蹤誤差，控制目標是設計控制輸入τ，使得系統姿態角漸近跟蹤參考值，即

(7)

2 改進的高階微分器設計及收斂性分析

針對式(6)描述的一階系統，設計IHOD在線實時估計未知函數f，分析和比較設計的IHOD與ESO的收斂性。

將一階系統式(6)中的未知函數f擴張為1個新的狀態，則系統式(6)擴張為1個新的二階系統，即

(8)

式中：[x1x2]T=[yf]T為系統的狀態。

HOD[21]能夠估計參考輸入和輸出的微分以及高階微分，但是不能估計系統未知模型。研究發現，如果引入系統的輸入量，可以作為未知模型的觀測器。系統輸出y可測，因此使用測量得到的y，針對一階系統式(6)，設計的IHOD為

(9)

圖2 二階IHOD與ESO原理對比圖

(10)

定理1對于系統式(6)，當未知函數f是二階無窮小函數時，所提出的IHOD對f的估計無差收斂，即

(11)

IHOD對f的估計是二型系統，當未知函數f的二階導有界時，所提出的IHOD對f的估計誤差有界。

證明定義未知函數f在t和t*=t-Δt時刻分別為f(t)和f(t*)，其中Δt為采樣長度。假設f(t)是二階無窮小，即f(t)的一階導有界，則f(t)在t*時刻的泰勒展開式為

(12)

(13)

其中：A是Hurwitz矩陣，有

(14)

即

(15)

(16)

其等效的單位負反饋系統的開環傳遞函數為

(17)

即式(9)設計的IHOD對未知函數f的估計是二型系統。證畢。■

注2針對系統式(6)，現有最為常用的二階ESO可以表示為

(18)

(19)

(20)

其等效的單位負反饋系統的開環傳遞函數為

(21)

由式(21)可知，ESO對未知函數f的估計是一型系統，只有當f是常值時，ESO的估計才能實現無差收斂；當f是斜坡函數時，ESO的估計誤差有界，雖然該界值隨觀測器帶寬的增大而減小，但帶寬增大會帶來量測噪聲的放大，因此采用增大帶寬方法不可取；當f是二階及以上函數時，ESO的估計誤差發散。

注3系統式(9)設計的IHOD對未知函數的估計比ESO高1個類型，主要原因是2個觀測器的輸出方程不同。

3 四旋翼飛行器姿態系統控制器設計

針對四旋翼飛行器的姿態系統，本節設計了基于不依賴模型控制策略的雙閉環控制器。圖3為四旋翼飛行器姿態系統的控制結構框圖，其中P表示比例控制器，MFC表示不依賴模型控制器(Model-free controller, MFC)。外環設計參考角速度指令，使四旋翼飛行器的姿態角漸近跟蹤參考的姿態角；外環控制器發出的角速度指令將作為內環控制器的參考值，通過內環設計高階微分反饋控制器得到控制力矩，使飛行器的姿態角速度漸近跟蹤該參考角速度。

圖3 QUAV姿態系統控制結構圖

3.1 雙閉環不依賴模型控制器設計

為方便控制器設計和閉環系統穩定性分析，假設姿態系統的運動學方程滿足

(22)

即歐拉角變化率等于機體角速度。

實際姿態實驗中遙控器的4個通道指令分別轉化為滾轉角、俯仰角的參考指令，偏航角速度參考指令和能使飛行器懸停的升力，因此外環只需考慮滾轉角和俯仰角的跟蹤控制。設計的滾轉角和俯仰角的角速度指令為

(23)

式中：kφ、kθ為外環比例控制器的增益，這2個角速度控制輸出量與遙控器發出的偏航角速度指令一起作為內環控制的參考角速度pr、qr、rr。針對式(6)描述的姿態系統動力學方程，本文設計基于IHOD的不依賴模型高階微分控制，即IHODFC，其控制力矩τ=[τxτyτz]T為

(24)

綜上，所設計的雙閉環控制策略僅依賴系統的參考信號和可測輸出，系統中的未知模型函數和微分信息通過改進的高階微分器估計得到，不需要已知系統精確的數學模型。下面將分析基于該控制策略的閉環系統穩定性和收斂性。

3.2 閉環系統穩定性和收斂性分析

四旋翼飛行器姿態系統中每個子系統的模型都可以用如下二階系統描述(以俯仰角為例)：

(25)

定義姿態角跟蹤誤差eθ=θr-θ，角速度跟蹤誤差eq=qre-q，針對該二階系統，設計的基于IHOD的IHODFC雙閉環控制策略為

(26)

定理2當四旋翼飛行器姿態子系統式(25)中未知函數f的一階導有界時，基于IHOD估計的未知模型函數，應用所設計IHODFC(式(26))控制四旋翼飛行器跟蹤參考姿態角，能實現閉環系統漸近穩定，姿態跟蹤誤差漸近收斂，即

(27)

(28)

(29)

設計Lyapunov函數為

(30)

式中：P是對稱正定矩陣且滿足如下Lyapunov方程

ΛTP+PΛ=-Q

(31)

其中：Q>0。

(32)

從而

(33)

4 數值仿真

4.1 IHOD和ESO估計未知函數的對比

圖4 IHOD和ESO分別估計3個類型未知函數的效果

由圖4可知，當未知函數f=t(一階導有界)時，IHOD估計f實現無差收斂，ESO估計f的誤差是非零有界的；當未知函數f=0.5t2(二階導有界)時，IHOD估計f的誤差有界，而ESO估計f的誤差發散；對于一般的未知函數f=sin(3t)，IHOD估計f的平均絕對誤差為0.034 7，ESO的平均絕對誤差為0.4907，在相同參數條件下IHOD的估計精度比ESO提高了約14倍，這個估計精度的提高是非常可觀的，在控制中，如果未知函數估計精度低會影響控制暫態性能，甚至穩態誤差。

4.2 四旋翼飛行器姿態角跟蹤仿真測試

將3.1節設計的基于IHODFC的雙閉環控制策略應用于四旋翼飛行器的姿態控制，并與傳統的PID，ADRC和HODFC方法比較，測試了本文所提方法的可行性和優越性。仿真中將系統式(1)作為四旋翼飛行器姿態模型，其中慣量積取主慣量的10%，即

Ixy=Ixz=0.1Ixx=0.002 11(kg·m/s2)

Iyx=Iyz=0.1Iyy=0.002 19(kg·m/s2)

Izx=Izy=0.1Izz=0.003 66(kg·m/s2)

d=-0.03ω+d0，其中d0為外部擾動。系統的初始狀態取Θ0=ω0=0。仿真中4種控制算法外環采用的比例控制參數均為6.5，內環選取的控制器參數如表1所示。由于參數b與系統結構有關，準確來說與主慣量有關，所以首先確定b的取值；觀測器ESO，HOD和IHOD的參數越大，估計精度越高。為公平起見，在參數一致的條件下，比較了4種控制算法的跟蹤性能和抗擾性能。以滾轉通道為例，設置期望滾轉角度為φre=0.15 rad≈8.60°，6～10 s之間的外部擾動為dφ0=0.01sin(5t)，滾轉通道的跟蹤及抗擾效果如圖5所示。

表1 姿態角速度環4種控制算法的參數

圖5(a)結果表明，具有未知函數補償能力的HODFC、ADRC和IHODFC控制效果優于PID。在相同參數條件下，所提出的IHODFC在暫態響應和抗擾能力方面比HODFC和ADRC都好；且仿真發現，增大觀測器參數和控制增益參數，這3種方法控制性能都有所提高，因此本文所提出的IHODFC能實現其他控制器需在“大帶寬，大增益”條件下的控制性能。對比圖5(b)和圖5(c)，進一步說明IHOD對實際系統中外部擾動和耦合項等未知模型函數的估計精度要比相同參數條件下的ESO精度高，從而表明IHODFC在暫態性能和抗擾能力方面的優勢。

圖5 滾轉角跟蹤及未知模型函數估計對比

5 實驗驗證

實驗驗證閉環控制系統。實驗采用基于Pixhawk的四旋翼飛行器控制算法開發平臺，被控對象為大疆F450機架四旋翼飛行器，實驗室搭建了四旋翼飛行器姿態系統控制的測試架，如圖6所示。進行3組實驗，分別是滾轉角跟蹤階躍參考、俯仰角跟蹤柔化的階躍參考、以及滾轉角的抗擾測試。控制策略是雙閉環控制，外環控制方案相同，即比例控制，內環分別采用4種控制方案，即PID、ADRC、HODFC和所提出的IHODFC(式(26))，并進行對比分析，實驗驗證了所提控制算法在控制性能上優于其他控制策略。

圖6 基于Pixhawk的QUAV姿態系統控制測試平臺

四旋翼飛行器在萬向節底座上的初始狀態約為[φ0θ0ψ0]T=[0° -35° 0°]T，參考的初始狀態為[φre0θre0ψre0]T=[0° 0° 0°]T，四旋翼飛行器在萬向節底座上的滾轉角和俯仰角最大值為φmax=θmax=±35°。

實驗中的控制器參數參考仿真參數，再根據飛行器實際飛行效果進行了調整，最終采用的參數如表1所示。

5.1 滾轉角跟蹤階躍參考

本組實驗測試4種控制算法跟蹤階躍參考的性能，滾轉角的參考值φre通過遙控器實時設置

(34)

式中：ch1代表遙控器的滾轉通道搖桿，對應的PWM值范圍是1 100～1 900 μs。首先通過遙控器的油門通道(ch4)使機體到達參考的初始姿態[φre0θre0ψre0]T=[0° 0° 0°]T，并保持懸停一段時間，當ch1值大于1 700 s時，滾轉角的參考值變為8.6°。基于4種控制算法跟蹤參考滾轉角的結果如圖7所示。

圖7 滾轉角基于4種控制算法跟蹤階躍參考

該組實驗與圖5所示的仿真結果相比，在控制器參數完全相同或類似的情況下，實際系統跟蹤參考姿態角的超調更大，響應速度更慢。其中ADRC的暫態表現出明顯振蕩，主要是由于ESO的觀測器參數較小，面對實際系統中更大的不確定性還需要繼續增大參數來提高估計精度，但觀測器參數越大容易引入高頻噪聲。相比之下，HODFC和IHODFC在選擇比仿真時更小的觀測器參數條件下，響應速度也要比前兩者快，且IHODFC的超調更小。

5.2 俯仰角跟蹤柔化的階躍參考

本組實驗測試俯仰角跟蹤柔化的階躍參考的控制性能，俯仰角的參考值通過實時改變遙控器的舵量給出，以模擬實際中給定的緩慢變化的參考信號。4種控制算法下飛行器跟蹤參考俯仰角的結果如圖8所示。

圖8 俯仰角基于4種控制算法跟蹤柔化的階躍參考

與圖7相比，參考信號變化較小，在跟蹤緩慢變化的參考信號時，四種控制方法的超調均有所減小。IHODFC的控制效果仍然是最佳的，幾乎沒有超調，且響應更快，跟蹤精度更高。

5.3 滾轉角抗擾測試

本組實驗測試控制算法對擾動的抑制能力。實驗前在飛行器機體左側懸掛同一重物，并且先將其托起，使它不影響飛行器的正常飛行。在飛行器保持懸停一段時間后，瞬間釋放機體左側懸掛的重物，且該重物不移除，模擬機體可能受到的外部擾動。4種控制算法的抗擾測試結果如圖9所示。

可以看出，在PID控制下，滾轉角受到擾動之后，有很大的超調，且無法克服擾動影響重新回到原來的平衡位置，即有穩態誤差；HODFC和ADRC較PID的超調小，但ADRC有長時間的振蕩，二者都能克服擾動影響重新回到平衡位置；IHODFC能快速抑制擾動，以更高精度重新保持平衡狀態，抗擾能力有很大提升。

圖9 基于4種控制算法的抗擾測試

因此，通過測試四旋翼飛行器的姿態角跟蹤階躍參考和柔化的階躍參考控制，與其他3種不依賴模型控制方案對比，提出的IHODFC控制策略大幅減小了超調，提高了飛行器的跟蹤速度和穩態精度，且具有更強的抗干擾能力。

6 結 論

1) 估計誤差收斂性分析表明，改進的高階微分器(IHOD)對于未知函數的估計比擴張狀態觀測器(ESO)高1個類型。對于二階無窮小函數，IHOD的估計誤差能收斂至零，而ESO的估計誤差有界；當未知函數二階導有界時，IHOD的估計誤差有界，而ESO發散。仿真驗證了IHOD的收斂性和較高的估計精度。

2) 利用Lyapunov函數證明了基于改進的高階微分反饋控制策略的閉環系統的穩定性和收斂性。

3) 在基于Pixhawk的四旋翼飛行器控制測試平臺上，驗證了所提出的改進的高階微分反饋控制策略的有效性，并與經典的PID控制，自抗擾控制，傳統的高階微分反饋控制做了對比。實驗結果表明所提方法使四旋翼飛行器跟蹤參考姿態的超調大幅減小，響應時間大幅縮短，跟蹤參考姿態的精度比PID、ADRC和HODFC都有所提高；且抗擾能力較PID、ADRC和HODFC更強。