0 引言

　　倒立擺是一種研究控制理論的理想平臺(tái)，其結(jié)構(gòu)簡(jiǎn)單、成本低，是一個(gè)典型的多變量、快速、非線性和自然不穩(wěn)定系統(tǒng)。旋轉(zhuǎn)倒立擺在外力的作用下，從一個(gè)穩(wěn)定的平衡狀態(tài)自動(dòng)轉(zhuǎn)移到另一個(gè)平衡狀態(tài)的過程中，既要求起擺迅速，又要求在到達(dá)新的平衡點(diǎn)時(shí)能快速進(jìn)行穩(wěn)擺控制[1-4]。

　　圖1為旋轉(zhuǎn)倒立擺結(jié)構(gòu)圖。旋轉(zhuǎn)倒立擺的原理為：當(dāng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂在水平平面內(nèi)來(lái)回轉(zhuǎn)動(dòng)時(shí)，擺桿由于慣性將會(huì)繞轉(zhuǎn)軸2在豎直平面內(nèi)來(lái)回?cái)[動(dòng)，當(dāng)能量達(dá)到一定數(shù)值時(shí)，擺動(dòng)幅度將超過180°形成倒立狀態(tài)。通過單片機(jī)系統(tǒng)控制擺桿迅速倒立是本文研究的主要內(nèi)容。首先建立數(shù)學(xué)模型，然后設(shè)計(jì)單片機(jī)軟、硬件系統(tǒng)，最后進(jìn)行調(diào)試分析，形成旋轉(zhuǎn)倒立擺單片機(jī)控制系統(tǒng)。

1 系統(tǒng)數(shù)學(xué)模型創(chuàng)建

　　建立數(shù)學(xué)模型前，設(shè)空氣阻力忽略不計(jì)。系統(tǒng)由電機(jī)和質(zhì)量均勻的擺桿m組成，擺桿質(zhì)心到轉(zhuǎn)軸2的距離為L(zhǎng)，轉(zhuǎn)軸2軸心到轉(zhuǎn)軸1軸心距離為r，某時(shí)刻旋轉(zhuǎn)臂在水平面上轉(zhuǎn)動(dòng)角度為?茲，擺桿在豎直平面內(nèi)轉(zhuǎn)動(dòng)角度為?琢，設(shè)擺桿和旋轉(zhuǎn)臂的角度以逆時(shí)針方向?yàn)檎齕5]。其受力分析如圖2所示。

　　旋轉(zhuǎn)臂和擺桿一起動(dòng)作，擺桿質(zhì)心x、y方向的速度分量分別如下：

　　系統(tǒng)總動(dòng)能T由4個(gè)部分組成：旋臂水平面內(nèi)的轉(zhuǎn)動(dòng)動(dòng)能、擺桿豎直平面內(nèi)的轉(zhuǎn)動(dòng)動(dòng)能、擺桿質(zhì)心沿x軸方向和y軸方向的動(dòng)能。由此可得到Lagrange函數(shù)：

　　Lagrange方程：。其中，q是系統(tǒng)的廣義坐標(biāo)，T為系統(tǒng)總動(dòng)能，V為系統(tǒng)的勢(shì)能，L為拉格朗日算子。Lagrange方程表示為：

　　在倒立擺系統(tǒng)中i=1，2，q=(?茲，?琢)，?茲為旋臂角位移，?琢為擺桿角位移，Qi為系統(tǒng)沿該廣義坐標(biāo)方向上的外力，得到以下方程組：

       令，得出系統(tǒng)線性化狀態(tài)方程：

　　根據(jù)表1相關(guān)參數(shù)，當(dāng)旋轉(zhuǎn)倒立擺系統(tǒng)靠近平衡點(diǎn)時(shí)，得到如下線性化數(shù)學(xué)模型：

2 控制系統(tǒng)硬件電路設(shè)計(jì)

　　2.1 控制系統(tǒng)總體框圖

　　控制系統(tǒng)主要由單片機(jī)系統(tǒng)、驅(qū)動(dòng)模塊、直流減速電機(jī)、角度傳感器、LCD液晶顯示模塊等組成。單片機(jī)通過實(shí)時(shí)采集角度傳感器數(shù)據(jù)，經(jīng)分析得出擺桿當(dāng)前狀態(tài)，通過計(jì)算得出相應(yīng)控制量來(lái)控制電機(jī)轉(zhuǎn)動(dòng)，從而改變擺桿狀態(tài)，使其達(dá)到預(yù)期控制的目的。系統(tǒng)總體框圖如圖3所示。

　　2.2 控制系統(tǒng)電路原理圖

