簡易旋轉倒立擺及控制裝置剖析

1、簡易旋轉倒立擺及控制裝置(參賽隊員姓名：指導教師姓名參賽隊編號：參賽學校：簡易旋轉倒立擺及控制裝置（C題）摘要：簡易旋轉倒立擺及控制裝置是復雜的高階閉環(huán)控制系統(tǒng)，控制復雜度較高。系統(tǒng)以飛思卡爾MK10DN512ZVLL10單片機為核心，以Mini1024j編碼器為角度傳感器，配合直流電機組成旋轉倒立擺系統(tǒng)，經過充分的系統(tǒng)建模，并考慮單片機運算速度，最終確定采用改進的“模糊PID”控制算法，通過軟件控制，可以滿足基本部分要求和發(fā)揮部分要求。系統(tǒng)的突出特點在于充分的力學理論分析，通過力學建模和控制系統(tǒng)仿真，獲得了大量的定性分析結果，為系統(tǒng)的建立提供了很好的理論依據。關鍵字：倒立擺模糊PID力學建模

2、狀態(tài)機6一、系統(tǒng)方案1 .系統(tǒng)方案論證與選擇倒立擺系統(tǒng)是一個復雜的快速、非線性、多變量、強耦合、自然不穩(wěn)定的系統(tǒng)。對于該控制系統(tǒng)而言，合適的控制算法、精確的反饋信號、適合的電機驅動等都對系統(tǒng)的穩(wěn)定性、控制精度及抗干擾性起重要作用。針對上述問題，分別設計多種不同的解決方案，并進行選擇論證。（ 1）控制算法選擇方案一：采用傳統(tǒng)PID控制算法。傳統(tǒng)PID控制算法是運用反饋求和后的誤差信號的比例（0階位置項）、積分（誤差累積項）、微分（1階速度項）進行系統(tǒng)校正的一種控制算法?？捎糜诒豢貙ο蟮慕Y構和參數(shù)不能完全掌握，或得不到的精確數(shù)學模型的情況，控制器的結構和參數(shù)必須依靠經驗和反復調試來確定。方案二：采

3、用模糊PID控制算法模糊PID控制算法根據PID控制器的三個參數(shù)與偏差e和偏差的變化ec之間的模糊關系，在運行時不斷檢測e及ec，通過事先確定的關系，利用模糊推理的方法，在線修改PID控制器的三個參數(shù)，讓PID參數(shù)可自整定。將模糊控制算法與傳統(tǒng)PID控制算法巧妙結合，不但具有PID控制算法精度高等優(yōu)點，又兼有模糊控制靈活、適應性強的優(yōu)點。綜合考慮選擇方案二的模糊PID控制算法。（ 2）電動機選型方案一：選擇步進電動機步進電動機是將電脈沖激勵信號轉換成相應的角位移或線位移的離散值控制電動機，這種電動機每當輸入一個電脈沖就動一步。雖然控制時序和驅動電路相對復雜，但步進距離很小，保持力矩大，制動能力

4、強。但步進電機速度只在一定范圍可調，并且一般步進電機在不旋轉時仍有若干相通電，功耗太大。方案二：選擇直流電動機直流電動機控制簡單，利用雙極性PWM即可實現(xiàn)調速和正、反轉，功率調節(jié)范圍廣、適應性好。直流電機的起動、制動轉矩大，易于快速起動、停車，易于控制，且直流電機的調速性能好，調速范圍廣，易于平滑調節(jié)。綜上考慮選擇方案二的直流電動機。（ 3）傳感器的選擇方案一：使用角位移傳感器角位移傳感器是一個高精度的電位器，它輸出為模擬量。但是在使用角位移傳感器時，為得到其與豎直方向（即重力方向）的夾角，要使用重擺，且在角度變化小時，由于傳感器自身扭矩，將不會發(fā)生角位移，從而得不到采樣數(shù)據。方案二：使用主軸

5、編碼器主軸編碼器采用與主軸同步的光電脈沖發(fā)生器，通過中間軸上的齒輪1:1地同步傳動。一般是發(fā)光二極管發(fā)出紅外光束，通過動、靜兩片光柵后，到達光電二極管，接收到脈沖信號，變換成數(shù)字量輸出。按編碼方式不同，分為增量式編碼器和絕對編碼器。前者輸出脈沖，后者輸出8421碼。絕對值編碼器減輕了電子接收設備的計算任務，從而省去了復雜的和昂貴的輸入裝置，而且，當機器合上電源或電源故障后再接通電源，不需要回到位置參考點，就可利用當前的位置值，得到了廣泛的應用。綜上考慮選擇方案二的絕對值式主軸編碼器。2 .系統(tǒng)結構基于題目要求及以上分析，本系統(tǒng)以飛思卡爾MK10DN512ZVLL10單片機作為核心處理芯片，包括

6、擺桿狀態(tài)檢測、電機驅動、液晶顯示等模塊。系統(tǒng)框圖如圖1所示。圖1系統(tǒng)結構框圖系統(tǒng)各部分功能如下。中央處理模塊：該模塊采用飛思卡爾MK10DN512ZVLL10單片機作為主控制器，完成系統(tǒng)的控制功能。擺桿檢測模塊：該模塊由主軸編碼器構成，用于檢測擺桿的角位移及角速度，并將信息反饋給MCU。電機驅動模塊：該模塊由步進電動機實現(xiàn)，用于驅動旋轉臂做往復運動。液晶顯示模塊：該模塊由1.8寸SPITFT全彩屏構成，用于實時動態(tài)顯示擺桿角度，以及擺桿保持平衡狀態(tài)后維持的時間。二、理論分析與計算1.系統(tǒng)模型的建立一級旋轉式倒立擺系統(tǒng)由一個水平旋臂和一級擺桿組成，旋臂由電機驅動在水平面內作圓周運動，通過耦合作用

7、帶動擺桿轉動。如圖2所示，旋臂和擺桿可以抽象為兩個勻質桿，其中旋臂長度為R,相對y軸的角位移為a；擺桿長度為L,擺桿質心到連接點的距離為L'=L/2,相對z軸的角位移為日zL圖2系統(tǒng)建模示意圖由動力學理論，擺桿質心在x和z方向的速度分量為:Vx=R：|-Lcosi4Vy=.Lsin首系統(tǒng)總動能系統(tǒng)的動能由4部分構成，包括：旋臂在水平面上的轉動，擺桿在鉛直平面內的轉動，擺桿質心沿x軸、z軸方向的運動。對應的動能分量分別用T1,T2,T3,T4表示，因此系統(tǒng)動能T為這四個動能分量的和，系統(tǒng)動能如式(2):T=J1：；22Jp22m(R：-L'cosu(W)22m(-L'si

8、nud)2,2(2)其中，J2=mL'2/3。系統(tǒng)總勢能以旋臂所在水平面為零勢能面，則系統(tǒng)的勢能為擺桿的重力勢能：V=mgL'cosi(3)拉格朗日方程由拉格朗日算子H=T-V可推導出拉格朗日函數(shù)：H=J1u|2+J2H2/2+mR242/2-mL'Rcos8(4)(d)-mgL'cosH(4)已知系統(tǒng)的廣義坐標q=G,e,則由拉格朗日方程-=fi,Ft：他七：H=M- 4(i=1,2)可得方程組:.:H次I班J三以_M為電動機輸出轉2tq汩其中，fi為系統(tǒng)廣義坐標上非有勢力對應的廣義外力,矩，M=VgKiKg(Um-KgKm«)/Rm,巴為旋臂繞電機

9、轉軸轉動的阻尼系數(shù)，U為電機電樞電壓。將式(5)中的方程在(口aed)T=(0000)T處線性化，忽略高次項，最終可以得到系統(tǒng)的狀態(tài)方程如式(6)所示。-0di00NG4JmR2-3RG(4Ji mR2)L'103R4Ji mR22、3(J1 mR )g(4Ji mR2)L'011000I 4"mKiKg(4Ji+mR2)。3Rm%KiKg：(4Ji+mR2)L'Rm(6)其中，G=(mgKiKmK2凡).凡將系統(tǒng)的機械參數(shù)值帶入式 所示。(6),可得倒立擺系統(tǒng)的線性化數(shù)學模型如式-dd41-000.00046.8151.5110-11.87-8.950110

10、0my=日100a日上述建模過程所需各機械參數(shù)如表1所示表1系統(tǒng)物理參數(shù)表物理參數(shù)物理意義參數(shù)值L擺桿度0.199mm擺桿質量0.005kgR旋臂長度0.2mM旋臂質量0.01kgKi電機力矩系數(shù)3.87X10-3Nm-A-1Km反向電勢系數(shù)4x10-3Vsrad-1Kg變速器齒輪比4.2:1Rm直流電機電樞電阻0.476nm直流電機效率64.6%g變速器效率95%阻尼系數(shù)4.16x10-3Nms-rad-12.控制器算法的設計自適應模糊PID控制器以誤差e和誤差變化ec作為輸入，可以滿足不同時刻的e和ec對PID參數(shù)自整定的要求。利用模糊控制規(guī)則在線對PID參數(shù)進行修改，便構成了自適應模糊P

11、ID控制器。但是這里有一個問題：通過上述建模，若將a日dd作為模糊控制器的4個輸入，每個輸入又選定7個詞集的話，那么規(guī)則將有74=2401條，考慮到單片機的運算速度，這里考慮設計兩個控制器，分別控制豆、d和日、甘，由一個單片機并行運算處理。然后，將它們的輸出決策相加作為電動機的控制信號，來控制倒立擺的平衡。其結構如圖3所示圖3模*®PID控制器結構隸屬度函數(shù)對于旋臂的控制，取輸入“，&和輸出Kp,T,Td模糊子集為NB,NM,NS,Z,PS,PM,PB,分別代表負大，負中，負小，零,正小，正中，正大。論域為-3,3,量化等級為-3,-2,-1,0,1,2,3。輸入輸出隸屬度函

12、數(shù)均采用三角形，如圖4所示。圖4輸入輸出量的隸屬度函數(shù)對于擺桿的控制，取輸入e,d和輸出Kp，T，Td模糊子集為NB,NM,NS,Z,PS,PM,PB,同上述對于擺桿的控制，輸入輸出隸屬度函數(shù)也均采用三角形，但考慮倒立擺系統(tǒng)的控制以擺桿控制為主，要求擺桿在角度為零時能夠平衡倒立.故在零點附近分檔較細，如圖5所示。圖5輸入輸出量的隸屬度函數(shù)模糊控制規(guī)則根據Kp,Ti，Td對輸出特性的影響，可以歸納出在不同輸入下，Kp,Ti,Td的自整定要求，根據控制經驗，可得模糊規(guī)則，如表2所示。對應模糊規(guī)則，可以完成模糊推理，決策出模糊輸出量。表2Kp,T,Td模糊控制規(guī)則NBNMNSZOPSPMPBNBPB

13、/NB/PSPB/NB/PSPM/NB/ZOPM/NM/ZOPS/NM/ZOPS/ZO/PBPZO/ZO/PBNMPB/NB/NSPB/NB/NSPM/NM/NSPM/NM/NSPS/NS/ZOZO/ZO/PSPNM/PS/PMNSPM/NM/NBPM/NM/NBPM/NM/NSPS/NS/NSZO/ZO/ZONS/PS/PSNM/PM/PMZOPM/NM/NBPS/NS/NMPS/NS/NMZO/ZO/NSNS/PS/ZONM/PS/PSNM/PM/PSPSPS/NS/NBPS/NS/NMZO/ZO/NSNS/PS/NSNS/PS/ZONM/PM/PS；NB/PB/PSPMZO/ZO/NMZ

14、O/ZO/NSNS/PS/NSNM/PM/NSNM/PM/ZONM/PB/PSNB/PB/PSPBZO/ZO/PSNS/ZO/ZONS/PS/ZONM/PM/ZSNM/PB/ZONB/PB/PBNB/PB/PB最后，進行反模糊判決，利用重心法去模糊化，將模糊輸出分解成實際作用于電機的物理量。三、電路與程序設計1 .硬件電路設計(1)主控電路設計主控電路設計如圖6所示，系統(tǒng)采用飛思卡爾MK10DN512ZVLL1作為主控芯片，采集編碼器的位置信息，并根據倒立擺的角度和角加速度等數(shù)據計算搖臂運動速度和方向，從而實現(xiàn)倒立擺的平衡。班巾A3FPIIU MiwffTl3CK 1X Id 30 EIT ?

15、 AZ-L1L：5B_YUU=_gP«WDr«12Z,i,LL1DflJ :S1 JIM- MI UKIK2JUH1 中Ki亭 I ADC '券上父即刈I ADC$K I4 DCDI TIKTI :DAI ft UEJilLMJtfLlJlDt-.SUPPLY :上5m 3 SCL3 CAK U K 11W I5& 觸 Ellflti m? TZUOTA'UIPC 11 DEIL MAT I 3 ATL 1M 加為 CAfl. I 3tLIW IITII 附中 X'tuni wuaxim 種 I f HR AC4 IySAf_MEABU.53

16、ES-DIF矗1 J I3UD 1 -I1)tore C1+甲 TU1-111J i n at rnnR 33 T1I5 Eq.HttKH 31, TCI B.rtEft n 9*VCKK 35 EM n X FRJraK巴：5 FEBEICAT2 m n Herat i n.L JSESTATa m »rm由Emc n 33 TRAEEHT3 P2 ：ITRZEKT3H "TIUZEKTE&-RXj：LxmaKTALL皿3 E3T3C礪率K 算 MX : CAB i MAS ：3FC ：E AD： I 亡皿 2 ILU ZZ.HZ ¥眼 附44： 312

