曾憲陽(yáng)，楊紅莉，郁漢琪，褚南峰

(1.南京工程學(xué)院 工業(yè)中心，江蘇 南京 211167；2.南京大學(xué) 數(shù)學(xué)系，江蘇 南京 210093；3.南京工程學(xué)院 數(shù)理部，江蘇 南京211167；4.南京大學(xué) 大氣科學(xué)學(xué)院，江蘇 南京 210093)

基于單片機(jī)控制的旋轉(zhuǎn)倒立擺建模分析與系統(tǒng)設(shè)計(jì)*

曾憲陽(yáng)1，2，楊紅莉3，4，郁漢琪1，褚南峰1

(1.南京工程學(xué)院 工業(yè)中心，江蘇 南京 211167；2.南京大學(xué) 數(shù)學(xué)系，江蘇 南京 210093；3.南京工程學(xué)院 數(shù)理部，江蘇 南京211167；4.南京大學(xué) 大氣科學(xué)學(xué)院，江蘇 南京 210093)

旋轉(zhuǎn)倒立擺結(jié)構(gòu)簡(jiǎn)單，但控制系統(tǒng)較為復(fù)雜。介紹了旋轉(zhuǎn)倒立擺的結(jié)構(gòu)原理，運(yùn)用分析力學(xué)中的 Lagrange方程建立了旋轉(zhuǎn)倒立擺的線性數(shù)學(xué)模型，推導(dǎo)出其公式及較為準(zhǔn)確的狀態(tài)方程描述，分析了旋轉(zhuǎn)倒立擺的不穩(wěn)定性和可控性。設(shè)計(jì)了控制系統(tǒng)硬件電路及軟件程序編寫(xiě)，對(duì)系統(tǒng)整個(gè)控制過(guò)程進(jìn)行了數(shù)據(jù)采集、圖形展示，分析了擺桿從自然下垂到擺起180°實(shí)現(xiàn)倒立的整個(gè)過(guò)程。系統(tǒng)采用離散PID調(diào)節(jié)算法進(jìn)行控制，保證了系統(tǒng)的穩(wěn)定性，融入卡爾曼濾波算法濾除可能出現(xiàn)的干擾信號(hào)，保證了數(shù)據(jù)的準(zhǔn)確性。

旋轉(zhuǎn)倒立擺；Lagrange；數(shù)學(xué)模型；卡爾曼濾波；PID調(diào)節(jié)

0　引言

倒立擺是一種研究控制理論的理想平臺(tái)，其結(jié)構(gòu)簡(jiǎn)單、成本低，是一個(gè)典型的多變量、快速、非線性和自然不穩(wěn)定系統(tǒng)。旋轉(zhuǎn)倒立擺在外力的作用下，從一個(gè)穩(wěn)定的平衡狀態(tài)自動(dòng)轉(zhuǎn)移到另一個(gè)平衡狀態(tài)的過(guò)程中，既要求起擺迅速，又要求在到達(dá)新的平衡點(diǎn)時(shí)能快速進(jìn)行穩(wěn)擺控制[1-4]。

圖1為旋轉(zhuǎn)倒立擺結(jié)構(gòu)圖。旋轉(zhuǎn)倒立擺的原理為：當(dāng)電機(jī)帶動(dòng)旋轉(zhuǎn)臂在水平平面內(nèi)來(lái)回轉(zhuǎn)動(dòng)時(shí)，擺桿由于慣性將會(huì)繞轉(zhuǎn)軸2在豎直平面內(nèi)來(lái)回?cái)[動(dòng)，當(dāng)能量達(dá)到一定數(shù)值時(shí)，擺動(dòng)幅度將超過(guò)180°形成倒立狀態(tài)。通過(guò)單片機(jī)系統(tǒng)控制擺桿迅速倒立是本文研究的主要內(nèi)容。首先建立數(shù)學(xué)模型，然后設(shè)計(jì)單片機(jī)軟、硬件系統(tǒng)，最后進(jìn)行調(diào)試分析，形成旋轉(zhuǎn)倒立擺單片機(jī)控制系統(tǒng)。

1　系統(tǒng)數(shù)學(xué)模型創(chuàng)建

建立數(shù)學(xué)模型前，設(shè)空氣阻力忽略不計(jì)。系統(tǒng)由電機(jī)和質(zhì)量均勻的擺桿m組成，擺桿質(zhì)心到轉(zhuǎn)軸2的距離為L(zhǎng)，轉(zhuǎn)軸2軸心到轉(zhuǎn)軸1軸心距離為r，某時(shí)刻旋轉(zhuǎn)臂在水平面上轉(zhuǎn)動(dòng)角度為θ，擺桿在豎直平面內(nèi)轉(zhuǎn)動(dòng)角度為α，設(shè)擺桿和旋轉(zhuǎn)臂的角度以逆時(shí)針?lè)较驗(yàn)檎齕5]。其受力分析如圖2所示。

