蔣 龍 羅 亮 王 欣

(西南科技大學(xué)信息工程學(xué)院 四川綿陽 621010)

?

基于積分分離PID控制的自平衡小車設(shè)計(jì)

蔣 龍 羅 亮 王 欣

(西南科技大學(xué)信息工程學(xué)院 四川綿陽 621010)

以STM32F103VET6作為控制核心芯片，采用帶加速度計(jì)的陀螺儀MPU6050構(gòu)成小車姿態(tài)檢測(cè)裝置，采用雙路驅(qū)動(dòng)器TB6612FNG驅(qū)動(dòng)帶數(shù)字編碼器的直流電機(jī)搭建了自平衡小車系統(tǒng)。采用積分分離式PID算法實(shí)現(xiàn)了車身平衡的快速穩(wěn)定控制。經(jīng)實(shí)驗(yàn)證明積分分離式PID算法能夠快速、穩(wěn)定地控制小車系統(tǒng)達(dá)到平衡狀態(tài)。

積分分離式PID 平衡小車 姿態(tài)檢測(cè)

兩輪自平衡小車是一種典型的欠驅(qū)動(dòng)系統(tǒng)(underactuated system)、非完整系統(tǒng)(nonholonomic system)，其核心問題是對(duì)小車的平衡控制和運(yùn)動(dòng)控制[1]。常見的平衡小車控制算法有普通PID算法、模糊控制算法和神經(jīng)網(wǎng)絡(luò)算法等[2]。普通的PID算法能夠有效實(shí)現(xiàn)小車系統(tǒng)的平衡控制，但當(dāng)小車系統(tǒng)的傾角比較大時(shí)，算法速度很容易跟不上，導(dǎo)致小車趨于發(fā)散狀態(tài)，以至于無法維持自身的平衡狀態(tài)；模糊控制算法和神經(jīng)網(wǎng)絡(luò)算法在一定程度上可以解決這一問題，但在線運(yùn)算時(shí)需要控制器提供大量的內(nèi)存存儲(chǔ)數(shù)據(jù)，對(duì)控制器的性能要求特別高。基于以上控制算法的不足，本文采用了積分分離式PID算法對(duì)小車進(jìn)行控制，使其可在大范圍傾角內(nèi)快速達(dá)到平衡，并且該算法占用主控芯片內(nèi)存不大，對(duì)控制器的要求也不高。

1 系統(tǒng)總體設(shè)計(jì)

系統(tǒng)總體設(shè)計(jì)框圖和實(shí)物圖如圖1所示，自動(dòng)平衡小車系統(tǒng)主要由陀螺儀MPU6050、驅(qū)動(dòng)器TB6612FNG、控制器STM32F103ZET6和帶編碼器的直流電機(jī)組成。小車系統(tǒng)采用兩塊7.5 V鋰電池串聯(lián)，利用LM2578和LM2576分別把串聯(lián)電壓轉(zhuǎn)換為12 V和5 V電壓給系統(tǒng)供電。

小車系統(tǒng)的主控制器通過定時(shí)器中斷,利用I2C協(xié)議讀取陀螺儀MPU6050檢測(cè)的姿態(tài)信息，并經(jīng)過主控器的卡爾曼濾波和數(shù)據(jù)融合處理后，與串口通信，把姿態(tài)信息實(shí)時(shí)顯示在上位機(jī)上，采用積分分離式PID算法控制定時(shí)器輸出PWM波形，再利用雙路驅(qū)動(dòng)器TB6612FNG驅(qū)動(dòng)電機(jī)。小車系統(tǒng)采用帶光柵編碼器測(cè)速的直流電機(jī)，通過光柵測(cè)速把電機(jī)速度反饋給控制器，形成負(fù)反饋控制系統(tǒng)。本系統(tǒng)的主控制器通過串口實(shí)時(shí)向上位機(jī)發(fā)送姿態(tài)檢測(cè)信息，同時(shí)也便于積分分離式PID算法的參數(shù)整定；串口通信還可以把控制小車系統(tǒng)穩(wěn)定的時(shí)間，即調(diào)節(jié)時(shí)間顯示在上位機(jī)上，為測(cè)試實(shí)驗(yàn)指標(biāo)提供一定的實(shí)驗(yàn)條件。

圖1 系統(tǒng)總體設(shè)計(jì)框圖和實(shí)物圖Fig.1 Overall system design diagram and physical map

2 硬件設(shè)計(jì)

2.1 主控制器

