基于SimMechanics的直線二級(jí)倒立擺仿真研究

余永俊， 任一峰， 安 坤， 常超勇

(中北大學(xué)電氣與控制工程學(xué)院,太原 030051)

倒立擺被譽(yù)為“控制領(lǐng)域的一顆璀璨明珠[1]”,它的強(qiáng)耦合、非線性、高階次等特性吸引了眾多研究者,他們將其視為理想的實(shí)驗(yàn)平臺(tái)用來(lái)驗(yàn)證各算法的可靠性,并將研究的新控制方法廣泛用于航天、軍工、機(jī)器人、人工智能等各領(lǐng)域中[2-4]。同時(shí),在一些高等學(xué)府中,倒立擺系統(tǒng)也作為實(shí)驗(yàn)器材應(yīng)用到課堂和實(shí)驗(yàn)中。研究倒立擺系統(tǒng)不僅是揭示控制論諸多特性,而且具有重大商業(yè)價(jià)值和實(shí)際意義。

近年來(lái),中外控制界對(duì)倒立擺進(jìn)行了深入鉆研,研究者已用經(jīng)典比例積分微分(proportional integral derivative, PID)控制、現(xiàn)代控制理論和各智能控制方法實(shí)現(xiàn)了對(duì)各級(jí)倒立擺控制[5]。但大多數(shù)研究者致力于用Simulink搭建數(shù)學(xué)模型,而利用SimMechanics 搭建三維可視化模型仿真相對(duì)甚少。SimMechanics工具箱為多體動(dòng)力機(jī)械系統(tǒng)提供了直觀有效的建模分析手段,并在仿真過(guò)程中生成系統(tǒng)三維動(dòng)畫(huà),更便于系統(tǒng)的分析和驗(yàn)證。文獻(xiàn)[6]使用第二代SimMechanics工具箱建立旋轉(zhuǎn)倒立擺物理模型,通過(guò)極點(diǎn)配置、PD控制和基于線性二次型(linear quadratic regulator, LQR)控制以平衡倒立擺。文獻(xiàn)[7]用SimMechanics建立單級(jí)倒立擺模型,通過(guò)使用線性融合將多維模糊控制轉(zhuǎn)換單維模糊控制并實(shí)現(xiàn)倒立擺穩(wěn)定。文獻(xiàn)[8]針對(duì)單級(jí)倒立擺控制設(shè)計(jì)PID控制器,采用遺傳算法(genetic algorithm, GA)和人工蜂群算法(artificial bee colony, ABC)用于調(diào)整PID控制獲得最佳增益,并用于SimMechanics 模型中。文獻(xiàn)[9]設(shè)計(jì)一種全狀態(tài)觀反饋控制器實(shí)現(xiàn)單級(jí)倒立擺SimMechanics模型控制,并表明SimMechanics可用于不穩(wěn)定的非線性系統(tǒng)。文獻(xiàn)[10]對(duì)單極倒立擺自擺起和平衡控制問(wèn)題提出Bang-Bang控制和LQR控制,并運(yùn)用于單級(jí)倒立擺SimMechanics仿真。文獻(xiàn)[11]利用SimMechanics 工具箱建立了直線六級(jí)倒立擺模型,并利用LQR設(shè)計(jì)狀態(tài)反饋控制器實(shí)現(xiàn)了仿真控制。

現(xiàn)利用第二代SimMechanics工具箱對(duì)直線二級(jí)倒立擺進(jìn)行三維建模,并設(shè)計(jì)主導(dǎo)極點(diǎn)配置方法整定PID參數(shù)和基于LQR融合模糊控制進(jìn)行仿真,驗(yàn)證兩種算法的可行性并進(jìn)行對(duì)比分析。

1 二級(jí)倒立擺的數(shù)學(xué)模型

忽略空氣阻力和各種摩擦力之后,可視二級(jí)直線倒立擺系統(tǒng)是一個(gè)運(yùn)動(dòng)剛體系統(tǒng),其簡(jiǎn)化結(jié)構(gòu)如圖1所示,建模過(guò)程所用物理參數(shù)意義及取值如表1所示。