圖1　旋轉(zhuǎn)倒立擺結(jié)構(gòu)圖

圖2　旋轉(zhuǎn)倒立擺受力分析圖

旋轉(zhuǎn)臂和擺桿一起動(dòng)作，擺桿質(zhì)心x、y方向的速度分量分別如下：

系統(tǒng)總動(dòng)能T由4個(gè)部分組成：旋臂水平面內(nèi)的轉(zhuǎn)動(dòng)動(dòng)能、擺桿豎直平面內(nèi)的轉(zhuǎn)動(dòng)動(dòng)能、擺桿質(zhì)心沿x軸方向和y軸方向的動(dòng)能。由此可得到Lagrange函數(shù)：

在倒立擺系統(tǒng)中 i=1，2，q=(θ，α)，θ為旋臂角位移，α為擺桿角位移，Qi為系統(tǒng)沿該廣義坐標(biāo)方向上的外力，得到以下方程組：

令 α=x1，=x2，θ=y1，=y2，得出系統(tǒng)線性化狀態(tài)方程：

根據(jù)表1相關(guān)參數(shù)，當(dāng)旋轉(zhuǎn)倒立擺系統(tǒng)靠近平衡點(diǎn)時(shí)，得到如下線性化數(shù)學(xué)模型：

表1　相關(guān)物理參數(shù)表

2　控制系統(tǒng)硬件電路設(shè)計(jì)

2.1控制系統(tǒng)總體框圖

控制系統(tǒng)主要由單片機(jī)系統(tǒng)、驅(qū)動(dòng)模塊、直流減速電機(jī)、角度傳感器、LCD液晶顯示模塊等組成。單片機(jī)通過(guò)實(shí)時(shí)采集角度傳感器數(shù)據(jù)，經(jīng)分析得出擺桿當(dāng)前狀態(tài)，通過(guò)計(jì)算得出相應(yīng)控制量來(lái)控制電機(jī)轉(zhuǎn)動(dòng)，從而改變擺桿狀態(tài)，使其達(dá)到預(yù)期控制的目的。系統(tǒng)總體框圖如圖3所示。

圖3　旋轉(zhuǎn)倒立擺系統(tǒng)總體框圖

2.2控制系統(tǒng)電路原理圖

圖4為控制系統(tǒng)傳感器及驅(qū)動(dòng)原理圖。為保證控制精度和速度，采用增強(qiáng)型 51單片機(jī) STC12C5A60S2[6]作為控制核心；電機(jī)驅(qū)動(dòng)模塊采用L298N大電流電機(jī)驅(qū)動(dòng)芯片，由單片機(jī)P2口部分口線控制；角度傳感器采用MPU6050六軸運(yùn)動(dòng)處理組件，能檢測(cè)擺桿在垂直方向上的角度值，由P1口部分口線控制，中斷引腳接至單片機(jī)外部中斷0引腳。此外電路還包括LCD12864顯示電路及按鍵電路，用來(lái)實(shí)現(xiàn)人機(jī)交互及當(dāng)前狀態(tài)顯示。

圖4　系統(tǒng)傳感器及驅(qū)動(dòng)電路原理圖

3　控制系統(tǒng)軟件設(shè)計(jì)

3.1資源分配及程序流程

系統(tǒng)主要控制任務(wù)包括：角度數(shù)據(jù)采集與處理、電機(jī)驅(qū)動(dòng)控制、按鍵輸入讀取、數(shù)據(jù)顯示等。其中數(shù)據(jù)顯示任務(wù)為非緊急事件，由主程序循環(huán)調(diào)用即可完成；角度數(shù)據(jù)采集與處理、電機(jī)驅(qū)動(dòng)控制任務(wù)具有連貫性及較高的實(shí)時(shí)性，屬于周期性的緊急事件，應(yīng)由定時(shí)器T0中斷服務(wù)子程序一并完成；按鍵輸入讀取任務(wù)屬于隨機(jī)性的緊急事件，應(yīng)由外部中斷0子程序來(lái)完成。

定時(shí)器T0中斷服務(wù)子程序流程圖如圖5所示，主要執(zhí)行讀取傳感器數(shù)據(jù)、卡爾曼濾波、PID調(diào)節(jié)任務(wù)，為避免中斷程序溢出，此三項(xiàng)任務(wù)的執(zhí)行時(shí)間應(yīng)小于5 ms，因此單片機(jī)外部晶振頻率選為 24 MHz方能滿足速度需要。

圖5　定時(shí)器T0中斷子程序流程圖

3.2控制調(diào)節(jié)算法