本系統(tǒng)的主控芯片采用了ST公司的STM32F103ZET6，該主控芯片是擁有64KSRAM，512K嵌入式高速F1ash閃存的Cortex-M3的微控制器，操作頻率可達(dá)72 MHz。STM32F103ZET6支持I2C和USART通信，擁有多達(dá)112個(gè)快速I/O端口，所有I/O口可以映像到16個(gè)外部中斷，幾乎所有端口均可容忍5 V信號(hào)，驅(qū)動(dòng)能力一般為3.3 V。該主控芯片具有多達(dá)11個(gè)定時(shí)器，2個(gè)16位帶死區(qū)控制和緊急剎車的高級(jí)定時(shí)器，能夠產(chǎn)生用于電機(jī)控制的PWM波。

2.2 陀螺儀MPU6050

MPU6050是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀、3軸MEMS加速度計(jì)以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP(Digital Motion Processor)，可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)[3]。MPU6050對(duì)陀螺儀和加速度計(jì)分別用了3個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量，通過I2C協(xié)議與控制器實(shí)時(shí)發(fā)送姿態(tài)信息。

2.3 TB6612FNG模塊接口電路設(shè)計(jì)

TB6612FNG是東芝半導(dǎo)體公司生產(chǎn)的一款直流電機(jī)驅(qū)動(dòng)器件，它具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)2個(gè)電機(jī)。TB6612每通道輸出最高1.2 A的連續(xù)驅(qū)動(dòng)電流，啟動(dòng)峰值電流達(dá)2 A/3.2 A(連續(xù)脈沖/單脈沖)；4種電機(jī)控制模式：正轉(zhuǎn)、反轉(zhuǎn)、制動(dòng)和停止；PWM支持頻率高達(dá)100 kHz；芯片內(nèi)設(shè)計(jì)有低壓檢測(cè)電路與熱停機(jī)保護(hù)電路。TB6612FNG接口電路圖為圖2所示。

圖2 TB6612FNG接口電路圖Fig.2 VTB6612FNG interface circuit

2.4 帶數(shù)字編碼器的直流電機(jī)模塊

本系統(tǒng)采用自帶編碼器測(cè)速碼盤的直流減速電機(jī)，工作電壓為：6～24 V，額定電壓為12 V，最高轉(zhuǎn)速可達(dá)到1 000 r/min，減速比為1:30。編碼器采用448線AB相輸出，448線碼盤保證了測(cè)速精度，也就是電機(jī)轉(zhuǎn)一圈輸出448×30個(gè)脈沖。電機(jī)模塊帶有高速4腳霍爾芯片，保證輸出波形為理想的矩形波，用示波器檢測(cè)相當(dāng)穩(wěn)定，也可以直接接入微處理器觸發(fā)中斷，不需要對(duì)觸發(fā)信號(hào)進(jìn)行任何其他處理。

3 軟件設(shè)計(jì)

3.1 總體軟件設(shè)計(jì)

軟件設(shè)計(jì)的總框圖如圖3所示。

圖3 軟件設(shè)計(jì)的總框圖Fig.3 Overall block diagram of the software design

將自平衡小車軟件設(shè)計(jì)的功能劃分為6個(gè)任務(wù)來完成，分別為：陀螺儀MPU6050的姿態(tài)檢測(cè)信息的獲取、數(shù)據(jù)的濾波和數(shù)據(jù)融合算法的實(shí)現(xiàn)、串口通信協(xié)議的實(shí)現(xiàn)、定時(shí)器1產(chǎn)生PWM波驅(qū)動(dòng)驅(qū)動(dòng)器控制電機(jī)、對(duì)編碼器負(fù)反饋的中斷處理、積分分離PID平衡算法的實(shí)現(xiàn)。

姿態(tài)信息是利用I2C的高速傳輸協(xié)議進(jìn)行加速度和角度位置信息的傳送；為了減小誤差、提高精度，采用了卡爾曼一階濾波對(duì)數(shù)據(jù)進(jìn)行處理，對(duì)多傳感器的數(shù)據(jù)利用數(shù)據(jù)融合算法進(jìn)行數(shù)據(jù)融合，實(shí)現(xiàn)高效且精確地處理多傳感器的效果；采用主控芯片內(nèi)部串口功能，實(shí)現(xiàn)本系統(tǒng)與電腦的通信；驅(qū)動(dòng)電機(jī)是利用高級(jí)定時(shí)器1產(chǎn)生兩路PWM波驅(qū)動(dòng)TB6612FNG來控制電機(jī)；在控制算法上，本系統(tǒng)采用的積分分離PID算法控制在經(jīng)典PID算法的基礎(chǔ)上進(jìn)行一定的優(yōu)化，通過對(duì)姿態(tài)位置信息的校準(zhǔn)，控制電機(jī)的轉(zhuǎn)速大小和方向，實(shí)現(xiàn)系統(tǒng)快速達(dá)到平衡，并維持平衡狀態(tài)。