圖1 二級(jí)倒立擺結(jié)構(gòu)Fig.1 Double inverted pendulum structure

表1 直線二級(jí)倒立擺參數(shù)Table 1 Line double inverted pendulum parameters

利用拉格朗日方程推導(dǎo)系統(tǒng)數(shù)學(xué)模型,拉格朗日方程為

式(1)中：L為拉格朗日算子；T為系統(tǒng)動(dòng)能；V為系統(tǒng)勢(shì)能；q為系統(tǒng)的廣義坐標(biāo)。

式(2)中：i=1,2,3，…，n；fi是系統(tǒng)在廣義坐標(biāo)qi方向上的廣義力,在本系統(tǒng)中,假設(shè)系統(tǒng)的3個(gè)廣義坐標(biāo)分別是x、θ1、θ2。

系統(tǒng)的總動(dòng)能為

系統(tǒng)的總勢(shì)能為

V=Vm1+Vm2+Vm3=m1y1+m2y2+m3y3=

m1l1cosθ1+m2(2l1cosθ1+l2cosθ2)+2m3l1cosθ1(4)

由于在廣義坐標(biāo)θ1、θ2上均無(wú)外力作用,則有以下等式成立:

取平衡位置時(shí)各變量初值為零,在平衡位置進(jìn)行泰勒級(jí)數(shù)展開(kāi)并線性化,將各參數(shù)值帶入,得到系統(tǒng)的狀態(tài)方程為

2 倒立擺SimMechanics模型

SimMechanics是Simulink的一個(gè)建模工具箱,可以按照物理學(xué)原理對(duì)剛體系統(tǒng)進(jìn)行建模和仿真[12]。并包含了一系列帶有坐標(biāo)系變換、靜力學(xué)約束、驅(qū)動(dòng)、鉸鏈等系統(tǒng)原件用以搭建和求解實(shí)際物理系統(tǒng)的模型,并能與Simulink設(shè)計(jì)的控制器連接進(jìn)行綜合仿真。SimMechanics有一代和二代,由于第二代模型相對(duì)直觀,能夠直接建立機(jī)械結(jié)構(gòu)的幾何特性,因此將采用第二代工具箱進(jìn)行模型構(gòu)建。基于上述建立的數(shù)學(xué)模型,可構(gòu)建倒立擺SimMechanics結(jié)構(gòu)如圖2所示,可視化模型如圖3所示。

圖2 倒立擺SimMechanics模型Fig.2 Inverted pendulum SimMechanics model

圖3 倒立擺可視化模型Fig.3 Inverted pendulum visualization model

圖2中,Mechanism Configuration是機(jī)構(gòu)配置模塊,用于整個(gè)機(jī)器的機(jī)械和模擬參數(shù)；Configuration和World Frame分別是機(jī)構(gòu)求解和世界坐標(biāo)系；上述3個(gè)模塊是構(gòu)建任何模型的基礎(chǔ)。Rigid Transform是兩個(gè)結(jié)構(gòu)體進(jìn)行連接的模塊；Revolute Jonit是連接兩根擺桿間的轉(zhuǎn)動(dòng)模塊；SPS是將單位的仿真輸入轉(zhuǎn)變?yōu)槲锢磔敵?PSS則相反；而長(zhǎng)板、小車(chē)、擺桿1、擺桿2是各模塊子系統(tǒng)，里面集成了Solid剛體模塊和坐標(biāo)系連接；Solid表示將幾何圖形、慣性和質(zhì)量以及圖形組件合并為單個(gè)單元實(shí)體；Disturbance 是指在所附的框架上應(yīng)用外部力和扭矩。p、v、q1、w1、q2、w2是輸出,分別代表小車(chē)位移、小車(chē)速度、擺桿1角度、擺桿1角速度、擺桿2角度、擺桿2角速度。f為輸入,df為加在第2根擺桿的擾動(dòng)。

3 倒立擺SimMechanics模型仿真

3.1 PID控制算法仿真