　　圖4為控制系統(tǒng)傳感器及驅(qū)動(dòng)原理圖。為保證控制精度和速度，采用增強(qiáng)型51單片機(jī)STC12C5A60S2[6]作為控制核心；電機(jī)驅(qū)動(dòng)模塊采用L298N大電流電機(jī)驅(qū)動(dòng)芯片，由單片機(jī)P2口部分口線控制；角度傳感器采用MPU6050 六軸運(yùn)動(dòng)處理組件，能檢測(cè)擺桿在垂直方向上的角度值，由P1口部分口線控制，中斷引腳接至單片機(jī)外部中斷0引腳。此外電路還包括LCD12864顯示電路及按鍵電路，用來(lái)實(shí)現(xiàn)人機(jī)交互及當(dāng)前狀態(tài)顯示。

3 控制系統(tǒng)軟件設(shè)計(jì)

　　3.1 資源分配及程序流程

　　系統(tǒng)主要控制任務(wù)包括：角度數(shù)據(jù)采集與處理、電機(jī)驅(qū)動(dòng)控制、按鍵輸入讀取、數(shù)據(jù)顯示等。其中數(shù)據(jù)顯示任務(wù)為非緊急事件，由主程序循環(huán)調(diào)用即可完成；角度數(shù)據(jù)采集與處理、電機(jī)驅(qū)動(dòng)控制任務(wù)具有連貫性及較高的實(shí)時(shí)性，屬于周期性的緊急事件，應(yīng)由定時(shí)器T0中斷服務(wù)子程序一并完成；按鍵輸入讀取任務(wù)屬于隨機(jī)性的緊急事件，應(yīng)由外部中斷0子程序來(lái)完成。

　　定時(shí)器T0中斷服務(wù)子程序流程圖如圖5所示，主要執(zhí)行讀取傳感器數(shù)據(jù)、卡爾曼濾波、PID調(diào)節(jié)任務(wù)，為避免中斷程序溢出，此三項(xiàng)任務(wù)的執(zhí)行時(shí)間應(yīng)小于5 ms，因此單片機(jī)外部晶振頻率選為24 MHz方能滿足速度需要。

　　3.2 控制調(diào)節(jié)算法

　　PID調(diào)節(jié)是按偏差的比例、積分、微分進(jìn)行控制的一種閉環(huán)調(diào)節(jié)控制，原理框圖如圖6所示，其中r(t)為系統(tǒng)給定值，c(t)為實(shí)際輸出，u(t)為輸出控制量。引入PID調(diào)節(jié)算法可保證系統(tǒng)處于閉環(huán)控制狀態(tài)，并且能讓系統(tǒng)快速準(zhǔn)確達(dá)到設(shè)置的平衡狀態(tài)[7-11]。

　　系統(tǒng)采用計(jì)算量較小的增量式PID算法，由模擬PID離散化得到在k-1時(shí)刻的輸出：

　　其中，T為采樣時(shí)間。單片機(jī)控制系統(tǒng)將采用恒定的采樣周期T，一旦確定A、B、C，只要使用前后3次測(cè)量的偏差值，就可以求出控制量。

　　3.3 系統(tǒng)抗干擾設(shè)計(jì)

