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

1、簡(jiǎn)易旋轉(zhuǎn)倒立擺及控制裝置摘要：本文設(shè)計(jì)并制作了一套旋轉(zhuǎn)倒立擺及其控制裝置，通過(guò)單片機(jī)控制直流電機(jī)帶動(dòng)旋轉(zhuǎn)臂轉(zhuǎn)動(dòng)，實(shí)現(xiàn)了快速起擺、倒立保持、抗擾動(dòng)等功能。通過(guò)將擺桿倒立處的物理方程線性化得到了該系統(tǒng)的狀態(tài)空間方程。用能量補(bǔ)償方法實(shí)現(xiàn)了起擺，起擺后利用線性二次最優(yōu)控制(LQR)算法對(duì)模型進(jìn)行穩(wěn)定控制。實(shí)際測(cè)試結(jié)果表明該系統(tǒng)起擺速度快、穩(wěn)定時(shí)間長(zhǎng)、具有良好的抗干擾能力。 關(guān)鍵字：旋轉(zhuǎn)倒立擺；起擺控制；穩(wěn)定控制；線性二次型最優(yōu)控制一、 系統(tǒng)方案系統(tǒng)由stm32單片機(jī)、增量式編碼器、直流伺服電機(jī)及其驅(qū)動(dòng)電路、人機(jī)交互接口等部分構(gòu)成。直流伺服電機(jī)上的編碼器可以測(cè)量旋轉(zhuǎn)臂轉(zhuǎn)過(guò)的角度，擺桿和旋轉(zhuǎn)臂之間安裝的編

2、碼器可以測(cè)量擺桿轉(zhuǎn)過(guò)的角度。利用測(cè)量到的兩個(gè)角度和角速度，計(jì)算得到驅(qū)動(dòng)直流電機(jī)所需的占空比和轉(zhuǎn)動(dòng)方向，通過(guò)旋轉(zhuǎn)臂帶動(dòng)擺桿擺動(dòng)使擺桿完成要求的動(dòng)作，系統(tǒng)原理方框圖如圖 1所示。圖 1系統(tǒng)框架1. 電機(jī)選擇方案方案一： 采用步進(jìn)電機(jī)。步進(jìn)電機(jī)是開(kāi)環(huán)控制的，輸入一個(gè)脈沖步進(jìn)電機(jī)就會(huì)轉(zhuǎn)過(guò)一個(gè)固定的角度，驅(qū)動(dòng)相對(duì)方便。但是步進(jìn)電機(jī)響應(yīng)速度不夠快，另外控制不當(dāng)可能引起共振和失步。方案二：采用直流電機(jī)。直流電機(jī)響應(yīng)速度快，轉(zhuǎn)矩比較大。但是需要額外的編碼器來(lái)反饋得到電機(jī)的轉(zhuǎn)速和位置，不能像步進(jìn)電機(jī)那樣通過(guò)脈沖個(gè)數(shù)來(lái)控制轉(zhuǎn)過(guò)的角度?？紤]到旋轉(zhuǎn)倒立擺在啟動(dòng)和維持倒立狀態(tài)時(shí)都需要較快的反應(yīng)速度和較大扭矩，扭矩和響應(yīng)

3、速度是關(guān)鍵的因素，因此選擇了方案二。并且在直流電機(jī)上安裝了編碼器用于獲取轉(zhuǎn)過(guò)的角度。2. 角度傳感器選擇方案一：電位器式的角度傳感器。這類傳感器線性精度高且穩(wěn)定，溫度系數(shù)小，而且輸出量是絕對(duì)式的，使用前不需要額外的校準(zhǔn)調(diào)零，成本較低。但是這類傳感器輸出信號(hào)為模擬量需要經(jīng)AD采集后才能轉(zhuǎn)換成相應(yīng)的角度，分辨率受AD限制，高頻特性較差。方案二：加速度式的角度傳感器。這類傳感器依靠測(cè)重力加速度在某個(gè)軸上的分量測(cè)得角度。只有讀取重力加速度在某個(gè)軸上的分量，通過(guò)就可以計(jì)算出角。這類傳感器只適合測(cè)量靜態(tài)的角度，在有振動(dòng)或轉(zhuǎn)動(dòng)的場(chǎng)合下會(huì)引入額外的加速度，使得測(cè)量不準(zhǔn)確。 方案三：增量式編碼器。這類

