熊躍軍，史明政，林金勇，方　燁

(長沙大學(xué)電子信息與電氣工程系，湖南 長沙 410022)

?

一種兩輪自平衡小車的設(shè)計(jì)與實(shí)現(xiàn)

熊躍軍，史明政，林金勇，方燁

(長沙大學(xué)電子信息與電氣工程系，湖南 長沙 410022)

摘要：針對(duì)于現(xiàn)在流行的兩輪自平衡車，設(shè)計(jì)了一個(gè)與其原理相似的平衡小車控制系統(tǒng).自平衡小車本身是一個(gè)不穩(wěn)定體，類似于傳統(tǒng)的倒立擺與蹊蹺板平衡系統(tǒng).以STM32F103C8T6為主控芯片，利用MPU6050采集小車的姿態(tài)數(shù)據(jù)，通過卡爾曼濾波算法對(duì)數(shù)據(jù)進(jìn)行處理，最后采用PID控制算法計(jì)算電機(jī)的PWM值以控制電機(jī)的轉(zhuǎn)動(dòng)，實(shí)現(xiàn)小車的平衡.系統(tǒng)采用藍(lán)牙通信技術(shù)，通過手機(jī)APP實(shí)現(xiàn)控制小車的運(yùn)動(dòng).

關(guān)鍵詞：STM32；兩輪自平衡小車；MPU6050；卡爾曼濾波；PID

早在1987年，日本電信大學(xué)教授山藤一雄就提出了兩輪自平衡機(jī)器人的概念.這個(gè)基本的概念就是用數(shù)字處理器來偵測平衡的改變，然后以平行的雙輪來保持機(jī)器的平穩(wěn).本世紀(jì)初瑞士聯(lián)邦工業(yè)大學(xué)的Joe、美國的SegwayN等兩輪自平衡機(jī)器人相繼問世，世界各國越來越多的機(jī)器人愛好者和研究者開始關(guān)注兩輪自平衡機(jī)器人.美國發(fā)明家狄恩?卡門與他的DEKA研發(fā)公司研發(fā)出了可以用于載人的兩輪自平衡車命名為賽格威，并已投入市場.由于兩輪自平衡車有著活動(dòng)靈便，環(huán)境無害等優(yōu)點(diǎn)，其被廣泛應(yīng)用于各類高規(guī)格社會(huì)活動(dòng)，目前該車已用于奧運(yùn)會(huì)以及世博會(huì)等大型場合[1].為了提高小車的控制精度與穩(wěn)定性，本文采用高性能的STM32芯片，模擬兩輪直立自平衡電動(dòng)車的原理，采用藍(lán)牙通信技術(shù)來實(shí)現(xiàn)對(duì)小車的操作.

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

本系統(tǒng)采用Cortex-M3內(nèi)核的STM32處理器為主控芯片，利用MPU6050來采集小車的姿態(tài)數(shù)據(jù)，通過卡爾曼濾波算法獲得使小車更穩(wěn)定的加速度和角速度，之后，利用PID控制算法得到準(zhǔn)確的PWM值，通過TB6612直流電機(jī)驅(qū)動(dòng)模塊有效地控制電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向，使得小車達(dá)到平衡狀態(tài).其次，本設(shè)計(jì)采用藍(lán)牙無線通信技術(shù)，通過手機(jī)APP來操作小車的行駛[2].

1.1系統(tǒng)硬件設(shè)計(jì)方案

系統(tǒng)硬件結(jié)構(gòu)主要由處理器、姿態(tài)控制模塊，電機(jī)、顯示、電源以及手機(jī)APP組成，系統(tǒng)結(jié)構(gòu)圖如圖1所示[3].

1.1.1主控芯片

系統(tǒng)采用的主控芯片是STM32F103C8T6單片機(jī)，該單片機(jī)采用32位Cortex-M3內(nèi)核，最高工作頻率為72MHz.芯片上集成了64KB Flash存儲(chǔ)器和20KB SRAM存儲(chǔ)器，同時(shí)也集成了SPI、USART、I2C等通信接口，12位、16通道的AD轉(zhuǎn)換器，能夠產(chǎn)生多路PWM控制電機(jī).

