基于SimMechanics的單級倒立擺建模和仿真

詹長書，孫世磊，葛 強，丁瑋琛，郭 柯，高 峰

(東北林業(yè)大學 交通學院，哈爾濱 150040)

倒立擺系統(tǒng)是研究控制理論的實驗平臺，由于它的行為與火箭飛行以及兩足機器人行走有很大相似性，因而對其研究具有重大的理論和實踐意義。近30年來一直是控制界關(guān)注的焦點。倒立擺本身是一個多變量、強耦合、非線性和不穩(wěn)定系統(tǒng)。通過倒立擺可以檢驗各種控制策略的可行性。人們已經(jīng)利用經(jīng)典PID控制理論[1-2]、現(xiàn)代控制1理論[3-5]以及各種智能控制方法實現(xiàn)了多種倒立擺系統(tǒng)的穩(wěn)定控制。本文利用Matlab中的SimMechanics工具箱對倒立擺建立了動力學仿真模型，實現(xiàn)了PID控制、極點配置控制和LQR控制。為下一步的實驗奠定了堅實的理論基礎(chǔ)。

1 單級倒立擺的數(shù)學模型

在忽略了空氣阻力及各種摩擦力之后，可將倒立擺系統(tǒng)抽象成小車和勻質(zhì)桿的系統(tǒng)，如圖1所示。

圖1 單級倒立擺模型示意圖

圖1中，M為小車質(zhì)量(本實驗系統(tǒng)5 Kg)；m為擺桿質(zhì)量(本實驗系統(tǒng)0.2 Kg)；b為小車摩擦系數(shù)(本實驗系統(tǒng)0.1N/m/sec)；l為擺桿轉(zhuǎn)動軸心到桿質(zhì)心的長度(0.25 m)；I為擺桿慣量(0.006 kg*m*m)；F為加在小車上的力；x為小車位置；φ為擺桿與垂直向上方向的夾角；θ為擺桿與垂直向下方向的夾角(考慮到擺桿初始位置為豎直向下)。

對小車水平方向所受的合力進行分析，應(yīng)用Newton方法來建立系統(tǒng)的動力學方程過程。

小車水平方向進行受力分析，可以得到以下方程：

擺桿水平方向的受力分析可以得到下面等式：

即

把這個等式代入上式中，就得到系統(tǒng)的第一個運動方程：

(1)

對擺桿垂直方向上的受力進行分析，可以得到系統(tǒng)的第二個運動方程：

力矩平衡方程如下：

注意：此方程中力矩的方向，由于θ=π+φ,cosφ=-cosθ,sinφ=-sinθ，故等式前面有負號。

(2)

(3)

整理后得到傳遞函數(shù)：

其中：

q=[(M+m)(I+ml2)-(ml)2]

根據(jù)倒立擺的傳遞函數(shù)和狀態(tài)空間模型可得系統(tǒng)的零極點為，倒立擺是一不穩(wěn)定系統(tǒng)。系統(tǒng)可控性矩陣秩為4，和狀態(tài)空間矩陣維數(shù)相等，因此倒立擺是能控系統(tǒng)。

2 倒立擺的SimMechanics模型

SimMechanics 為Simulink環(huán)境下的多體動力機械系統(tǒng)及其控制系統(tǒng)的建模工具箱。它包含了實際系統(tǒng)的元件，如：剛體、鉸鏈、約束、坐標系統(tǒng)和傳感器等。使用這些模塊可以方便的建立復雜機械系統(tǒng)的圖示化模型，進行機械系統(tǒng)的單獨分析或與任何Simulink設(shè)計的控制器及其它動態(tài)系統(tǒng)相連進行綜合仿真。

根據(jù)倒立擺機構(gòu)動力學模型，用Simulink和SimMechanics工具箱中提供的模塊，參照圖1所示的運動簡圖，將需要的模塊拖放到窗口中，連接起來，得到仿真模型(如圖2所示)。其中，Ground和Machine Enviroment為機架模塊；Cart、Pendulum分別為小車和倒立擺剛體模塊；Revolute為轉(zhuǎn)動副模塊；Primatic為移動副模塊；Joint Sensor、Joint Sensor1分別為小車和倒立擺傳感器模塊；Joint Actuator為小車驅(qū)動模塊；Scope為顯示模塊。

圖2 倒立擺的SimMechanics模型

3 單級倒立擺的控制器設(shè)計

3.1 PID控制器設(shè)計

系統(tǒng)輸出量為擺桿的位置，假定初始位置為垂直向下，控制擺桿擺起后，利用PID控制穩(wěn)擺，觀察擺桿的輸出響應(yīng)。PID控制器只對擺桿擺角進行了控制，沒有控制小車的位置。系統(tǒng)框圖如圖3所示，其中KD(s)，G(s)分別為PID控制器和系統(tǒng)模型?？刂破髦蠵、I、D的參數(shù)確定為300、0.5、20。

圖3 PID系統(tǒng)框圖

從仿真曲線圖4中可以看出系統(tǒng)超調(diào)量為0.025，穩(wěn)定時間約為0.6 s左右，響應(yīng)滿足指標要求。

圖4 調(diào)節(jié)PID參數(shù)后擺角輸出圖

3.2 極點配置控制

采用極點配置法設(shè)計多輸出的倒立擺系統(tǒng)的控制方案?？梢杂猛耆珷顟B(tài)反饋來解決，控制擺桿和小車的位置。如圖5所示是控制系統(tǒng)的示意圖。

圖5 控制系統(tǒng)框圖

