基于STM32的兩輪自平衡小車系統(tǒng)設(shè)計

王素青， 熊維堂

(南京航空航天大學(xué) 金城學(xué)院，江蘇 南京 211156)

?

基于STM32的兩輪自平衡小車系統(tǒng)設(shè)計

王素青， 熊維堂

(南京航空航天大學(xué) 金城學(xué)院，江蘇 南京 211156)

針對小車控制系統(tǒng)的復(fù)雜性，設(shè)計一個以STM32F103C8T6微處理器為主控制器，以MPU-6050傳感器為姿態(tài)檢測部件的自平衡小車系統(tǒng)。由于陀螺儀和加速度計在測量時存在噪聲干擾和隨機漂移誤差，采用卡爾曼濾波算法對陀螺儀和加速度計數(shù)據(jù)進行融合，補償傳感器測量誤差，計算出小車傾角與角速度的最優(yōu)估計值。并以最優(yōu)姿態(tài)角和小車速度為反饋量構(gòu)成雙閉環(huán)控制，利用PID控制算法實現(xiàn)小車系統(tǒng)的自平衡控制。通過系統(tǒng)的軟硬件設(shè)計、調(diào)試及運行情況，證明自平衡小車能夠穩(wěn)定地實現(xiàn)自平衡控制。

兩輪自平衡小車； STM32； MPU-6050傳感器； 卡爾曼濾波； PID控制

0 引 言

兩輪自平衡小車系統(tǒng)類似于倒立擺系統(tǒng)，具有多變量、非線性、強耦合等特點，是研究各種控制方法的理想平臺[1]。兩輪自平衡小車系統(tǒng)的控制過程是微控制器對姿態(tài)檢測傳感器和編碼器等采集的數(shù)據(jù)進行分析處理，計算出使系統(tǒng)恢復(fù)平衡的實時控制量，從而驅(qū)動電機實現(xiàn)系統(tǒng)的動態(tài)平衡。要控制小車系統(tǒng)的精度及穩(wěn)定性其關(guān)鍵是小車姿態(tài)信息采集的準確性和實時性，以及小車速度測量的準確性和實時性。針對自平衡小車系統(tǒng)的復(fù)雜性，國內(nèi)外很多學(xué)者對其控制方法進行了大量的研究，如狀態(tài)反饋控制器(LQR)控制法[2]、分層模糊控制法、卡爾曼濾波算法[3]等。針對小車系統(tǒng)的復(fù)雜性，本文提出了將卡爾曼濾波算法和雙閉環(huán)PID控制算法相結(jié)合的方法，既利用卡爾曼濾波算法對MPU-6050傳感器采集的傾斜角度和角速度數(shù)據(jù)進行融合，得到小車平衡姿態(tài)的最優(yōu)估計值，又利用以姿態(tài)信息、速度為反饋控制量構(gòu)成雙閉環(huán)PID控制算法，再結(jié)合采用高性能STM32F103C8T6作為主控制器，從而提高兩輪小車系統(tǒng)穩(wěn)定性和抗干擾能力。

1 系統(tǒng)的硬件設(shè)計

系統(tǒng)的硬件電路結(jié)構(gòu)框圖如圖1所示，其硬件電路主要由主控制器模塊、姿態(tài)檢測模塊、直流電機驅(qū)動模塊、編碼器及電源模塊等幾部分構(gòu)成。電源模塊負責系統(tǒng)各個模塊控制電路的電源。主控制器模塊是系統(tǒng)的控制核心，用來接收傳感器模塊采集的數(shù)據(jù)，對數(shù)據(jù)進行處理及運算，將控制信號輸出給直流電機驅(qū)動模塊。姿態(tài)檢測模塊實時地對小車角速度及角加速度進行采集。直流電機驅(qū)動模塊負責將主控制器輸出的PWM信號轉(zhuǎn)換為控制信號驅(qū)動2個直流電機的轉(zhuǎn)速和方向。編碼器負責測量直流電機的速度和方向，將測量數(shù)據(jù)反饋給主控制器，從而形成閉環(huán)控制。

圖1 系統(tǒng)的硬件電路結(jié)構(gòu)框圖

1.1 電源模塊

本系統(tǒng)選用的直流電機工作電壓為12 V，因此采用三節(jié)3.7 V可充電電池構(gòu)成12 V直流電源。但STM32主控制器和MPU-6050需要的是3.3 V直流電源，因此選用LM2596S DC-DC降壓模塊得到3.3 V電源。

1.2 主控制器模塊

