簡易旋轉(zhuǎn)倒立擺及控制裝置剖析

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

2、據(jù)。關(guān)鍵字 ：倒立擺 模糊 PID 力學(xué)建模 狀態(tài)機一、系統(tǒng)方案1. 系統(tǒng)方案論證與選擇 倒立擺系統(tǒng)是一個復(fù)雜的快速、非線性、多變量、強耦合、自然不穩(wěn)定的系 統(tǒng)。對于該控制系統(tǒng)而言，合適的控制算法、精確的反饋信號、適合的電機驅(qū)動 等都對系統(tǒng)的穩(wěn)定性、控制精度及抗干擾性起重要作用。針對上述問題， 分別設(shè) 計多種不同的解決方案，并進(jìn)行選擇論證。（ 1）控制算法選擇 方案一：采用傳統(tǒng) PID 控制算法。傳統(tǒng) PID 控制算法是運用反饋求和后的誤差信號的比例 （0 階位置項 ）、積分 （誤差累積項 ）、微分（1 階速度項 ）進(jìn)行系統(tǒng)校正的一種控制算法。 可用于被控對象 的結(jié)構(gòu)和參數(shù)不能完全掌握， 或得

3、不到的精確數(shù)學(xué)模型的情況， 控制器的結(jié)構(gòu)和 參數(shù)必須依靠經(jīng)驗和反復(fù)調(diào)試來確定。方案二：采用模糊 PID 控制算法模糊 PID 控制算法根據(jù) PID 控制器的三個參數(shù)與偏差 e 和偏差的變化 ec 之 間的模糊關(guān)系， 在運行時不斷檢測 e及 ec，通過事先確定的關(guān)系， 利用模糊推理 的方法，在線修改 PID 控制器的三個參數(shù)，讓 PID 參數(shù)可自整定。將模糊控制 算法與傳統(tǒng) PID 控制算法巧妙結(jié)合，不但具有 PID 控制算法精度高等優(yōu)點，又 兼有模糊控制靈活、適應(yīng)性強的優(yōu)點。綜合考慮選擇方案二的模糊 PID 控制算法。（ 2）電動機選型 方案一：選擇步進(jìn)電動機 步進(jìn)電動機是將電脈沖激勵信號轉(zhuǎn)換

4、成相應(yīng)的角位移或線位移的離散值控 制電動機， 這種電動機每當(dāng)輸入一個電脈沖就動一步。 雖然控制時序和驅(qū)動電路 相對復(fù)雜，但步進(jìn)距離很小，保持力矩大，制動能力強。但步進(jìn)電機速度只在一 定范圍可調(diào)，并且一般步進(jìn)電機在不旋轉(zhuǎn)時仍有若干相通電，功耗太大。方案二：選擇直流電動機 直流電動機控制簡單，利用雙極性 PWM 即可實現(xiàn)調(diào)速和正、反轉(zhuǎn)，功率調(diào) 節(jié)范圍廣、適應(yīng)性好。直流電機的起動、制動轉(zhuǎn)矩大，易于快速起動、停車，易 于控制，且直流電機的調(diào)速性能好，調(diào)速范圍廣，易于平滑調(diào)節(jié)。綜上考慮選擇方案二的直流電動機。（3）傳感器的選擇 方案一：使用角位移傳感器 角位移傳感器是一個高精度的電位器， 它輸出為模擬量

5、。 但是在使用角位移 傳感器時，為得到其與豎直方向 （即重力方向 ）的夾角，要使用重擺，且在角度變 化小時，由于傳感器自身扭矩，將不會發(fā)生角位移，從而得不到采樣數(shù)據(jù)。方案二：使用主軸編碼器 主軸編碼器采用與主軸同步的光電脈沖發(fā)生器， 通過中間軸上的齒輪 1:1 地 同步傳動。一般是發(fā)光二極管發(fā)出紅外光束，通過動、靜兩片光柵后，到達(dá)光電 二極管，接收到脈沖信號，變換成數(shù)字量輸出。按編碼方式不同，分為增量式編 碼器和絕對編碼器。前者輸出脈沖，后者輸出 8421 碼。絕對值編碼器減輕了電 子接收設(shè)備的計算任務(wù)，從而省去了復(fù)雜的和昂貴的輸入裝置，而且， 當(dāng)機器合 上電源或電源故障后再接通電源， 不需要

