基于擴(kuò)張狀態(tài)觀測(cè)器的無(wú)人機(jī)云臺(tái)系統(tǒng)控制算法

高胡震，杜昌平，鄭 耀

（浙江大學(xué) 航空航天學(xué)院，杭州 310027）

0 引言

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

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

鑒于此，本文提出一種適用于小型無(wú)人機(jī)目標(biāo)跟蹤任務(wù)的基于擴(kuò)張狀態(tài)觀測(cè)器的串級(jí)PID 控制方法。首先，通過(guò)姿態(tài)解算得到云臺(tái)在機(jī)體坐標(biāo)系下的姿態(tài)角，根據(jù)實(shí)際姿態(tài)角與期望姿態(tài)角的誤差構(gòu)建串級(jí)PID 控制回路；同時(shí)引入擴(kuò)張狀態(tài)觀測(cè)器（Extended State Observer，ESO），估計(jì)系統(tǒng)的耦合作用和外界擾動(dòng)，補(bǔ)償了控制輸入。本文提出的ESO_PID復(fù)合控制算法主要有以下優(yōu)點(diǎn)：

1）通過(guò)對(duì)角速度的準(zhǔn)確估計(jì)，避免了強(qiáng)耦合作用帶來(lái)的復(fù)雜動(dòng)力學(xué)建模問(wèn)題。

2）估計(jì)了外界擾動(dòng)，補(bǔ)償了輸入端，提高了抗干擾能力。

3）算法復(fù)雜度低，適合小型無(wú)人機(jī)目標(biāo)跟蹤任務(wù)下的云臺(tái)控制。

1 機(jī)載云臺(tái)數(shù)學(xué)模型

機(jī)載云臺(tái)通過(guò)基座固連在UAV 上，可繞轉(zhuǎn)軸轉(zhuǎn)動(dòng)。按照自由度劃分，一般可分為二自由度云臺(tái)和三自由度云臺(tái)，本文以三自由度云臺(tái)作為控制對(duì)象。云臺(tái)系統(tǒng)包括基座、偏航框、俯仰框、滾轉(zhuǎn)框和對(duì)應(yīng)電機(jī)，其中：基座固連在UAV上；偏航電機(jī)安裝在基座上，負(fù)載偏航框；滾轉(zhuǎn)電機(jī)安裝在偏航框上，負(fù)載滾轉(zhuǎn)框；俯仰電機(jī)安裝在滾轉(zhuǎn)框上，負(fù)載俯仰框，云臺(tái)相機(jī)則安裝在俯仰框上。假設(shè)云臺(tái)3 個(gè)框架的轉(zhuǎn)軸交點(diǎn)與整個(gè)云臺(tái)系統(tǒng)的質(zhì)心重合。

1.1 云臺(tái)系統(tǒng)坐標(biāo)系

機(jī)體坐標(biāo)系與UAV 固連，它的原點(diǎn)位于云臺(tái)系統(tǒng)的質(zhì)心，遵循右手法則。xb軸位于UAV 參考平面內(nèi)平行于機(jī)身軸線并指向無(wú)人機(jī)前方，yb軸垂直于UAV 參考面并指向UAV 右方，zb軸在參考面內(nèi)垂直于xbyb平面，指向UAV 下方。

偏航框負(fù)責(zé)偏航方向的轉(zhuǎn)動(dòng)。偏航框坐標(biāo)系與偏航框固連，它的原點(diǎn)位于云臺(tái)系統(tǒng)的質(zhì)心，遵循右手法則。當(dāng)云臺(tái)偏航角為ψ時(shí)，由機(jī)體坐標(biāo)系到偏航框坐標(biāo)系的旋轉(zhuǎn)矩陣為：

滾轉(zhuǎn)框負(fù)責(zé)滾轉(zhuǎn)方向的轉(zhuǎn)動(dòng)。滾轉(zhuǎn)框坐標(biāo)系與滾轉(zhuǎn)框固連，它的原點(diǎn)位于云臺(tái)系統(tǒng)的質(zhì)心，遵循右手法則。當(dāng)云臺(tái)滾轉(zhuǎn)角為?時(shí)，由偏航框坐標(biāo)系到滾轉(zhuǎn)框坐標(biāo)系的旋轉(zhuǎn)矩陣為：

