兩輪平衡機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

韓竺秦，彭昕昀，劉文秀，張麗娜

（1.韶關(guān)學(xué)院 自動(dòng)化系，韶關(guān) 512005；2.韶關(guān)學(xué)院 教育學(xué)院，韶關(guān) 512126）

0 引言

本文研究?jī)?nèi)容有兩輪自平衡機(jī)器人的姿態(tài)檢測(cè)算法和PID控制算法兩方面。姿態(tài)檢測(cè)算法通過(guò)Kalman濾波器融合姿態(tài)傳感器測(cè)量的數(shù)據(jù)，獲得準(zhǔn)確的姿態(tài)信息[1]，PID調(diào)節(jié)器則利用這些姿態(tài)信息，輸出電機(jī)控制信號(hào)，控制電機(jī)的轉(zhuǎn)動(dòng)，使機(jī)器人得以平衡。

1 系統(tǒng)模型設(shè)計(jì)

現(xiàn)有的自平衡機(jī)器人結(jié)構(gòu)種類繁多，本設(shè)計(jì)將采用圖1的結(jié)構(gòu)，由上中下三層組成，上層電池層、中間主控層和底層電機(jī)驅(qū)動(dòng)層。電池層用于放置給整個(gè)系統(tǒng)供電的12V電池，主控層由主控芯片最小系統(tǒng)和傳感器模塊組成，電機(jī)驅(qū)動(dòng)層接受單片機(jī)信號(hào)，并控制電機(jī)。每個(gè)層都是功能模塊的電路路板之間用銅柱支撐固定，電機(jī)外殼與電機(jī)驅(qū)動(dòng)電路板固定，電機(jī)轉(zhuǎn)軸與兩只輪胎相連。

圖1 平衡機(jī)器人機(jī)械結(jié)構(gòu)

兩輪平衡機(jī)器人，其核心問(wèn)題就是運(yùn)動(dòng)平衡控制問(wèn)題，兩輪機(jī)器人始終要保持直立的姿勢(shì)，在保持姿勢(shì)平衡的同時(shí)，還需要完成各種任務(wù)，行進(jìn)、旋轉(zhuǎn)、爬坡、越障等。完成這些任務(wù)必然會(huì)有俯仰角θ角度的產(chǎn)生，因此，欲使小車平衡，必須要減小俯仰角θ角甚至要消除θ角，一般都需要把θ控制在一定的范圍內(nèi)[2]。其整體控制思想流程如圖2所示。

圖2 機(jī)器人平衡原理流程圖

消除θ角度的有效方法是：STM32控制電機(jī)的轉(zhuǎn)動(dòng)帶動(dòng)底盤部分進(jìn)行移動(dòng)，確保與車身上部和底部在同一個(gè)垂直線上。

2 系統(tǒng)方案分析

系統(tǒng)采用STM32F103C8T6為主控芯片，通過(guò)I2C接口讀取傳感器L3G4200和傳感器ADXL345的數(shù)據(jù)，陀螺儀和加速度級(jí)的數(shù)據(jù)融合測(cè)出自平衡機(jī)器人的相關(guān)姿態(tài)，最終通過(guò)PID輸出PWM電機(jī)控制信號(hào)對(duì)電機(jī)進(jìn)行有效的控制。在系統(tǒng)運(yùn)行時(shí)將需要觀察的數(shù)據(jù)通過(guò)串口發(fā)送到上位機(jī)，以記錄相關(guān)的數(shù)據(jù)和繪制出有關(guān)波形，便于查看濾波和PID效果。系統(tǒng)總體框架設(shè)計(jì)圖如圖3所示。

圖3 兩輪機(jī)器人系統(tǒng)總體框架設(shè)計(jì)

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

3.1 電源系統(tǒng)設(shè)計(jì)

系統(tǒng)電源輸入采用12V電源進(jìn)行供電，LM2596開(kāi)關(guān)電壓調(diào)節(jié)器是降壓型電源管理單片集成電路，能夠輸出3A的驅(qū)動(dòng)電流，同時(shí)具有很好的線性和負(fù)載調(diào)節(jié)特性[3,4]。STM32F103VET6以及其它的外圍芯片都是3.3V供電，所以我們需要用到5V轉(zhuǎn)3.3V的芯片，AMS1117-3.3V電源芯片。這個(gè)電源芯片屬于線性電源芯片，比開(kāi)關(guān)電源芯片的噪聲要好很多。最大輸出1A電流，給主控板供電完全夠用。電源供電系統(tǒng)如圖4所示。

圖4 系統(tǒng)供電系統(tǒng)

3.2 加速度傳感器ADXL345

ADXL345是一款由ANALOG DEVICES公司生產(chǎn)3軸加速度計(jì)，該加速度傳感器電路的應(yīng)用電路如圖5所示。

圖5 加速度傳感器電路

ADXL345可以在傾斜檢測(cè)應(yīng)用中測(cè)量靜態(tài)重力加速度，還可以測(cè)量運(yùn)動(dòng)或沖擊導(dǎo)致的動(dòng)態(tài)加速度。其高分辨率（3.9mg/LSB），能夠測(cè)量不到1.0°的傾斜角度變化[5]。主控芯片通過(guò)I2C即可初始化該芯片并讀取三軸的加速度值。單片機(jī)可以通過(guò)此外部中斷，觸發(fā)對(duì)三軸加速度數(shù)據(jù)的讀取，用中斷觸發(fā)的方式可以較少系統(tǒng)計(jì)算周期，提高電機(jī)有效控制頻率。

從傳感器讀出的數(shù)據(jù)通過(guò)一定的運(yùn)行可以算出傳感器的各類傾斜角，其中，θ正是平衡車要消除的角度，車身傾斜角度可以通過(guò)式(1)計(jì)算出。式(1)中，A是各軸輸出的加速度分量。

