王史春

(1.臺(tái)州職業(yè)技術(shù)學(xué)院，浙江臺(tái)州 318000；2.浙江大學(xué)智能系統(tǒng)與控制研究所，工業(yè)控制技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室,浙江杭州 310027)

四旋翼飛行器是一個(gè)具有六自由度和4個(gè)控制輸入的非線性欠驅(qū)動(dòng)系統(tǒng)，具有對(duì)外界和自身抗干擾敏感的特性，控制的主要問題是解決強(qiáng)耦合性和不穩(wěn)定的動(dòng)力特性。姿態(tài)控制是整個(gè)飛行控制的關(guān)鍵，如果能精確控制飛行器姿態(tài)，模型相對(duì)還是比較明確的，難度仍然在PID參數(shù)的整定上[1]，其參數(shù)調(diào)整很大程度上依賴于設(shè)計(jì)人員自身經(jīng)驗(yàn)，只能人為的多次調(diào)試才能確定，達(dá)到穩(wěn)定狀態(tài)，但不一定是最佳參數(shù)。針對(duì)本實(shí)驗(yàn)室在設(shè)計(jì)時(shí)遇到的以上問題，提出了改進(jìn)的PSO算法[2]和遺傳算法相結(jié)合方法來優(yōu)化PID參數(shù)。四旋翼飛行器是多變量系統(tǒng)，需多目標(biāo)搜索，PSO算法全局搜索能力強(qiáng)，算法實(shí)現(xiàn)簡(jiǎn)單，能夠?qū)崿F(xiàn)多個(gè)目標(biāo)尋優(yōu)，尋找合適的參數(shù)，以達(dá)到理想的效果。遺傳算法在不需要給出控制器初始參數(shù)的情況下，通過復(fù)制、交叉、變異操作，尋找到合適的參數(shù)，達(dá)到尋優(yōu)，使控制目標(biāo)滿足要求，同時(shí)能解決多值函數(shù)問題以及在多參數(shù)尋優(yōu)中，容易造成尋優(yōu)失敗或時(shí)間過長(zhǎng)的問題，具有局部搜索能力強(qiáng)的特點(diǎn)，并能從多點(diǎn)開始并行操作，高效啟發(fā)式搜索，避免了過早陷入局部最優(yōu)解[3]。

1 動(dòng)力性建模

四旋翼飛行器是一個(gè)非線性、多變量、高度耦合的欠驅(qū)動(dòng)系統(tǒng)[4]。為實(shí)現(xiàn)對(duì)四旋翼飛行器這一欠驅(qū)動(dòng)系統(tǒng)的有效控制，必須準(zhǔn)確建立其在各種飛行狀態(tài)下的數(shù)學(xué)模型。為了得到四旋翼飛行器的數(shù)學(xué)模型，先建立2個(gè)基本坐標(biāo)系：如圖1所示，地面坐標(biāo)系G(OXYZ)和機(jī)體坐標(biāo)系S(oxyz)。在飛行器飛行過程中，地面坐標(biāo)系保持不變。如圖2所示，地面坐標(biāo)系G與機(jī)體坐標(biāo)系S的定位關(guān)系分別用歐拉角表示，分別為俯仰角φ，翻滾角θ，偏航角ψ。兩坐標(biāo)系的旋轉(zhuǎn)關(guān)系通過旋轉(zhuǎn)矩陣表示為

(1)

(2)

圖1 四旋翼飛行器示意圖Fig.1 Schematic diagram of quadrotor

圖2 歐拉角Fig.2 Euler angle

為了簡(jiǎn)化四旋翼飛行器的動(dòng)力學(xué)模型[4]，根據(jù)牛頓第二定律[5]，飛行器動(dòng)力學(xué)方程表示為

(3)

(4)

式中：F為飛行器所加外力和；m為飛機(jī)質(zhì)量；v為飛行器速度；M為飛行器所有外力距[6]的和；H為飛行器相對(duì)于地面坐標(biāo)系絕對(duì)動(dòng)量距。

根據(jù)動(dòng)力學(xué)方程，假設(shè)Fx，F(xiàn)y，F(xiàn)z為F在飛行器坐標(biāo)系3個(gè)坐標(biāo)軸上的分量;p，q，r為角速度ω在飛行器坐標(biāo)系3個(gè)軸的分量。根據(jù)式(1)，式(3)，式(4)得到機(jī)體坐標(biāo)系下線運(yùn)動(dòng)方程(Ki為綜合阻力系數(shù))：

(5)

(6)

(7)