俯仰框負(fù)責(zé)俯仰方向的轉(zhuǎn)動(dòng)。俯仰框坐標(biāo)系與俯仰框固連，它的原點(diǎn)位于云臺(tái)系統(tǒng)的質(zhì)心，遵循右手法則。當(dāng)云臺(tái)俯仰角為θ時(shí)，由滾轉(zhuǎn)框坐標(biāo)系到俯仰框坐標(biāo)系的旋轉(zhuǎn)矩陣為：

1.2 機(jī)載云臺(tái)姿態(tài)模型

當(dāng)無(wú)人機(jī)機(jī)動(dòng)時(shí)，由于云臺(tái)基座與UAV 固連，無(wú)人機(jī)的姿態(tài)會(huì)影響云臺(tái)相機(jī)的拍攝。云臺(tái)相機(jī)的姿態(tài)角由基座相對(duì)慣性空間的歐拉角和云臺(tái)相機(jī)相對(duì)基座的歐拉角合成得到。設(shè)無(wú)人機(jī)在慣性空間下的姿態(tài)角為[ψθ?]T，相機(jī)云臺(tái)偏航框、俯仰框和滾轉(zhuǎn)框的轉(zhuǎn)動(dòng)角分別為ψc、θc和?c，即相機(jī)云臺(tái)在機(jī)體坐標(biāo)系下的姿態(tài)角為[ψcθc?c]T，得到由機(jī)體坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣：

通過(guò)與UAV 和云臺(tái)相機(jī)分別捷聯(lián)的陀螺儀，可以實(shí)時(shí)獲得UAV 和云臺(tái)相機(jī)在慣性坐標(biāo)系下的姿態(tài)角。設(shè)分別為慣性坐標(biāo)系到機(jī)體坐標(biāo)系的旋轉(zhuǎn)矩陣和慣性坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣，且均可通過(guò)兩組姿態(tài)角得到。由慣性坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣可以表示為：

結(jié)合式（4），可得云臺(tái)三軸的轉(zhuǎn)角為：

故通過(guò)機(jī)載的陀螺儀測(cè)量得到的UAV 姿態(tài)角和云臺(tái)相機(jī)姿態(tài)角，可以解算出當(dāng)前云臺(tái)在三軸方向的轉(zhuǎn)角，即云臺(tái)在機(jī)體坐標(biāo)系內(nèi)的姿態(tài)角。另外，云臺(tái)角速度相關(guān)的信息也需要進(jìn)一步解算。

設(shè)UAV 機(jī)身角速度為ωb，偏航、俯仰和滾轉(zhuǎn)框架的電機(jī)角速度分別為ω1、ω2和ω3，則云臺(tái)相機(jī)的角速度在相機(jī)坐標(biāo)系下的描述可以表示為：

可以觀察到，由于云臺(tái)的構(gòu)造關(guān)系，3 個(gè)框架的角速度均由兩部分構(gòu)成：一是控制該框架自身轉(zhuǎn)動(dòng)的角速度；二是上層角速度在自身坐標(biāo)系下的耦合部分。角加速度的耦合作用則更加復(fù)雜。

由此可見(jiàn)，若直接根據(jù)系統(tǒng)的運(yùn)動(dòng)學(xué)模型去設(shè)計(jì)，3 個(gè)通道的耦合作用將使得控制系統(tǒng)變得相當(dāng)復(fù)雜。故將整體系統(tǒng)的設(shè)計(jì)簡(jiǎn)化到3 個(gè)通道的自抗擾控制上。針對(duì)控制器的耦合效應(yīng)，可以將偏航、俯仰、滾轉(zhuǎn)通道均作為獨(dú)立的子系統(tǒng)，其他通道的耦合作用可以作為該子系統(tǒng)的外擾作用，通過(guò)自抗擾控制的介入對(duì)系統(tǒng)的總擾動(dòng)估計(jì)并加以補(bǔ)償。

2 機(jī)載云臺(tái)控制系

