倒立擺_周長興組

1、倒立擺學(xué) 校：西安交通大學(xué)作 者：周長興 萬 星 邵永勝指導(dǎo)老師：張鵬輝 王中方 張翠翠摘 要：系統(tǒng)采用stm32f407作為倒立擺系統(tǒng)控制核心，實現(xiàn)擺的一系列功能。硬件由電源電路、CPU最小系統(tǒng)模塊、電機驅(qū)動模塊、編碼器模塊、顯示模塊等組成。算法采用雙環(huán)PID進行控制，內(nèi)環(huán)使用擺桿角度誤差量作為反饋對電機速度進行控制，外環(huán)使用電機的角度位置進行反饋，與內(nèi)環(huán)級聯(lián)。測試表明各項指標(biāo)都符合設(shè)計任務(wù)要求。關(guān)鍵詞：STM32f407單片機、雙環(huán)PID、倒立擺Abstract：System uses stm32f407 as the core of control system to achieve a

2、 series of functions of inverted pendulum. Hardware consists of the power circuit, CPU minimum system modules, motor drive module, the encoder module, display module and other components. We use Double loop PID as the control algorithm, the pendulum angle of error as a feedback control of the motor

3、speed in the inner loop, the angular position of the motor feedback in the outer loop. Tests show that the indicators are in line with the design task requirements.Keywords：STM32f407； Double Loop PID；Inverted PendulumI一、系統(tǒng)方案與設(shè)計根據(jù)題目要求，系統(tǒng)主要實現(xiàn)倒立擺的各項功能，系統(tǒng)框圖如圖1.1所示，可分為電源模塊、CPU最小系統(tǒng)模塊、電機驅(qū)動模塊、編碼器模塊、顯示模塊。圖1.

4、1 系統(tǒng)模塊框圖下面分別論證這幾個模塊的選擇與機械結(jié)構(gòu)的設(shè)計。1. 控制器模塊的論證與選擇 方案一：采用以增強型80C51內(nèi)核的STC系列單片機STC12C5A60S2，其片內(nèi)集成了60KB程序Flash，2通道PWM、16位定時器等資源3，操作也較為簡單，具有在系統(tǒng)調(diào)試功能（ISD），開發(fā)環(huán)境非常容易搭建。但實際使用了編碼器等對速度要求較高的外設(shè)，因此無法很好地符合設(shè)計的需要。 方案二： 采用以ARM Cortex-M4為內(nèi)核的STM32系列控制芯片，STM32系列芯片時鐘頻率高達168MHz 具有512K字節(jié)SRAM，具有極強的處理計算能力1。較為適合需要快速反應(yīng)的倒立擺系統(tǒng)。通過比較，我

5、們選擇方案二，采用STM系列單片機STM32f407作為控制器。2電源模塊的論證與選擇 方案一：采用兩個電源供電。將電動機驅(qū)動電源與單片機以及其周邊電路電源完全隔離，利用光電耦合器傳輸信號。這樣可以使電動機驅(qū)動所造成的干擾徹底消除，提高了系統(tǒng)的穩(wěn)定性。 方案二：采用單一電源供電。電源直接給電動機供電，因電動機啟動瞬間電流較大，會造成電源電壓波動，因而控制與檢測部分電路通過集成穩(wěn)壓模塊分別供電。其供電電路比較簡單。通過比較，穩(wěn)壓模塊的穩(wěn)定性較好，可以滿足系統(tǒng)的要求?？紤]到系統(tǒng)的精簡性，我們選擇方案二單電源對系統(tǒng)進行供電。3. 電動機驅(qū)動模塊的論證與選擇使用stm32f101作為電機控制盒的芯片，

