引言

兩輪自平衡小車系統(tǒng)類似于倒立擺系統(tǒng)，具有多變量、非線性、強(qiáng)耦合等特點(diǎn)，是研究各種控制方法的理想平臺(tái)。兩輪自平衡小車系統(tǒng)的控制過程是微控制器對姿態(tài)檢測傳感器和編碼器等采集的數(shù)據(jù)進(jìn)行分析處理，計(jì)算出使系統(tǒng)恢復(fù)平衡的實(shí)時(shí)控制量，從而驅(qū)動(dòng)電機(jī)實(shí)現(xiàn)系統(tǒng)的動(dòng)態(tài)平衡。

針對小車系統(tǒng)的復(fù)雜性，本文提出了將卡爾曼濾波算法和雙閉環(huán)PID控制算法相結(jié)合的方法，既利用卡爾曼濾波算法對MPU-6050傳感器采集的傾斜角度和角速度數(shù)據(jù)進(jìn)行融合，得到小車平衡姿態(tài)的最優(yōu)估計(jì)值，又利用以姿態(tài)信息、速度為反饋控制量構(gòu)成雙閉環(huán)PID控制算法，再結(jié)合采用高性能STM32F103C8T6作為主控制器，從而提高兩輪小車系統(tǒng)穩(wěn)定性和抗干擾能力。

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

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

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

1．1、電源模塊

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

1．2、主控制器模塊

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

1．3、姿態(tài)檢測模塊

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

圖2 ?MPU-6050模塊的電路

1．4、電機(jī)驅(qū)動(dòng)模塊

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

圖3 ?直流電機(jī)驅(qū)動(dòng)模塊的電路

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

2．1、系統(tǒng)的總體流程設(shè)計(jì)

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

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

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

2．2、系統(tǒng)初始化程序

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

STM32的工作頻率為72MHz或36MHz，本系統(tǒng)對時(shí)鐘頻率的要求很苛刻，如果頻率過低，系統(tǒng)就會(huì)工作在不穩(wěn)定狀態(tài)下，系統(tǒng)產(chǎn)生的誤差會(huì)變大，又因系統(tǒng)使用的濾波方式是卡爾曼濾波，因此在時(shí)鐘初始化程序中需將時(shí)鐘頻率設(shè)置為72MHz。通信（I2C）初始化主要設(shè)置串口波特率為9600b/s，需設(shè)置中斷允許標(biāo)志位和模塊的工作模式。

2．3、卡爾曼濾波數(shù)據(jù)融合算法

2．3．1、卡爾曼濾波算法

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

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

2．3．2、卡爾曼濾波器設(shè)計(jì)

卡爾曼濾波方程如下所示：

2．4、PID控制算法

控制算法的編寫是小車平衡控制的核心問題，也是軟件編寫和程序調(diào)試中的關(guān)鍵技術(shù)。本系統(tǒng)中采用了常用的增量式PID控制算法。PID控制的原理是根據(jù)系統(tǒng)的設(shè)定值和測量值之間的偏差，對偏差進(jìn)行比例、積分、微分計(jì)算出控制量。系統(tǒng)采用雙閉環(huán)PID控制算法對小車系統(tǒng)進(jìn)行自平衡控制，其結(jié)構(gòu)框圖如圖5所示。

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

2．5、電機(jī)控制策略

本小車系統(tǒng)使用的電機(jī)是小型12V直流電機(jī)，其轉(zhuǎn)速是由施加給電機(jī)的電壓大小來決定，轉(zhuǎn)向是通過改變施加給電機(jī)電源的正負(fù)極性。電機(jī)驅(qū)動(dòng)模塊輸出兩路PWM信號，使用過程中通過給其中一路占空比為零，另外一路不為零來控制電機(jī)的正反轉(zhuǎn)。

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

程序下載后對小車系統(tǒng)各硬件模塊部分分別進(jìn)行測試，觀察小車動(dòng)作情況。若測試均能正常通過，則表明系統(tǒng)硬件電路工作正常。調(diào)試時(shí)，檢測到小車的角度為30．90439°，小車電機(jī)轉(zhuǎn)速為－52．09581。

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

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

圖6 ?小車平衡時(shí)效果圖

4、結(jié)語

本文設(shè)計(jì)了一個(gè)兩輪自平衡小車系統(tǒng)，該系統(tǒng)基于STM32F103C8T6微處理器、采用卡爾曼濾波算法和雙閉環(huán)PID控制算法對小車進(jìn)行自平衡控制。實(shí)驗(yàn)測試結(jié)果證明，采用卡爾曼濾波算法能夠有效地消除陀螺儀和加速度計(jì)在測量過程中的噪聲干擾和隨機(jī)漂移誤差，準(zhǔn)確地計(jì)算出小車的姿態(tài)傾角。采用高性能的微處理器，使得卡爾曼濾波算法和PID控制算法能夠快速完成，提高了數(shù)據(jù)采集和控制的實(shí)時(shí)性，提高了系統(tǒng)的穩(wěn)定性和抗干擾能力。