基于擴張狀態觀測器的無人機云臺系統控制算法

高胡震，杜昌平，鄭 耀

（浙江大學 航空航天學院，杭州 310027）

0 引言

隨著技術的不斷發展，無人機(Unmanned Aerial Vehicle,UAV)在軍事和民用領域得到了越來越廣泛的應用。通過掛載各種載荷設備，UAV 能完成各種任務，諸如航拍、偵查、搜救和測繪等［1-2］。UAV 通常通過機載相機跟蹤地面或空中的動目標，理想情況下目標需要時刻保持在相機視場的中心。由于UAV 飛行時機身姿態不斷變化，且容易受到外界環境的擾動，直接將相機與UAV 固連并不能獲得預期的效果，而通過將相機裝載在增穩云臺上可以抑制無人機機身姿態變化帶來的干擾角運動。因此，相機云臺系統的控制在目標跟蹤任務中起到至關重要的作用［3］。

針對云臺系統的控制問題，國內外研究人員提出了各種穩定性算法。李樹勝等［4］將三軸平臺控制系統簡化為3 個單框架子系統，提出了三環復合比例-積分-微分（Proportional-Integral-Derivative，PID）控制方案，包括電流回路、穩定回路和跟蹤回路，并驗證了平臺的穩態精度和動態響應能力；但控制系統的參數較多，參數整定較為復雜。Abdo 等［5］在經典PID 控制的基礎上應用模糊控制，設計了一種自調節模糊PID 控制器，在線實時整定PID 參數；但模糊邏輯的設計需要專家經驗和大量實驗分析，并且模糊規則過多時容易造成規則爆炸。Altan 等［6］采用非線性Hammerstein 塊結構對三軸云臺系統進行建模，并提出了基于模型預測控制（Model Predictive Control，MPC）算法，實現實時的目標跟蹤。Qadir等［7］提出了一種基于視覺的模糊神經網絡（Fuzzy Neural Network，FNN）控制方法，將相機的視覺信息作為控制輸入，通過FNN 生成云臺俯仰和偏航方向的運動以及轉速指令，使待追蹤的目標時刻保持在畫面中心。網絡模型首先離線訓練，當誤差滿足要求后再在線訓練，以適應空速變化造成的誤差。綜上，目前UAV 云臺控制方法主要分為兩個方向：一是經典控制方法，如經典PID 控制以及在它的基礎上改進的串級PID 控制和模糊PID 控制。傳統PID 控制實現較簡單，但參數整定困難，精度難以提升，且對外界擾動的抑制不強；而模糊控制雖然精度較高，但規則設計較復雜。二是智能控制方法，如MPC、FNN 等，這類控制方法對模型精度要求較低，但是對求解器的要求較高，需要大量快速的計算，實時性難以滿足。

鑒于此，本文提出一種適用于小型無人機目標跟蹤任務的基于擴張狀態觀測器的串級PID 控制方法。首先，通過姿態解算得到云臺在機體坐標系下的姿態角，根據實際姿態角與期望姿態角的誤差構建串級PID 控制回路；同時引入擴張狀態觀測器（Extended State Observer，ESO），估計系統的耦合作用和外界擾動，補償了控制輸入。本文提出的ESO_PID復合控制算法主要有以下優點：

1）通過對角速度的準確估計，避免了強耦合作用帶來的復雜動力學建模問題。

2）估計了外界擾動，補償了輸入端，提高了抗干擾能力。

3）算法復雜度低，適合小型無人機目標跟蹤任務下的云臺控制。

1 機載云臺數學模型

機載云臺通過基座固連在UAV 上，可繞轉軸轉動。按照自由度劃分，一般可分為二自由度云臺和三自由度云臺，本文以三自由度云臺作為控制對象。云臺系統包括基座、偏航框、俯仰框、滾轉框和對應電機，其中：基座固連在UAV上；偏航電機安裝在基座上，負載偏航框；滾轉電機安裝在偏航框上，負載滾轉框；俯仰電機安裝在滾轉框上，負載俯仰框，云臺相機則安裝在俯仰框上。假設云臺3 個框架的轉軸交點與整個云臺系統的質心重合。