6、產(chǎn)生PWM波控制，采用H橋驅(qū)動，系統(tǒng)框圖如下。額外采用一個芯片進行電機控制可以有效減輕主芯片的工作壓力，獲得較好的控制性能，同時控制盒作為一個獨立的整體，使用較為靈活方便，簡化系統(tǒng)，易于維護。電機驅(qū)動程序流程圖如下。圖1.2 電機驅(qū)動程序流程圖4. 機械結(jié)構(gòu)設(shè)計我們使用加重過的底盤，克服電機瞬間加減速時巨大的震動。擺桿使用實心的鋼棒，充足的重量保證了其轉(zhuǎn)動時不太受阻尼的影響而不靈活。與擺桿鏈接的轉(zhuǎn)軸使用軸承座與水平臂連接，保證擺桿優(yōu)良的靈活性。編碼器接在轉(zhuǎn)軸的后端，直接測量擺桿的角度值。電機使用伺服電機，其性能非常優(yōu)異，可以對速度等進行精準(zhǔn)的控制。電機與水平臂通過聯(lián)軸器連接，避免了不同軸的問題

7、對電機的傷害。水平臂做成雙方向的，在擺桿的反方向加上配重，用以平衡重力與轉(zhuǎn)矩，使整個系統(tǒng)能夠做到靜平衡和動平衡。這樣，我們就搭建了一個穩(wěn)定可靠的機械系統(tǒng)，這是的系統(tǒng)的各項參數(shù)處于比較穩(wěn)定的狀態(tài)，有利于后期控制系統(tǒng)的調(diào)節(jié)。二、系統(tǒng)理論分析與計算1. 環(huán)形倒立擺物理模型建立在忽略了空氣阻力、各種摩擦之后，將環(huán)形倒立擺系統(tǒng)抽象成勻質(zhì)擺桿和水平桿組成的剛體系統(tǒng).環(huán)形一級倒立擺的結(jié)構(gòu)如圖所示：圖2.1 環(huán)形一級倒立擺的結(jié)構(gòu)圖水平桿與的夾角：，擺桿與垂直方向的夾角： 水平桿的質(zhì)量 0.308 kg水平桿繞端點的轉(zhuǎn)動慣量 0.004 擺桿的質(zhì)量 0.1323 kg擺桿繞質(zhì)心的轉(zhuǎn)動慣量 0.0115水平桿的長

8、度 0.2535 m擺桿質(zhì)心到轉(zhuǎn)軸的距離 0.2433m表2.1 環(huán)形一級倒立擺的物理參數(shù)系統(tǒng)的拉格朗日算子：，其中為拉格朗日算子、為系統(tǒng)的廣義坐標(biāo)、為系統(tǒng)的總動能、為系統(tǒng)的總勢能.拉格朗日方程： (1)其中，、為系統(tǒng)沿廣義坐標(biāo)方向上的外力.在環(huán)形一級倒立擺系統(tǒng)中廣義坐標(biāo)：環(huán)形一級倒立擺系統(tǒng)的動能： （2）其中，為水平桿的動能、為擺桿的動能.水平桿的動能： 在距擺桿轉(zhuǎn)動中心距離處取一小段，這一小段的坐標(biāo)如下：則，這一小段的動能：擺桿的動能： （3）以水平桿所在的水平面為零勢能面，則系統(tǒng)的勢能即為擺桿的重力勢能： （4）則，拉格朗日方程： （5）其中，為施加到水平桿上的控制力矩.在倒立擺實物控制

9、中，我們采用水平桿的角加速度作為輸入即：2. 雙環(huán)PID控制算法控制框圖如下：圖2.2 雙環(huán)PID控制算法三、電路與程序設(shè)計1. 電路的設(shè)計 經(jīng)過上述的分析和論證，決定了系統(tǒng)各模塊采用的最終方案如下： 控制模塊：采用stm32f407芯片； 電源模塊：采用單電源供電，電機與控制模塊分別用集成穩(wěn)壓芯片供電； 鍵盤模塊：采用獨立鍵盤； 電機驅(qū)動模塊：采用stm32f101配合H橋?qū)崿F(xiàn)；編碼模塊：利用編碼器采集擺桿的角度； A. 系統(tǒng)總體框圖（模塊電路圖詳見附件1.1）系統(tǒng)總的框圖如圖3.1所示。 圖3.1 系統(tǒng)總體框圖B.單片機最小系統(tǒng)3單片機最小系統(tǒng)采用5V供電，并接有指示燈， 5mA左右電流足

