兩輪自平衡機器人控制系統設計

孫夢凡 桂文杰

（安徽工業大學電氣與信息工程學院 安徽 馬鞍山 243002）

0.引言

兩輪機器人在近十幾年得到了飛速發展，可廣泛應用于戰場偵察，緊急服務，智能輪椅[1]，郵件派送，危險物品的運輸等等，其便捷、靈活、節能的特點不僅可以提供便捷服務，還可以降低人力成本提高工作效率。此外，兩輪自平衡機器人集中運用了微電子，傳感器，計算機，通信，人工智能，自動化控制等等這些技術，是典型的高新技術綜合體[2]。其對保障交通安全，緩解交通擁擠，改善交通環境，完善未來智能交通系統也具有深遠的意義，與傳統輪式機器人相比，兩輪自平衡機器人具有如下特點：

（1）機器人移動路徑和方向更加靈活，容易控制，還可實現較小的移動范圍，因此相比傳統移動機器人應用領域更廣泛。

（2）車體結構大大簡化，為進一步實現新的功能提供空間，也適用于活動空間小或要求靈活運輸的場合，例如步行街，廣場，游樂會場等。

（3）平衡車驅動功率小，續航時間長，提出了環保輕型車的一種新模式[3]。

（4）相較于四輪車，平衡車的剎車系統更為靈敏和安全，車身比四輪車小很多，所產生的動能也相應小了很多。

雖然兩輪機器人應用場合廣泛，性能也較四輪機器人具有明顯優勢。然而如何維持機器人在復雜環境下的平衡和穩定一直是人們關注的問題，本文針對這一問題提出了一種兩輪自平衡機器人控制系統。該系統以STM32F103C8T6 單片機為控制核心，整合分析MPU6050 采集的角度和加速度信息以及光電碼盤采集的速信息來調整左右電機轉速來維持機器人平衡。該系統不僅能在移動時維持平衡而且能夠在一定干擾下自主調整姿態恢復平衡和穩定，同時具備在不同路徑下自主循跡的功能，較好的滿足了人們對機器人高穩定度和適應性強的要求。

1.系統總體控制方案

本系統總體分為兩大模塊：硬件設計模塊、軟件設計模塊。硬件設計模塊主要由STM32F103C8T6 單片機作為控制核心，MPU6050 傳感器采集機器人的姿態，即機器人傾角θ 和角加速度X(t)，光電碼盤檢測機器人的速度，所有檢測信息反饋給單片機后經分析計算控制調整左右電機轉速以實現機器人平衡以及其他移動方式。在機器人維持平穩移動基礎上模擬灰度傳感器檢測輸出模擬信號AD 值，AD 值較大即檢測白色區域，相反則黑色區域，根據黑色軌跡所在檢測區域的位置調整機器人轉向使之行駛回正確軌跡，實現循跡功能。軟件設計模塊主要分為三類控制設計：平衡控制、速度控制、方向控制，三者在控制運動過程中相互聯系相互影響，其中平衡控制是基礎，其他控制均在平衡控制的基礎上實現。

所設計系統根據陀螺儀和光電碼盤采集的數據結合卡爾曼濾波控制算法對機器人進行融合控制，實現機器人平穩運行以及其他移動方式。系統總體控制方案圖如圖1 所示。

圖1 系統總體控制方案圖

2.機器人硬件設計方案

為降低機器人的重心，使整個系統穩定性更高，系統采用對稱布局的雙層結構。如圖2 是機器人的整體三維結構圖。機身由亞克力板作為上底板，不銹鋼板作為下底板，PCB 板搭在車底盤上，底盤前方安裝四個模擬灰度傳感器，底盤后方安裝兩個數字灰度傳感器，中部放置驅動以及穩壓裝置，單片機主板等。

圖2 機器人三維結構圖

系統采用STM32F103C8T6 單片機作為控制核心，通過MPU6050傳感器采集機器人的姿態，傳感器采集的是機器人傾角θ 和角加速度X(t)，安裝在兩個電機上的光電碼盤檢測得到機器人的速度，檢測信息反饋給單片機后經分析計算相應調整電機轉速以實現機器人平衡以及其他移動方式。在機器人維持平穩移動基礎上模擬灰度傳感器檢測輸出模擬信號AD 值，機器人根據AD 值的大小來判斷檢測的是黑色區域還是白色區域，當檢測到黑色軌跡在機器人正中位置，保持直線行駛；當黑色軌跡在機器人正中偏右（左）位置，左（右）輪電機比右（左）輪電機轉速快，從而調整機器人位置使之行駛回正確軌跡，實現循跡功能。兩輪自平衡機器人硬件控制系統如圖3 所示。

圖3 機器人硬件控制系統

2.1 電源模塊