6、回到位置參考點， 就可利用當(dāng)前的位置 值，得到了廣泛的應(yīng)用。綜上考慮選擇方案二的絕對值式主軸編碼器。2. 系統(tǒng)結(jié)構(gòu)基于題目要求及以上分析，本系統(tǒng)以飛思卡爾 MK10DN512ZVLL10 單片機 作為核心處理芯片，包括擺桿狀態(tài)檢測、電機驅(qū)動、液晶顯示等模塊。系統(tǒng)框圖 如圖 1 所示。圖1 系統(tǒng)結(jié)構(gòu)框圖系統(tǒng)各部分功能如下。中央處理模塊：該模塊采用飛思卡爾 MK10DN512ZVLL10 單片機作為主控 制器，完成系統(tǒng)的控制功能。擺桿檢測模塊： 該模塊由主軸編碼器構(gòu)成， 用于檢測擺桿的角位移及角速度， 并將信息反饋給 MCU 。電機驅(qū)動模塊：該模塊由步進(jìn)電動機實現(xiàn)，用于驅(qū)動旋轉(zhuǎn)臂做往復(fù)運動。液晶顯

7、示模塊：該模塊由 1.8寸 SPI TFT全彩屏構(gòu)成，用于實時動態(tài)顯示擺 桿角度，以及擺桿保持平衡狀態(tài)后維持的時間。二、理論分析與計算1. 系統(tǒng)模型的建立一級旋轉(zhuǎn)式倒立擺系統(tǒng)由一個水平旋臂和一級擺桿組成， 旋臂由電機驅(qū)動在 水平面內(nèi)作圓周運動，通過耦合作用帶動擺桿轉(zhuǎn)動。如圖 2 所示，旋臂和擺桿可 以抽象為兩個勻質(zhì)桿，其中旋臂長度為 R，相對 y軸的角位移為 ；擺桿長度為L ，擺桿質(zhì)心到連接點的距離為 L L 2，相對 z 軸的角位移為z圖2 系統(tǒng)建模示意圖由動力學(xué)理論，擺桿質(zhì)心在 x和 z方向的速度分量為：Vx R Lcos(1) Vy Lsin 系統(tǒng)總動能系統(tǒng)的動能由 4 部分構(gòu)成， 包括