4、傳感器輸出信號(hào)為兩路正交的矩形波，通過(guò)單片機(jī)的正交編碼器接口即可得到一個(gè)相對(duì)的角度，測(cè)量的精度取決于光柵碼盤上的線數(shù)。這類傳感器穩(wěn)定性好，精度高。綜合考慮系統(tǒng)的穩(wěn)定性和精度，選擇了方案三。二、 理論分析與計(jì)算1. 電機(jī)選型 選用MAXON RE25電機(jī)，該電機(jī)為額定電壓24V，額定工作電流1.17A，能夠輸出額定功率為28.08W。由于旋轉(zhuǎn)桿繞中心旋轉(zhuǎn)帶動(dòng)擺桿擺動(dòng)，則系統(tǒng)所需最大力矩為(物理參數(shù)見(jiàn)附錄2)，小于電機(jī)的額定轉(zhuǎn)矩。該電機(jī)最大效率為86%，帶減速比電機(jī)轉(zhuǎn)速為，速度較快，能夠滿足題目響應(yīng)時(shí)間要求。所以選擇該電機(jī)。2. 擺桿狀態(tài)檢測(cè)擺桿狀態(tài)檢測(cè)采用1000線的增量式編碼器，其輸出信號(hào)為兩

5、路正交的矩形波，根據(jù)相位對(duì)矩形波脈沖個(gè)數(shù)計(jì)數(shù)就可以得到轉(zhuǎn)過(guò)的相對(duì)角度。由于輸出的是相對(duì)角度，使用前需要確認(rèn)零點(diǎn)位置。經(jīng)4倍頻處理后，轉(zhuǎn)動(dòng)一周計(jì)數(shù)器計(jì)數(shù)到4000，最小可分辨角為。若當(dāng)前計(jì)數(shù)值為a，則轉(zhuǎn)過(guò)的角度就是。利用單片機(jī)定時(shí)器產(chǎn)生一個(gè)定時(shí)間隔，記錄定時(shí)器開(kāi)始和定時(shí)結(jié)束時(shí)碼盤計(jì)算器的值和，則角速度為。3. 數(shù)學(xué)模型及穩(wěn)定控制算法旋轉(zhuǎn)倒立擺由一個(gè)旋轉(zhuǎn)臂和一個(gè)擺桿組成，其參考坐標(biāo)系見(jiàn)下圖 2。根據(jù)圖 2 旋轉(zhuǎn)倒立擺數(shù)學(xué)模型參考坐標(biāo)系剛體轉(zhuǎn)動(dòng)慣量可以直接寫出旋轉(zhuǎn)臂和B處角度傳感器的動(dòng)能，分別計(jì)為和，利用微分法可以計(jì)算出擺桿的動(dòng)能。系統(tǒng)的總動(dòng)能就可以計(jì)為： 公式(1)以擺桿自然下垂時(shí)質(zhì)心位置為勢(shì)能零

