倒立擺_周長(zhǎng)興組

1、倒立擺學(xué) 校：西安交通大學(xué)作 者：周長(zhǎng)興 萬(wàn) 星 邵永勝指導(dǎo)老師：張鵬輝 王中方 張翠翠摘 要：系統(tǒng)采用stm32f407作為倒立擺系統(tǒng)控制核心，實(shí)現(xiàn)擺的一系列功能。硬件由電源電路、CPU最小系統(tǒng)模塊、電機(jī)驅(qū)動(dòng)模塊、編碼器模塊、顯示模塊等組成。算法采用雙環(huán)PID進(jìn)行控制，內(nèi)環(huán)使用擺桿角度誤差量作為反饋對(duì)電機(jī)速度進(jìn)行控制，外環(huán)使用電機(jī)的角度位置進(jìn)行反饋，與內(nèi)環(huán)級(jí)聯(lián)。測(cè)試表明各項(xiàng)指標(biāo)都符合設(shè)計(jì)任務(wù)要求。關(guān)鍵詞：STM32f407單片機(jī)、雙環(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ì)根據(jù)題目要求，系統(tǒng)主要實(shí)現(xiàn)倒立擺的各項(xiàng)功能，系統(tǒng)框圖如圖1.1所示，可分為電源模塊、CPU最小系統(tǒng)模塊、電機(jī)驅(qū)動(dòng)模塊、編碼器模塊、顯示模塊。圖1.

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

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

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

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

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

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

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

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

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

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

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

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