倒立擺模型推導(dǎo)

1、倒立擺系統(tǒng)模型研究控制系統(tǒng)的數(shù)學(xué)模型是描述系統(tǒng)內(nèi)部物理量或變量之間關(guān)系的數(shù)學(xué)表達(dá)式。在靜態(tài)條件下(即變量各階導(dǎo)數(shù)為零)，描述變量之間關(guān)系的代數(shù)方程稱為靜態(tài)數(shù)學(xué)模型；而描述變量各 階導(dǎo)數(shù)之間關(guān)系的微分方程稱為動態(tài)數(shù)學(xué)模型。如果已知輸入量及變量的初始條件，對微分方程求解，則可以得到系統(tǒng)輸出量的表達(dá)式，并由此對系統(tǒng)進(jìn)行性能分析。因此，建立控制系統(tǒng)的數(shù)學(xué)模型是進(jìn)行控制系統(tǒng)分析和設(shè)計(jì)的首要工作。系統(tǒng)建模可以分為兩種方式：實(shí)驗(yàn)建模和機(jī)理建模。實(shí)驗(yàn)建模是通過在研究對象上加入 各種由研究者事先確定的輸入信號，激勵研究對象，并通過傳感器檢測其可觀測的輸出，應(yīng)用系統(tǒng)辯識的手法分析輸入 -輸出關(guān)系，建立適當(dāng)?shù)臄?shù)學(xué)模

2、型逼近實(shí)際系統(tǒng)。機(jī)理建模就是 在了解研究對象的運(yùn)動規(guī)律基礎(chǔ)上，通過物理、化學(xué)的知識和數(shù)學(xué)手段建立起系統(tǒng)的運(yùn)動方程。對于倒立擺系統(tǒng)，由于其本身是自不穩(wěn)定的系統(tǒng)，實(shí)驗(yàn)建模存在一定的困難，故而選用機(jī)理建模的方法。為了在數(shù)學(xué)上推導(dǎo)和分析的方便，可作出如下假設(shè)：1) 擺桿在運(yùn)動中是不變形的剛體；2) 齒型帶與輪之間無相對滑動，齒型帶無拉長現(xiàn)象；3) 各種摩擦系數(shù)固定不變；4) 忽略空氣阻力；在忽略掉這些次要的因素后，倒立擺系統(tǒng)就是一個典型的運(yùn)動剛體系統(tǒng)，可以在慣性坐標(biāo)系內(nèi)應(yīng)用經(jīng)典力學(xué)理論建立系統(tǒng)的動力學(xué)方程。本文采用分析力學(xué)Lagrange方程建立一、二級倒立擺的數(shù)學(xué)模型。Lagrange方程有如下特點(diǎn)

3、：1) 它是以廣義坐標(biāo)表達(dá)任意完整系統(tǒng)的運(yùn)動方程式，方程的數(shù)目和系統(tǒng)的自由度數(shù)是一致的。2) 理想的約束反力不出現(xiàn)在方程組中，因此在建立系統(tǒng)的運(yùn)動方程時，只需分析已知的主動力，而不必分析未知的約束反力。3) Lagrange方程是以能量的觀點(diǎn)建立起來的運(yùn)動方程式，為了列出系統(tǒng)的運(yùn)動方程式，只需從兩個方面進(jìn)行分析，一個是表征系統(tǒng)運(yùn)動的動力學(xué)能量一一系統(tǒng)的動能，另一個是表征主動力作用的動力學(xué)量一一廣義力。因此，用Lagrange建?？梢源蟠蠛喕到y(tǒng)的建模過程。采用拉格朗日的方法建立系統(tǒng)的數(shù)學(xué)模型。Lagrange算子可以描述如下：L(q,d) =T(q,d)-V(q)(1.1)其中:系統(tǒng)的動能:系

4、統(tǒng)的勢能:系統(tǒng)的廣義坐標(biāo)則系統(tǒng)的動力學(xué)方程可用Lagrange算子描述如下:LfD一 一 =U(1.3)Lagrange方程可以簡單的理解為系統(tǒng)的能量的變化隨著系統(tǒng)外加作用力的變化而變化。1.1 一級倒立擺系統(tǒng)1.1.1拉格朗日方法建立一級倒立擺系統(tǒng)的數(shù)學(xué)模型可以將一級倒立擺系統(tǒng)抽象成小車和質(zhì)量均勻的擺桿組成，小車以向左方向運(yùn)動為正, 擺桿角度以自然下垂位置為零點(diǎn)，逆時針為正，如圖 2.1所示。圖2.1 一級倒立擺示意圖各參數(shù)的物理意義及取值如表2.1:表2.1倒立擺物理參數(shù)符號意義及取值物理意義M小車質(zhì)里1.096 kgm擺桿質(zhì)里0.109 kgC0小車摩擦系數(shù)0.1 Nm-1sec-1C1