17、狗m 31 W1£J-JEL dCAPLJP Iffill 5n«R> U UA- L：,皿£L tAPL ： PC，LI >：AP1 J> NI5CI 1LU U ki：gan 乂制"PC' L 5 11 苫3 0 1LHQ 2 CAPC 1wosi a(2)電機驅動電路系統(tǒng)中的電機采用MOS管組成H橋驅動電路，該驅動就有電路可靠，驅動能力強，成本低等優(yōu)點，完全滿足本系統(tǒng)的要求。電路如圖7所示。圖7直流電機驅動圖2 .系統(tǒng)軟件設計(1)軟件設計為了支撐上述題目要求和控制算法，并且降低模塊間的耦合度從而為測試工作提供方便，我們設

18、計了如下的軟件構架：設備驅動層設備驅動層是針對單片機外設的驅動控制信號和反饋信號進行初步的封裝，從而實現(xiàn)高層邏輯不必關心時序或電壓等數(shù)字、模擬信號層數(shù)據。I/O數(shù)據配接層I/O數(shù)據配接層實現(xiàn)輸入輸出數(shù)據與控制邏輯數(shù)據的匹配，包括數(shù)據類型轉換(floatunsignedint)和數(shù)據緩沖維護。控制決策層控制決策層是控制系統(tǒng)的核心，其中包括系統(tǒng)狀態(tài)控制和運動決策控制。圖8控制決策層框圖由圖8所示，控制決策層首先從數(shù)據配接層的輸入緩沖讀入控制外設和傳感器外設的反饋數(shù)據，首先根據數(shù)據進行狀態(tài)控制，在確定系統(tǒng)狀態(tài)之后再配合狀態(tài)數(shù)據進行運動決策，將決策結果發(fā)送至I/O輸出數(shù)據配接模塊從而實現(xiàn)對設備的驅動。