本研究是用慣性測(cè)量單元測(cè)量四旋翼旋轉(zhuǎn)[7]，而歐拉角是時(shí)域變化的連續(xù)函數(shù)，三軸轉(zhuǎn)動(dòng)角速度，通過矩陣表示為

(8)

經(jīng)拉格朗日推導(dǎo)式(8)可得運(yùn)動(dòng)方程表示為

(9)

由式(9)可推出:

(10)

2 遺傳算法優(yōu)化PID控制器參數(shù)

2.1 PID控制

為了把四旋翼飛行器非線性耦合模型分解為4個(gè)獨(dú)立的控制通道[8]，經(jīng)過實(shí)驗(yàn)，利用小擾動(dòng)原理得到的線性方程組用來分析飛行器操縱性、穩(wěn)定性、有效性，能得到較好效果和好的準(zhǔn)確度。在小擾動(dòng)下進(jìn)行線性化處理，可得到飛行器小擾動(dòng)運(yùn)動(dòng)，其中，狀態(tài)變量：

x=[uvwpqrθφψ]T，

(11)

控制變量：

u=[u1u2u3u4]T。

(12)

為了簡(jiǎn)化控制系統(tǒng)的設(shè)計(jì)，假設(shè)姿態(tài)角和角速度之間為簡(jiǎn)單的積分關(guān)系，即

(13)

根據(jù)實(shí)驗(yàn)室的四旋翼飛行器有關(guān)參數(shù)和環(huán)境數(shù)據(jù)以及系統(tǒng)的傳遞函數(shù)G(S)=(s×I-A)-1B和線運(yùn)動(dòng)方程(5)、方程(6)、方程(7),可得各控制通道的傳遞函數(shù)。

俯仰通道：

(14)

翻滾通道：

(15)

偏航角：

(16)

X軸與俯仰角：

(17)

Z軸:

(18)

Y軸與翻滾角：

(19)

2.2 GA-PSO算法優(yōu)化PID參數(shù)

為了優(yōu)化各通道的PID的3個(gè)參數(shù)，GA-PSO算法的PID控制器以誤差e和誤差變化率ec作為輸入，以滿足不同時(shí)刻的e和ec對(duì)PID參數(shù)整定的要求,如圖3所示。PID控制器的控制算法為

(20)

在文獻(xiàn)[10]中,提出標(biāo)準(zhǔn)的PSO算法：

(21)

(22)

優(yōu)化PID控制器參數(shù)也取得了一定的成效。標(biāo)準(zhǔn)的PSO算法中粒子的移動(dòng)是以自身的局部最優(yōu)和全局最優(yōu)來改變粒子的方向和速度，容易陷入局部最優(yōu)值，特別對(duì)于多峰值函數(shù)。

圖3 PID控制器優(yōu)化結(jié)構(gòu)圖Fig.3 Optimization of PID controller structure

為了更好地對(duì)PID參數(shù)進(jìn)行優(yōu)化控制，提出將改進(jìn)的PSO算法和遺傳算法結(jié)合，其流程如圖4所示，算法的核心思想是通過改進(jìn)PSO算法，算法如下：

(23)

這里的pm是隨機(jī)選取周邊2個(gè)粒子，通過比較選取適配值高的粒子位置作為pm的值；Xi(k)和Vi(k)是粒子當(dāng)前的位置和方向；C是常數(shù)，一般取值為2。ω是權(quán)重，隨著粒子搜索的進(jìn)程，值會(huì)減少，提高搜索精度。標(biāo)準(zhǔn)的PSO和改進(jìn)的PSO算法最大不同在于學(xué)習(xí)過程不一樣，標(biāo)準(zhǔn)PSO算法通過自身最優(yōu)和局部最優(yōu)改變自身的速度和位置；而改進(jìn)的PSO算法通過周邊粒子綜合學(xué)習(xí)來改變自身的速度和位置，有利于充分學(xué)習(xí)，達(dá)到好的性能。當(dāng)?shù)谝粋€(gè)粒子通過周邊粒子綜合學(xué)習(xí)后，更新到一個(gè)較優(yōu)的位置，為了避免陷入局部最優(yōu)，特別對(duì)于多峰值函數(shù)來說，容易陷入局部最優(yōu)，通過遺傳算法的選擇、交叉、變異來對(duì)粒子進(jìn)行重新組合，避免了粒子早熟的現(xiàn)象。

本文的算法具有如下創(chuàng)新：

1)PSO和GA結(jié)合，發(fā)揮各自特長(zhǎng)，改善系統(tǒng)的性能，增強(qiáng)算法的適應(yīng)性；

2)PSO算法的權(quán)重系統(tǒng)ω隨搜索進(jìn)行逐步減小，有利于搜索速度與精度的提高；