6、點(diǎn)，則系統(tǒng)總勢(shì)能也可以直接寫出，記為。記拉格朗日算子，系統(tǒng)廣義坐標(biāo)，廣義坐標(biāo)上的非有勢(shì)力對(duì)應(yīng)廣義外力為電機(jī)輸出轉(zhuǎn)矩和摩擦力，上非有勢(shì)力對(duì)應(yīng)廣義外力為摩擦力，由拉格朗日方程： 公式(2)由拉格朗日方程結(jié)合系統(tǒng)的動(dòng)能和勢(shì)能方程可以得到系統(tǒng)的運(yùn)動(dòng)方程。系統(tǒng)在處是平衡的，將運(yùn)動(dòng)方程在線性化，忽略高次項(xiàng)并令，整理后得到系統(tǒng)的狀態(tài)方程： 公式(3)限于篇幅，詳細(xì)推導(dǎo)過(guò)程在附錄1中。系統(tǒng)的物理參數(shù)和A、B、C矩陣求解結(jié)果見(jiàn)附錄2。系統(tǒng)可控性矩陣和可觀性矩陣都是滿秩的，所以系統(tǒng)是可觀可控的。因此可以設(shè)計(jì)一個(gè)基于狀態(tài)反饋的控制器對(duì)系統(tǒng)進(jìn)行極點(diǎn)配置，這里采用基于最優(yōu)控制的線性二次狀態(tài)調(diào)節(jié)器(LQR)來(lái)設(shè)計(jì)控制器。

7、LQR控制的意義是求取控制量u，使得二次型的性能指標(biāo)： 公式(4)達(dá)到最優(yōu)，其中Q和R分別是狀態(tài)向量和控制向量的權(quán)重矩陣。LQR的問(wèn)題歸結(jié)于求解Riccatti方程： 公式(5)求得矩陣，就可以得到狀態(tài)反饋形式的控制量，K稱為是最優(yōu)反饋增益矩陣。結(jié)合上面求出的A、B矩陣，利用matlab的lqr命令可以方便地求得矩陣。加入反饋矩陣K后的方框圖如圖 3，在擺桿處于穩(wěn)定倒立情況下輸入r為0。圖 3 閉環(huán)控制方框圖4. 起擺控制算法倒立擺的起擺是一個(gè)復(fù)雜的非線性系統(tǒng)控制問(wèn)題。本設(shè)計(jì)通過(guò)控制擺的能量來(lái)直接實(shí)現(xiàn)起擺控制。在圖 2所示的坐標(biāo)系中，定義擺桿在倒立位置處能量為： (公式6)其導(dǎo)數(shù) (公式7)由

8、于 (公式8)則 (公式9)應(yīng)用李亞普諾夫方法，令李亞普諾夫函數(shù)，則令，則由于擺桿不可能一直在水平位置，即不會(huì)一直為0，因此函數(shù)V會(huì)逐漸減小，直到，此時(shí)擺桿起擺，實(shí)現(xiàn)了擺桿倒立。在不影響李亞普諾夫函數(shù)遞減的情況下對(duì)控制率進(jìn)行調(diào)整。調(diào)整原則是控制力矩盡可能大，以使擺桿在最短時(shí)間內(nèi)起擺。取驅(qū)動(dòng)電壓為，k是一個(gè)常數(shù)，sign是符號(hào)函數(shù)。三、 電路與程序設(shè)計(jì)1. 編碼器電平轉(zhuǎn)換電路編碼器型號(hào)為E40S6-1000-3-T-24，該編碼器供電電壓為1224V。如果采用12V供電編碼器輸出高電平為+12V，需要采用電平轉(zhuǎn)換來(lái)輸入單片機(jī)。HCPL 2601是雙路光耦，能將+12V轉(zhuǎn)換成+5V的邏輯電平，且光

9、耦能夠?yàn)V去數(shù)字信號(hào)上的噪聲。數(shù)字光電編碼器驅(qū)動(dòng)電路如下圖 4。圖 4 編碼器電平轉(zhuǎn)換電路2. 電機(jī)驅(qū)動(dòng)電路電機(jī)驅(qū)動(dòng)電路由一片L6205和相關(guān)外圍元件組成，L6205為雙路電機(jī)驅(qū)動(dòng)。在本題目中電機(jī)需要消耗很大的電流，為了避免電流大幅波動(dòng)影響到供電電壓，因此在電機(jī)驅(qū)動(dòng)板上安裝了大電容，具體電路見(jiàn)圖 5。圖 5 電機(jī)驅(qū)動(dòng)電路3. 系統(tǒng)軟件設(shè)計(jì)及流程圖系統(tǒng)整體框圖如下圖 6，單片機(jī)復(fù)位后，初始化各個(gè)任務(wù)模塊然后根據(jù)按鍵輸入執(zhí)行相應(yīng)任務(wù)，共有4個(gè)任務(wù)模塊分別是往復(fù)運(yùn)動(dòng)、松手穩(wěn)定、下垂起擺和其它功能。圖 6 系統(tǒng)整體框圖下面重點(diǎn)描述起擺及維持穩(wěn)定的功能，先輸入一個(gè)觸發(fā)信號(hào)，讓擺桿開(kāi)始擺動(dòng)，然后利用能量補(bǔ)償

