賴義漢，王 凱

(龍巖學院 物理與機電工程學院 ，福建 龍巖 364012)

基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計

賴義漢，王 凱

(龍巖學院 物理與機電工程學院 ，福建 龍巖 364012)

雙輪平衡車是一個高度不穩(wěn)定兩輪機器人，單獨使用陀螺儀或者加速度計都不能提供有效而可靠的信息來保證車體的平衡.介紹了以STC12C5A60S2單片機作為核心控制器的雙輪平衡車控制系統(tǒng)設(shè)計方案，采用MPU6050六軸傳感器采集角度和角速度信號，應用卡爾曼融合濾波方式和PID控制算法，以實現(xiàn)雙輪平衡車的平衡和簡單的直立行走.

雙輪平衡車；MPU6050；控制系統(tǒng)

圖1 系統(tǒng)總體框圖Fig.1 System structure diagram

1 系統(tǒng)總體框圖

雙輪平衡車控制系統(tǒng)主要由控制核心(MCU)模塊、姿態(tài)傳感器模塊、無線遙控模塊、電源模塊、電機驅(qū)動模塊、車速檢測模塊及顯示模塊等組成，系統(tǒng)總體框圖如圖1所示.姿態(tài)傳感器采用MPU6050，主要采集小車的角度和角速度信號；MCU采用宏晶科技有限公司的STC12C5A60S2單片機[2]作為控制芯片，該芯片具有I2C接口和PWM輸出，抗干擾能力強；顯示模塊的主要功能為顯示車體的傾角等相關(guān)信息.

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

在雙輪平衡車姿態(tài)檢測系統(tǒng)中，加速度計、陀螺儀主要用于檢測車體傾斜角和傾斜角的變化速度，其中加速度計屬于測量線性運動的傳感器，其靜態(tài)響應好，能夠準確測量車體靜態(tài)時的角度，但對震動較敏感，受動態(tài)加速度的影響較大.在角度測量時，除了小車角度變化的信號外，還伴隨著因車體運動而產(chǎn)生的噪聲，這個噪聲會隨著車體運動速度的增加而增大.陀螺儀屬于測量旋轉(zhuǎn)運動的傳感器，其輸出值圍繞某個軸向的旋轉(zhuǎn)角速率，通過角速率對時間積分即可得到角度值.系統(tǒng)采用微控制器循環(huán)采樣獲取陀螺儀的角速率信息，即每隔一段很短的時間采樣一次，采用累加的方法實現(xiàn)積分的功能來計算角度值，所以存在累積漂移誤差，不適合長時間單獨工作[3].

為獲得準確的車體傾角值，需對加速度計和陀螺儀的輸出值進行融合，系統(tǒng)采用一種簡易的卡爾曼濾波方法進行數(shù)據(jù)融合.卡爾曼濾波器是一種遞歸的估算，例如要估算K時刻車體的實際角度值，首先要根據(jù)K-1時刻的角度值預測得到K時刻的角度值，再根據(jù)K時刻的預測角度值和高斯噪聲的方差，進行遞歸運算，直到獲得最優(yōu)的車體角度值[4].

圖2 角度與角速度數(shù)據(jù)融合及輸出控制框圖Fig.2 The output control diagram of angle and angular velocity data fusion

當雙輪平衡車車體產(chǎn)生傾斜時，系統(tǒng)采用PID控制算法[5]，通過整合車體角度、角速度、車體速度和位置等參數(shù)值，輸出PWM信號驅(qū)動電機，產(chǎn)生相應的力矩，從而保持車體的動態(tài)平衡,其結(jié)構(gòu)框圖如圖2所示，圖中的Kd，Kp，Ksp，Kis是PID控制器的相關(guān)參數(shù).

表1 MPU6050的主要管腳功能Tab.1 The functions of MPU6050 pins

3 主要硬件電路設(shè)計

3.1MPU6050傳感器電路

MPU6050六軸傳感器芯片集成了3軸MEMS陀螺儀和3軸MEMS加速度計，每個軸對應有一個16位AD轉(zhuǎn)換器，測量范圍達±16 g，其高分辨率(3.9 mg/LSB)能夠測量不到1.0°的傾斜角度變化.芯片正常工作時，陀螺儀和加速度計分別采集x軸、y軸和z軸的電壓值，然后通過AD轉(zhuǎn)換，轉(zhuǎn)換成數(shù)字信號，最后通過I2C總線傳送到控制芯片，但此時得到的值并不是實際的角度和角速度值，還必須經(jīng)過一定的比例關(guān)系進行轉(zhuǎn)換，才能得到實際的角度和角速度值，其電路如圖3所示.表1為MPU6050芯片主要的引腳功能.

圖3 MPU6050接口電路Fig.3 Interface circuit of MPU6050

