重慶科創(chuàng)職業(yè)學(xué)院信息與機(jī)電工程學(xué)院　萬(wàn)　軍　呂值敏　文方國(guó)

?

基于STM32平衡兩輪移動(dòng)機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)

重慶科創(chuàng)職業(yè)學(xué)院信息與機(jī)電工程學(xué)院萬(wàn)軍呂值敏文方國(guó)

【摘要】本文設(shè)計(jì)了一個(gè)兩輪自平衡小車系統(tǒng)，它以STM32嵌入式微處理器為核心，以MPU6050傳感器采集車體姿態(tài)，利用卡爾曼（kalman）濾波融合算法獲取車體角度和角速度，最后使用PID控制算法獲取合適的PWM值，通過(guò)控制電機(jī)的正、反轉(zhuǎn)來(lái)最終達(dá)到車體的平衡，得到了較為理想的運(yùn)行控制效果。兩輪自平衡小車系統(tǒng)是一種特殊的輪式移動(dòng)機(jī)器人，它運(yùn)動(dòng)靈活，成本低，適合在狹小和危險(xiǎn)的空間工作，有著廣泛的應(yīng)用前景。

【關(guān)鍵詞】STM32；平衡；機(jī)器人；控制

0　引言

自平衡兩輪機(jī)器人是一個(gè)典型的非線性、強(qiáng)耦合、多變量、時(shí)變和自然不穩(wěn)定系統(tǒng)，是近年來(lái)研究倒立擺與機(jī)器人控制問(wèn)題的又一研究平臺(tái)，是檢驗(yàn)各種控制理論的理想模型。仿人智能控制理論從分層遞階智能控制系統(tǒng)的最低層(運(yùn)行控制級(jí))著手，充分應(yīng)用已有的智能裝備技術(shù)和控制技術(shù)，直接對(duì)人的控制經(jīng)驗(yàn)、技巧和各種直覺推理邏輯，即人體的動(dòng)覺智能進(jìn)行測(cè)辨、概括和總結(jié)，并將其編制成各種簡(jiǎn)單實(shí)用、精度高、能實(shí)時(shí)運(yùn)行的控制算法(動(dòng)覺智能圖式)，直接應(yīng)用于實(shí)際控制系統(tǒng)。本文依據(jù)動(dòng)覺智能圖式的仿人智能控制理論實(shí)現(xiàn)兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)控制，對(duì)自平衡小車機(jī)器人提出了控制思想，實(shí)現(xiàn)了機(jī)器人自平衡控制，得到了較為理想的平衡和運(yùn)行控制效果。

1　系統(tǒng)整體設(shè)計(jì)方案

平衡系統(tǒng)以STM32嵌入式微處理器為核心，以MPU6050傳感器采集車體姿態(tài)，利用卡爾曼(kalman)濾波融合算法獲取車體角度和角速度，最后使用PID控制算法獲取合適的PWM值，通過(guò)控制電機(jī)的正、反轉(zhuǎn)來(lái)最終達(dá)到車體的平衡，通過(guò)人為控制左右紅外避障傳感器的信號(hào)，來(lái)實(shí)現(xiàn)兩輪平衡車的停止、前進(jìn)、左轉(zhuǎn)、右轉(zhuǎn)等功能，最終實(shí)現(xiàn)平衡兩輪移動(dòng)機(jī)器人的避障搬運(yùn)功能[1]。本系統(tǒng)的整體方框圖如圖1所示。

圖1　系統(tǒng)方框圖

2　系統(tǒng)硬件設(shè)計(jì)

2.1系統(tǒng)主控芯片

本系統(tǒng)采用STM32F103作為主控芯片，該芯片采用Cortex-M3為內(nèi)核，具備72MHz最高工作頻率，64K字節(jié)的SRAM，2個(gè)DMA控制器，支持ADC、定時(shí)器、SPI、USB、IIC和UART等外圍設(shè)備。主控芯片工作及控制電路接線圖如圖2所示。

圖2　STM32F103控制電路圖

2.2電機(jī)驅(qū)動(dòng)與編碼器模塊

(1)編碼器：本系統(tǒng)采用JGA25-371自帶編碼器測(cè)速碼盤馬達(dá)減速電機(jī)，工作電壓為6-24VDC。編碼器芯片上已集成了脈沖整形觸發(fā)電路，輸出的是矩形波，直接連接芯片PA1、PA0、PB6、PB7引腳，主控芯片STM32F103與編碼器JGA25731接線如圖2所示。自帶編碼器電機(jī)測(cè)速精度高，采用334線碼盤即電機(jī)每轉(zhuǎn)動(dòng)一圈就輸出334個(gè)脈沖。