系統包含的電壓有3.3V，5V，12V，其中整個系統包含的電源來源為12V 18650 鋰電池，5V 電源由7805 穩壓器提供[4]，作為傳感器模塊的供電電壓，LM2596 穩壓芯片提供3.3V 電源，作為單片機的供電電壓。

2.2 電機及驅動模塊

系統選用ASLONG JGA25-370 直流減速電機，這款電機可工作在6V 電壓下，空載轉速可達到77rpm，扭矩0.4，能夠滿足設計需要。驅動選用的是電機驅動器件TB6612FNG，該器件可以驅動兩個電機，STBY 口接單片機的IO 口，清零電機全部停止，置1 通過AIN1、AIN2，BIN1,BIN2來控制正反轉，VM接12V以內電源，VCC接5V電源，TB6612FNG 驅動電路圖如圖4所示。

圖4 TB6612FNG 驅動電路圖

2.3 灰度傳感器

灰度傳感器是模擬傳感器，灰度傳感器原理圖如圖5 所示，發光二極管作為光源與1k 電阻串聯，照在檢測物體上，1K 電阻起限流作用，光敏電阻與10K 電阻串聯，光敏電阻具有光越強阻值越小的特性。當發光二極管照在白色物體上，白色物體反射所有光，亮度強，光敏電阻阻值小，分壓小，輸出的電壓值高；當發光二極管照在黑色物體上，黑色物體吸收所有光，亮度弱，光敏電阻阻值大，分壓大，輸出的電壓值低。單片機根據灰度傳感器輸出的高低電平分析控制左右電機轉速實現機器人循跡。

圖5 灰度傳感器原理圖

2.4 MPU6050

MPU6050 作為機器人姿態測量模塊是一種9 軸運動處理組件，由三軸陀螺儀和三軸加速度計組合而成，加速度計和陀螺儀用來計算機器人行進過程中實時角速度和加速度信息，用以保持車模穩定前進[5]工作電壓為2.4V 至3.5V，其中陀螺儀和加速度計輸出的模擬量經過三個16 位ADC 轉化為數字量[6]，陀螺儀實質是角速度檢測器，在機器人移動過程中單片機每隔一定時間讀取陀螺儀的角速度，經計算得到當前車輪所在位置的角度θ。

式中?i為第i 次陀螺檢測的角速度，ti為第i 次陀螺儀檢測的時間間隔，n 為一定時間檢測的次數。由于陀螺儀運轉一段時間以后，溫度，白噪聲，積分誤差的累加導致的飄移現象等影響，積分并不能夠得到完全準確的姿態，需要加速度傳感器輔助校正，加速度計的低頻特性好，可以測量低速的靜態加速度，機器人根據加速度傳感器測出的重力加速度在x,y,z 軸的分量與重力加速度的比值計算得到傾角α，單位弧度。

式中g 為重力加速度，gx為重力加速度沿x 軸分量，k 為比例系數。最后加速度計讀出的角度和陀螺儀讀出的角度經過卡爾曼濾波融合處理后，所得角度更加準確。

3.系統控制原理分析

系統的控制模塊分為三個部分：平衡控制、方向控制、速度控制。其中平衡控制是其他控制的基礎，所有的控制都是在平衡控制的基礎上完成的。三者相互調節控制系統的三種運動狀態即靜止狀態、直線運動、曲線運動。具體控制原理結構圖如圖6 所示。

圖6 系統控制原理圖

3.1 平衡控制原理

實際控制中，傳感器得到的角速度和加速度傳回給處理器得到小車的傾斜角度，控制系統通過控制電機使兩個車輪向車身傾斜的方向滾動，并相應調整轉速產生扭矩力以維持動態平衡，宏觀上表現為靜止。這里通過一個外力干擾模擬機器人維持平衡的條件，假設外力干擾引起車模產生角加速度x(t)，如圖7 所示，沿著垂直底盤方向進行受力分析，可以得到小車傾角與車輪加速度以及外力干擾加速度a(t)x(t)之間的方程如下：

由以上公式分析得到，在精確檢測機器人傾角θ，控制車輪加速度α 情況下，可以維持平衡，從而驗證機器人具備一定的抗干擾能力。

3.2 速度控制原理

一般的四輪機器人速度控制機制是通過編碼器采集脈沖信號，通過單片機正交解碼之后解析成當前速度值，當前值對比設定值，通過pi算法更新速度誤差，累計誤差到一定數值之后清零，以控制輪子轉速在設定范圍內波動，為負反饋調節機制。對于兩輪直立平衡機器人來說，速度控制在一定意義上是“正反饋控制”，速度環計算得出的量按比例縮小疊加到角度環上，宏觀上為了保持平衡，車身會在設定范圍內加速運動。

3.3 方向控制原理