19、圖8比較直觀地說明了決策層在控制系統(tǒng)中所處的位置。根據上述分析,軟件設計流程如圖9所示。12基總4分是H算入本步,/YNI$日啟動倒立擺啟動倒立擺口“是否達到、.，檢薊是否到達飛、60。J、165。附近1;YYX一反否達至卜、.，檢測是否朋_nNJ、360：./.'一掉外力工Y,Ur:結束啟動平衡程序<一八、.J檢測是否倒工.二一1Y:檢測時間是.N,丁否大于5sY（結束）圖9軟件流程圖發(fā)揮部分啟動倒立擺zziZ啟動平衡程序十|是否平衡;1XN.是否人于、<1待擾虹.里Y啟動平衡程序）一|塞否平截一NJY1啟動旋轉平衡程序匕一是音施轉N、360。一,Y:.結束四、系統(tǒng)測試及結果分析針對競賽最終測試項目及計劃演示的擴展表演項目內容，制訂并進行了如下測試，測試結果如表3所示。表3競賽測試數(shù)據表測試項目測試次數(shù)成功（準確）次數(shù)成功（準確）率備注傳感器部分靜態(tài)角度測量2020100%誤差0.1度動態(tài)角度測量2015（誤差小于0.5度次數(shù)）75%已通過算法優(yōu)化基本部分自啟動超60度2020100%自啟動超360度2020100%165度自平衡201995%165自平衡旋轉角度20125（平均最大轉角）138%（超過90度）統(tǒng)計規(guī)律發(fā)揮部分自啟動倒立202