三級倒立擺的LQG最優控制應用研究

文章編號：1003-6199(2011)04-0009-05

摘 要：針對三級倒立擺的強耦合不穩定性，應用線性二次型高斯最優控制的方法， 設計LQG控制器，對其進行Matlab仿真研究，仿真結果表明，設計的控制器能很好的對倒立擺進行穩定控制，且具有較好的抑制噪聲和抗干擾能力。



關鍵詞：三級倒立擺系統；LQG最優控制；Matlab 仿真

中圖分類號： TP273 文獻標識碼：A



The Applications Research of Thriple Inverted Pendulum Based on LQG Optimal Control



YE Jianbin，GUO Hongwu

（College of Mechatronics and Automation of NUDT，Changsha 410073，China）

Abstract:This paper contrapose the strong coupling and nonstability of thriple inverted pendulum，using the theory of linear quadratic Gauss optimal control for Matlab simulation，design a LQG Controller，and experiment show that the presented method is effective，and the system has good restrain noise and good robustness.



Key words:triple inverted pendulum system; LQG Optimal control;Matlab simulation



1 引 言三級倒立擺自身是一個高階次、不穩定、多變量、非線性、強耦合的系統，近年來一直都是控制領域研究的熱點。目前對三級倒立擺控制研究較多的是線性二次最優控制（LQR）方法，文獻［2］重點研究了三級倒立擺系統LQR方法的加權陣Q的選擇，文獻［3］對三級倒立擺的數學模型設計了模糊控制器和LQR控制器的混合控制器，并進行了仿真，文獻［4］就二級倒立擺的線性化數學模型設計了最優控制器并仿真。