1.1.2車體姿態(tài)平衡模塊

MPU6050六軸傳感器芯片整合了3軸陀螺儀和3軸加速度計(jì)，每個(gè)軸對(duì)應(yīng)一個(gè)16位的AD轉(zhuǎn)換器，分別采集x軸、y軸、z軸的電壓值，將其測量的模擬量轉(zhuǎn)換成數(shù)字量輸出，最后通過I2C總線傳送到STM32，但此時(shí)得到的值并不是實(shí)際的角度和角速度值，還需經(jīng)過一定的的比例關(guān)系進(jìn)行轉(zhuǎn)換，才能得到實(shí)際的角度和角速度值[4].

圖1　系統(tǒng)硬件結(jié)構(gòu)框圖

圖2　小車姿態(tài)平衡控制

為了減少陀螺儀的漂移及車體擺動(dòng)等對(duì)小車加速度計(jì)的影響，獲得準(zhǔn)確的車體傾角值，需要對(duì)加速度計(jì)和陀螺儀的輸出值進(jìn)行融合，系統(tǒng)采用卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合[5].卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器)，它能從一系列的不完全包括噪聲的測量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài).當(dāng)兩輪自平衡小車產(chǎn)生傾斜時(shí)，系統(tǒng)采用PID控制算法，通過整合車體角度、角速度等值，輸出PWM信號(hào)驅(qū)動(dòng)電機(jī)，從而使小車保持動(dòng)態(tài)平衡，其結(jié)構(gòu)框圖如圖2所示，圖中的Kd,kp,ksp,kis是PID控制器的相關(guān)參數(shù)[6].

1.1.3電機(jī)驅(qū)動(dòng)模塊

本系統(tǒng)采用額定轉(zhuǎn)速285rpm、自帶高精度磁編碼器的直流減速電機(jī)，精度高，抗干擾能力強(qiáng).TB6612是一款新型的驅(qū)動(dòng)器件，能夠獨(dú)立控制2個(gè)直流電機(jī)，每個(gè)通道輸出可達(dá)1.2A的連續(xù)驅(qū)動(dòng)電流，啟動(dòng)峰值電流達(dá)2A～3.2A，有正傳、反轉(zhuǎn)、制動(dòng)、停止4種電機(jī)控制模式.

1.1.4藍(lán)牙通信模塊

藍(lán)牙無線模塊采用SPP-C藍(lán)牙模塊，遵循V2.1+EDR藍(lán)牙規(guī)范，采用串口協(xié)議，具有成本低、體積小、功耗低、收發(fā)靈敏性高等優(yōu)點(diǎn).手機(jī)圖形界面的中間最小圓圈滑置不同的位置時(shí)，STM32產(chǎn)生中斷并作出相應(yīng)的處理[7].

1.1.5電源模塊

系統(tǒng)包含12V、5V、3.3V電源.采用12V的航模電池，同時(shí)也是直流電機(jī)的供電來源.電機(jī)驅(qū)動(dòng)模塊和藍(lán)牙通信模塊采用5V的電源，5V電源是通過LM2596穩(wěn)壓得到的.STM32主控芯片的工作電源是3.3V，5V通過AMS1117穩(wěn)壓得到的.

1.2系統(tǒng)軟件設(shè)計(jì)方案

1.2.1系統(tǒng)程序設(shè)計(jì)總方案

圖3　系統(tǒng)程序設(shè)計(jì)

軟件設(shè)計(jì)的任務(wù)是通過卡爾曼濾波算法得到準(zhǔn)確的角速度與角度，在通過PID控制算法精確的控制小車的正反轉(zhuǎn).系統(tǒng)程序設(shè)計(jì)流程圖如圖3所示.

1.2.2卡爾曼濾波算法