由構(gòu)建的倒立擺SimMechanics模型(圖2)可知,直線二級(jí)倒立擺是單輸入多輸出系統(tǒng),而PID是單變量調(diào)節(jié)器,一般用來(lái)研究單輸入單輸出系統(tǒng)。一個(gè)PID調(diào)節(jié)器只能控制一個(gè)輸出。而在整個(gè)倒立擺系統(tǒng)中,系統(tǒng)的輸出為小車(chē)位移、擺體1偏角和擺體2偏角,所以需要用3個(gè)PID調(diào)節(jié)器構(gòu)成并聯(lián)形式分別對(duì)倒立擺系統(tǒng)的小車(chē)位移、一二級(jí)擺體偏角進(jìn)行控制。由于倒立擺系統(tǒng)對(duì)響應(yīng)速度有一定要求,而積分調(diào)節(jié)器會(huì)導(dǎo)致系統(tǒng)穩(wěn)定性降低,動(dòng)態(tài)響應(yīng)變慢。因此,將整個(gè)系統(tǒng)加入3個(gè)PD調(diào)節(jié)器構(gòu)成閉環(huán)反饋控制,PID控制仿真結(jié)構(gòu)如圖4所示。

圖4 倒立擺PID控制Fig.4 Inverted pendulum PID control

由PID控制結(jié)構(gòu)可知,待整定參數(shù)為K1～K6。這6個(gè)參數(shù)與倒立擺狀態(tài)變量一一對(duì)應(yīng),分別為小車(chē)位移、小車(chē)速度、擺桿1偏角、擺桿1角速度、擺桿2偏角、擺桿2角速度。仔細(xì)發(fā)現(xiàn)PD控制倒立擺結(jié)構(gòu)與狀態(tài)反饋控制結(jié)構(gòu)類(lèi)似,又因倒立擺系統(tǒng)是完全能控能觀的,根據(jù)文獻(xiàn)[13]可以通過(guò)極點(diǎn)配置確定三路PD控制器的6個(gè)參數(shù),而筆者想到了用主導(dǎo)極點(diǎn)配置進(jìn)行PD控制器的參數(shù)整定。 系統(tǒng)中Subsystem是倒立擺SimMechanics模型封裝的子系統(tǒng),并對(duì)df擾動(dòng)輸入加入了脈沖干擾模塊,并改變脈沖幅值的大小,確定PID控制所能控制最大脈沖干擾幅值。K1～K6參數(shù)根據(jù)主導(dǎo)極點(diǎn)配置去確定,即選取一對(duì)靠近虛軸主導(dǎo)極點(diǎn),其他極點(diǎn)遠(yuǎn)離該主導(dǎo)極點(diǎn)。這里選取極點(diǎn)為P=[-2±2i -20±4i -18±6i],利用MATLAB中place函數(shù)求得K1～K6參數(shù)為K1=82.212，K2=57.232，K3=-180.392，K4=-33.506，K5=-521.915 6，K6=-38.690。

設(shè)置倒立擺初始位置為擺桿1:10°,擺桿2:-10°,其他狀態(tài)變量為0，得圖5所示仿真曲線。

圖5 PID控制仿真曲線Fig.5 PID control simulation curve

由圖5可知,系統(tǒng)調(diào)節(jié)時(shí)間大約在2.3 s,超調(diào)量較大,延遲5 s后加入脈沖干擾,小車(chē)來(lái)回振蕩幾次后又回到穩(wěn)定狀態(tài),因此,系統(tǒng)具有一定抗干擾能力,而PID控制所能控制最大脈沖干擾幅值為0.1 N。且采用主導(dǎo)極點(diǎn)配置整定PID參數(shù)方法要比傳統(tǒng)極點(diǎn)配置整定PID參數(shù)動(dòng)態(tài)性能更優(yōu)。

3.2 模糊控制算法仿真

由于二級(jí)倒立擺系統(tǒng)有6個(gè)狀態(tài)變量,如果每個(gè)變量的論域有7個(gè)模糊子集,那么將有上萬(wàn)條規(guī)則,引起模糊規(guī)則爆炸?，F(xiàn)利用LQR算法來(lái)構(gòu)造融合函數(shù)[14]，可以有效地簡(jiǎn)化模糊控制器設(shè)計(jì),采用融合函數(shù)模糊控制器框圖如圖6所示。

