一種兩輪自平衡小車的設計與實現

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

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

?

一種兩輪自平衡小車的設計與實現

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

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

摘要：針對于現在流行的兩輪自平衡車，設計了一個與其原理相似的平衡小車控制系統.自平衡小車本身是一個不穩定體，類似于傳統的倒立擺與蹊蹺板平衡系統.以STM32F103C8T6為主控芯片，利用MPU6050采集小車的姿態數據，通過卡爾曼濾波算法對數據進行處理，最后采用PID控制算法計算電機的PWM值以控制電機的轉動，實現小車的平衡.系統采用藍牙通信技術，通過手機APP實現控制小車的運動.

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

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

1系統總體設計方案

本系統采用Cortex-M3內核的STM32處理器為主控芯片，利用MPU6050來采集小車的姿態數據，通過卡爾曼濾波算法獲得使小車更穩定的加速度和角速度，之后，利用PID控制算法得到準確的PWM值，通過TB6612直流電機驅動模塊有效地控制電機的轉速和轉向，使得小車達到平衡狀態.其次，本設計采用藍牙無線通信技術，通過手機APP來操作小車的行駛[2].

1.1系統硬件設計方案

系統硬件結構主要由處理器、姿態控制模塊，電機、顯示、電源以及手機APP組成，系統結構圖如圖1所示[3].

1.1.1主控芯片

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

1.1.2車體姿態平衡模塊

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

圖1　系統硬件結構框圖

圖2　小車姿態平衡控制

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

1.1.3電機驅動模塊

本系統采用額定轉速285rpm、自帶高精度磁編碼器的直流減速電機，精度高，抗干擾能力強.TB6612是一款新型的驅動器件，能夠獨立控制2個直流電機，每個通道輸出可達1.2A的連續驅動電流，啟動峰值電流達2A～3.2A，有正傳、反轉、制動、停止4種電機控制模式.

1.1.4藍牙通信模塊

藍牙無線模塊采用SPP-C藍牙模塊，遵循V2.1+EDR藍牙規范，采用串口協議，具有成本低、體積小、功耗低、收發靈敏性高等優點.手機圖形界面的中間最小圓圈滑置不同的位置時，STM32產生中斷并作出相應的處理[7].

1.1.5電源模塊

系統包含12V、5V、3.3V電源.采用12V的航模電池，同時也是直流電機的供電來源.電機驅動模塊和藍牙通信模塊采用5V的電源，5V電源是通過LM2596穩壓得到的.STM32主控芯片的工作電源是3.3V，5V通過AMS1117穩壓得到的.

1.2系統軟件設計方案

1.2.1系統程序設計總方案

圖3　系統程序設計

軟件設計的任務是通過卡爾曼濾波算法得到準確的角速度與角度，在通過PID控制算法精確的控制小車的正反轉.系統程序設計流程圖如圖3所示.

1.2.2卡爾曼濾波算法

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

void Kalman_Filter(float Accel,float Gyro)

{

angle+=(Gyro - Q_bias) * dt; //先驗估計

Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協方差的微分

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

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

Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt; // Pk-先驗估計誤差協方差的積分

PP[0][1] += Pdot[1] * dt; // =先驗估計誤差協方差

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

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

Angle_err = Accel - angle;//zk-先驗估計

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; //后驗估計誤差協方差

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

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

Angle+= K_0 * Angle_err; //后驗估計

Q_bias+= K_1 * Angle_err; //后驗估計

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

}

1.2.3PID控制算法

對于小車的平衡控制算法，可以從以下三個方面去調整：直立環、速度環、轉向環[9].

(1)直立環程序

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

{

float Bias,kp=300,kd=1;

int balance;

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

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

return balance;

}

(2)速度環程序

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;//積分得到位移和時間

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;//電機關閉清除數據

return Velocity;

}

(3)轉向環程序

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) //調整小車的起始速度，增加小車的適應性

{

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結束語

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

圖4　系統實物

參考文獻：

[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]盧秀和,力嗨銘.兩輪自平衡小車姿態平衡控制系統的研究[J].數控技術,2013，(2):9-12.

[3]王良成, 楊志民, 胡聰聰,等. 兩輪自平衡小車的設計與實現[J]. 實驗室科學,2012,(6):52-55.

[4]賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統設計[J].河南工程學院學報(自然科學版),2014，(1):53-57.

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

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

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

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

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

(作者本人校對)

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

中圖分類號:TP273

文獻標識碼：A

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

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

收稿日期：2015-12-09