車體方向控制主要通過控制兩個車輪的速度差實現，由速度環可知，通過改變兩邊輪子的設定速度，車身為了保持平衡，宏觀上會有轉向的表現。車體左轉向時，在在小車平衡的基礎上控制右輪轉速增加一定值，左輪轉速減少一定值；相應的右轉向時在小車平衡的基礎上控制左輪轉速增加一定值，右輪轉速減少一定值。

3.4 狀態分析

小車有靜止、直線運動，曲線運動三種運動狀態，機器人在維持自身平衡的基礎上完成這三種狀態的控制，通過改變角度設定值，會使車身在保持平衡的基礎上，完成各種路徑的運動。角度設定偏差越大，車身運動越快，變化也就越大。

(1)靜止：若車身重心位于電機軸心線正上方，機器人宏觀上將保持靜止狀態，但調控過程中單片機讀取計算出的測量偏差的存在使得電機始終以微小的轉速產生較小的扭矩力來維持小車的平衡。

(2)直線運動：在車身保持平衡基礎上，給予“往前或往后一個方向的力”微觀上表現為設定的零點改變，車身為了保持平衡會不停的往設定方向移動。機器人直線運動中的三種姿態如圖8 所示。

圖8 機器人運動姿態

(3)曲線運動：在車身保持平衡的基礎上，給予“左邊或右邊一個方向的力”微觀上表現為設定差速運動，左右兩輪設定的速度不一樣，宏觀上車身為了保持平衡會保持差速運動即曲線運動。

4.控制系統軟件設計與實現

4.1 平衡控制算法概述

自平衡機器人平衡算法主要由速度環，轉向環和角度環組成，速度環和轉向環控制疊加在角度環上，由圖像處理得到的偏差值疊加到速度設定值，由于車身在賽道上可能會左偏或者右偏，所以左輪右輪單獨控制，兩者基礎設定值一致，根據跑道的不同情況，當前設定值會增大或者減小。根據當前的設定值和編碼器讀取的當前量，會得出速度環的偏差，寫進速度pid 中，得到一個輸出量，兩輪的輸出量根據跑道情況而有所改變。最終p 波輸出的值為（速度環+轉向環）*0.3+角度環*0.7.宏觀表現為車身在姿態矯正的同時實現轉向和直線加速。

用零點減去當前讀取值乘上一個比例系數，歸一化處理，得到兩個新變量G_Now,a_offset。

具體代碼如下：

Gyro_Now=(GYRO_VAL-gyro_z)*Gyro_ratio;//陀螺儀采集到的角速度歸一化

angle_offset_vertical= (MMA7361_vertical-accle_z)*MMA7361_ratio;//加速度計歸一化

最后對加速度計算值作限幅處理，控制在-90～90 范圍內。

代碼如下：

if(angle_offset_vertical>90)angle_offset_vertical=90;

if(angle_offset_vertical＜-90)angle_offset_vertical=-90;

得到兩個變量后，使用卡爾曼濾波進行融合處理

float angle,angle_dot;

const float Q_angle=0.2,Q_gyro=0.1,R_angle=0.5, dt=0.005;

static floatP[2][2]={

{1,0},

{0,1}

};

static float Pdot[4]={0,0,0,0};

static const char C_0=1;

static float q_bias,angle_err,PCt_0,PCt_1,E,K_0,K_1,t_0,t_1;

void Kalman_Filter(float angle_m,floatgyro_m)//gyro_m:gyro_measure

{

angle+=(gyro_m-q_bias)*dt;

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

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

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

Pdot[3]=Q_gyro;

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

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

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

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

angle_err=angle_m-angle;

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;

K_1=PCt_1/E;

t_0=PCt_0;

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

P[0][0]-=K_0*t_0;

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

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

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

angle+=K_0*angle_err;

q_bias+=K_1*angle_err;

angle_dot=gyro_m-q_bias;

}

最后得到擬合角度和角速度的計算值。

最后輸出p 波值speed_Start=angle*P_ANGLE+angle_dot*D_ANGLE;

4.2 速度控制

機器人速度控制是實現自平衡機器人平穩運行最重要的一步，它與機器人的方向控制，直立控制息息先相關[7]，與一般的控制不同的是，速度環是“正反饋”速度環計算出的誤差乘以一個比例系數，按比例縮小，加進角度環，宏觀表現車體為了保持平衡，向前或向后有加速現象。速度控制流框圖如圖9所示。

圖9 速度控制框圖

4.3 轉向控制

轉向環的控制是由攝像頭采集計算一個向左或向右的偏差，加進角度環中，相應左右輪加速度或減速度，宏觀表現車身為了保持平衡，有差速轉向表現。由P*err+d*last_err 計算出的偏差是向左或向右，相應進行特殊處理。如果偏差向左，左輪速度期望值在原基礎上增加轉向所需量，右輪相應減少相應量。由于是雙輪單獨控制，會有兩個獨立的速度環，通過增加或減少預期值以達到讓車子差速轉向的宏觀表現。轉向控制的控制框圖如圖10所示。