本系統(tǒng)選用意法半導(dǎo)體公司的STM32F103C8T6作為主控制器，該控制器是一種基于Cortex-M3內(nèi)核的32 b CPU，最高工作頻率為72 MHz，片上集成32～512 kB的Flash存儲器，6～64 kB的SRAM存儲器。片上集成了SPI，I2C和USART等通信接口，方便對MPU-6050姿態(tài)檢測傳感器的數(shù)據(jù)進行采集。含有7個定時器，可輸出多路PWM信號使得電機驅(qū)動模塊獲得穩(wěn)定的PWM波形。

1.3 姿態(tài)檢測模塊

本系統(tǒng)中姿態(tài)檢測模塊選用的是美國InvenSense公司的MPU-6050，MPU-6050為全球首例整合性6軸運動處理器，整合了3軸陀螺儀和3軸加速度計，能夠準確的對小車的姿態(tài)進行實時檢測[4]。該處理器的角速度感測范圍為±250、±500、±1 000與±2 000°/s，加速度感測范圍為±2 g、±4 g、±8 g與±16 g，加速度感測范圍可通過編程來控制[5]。MPU-6050可在不同電壓下工作，VDD供電電壓為(2.5±0.125) V、(3.0±0.15) V或(3.3±0.165) V，本系統(tǒng)中為MPU-6050提供的是3.3 V直流電源。MPU-6050模塊的電路如圖2所示，本系統(tǒng)中MPU-6050作為從機使用，使用SDA和SCL端口和主機STM32進行通信，MPU-6050檢測的數(shù)據(jù)經(jīng)過內(nèi)置的16位AD轉(zhuǎn)換器進行轉(zhuǎn)換，然后將16位數(shù)字量通過I2C總線接口發(fā)送給主機。

圖2 MPU-6050模塊的電路

1.4 電機驅(qū)動模塊

本系統(tǒng)中直流電機驅(qū)動模塊設(shè)計采用ST公司的L298N組成的雙H橋電機驅(qū)動電路，其電路如圖3所示。該模塊具有驅(qū)動能力強，發(fā)熱量低，抗干擾能力強等特點。可以同時驅(qū)動2臺直流電機，當圖3中的使能端ENA和ENB使能后，從IN1、IN2輸入的PWM信號驅(qū)動電機1的轉(zhuǎn)速和方向，從IN3、IN4輸入的PWM信號驅(qū)動電機2的轉(zhuǎn)速和方向。

2 系統(tǒng)的軟件設(shè)計

2.1 系統(tǒng)的總體流程設(shè)計

本系統(tǒng)的主程序流程圖如圖4所示，主要由初始化程序、中斷響應(yīng)程序、傳感器數(shù)據(jù)采集程序、卡爾曼濾波程序、車速及方向檢測程序、PID控制程序、直流電機PWM控制程序等幾部分組成。

系統(tǒng)啟動后，首先進行初始化，然后執(zhí)行中斷程序，判斷5 ms時間是否到，若時間到，則讀取MPU-6050傳感器采集的角度和角速度的數(shù)據(jù)，然后利用卡爾曼濾波算法對讀取的數(shù)據(jù)進行融合得到最優(yōu)估計值。同樣采用中斷的方式讀取編碼器的脈沖數(shù)從而得到直流電機的轉(zhuǎn)速以及方向。然后利用增量式PID控制算法分別對小車的角度和角速度、轉(zhuǎn)速進行PID控制，調(diào)節(jié)PID參數(shù)即可改變PWM占空比，利用改變占空比的方法即可改變電機的速度，從而實現(xiàn)對電機速度和方向的控制[6]。

圖3 直流電機驅(qū)動模塊的電路

圖4 系統(tǒng)的主程序流程圖

2.2 系統(tǒng)初始化程序

系統(tǒng)初始化程序主要包括：系統(tǒng)時鐘初始化、NVIC中斷初始化、定時器外部中斷初始化、通信(I2C)初始化、測速模塊初始化、MPU-6050初始化和電機PWM初始化等。

STM32的工作頻率為72 MHz或36 MHz，本系統(tǒng)對時鐘頻率的要求很苛刻，如果頻率過低，系統(tǒng)就會工作在不穩(wěn)定狀態(tài)下，系統(tǒng)產(chǎn)生的誤差會變大，又因系統(tǒng)使用的濾波方式是卡爾曼濾波，因此在時鐘初始化程序中需將時鐘頻率設(shè)置為72 MHz。

通信(I2C)初始化主要設(shè)置串口波特率為9 600 b/s，需設(shè)置中斷允許標志位和模塊的工作模式。