PID調(diào)節(jié)是按偏差的比例、積分、微分進(jìn)行控制的一種閉環(huán)調(diào)節(jié)控制，原理框圖如圖6所示，其中 r(t)為系統(tǒng)給定值，c(t)為實(shí)際輸出，u(t)為輸出控制量。引入PID調(diào)節(jié)算法可保證系統(tǒng)處于閉環(huán)控制狀態(tài)，并且能讓系統(tǒng)快速準(zhǔn)確達(dá)到設(shè)置的平衡狀態(tài)[7-11]。

圖6　模擬PID控制器的原理框圖

系統(tǒng)采用計(jì)算量較小的增量式PID算法，由模擬PID離散化得到在k-1時(shí)刻的輸出：

其中，T為采樣時(shí)間。單片機(jī)控制系統(tǒng)將采用恒定的采樣周期 T，一旦確定 A、B、C，只要使用前后 3次測(cè)量的偏差值，就可以求出控制量。

3.3系統(tǒng)抗干擾設(shè)計(jì)

由于角度傳感器采集到的數(shù)據(jù)含有噪聲信號(hào)，主要來(lái)自外界電磁干擾及角度傳感器本身的噪聲。在軟件系統(tǒng)中采用卡爾曼濾波算法[12-15]將噪聲濾去。設(shè)采樣率為200 Hz，對(duì)擺桿擺起發(fā)整個(gè)過(guò)程進(jìn)行角度數(shù)據(jù)采集，經(jīng)卡爾曼濾波前后圖像對(duì)比如圖7所示,其展示了擺桿從自然下垂?fàn)顟B(tài)開(kāi)始實(shí)現(xiàn)倒立的整過(guò)過(guò)程。擺桿靜止時(shí)刻自然下垂為平衡狀態(tài)Ⅰ，與垂直方向夾角為0°(圖7(b)中2段所示)。擺桿在電機(jī)帶動(dòng)下獲取一定能量來(lái)回振蕩(圖7(b)中3所示），能量達(dá)到一定程度擺桿越過(guò)180°實(shí)現(xiàn)倒立，經(jīng)過(guò)短時(shí)間幾次小的振蕩后(圖7(b)中 4所示)，到達(dá)新的平衡狀態(tài)Ⅱ(圖7(b)中 5所示)，實(shí)現(xiàn)了擺桿倒立。停止電機(jī)控制，擺桿將回到原平衡狀態(tài)Ⅰ，并在0°附近自由振蕩幾次后達(dá)到靜止(圖7(b)中 6所示)。由于卡爾曼濾波在初始時(shí)刻濾波誤差較大，屬于盲區(qū)(圖7(b)中1所示)，軟件編程時(shí)應(yīng)延時(shí)幾秒以避開(kāi)盲區(qū)。

3.4主要程序源碼

定時(shí)器 T0中斷子程序代碼如下所示[16]：

4　測(cè)試結(jié)果與結(jié)論

表2為擺起模式運(yùn)行結(jié)果測(cè)量表。通過(guò)按鍵設(shè)置擺起模式，分別選擇擺起角度為 45°～180°之間的數(shù)據(jù)，擺桿會(huì)在電機(jī)帶動(dòng)下做往復(fù)擺動(dòng)，擺角很快達(dá)到超過(guò)-60°～+60°。當(dāng)擺桿角度接近180°時(shí)，通過(guò)PID調(diào)節(jié)，迅速使擺桿保持穩(wěn)定倒立狀態(tài)，時(shí)間維持5 s以上。通過(guò)一系列實(shí)驗(yàn)驗(yàn)證，該旋轉(zhuǎn)倒立擺系統(tǒng)能完成任意角度的擺起，并能實(shí)現(xiàn)快速倒立，系統(tǒng)能很好地滿足設(shè)計(jì)要求。

表2　設(shè)定擺起模式運(yùn)行結(jié)果測(cè)量表

[1]王家軍，劉棟良，王寶軍.X-Z倒立擺的一種飽和非線性穩(wěn)定控制方法的研究[J].自動(dòng)化學(xué)報(bào)，2013，39(1)：92-96.

[2]趙建軍，魏毅，夏時(shí)洪，等.基于二階倒立擺的人體運(yùn)動(dòng)合成[J].計(jì)算機(jī)學(xué)報(bào)，2014，37(10)：2188-2195.

[3]周昆，孫明瑋，陳增強(qiáng).快速顯式預(yù)測(cè)控制在倒立擺系統(tǒng)中的應(yīng)用[J].儀器儀表學(xué)報(bào)，2014，35(9)：2037-2044.

[4]李雪冰，馬莉，丁世宏.一類新的二階滑?？刂品椒捌湓诘沽[控制中的應(yīng)用[J].自動(dòng)化學(xué)報(bào)，2015，41(1)：194-202.

[5]吳愛(ài)國(guó)，張小明，張釗.基于Lagrange方程建模的單級(jí)旋轉(zhuǎn)倒立擺控制[J].中國(guó)工程科學(xué)，2005，7(10)：11-15.

[6]南通國(guó)芯.STC12C5A60S2+系列單片機(jī)手冊(cè)[DB/OL].http://www.stcmcu.com.

[7]曹敏，徐凌樺.單神經(jīng)元PID算法在倒立擺控制系統(tǒng)中的應(yīng)用[J].微計(jì)算機(jī)信息，2009，25(1-1)：70-71.

[8]楊智，朱海鋒，黃以華.PID控制器設(shè)計(jì)與參數(shù)整定方法綜述[J].化工自動(dòng)化及儀表，2005，32(5)：1-7.

[9]王述彥，師宇，馮忠緒.基于模糊 PID控制器的控制方法研究[J].機(jī)械科學(xué)與技術(shù)，2011，30(1)：166-172.

[10]陳新海，李言俊.自適應(yīng)控制及應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，1998.

[11]王立新.模糊系統(tǒng)與模糊控制教程[M].北京：清華大學(xué)出版社，2003.

[12]付夢(mèng)印，鄧志紅，閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用(第二版)[M].北京：科學(xué)出版社，2010.

[13]DAUM F.Nonlinear filters beyond the Kalman filter[J].IEEE A&E Systems Magazine，2005，20(8)：57-69.

[14]KIM J，VADDI S S，MENON P K，et al.Comparison between nonlinear filtering techniques forspiraling ballistic missile state estimation[J].IEEE Transactions on Aerospace and Elec-tronic Systems，2012，48(1)：313-328.

[15]KALMAN R E.A new approach to linear filtering and prediction problems[J].Transactions of the AMSE Journal of Basic Engineering，1960，82：35-45.

[16]譚浩強(qiáng).C語(yǔ)言程序設(shè)計(jì)(第三版)[M].北京：清華大學(xué)出版社，2005.

The modeling analysis and system design of rotational inverted pendulum based on single chip microcomputer control

Zeng Xianyang1，2，Yang Hongli3，4，Yu Hanqi1，Chu Nanfeng1
(1.Industry Center，Nanjing Institute of Technology，Nanjing 211167，China；2.Department of Mathematics，Nanjing University，Nanjing 210093，China；3.Department of Mathematics and Physics，Nanjing Institute of Technology，Nanjing 211167，China；4.School of Atmospheric Sciences，Nanjing University，Nanjing 210093，China)

Structure of rotational inverted pendulum is simple,but the control system is more complicated.This paper introduces the structure of rotational inverted pendulum,Lagrange equation in analytical mechanics linear mathematical model of rotational inverted pendulum is established,the formula and a more accurate equation of state description are deduced,and the instability and controllability of rotational inverted pendulum are analyzed.The control system hardware circuit and software programming are designed.The control process of the system for data acquisition,graphic display,swinging rod is analyzed from natural prolapse to set up the whole process of 180°handstand.System control uses the discrete PID algorithm to ensure the stability of the system.And the infusion of the Kalman filtering algorithm filters the possible jamming signal,which ensures the accuracy of the data.

rotational inverted pendulum;Lagrange；mathematical model；Kalman filter；PID control

TK323

A

10.16157/j.issn.0258-7998.2016.09.020

南京工程學(xué)院青年基金重點(diǎn)項(xiàng)目（QKJA201404）；南京工程學(xué)院 2014年校級(jí)教學(xué)改革項(xiàng)目（JG201440）；南京工程學(xué)院創(chuàng)新基金重大項(xiàng)目（CKJA201410）

2016-04-27)(

2016-05-16)

曾憲陽(yáng)（1979-），男，博士研究生，主要研究方向：電子技術(shù)應(yīng)用、控制系統(tǒng)及數(shù)學(xué)建模。

楊紅莉（1980-），通信作者，女，博士后，主要研究方向：計(jì)算數(shù)學(xué)，E-mail：yanghongli1016@163.com。

郁漢琪（1961-），男，碩士，教授，主要研究方向：電子技術(shù)應(yīng)用。

中文引用格式：曾憲陽(yáng)，楊紅莉，郁漢琪，等.基于單片機(jī)控制的旋轉(zhuǎn)倒立擺建模分析與系統(tǒng)設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2016，42(9)：76-79.

英文引用格式：Zeng Xianyang，Yang Hongli，Yu Hanqi，et al.The modeling analysis and system design of rotational inverted pendulum based on single chip microcomputer control[J].Application of Electronic Technique，2016，42(9)：76-79.