1.1 云臺系統坐標系

機體坐標系與UAV 固連，它的原點位于云臺系統的質心，遵循右手法則。xb軸位于UAV 參考平面內平行于機身軸線并指向無人機前方，yb軸垂直于UAV 參考面并指向UAV 右方，zb軸在參考面內垂直于xbyb平面，指向UAV 下方。

偏航框負責偏航方向的轉動。偏航框坐標系與偏航框固連，它的原點位于云臺系統的質心，遵循右手法則。當云臺偏航角為ψ時，由機體坐標系到偏航框坐標系的旋轉矩陣為：

滾轉框負責滾轉方向的轉動。滾轉框坐標系與滾轉框固連，它的原點位于云臺系統的質心，遵循右手法則。當云臺滾轉角為?時，由偏航框坐標系到滾轉框坐標系的旋轉矩陣為：

俯仰框負責俯仰方向的轉動。俯仰框坐標系與俯仰框固連，它的原點位于云臺系統的質心，遵循右手法則。當云臺俯仰角為θ時，由滾轉框坐標系到俯仰框坐標系的旋轉矩陣為：

1.2 機載云臺姿態模型

當無人機機動時，由于云臺基座與UAV 固連，無人機的姿態會影響云臺相機的拍攝。云臺相機的姿態角由基座相對慣性空間的歐拉角和云臺相機相對基座的歐拉角合成得到。設無人機在慣性空間下的姿態角為[ψθ?]T，相機云臺偏航框、俯仰框和滾轉框的轉動角分別為ψc、θc和?c，即相機云臺在機體坐標系下的姿態角為[ψcθc?c]T，得到由機體坐標系到相機坐標系的旋轉矩陣：

通過與UAV 和云臺相機分別捷聯的陀螺儀，可以實時獲得UAV 和云臺相機在慣性坐標系下的姿態角。設分別為慣性坐標系到機體坐標系的旋轉矩陣和慣性坐標系到相機坐標系的旋轉矩陣，且均可通過兩組姿態角得到。由慣性坐標系到相機坐標系的旋轉矩陣可以表示為：

結合式（4），可得云臺三軸的轉角為：

故通過機載的陀螺儀測量得到的UAV 姿態角和云臺相機姿態角，可以解算出當前云臺在三軸方向的轉角，即云臺在機體坐標系內的姿態角。另外，云臺角速度相關的信息也需要進一步解算。

設UAV 機身角速度為ωb，偏航、俯仰和滾轉框架的電機角速度分別為ω1、ω2和ω3，則云臺相機的角速度在相機坐標系下的描述可以表示為：

可以觀察到，由于云臺的構造關系，3 個框架的角速度均由兩部分構成：一是控制該框架自身轉動的角速度；二是上層角速度在自身坐標系下的耦合部分。角加速度的耦合作用則更加復雜。

由此可見，若直接根據系統的運動學模型去設計，3 個通道的耦合作用將使得控制系統變得相當復雜。故將整體系統的設計簡化到3 個通道的自抗擾控制上。針對控制器的耦合效應，可以將偏航、俯仰、滾轉通道均作為獨立的子系統，其他通道的耦合作用可以作為該子系統的外擾作用，通過自抗擾控制的介入對系統的總擾動估計并加以補償。

2 機載云臺控制系

云臺系統期望值和反饋值的誤差作為控制器的輸入、輸出控制云臺轉動，使云臺到達期望值。本文設計的云臺控制系統面向UAV 目標跟蹤的任務場景，控制目標是保持云臺相機的視軸指向穩定，且視軸相對目標連線的偏差越小越好，故將云臺的姿態角作為控制量，控制器輸出各通道電機的控制電壓。云臺系統的控制結構如圖1 所示。

