兩輪自平衡機(jī)器人控制系統(tǒng)設(shè)計(jì)

孫夢凡 桂文杰

（安徽工業(yè)大學(xué)電氣與信息工程學(xué)院 安徽 馬鞍山 243002）

0.引言

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

（1）機(jī)器人移動(dòng)路徑和方向更加靈活，容易控制，還可實(shí)現(xiàn)較小的移動(dòng)范圍，因此相比傳統(tǒng)移動(dòng)機(jī)器人應(yīng)用領(lǐng)域更廣泛。

（2）車體結(jié)構(gòu)大大簡化，為進(jìn)一步實(shí)現(xiàn)新的功能提供空間，也適用于活動(dòng)空間小或要求靈活運(yùn)輸?shù)膱龊希绮叫薪?，廣場，游樂會場等。

（3）平衡車驅(qū)動(dòng)功率小，續(xù)航時(shí)間長，提出了環(huán)保輕型車的一種新模式[3]。

（4）相較于四輪車，平衡車的剎車系統(tǒng)更為靈敏和安全，車身比四輪車小很多，所產(chǎn)生的動(dòng)能也相應(yīng)小了很多。

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

1.系統(tǒng)總體控制方案

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

所設(shè)計(jì)系統(tǒng)根據(jù)陀螺儀和光電碼盤采集的數(shù)據(jù)結(jié)合卡爾曼濾波控制算法對機(jī)器人進(jìn)行融合控制，實(shí)現(xiàn)機(jī)器人平穩(wěn)運(yùn)行以及其他移動(dòng)方式。系統(tǒng)總體控制方案圖如圖1 所示。

圖1 系統(tǒng)總體控制方案圖

2.機(jī)器人硬件設(shè)計(jì)方案

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

圖2 機(jī)器人三維結(jié)構(gòu)圖

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

圖3 機(jī)器人硬件控制系統(tǒng)

2.1 電源模塊

系統(tǒng)包含的電壓有3.3V，5V，12V，其中整個(gè)系統(tǒng)包含的電源來源為12V 18650 鋰電池，5V 電源由7805 穩(wěn)壓器提供[4]，作為傳感器模塊的供電電壓，LM2596 穩(wěn)壓芯片提供3.3V 電源，作為單片機(jī)的供電電壓。

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

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

圖4 TB6612FNG 驅(qū)動(dòng)電路圖

2.3 灰度傳感器

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

圖5 灰度傳感器原理圖

2.4 MPU6050

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

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

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

3.系統(tǒng)控制原理分析

系統(tǒng)的控制模塊分為三個(gè)部分：平衡控制、方向控制、速度控制。其中平衡控制是其他控制的基礎(chǔ)，所有的控制都是在平衡控制的基礎(chǔ)上完成的。三者相互調(diào)節(jié)控制系統(tǒng)的三種運(yùn)動(dòng)狀態(tài)即靜止?fàn)顟B(tài)、直線運(yùn)動(dòng)、曲線運(yùn)動(dòng)。具體控制原理結(jié)構(gòu)圖如圖6 所示。

圖6 系統(tǒng)控制原理圖

3.1 平衡控制原理

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

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

3.2 速度控制原理

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

3.3 方向控制原理

車體方向控制主要通過控制兩個(gè)車輪的速度差實(shí)現(xiàn)，由速度環(huán)可知，通過改變兩邊輪子的設(shè)定速度，車身為了保持平衡，宏觀上會有轉(zhuǎn)向的表現(xiàn)。車體左轉(zhuǎn)向時(shí)，在在小車平衡的基礎(chǔ)上控制右輪轉(zhuǎn)速增加一定值，左輪轉(zhuǎn)速減少一定值；相應(yīng)的右轉(zhuǎn)向時(shí)在小車平衡的基礎(chǔ)上控制左輪轉(zhuǎn)速增加一定值，右輪轉(zhuǎn)速減少一定值。

3.4 狀態(tài)分析

小車有靜止、直線運(yùn)動(dòng)，曲線運(yùn)動(dòng)三種運(yùn)動(dòng)狀態(tài)，機(jī)器人在維持自身平衡的基礎(chǔ)上完成這三種狀態(tài)的控制，通過改變角度設(shè)定值，會使車身在保持平衡的基礎(chǔ)上，完成各種路徑的運(yùn)動(dòng)。角度設(shè)定偏差越大，車身運(yùn)動(dòng)越快，變化也就越大。

(1)靜止：若車身重心位于電機(jī)軸心線正上方，機(jī)器人宏觀上將保持靜止?fàn)顟B(tài)，但調(diào)控過程中單片機(jī)讀取計(jì)算出的測量偏差的存在使得電機(jī)始終以微小的轉(zhuǎn)速產(chǎn)生較小的扭矩力來維持小車的平衡。

(2)直線運(yùn)動(dòng)：在車身保持平衡基礎(chǔ)上，給予“往前或往后一個(gè)方向的力”微觀上表現(xiàn)為設(shè)定的零點(diǎn)改變，車身為了保持平衡會不停的往設(shè)定方向移動(dòng)。機(jī)器人直線運(yùn)動(dòng)中的三種姿態(tài)如圖8 所示。

圖8 機(jī)器人運(yùn)動(dòng)姿態(tài)

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

4.控制系統(tǒng)軟件設(shè)計(jì)與實(shí)現(xiàn)

4.1 平衡控制算法概述

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

用零點(diǎn)減去當(dāng)前讀取值乘上一個(gè)比例系數(shù)，歸一化處理，得到兩個(gè)新變量G_Now,a_offset。

具體代碼如下：

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

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

最后對加速度計(jì)算值作限幅處理，控制在-90～90 范圍內(nèi)。

代碼如下：

if(angle_offset_vertical>90)angle_offset_vertical=90;

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

得到兩個(gè)變量后，使用卡爾曼濾波進(jìn)行融合處理

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;

}

最后得到擬合角度和角速度的計(jì)算值。

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

4.2 速度控制

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

圖9 速度控制框圖

4.3 轉(zhuǎn)向控制

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

圖10 轉(zhuǎn)向控制框圖

代碼如下：

//$確定速度調(diào)節(jié)差值$//

speed_delta_thelast=speed_delta_last;

speed_delta_last=speed_delta_now;

speed_delta_now=expect_speed-speed_pulse;

//$速度閉環(huán)PID 調(diào)節(jié)$//

//速度 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;

//電機(jī)輸出值限幅

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 控制系統(tǒng)軟件總體設(shè)計(jì)

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

圖11 系統(tǒng)總體控制關(guān)系圖

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

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

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

5.系統(tǒng)調(diào)試結(jié)果

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

測試二：循跡檢測。機(jī)器人在循跡過程中，機(jī)身距離中央黑線偏差在2cm 以內(nèi)，人為改變機(jī)身偏角后，機(jī)器人自動(dòng)調(diào)整轉(zhuǎn)向沿原路徑循跡，說明系統(tǒng)實(shí)現(xiàn)自主循跡，具有一定的適應(yīng)性。

6.結(jié)語

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