引言
兩輪自平衡小車系統(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的SRAM存儲(chǔ)器。片上集成了SPI,I2C和USART等通信接口,方便對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)定性和抗干擾能力。
評論