圖1 云臺系統的控制結構Fig.1 Control structure of gimbal system

2.1 擴張狀態觀測器

UAV 實際飛行過程中存在許多未知擾動，控制的精度會受到影響。作為自抗擾控制（Active Disturbance Rejection Control，ADRC）的核心部分之一的ESO 可有效抑制外界不確定因素對于系統的干擾，它的核心思想是把能夠影響被控輸出的擾動作用擴張成新的狀態變量，并抑制擾動以將系統線性化，轉化為積分器串聯型系統進行控制［8-9］。

針對n階非線性時變系統：

式中：x為系統狀態變量，f(x1，x2，…，xn，w(t)，t)為系統的總和擾動作用，w(t)為外擾作用部分，b為控制增益，u為系統控制量。

將未知部分（即系統總合擾動作用）擴張為新的狀態量：

則擴張后的系統狀態可表示為：

將擴張后的系統狀態量的估計值設為Z=[z1z2…znzn+1]T，可以建立如下狀態觀測器：

原來的非線性系統（9）變成了線性的積分器串聯型系統。

飽和函數fal(e，α，δ)是原點附近具有線性段的連續的冪次函數，其增益與誤差之間存在類似反比例的關系，目的是在數值仿真時避免高頻顫振現象的出現：

其中：e為觀測值和狀態值的誤差；α決定了函數增益的大??；δ與函數線性段的斜率成反比，決定了函數的線性區寬度［10］。

無論函數f(x1，x2，t，w(t))是否連續，是否已知，只要它在過程進程中的實時作用量a(t)=f(x1(t)，x2(t)，t，w(t))是有界的，即參數b已知，總可以選擇適當的參數β使ESO 能很好地實時估計對象的狀態和被擴張的狀態［11-12］。

2.2 PID串級控制原理

PID 控制是一種經典控制方法，以被控對象與期望值的誤差為基礎，由比例、積分和微分三部分組成?；镜腜ID控制器算法為：

式中：u(t)為控制器輸出；e(t)為系統誤差，作為控制器輸入；Kp、Ti、Td分別為控制器的比例放大系數、積分時間和微分時間。設u(k)為第k次采樣時刻控制器的輸出，可得到離散型PID 算法：

通過串級PID 控制云臺，分為位置環和速度環：位置環為外環，輸入是期望的角度位置，輸出是期望的角速度信息并作為內環的輸入，采用比例控制；速度環為內環，輸入是期望的角速度，負責控制電機轉速，采用PI 控制。云臺的PID串級控制結構如圖2 所示。

圖2 云臺PID串級控制結構Fig.2 PID serial control structure for gimbal system

2.3 ESO_PID復合控制算法

PID 控制設計簡單，并且對期望目標的跟蹤效果良好。但是PID 的控制精度受到系統模型的限制，而工程實際中很難獲得系統精確的數學模型，尤其是三軸云臺這種強耦合的系統，云臺角速度受到其余通道的耦合作用。只有陀螺儀能夠實時獲取角速度數據，但要得到各通道的角速度信息需要解算機體角速度，過于繁瑣。而ESO 可以實時估計不確定系統的各狀態量［13］，通過引入ESO 到串級PID 控制系統中，可以對云臺的角速度信息做出準確的觀測，同時補償擾動，以提高云臺控制系統的控制精度。根據式（7），通過實時采集UAV 和云臺相機的姿態信息，可以解算出云臺3 個通道的轉角，將該轉角信息作為ESO 模塊的狀態值，便可以實時得到準確的角速度觀測值。

云臺電壓與角速度有如下關系［14］：

設耦合角加速度和未知擾動對角加速度的影響合為f，控制增益為b，則各控制通道的狀態空間表達式為：

式中：x1為姿態角，x2為角速度。將未知部分f擴張為新的狀態量：

則擴張后的系統狀態可表示為：

將擴張后的狀態量的估計值設為Z=[z1z2z3]T，可以建立如下狀態觀測器：

