基于SimMechanics的直線二級倒立擺仿真研究

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

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

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

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

現利用第二代SimMechanics工具箱對直線二級倒立擺進行三維建模,并設計主導極點配置方法整定PID參數和基于LQR融合模糊控制進行仿真,驗證兩種算法的可行性并進行對比分析。

1 二級倒立擺的數學模型

忽略空氣阻力和各種摩擦力之后,可視二級直線倒立擺系統是一個運動剛體系統,其簡化結構如圖1所示,建模過程所用物理參數意義及取值如表1所示。

圖1 二級倒立擺結構Fig.1 Double inverted pendulum structure

表1 直線二級倒立擺參數Table 1 Line double inverted pendulum parameters

利用拉格朗日方程推導系統數學模型,拉格朗日方程為

式(1)中：L為拉格朗日算子；T為系統動能；V為系統勢能；q為系統的廣義坐標。

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

系統的總動能為

系統的總勢能為

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

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

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

取平衡位置時各變量初值為零,在平衡位置進行泰勒級數展開并線性化,將各參數值帶入,得到系統的狀態方程為

2 倒立擺SimMechanics模型

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

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

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

圖2中,Mechanism Configuration是機構配置模塊,用于整個機器的機械和模擬參數；Configuration和World Frame分別是機構求解和世界坐標系；上述3個模塊是構建任何模型的基礎。Rigid Transform是兩個結構體進行連接的模塊；Revolute Jonit是連接兩根擺桿間的轉動模塊；SPS是將單位的仿真輸入轉變為物理輸出,PSS則相反；而長板、小車、擺桿1、擺桿2是各模塊子系統，里面集成了Solid剛體模塊和坐標系連接；Solid表示將幾何圖形、慣性和質量以及圖形組件合并為單個單元實體；Disturbance 是指在所附的框架上應用外部力和扭矩。p、v、q1、w1、q2、w2是輸出,分別代表小車位移、小車速度、擺桿1角度、擺桿1角速度、擺桿2角度、擺桿2角速度。f為輸入,df為加在第2根擺桿的擾動。

3 倒立擺SimMechanics模型仿真

3.1 PID控制算法仿真

由構建的倒立擺SimMechanics模型(圖2)可知,直線二級倒立擺是單輸入多輸出系統,而PID是單變量調節器,一般用來研究單輸入單輸出系統。一個PID調節器只能控制一個輸出。而在整個倒立擺系統中,系統的輸出為小車位移、擺體1偏角和擺體2偏角,所以需要用3個PID調節器構成并聯形式分別對倒立擺系統的小車位移、一二級擺體偏角進行控制。由于倒立擺系統對響應速度有一定要求,而積分調節器會導致系統穩定性降低,動態響應變慢。因此,將整個系統加入3個PD調節器構成閉環反饋控制,PID控制仿真結構如圖4所示。

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

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

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

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

由圖5可知,系統調節時間大約在2.3 s,超調量較大,延遲5 s后加入脈沖干擾,小車來回振蕩幾次后又回到穩定狀態,因此,系統具有一定抗干擾能力,而PID控制所能控制最大脈沖干擾幅值為0.1 N。且采用主導極點配置整定PID參數方法要比傳統極點配置整定PID參數動態性能更優。

3.2 模糊控制算法仿真

由于二級倒立擺系統有6個狀態變量,如果每個變量的論域有7個模糊子集,那么將有上萬條規則,引起模糊規則爆炸?，F利用LQR算法來構造融合函數[14]，可以有效地簡化模糊控制器設計,采用融合函數模糊控制器框圖如圖6所示。

圖6 模糊控制系統框圖Fig.6 Fuzzy control system block diagram

3.2.1 融合函數設計

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

式(7)中:

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

可算出:

F1(x)=

3.2.2 模糊控制器設計

根據經驗,確定綜合誤差E的論域為E=[-6 6],誤差變化率Ec的論域為Ec=[-6 6]；輸出控制量U的論域為U=[-6 6]；此外,設置每個變量論域為7個模糊子集,符號表示為[NB,NM,NS,ZE,PS,PM,PB]。對控制及輸入變量的屬度函數選擇均為“三角形”,得到表2所示模糊控制規則表。

表2 模糊控制規則表Table 2 Fuzzy control rule table

3.2.3 模糊控制仿真

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

圖7 模糊控制結構Fig.7 Fuzzy control structure

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

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

4 仿真結果對比分析

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

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

(1)動態性能分析:無論是超調量還是調節時間,模糊控制動態性能都優于PID控制,且PID控制要比模糊控制震蕩劇烈。

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

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

5 結論

圍繞直線二級倒立擺系統,建立了其數學模型及SimMechanics可視化模型,通過仿真驗證了PID和模糊控制算法都能對倒立擺進行穩擺控制,由仿真曲線及直觀觀看可視化動畫,可知模糊控制無論是在動態性能、抗干擾能力和穩態精度上都比PID控制性能好,這體現了模糊控制的智能性,并為下一步倒立擺實物控制打下了理論基礎。