云臺(tái)系統(tǒng)期望值和反饋值的誤差作為控制器的輸入、輸出控制云臺(tái)轉(zhuǎn)動(dòng)，使云臺(tái)到達(dá)期望值。本文設(shè)計(jì)的云臺(tái)控制系統(tǒng)面向UAV 目標(biāo)跟蹤的任務(wù)場(chǎng)景，控制目標(biāo)是保持云臺(tái)相機(jī)的視軸指向穩(wěn)定，且視軸相對(duì)目標(biāo)連線的偏差越小越好，故將云臺(tái)的姿態(tài)角作為控制量，控制器輸出各通道電機(jī)的控制電壓。云臺(tái)系統(tǒng)的控制結(jié)構(gòu)如圖1 所示。

圖1 云臺(tái)系統(tǒng)的控制結(jié)構(gòu)Fig.1 Control structure of gimbal system

2.1 擴(kuò)張狀態(tài)觀測(cè)器

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

針對(duì)n階非線性時(shí)變系統(tǒng)：

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

將未知部分（即系統(tǒng)總合擾動(dòng)作用）擴(kuò)張為新的狀態(tài)量：

則擴(kuò)張后的系統(tǒng)狀態(tài)可表示為：

將擴(kuò)張后的系統(tǒng)狀態(tài)量的估計(jì)值設(shè)為Z=[z1z2…znzn+1]T，可以建立如下?tīng)顟B(tài)觀測(cè)器：

原來(lái)的非線性系統(tǒng)（9）變成了線性的積分器串聯(lián)型系統(tǒng)。

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

其中：e為觀測(cè)值和狀態(tài)值的誤差；α決定了函數(shù)增益的大??；δ與函數(shù)線性段的斜率成反比，決定了函數(shù)的線性區(qū)寬度［10］。

無(wú)論函數(shù)f(x1，x2，t，w(t))是否連續(xù)，是否已知，只要它在過(guò)程進(jìn)程中的實(shí)時(shí)作用量a(t)=f(x1(t)，x2(t)，t，w(t))是有界的，即參數(shù)b已知，總可以選擇適當(dāng)?shù)膮?shù)β使ESO 能很好地實(shí)時(shí)估計(jì)對(duì)象的狀態(tài)和被擴(kuò)張的狀態(tài)［11-12］。

2.2 PID串級(jí)控制原理

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

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

通過(guò)串級(jí)PID 控制云臺(tái)，分為位置環(huán)和速度環(huán)：位置環(huán)為外環(huán)，輸入是期望的角度位置，輸出是期望的角速度信息并作為內(nèi)環(huán)的輸入，采用比例控制；速度環(huán)為內(nèi)環(huán)，輸入是期望的角速度，負(fù)責(zé)控制電機(jī)轉(zhuǎn)速，采用PI 控制。云臺(tái)的PID串級(jí)控制結(jié)構(gòu)如圖2 所示。

圖2 云臺(tái)PID串級(jí)控制結(jié)構(gòu)Fig.2 PID serial control structure for gimbal system

2.3 ESO_PID復(fù)合控制算法

PID 控制設(shè)計(jì)簡(jiǎn)單，并且對(duì)期望目標(biāo)的跟蹤效果良好。但是PID 的控制精度受到系統(tǒng)模型的限制，而工程實(shí)際中很難獲得系統(tǒng)精確的數(shù)學(xué)模型，尤其是三軸云臺(tái)這種強(qiáng)耦合的系統(tǒng)，云臺(tái)角速度受到其余通道的耦合作用。只有陀螺儀能夠?qū)崟r(shí)獲取角速度數(shù)據(jù)，但要得到各通道的角速度信息需要解算機(jī)體角速度，過(guò)于繁瑣。而ESO 可以實(shí)時(shí)估計(jì)不確定系統(tǒng)的各狀態(tài)量［13］，通過(guò)引入ESO 到串級(jí)PID 控制系統(tǒng)中，可以對(duì)云臺(tái)的角速度信息做出準(zhǔn)確的觀測(cè)，同時(shí)補(bǔ)償擾動(dòng)，以提高云臺(tái)控制系統(tǒng)的控制精度。根據(jù)式（7），通過(guò)實(shí)時(shí)采集UAV 和云臺(tái)相機(jī)的姿態(tài)信息，可以解算出云臺(tái)3 個(gè)通道的轉(zhuǎn)角，將該轉(zhuǎn)角信息作為ESO 模塊的狀態(tài)值，便可以實(shí)時(shí)得到準(zhǔn)確的角速度觀測(cè)值。