考慮到對三級倒立擺進行控制的時候，總是需要先將擺桿扶到倒立位置，系統才開始控制，此時外界（手）的作用力相當于加在擺桿上的一個噪聲干擾，再加上每根擺桿連接處傳感器及小車位置傳感器的量測誤差，這些對系統的穩定控制都產生了較大的影響。本文以固高三級倒立擺系統為對象，先建立三級倒立擺的動力學非線性數學模型，而后利用線性二次型高斯控制器(LQG）實現了三級倒立擺的動態平衡。仿真結果表明，該控制系統取得了預期的效果。

2 倒立擺數學模型

21 系統組成^［1］

固高倒立擺控制系統包括1－小車、2－擺桿、3－直流力矩電機、4－軸角編碼器、5－驅動和接口裝置、6－控制計算機等部分，如圖1所示。小車在水平軌道上，由電機驅動通過滑輪牽引皮帶來控制其作直線運動，小車的位置可由鋼絲帶動的光電碼盤的測量信號轉換得到。擺桿的傾斜角度則可由在支點處同軸安裝的光電碼盤測出。

22 三級倒立擺非線性模型

為使系統的模型不致過于復雜，針對問題本質，首先作以下假設：擺桿及小車都是剛體，且擺桿為勻質剛體；小車的牽引機構是理想的。直線三級倒立擺模型結構如圖2所示。在實驗過程中，由于各擺桿間的質量塊（即測角度傳感器）相對擺桿質量來說，對系統的影響非常大，以及系統的摩擦對系統也有較大影響，故在建模中重點考慮了各擺桿間的質量塊和系統的摩擦，使模型更加接近實際系統。

倒立擺參數定義如下:

m0小車質量 

m1擺桿1的質量

m2 擺桿2的質量

m3 擺桿3的質量

m4質量塊1的質量

m5 質量塊2的質量

f0 小車與導軌間的摩擦

f1 擺桿1與轉軸間的摩擦

f2 擺桿2與轉軸間的摩擦

f3 擺桿3與轉軸間的摩擦

l1擺桿1中心到轉動中心的距離

l2擺桿2中心到轉動中心的距離

l3擺桿3中心到轉動中心的距離

Ji 擺桿i的轉動慣量

θi擺桿i與豎直方向的夾角 

F作用在系統上的外力

利用拉格朗口方程推導運動學方程，倒立擺系統的拉格朗日方程為：

L(q，)=T(q，)－V(q，) (1)

其中，L為拉格朗日算子，q為系統的廣義坐標，對于直線三級倒立擺為(x，θ1，θ2，θ3)。T為系統的動能，V為系統的勢能，其中零勢能面取擺桿支點所在水平面。拉格朗日方程由廣義坐標q i和L表示為

ddt(Li)－Lqi+Di=Fqi (2)

其中，i=1，2，3…n，，n為倒立擺系統的自由度，D為系統總耗散能，Fqi為系統沿該廣義坐標方向上的外力，對直線三級倒立擺，有Fx=u－f0，Fθ1=0，Fθ2=0，Fθ3=0.

先計算系統的動能：

T=12m02+12m1d(x-l1sinθ1)dt2+

d(l1cosθ1)dt2+16m1l2121+

12m2d(x-2l1sinθ1-l2sinθ2)dt2+

d(2l1cosθ1+l2cosθ2)dt2+

12m3d(x-2l1sinθ1-2l2sinθ2-l3sinθ3)dt2+

d(2l1cosθ1+2l2cosθ2+l3cosθ3)dt2+

16m2l2222+16m3l2323+12m4

d(x-2l1sinθ1)dt2+d(2l1cosθ1)dt2+

12m5d(x-2l1sinθ1-2l2sinθ2)dt2+

d(2l1cosθ1+2l2cosθ2)dt2(3)

系統的勢能為：

V=m1gl1cos θ1+m2g(2l1cos θ1+

l2cos θ2)+m3g(2l1cos θ1+2l2cos θ2+

l3cos θ3)+m4g2l1cos θ1+

m5g(2l1cos θ1+2l2cos θ2) (4)

系統的損耗能為：

D=12f02+12f121+

12f2(2-1)2+12f3(3-2)2(5)

將L=T-V和D的表達示代入方程(2)，得到系統的非線性微分方程如下

a0-(a1+b1) 1cosθ1-(a2+b)2cosθ2-

b33cosθ3+(a1+b1)21sinθ1+(a2+

b2)22sinθ2+b323sinθ3+2f0=F(6)

-(a1+b1) cosθ1-(2a1l1+c1)1+

2a3l1l22cos(θ1-θ2)+2b3l13cos(θ1-

θ3)- 2a3l1l222sin(θ1-θ2)+

2b3l123sin(θ1-θ3)-(a1+b1)gsinθ1+

(f1+f2)1-f22=0(7)

-(a2+b2) cosθ2+2a3l1l22cos(θ1-θ2)+

(2a2l2+c2)2+2b3l23cos(θ2-θ3)- 

2a3l1l221sin(θ1-θ2)+2b32223sin(θ2-θ3)-

(a1+b2)gsinθ2-f21+(f2-f3)2-

f22+f33=0 (8)

-b3cosθ3+2b3l11cos(θ2-θ3)+2b3l22cos(θ2-

θ3)+c33-2b3l121sin(θ1-θ3)-2b3l222sin(θ1-

θ3)-b3gsinθ3+f3(3-2)=0(9)

其中a0=∑5i=0mi，a1=2l1∑5i=2mi，

a2=2l2(m3+5)，a3=m2+2m3+2m5，

bi=mili，ci=mil2i+Ji，(i=1，2，3)

23 系統在平衡點處線性化后的狀態方程

選擇系統的狀態變量為X={x，θ1，θ2，θ3，，1，2，3}，代入系統參數，并在系統平衡點（X=0）附近進行線性化，得到系統的狀態方程：

(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t) (10)

其中：

A=0 0 0 0 1.0000 0 0 0

0 0 0 0 0 1.0000 0 0

0 0 0 0 0 0 1.0000 0

0 0 0 0 0 0 0 1.0000

0 0.08256 -19.25887 19.46906 -0.00332 -0.07992 0.30309 -0.22344

0 5.92430 0.02357 0.00528 -0.17782 -0.03737 0.01803 0.000036

0 183.89786 -83.36183 0.025198 -1.13535 -1.50669 1.18700 -0.27944

0 -112.0799 120.53752 -7.90254 -0.00607 1.20576 -1.30887 0.46827 

B=［0 0 0 0 0.01658 0.88911 5.67676 0.03037］′

C=1 0 0 0 0 0 0 0;

0 1 0 0 0 0 0 0; 0 0 1 0 0 0 0 0; 0 0 0 1 0 0 0 0;

D=［0; 0; 0; 0］;

3 線性二次型高斯最優控制

考慮系統隨機輸入噪聲與隨機噪聲的線性二次型的最優控制叫線性二次型高斯最優控制（LQG），用卡爾曼濾波器觀測系統狀態。這是一種輸出反饋控制，對解決線性二次型最優高斯控制問題更具有實用性。

31 LQG最優控制原理

對于上述三級倒立擺系統，考慮干擾噪聲和量測噪聲的狀態方程為

(t)=Ax(t)+Bu(t)+Γω(t)y(t)=Cx(t)+Du(t)+v(t) (11)

式中， ω(t)為系統干擾噪聲；v(t)為傳感器帶來的量測噪聲。

根據LQG問題的分離原理，典型的線性二次型高斯最優控制的解可以分解為兩個問題，

即LQ最優狀態反饋控制問題和帶有擾動的狀態估計問題。

32 線性二次型最優控制器的設計

對狀態方程(11)，使控制性能指標：J=12∫

SymboleB@ 0(xT(t)Qx(t)+uT(t)Ru(t))dt達到最小，即要確定最佳控制量u(t)=－KX=－R－1BTPx的反饋矩陣K。其中，Q，R為正定（或半正定）的厄米特或實對稱矩陣；K為最優反饋增益矩陣；P為常值正定矩陣，必須滿足黎卡提方程PA+ATP－PBR－1BP+Q=0的解^［5］。Q，R分別是系統狀態變量和控制量的加權矩陣，權矩陣Q對角線上各權系數代表各項指標誤差的相對重要性；R代表了能量損耗的相對重要性，其作用在于限制控制器的輸出不至太大而導致難于控制。

根據式(11)，為滿足系統穩定性控制的要求，權矩陣Q中與各主要被控量相對應的權系數一般取值較大，并且當Q矩陣的值較大時，系統能更快地達到穩定狀態^［2］。通過大量的仿真實驗，發現Q矩陣的值在一定范圍內越大系統的調節時間就越短，但當其過大時會造成比較大的抖動；考慮到小車的速度和三根擺桿的角度對控制系統的影響較大，這里取Q=diag(5000，3000，2000，4000，0，0，0，0) ，R=1求出最優狀態反饋矩陣：

K=［ 70.7 339 -593.5 1719.7 -91.5 23.2 -24.1 276.1］

33 Kalman濾波器的設計

Kalman濾波器就是最優觀測器，能夠抑制或濾掉噪聲對系統的干擾和影響。利用Kalman濾波器對系統進行最優控制是非常有效的^［6］。三級倒立擺系統在考慮干擾噪聲ω(t)和量測噪聲v(t)時，采用LQG控制器能更好的對系統實施控制。

根據Kalman濾波理論，Kalman濾波器的增益矩陣L=PfCTΘ－1，其中Pf滿足黎卡提方程PfAT+APf－PfCTΘ－1CPf+ΓΞΓT=0，且Pf=PTf≥0，Ξ=E［ω(t)ωT(t)］≥0，Θ=E［v(t)vT(t)］≥0。

對于LQG控制器，干擾噪聲可視為從輸入端加入，當對三級倒立擺進行控制的時候，要將倒立擺扶到倒立位置，此時相對輸出量測噪聲來說它對系統的影響較大，隨著系統的逐漸穩定，干擾噪聲相對輸出量測噪聲對系統的影響也逐漸減小。假定這些噪聲信號為零均值的Gauss過程，并假設ω(t)和v(t)為相互獨立的隨機變量^［7］；因此根據輸入、輸出信號的幅值范圍，可選定干擾噪聲ω(t)的協方差Ξ=2，量測噪聲v(t)的協方差Θ=［1 0 0 0;0 1 0 0; 0 0 1 0;0 0 0 1］，Γ為干擾噪聲項ω(t)的系數，在式（11）中值為1。由此可求得卡爾曼濾波器的估計增益:

L=

1.6294 0.1087 0.0866 0.0510 0.1087 21.4920 -9.8163 -0.9944 0.0866 -9.8163 15.5468 -2.5152 0.0510 -0.9944 -2.5152 12.2630 1.3384 0.3506 0.2625 0.1298 1.2612 279.6333 -150.2164 -8.1744 0.0294 -210.8583 172.1991 -26.8183 0.2535 -0.6967 -33.3637 78.8500

4 Matlab仿真

下面利用Matlab進行仿真驗證LQG控制器的設計效果，仿真中采用均值為0，方差為1的高斯噪聲，給定的系統初始條件為［0.1;0.05;0.05;0.05;0;0;0;0］，采樣時間取0.005s，仿真時間設定為15s，得到如圖3所示的仿真曲線。

(a)小車位置響應

(b)擺桿1角度變化曲線

(c)擺桿2角度變化曲線

(d)擺桿3角度變化曲線

5 結 論

本文首先用Lagrange方程對三級倒立擺系統進行了建模，在建模中重點考慮了各擺桿間的質量塊和系統的摩擦，使模型更加接近實際系統。考慮到系統干擾噪聲和量測噪聲的存在，采用基于LQG的最優控制，進行LQG控制器的設計和仿真研究，從仿真結果可以看出，系統的超調量比較小，在有噪聲輸入干擾的情況下系統也能較快的趨于平穩，且系統具有一定的魯棒穩定性，證明了建模、控制器的設計和仿真過程的正確性。

參考文獻

［1］ 倒立擺與自動控制原理實驗［S］. 固高科技（深圳）有限公司，2005.

［2］ 李宇成，朱興，鄭興凱，等. 基于LQR算法的三級倒立擺控制系統的仿真研究［J］.北方工業大學學報，2006，45(29):245-248.

［3］ Beichen Ji，Shuang Cong. Modeling and simulation for a triple invertedpendulum［C］. Proceedings of the 5th World Congress on Intelligent Control and Automation. 2004.

［4］ 高原平. 基于MATLAB的二級倒立擺線性最優控制器的設計和仿真［J］.建筑與工程，2011，14-0119-02:119-120.

［5］ 謝學書. 最優控制［M］.北京: 清華大學出版社，1986.

［6］ 林紅，馮志華. 基于LQG的彈性倒立擺動態穩定性控制［J］.系統工程與電子技術，2003，25(12):1511-1512.

［7］ 張靜. MATLAB在控制系統中的應用［M］.北京：電子工業出版社，2007.

收稿日期：2011-09-29

作者簡介：葉建斌(1985—)，男，福建武夷山人，碩士研究生，研究方向：精確制導與控制(E-mail：41789730@163.com);郭鴻武(1973—)，男，廣西合浦人，副教授，研究方向：精確制導與控制。