陳柔 劉寅生*沈陽工學院

?

基于STC12C5A60S2單片機的雙輪自平衡小車控制系統(tǒng)設計

陳柔 劉寅生*
沈陽工學院

摘要：本文介紹了由單片機STC12C5A60S2、6軸集成姿態(tài)傳感器MPU6050、電機驅動模塊TB6612FNG等構成的雙輪自平衡小車控制系統(tǒng)構成，軟件部分應用了卡爾曼濾波方法與PID控制算法，實現(xiàn)雙輪平衡小車的直立與行走。

關鍵詞：雙輪自平衡小車 姿態(tài)傳感器 卡爾曼濾波 PID算法

1　雙輪自平衡小車總體系統(tǒng)設計

雙輪自平衡小車的總控制系統(tǒng)由微理器（STC12C5A60S2）模塊、姿態(tài)傳感器（MPU6050）模塊、藍牙模塊（HC系列）、電源模塊、電機驅動模塊（TB6612FNG）及速度檢測模塊等組成。姿態(tài)傳感器主要采用MPU6050，用于采集小車的角度和小車的角速度信息；選用宏晶科技有限公司的STC12C5A60S2單片機作為主控芯片。

2　姿態(tài)傳感器與姿態(tài)信號處理

傳感器MPU6050是全球首個將陀螺儀與加速度計整合集成的運動處理組件，相對于較多組件的方案，免除了組合陀螺儀與加速度計之間的軸差問題，減少了大量的封裝空間。

系統(tǒng)采用控制算法是PID控制，通過卡爾曼濾波整合車體角度、角速度、車體速度和位置等參數(shù)，輸出PWM信號驅動電機，產(chǎn)生相應的力矩，從而使小車達到平衡。

3　驅動模塊與速度檢測

雙輪平衡小車的兩個直流減速電動機由TB6612FNG驅動模塊驅動。TB6612FNG是東芝半導體公司生產(chǎn)的一款直流電機驅動器件，是一款新型驅動器件，它具有很高的集成度，同時能提供足夠的輸出能力，運行性能和能耗方面也具有優(yōu)勢。因此，在集成化、小型化的電機控制系統(tǒng)中，它可以作為理想的電機驅動器。

速度檢測方面選擇在電機上裝一個編碼盤，整合數(shù)電電路，使碼盤直接輸出數(shù)字信號便于MCU使用。

4　軟件設計

在雙輪平衡車的控制系統(tǒng)中，程序的功能主要是為了采集與分析來自傳感器的信號以及產(chǎn)生PWM信號用于對電機的控制。數(shù)據(jù)處理過程如圖2所示。

圖2 數(shù)據(jù)處理框圖

在軟件中主要定義了以下函數(shù)：

⑴小車角度計算函數(shù)，根據(jù)MPU6050采集到加速度與角速度計算出小車電機的角度，這里可以直接調用MPU6050輸出的數(shù)據(jù)，也可以自行使用一些濾波算法進行數(shù)據(jù)融合。

⑵小車直立控制函數(shù)，根據(jù)小車角度和角速度計算車模電機的控制量。

⑶小車速度控制函數(shù)，根據(jù)小車采集到的電機轉速和速度設定值，計算電機的控制量。

⑷方向控制函數(shù)，根據(jù)車模采集到的左右兩個編碼盤的數(shù)值計算出角度控制的量。

⑸電機輸出量匯集函數(shù)，根據(jù)前面的直立控制、速度控制和方向控制所得到的控制量進行疊加，分別得到左右兩個電極的輸出電壓控制量。對疊加后的輸出量進行飽和處理。

⑺電機PWM輸出計算函數(shù)，根據(jù)左右兩個電極的輸出控制量的正負極性，疊加上一個小的死區(qū)數(shù)值，克服車模機械靜態(tài)摩擦力。

⑺PWM輸出函數(shù)，根據(jù)兩個電機的輸出量，計算出PWM控制寄存器的數(shù)值，設置四個PWM控制寄存器的數(shù)值。

5　總結

本文主要論述了雙輪自平衡小車控制系統(tǒng)的設計過程?？刂破鞑捎肧TC12C5A60S2芯片，采用MPU6050芯片為姿態(tài)傳感器，應用了簡單的卡爾曼濾波與PID控制算法。在調試中逐步測定各個微分、比例、積分環(huán)節(jié)中的系數(shù)值，實現(xiàn)了雙輪自平衡小車的動態(tài)平衡與運動控制。

參考文獻

［1］ 趙杰，王曉宇等.基于 UKF 的兩輪自平衡機器人姿態(tài)最優(yōu)估計研究［J］.機器人. 2006： （11），605—609.

［2］ 王艷秋.自動控制原理［M］.北京：清華大學出版社，2009.

［3］ 譚浩強.C 程序設計［M］.北京：清華大學出版社，2005．

［4］ 秦永元，張洪鉞.卡爾曼濾波與組合導航原理［M］.西安：西北工業(yè)大學出版社，1998.

*通訊作者：劉寅生