　　由于角度傳感器采集到的數(shù)據(jù)含有噪聲信號(hào)，主要來(lái)自外界電磁干擾及角度傳感器本身的噪聲。在軟件系統(tǒng)中采用卡爾曼濾波算法[12-15]將噪聲濾去。設(shè)采樣率為200 Hz，對(duì)擺桿擺起發(fā)整個(gè)過程進(jìn)行角度數(shù)據(jù)采集，經(jīng)卡爾曼濾波前后圖像對(duì)比如圖7所示,其展示了擺桿從自然下垂?fàn)顟B(tài)開始實(shí)現(xiàn)倒立的整過過程。擺桿靜止時(shí)刻自然下垂為平衡狀態(tài)Ⅰ，與垂直方向夾角為0°(圖7(b)中2段所示)。擺桿在電機(jī)帶動(dòng)下獲取一定能量來(lái)回振蕩(圖7(b)中3所示），能量達(dá)到一定程度擺桿越過180°實(shí)現(xiàn)倒立，經(jīng)過短時(shí)間幾次小的振蕩后(圖7(b)中4所示)，到達(dá)新的平衡狀態(tài)Ⅱ(圖7(b)中5所示)，實(shí)現(xiàn)了擺桿倒立。停止電機(jī)控制，擺桿將回到原平衡狀態(tài)Ⅰ，并在0°附近自由振蕩幾次后達(dá)到靜止(圖7(b)中6所示)。由于卡爾曼濾波在初始時(shí)刻濾波誤差較大，屬于盲區(qū)(圖7(b)中1所示)，軟件編程時(shí)應(yīng)延時(shí)幾秒以避開盲區(qū)。

　　3.4 主要程序源碼

　　定時(shí)器T0中斷子程序代碼如下所示[16]：

　　void Timer0 interrupt 1  using 1

　　{

　　TH0=(65536-2500)/256;

　　TL0=(65536-2500)%256;//重裝初值

　　ReadMpu6050();//讀傳感器數(shù)據(jù)

　　KalmanFilter();//卡爾曼濾波

　　if(DelayCounter==2000) //計(jì)數(shù)判斷

　　{Motor0=PIDControl();//PID調(diào)節(jié)控制

　　DispBuffUpdata();} //更新顯示緩沖區(qū)

　　else DelayCounter++;

　　}

4 測(cè)試結(jié)果與結(jié)論

　　表2為擺起模式運(yùn)行結(jié)果測(cè)量表。通過按鍵設(shè)置擺起模式，分別選擇擺起角度為45°～180°之間的數(shù)據(jù)，擺桿會(huì)在電機(jī)帶動(dòng)下做往復(fù)擺動(dòng)，擺角很快達(dá)到超過-60°～+60°。當(dāng)擺桿角度接近180°時(shí)，通過PID調(diào)節(jié)，迅速使擺桿保持穩(wěn)定倒立狀態(tài)，時(shí)間維持5 s以上。通過一系列實(shí)驗(yàn)驗(yàn)證，該旋轉(zhuǎn)倒立擺系統(tǒng)能完成任意角度的擺起，并能實(shí)現(xiàn)快速倒立，系統(tǒng)能很好地滿足設(shè)計(jì)要求。

　　參考文獻(xiàn)

　　[1] 王家軍，劉棟良，王寶軍.X-Z倒立擺的一種飽和非線性穩(wěn)定控制方法的研究[J].自動(dòng)化學(xué)報(bào)，2013，39(1)：92-96.

　　[2] 趙建軍，魏毅，夏時(shí)洪，等.基于二階倒立擺的人體運(yùn)動(dòng)合成[J].計(jì)算機(jī)學(xué)報(bào)，2014，37(10)：2188-2195.

　　[3] 周昆，孫明瑋，陳增強(qiáng).快速顯式預(yù)測(cè)控制在倒立擺系統(tǒng)中的應(yīng)用[J].儀器儀表學(xué)報(bào)，2014，35(9)：2037-2044.

　　[4] 李雪冰，馬莉，丁世宏.一類新的二階滑?？刂品椒捌湓诘沽[控制中的應(yīng)用[J].自動(dòng)化學(xué)報(bào)，2015，41(1)：194-202.

　　[5] 吳愛國(guó)，張小明，張釗.基于Lagrange方程建模的單級(jí)旋轉(zhuǎn)倒立擺控制[J].中國(guó)工程科學(xué)，2005，7(10)：11-15.

　　[6] 南通國(guó)芯.STC12C5A60S2+系列單片機(jī)手冊(cè)[DB/OL].http：//www.stcmcu.com.

　　[7] 曹敏，徐凌樺.單神經(jīng)元PID算法在倒立擺控制系統(tǒng)中的應(yīng)用[J].微計(jì)算機(jī)信息，2009，25(1-1)：70-71.

　　[8] 楊智，朱海鋒，黃以華.PID控制器設(shè)計(jì)與參數(shù)整定方法綜述[J].化工自動(dòng)化及儀表，2005，32(5)：1-7.

　　[9] 王述彥，師宇，馮忠緒.基于模糊PID控制器的控制方法研究[J].機(jī)械科學(xué)與技術(shù)，2011，30(1)：166-172.

　　[10] 陳新海，李言俊.自適應(yīng)控制及應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，1998.

　　[11] 王立新.模糊系統(tǒng)與模糊控制教程[M].北京：清華大學(xué)出版社，2003.

　　[12] 付夢(mèng)印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用(第二版)[M].北京：科學(xué)出版社，2010.

　　[13] DAUM F.Nonlinear filters beyond the Kalman filter[J].IEEE A&E Systems Magazine，2005，20(8)：57-69.

　　[14] KIM J，VADDI S S，MENON P K，et al.Comparison between nonlinear filtering techniques forspiraling ballistic missile state estimation[J].IEEE Transactions on Aerospace and Elec-tronic Systems，2012，48(1)：313-328.

　　[15] KALMAN R E.A new approach to linear filtering and prediction problems[J].Transactions of the AMSE Journal of Basic Engineering，1960，82：35-45.

　　[16] 譚浩強(qiáng).C語(yǔ)言程序設(shè)計(jì)(第三版)[M].北京：清華大學(xué)出版社，2005.