2.3 卡爾曼濾波數(shù)據(jù)融合算法

2.3.1 卡爾曼濾波算法

陀螺儀動態(tài)性能較好，可以提供瞬間的動態(tài)角度變化，但存在輸出信號有偏差或漂移，經(jīng)過積分后形成逐漸增大的累積誤差，不適合單獨進行長時間的動態(tài)角度測量。加速度計靜態(tài)性能較好，可以提供較為準確的靜態(tài)角度值，但受重力加速度影響比較大，不適宜測量動態(tài)系統(tǒng)的角度值[7]。由于自平衡小車系統(tǒng)需要在動靜態(tài)情況下均能準確的測量出小車的傾角，因此需要對陀螺儀和加速度計的采集數(shù)據(jù)進行數(shù)據(jù)融合得到最優(yōu)值。本系統(tǒng)采用卡爾曼濾波算法對陀螺儀和加速度計的數(shù)據(jù)進行融合。

卡爾曼濾波算法是以最小均方誤差為最佳估計準則的遞歸數(shù)據(jù)處理算法，利用前一時刻的估計值和當前時刻的測量值來更新對狀態(tài)變量的估計，最終求出當前時刻的最優(yōu)估計值。

2.3.2 卡爾曼濾波器設(shè)計

根據(jù)卡爾曼濾波原理，要估計k時刻的傾角，需要根據(jù)k-1時刻的傾角值來預(yù)測k時刻的傾角，并得到k時刻的高斯噪聲偏差。本系統(tǒng)中，陀螺儀測量的角速度作為預(yù)測值的控制量，加速度計測量的角度作為測量值，根據(jù)兩者的協(xié)方差不斷進行遞歸算法直到估計出最優(yōu)傾角。卡爾曼濾波方程如下所示[8]：

X(k|k-1)=AX(k-1|k-1)+BU(k)

(1)

P(k|k-1)=AP(k-1|k-1)AT+Q

(2)

Kg(k)=P(k|k-1)HT/(HP(k|k-1)HT+R)

(3)

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-

HX(k|k-1))

(4)

P(k|k)=(I-Kg(k)H)P(k|k-1)

(5)

因漂移噪聲和角度噪聲是相互獨立，則

cov(Angle,Q_bias)=0， cov(Q_bias,Angle)=0

式(4)為狀態(tài)估計方程，Z(k)=Accel為加速度計的角度測量值。

2.4 PID控制算法

控制算法的編寫是小車平衡控制的核心問題，也是軟件編寫和程序調(diào)試中的關(guān)鍵技術(shù)。本系統(tǒng)中采用了常用的增量式PID控制算法。PID控制的原理是根據(jù)系統(tǒng)的設(shè)定值和測量值之間的偏差，對偏差進行比例、積分、微分計算出控制量。系統(tǒng)采用雙閉環(huán)PID控制算法對小車系統(tǒng)進行自平衡控制，其結(jié)構(gòu)框圖如圖5所示。對編碼器產(chǎn)生的脈沖轉(zhuǎn)換得到實時速度信息，以該信息作為反饋量實現(xiàn)速度閉環(huán)控制[11-12]；以融合后的小車傾角最優(yōu)值作為反饋量實現(xiàn)小車姿態(tài)閉環(huán)控制。姿態(tài)閉環(huán)控制采用了PD控制器，利用卡爾曼濾波輸出的小車傾角和角速度的優(yōu)化值來計算電機PWM信號的占空比，表達式為：

(6)

速度閉環(huán)控制利用了P控制，對增量編碼器檢測的小車速度進行比例控制，其輸出作為PWM信號占空比的增量值，表達式為：

ΔPWM=Ksp×speed

(7)

姿態(tài)閉環(huán)控制和速度閉環(huán)控制是相互影響的[13]，將其融合后的由主控制器輸出給直流電機驅(qū)動模塊的PWM信號為：

(8)

圖5 系統(tǒng)雙閉環(huán)PID控制算法框圖

2.5 電機控制策略

本小車系統(tǒng)使用的電機是小型12V直流電機，其轉(zhuǎn)速是由施加給電機的電壓大小來決定，轉(zhuǎn)向是通過改變施加給電機電源的正負極性。電機驅(qū)動模塊輸出兩路PWM信號，使用過程中通過給其中一路占空比為零，另外一路不為零來控制電機的正反轉(zhuǎn)[14-15]。電機電壓的設(shè)定值不是直接用來控制電機的，而是將其轉(zhuǎn)化為PWM占空比，通過增大或減小PWM占空比的方法來實現(xiàn)電機的加減速控制。通過PID控制算法后的PWM信號是STM32控制器直接發(fā)送給直流電機驅(qū)動模塊，從而控制直流電機的速度和方向。小車轉(zhuǎn)彎過程是通過控制兩個電機的速度差來實現(xiàn)的。