卡爾曼濾波器是一個(gè)“最優(yōu)化自回歸數(shù)據(jù)處理算法”.對(duì)于解決大部分的問題，它是最優(yōu)，效率最高甚至是最有用的.其廣泛應(yīng)用已經(jīng)超過30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等.近年來更被應(yīng)用于計(jì)算機(jī)圖像處理，例如，面部識(shí)別，圖像分割，圖像邊緣檢測等方面.在小車控制系統(tǒng)中，卡爾曼濾波算法是用來得到小車的角速度和加速度[8].程序如下.

void Kalman_Filter(float Accel,float Gyro)

{

angle+=(Gyro - Q_bias) * dt; //先驗(yàn)估計(jì)

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

Pdot[1]=-PP[1][1];

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

Pdot[3]=Q_gyro;

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

PP[0][1] += Pdot[1] * dt; // =先驗(yàn)估計(jì)誤差協(xié)方差

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

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

Angle_err = Accel - angle;//zk-先驗(yàn)估計(jì)

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

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

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * PP[0][1];

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

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

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

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

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

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

}

1.2.3PID控制算法

對(duì)于小車的平衡控制算法，可以從以下三個(gè)方面去調(diào)整：直立環(huán)、速度環(huán)、轉(zhuǎn)向環(huán)[9].

(1)直立環(huán)程序

int balance(float Angle,float Gyro)// 比例微分PD控制器

{

float Bias,kp=300,kd=1;

int balance;

Bias=Angle-1;//求出平衡的角度中值和機(jī)械中值

balance=kp*Bias+Gyro*kd;//計(jì)算平衡控制的電機(jī)PWM

return balance;

}

(2)速度環(huán)程序

int velocity(int encoder_left,int encoder_right)// 比例積分PI控制器

{

static float Velocity,Encoder_Least,Encoder,Movement;

static float Encoder_Integral;

float kp=80,ki=0.40;

if(1==Flag_Qian)Movement=-90/Flag_sudu;

else if(1==Flag_Hou)Movement=90/Flag_sudu;

elseMovement=0;

Encoder_Least=(Encoder_Left+Encoder_Right)-0;//獲得最新速度偏差

Encoder *= 0.7;//一階低通濾波器

Encoder += Encoder_Least*0.3;//一階低通濾波器

Encoder_Integral +=Encoder;//積分得到位移和時(shí)間

Encoder_Integral=Encoder_Integral-Movement;

if(Encoder_Integral>10000)Encoder_Integral=10000;//積分限幅

if(Encoder_Integral<-10000)Encoder_Integral=-10000;//積分限幅

Velocity=Encoder*kp+Encoder_Integral*ki;//速度控制

if(Turn_Off(Angle_Balance,Voltage)==1) Encoder_Integral=0;//電機(jī)關(guān)閉清除數(shù)據(jù)

return Velocity;

}

(3)轉(zhuǎn)向環(huán)程序

int turn(int encoder_left,int encoder_right,float gyro)// 比例P控制器

{

static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;

float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=0;

if(1==Flag_Left||1==Flag_Right) //調(diào)整小車的起始速度，增加小車的適應(yīng)性

{

if(++Turn_Count==1)

Encoder_temp=myabs(encoder_left+encoder_right);

Turn_Convert=50/Encoder_temp;

if(Turn_Convert<0.6)Turn_Convert=0.6;

if(Turn_Convert>3)Turn_Convert=3;

}

Else

{

Turn_Convert=0.9;

Turn_Count=0;

Encoder_temp=0;

}

if(1==Flag_Left)Turn_Target-=Turn_Convert;

else if(1==Flag_Right)Turn_Target+=Turn_Convert;

else Turn_Target=0;

if(Turn_Target>Turn_Amplitude)Turn_Target=Turn_Amplitude;if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;

if(Flag_Qian==1||Flag_Hou==1)Kd=1;

else Kd=0;//模糊PID控制

Turn=-Turn_Target*Kp -gyro*Kd;

return Turn;

}

2結(jié)束語