云臺(tái)電壓與角速度有如下關(guān)系［14］：

設(shè)耦合角加速度和未知擾動(dòng)對(duì)角加速度的影響合為f，控制增益為b，則各控制通道的狀態(tài)空間表達(dá)式為：

式中：x1為姿態(tài)角，x2為角速度。將未知部分f擴(kuò)張為新的狀態(tài)量：

則擴(kuò)張后的系統(tǒng)狀態(tài)可表示為：

將擴(kuò)張后的狀態(tài)量的估計(jì)值設(shè)為Z=[z1z2z3]T，可以建立如下?tīng)顟B(tài)觀測(cè)器：

當(dāng)這個(gè)系統(tǒng)進(jìn)入穩(wěn)態(tài)時(shí)，方程右端全收斂于0，因此誤差系統(tǒng)的穩(wěn)態(tài)誤差為：

只要β3足夠大于w0，這些估計(jì)誤差都會(huì)足夠小［11］。只要擾動(dòng)有界，總能選取合適的參數(shù)來(lái)保證ESO 的收斂。

在Simulink 中建立的ESO 模型如圖3 所示，k 是Simulink里因?yàn)閿?shù)字太大無(wú)法顯示自動(dòng)標(biāo)注的符號(hào)。

2.2.2 專家權(quán)威程度 專家對(duì)咨詢內(nèi)容的判斷依據(jù)(Ca)以及對(duì)問(wèn)題的熟悉程度(Cs)進(jìn)行自評(píng)，得出權(quán)威程度系數(shù)(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

將控制量和解算得到的轉(zhuǎn)角信息作為ESO 模塊的輸入，輸出[z1z2z3]T便是對(duì)狀態(tài)量[x1x2x3]T的估計(jì)值。

根據(jù)式（16）中云臺(tái)電壓和角加速度的關(guān)系，通過(guò)擴(kuò)張狀態(tài)觀測(cè)器觀測(cè)到的未知擾動(dòng)Z3可以用于補(bǔ)償控制輸入：

補(bǔ)償系數(shù)即為ESO 中控制輸入增益系數(shù)的倒數(shù)，參數(shù)均可通過(guò)查閱電機(jī)的相關(guān)參數(shù)得到。

將ESO 引入串級(jí)PID 控制中，可以得到偏航通道下的ESO_PID 復(fù)合控制算法，如圖4 所示。其中，期望偏航角由姿態(tài)解算得到，作為位置環(huán)的輸入；位置環(huán)PID 計(jì)算得到期望偏航角速度，作為速度環(huán)的輸入；速度環(huán)PID 計(jì)算得到期望的電機(jī)電壓，經(jīng)ESO 的輸出z3補(bǔ)償后，作為偏航電機(jī)的輸入；偏航電機(jī)輸出對(duì)應(yīng)的轉(zhuǎn)速，經(jīng)積分環(huán)節(jié)后與控制輸入一同作為ESO 的輸入；ESO 的輸出z1、z2和z3分別為偏航角、偏航角速度和耦合角加速度的觀測(cè)量，z2作為速度環(huán)PID 的輸入，z3作為控制輸入的補(bǔ)償量，構(gòu)成整體的控制回路。

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

3 實(shí)驗(yàn)驗(yàn)證

本章對(duì)ESO_PID 復(fù)合算法仿真驗(yàn)證。仿真平臺(tái)采用Windows 10，CPU 采用Intel 酷睿i7 8700K，仿真環(huán)境采用Simulink，針對(duì)無(wú)人機(jī)航拍目標(biāo)跟蹤的任務(wù)場(chǎng)景，構(gòu)建了兩類飛行場(chǎng)景，分別對(duì)控制算法仿真驗(yàn)證，并和傳統(tǒng)PID算法對(duì)比。

3.1 算法驗(yàn)證

對(duì)ESO_PID 復(fù)合控制算法驗(yàn)證。在無(wú)指令情況下和給定指令情況下分別驗(yàn)證控制算法的性能，定量指標(biāo)采用姿態(tài)角的均方根誤差（Root Mean Squared Error，RMSE）。云臺(tái)各通道的傳遞函數(shù)［15］為：

根據(jù)文獻(xiàn)［16］提出的參數(shù)β與斐波那契數(shù)列的關(guān)系，給出了擴(kuò)張狀態(tài)觀測(cè)器參數(shù)與仿真步長(zhǎng)h的關(guān)系：

結(jié)合實(shí)驗(yàn)，選取了對(duì)應(yīng)的參數(shù)β，采用PI 控制，各控制參數(shù)根據(jù)經(jīng)驗(yàn)選取。仿真的相關(guān)參數(shù)如表1 所示。

表1 仿真參數(shù)設(shè)置Tab.1 Simulation parameter setting

3.1.1 無(wú)指令情況

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

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

3.1.2 給定指令情況

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

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

通過(guò)以上測(cè)試，在數(shù)值仿真上驗(yàn)證了ESO_PID 復(fù)合控制算法的可行性和穩(wěn)定性。

3.2 飛行場(chǎng)景及參數(shù)設(shè)置

針對(duì)無(wú)人機(jī)不同的飛行狀態(tài)，使用Simulink 中無(wú)人機(jī)工具箱（UAV Toolbox）中的航路點(diǎn)跟隨（Waypoint Follower）模塊，構(gòu)建了矩形及圓形的飛行路徑，相關(guān)航點(diǎn)設(shè)置如下，飛行場(chǎng)景的軌跡如圖7 所示。

圖7 無(wú)人機(jī)飛行路徑及對(duì)應(yīng)俯視圖Fig.7 UAV flight paths and corresponding top views

矩形路徑航點(diǎn) ［0，0，50］，［500，0，50］，［500，500，50］，［0，500，50］，［0，0，50］。

圓形路徑航點(diǎn) 以［250，250，50］為圓心、250 為半徑的圓上均勻分布的12 個(gè)點(diǎn)。

根據(jù)1.2 節(jié)中的姿態(tài)解算方法，構(gòu)建了姿態(tài)解算模塊，如圖8 所示，根據(jù)UAV 和云臺(tái)相機(jī)在慣性系中的姿態(tài)角測(cè)量值，計(jì)算出云臺(tái)相機(jī)在相機(jī)系下的3 個(gè)通道的期望角度ψref、θref和?ref，作為控制算法的輸入，用于仿真UAV 目標(biāo)跟蹤場(chǎng)景下云臺(tái)控制算法的效果。

圖8 姿態(tài)解算模塊Fig.8 Attitude calculation module

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

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

兩種控制方法的對(duì)比結(jié)果如表2 所示，其中的誤差為偏航、俯仰、滾轉(zhuǎn)三個(gè)通道下角度RMSE 的平均值。ESO_PID復(fù)合控制方法在各個(gè)場(chǎng)景下的平均誤差為0.946 3°，相較于傳統(tǒng)PID 算法的平均誤差1.909 0°，降低了50.43%。

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

4 結(jié)語(yǔ)

本文提出一種適用于目標(biāo)跟蹤任務(wù)的無(wú)人機(jī)云臺(tái)控制算法，在串級(jí)PID 控制的基礎(chǔ)上，結(jié)合了擴(kuò)張狀態(tài)觀測(cè)器提高算法的性能。針對(duì)三軸云臺(tái)高度耦合的特性，利用擴(kuò)張狀態(tài)觀測(cè)器觀測(cè)云臺(tái)的角速度信息，避免了建立煩冗的數(shù)學(xué)模型；同時(shí)根據(jù)偏航、俯仰和滾轉(zhuǎn)各通道的電機(jī)參數(shù)補(bǔ)償控制輸入。仿真實(shí)驗(yàn)表明，ESO_PID 復(fù)合控制算法在保證控制精度的同時(shí)，大幅提高了系統(tǒng)對(duì)外源擾動(dòng)的抗干擾能力，在不同飛行場(chǎng)景下的表現(xiàn)也比傳統(tǒng)控制算法更加優(yōu)異。

后續(xù)的工作中，結(jié)合自適應(yīng)算法調(diào)節(jié)相關(guān)參數(shù)，在大誤差時(shí)提高收斂速度，小誤差時(shí)提高控制精度，并在實(shí)物上驗(yàn)證所提算法的性能。