10、以驅(qū)動LED，LED導(dǎo)通電壓1.7V左右，因此配上1K電阻；單片機根據(jù)接收的按鍵輸入數(shù)據(jù)和傳感器輸出電平信號，通過uart與電機控制盒通信，進而控制電機A、電機B和電機C的轉(zhuǎn)動，從而控制小車的運動。C.電源電路設(shè)計電源電路采用DC-DC模塊分別為電機和控制器等模塊供電，電池采用11.1V、2200mAh大容量鋰電池，以保證小車在運行過程中有足夠的電量。兩路電源分別帶有指示燈，以指明電源有無。D.電機驅(qū)動電路設(shè)計電路采用PWM驅(qū)動形式，四路輸入和輸出以控制電機運轉(zhuǎn)。電路主要由控制芯片stm32f101、光電耦合器、穩(wěn)壓二極管組成，驅(qū)動電路可控制電機的正反轉(zhuǎn)。電路，與單片機的接口采用光電耦合器，利

11、用光電耦合器傳輸信號可以消除相互之間的干擾，提高系統(tǒng)的穩(wěn)定性。單片機控制H橋4個開關(guān)管的通斷實現(xiàn)對電機的控制。2程序的設(shè)計系統(tǒng)采用C語言編程實現(xiàn)各項功能。C語言本身帶有各種庫函數(shù)，運算能力較強，而本系統(tǒng)的軟件中算數(shù)運算比較多，利用C語言編程可以體現(xiàn)出一定優(yōu)勢。程序是在Window7環(huán)境下采用Keil uVision4軟件編寫的，可實現(xiàn)對倒立擺的精確控制，對傳感器輸入信號的處理、對按鍵輸入的查詢以及聲光提示等功能。四、測試方案與測試結(jié)果1測試方案（1）硬件測試分別對每個模塊做相應(yīng)的測試。（2）軟件仿真測試使用Keil MDK軟件進行仿真。（3）硬件軟件聯(lián)調(diào)對倒立擺系統(tǒng)的測試是在一個符合要求的環(huán)境

12、中測試的，逐步按要求進行測試。2測試條件與儀器（1）測試條件：22.2V電池供電。（2）測試儀器：高精度的數(shù)字毫伏表，模擬示波器，數(shù)字示波器，數(shù)字萬用表，指針式萬用表，秒表等。3測試結(jié)果A. 基本要求：1）擺桿從處于自然下垂?fàn)顟B(tài)（擺角 0）開始，驅(qū)動電機帶動旋轉(zhuǎn)臂作往復(fù)旋轉(zhuǎn)使擺桿擺動，并盡快使擺角達到或超過-60 +60。測試結(jié)果如下：測試次數(shù)擺角是否達到要求時間（s）1是12是13是14是15是1表4.1 擺腳測試結(jié)果2) 從擺桿處于自然下垂?fàn)顟B(tài)開始，盡快增大擺桿的擺動幅度，直至完成圓周運動。測試結(jié)果如下：測試次數(shù)是否完成圓周運動時間（s）1是22是23是24是25是2表4.2 圓周運動測試

13、結(jié)果3)在擺桿處于自然下垂?fàn)顟B(tài)下，外力拉起擺桿至接近 165位置，外力撤除同時，啟動控制旋轉(zhuǎn)臂使擺桿保持倒立狀態(tài)時間不少于 5s；期間旋轉(zhuǎn)臂的轉(zhuǎn)動角度不大于 90。測試次數(shù)是否倒立倒立保持時間是否超過5s旋轉(zhuǎn)臂轉(zhuǎn)動角度1是是52是是53是是54是是55是是5表4.3 倒立測試結(jié)果B發(fā)揮部分（1）從擺桿處于自然下垂?fàn)顟B(tài)開始，控制旋轉(zhuǎn)臂作往復(fù)旋轉(zhuǎn)運動，盡快使擺桿擺起倒立，保持倒立狀態(tài)時間不少于 10s。測試結(jié)果如下：測試次數(shù)是否倒立倒立保持時間是否達到10s1是是2是是3是是4是是5是是表4.4 倒立測試結(jié)果（2）在擺桿保持倒立狀態(tài)下，施加干擾后擺桿能繼續(xù)保持倒立或 2s 內(nèi)恢復(fù)倒立狀態(tài)。測試結(jié)果