3.3 陀螺儀傳感器L3G4200

ADXL345主要是用來(lái)測(cè)量靜態(tài)重力加速度，但是系統(tǒng)的動(dòng)態(tài)的加速度值是干擾，用L3G4200測(cè)量出自平衡車的角速度值能在一定程度上消除動(dòng)態(tài)的干擾，利用ADXL345和L3G4200能夠解算出車的傾角信息。L3G4200提供一個(gè)16位數(shù)據(jù)輸出[6]，可配置的低通和高通濾波器等嵌入式數(shù)字功能。與加速度傳感器的數(shù)字接口一致，通過(guò)用SDA和SCL與主控芯片的硬件I2C接口進(jìn)行通訊，采用3.3V供電，其應(yīng)用電路如圖4所示。

圖6 陀螺儀傳感器電路

3.4 電機(jī)驅(qū)動(dòng)電路

使用減速直流電機(jī)減速比為1:30的GB37電機(jī)作為兩輪自平衡機(jī)器人的驅(qū)動(dòng)電機(jī)，電機(jī)采用H橋驅(qū)動(dòng)方式。電機(jī)驅(qū)動(dòng)器件采用TB6612FNG，具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)兩個(gè)GB37電機(jī)，外圍驅(qū)動(dòng)電路簡(jiǎn)單[7]。對(duì)于PWM信號(hào)輸入頻率范圍，高達(dá)100kHz的頻率足以滿足我們的需求了。

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

軟件程序設(shè)計(jì)流程上，系統(tǒng)上電復(fù)位之后，先初始化各個(gè)功能模塊，并進(jìn)行平衡機(jī)器人的姿態(tài)檢測(cè)和PID控制。同時(shí)為了前期調(diào)試已經(jīng)查看數(shù)據(jù)，使用了STM32主控的串口將程序中產(chǎn)生的數(shù)據(jù)如估算出的最終角度等，通過(guò)UART串口上傳到上位機(jī)，可以在串口測(cè)試軟件上看到數(shù)據(jù)的特性。系統(tǒng)軟件總體流程如圖7所示。

圖7 系統(tǒng)軟件總流程圖

自平衡車采用PID控制，內(nèi)環(huán)用于轉(zhuǎn)速測(cè)量和控制，控制電機(jī)輸出，使平衡機(jī)器人車輪轉(zhuǎn)速與角度環(huán)的輸出值保持一致；外環(huán)是角度環(huán)，始終要保持小車傾斜角度盡可能等于0。圖中U為平衡機(jī)器人角度的目標(biāo)值（始終為0），作為系統(tǒng)的輸入。X0是角度環(huán)PID的輸出值，也是電機(jī)轉(zhuǎn)速目標(biāo)值。X1為車輪的轉(zhuǎn)速，Y為實(shí)際的傾斜角度。Z為擾動(dòng)輸入。PID控制器控制流圖如圖8所示。

圖8 PID控制流圖

5 實(shí)驗(yàn)結(jié)果

為了方便測(cè)試，將傳感器數(shù)據(jù)通過(guò)UART串口輸?shù)缴衔粰C(jī)，用串口波形軟件將數(shù)據(jù)以波形的形式在電腦顯示器上顯示。如圖9是由加速度計(jì)測(cè)出來(lái)的未經(jīng)過(guò)濾波處理的角度波形圖。

圖9 傾角值波形

圖9(a)是未經(jīng)濾波前的傾角圖，在第7秒之前靜放系統(tǒng)，此時(shí)測(cè)出來(lái)的傾角為0°，波形基本穩(wěn)定不變，但在7秒后加入擾動(dòng)，其波形開(kāi)始發(fā)生劇烈的變化。此波形說(shuō)明了僅由加速度計(jì)計(jì)算出來(lái)的傾角存在太多干擾。因此，需要一種有效的濾波器，濾除非重力加速度分量。對(duì)兩個(gè)傳感器的數(shù)據(jù)進(jìn)行融合，減少測(cè)量誤差。圖9(b)所示，加了低通濾波器的波形與未經(jīng)過(guò)濾波的波形進(jìn)行比較，濾波器最優(yōu)估計(jì)方法有較好的功效，角度跟隨質(zhì)量變好。

自平衡車設(shè)計(jì)的關(guān)鍵就是兩輪平衡機(jī)器人行進(jìn)速度和行進(jìn)方向的控制。設(shè)計(jì)中通過(guò)對(duì)PID控制器的調(diào)試，可以驗(yàn)證控制系統(tǒng)的性能穩(wěn)定和算法的有效性。

圖10 平衡測(cè)試曲線

由圖10可以看出，兩輪平衡機(jī)器人車在初始傾角為19°左右時(shí)，經(jīng)過(guò)0.8秒的振動(dòng)之后后迅速回到動(dòng)態(tài)平衡狀態(tài)，然后車的傾角在一定的范圍內(nèi)震蕩并達(dá)到新的平衡。

6 結(jié)束語(yǔ)

兩輪自平衡機(jī)器人是一個(gè)多變量、非線性、強(qiáng)耦合的系統(tǒng)，分析了自平衡機(jī)器人的主要控制原理，研發(fā)了TM32F103C8T6為核心的控制系統(tǒng)，利用Kalman濾波對(duì)采集的數(shù)融合，設(shè)計(jì)了一款自平衡性較好的兩輪機(jī)器人，實(shí)現(xiàn)了平衡轉(zhuǎn)向、行走等控制任務(wù)，實(shí)驗(yàn)結(jié)果顯示自平衡性顯著，抗抖動(dòng)性較好，證明所設(shè)計(jì)控制方案可行，滿足設(shè)計(jì)要求。