當這個系統進入穩態時，方程右端全收斂于0，因此誤差系統的穩態誤差為：

只要β3足夠大于w0，這些估計誤差都會足夠?。?1］。只要擾動有界，總能選取合適的參數來保證ESO 的收斂。

在Simulink 中建立的ESO 模型如圖3 所示，k 是Simulink里因為數字太大無法顯示自動標注的符號。

2.2.2 專家權威程度 專家對咨詢內容的判斷依據(Ca)以及對問題的熟悉程度(Cs)進行自評，得出權威程度系數(Cr)，Cr=(Ca+Cs)/2，Cr＞0.7 為可接受程度。本研究中，Ca=0.9861，Cs=0.9139，Cr=0.950。

圖3 Simulink模型中的ESO模塊Fig.3 ESO module in Simulink

將控制量和解算得到的轉角信息作為ESO 模塊的輸入，輸出[z1z2z3]T便是對狀態量[x1x2x3]T的估計值。

根據式（16）中云臺電壓和角加速度的關系，通過擴張狀態觀測器觀測到的未知擾動Z3可以用于補償控制輸入：

補償系數即為ESO 中控制輸入增益系數的倒數，參數均可通過查閱電機的相關參數得到。

將ESO 引入串級PID 控制中，可以得到偏航通道下的ESO_PID 復合控制算法，如圖4 所示。其中，期望偏航角由姿態解算得到，作為位置環的輸入；位置環PID 計算得到期望偏航角速度，作為速度環的輸入；速度環PID 計算得到期望的電機電壓，經ESO 的輸出z3補償后，作為偏航電機的輸入；偏航電機輸出對應的轉速，經積分環節后與控制輸入一同作為ESO 的輸入；ESO 的輸出z1、z2和z3分別為偏航角、偏航角速度和耦合角加速度的觀測量，z2作為速度環PID 的輸入，z3作為控制輸入的補償量，構成整體的控制回路。

圖4 偏航通道ESO_PID復合控制算法Fig.4 ESO_PID composite algorithm in yaw channel

3 實驗驗證

本章對ESO_PID 復合算法仿真驗證。仿真平臺采用Windows 10，CPU 采用Intel 酷睿i7 8700K，仿真環境采用Simulink，針對無人機航拍目標跟蹤的任務場景，構建了兩類飛行場景，分別對控制算法仿真驗證，并和傳統PID算法對比。

3.1 算法驗證

對ESO_PID 復合控制算法驗證。在無指令情況下和給定指令情況下分別驗證控制算法的性能，定量指標采用姿態角的均方根誤差（Root Mean Squared Error，RMSE）。云臺各通道的傳遞函數［15］為：

根據文獻［16］提出的參數β與斐波那契數列的關系，給出了擴張狀態觀測器參數與仿真步長h的關系：

結合實驗，選取了對應的參數β，采用PI 控制，各控制參數根據經驗選取。仿真的相關參數如表1 所示。

表1 仿真參數設置Tab.1 Simulation parameter setting

3.1.1 無指令情況

考慮無指令情況下，云臺僅受到外界隨機擾動，在偏航通道下對兩種控制算法仿真實驗。在控制輸入u上添加功率為5，采樣時間為0.05 s 的隨機白噪聲，對ESO_PID 復合控制算法和傳統PID 算法的抗干擾能力進行仿真，結果如圖5所示，其中，REF 為云臺偏航通道的角度隨時間變化的期望曲線，由于是無指令情況，期望偏航角始終保持為0°。兩種算法的RMSE 分別為0.235 7°和0.771 1°，ESO_PID 復合控制算法相較于傳統PID 算法在無指令、隨機擾動的情況下偏航通道的角誤差降低了69.43%，可見所提算法對外界擾動的抗干擾能力更強。

圖5 偏航通道在隨機外擾下的角誤差Fig.5 Angle error of yaw channel under random disturbance