8、：旋臂在水平面上的轉(zhuǎn)動， 擺桿在鉛直平面內(nèi)的轉(zhuǎn)動，擺桿質(zhì)心沿 x軸、z軸方向的運動。對應(yīng)的動能分量分別用 T1，T2 ，T3，T4 表示，因此系統(tǒng)動能 T 為這四個動能分量的和，系統(tǒng)動能如式 (2) ：T J1 2 2 J2 2 2 m(R Lcos ( )2 2 m( Lsin ( )2 2 (2) 其中， J2 mL2 3 。 系統(tǒng)總勢能 以旋臂所在水平面為零勢能面，則系統(tǒng)的勢能為擺桿的重力勢能：V mgL cos(3) 拉格朗日方程 由拉格朗日算子 H T V 可推導(dǎo)出拉格朗日函數(shù)：H J1 2 2 J2 2 2mR22 2 mL Rcos ( )( )mgLcos(4)已知系統(tǒng)的廣義坐

9、標(biāo) q,，則由拉格朗日方程HHfi ，t qiqii(i 1,2) 可得方程組：HHMtH H (5) t0其中， fi為系統(tǒng)廣義坐標(biāo)上非有勢力對應(yīng)的廣義外力， M 為電動機輸出轉(zhuǎn) 矩，M m gKiKg(Um KgKm ) Rm ， 為旋臂繞電機轉(zhuǎn)軸轉(zhuǎn)動的阻尼系數(shù)， U 為電機電樞電壓。將式(5)中的方程在 ()T (0 0 0 0)T 處線性化，忽略高次項，最終可以得到系統(tǒng)的狀態(tài)方程如式 (6)所示。004G4J1 mR23RG(4J1 mR2)L103R4J1 mR22 3(J1 mR2)g (4J1 mR2)L0100004 m gKi Kgm g i g(4J1 mR2)Rm3R m

10、 g KiKg(4J1 mR2 )L RmUm(6)其中， G ( m gKi KmKg2Rm) Rm將系統(tǒng)的機械參數(shù)值帶入式 所示。(6)，可得倒立擺系統(tǒng)的線性化數(shù)學(xué)模型如式(5)y1001000046.8151.511011.878.9501000055.1583.14Um(7)上述建模過程所需各機械參數(shù)如表 1 所示表1 系統(tǒng)物理參數(shù)表物理參數(shù)物理意義參數(shù)值L擺桿長度0.199 mm擺桿質(zhì)量0.005 kgR旋臂長度0.2 mM旋臂質(zhì)量0.01 kgKi電機力矩系數(shù)-3 -13.8710-3 N m A-1Km反向電勢系數(shù)4 10-3 V srad -1Kg變速器齒輪比4.2:1Rm直流

11、電機電樞電阻0.476m直流電機效率64.6 %g變速器效率95 %阻尼系數(shù)4.1610-3 N ms rad -12. 控制器算法的設(shè)計自適應(yīng)模糊 PID 控制器以誤差 e和誤差變化 ec 作為輸入，可以滿足不同時 刻的 e和ec對 PID參數(shù)自整定的要求。利用模糊控制規(guī)則在線對 PID參數(shù)進(jìn)行 修改，便構(gòu)成了自適應(yīng)模糊 PID 控制器。但是這里有一個問題：通過上述建模， 若將 作為模糊控制器的 4個輸入，每個輸入又選定 7 個詞集的話， 那么 規(guī)則將有 74=2401 條，考慮到單片機的運算速度，這里考慮設(shè)計兩個控制器，分 別控制 、 和 、 ，由一個單片機并行運算處理。然后，將它們的輸出

12、決策相加作為電動機的控制信號，來控制倒立擺的平衡。其結(jié)構(gòu)如圖 3 所示圖3 模糊 PID控制器結(jié)構(gòu) 隸屬度函數(shù)對 于 旋 臂 的 控 制 ， 取 輸 入 , 和 輸 出 Kp,Ti,Td 模 糊 子 集 為 NB,NM,NS,Z,PS,PM,PB ，分別代表負(fù)大， 負(fù)中，負(fù)小，零，正小，正中，正大。 論域為-3,3 ，量化等級為 -3,-2,-1,0,1,2,3 。輸入輸出隸屬度函數(shù)均采用三角形， 如圖 4 所示。圖4 輸入輸出量的隸屬度函數(shù)對 于 擺 桿 的 控 制 ， 取 輸 入 , 和 輸 出 Kp,Ti,Td 模 糊 子 集 為 NB,NM,NS,Z,PS,PM,PB ，同上述對于擺桿

13、的控制， 輸入輸出隸屬度函數(shù)也均采 用三角形， 但考慮倒立擺系統(tǒng)的控制以擺桿控制為主， 要求擺桿在角度為零時能 夠平衡倒立故在零點附近分檔較細(xì)，如圖 5 所示。圖5 輸入輸出量的隸屬度函數(shù) 模糊控制規(guī)則根據(jù) Kp,Ti,Td對輸出特性的影響，可以歸納出在不同輸入下， K p ,Ti , Td 的自整定要求，根據(jù)控制經(jīng)驗，可得模糊規(guī)則，如表 2 所示。對應(yīng)模糊規(guī)則，可以完 成模糊推理，決策出模糊輸出量。表 2 Kp,Ti ,Td模糊控制規(guī)則e ecNBNMNSZOPSPMPBNBPB/NB/PSPB/NB/PSPM/NB/ZOPM/NM/ZOPS/NM/ZOPS/ZO/PBZO/ZO/PBNMP

14、B/NB/NSPB/NB/NSPM/NM/NSPM/NM/NSPS/NS/ZOZO/ZO/PSNM/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/PSNB/PB/PSPMZO/ZO/NMZO/ZO/NSNS/PS/NSNM/PM/NSNM/PM/ZONM/PB/PSNB/PB/PSPBZO/ZO/PSNS/

15、ZO/ZONS/PS/ZONM/PM/ZSNM/PB/ZONB/PB/PBNB/PB/PB最后，進(jìn)行反模糊判決，利用重心法去模糊化，將模糊輸出分解成實際作用 于電機的物理量。三、電路與程序設(shè)計1. 硬件電路設(shè)計（1）主控電路設(shè)計主控電路設(shè)計如圖 6 所示，系統(tǒng)采用飛思卡爾 MK10DN512ZVLL10作為主控 芯片，采集編碼器的位置信息， 并根據(jù)倒立擺的角度和角加速度等數(shù)據(jù)計算搖臂 運動速度和方向，從而實現(xiàn)倒立擺的平衡。圖 6 主控電路圖（ 2）電機驅(qū)動電路 系統(tǒng)中的電機采用 MOS管組成 H 橋驅(qū)動電路，該驅(qū)動就有電路可靠，驅(qū)動 能力強，成本低等優(yōu)點，完全滿足本系統(tǒng)的要求。電路如圖 7 所

16、示。圖 7 直流電機驅(qū)動圖2. 系統(tǒng)軟件設(shè)計（ 1） 軟件設(shè)計為了支撐上述題目要求和控制算法， 并且降低模塊間的耦合度從而為測試工 作提供方便，我們設(shè)計了如下的軟件構(gòu)架：設(shè)備驅(qū)動層 設(shè)備驅(qū)動層是針對單片機外設(shè)的驅(qū)動控制信號和反饋信號進(jìn)行初步的封裝， 從而實現(xiàn)高層邏輯不必關(guān)心時序或電壓等數(shù)字、模擬信號層數(shù)據(jù)。I/O 數(shù)據(jù)配接層I/O 數(shù)據(jù)配接層實現(xiàn)輸入輸出數(shù)據(jù)與控制邏輯數(shù)據(jù)的匹配，包括數(shù)據(jù)類型轉(zhuǎn) 換（ floatunsigned int）和數(shù)據(jù)緩沖維護(hù)?？刂茮Q策層 控制決策層是控制系統(tǒng)的核心，其中包括系統(tǒng)狀態(tài)控制和運動決策控制。圖 8 控制決策層框圖由圖 8 所示，控制決策層首先從數(shù)據(jù)配接層的輸

17、入緩沖讀入控制外設(shè)和傳感 器外設(shè)的反饋數(shù)據(jù)， 首先根據(jù)數(shù)據(jù)進(jìn)行狀態(tài)控制， 在確定系統(tǒng)狀態(tài)之后再配合狀 態(tài)數(shù)據(jù)進(jìn)行運動決策， 將決策結(jié)果發(fā)送至 I/O 輸出數(shù)據(jù)配接模塊從而實現(xiàn)對設(shè)備 的驅(qū)動。圖 8 比較直觀地說明了決策層在控制系統(tǒng)中所處的位置。 根據(jù)上述分析， 軟件設(shè)計流程如圖 9 所示。開始發(fā)揮部分基本部分是否為本步啟動倒立擺啟動倒立擺N是否達(dá)60YY結(jié)束360N是否達(dá)到啟動倒立擺YY測是否撤N掉外力N測是否到達(dá)165附近啟動平衡程序NYN10sYYN等待擾動啟動平衡程序N檢測是否倒立Y啟動平衡程序NYYN測時間是否大于5啟動旋轉(zhuǎn)平衡程序Y是否旋360結(jié)束圖 9 軟件流程圖四、系統(tǒng)測試及結(jié)果

18、分析 針對競賽最終測試項目及計劃演示的擴展表演項目內(nèi)容， 制訂并進(jìn)行了如下 測試，測試結(jié)果如表 3 所示。表 3 競賽測試數(shù)據(jù)表測試項目測試次 數(shù)成功（準(zhǔn)確）次 數(shù)成功（準(zhǔn)確）率備注傳感器部分靜態(tài)角度測量2020100%誤差0.1度動態(tài)角度測量2015（ 誤差小于 0.5 度次數(shù)）75%已通過算法 優(yōu)化基本部分自啟動超 60 度2020100%自啟動超 360 度2020100%165 度自平衡201995%165 自平衡旋轉(zhuǎn)角度20125（平均最大 轉(zhuǎn)角）138%（超過 90 度）統(tǒng)計規(guī)律發(fā)揮部分自啟動倒立2020100%5s 內(nèi)穩(wěn)定施加擾動倒立2020100%倒立轉(zhuǎn)圈2020100%擴展液晶顯示角度2020100%大擾動演示2017 次85%20g砝碼， 90度干擾結(jié)果分析測試結(jié)果顯示， 本設(shè)計已基本完成設(shè)計要求中所規(guī)定的