(2)電機(jī)模塊：采用雙H橋驅(qū)動(dòng)芯片L298N。其電機(jī)特點(diǎn)為：內(nèi)部有4通道邏輯驅(qū)動(dòng)電路或一個(gè)兩相步進(jìn)電機(jī)，驅(qū)動(dòng)電機(jī)的速度可以由控制芯片的使能端直接控制。電機(jī)驅(qū)動(dòng)L298N芯片采用5 V(VSS)與12 V(VS)直流供電，ENA和ENB分別用STM32F103主控芯片的PB0/TIM3 CH3和PB1/ADC_ IN9/TIM3_CH4控制，產(chǎn)生的PWM波分別為PWM1和PWM2兩路輸出，IN1-IN4分別接PA6、PA7、PB0、PB1控制電機(jī)轉(zhuǎn)動(dòng)方向[2]，主控芯片STM32F103與電機(jī)驅(qū)動(dòng)芯片L298N連接圖如圖2所示。

2.3電源電路設(shè)計(jì)

本系統(tǒng)由于電路模塊較多，每個(gè)模塊所需電壓不同，因此電源電路模塊需設(shè)計(jì)多種電源供電，同時(shí)本系統(tǒng)進(jìn)行了電源轉(zhuǎn)換設(shè)計(jì)，以滿足各模塊供電需求。主控芯片STM32F103需3.3 V供電，電機(jī)驅(qū)動(dòng)模塊分別采用5V、12 V電源供電，紅外收發(fā)檢測(cè)模塊需采用5 V供電，外部電源模塊需采用12 V的直流電壓供電。

(1)直流電壓12V轉(zhuǎn)換成5V電壓的電路

采用KA7805芯片實(shí)現(xiàn)12 V到5 V的轉(zhuǎn)換。KA7805是常用的穩(wěn)壓芯片，使用方便,管腳較少，易于連接和實(shí)現(xiàn)，穩(wěn)定性高，用很簡(jiǎn)單的電路便能夠?qū)?2V直流電壓，轉(zhuǎn)換成輸出5V的直流電壓，電路如圖3所示。

(2)直流電壓5V轉(zhuǎn)換成3.3V電壓的電路

本系統(tǒng)采用LM1117將直流電壓5V轉(zhuǎn)換成3.3V電壓，具體電路設(shè)計(jì)如圖3所示。LM1117是一款正電壓輸出的低壓降三端線性穩(wěn)壓電路，它有兩種類型即輸出版本固定電壓輸出版本和可調(diào)電壓輸出版本。固定輸出電壓有1.5V、1.8V、2.5V、3.3V、5.0V；可調(diào)電壓版本輸出范圍是1.25V-13.8V，電壓精度為1%；它內(nèi)部集成過(guò)熱保護(hù)和限流電路，適用于各類電子產(chǎn)品。

圖3　電源電路

2.4車體姿態(tài)控制設(shè)計(jì)

本系統(tǒng)車體姿態(tài)硬件控制設(shè)計(jì)采用MPU6050模塊實(shí)現(xiàn)，如圖2所示。MPU6050的角速度全格感測(cè)范圍為±250、±500、±1000與±2000°/sec (dps)，可準(zhǔn)確追蹤快速與慢速動(dòng)作，并且，可程式控制的加速器全格感測(cè)范圍為±2g、±4g、±8g與±16g[3]。MPU6050可工作在不同電壓下，供電電壓為2.5V±5%、3.0V±5%或3.3V ±5%。邏輯接口VVDIO供電為1.8V± 5%，本系統(tǒng)中MPU-6000 (6050)與芯片PB10、PB11相連，主控芯片STM32與MPU6050傳感器接線圖如圖2所示，車體姿態(tài)平衡控制結(jié)構(gòu)框圖如圖4所示。

圖4　平衡控制結(jié)構(gòu)框圖

3　系統(tǒng)軟件設(shè)計(jì)

3.1系統(tǒng)軟件

系統(tǒng)軟件設(shè)計(jì)主要包括系統(tǒng)初始化、MPU6050數(shù)據(jù)采集、Kalman數(shù)據(jù)濾波數(shù)據(jù)融合、傾角PID控制算法、電機(jī)PWM輸出等，系統(tǒng)軟件流程圖如圖5所示。

3.2Kalman濾波融合算法

Kalman卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器，它能夠從一系列的不完全包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)?；舅枷胧遣捎眯盘?hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，求出現(xiàn)在時(shí)刻的估計(jì)值實(shí)質(zhì)，由量測(cè)值重構(gòu)系統(tǒng)的狀態(tài)向量。它以“預(yù)測(cè)—實(shí)測(cè)—修正”的順序遞推，根據(jù)量測(cè)值來(lái)消除隨機(jī)干擾，再現(xiàn)系統(tǒng)的狀態(tài)[4]。