3)改進(jìn)的PSO粒子得到充分學(xué)習(xí)，遺傳算法又可避免粒子的早熟現(xiàn)象。

圖4中，X1={x11，x12,x13},D是維數(shù),g是代數(shù)，max_gen指最大代數(shù)pm={pm1,pm2,pm3}。

3 仿真實(shí)驗(yàn)和飛行實(shí)踐

本仿真實(shí)驗(yàn)是通過對(duì)標(biāo)準(zhǔn)PSO算法和GA-PSO算法進(jìn)行比較，來驗(yàn)證算法的優(yōu)越性。采用時(shí)間為1 s,輸入指令為一階躍信號(hào)。為獲取滿意的過度過程動(dòng)態(tài)特性，采用誤差絕對(duì)值時(shí)間積分性能指標(biāo)作為參數(shù)選擇的最小目標(biāo)函數(shù)[9]。為了防止控制能量過大，在目標(biāo)函數(shù)中加入控制輸入的平方項(xiàng)。選用式(24)作為參數(shù)選取的最優(yōu)指標(biāo)：

(24)

圖4 GA-PSO控制優(yōu)化流程圖Fig.4 Chart of GA-PSO control and optimization

其中：e(t)為系統(tǒng)誤差;u(t)為控制器輸出;tu為上升時(shí)間；ω1,ω2,ω3為權(quán)值。

為了避免超調(diào)，采用了懲罰函數(shù)[10]，即一旦產(chǎn)生超調(diào)，將超調(diào)量作為最優(yōu)指標(biāo)的一項(xiàng)，此時(shí)最優(yōu)指標(biāo)為

Ife(t)<0,

ω4|e(t)|dt)+ω3×tu。

(25)

PSO算法中粒子數(shù)為40，最大代數(shù)為100，學(xué)習(xí)因子為1.51，權(quán)重ω代入式(25)計(jì)算可得。遺傳算法交叉概率和變異概率分別為pc=0.92,pm=0.035。仿真實(shí)驗(yàn)結(jié)果如圖5—圖10所示，標(biāo)準(zhǔn)PSO算法和GA-PSO算法都達(dá)到了較好的效果，相對(duì)來說，GA-PSO算法達(dá)到更好的控制效果。俯仰角如圖5所示，標(biāo)準(zhǔn)PSO算法在0.1 s時(shí)有較大的波動(dòng)，而GA-PSO能較快地達(dá)到穩(wěn)定效果；偏航角如圖6所示，達(dá)到穩(wěn)定時(shí)間GA-PSO有明顯的改善，相差10倍左右，穩(wěn)定性比標(biāo)準(zhǔn)PSO更好；翻滾角如圖7所示，達(dá)到穩(wěn)定時(shí)間和穩(wěn)定性相近。X軸方向如圖8所示，GA-PSO較快達(dá)到穩(wěn)定狀態(tài)；Y軸方向如圖9所示，達(dá)到穩(wěn)定時(shí)間和穩(wěn)定的效果相近；Z軸方向如圖10所示，GA-PSO穩(wěn)定性較好；達(dá)到穩(wěn)定時(shí)間比PSO快。經(jīng)過對(duì)飛行器PID優(yōu)化，得出PID各參數(shù)的值，如表1所示。各項(xiàng)數(shù)據(jù)表明，各項(xiàng)指標(biāo)在同行研究中達(dá)到領(lǐng)先水平，表明本文提出的GA-PSO算法在飛行器中達(dá)到了優(yōu)化的效果。

飛行器在實(shí)際飛行中，會(huì)受到各種信號(hào)的干擾和風(fēng)力的影響，導(dǎo)致飛行器的位置和姿態(tài)角發(fā)生變化，因此在仿真中加干擾信號(hào)進(jìn)行測(cè)試，在1 s，2 s,3 s的時(shí)間給姿態(tài)角加一個(gè)隨機(jī)干擾突變信號(hào)，仿真結(jié)果如圖11所示，飛行器的姿態(tài)角在0～3 s時(shí)間內(nèi)出現(xiàn)波動(dòng)，范圍在0.7～1 rad之間，波動(dòng)是非常大的，產(chǎn)生不穩(wěn)定，但最終通過控制器的調(diào)節(jié)回到了穩(wěn)定狀態(tài)。

圖5 俯仰角Fig.5 Angle of pitch

圖6 偏航角Fig.6 Yaw angle

圖7 翻滾角Fig.7 Rolling angle

圖8 X軸方向Fig.8 X direction

圖9 Y軸方向Fig.9 Y direction

圖10 Z軸方向Fig.10 Z direction