圖4 車速檢測電路Fig.4 Detection circuit of vehicle speed

3.2車速檢測電路

在小車的車輪上安裝碼盤，通過紅外光電傳感器GK102組成測速電路，利用控制單片機的計數(shù)器測量在固定時間間隔內(nèi)速度脈沖信號的個數(shù)來得到電機的轉(zhuǎn)速，再通過變換得到車體的速度.光電傳感器GK102外接電路如圖4所示.

P2P網(wǎng)貸平臺的刑事治理有一定困難之處:第一，P2P平臺非法集資行為具有一定的隱蔽性，所以發(fā)現(xiàn)難;第二，對P2P平臺非法集資行為的罪與非罪、此罪與彼罪和共同犯罪的成立等問題的定性難;第三，對P2P網(wǎng)絡(luò)集資相關(guān)犯罪的追贓難，一方面是定性贓款的范圍有難度，另一方面案件發(fā)生后，非法所得都被揮霍一空;第四，由于犯罪成本低、司法機關(guān)重視事后制裁，所以對P2P網(wǎng)絡(luò)非法集資行為預防難。

圖5 無線遙控模塊電路Fig.5 Circuit of wireless remote control module

3.3無線遙控模塊電路

無線遙控模塊采用nRF24L01+，該芯片是一款工作在2.4～2.5 GHz世界通用ISM 頻段的單片無線收發(fā)器芯片.無線收發(fā)器包括頻率發(fā)生器、增強型SchockBurst模式控制器、功率放大器、晶體振蕩器、調(diào)制器、解調(diào)器等.輸出功率頻道選擇和協(xié)議的設(shè)置可以通過SPI接口進行設(shè)置，其外圍接口電路如圖5所示，該模塊主要用來遙控小車的前進和后退等工作方式.

3.4電機的驅(qū)動電路

雙輪平衡小車執(zhí)行機構(gòu)由兩個減速直流電機構(gòu)成，其驅(qū)動電路由L298N組成，電路如圖6所示.其中,OUT1和OUT2以及OUT3和OUT4之間分別接兩直流電機M1和M2，IN1～IN4腳為輸入控制電平，通過光電耦合器與單片機相連，控制電機正反轉(zhuǎn)，ENA和ENB為控制使能端，與單片機的PWM 輸出口(P1.2和P1.3)相接，控制電機轉(zhuǎn)速， L298N的 1腳和15腳是輸出電流反饋引腳，通?？芍苯咏拥?其電機控制功能模式如表2所示.

表2 直流電機控制功能表Tab.2 The table of DC motor control function

圖6 電機驅(qū)動電路Fig.6 The motor drive circuit

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

系統(tǒng)軟件主要完成傳感器數(shù)據(jù)的采集及相關(guān)數(shù)據(jù)的處理并輸出PWM控制信號控制電機的運行，主程序流程如圖7所示.其中，Angle_Calcu()為角度和角速度處理函數(shù)，主要讀取傳感器MPU6050的角度和角速度信號并進行融合濾波，得到車體的角度值，其程序流程如圖8所示.Psn_calcu()為速度位置處理函數(shù)， PWM_Calcu()函數(shù)主要完成PID控制算法，并輸出PWM信號.

圖7 系統(tǒng)主流程Fig.7 The main flow chart of system

圖8 角度、角速度處理函數(shù)流程Fig.8 Function flow chart of angle and angular velocity

5 系統(tǒng)測試

5.1角度和角速度的零點偏移值測定

根據(jù)MPU6050傳感器擺放的位置，選擇加速度計y軸數(shù)據(jù)進行角度計算，選擇陀螺儀x軸數(shù)據(jù)進行角速度計算.單片機通過I2C總線分別讀取MPU6050加速度計的角度值A(chǔ)ccel_y和陀螺儀的角速度值Gyro_x.在靜止狀態(tài)下把車放置在平衡位置，待傳感器穩(wěn)定后，測量MPU6050傳感器的10組數(shù)據(jù)值，如表3所示，通過求平均值計算此時角度和角速度的零點偏移值分別為大約1 100 LSB和8 LSB.

表3 角度和角速度零點偏移Tab.3 Zero offset of angle and angular velocity

當偏轉(zhuǎn)角度不太大時，可通過公式(1)和(2)轉(zhuǎn)換成實際的角度和角速度:

(1)

Gyro_x=(Gyro_x-8)/16.4.

(2)