10、算法計(jì)算需要輸出的電壓讓擺桿起擺，判斷擺桿的狀態(tài)決定在何時(shí)開(kāi)始進(jìn)入維持倒立的任務(wù)。維持倒立狀態(tài)采用了LQR算法進(jìn)行控制，如果在外界擾動(dòng)下，不能滿足，那么就重新進(jìn)入起擺任務(wù)讓擺桿重新起擺。圖 7 起擺及維持穩(wěn)定流程圖四、 測(cè)試方案與測(cè)試結(jié)果1. 測(cè)試工具表 1 測(cè)試工具表名稱型號(hào)數(shù)量1直流穩(wěn)壓電源DF1743003C12量角器ZG1981(精度1度)13鋼尺無(wú)(精度1mm)14秒表無(wú)(精度0.01s)15電子天平JT1001N (精度0.1g)12. 測(cè)試方案與結(jié)果1) 起擺用時(shí)測(cè)試擺桿從下垂?fàn)顟B(tài)開(kāi)始，驅(qū)動(dòng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂作往復(fù)轉(zhuǎn)動(dòng)，記錄從開(kāi)始擺動(dòng)到擺角超過(guò)60度和擺桿完成圓周運(yùn)動(dòng)所用的時(shí)間。表

11、2 起擺用時(shí)測(cè)試結(jié)果第1次測(cè)試第2次測(cè)試第3次測(cè)試平均用時(shí)超過(guò)60度用時(shí)/s0.1s0.2s0.2s0.17s完成圓周運(yùn)動(dòng)用時(shí)/s0.7s0.6s0.8s0.7s2) 倒立狀態(tài)恢復(fù)測(cè)試在擺桿處于自然下垂?fàn)顟B(tài)下，外力拉起擺桿至接近 165°位置，外力撤除同時(shí)，啟動(dòng)控制旋轉(zhuǎn)臂。用秒表記錄恢復(fù)到倒立狀態(tài)所用的時(shí)間和旋轉(zhuǎn)臂轉(zhuǎn)過(guò)的最大角度。表 3 倒立狀態(tài)恢復(fù)測(cè)試結(jié)果擺桿角度/度160162165167170旋轉(zhuǎn)臂最大角度/度3533302015時(shí)間（s）15s1.51.4s1.4s1.2s3) 擺桿自下垂到倒立測(cè)試讓擺桿處于自然下垂?fàn)顟B(tài)開(kāi)始，控制旋轉(zhuǎn)臂作往復(fù)旋轉(zhuǎn)運(yùn)動(dòng)，直至擺桿倒立，記錄從開(kāi)始

12、擺動(dòng)到倒立狀態(tài)和倒立狀態(tài)維持的時(shí)間表 4 擺桿自下垂到倒立測(cè)試結(jié)果第1次測(cè)試第2次測(cè)試第3次測(cè)試平均時(shí)間到達(dá)倒立狀態(tài)用時(shí)/s0.7s0.8s0.6s0.7s倒立狀態(tài)維持時(shí)間/s240s以上240s以上240s以上240s以上4) 擺桿倒立時(shí)旋轉(zhuǎn)臂作圓周運(yùn)動(dòng)測(cè)試在擺桿保持倒立狀態(tài)的前提下，旋轉(zhuǎn)臂作圓周運(yùn)動(dòng)，記錄旋轉(zhuǎn)臂作一次圓周運(yùn)動(dòng)所用時(shí)間和擺桿與倒立位置間的最大角度表 5 擺桿倒立時(shí)旋轉(zhuǎn)臂作圓周運(yùn)動(dòng)測(cè)試結(jié)果第1次測(cè)試第2次測(cè)試第3次測(cè)試平均結(jié)果圓周運(yùn)動(dòng)用時(shí)/s3s4s4s5s擺桿最大偏移角/度108963. 結(jié)果分析擺桿起擺速度很快，能夠在第一次或第二次擺動(dòng)后就維持在倒立狀態(tài)，響應(yīng)時(shí)間很短，且倒