3 系統(tǒng)調(diào)試

3.1 系統(tǒng)的硬件調(diào)試

程序下載后對小車系統(tǒng)各硬件模塊部分分別進行測試，觀察小車動作情況。若測試均能正常通過，則表明系統(tǒng)硬件電路工作正常。調(diào)試時，檢測到小車的角度為30.904 39°，小車電機轉(zhuǎn)速為-52.095 81。

3.2 系統(tǒng)的姿態(tài)檢測模塊調(diào)試

系統(tǒng)的姿態(tài)檢測模塊調(diào)試也是整個調(diào)試過程的重點和難點之一，其直接決定了小車能否平衡。

對陀螺儀及加速度計分別進行調(diào)試，保證輸出信號的準確性。在小車調(diào)試過程中，先使小車與地面行成135°、90°和45°的傾角，通過通信將姿態(tài)檢測傳感器測量的角度和角加速度數(shù)據(jù)顯示在計算機上，計算設(shè)定值與實際測量值之間的差值，來判斷是硬件電路還是軟件問題，好及時調(diào)整。上電檢測到陀螺儀的數(shù)值為-0.061，角加速度的數(shù)值為34.562 85。

3.3 系統(tǒng)的PID控制參數(shù)整定

系統(tǒng)中采用雙閉環(huán)PID控制算法，其PID控制參數(shù)需要通過多次實驗進行試湊法進行整定，整定出能使小車系統(tǒng)實現(xiàn)自平衡控制的最佳參數(shù)。

姿態(tài)閉環(huán)PID控制的調(diào)試過程：① 調(diào)整Kp值，讓小車慢慢的站起來，但小車會振蕩；② 減小Kp值，改為原先的75%，再慢慢的把Kd值變大，當小車可以平衡時，Kd值停止增加。③ 調(diào)整上述的兩個數(shù)值Kp和Kd，將Kp值加大一點，再把Kd值進行微調(diào)，最終調(diào)試得到當Kp=5.35、Kd=0.85時，能夠使小車在較快的時間內(nèi)達到穩(wěn)定平衡狀態(tài)。

采用同樣的方法對速度閉環(huán)PID控制參數(shù)進行調(diào)試，調(diào)試得到當Ksp=7.0時，小車能夠快速穩(wěn)定的達到自平衡狀態(tài)。

3.4 系統(tǒng)的動態(tài)調(diào)試

在小車自平衡過程中，若給小車一個外力來破壞小車的平衡，則小車要能迅速響應(yīng)這個變化，立即運動去減小傾角的變化，建立一個穩(wěn)定平衡狀態(tài)。開始調(diào)試時，能響應(yīng)，但波動較大，建立平衡的時間較長。通過修正PID參數(shù)，最終能使小車快速達到平衡狀態(tài)。

3.5 調(diào)試結(jié)果

小車平衡時效果圖如圖6所示。系統(tǒng)通電后，小車完全實現(xiàn)自平衡的時間為12 s，在普通水磨石地面上，平衡時傾角范圍約在±10o以內(nèi)。若給予一定外力干擾時，小車在20 s內(nèi)能夠自動調(diào)節(jié)實現(xiàn)自平衡狀態(tài)。

圖6 小車平衡時效果圖

4 結(jié) 語

本文設(shè)計了一個兩輪自平衡小車系統(tǒng)，該系統(tǒng)基于STM32F103C8T6微處理器、采用卡爾曼濾波算法和雙閉環(huán)PID控制算法對小車進行自平衡控制。實驗測試結(jié)果證明，采用卡爾曼濾波算法能夠有效地消除陀螺儀和加速度計在測量過程中的噪聲干擾和隨機漂移誤差，準確地計算出小車的姿態(tài)傾角。采用雙閉環(huán)PID控制算法能夠?qū)崟r的將傾角和速度的設(shè)定值與當

前的傾角和速度值進行比較，根據(jù)誤差信息進行實時的閉環(huán)控制，從而使小車穩(wěn)定地達到平衡狀態(tài)。采用高性能的微處理器，使得卡爾曼濾波算法和PID控制算法能夠快速完成，提高了數(shù)據(jù)采集和控制的實時性，提高了系統(tǒng)的穩(wěn)定性和抗干擾能力。