3.1.2 給定指令情況

給定俯仰通道的期望角信號，仿真驗證兩種控制算法對它的跟蹤能力，同樣在控制輸入u上添加隨機白噪聲。兩種算法的跟蹤效果如圖6 所示，其中，REF 為云臺俯仰通道的角度隨時間變化的期望曲線。兩種算法的RMSE 分別為0.631 7°和1.352 5°，ESO_PID 復合控制算法相較于傳統PID 算法在給定指令、隨機擾動的情況下俯仰通道的角誤差降低了53.29%，可見所提算法跟蹤的精度更高，角度誤差更小。

圖6 俯仰通道下對給定期望角的跟蹤效果Fig.6 Tracking effect of pitch channel under expected angle

通過以上測試，在數值仿真上驗證了ESO_PID 復合控制算法的可行性和穩定性。

3.2 飛行場景及參數設置

針對無人機不同的飛行狀態，使用Simulink 中無人機工具箱（UAV Toolbox）中的航路點跟隨（Waypoint Follower）模塊，構建了矩形及圓形的飛行路徑，相關航點設置如下，飛行場景的軌跡如圖7 所示。

圖7 無人機飛行路徑及對應俯視圖Fig.7 UAV flight paths and corresponding top views

矩形路徑航點 ［0，0，50］，［500，0，50］，［500，500，50］，［0，500，50］，［0，0，50］。

圓形路徑航點 以［250，250，50］為圓心、250 為半徑的圓上均勻分布的12 個點。

根據1.2 節中的姿態解算方法，構建了姿態解算模塊，如圖8 所示，根據UAV 和云臺相機在慣性系中的姿態角測量值，計算出云臺相機在相機系下的3 個通道的期望角度ψref、θref和?ref，作為控制算法的輸入，用于仿真UAV 目標跟蹤場景下云臺控制算法的效果。

圖8 姿態解算模塊Fig.8 Attitude calculation module

待跟蹤目標的飛行路徑也通過航路點跟隨模塊生成，有直線和盤旋兩種飛行情景，與UAV 的兩種飛行路徑進行組合，可以構建出4 種不同的飛行場景，并對不同的飛行情景仿真。其中，REF 為云臺各通道的角度隨時間變化的期望曲線。在場景一中，PID 算法在偏航、俯仰、滾轉通道的角度RMSE 分別為2.396 8°、1.443 3°、1.156 7°，平均為1.665 6°，ESO_PID 復合控制算法在偏航、俯仰、滾轉通道的角度RMSE分別為1.234 3°、0.588 9°、0.597 7°，平均為0.880 70°，控制效果如圖9 所示。

圖9 控制方法在場景1中的性能Fig.9 Control performance in scenario 1

兩種控制方法的對比結果如表2 所示，其中的誤差為偏航、俯仰、滾轉三個通道下角度RMSE 的平均值。ESO_PID復合控制方法在各個場景下的平均誤差為0.946 3°，相較于傳統PID 算法的平均誤差1.909 0°，降低了50.43%。

表2 PID與ESO_PID控制結果對比 單位：（°）Tab.2 Comparison of control results among PID and ESO_PID unit：（°）

4 結語

本文提出一種適用于目標跟蹤任務的無人機云臺控制算法，在串級PID 控制的基礎上，結合了擴張狀態觀測器提高算法的性能。針對三軸云臺高度耦合的特性，利用擴張狀態觀測器觀測云臺的角速度信息，避免了建立煩冗的數學模型；同時根據偏航、俯仰和滾轉各通道的電機參數補償控制輸入。仿真實驗表明，ESO_PID 復合控制算法在保證控制精度的同時，大幅提高了系統對外源擾動的抗干擾能力，在不同飛行場景下的表現也比傳統控制算法更加優異。

后續的工作中，結合自適應算法調節相關參數，在大誤差時提高收斂速度，小誤差時提高控制精度，并在實物上驗證所提算法的性能。