圖5　系統(tǒng)軟件設(shè)計(jì)流程

程序段如下所示：

void Kalman_Filter(float Angle_Kal,float Gyro_Kal)

{

........

g_fAngle += (Gyro_Kal - q_bias) * dt;//先驗(yàn)估計(jì)

Pdot[0] = Q_angle - P[0][1] - P[1][0]; // Pk-’ 先驗(yàn)估計(jì)誤差協(xié)方差的微分

Pdot[1] =- P[1][1];

Pdot[2] =- P[1][1];

Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] * dt;// Pk- 先驗(yàn)估計(jì)誤差協(xié)方差微分的積分 = 先驗(yàn)估計(jì)誤差協(xié)方差

P[0][1] += Pdot[1] * dt;

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

angle_err = Angle_Kal - g_fAngle;//zk-先驗(yàn)估計(jì)

PCt_0 = C_0 * P[0][0];

PCt_1 =C_0 *P[1][0];

E =R_angle+ C_0 *PCt_0;

K_0 = PCt_0 / E;//Kk

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = P[0][1];//C_0 *

P[0][0] -= K_0 * t_0;//后驗(yàn)估計(jì)誤差協(xié)方差

P[0][1] -= K_0 * t_1;

P[1][0] -= K_1 * t_0;

P[1][1] -= K_1 * t_1;

g_fAngle+= K_0 * angle_err;//后驗(yàn)估計(jì)

q_bias += K_1 * angle_err; //后驗(yàn)估計(jì)

g_fAngle_Dot = Gyro_Kal - q_bias;//輸出值(后驗(yàn)估計(jì))的微分 = 角速度

}

3.3電機(jī)控制算法

在本系統(tǒng)中，電機(jī)的控制算法起到了十分關(guān)鍵的作用，采用PID控制算法來(lái)控制電機(jī)，因?yàn)镻ID獲取的輸入是紅外傳感器的角度和角速度值的誤差值，并不是絕對(duì)數(shù)值，而是一個(gè)變量，因此采取增量PID算法。通過(guò)增量PID算法，每次輸出是電機(jī)PWM占空比的值[5]。如：

u(k) = u(k-1) + u

先將Ti設(shè)置為無(wú)窮大，Td設(shè)置為0，調(diào)節(jié)Kp，然后再調(diào)節(jié)Ti，最后是Td。

PID控制器的輸入輸出關(guān)系：

其中e(t)是實(shí)際值與設(shè)定值的偏差。

Kp Ki Kd 是系數(shù)

比如這里我們假設(shè)車子直立的角度為0(設(shè)定值)，實(shí)際車子向前傾斜了10°(實(shí)際值)，偏差就是10°

4　結(jié)論

本系統(tǒng)以STM32為主控芯片，實(shí)現(xiàn)對(duì)兩輪平衡移動(dòng)機(jī)器人的控制。系統(tǒng)利用STM32集成開發(fā)環(huán)境IAR EWARM 5.3編寫各模塊程序，包含卡爾曼濾波函數(shù)，速度位置閉環(huán)函數(shù)，陀螺儀加速度采集函數(shù)，避障模塊和紅外循跡模塊，并通過(guò)主控制程序?qū)⒏髂K鏈接，實(shí)現(xiàn)兩輪移動(dòng)機(jī)器人的前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)等自平衡動(dòng)作。整個(gè)系統(tǒng)硬件通過(guò)PROTEL DXP繪制原理圖，并組裝調(diào)試成實(shí)物。自平衡小車運(yùn)動(dòng)系統(tǒng)能與實(shí)際應(yīng)用相結(jié)合，實(shí)現(xiàn)物品的分揀、配料、搬運(yùn)等智能控制，具有普遍的實(shí)用價(jià)值和現(xiàn)實(shí)意義。

參考文獻(xiàn)

[1]楊莘.基于STM32的兩輪自平衡遙控小車[J].數(shù)控技術(shù)應(yīng)用,2014,5.

[2]董杰.基于STM32的智能循跡往返小車設(shè)計(jì)[J].電子設(shè)計(jì)工程,2013,12.

[3]王建明.四旋翼飛行器設(shè)計(jì)[J].軍民兩用技術(shù)與產(chǎn)品,2014,7.

[4]秦永元.卡爾曼濾波與組合導(dǎo)航原理[M].西北工業(yè)大學(xué)出版社,2012,6.

[5]劉金琨.先進(jìn)PID控制MATLAB仿真[M].電子工業(yè)出版社,2011,3.

萬(wàn)軍（1978—），男，四川廣安人，碩士，講師，重慶科創(chuàng)職業(yè)學(xué)院電信教研室教師，主要從事電子信息工程教學(xué)研究。

作者簡(jiǎn)介：