本系統(tǒng)介紹了一種以STM32F103C8T6微處理器、采用卡爾曼濾波算法和PID控制算法的兩輪自平衡小車控制系統(tǒng).實(shí)驗(yàn)測試表明，在一定的干擾的情況下，如斜坡、車頂放置一定量的物體，小車仍能保持平衡，小車在手機(jī)APP通過藍(lán)牙設(shè)備發(fā)出的指令下可實(shí)現(xiàn)前進(jìn)、倒退、轉(zhuǎn)彎、停止等動(dòng)作，該過程相對(duì)無遙控情況下更加穩(wěn)定.測試結(jié)果表明，利用卡爾曼濾波算法能有效的避免陀螺儀的漂移及車體擺動(dòng)等對(duì)小車加速度計(jì)的影響，準(zhǔn)確計(jì)算出姿態(tài)傾角，結(jié)合PID控制算法，從而實(shí)現(xiàn)小車的平衡，并且具有自動(dòng)恢復(fù)平衡的功能，適用于機(jī)器人排爆，狹小空間工作.系統(tǒng)實(shí)物如圖4.

圖4　系統(tǒng)實(shí)物

參考文獻(xiàn)：

[1]Wan E A, Ronell V D M. The unscented Kalman filter for nonlinear estimation[A]. Proceedings of the 2000 Symposium on Adaptive Systems for Signal Processing, Communications and Control[C]. New York,2000:153-158.

[2]盧秀和,力嗨銘.兩輪自平衡小車姿態(tài)平衡控制系統(tǒng)的研究[J].數(shù)控技術(shù),2013，(2):9-12.

[3]王良成, 楊志民, 胡聰聰,等. 兩輪自平衡小車的設(shè)計(jì)與實(shí)現(xiàn)[J]. 實(shí)驗(yàn)室科學(xué),2012,(6):52-55.

[4]賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計(jì)[J].河南工程學(xué)院學(xué)報(bào)(自然科學(xué)版),2014，(1):53-57.

[5]魏文,段晨東,高精隆,等.基于數(shù)據(jù)融合的兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)[J].電子技術(shù)應(yīng)用,2013,(5):79-81.

[6]馮智勇,曾瀚,張力,等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測量[J].西南師范大學(xué)學(xué)報(bào),2011,(4):137-41.

[7]楊莘,劉海濤.基于STM32的兩輪自平衡遙控小車[J].數(shù)字技術(shù)與開發(fā),2014,(5):151-152.

[8]任國華,唐昌進(jìn),蔣剛.一種基于Kalman 濾波的雙輪機(jī)器人姿態(tài)控制算法[J].機(jī)械設(shè)計(jì)與制造,2013,(2):39-44.

[9]毛歡,張彬彬.采用改進(jìn)PID 算法的雙輪自平衡小車研制[J].工業(yè)控制計(jì)算機(jī),2014,(1):55-56.

(作者本人校對(duì))

Design and Realization of a Two-wheel Self-balanced Vehicle

XIONG Yuejun, SHI Mingzheng, LIN Jinyong, FANG Ye

(Department of Electronic Information and Electrical Engineering, Changsha University，Changsha Hunan 410022, China)

Abstract：Aiming at the popular two-wheel self-balanced vehicle, this article designed a similar vehicle. It is an unstable system which is similar to traditional inverted pendulum balancing system. In this system, STM32F103C8T6 is selected as the microcontroller, and MPU6050 is used in attitude data acquisition. Kalman filtering algorithm is used to process the data, and PWM results for motor control are obtained by using PID algorithm. Ultimately, bluetooth communication and mobile APP technologies are used in the movement control of this vehicle .

Key Words：STM32; two-wheel self-balanced vehicle; MPU6050; Kalman filter; PID

中圖分類號(hào):TP273

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1008-4681(2016)02-0039-04

作者簡介：熊躍軍(1978— )，男，湖南汨羅人，長沙大學(xué)電子信息與電氣工程系講師，碩士.研究方向：嵌入式系統(tǒng).

收稿日期：2015-12-09