14、如下：測試次數(shù)干擾物抬起角度是否保持倒立15是215是325是435是545是表4.5 干擾下倒立測試結(jié)果（3）在擺桿保持倒立狀態(tài)的前提下，旋轉(zhuǎn)臂作圓周運動，并盡快使單方向轉(zhuǎn)過角度達到或超過 360。測試結(jié)果如下：測試次數(shù)是否倒立狀態(tài)下圓周運動轉(zhuǎn)過角度是否超過360完成時間（s）1是是62是是73是是74是是55是是7表4.6 倒立狀態(tài)下圓周運動測試結(jié)果（4）其他4. 結(jié)果分析根據(jù)上述測試數(shù)據(jù)，可以得出以下結(jié)論：（1）倒立擺系統(tǒng)能夠較好的完成基本功能和發(fā)揮功能。（2）倒立擺系統(tǒng)完成任務(wù)的時間較快。（3）倒立擺系統(tǒng)保持了比較好的穩(wěn)定性。綜上所述，本設(shè)計達到設(shè)計要求。五、參考文獻【1】全國大學(xué)生電

15、子設(shè)計競賽系統(tǒng)設(shè)計（第2版）/ 黃智偉 編著北京航空航天大學(xué)出版社，2011.2【2】ARM嵌入式應(yīng)用技術(shù)基礎(chǔ)/ 劉凱 清華大學(xué)出版社 2009.9【3】ARM嵌入式接口技術(shù)應(yīng)用/劉凱 清華大學(xué)出版社 2009.10【4】全國大學(xué)生電子設(shè)計競賽電路設(shè)計/ 黃智偉 編著北京航空航天大學(xué)出版社，2006.12【5】數(shù)字電子技術(shù)基礎(chǔ)（第五版）/ 閻 石 主編高等教育出版社，2006.514附錄11電路原理圖（1）單片機最小系統(tǒng)附圖1 單片機最小系統(tǒng)（2）電源模塊附圖2 電源模塊（3）電機驅(qū)動模塊附圖3 電機驅(qū)動（4）顯示模塊附圖4 顯示模塊2程序流程圖：（1） 主程序流程圖附圖5 主程序流程圖附錄2

16、：部分源程序#include #include #include all.h/* * addtogroup STM32 */*/extern int rt_application_init(void);#ifdef RT_USING_FINSHextern void finsh_system_init(void);extern void finsh_set_device(const char* device);#endif#ifdef _CC_ARMextern int Image$RW_IRAM1$ZI$Limit;#define STM32_SRAM_BEGIN (&Image$RW_IR

17、AM1$ZI$Limit)#elif _ICCARM_#pragma section=HEAP#define STM32_SRAM_BEGIN (_segment_end(HEAP)#elseextern int _bss_end;#define STM32_SRAM_BEGIN (&_bss_end)#endif/* Function Name : assert_failed* Description : Reports the name of the source file and the source line number* where the assert error has occ

18、urred.* Input : - file: pointer to the source file name* - line: assert error line source number* Output : None* Return : None*/void assert_failed(u8* file, u32 line)rt_kprintf(nr Wrong parameter value detected onrn);rt_kprintf( file %srn, file);rt_kprintf( line %drn, line);while (1) ;/* * This function will startup RT-Thread RTOS. */void rtthread_startup(void)/* init board */rt_hw_board_init();/* show version */rt_show_version();/* init tick */rt_system_tick_init();/* init kernel object */rt_system_object_init();/* init timer system */rt_system_