3.2 積分分離PID控制算法

PID控制器主要由比例單元(P)、積分單元(I)和微分單元(D)組成，如公式(1)：

(1)

對(duì)于傳統(tǒng)PID控制算法，3個(gè)調(diào)節(jié)參數(shù)對(duì)系統(tǒng)分別起到不同的作用和控制效果。比例系數(shù)只與偏差成正比，可迅速反應(yīng)誤差，從而減小穩(wěn)態(tài)偏差，但其無法消除偏差；積分控量以消除誤差；加大比例和積分系數(shù)，均會(huì)使系統(tǒng)出現(xiàn)不穩(wěn)定，超調(diào)增大或者發(fā)生震蕩，通過微分控制則可以減小超調(diào)，克服振蕩，提高系統(tǒng)的穩(wěn)定性，也加快系統(tǒng)的動(dòng)態(tài)響應(yīng)速度，縮短調(diào)整過程，改善動(dòng)態(tài)性能。在一般的PID控制系統(tǒng)中，當(dāng)開工、停工或大幅度下降給定值時(shí)，由于短時(shí)間內(nèi)產(chǎn)生很大偏差，加上系統(tǒng)有滯后，常會(huì)產(chǎn)生嚴(yán)重的積分飽和現(xiàn)象，這往往造成很大的超調(diào)和長時(shí)間的振蕩[4-5]。

針對(duì)平衡小車實(shí)際運(yùn)行情況，本文主要采用的是積分分離式PID控制算法[6]。積分分離式PID控制算法如式(2)所示：

(2)

上式中，β稱為邏輯系數(shù)，當(dāng)|ek|≤x時(shí)，β=1，采用PID控制；當(dāng)|ek|>x時(shí)，β=0；采用PD控制。對(duì)姿態(tài)檢測(cè)模塊采用的積分分離式PID控制原理如圖4(a)所示，通過陀螺儀檢測(cè)輸入角速度和角加速度數(shù)據(jù)融合后，比較誤差大小，選擇邏輯系數(shù)為1還是為0，即選用PID控制輸出或PD控制輸出給小車系統(tǒng)。

圖4 積分分離式PID控制原理圖Fig.4 Separate integral PID control schematic

對(duì)伺服電機(jī)驅(qū)動(dòng)模塊采用的積分分離式PID控制原理圖如圖4(b)所示，通過電機(jī)驅(qū)動(dòng)器輸入電壓驅(qū)動(dòng)電機(jī)，測(cè)量光柵測(cè)速編碼器轉(zhuǎn)速矢量，比較干擾誤差的大小，選擇邏輯系數(shù)為1還是為0，即選用PID控制輸出或PD控制輸出控制量給直流電機(jī)。

由于本系統(tǒng)有姿態(tài)角度信息和速度信息兩個(gè)負(fù)反饋誤差[7]，為了更好地減小誤差對(duì)本系統(tǒng)的影響和更進(jìn)一步地優(yōu)化系統(tǒng)的穩(wěn)定性，本系統(tǒng)對(duì)伺服電機(jī)驅(qū)動(dòng)模塊和姿態(tài)檢測(cè)模塊均采用積分分離式PID算法，積分分離式PID控制算法的程序框圖如圖5所示，給定值為Ak，測(cè)量值為Yk，計(jì)算出誤差值ek，控制器進(jìn)行判斷處理，判斷是進(jìn)行PD控制還是PID控制。

有時(shí)候控制系統(tǒng)輸入量會(huì)發(fā)生較大抖動(dòng)，如果僅是采用PD算法，PD計(jì)算出來的電機(jī)控制就會(huì)大于電機(jī)可控范圍，而采用積分分離式PID則可以解決輸出不可控的情況。而且本系統(tǒng)采用雙積分分離式PID優(yōu)化算法，很大程度上提高了對(duì)平衡小車的穩(wěn)定性控制精度。

4 系統(tǒng)測(cè)試