實(shí)驗(yàn)室的飛行控制器部分的結(jié)構(gòu)如圖12所示，飛行器姿態(tài)控制是通過PID參數(shù)的設(shè)置來達(dá)到飛行器的穩(wěn)定效果，PID參數(shù)設(shè)置如表1所示，為了檢驗(yàn)設(shè)計(jì)的效果，把表1數(shù)據(jù)應(yīng)用于實(shí)驗(yàn)室的飛行器，通過多次飛行試驗(yàn)，測(cè)試數(shù)據(jù)如表2所示，達(dá)到較好的效果。

圖11 干擾信號(hào)仿真圖Fig.11 Interfering signal simulation diagram

表1 PID參數(shù)優(yōu)化表Tab.1 PID parameters optimization rad

表2 飛行測(cè)試數(shù)據(jù)Tab.2 Flight test data rad

圖12 飛行控制器結(jié)構(gòu)Fig.12 Structure of the quadrotor

4 結(jié) 語

通過對(duì)實(shí)驗(yàn)室四旋翼飛行器進(jìn)行建模，提出改進(jìn)的GA-PSO算法，通過仿真實(shí)驗(yàn)結(jié)果表明，飛行器達(dá)到穩(wěn)定狀態(tài)快，精度高的特點(diǎn)。為了更好地驗(yàn)證算法的效果，對(duì)飛行器進(jìn)行多次試飛，測(cè)試數(shù)據(jù)表明，控制器具有較好的魯棒性和良好的控制效果。

參考文獻(xiàn)/References：

[1] 燕 翔，張持健.工業(yè)隨動(dòng)系統(tǒng)中模糊PID控制器的設(shè)計(jì)與應(yīng)用[J].河北工業(yè)科技,2011,28(2)：122-125.

YAN Xiang,ZHANG Chijian. The design and application of Industrial design of fuzzy PID controller in the servo system[J]. Hebei Journal of Industrial Science and Technology,2011,28(2):122-125.

[2] 宋 遙，李國(guó)名，徐 麗.采用蟻群算法模擬機(jī)器人尋路的仿真實(shí)驗(yàn)[J].河北工業(yè)科技,2010,27(5)：340-343.

SONG Yao, LI Guoming, XU Li. The simulation experiment of ant colony algorithm simulation robot in pathfinding[J]. Hebei Journal of Industrial Science and Technology,2010,27(5):340-343.

[3] 劉金琨.先進(jìn)PID控制MATLAB仿真[M].北京：電子工業(yè)出版社,2012.

LIU Jinkun. Simulation of Advanced PID Control and MATLAB[M].Beijing: Electronic industry press,2012.

[4] 黃溪流.一種四旋翼無人直升機(jī)飛行控制器的設(shè)計(jì)[D].南京：南京理工大學(xué)，2010.

HUANG Xiliu. Tontroller Design of Four-rotored Helicopters Flight[D].Nanjing：Nanjing University of Science and Technology,2010.

[5] MINH L D.Modeling and control of quadrotor MAV using vision based measurement[A].2010 International Forum on Strxtegic Technology(IFOST)[C].San:[s.n.],2010.70-75.

[6] SUBBARAO A K,LEWIS F.Dynamic inversion of quadrotor with zero-dynamics stabilization[A].IEEE International Conference on Control applications[C].San Antonio:[s.n.],2008.1 189-1 194.

[7] BOUABDALLAH S,SIEGWRT R.Design and control of quadrotors with application to autonomous flying[D]. Lausanne :EPFL,2006.

[8] 李 俊，李運(yùn)堂.四旋翼飛行器的動(dòng)力學(xué)建模及PID控制[J].遼寧工程技術(shù)大學(xué)學(xué)報(bào),2012,31(1)：114-117.

LI Jun,LI Yuntang. Four rotor aircraft dynamics modeling and PID control[J]. Journal of Liaoning Technology University,2012,31(1):114-117.

[9] 胡海兵，胡慶波，呂征宇.基于粒子群優(yōu)化PID伺服控制的設(shè)計(jì)[J].浙江大學(xué)學(xué)報(bào)(工學(xué)版),2006，40(12)：2 144-2 148.

HU Haibing， HU Qingbo， LYU Zhengyu. The design of based on particle swarm optimization PID servo control[J]. Journal of zhejiang University(Engineering Science),2006,40(12):2 144-2 148.

[10] 孫增圻.遺傳算法:智能控制理論與技術(shù)[M].北京：清華大學(xué)出版社,2011.

SUN Zenqi. Genetic Algorithm: Theory and Technology of Intelligent Control[M].Beijing: Tsinghua University Press,2011.