公式(1)中Accel_y為MPU6050傳感器加速度計y軸的數(shù)據(jù)，1 100為加速度計的零點偏移值，加速度計選±2 g，通過查MPU6050傳感器數(shù)據(jù)手冊可知，其對應的靈敏度為16 384 LSB/g，而180/3.14是為了把弧度轉(zhuǎn)換成度，此時得到的值不是它真正的角度值，需適當放大1.2倍，近似得到它的角度值.公式(2)中Gyro_x為MPU6050傳感器陀螺儀x軸的數(shù)據(jù)，8為陀螺儀的零點偏移值，陀螺儀選±2 000 (°/s).查MPU6050傳感器數(shù)據(jù)手冊可知，其對應的靈敏度為16.4 LSB/(°/s).此時的Angle_ay為(-90,90),平衡位置為0度.Gyro_x的值在靜止時為0，擺動時不為0，方向與角度的微分方向一致，符合要求.

5.2PID控制器的參數(shù)整定

通過對PID公式進行變換，得到PWM輸出表達式，驅(qū)動直流電機，其計算公式為

PWM=Kp×Angle+Kd×Gyro_x+Kis×Position+Kps×Speed，

(3)

公式(3)中，Angle代表角度，Gyro_x代表x軸角速度，Position代表車體速度的積分得到的位置，Speed代表測得的車體速度.

首先,通過Matlab進行仿真，確定PID控制器4個參數(shù)的大致范圍及比例關(guān)系，再通過實驗法來確定PID算法中的各個參數(shù)，具體步驟如下：

(1)首先確定角度的比例參數(shù)Kp和微分參數(shù)Kd，可以遵循先比例后微分的過程.首先,調(diào)整比例參數(shù)，使車模能夠保持直立并且開始來回擺動.然后,逐步增加微分參數(shù)，車模逐步保持直立穩(wěn)定.增大微分參數(shù)直到車模開始共振，這樣就可以確定微分參數(shù)的最大值.然后，適當減少微分參數(shù)、逐步增加比例參數(shù)，直到車模又開始震蕩，這樣便可以確定比例參數(shù)的最大值.最后，適當減少比例參數(shù).在這些參數(shù)附近進行實驗，直到尋找到一組車模直立控制的最優(yōu)參數(shù)為止.

(2)在調(diào)整車速的微分參數(shù)Kis和比例參數(shù)Kps時，采用類似方法，先逐步增加比例參數(shù)，使車模能夠在一定平衡點附近來回運動，然后再逐步增加微分參數(shù)，使車模能夠較快停留在平衡點處，再適當調(diào)整微分和比例控制參數(shù)，使車模在外力沖擊下能夠很快趨于靜止.通過幾次調(diào)整測試，可以初步確定速度控制參數(shù)的最佳組合.

6 結(jié)語

雙輪平衡車控制系統(tǒng)以STC12C5A60S2單片機為控制芯片，采用卡爾曼融合濾波方式對MPU6050傳感器的角度和角速度信號進行融合處理，從而得到實時、穩(wěn)定的角度和角速度信號，應用實驗測試法確定PID算法中的各個參數(shù)，從而實現(xiàn)對雙輪平衡車的控制.經(jīng)多次測試，能初步實現(xiàn)車體的平衡和簡單的直立行走.

[1] 張偉民,段曉明,趙艷花.兩輪自平衡小車控制研究[J].自動化技術(shù)與應用,2011，30(4)：10-13.

[2] 陳桂友,柴遠斌.單片機應用技術(shù)[M].北京：機械工業(yè)出版社,2008：214-220.

[3] 郜園園，阮曉鋼，宋洪軍,等．兩輪自平衡機器人慣性傳感器濾波問題的研究[J].傳感器技術(shù)學報，2010,23(5):696-699.

[4] 馮智勇，曾瀚，張力,等.基于陀螺儀及加速度計信號融合的姿態(tài)角度測量[J].西南師范大學學報:自然科學版，2011，36(4):137-140.

[5] 何躍,林春梅.PID控制系統(tǒng)的參數(shù)選擇研究及應用[J].計算機工程與設(shè)計,2006,27(8)：1496-1498 .

Thedesignofthetwo-wheeledbalancingvehiclecontrolsystembasedonMPU6050

LAI Yi-han，WANG kai

(PhysicsandMechanicsCollege,LongyanUniversity,Longyan364012,China)

Two-wheeled balancing car is a highly unstable two-wheeled robot, the information depending on the gyroscope or the accelerometer couldn't make sure the balance of the vehicle．The paper mainly introduced the design of the two-wheel balancing car control system, as the core controller of the STC12C5A60S2 microcontroller. System collects angle and angular velocity base on MPU6050 six axis sensors. Using Kalman filtering fusion method and PID control algorithm, the balance and walking upright of two-wheeled balancing is realized.

two-wheeled balancing vehicle；MPU605；control system

2013-10-28

賴義漢(1968-)，男，福建龍巖人，副教授，主要從事嵌入式系統(tǒng)與應用方面的研究.

TP212.9

A

1674-330X(2014)01-0053-05