實(shí)驗(yàn)條件為在平整的地面改變小車的傾角大小，通過串口通信實(shí)時(shí)接收傾角信息和時(shí)間信息。每個(gè)傾角測(cè)試50次，記錄成功平衡次數(shù)，取50次達(dá)到平衡穩(wěn)定所需的時(shí)間為最終控制的時(shí)間，測(cè)試誤差帶取3%誤差帶，即本小車系統(tǒng)達(dá)到目標(biāo)角的正負(fù)1.5°左右，視為小車系統(tǒng)已經(jīng)達(dá)到平衡狀態(tài)。因安裝姿態(tài)檢測(cè)模塊時(shí)沒有實(shí)現(xiàn)完全水平，本小車系統(tǒng)平衡穩(wěn)定時(shí)檢測(cè)的目標(biāo)角為0.7°左右。

圖5 積分分離式PID控制算法的程序框圖Fig.5 Separate integral PID control algorithm block diagram

如圖6所示，該圖為小車達(dá)到平衡狀態(tài)時(shí)位置狀態(tài)信息，X為水平面的旋轉(zhuǎn)角速度，Y為垂直平面的旋轉(zhuǎn)角度，Z為豎直平面的旋轉(zhuǎn)角度。試驗(yàn)測(cè)試數(shù)據(jù)如表1所示，從表中數(shù)據(jù)可以看到，采用積分分離式PID的平衡控制算法，平衡小車可以在大角度干擾下，快速地達(dá)到相對(duì)平衡狀態(tài)，且效果顯著。

圖6 串口實(shí)時(shí)顯示小車位置信息Fig.6 Serial Real-time display car location information

初始傾角/(°)10-1015-1520-2025-25平衡時(shí)間/s1.21.21.41.53.23.56.16.8成功次數(shù)/次5050504948484645成功率/%1001001009896969290

5 結(jié)論

采用STM32F103和MPU6050陀螺儀設(shè)計(jì)的自平衡小車，利用積分分離式PID控制算法進(jìn)行控制，小車能夠快速并穩(wěn)定的保持平衡狀態(tài)，可控角度變化范圍大。該系統(tǒng)具有一定的抗干擾性，即在一定的干擾下也可以保持平衡。試驗(yàn)結(jié)果表明本系統(tǒng)穩(wěn)定性較好，在維持平衡狀態(tài)的情況下，還具有一定的負(fù)載能力。

[1] 鄭曉偉,陳萬米.基于DSP的兩輪自平衡小車系統(tǒng)的研究[J].工業(yè)控制計(jì)算機(jī),2012,25(9):33-35.

[2] 黃亞新,耿安坤,陳樓,等.兩輪自平衡小車的PID控制[J].電子世界,2012,15(3):81-82.

[3] 丁鳳.一種新型兩輪自平衡小車的建模與控制[D].武漢:華中科技大學(xué),2012.

[4] 鄧先智.Smith-RBF-PID控制算法在工業(yè)平縫機(jī)腳踏板調(diào)速模塊中的應(yīng)用[J].西南科技大學(xué)學(xué)報(bào),2014,29(1):65-69.

[5] 楊久紅,王小增.積分分離PID算法的電阻爐溫度控制系統(tǒng)[J].計(jì)算機(jī)測(cè)量與控制,2012,20(1):66-69.

[6] 湯進(jìn)舉.注塑機(jī)微控制系統(tǒng)研究[D].浙江:浙江大學(xué),2002.

[7] 畢效輝,于春梅.自動(dòng)控制原理[M].北京：科學(xué)出版社,2008. 19-46.

Integral Separation PID Control Based on Self-balanced Car Design

JIANG Long, LUO Liang, WANG Xin

(SchoolofInformationScienceandEngineering,SouthwestUniversityofScienceandTechnology,Mianyang621010,Sichuan,China)

Based STM32F103VET6 control core chip, with accelerometer gyroscope MPU6050 constitute trolley posture detection device, with dual drive TB6612FNG drive with digital encoders DC motor a self-balanced car system was built. Integral separate PID algorithm was used to achieve a rapid and stable control of body balance. The experimental testing indicate integral separation PID algorithm can quickly and stably control the car system to reach equilibrium.

Integral separate PID; Balanced car; Attitude detection

2015-02-11

西南科技大學(xué)教改試點(diǎn)班“電子設(shè)計(jì)競(jìng)賽大學(xué)生創(chuàng)新實(shí)踐班”項(xiàng)目資助(190004)。

蔣龍(1993—)，男，本科生，專業(yè)方向?yàn)樽詣?dòng)化。E-mail:1417700745@qq.com

TP13

A

1671-8755(2015)02-0088-05