5、擺桿摩擦系數(shù)0.0022 Nm-1sec-1l擺桿轉(zhuǎn)動軸心到質(zhì)心的長度0.25 mJ擺桿慣量20.0034 kgmu控制力Nx小車位移mx小車速度-1m secQ擺桿角度rad擺桿角速度rad sec-1首先計(jì)算小車的動能（Tm）、擺桿的動能（）和系統(tǒng)的總動能（T）：t 1 m2TM =二 MX2Tm =j§2 +島 1 I221(1.3)1 i'd (x +1 sin e) f + id (l cos8) j j Idt IdtJT =TmTm不妨假定導(dǎo)軌所在的水平面勢能為零，在一級倒立擺的運(yùn)動過程中，小車的勢能始終為零，系統(tǒng)的總勢能為：V = mg(1 cos”小車與導(dǎo)軌

6、之間的摩擦力和擺桿與小車之間的摩擦力，使得系統(tǒng)能量的損失分別為:D = -C0X22D2 =】。按2則系統(tǒng)總共損失的能量為：d = g d2取系統(tǒng)的廣義坐標(biāo)系為： x、8 ,則拉格朗日算子為:L =T -V則系統(tǒng)的拉格朗日方程可以表示為：'d 住1_ ) clcDI -+=udt I 破 J excXd蘭一頭毛=0dt .七七 °(1.4)(1.5)(1.6)(1.7)(1.8)借助Mathemetica軟件，由以上方程組可以得到一級倒立擺系統(tǒng)的動力學(xué)方程，具體的M +mqmlcosHml cosuX c02ml J0-mlsinMYX' ' u 'q

7、 人們 <-mglsin9>(1.9)推導(dǎo)過程可以參看附錄一。1.1.2 一級倒立擺系統(tǒng)在倒立點(diǎn)附近線性化處理現(xiàn)行的許多一級倒立擺穩(wěn)擺控制39需要將倒立擺在倒立點(diǎn)附近做近似線性化處理。首先由式(2.9)可得：口ml cosB(mgl sin。一崩)一(J +ml2)(u c()X +ml sin晶2)X(M +m)(J +ml )-m l cos 6iamlcos nc0X(M m)G ml(ucosu (M m)g sin - ml cos sin " 2)日.99 99(M m)(J ml ) -m l cos (1.11)在倒立點(diǎn)附近，擺桿角度接近為零，角速度也較小

8、，可以認(rèn)為:sinu2r 0 , sin【), cosn )1將式(2.11)代入式(2.10),可得(J+ml )c0* + m l g8-(J+ml )u-c1mla一2(1.12)(M m)J Mmlmlc0X -uml (M m)mgh -(M m)c)2(M m)J Mml(1.13)將 2.12下：其中:X =aX BuY =CX(1.14)Du0012-(J +ml )c002| 2-m l g.、. . _. 2 .、.(M +m)J +Mml(M +m)J +Mml000_°m&(M +m)mgl. . _ _. 2 .(M +m)J +Mml(M +m)J

9、 +Mml-01J +ml2一1C = |!°(M+m)J +Mml2 0ml2A 二2B =0001.2(M +m)J +Mmlc1ml2(M m)J Mml1(M m)G(M m)J Mml寫成矩陣形式，可以得到一級倒立擺在倒立點(diǎn)附近線性化模型的狀態(tài)空間方程，如1.2二級倒立擺系統(tǒng) 1.2.1拉格朗日方法建立二級倒立擺系統(tǒng)的數(shù)學(xué)模型外擺桿組成，2.2所示。各參數(shù)的物理意義及取值如小車以向左方向運(yùn)動為正,將二級倒立擺系統(tǒng)抽象成小車和質(zhì)量均勻的內(nèi)、 擺桿角度以自然下垂位置為零點(diǎn)，逆時針為正，如圖 表2.2所示。表2.2倒立擺物理參數(shù)符號意義及取值M小車質(zhì)量1.32 kgmi內(nèi)桿質(zhì)里0