假定所有的狀態(tài)變量都可以測量和反饋，可以驗證倒立擺系統(tǒng)是狀態(tài)完全可控的。那么，利用狀態(tài)反饋的方法，設(shè)計合理的全狀態(tài)反饋增益矩陣，可以把閉環(huán)系統(tǒng)的極點配置到任何期望的位置，得到滿意的相應(yīng)特性。

設(shè)開環(huán)控制系統(tǒng)的離散狀態(tài)方程為：x(k+1)=Gx(k)+Hu(k)，其中，假設(shè)系統(tǒng)是狀態(tài)完全可控的；x(k)為在第k次采樣時刻的狀態(tài)矢量(n維矢量)；u(k)為在第k次采樣時刻的控制信號(標量)；G=n×n矩陣；H=n×1矩陣。

設(shè)極點配置的控制律形式為：

u(k)=-Kpx(k)。

式中：Kp為狀態(tài)反饋增益矩陣(1×n矩陣)，于是該系統(tǒng)就成為一個閉環(huán)控制系統(tǒng)，其閉環(huán)狀態(tài)方程為：x(k+1)=(G-HKp)x(k)。

注意，G-HKp的特征值就是所要求的閉環(huán)極點μ1,μ2,…μn。

利用狀態(tài)反饋u(k)=-Kpx(k)把閉環(huán)極點布置在z=μ1，z=μ2，…，z=μn。即要求特征方程為：

|zI-G+HKp|=(z-μ1)(z-μ2)…(z-μn)

=zn+a1zn-1+a2zn-2+…+an-1z+an=0。

根據(jù)Cayley_hamiton定理，經(jīng)過推導(此略)可以得到狀態(tài)反饋增益矩陣Kp：

Kp=[0 0 … 0 1][H?GH?…?Gn-1H]-1φ(G)。

其中：

φ(G)=Gn+a1Gn-1+a2Gn-2+…+an-1G+anI。

選擇期望的閉環(huán)極點或期望的特征方程是在誤差矢量響應(yīng)的快速性與對擾動和測量噪聲敏感型之間的一個折中方案。也就是說，如果使誤差響應(yīng)的速度提高，那么擾動和測量噪聲的有害影響往往也會增強。在確定給定系統(tǒng)的狀態(tài)反饋增益矩陣Kp時，通常比較按不同的期望閉環(huán)極點或期望特征方程得到的矩陣Kp，并從中選出使整個系統(tǒng)達到特性最好的那個矩陣Kp。在實際設(shè)計時，運用Matlab控制系統(tǒng)工具箱中的“place”函數(shù)直接進行仿真和運算。仿真結(jié)果如圖6所示。從圖6中可見小車和擺角的響應(yīng)速度和精度滿足設(shè)計要求。

圖6 極點配置零輸入響應(yīng)

3.3 LQR控制

設(shè)狀態(tài)反饋調(diào)節(jié)律的形式為：

u(k)=-Krx(k)。

可以求得：

Kr=[Rr+HTP(K+1)H]-1HTP(k+1)G。

其中：P由下列黎卡提方程獲得P(k)=Qr+GTP(k+1)G-GTP(k+1)H[Rr+HTP(k+1)H]-1HTP(k+1)G。

式中：Qr，Rr分別為狀態(tài)向量x(k)、控制向量u(k)控制的性能度量的相對重要性加權(quán)值。

在實際運算中運用Matlab控制系統(tǒng)工具箱中的“l(fā)qr”函數(shù)直接進行運算。

利用lqr函數(shù)，需要提供兩個權(quán)值矩陣：Q、R。通常取R=1，而對于Q只能通過不斷的湊取來得到。在權(quán)衡了響應(yīng)速度和超調(diào)量大小，最終選取了：

Q=[500 0 0 0；0 0 0 0；0 0 50 0；0 0 0 0]

此時的響應(yīng)曲線如圖7所示。

圖7 LQR反饋控制響應(yīng)圖

4 結(jié)束語

單級倒立擺是一個非常理想的控制理論實驗平臺。首先在建立了數(shù)學模型和SimMechanics環(huán)境下單級倒立擺的動力學模型基礎(chǔ)上，實現(xiàn)了起擺控制。然后分別切換到PID，極點配置和LQR三種控制方案，實現(xiàn)了對單級倒立擺的穩(wěn)擺控制，并優(yōu)化了三種方案的控制器參數(shù)。同時極點配置和LQR控制可以對小車和擺桿進行穩(wěn)定控制，PID可以對擺桿進行控制。仿真中響應(yīng)速度和超調(diào)量達到了均衡，取得了良好的效果，這為實驗奠定了理論基礎(chǔ)。

【參 考 文 獻】

[1]劉 璟，梁昔明.LQR控制與PID控制在單級倒立擺中的對比研究[J].控制理論與應(yīng)用，2007，26(1)：13-15.

[2]楊世勇，徐莉蘋，王培進.單級倒立擺的PID控制研究[J].控制工程，2007，14(S1)：23-25.

[3]楊 平，徐春梅，王 歡，等.直線型一級倒立擺狀態(tài)反饋控制設(shè)計及實現(xiàn)[J].上海電力學院學報.2007,23(3)：21-16.

[4]易 杰，俞 斌.倒立擺系統(tǒng)的狀態(tài)空間極點配置控制設(shè)計[J].電子測試，2008(8)：17-22

[5]李俊芳，牛文興，張振東.基于狀態(tài)空間法的倒擺系統(tǒng)穩(wěn)定控制[J].天津理工學院學報，2003(2)：34-36