引言

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

針對小車系統(tǒng)的復(fù)雜性，本文提出了將卡爾曼濾波算法和雙閉環(huán)PID控制算法相結(jié)合的方法，既利用卡爾曼濾波算法對MPU-6050傳感器采集的傾斜角度和角速度數(shù)據(jù)進(jìn)行融合，得到小車平衡姿態(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)檢測模塊、直流電機(jī)驅(qū)動模塊、編碼器及電源模塊等幾部分構(gòu)成。電源模塊負(fù)責(zé)系統(tǒng)各個模塊控制電路的電源。主控制器模塊是系統(tǒng)的控制核心，用來接收傳感器模塊采集的數(shù)據(jù)，對數(shù)據(jù)進(jìn)行處理及運(yùn)算，將控制信號輸出給直流電機(jī)驅(qū)動模塊。姿態(tài)檢測模塊實(shí)時地對小車角速度及角加速度進(jìn)行采集。直流電機(jī)驅(qū)動模塊負(fù)責(zé)將主控制器輸出的PWM信號轉(zhuǎn)換為控制信號驅(qū)動2個直流電機(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存儲器，6～64kB的SＲAM存儲器。片上集成了SPI，I2C和USAＲT等通信接口，方便對MPU-6050姿態(tài)檢測傳感器的數(shù)據(jù)進(jìn)行采集。含有7個定時器，可輸出多路PWM信號使得電機(jī)驅(qū)動模塊獲得穩(wěn)定的PWM波形。

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

本系統(tǒng)中姿態(tài)檢測模塊選用的是美國InvenSense公司的MPU-6050，MPU-6050為全球首例整合性6軸運(yùn)動處理器，整合了3軸陀螺儀和3軸加速度計，能夠準(zhǔn)確的對小車的姿態(tài)進(jìn)行實(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ū)動模塊

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

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

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

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

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

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

系統(tǒng)啟動后，首先進(jìn)行初始化，然后執(zhí)行中斷程序，判斷5ms時間是否到，若時間到，則讀取MPU6050傳感器采集的角度和角速度的數(shù)據(jù)，然后利用卡爾曼濾波算法對讀取的數(shù)據(jù)進(jìn)行融合得到最優(yōu)估計值。同樣采用中斷的方式讀取編碼器的脈沖數(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)時鐘初始化、NVIC中斷初始化、定時器外部中斷初始化、通信（I2C）初始化、測速模塊初始化、MPU-6050初始化和電機(jī)PWM初始化等。

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

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

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

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

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

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

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

2．4、PID控制算法

控制算法的編寫是小車平衡控制的核心問題，也是軟件編寫和程序調(diào)試中的關(guān)鍵技術(shù)。本系統(tǒng)中采用了常用的增量式PID控制算法。PID控制的原理是根據(jù)系統(tǒng)的設(shè)定值和測量值之間的偏差，對偏差進(jìn)行比例、積分、微分計算出控制量。系統(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ū)動模塊輸出兩路PWM信號，使用過程中通過給其中一路占空比為零，另外一路不為零來控制電機(jī)的正反轉(zhuǎn)。

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

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

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

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

圖6 ?小車平衡時效果圖

4、結(jié)語

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