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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 加速度傳感器ADXL345

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

圖5 加速度傳感器電路

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

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

3.3 陀螺儀傳感器L3G4200

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

圖6 陀螺儀傳感器電路

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

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

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

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

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

自平衡車采用PID控制，內(nèi)環(huán)用于轉(zhuǎn)速測量和控制，控制電機(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為實際的傾斜角度。Z為擾動輸入。PID控制器控制流圖如圖8所示。

圖8 PID控制流圖

5 實驗結(jié)果

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

圖9 傾角值波形

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

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

圖10 平衡測試曲線

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

6 結(jié)束語

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