圖10 轉向控制框圖

代碼如下：

//$確定速度調節差值$//

speed_delta_thelast=speed_delta_last;

speed_delta_last=speed_delta_now;

speed_delta_now=expect_speed-speed_pulse;

//$速度閉環PID 調節$//

//速度 P_Adjust

pwm_delta_float_p=(speed_delta_now-speed_delta_last)*S_kp;

G_SpeedCtrl=G_SpeedCtrl+pwm_delta_float_p;

//速度 I_Adjust

pwm_delta_float_i=speed_delta_now*S_ki;

G_SpeedCtrl=G_SpeedCtrl+pwm_delta_float_i;

//速度 D_Adjust

pwm _delt a_float_d=(spee d_de lta_now-speed_delt a_las+sp eed_delta_thelast-speed_delta_last)*S_kd;

G_SpeedCtrl=G_SpeedCtrl+pwm_delta_float_d;

//電機輸出值限幅

if(G_SpeedCtrl>3300)G_SpeedCtrl=3300;

else if(G_SpeedCtrl＜1000)G_SpeedCtrl=1000;

M1(G_SpeedCtrl);

M3(G_SpeedCtrl);

}

S_S=P*err+D*last_err;

If(err>0)

S_L=3000+S_S;

S_R=3000-S_S;

Else

S_L=3000-S_S;

S_R=3000+S_S;

If(err>0)

Speed_L=angle+（-abs（S_S）*0.1+abs(G_SpeedCtrl)*0.2）*P_ANGLE+angle_dot*D_ANGLE;

Speed_R=angle+（abs（S_S）*0.1+abs(G_SpeedCtrl)*0.2）*P_ANGLE+ angle_dot*D_ANGLE;

Else

Spee d_L=angle+（abs（S_S）*0.1+abs(G_SpeedCtrl)*0.2）*P_ANGLE+angle_dot*D_ANGLE;

Speed_R=angle+（-abs（S_S）*0.1+abs(G_SpeedCtrl)*0.2）*P_ANGLE+angle_dot*D_ANGLE;

4.4 控制系統軟件總體設計

軟件的主要功能有:系統初始化，傳感器信號的采集、處理，速度檢測，系統的運動狀態控制即平衡控制、速度控制、方向控制[8]。三者之間相互聯系，相互作用，三者關系圖如圖11 所示。具體工作過程和相互影響程度如下：

圖11 系統總體控制關系圖

(1)平衡控制：陀螺儀加速度計采集的當前角度，計算與設定角速度及加速度之間的偏差，輸出相應大小的p 波。平衡控制受速度控制和轉向控制的影響，速度環計算得出相應大小的p 波轉換為對應的角度偏差，疊加到平衡控制中，形成加速現象。陀螺儀讀取的三軸方向的加速度和角速度，計算融合成為歐拉角，通過歐拉角計算出轉向所需的角度偏差。

(2)速度控制：編碼器采集當前脈沖信號，反饋給單片機，與設定值相比，通過pid 矯正，將最終輸出值穩定在當前信號。速度控制對轉向控制的影響，當左右兩輪速度設定值有偏差時，宏觀上會有轉向的現象；速度控制也會受直立控制的影響，最終輸出值為直立控制計算出所需p 波大小加上速度控制設定大小融合而成。

(3)轉向控制：由陀螺儀三軸方向的加速度和角速度六個值融合成歐拉角，轉向所需設定值由歐拉角決定，疊加輸出到速度環中，宏觀上形成轉向。

5.系統調試結果

測試一：姿態檢測。機器人在正常移動時，機身傾角在0°附近變化，0°說明機身基本能維持直立。在人為施加一定外加干擾情況下，機身傾角以0 度為中心波動，且波動偏角越來越小，說明系統基本維持平衡，具備一定的抗干擾能力。

測試二：循跡檢測。機器人在循跡過程中，機身距離中央黑線偏差在2cm 以內，人為改變機身偏角后，機器人自動調整轉向沿原路徑循跡，說明系統實現自主循跡，具有一定的適應性。

6.結語

兩輪循跡機器人在研究平衡過程中采用模塊化分析，分為三大模塊：直立控制，速度控制，方向控制[9]，陀螺儀和光電碼盤采集的數據結合卡爾曼濾波控制算法對機器人進行融合控制實現平穩移動，灰度傳感器采集數值實現循跡，實驗采集的數據均經過單片機分析機處理后控制左右電機轉速實現機器人的功能。實際操作中，各功能模塊相互協調并正常工作，循跡過程中機器人移動范圍不超過2CM，適當干擾下，機器人能夠自主調整并恢復平穩運行狀態，實驗結果表明所提出的的控制方案簡單可靠，具有一定的實用價值。