10、.04 kgm2外桿質(zhì)量0.132 kgm3質(zhì)量塊質(zhì)量0.208 kgco小車摩擦系數(shù)0.1 N/m/secci內(nèi)桿-小車摩擦系數(shù)0 N/m/secC2內(nèi)-外桿摩擦系數(shù)0 N/m/secll內(nèi)桿轉(zhuǎn)動軸心到質(zhì)心的長度0.09 mLi內(nèi)桿長度0.18 mI2外桿轉(zhuǎn)動軸心到質(zhì)心的長度0.27 mJi內(nèi)桿慣量0.000108 kg*m2J?外桿慣量0.0034 kg*m2u控制力Nx小車位移mx小車速度m/seca內(nèi)桿角度radd1內(nèi)桿角速度rad/sec3外桿角度rad外桿角速度rad/sec首先計(jì)算小車的動能（Tm）和內(nèi)、夕卜擺桿的動能（Tmi、E2）以及質(zhì)量塊的動能Tm312Tm =二 MX、2

11、、 cosa) 21 . ,21 d(x lsin : ) d (1Tm1 = J：，一m2 2dtdt1 21Tm2 =二 J2 -m22 2i i'd(x +L1 sina +l2 sin E) j 十7( ' dtdtcos:1Tm3 = m32Md(x + L1 sin 口)f 土 勺(L1dt口 、2、 cosa l2cosP) .dtJ(1.15)則總動能為:T =TmTm1Tm2 Tm3(1.16)不妨假定導(dǎo)軌所在的水平面勢能為零，在二級倒立擺的運(yùn)動過程中，小車的勢能始終為零，可以計(jì)算內(nèi)外桿、質(zhì)量塊勢能分別為：Vm1 5歸丫1Vm2 = m2gY2Vm3 = m3

12、gY3(1.17)則總勢能為:V =Vm1 Vm2 Vm3(1.18)小車-導(dǎo)軌、內(nèi)桿-小車、外桿-內(nèi)桿之間的摩擦力，使得系統(tǒng)能量的損失分別為:Do =1融22* D =1糖212 1D2-力2.2(1.19)故系統(tǒng)總共損失的能量為：D = D1 D2 D3(1.20)取系統(tǒng)的廣義坐標(biāo)系為：x、a、E ,則則拉格朗日算子為:L =T -V系統(tǒng)的拉格朗日方程可以表示為:；:D=u:x；X；:D 八=0dt頃_!&二一dt M '- W初 cd:L :D 八=0借助mathemetica軟件，由以上方程組可以得到二級倒立擺系統(tǒng)的動力學(xué)方程, 推導(dǎo)過程可以參看附錄二。M(q)C(q,

13、q)=F(q)其中:q =(x a P )M(q) =c(q,q)F(q)=(mjM m1 m2 m3m2L m3L) cos：m2l2 cos :(m m2L m3L)cos：.222J1 m1l1 m2L m3Lm2l2Lcos(：-)m2(1.21)具體的(1.22)m2l2 cos :l2L cos(:- -)2J2 mc°01°-(m1l1 mL m3L)sin ： ：-m2l2sinlic1 02m2l2L sin(:"，)q_m2l2Lsin(：" I).- c2C2-(m1-m2L m3L)g sin 上 -m2gl2sin :1.2.2