圖6 模糊控制系統(tǒng)框圖Fig.6 Fuzzy control system block diagram

3.2.1 融合函數(shù)設(shè)計(jì)

根據(jù)LQR算法,取Q=diag(500,500,500,0,0,0),R=1,利用K=lqr(A,B,Q,R)函數(shù),求得K=[22.360 7 18.246 2 -50.811 6 -14.482 0 -278.729 8 -20.584 1],進(jìn)而構(gòu)造融合函數(shù)F1(x)為

式(7)中:

通過(guò)F1(x)把6個(gè)狀態(tài)變量降維,融合得到綜合誤差E和綜合誤差變化率Ec，即

可算出:

F1(x)=

3.2.2 模糊控制器設(shè)計(jì)

根據(jù)經(jīng)驗(yàn),確定綜合誤差E的論域?yàn)镋=[-6 6],誤差變化率Ec的論域?yàn)镋c=[-6 6]；輸出控制量U的論域?yàn)閁=[-6 6]；此外,設(shè)置每個(gè)變量論域?yàn)?個(gè)模糊子集,符號(hào)表示為[NB,NM,NS,ZE,PS,PM,PB]。對(duì)控制及輸入變量的屬度函數(shù)選擇均為“三角形”,得到表2所示模糊控制規(guī)則表。

表2 模糊控制規(guī)則表Table 2 Fuzzy control rule table

3.2.3 模糊控制仿真

采用基于LQR融合函數(shù)降低模糊控制維數(shù)的Simulink仿真結(jié)構(gòu)如圖7所示,并根據(jù)大量實(shí)驗(yàn)及試湊,確定量化因子及比例因子系數(shù)為:Ke=25,Kec=18,Ku=30。所得仿真曲線如圖8所示。

圖7 模糊控制結(jié)構(gòu)Fig.7 Fuzzy control structure

圖8 模糊控制仿真曲線Fig.8 Fuzzy control simulation curve

由仿真曲線可知,小車(chē)位移調(diào)節(jié)時(shí)間約2.2 s,擺桿1、擺桿2偏角約1 s,因此模糊控制有很好的快速性,系統(tǒng)略有超調(diào)但振蕩不是很劇烈,且模糊控制能控制最大脈沖干擾幅值是1.1 N,具有很好的抗干擾能力。

4 仿真結(jié)果對(duì)比分析

用兩種控制算法對(duì)倒立擺SimMechanics模型進(jìn)行了仿真,都能進(jìn)行穩(wěn)擺控制,且都具備一定的抗擾動(dòng)能力。為了對(duì)比兩種控制算法的控制性能,得到圖9所示對(duì)比曲線。

圖9 控制算法對(duì)比曲線Fig.9 Control algorithm comparison curve

(1)動(dòng)態(tài)性能分析:無(wú)論是超調(diào)量還是調(diào)節(jié)時(shí)間,模糊控制動(dòng)態(tài)性能都優(yōu)于PID控制,且PID控制要比模糊控制震蕩劇烈。

(2)抗干擾分析:PID所能控制最大脈沖干擾幅值為0.1 N,而模糊控制是1.1 N,幅值大小即擾動(dòng)力大小,因此,模糊控制具備更好的抗干擾能力。

(3)穩(wěn)態(tài)誤差分析:由小車(chē)位移曲線可知,進(jìn)入穩(wěn)態(tài)后,模糊控制可以回到初始位置,而PID偏離初始位置,因此,模糊控制穩(wěn)態(tài)精度更高。

5 結(jié)論

圍繞直線二級(jí)倒立擺系統(tǒng),建立了其數(shù)學(xué)模型及SimMechanics可視化模型,通過(guò)仿真驗(yàn)證了PID和模糊控制算法都能對(duì)倒立擺進(jìn)行穩(wěn)擺控制,由仿真曲線及直觀觀看可視化動(dòng)畫(huà),可知模糊控制無(wú)論是在動(dòng)態(tài)性能、抗干擾能力和穩(wěn)態(tài)精度上都比PID控制性能好,這體現(xiàn)了模糊控制的智能性,并為下一步倒立擺實(shí)物控制打下了理論基礎(chǔ)。