13、立狀態(tài)能夠維持很長(zhǎng)時(shí)間，擺桿倒立旋轉(zhuǎn)臂做圓周運(yùn)動(dòng)，耗時(shí)少且擺桿偏離倒立狀態(tài)角度小，較好地滿足了題目要求。五、 結(jié)論與收獲倒立狀態(tài)下采用基于狀態(tài)空間反饋及LQR極點(diǎn)配置方法，系統(tǒng)能夠及時(shí)且精確地控制擺桿的運(yùn)動(dòng)，具有較強(qiáng)的抗干擾能力。在起擺過(guò)程采用了基于能量補(bǔ)償?shù)姆椒?，起擺速度很快。參考文獻(xiàn)1康華光.電子技術(shù)基礎(chǔ)（模擬部分）M.高等教育出版社,2003. 2張培仁等.機(jī)器人系統(tǒng)設(shè)計(jì)與算法M.中國(guó)科學(xué)與技術(shù)大學(xué)出版社,2008. 3王正林.MATLAB/Simulink與控制系統(tǒng)仿真M.電子工業(yè)出版社,2012.4王成元.現(xiàn)代電機(jī)控制技術(shù)M.機(jī)械工業(yè)出版社,2009.5劉軍.例說(shuō)STM32M.北京航

14、空航天大學(xué)出版社,2011.附錄1：旋轉(zhuǎn)倒立擺狀態(tài)空間方程推導(dǎo)建立如圖 8所示坐標(biāo)系，設(shè)旋轉(zhuǎn)臂長(zhǎng)為，質(zhì)量為，擺桿長(zhǎng)為，質(zhì)量為，B位置處角度傳感器質(zhì)量為，為旋轉(zhuǎn)臂與軸正向夾角，為擺桿與軸正向夾角。旋轉(zhuǎn)臂的總動(dòng)能為： 公式(F-1) 由變分法求得擺桿的總動(dòng)能為： 公式(F-2)擺桿與旋轉(zhuǎn)臂連接處編碼器動(dòng)能為： 公式(F-3)系統(tǒng)總動(dòng)能為： 公式(F-4)以擺桿自然下垂時(shí)質(zhì)心位置為勢(shì)能零點(diǎn)，則系統(tǒng)總勢(shì)能為：圖 8旋轉(zhuǎn)數(shù)學(xué)模型參考坐標(biāo)系以擺桿自然下垂時(shí)質(zhì)心位置為勢(shì)能零點(diǎn)，則系統(tǒng)總勢(shì)能為： 公式(F-5)計(jì)拉格朗日算子，系統(tǒng)廣義坐標(biāo)，廣義坐標(biāo)上非有勢(shì)力對(duì)應(yīng)廣義外力為電機(jī)輸出轉(zhuǎn)矩和摩擦力，上非有勢(shì)力對(duì)應(yīng)廣義外力為摩擦力，由拉格朗日方程： 公式(F-6)由公式(1)(6)可得到系統(tǒng)的運(yùn)動(dòng)方程：其中為電機(jī)輸出轉(zhuǎn)矩,為電機(jī)輸入電壓，為電機(jī)力矩系數(shù)，為電機(jī)反電勢(shì)系數(shù)，、分別為旋轉(zhuǎn)臂和擺桿繞軸轉(zhuǎn)動(dòng)的阻尼系