14、二級倒立擺系統(tǒng)在倒立點(diǎn)附近線性化處理實(shí)現(xiàn)二級倒立擺穩(wěn)擺控制的 lqR4°方法，需要對系統(tǒng)模型做線性化處理, 近似為線性時不變系統(tǒng)。在本文所規(guī)定的符號與方向的情況下，線性化結(jié)果如下： 在倒立點(diǎn)附近存在：在倒立點(diǎn)附近一 0,sin、一、,cos、一 1,0,sin,cos1,cos(： -)一. 1(1.23)將式(2.23)代入式(2.22),二級倒立擺系統(tǒng)動力學(xué)方程可以近似為:M?(q (?(q,qp -(q)(1.24)其中:M?=-m2l2m2l2LJ2m2l2M m1 m2 m3-(mj m2L m3L)-m2l2-(m1l1 m2L m3L)222J1 m1l1 m2L m3

15、Lm2l2L00000C =0c1 +c2_C2G =0 (m1l 1+mL + m LJg0i°-C2C2 j1°0-m2gi2 ,q 新Fu Cq Gq可以發(fā)現(xiàn)式(2.24)是二級倒立擺在倒立點(diǎn)附近線性化處理后的系統(tǒng)方程，若令:x=(x但務(wù)x d由jX=(x d & x tH <)則可以得到二級倒立擺在倒立點(diǎn)附近線性化模型的狀態(tài)空間方程：0 'M?F ,(1.25)(1.26)X +u(1.27)1.3倒立擺微分方程數(shù)值解法對倒立擺系統(tǒng)的仿真分析，實(shí)質(zhì)上是對系統(tǒng)數(shù)學(xué)模型求數(shù)值解的過程。對于這樣的常微分方程數(shù)值解法按照求解步數(shù)可以分為單步法和多步法，

16、單步法的代表是Runge-Kutta法，多步法的代表是Adms法；按照求解步長可以分為固定步和變步長的求解方式；按照求解精度可以將求解方法歸為 2階、3階、4階等。下面不加推導(dǎo)的給出 4階經(jīng)典Runge-Kutta法的 計(jì)算格式和Adms可變步長的4階預(yù)測校正法的計(jì)算流程。已知微分方程初值條件，若x在區(qū)間a, b 取(N+1)個等距節(jié)點(diǎn)，求對應(yīng)的 y的近似值。y = f (x, y) , a 三 x £ b , y(a) =*.(1.28)對于這樣一個常微分方程的數(shù)值解問題，取步長 h=(b-a)/N, 4階經(jīng)典Runge-Kutta法求 解格式如下"：yi + = yh(

17、K2K2KK4)6K1 = f (x,% )kf *+方+?匕 I(1.29)K2 = f "i+?yi+2K2 jK = f (x +h,yi +hy )Adams變步長的4階預(yù)測校正算法的思路是：先用給定的初始步長，采用4階Runge-Kutta法求出最初的三個節(jié)點(diǎn)，接著依據(jù)采用 Adams-Bashforth 4步顯式方法(式3.10)預(yù)測下一個節(jié)點(diǎn)的值，用Adams-Bashforth 3步隱式方法(式3.11)校正下一個節(jié)點(diǎn)的值。采用兩種不同的方式計(jì)算的同一個節(jié)點(diǎn)的值， 兩個計(jì)算結(jié)果之差若在合理的范圍內(nèi)，則認(rèn)為計(jì)算精度滿足要求，無需改變步長；若過大則認(rèn)為計(jì)算精度不夠，需減小

18、步長以提高計(jì)算的準(zhǔn)確性；若過小則認(rèn)為計(jì)算精度超標(biāo)，需增大步長以提高計(jì)算效率。 若步長合適則保存結(jié)果， 并采取當(dāng)前步長繼 續(xù)預(yù)測、校正下一個節(jié)點(diǎn)。否則，改變步長重新采用Runge-Kutta法計(jì)算前面三個節(jié)點(diǎn)，然后對新步長做評價，不斷的重復(fù)這一過程直到找到合適的步長為止。在計(jì)算快要結(jié)束時應(yīng)當(dāng)注意選取合適的步長以包含最后一個節(jié)點(diǎn)。Adams-Bashforth 4 步顯式方法：hyi .1 = yi ' 24 55 f x , yi - 59 f x，yi37 f X| _2 , yi_2 - 9 f xi _3 , yi_3(1.30)Adams-Bashforth 3 步隱式方法：hy

19、i 1 = yi 9f 為.yi 119f x. 乂 -5f 為，y f 為次普24一 一 一 一(1.31)通常高階方法可能擁有更好的計(jì)算精度41,比如二、三、四階方法對應(yīng)的局部截斷誤差是分別是O(h2)、O(h3)、O(h4)。但并不是說高階的方法擁有更好的效果。這是由于插值多項(xiàng)式并不是次數(shù)越高逼近精度越好。另外，高階的方法將花費(fèi)更多的求解次數(shù)42，如表2.3。因此，常微分方程的數(shù)值解通常采用小于5階的求解方法。表2.3求解次數(shù)與截斷誤差每步求解次數(shù)2345< n< 78< n< 910V n取佳可能的截斷快差O(h2)O(h3) 4O(h)O(hn-1)O(hn-2)O(hn-3)在MATLAB當(dāng)中能方便的實(shí)現(xiàn)微分方程的數(shù)值解，常用的求解器及說明如表2.4 :表2.4解常微分方程初值問題 MATLAB的求解器ode232、3 階 Runge-Kutta 法ode454、5 階 Runge-Kutta 法ode113多步Adams法ode23t適度剛性問題梯形法od