[1] 季鵬飛，朱 燕. 基于STM32的兩輪自平衡小車控制系統(tǒng)設(shè)計[J]. 電子科技，2014，27(11)：96-99.

[2] 武俊峰，孫 雷. 兩輪自平衡機器人的控制方法研究[J]. 哈爾濱理工大學(xué)學(xué)報，2014，19(6)：22-26.

[3] 劉二林，姜香菊. 基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測[J]. 自動化與儀器儀表，2015(2)：52-54.

[4] 盧秀和，李建波. 基于STM32的四旋翼姿態(tài)控制系統(tǒng)[J]. 計算機測量與控制，2014，22(3)：761-763.

[5] 楊 莘，劉海濤. 基于STM32的兩輪自平衡遙控小車[J]. 數(shù)字技術(shù)與應(yīng)用，2014，(5)：151-152.

[6] 孟 騰，章 政. 基于陀螺儀與加速度計的二輪自平衡控制系統(tǒng)設(shè)計[J]. 傳感器與微系統(tǒng)，2014，33(10)：61-64.

[7] 蔣緯洋，鄧 遲，肖曉萍. 兩輪自平衡小車系統(tǒng)制作研究[J]. 國外電子測量技術(shù)，2012，31(6)：76-79.

[8] Liu Kun, Ming Bai, Yuhua Ni. Two-wheel self-balanced carbased on Kalman filter-ing and PID algorithm[C]∥Industrial Engi-neering and Engineering Management (IE&EM) 2011 IEEE 18thInternational Con-ference, 2011, Part 1 : 281-285.

[9] 張團善，何 穎. 卡爾曼濾波在兩輪自平衡代步車姿態(tài)檢測中的應(yīng)用[J]. 單片機與嵌入式系統(tǒng)應(yīng)用，2014(5)：33-35.

[10] 楊凌霄，李曉陽. 基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測方法[J]. 計算機仿真，2014，31(6)：406-409.

[11] 魏 文，段晨東. 基于數(shù)據(jù)融合的兩輪自平衡小車控制系統(tǒng)設(shè)計[J]. 電子技術(shù)應(yīng)用，2013，39(5)：79-81.

[12] 馬思遠，魯庭勇，張麗君. 兩輪自平衡車運動姿態(tài)的測量和控制[J]. 測控技術(shù)，2015，34(4)：71-73.

[13] 梁光勝，杜夢楠. 基于互補濾波的兩輪自平衡車姿態(tài)控制[J]. 測控技術(shù)，2015，34(5)：72-74.

[14] 柯國松，陳 陽，許志文. 兩輪自平衡小車的研究與實現(xiàn)[J]. 工業(yè)控制計算機，2014，27(11)：44-46.

[15] 周牡丹，康 愷，蔡普郎，等. 兩輪自平衡車控制系統(tǒng)的設(shè)計與實現(xiàn)[J]. 自動化技術(shù)與應(yīng)用，2014，33(10)：4-8.

The Design of the Two-wheel Self-balancing Vehicle System Based on STM32

WANGSu-qing,XIONGWei-tang

(Jincheng College, Nanjing University of Aeronautics and Astronautics, Nanjing 211156, China)

The two-wheel self-balancing vehicle system is a system which is similar to an inverted pendulum, and holds multivariable, nonlinear, strong coupling characteristics. For the complexity of the vehicle control system, we designed a self-balancing vehicle system by using STM32F103C8T6 microprocessor as the main controller, and MPU-6050 sensor as the attitude detection unit. Because gyroscope and accelerometer exist measurement noise and random drift error, Kalman filter algorithm is used to realize data fusion of the gyroscope and the accelerometer. In this way the sensor measurement error is compensated and the best estimates of the vehicle inclination and angular velocity are obtained. A double closed loop control is established with the feedback of optimal attitude angle and vehicle speed. PID control is used to achieve self-balancing control of the vehicle system. The hardware and software are also designed. By commissioning and operation the self-balancing vehicle can achieve stable self-balancing control.

two-wheel self-balancing vehicle; STM32; MPU-6050 sensor; Kalman filter; PID control

2015-09-10

王素青(1976—)，女，江蘇鹽城人，碩士，講師，主要從事網(wǎng)絡(luò)控制系統(tǒng)方面的研究。

Tel.:15335198016; E-mail: wsq0214@nuaa.edu.cn

TP 273

A

1006-7167(2016)05-0146-05