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

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

（1.韶關學院 自動化系，韶關 512005；2.韶關學院 教育學院，韶關 512126）

0 引言

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

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

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

圖1 平衡機器人機械結構

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

圖2 機器人平衡原理流程圖

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

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

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

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

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

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

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

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

3.2 加速度傳感器ADXL345

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

圖5 加速度傳感器電路

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

從傳感器讀出的數(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接口進行通訊，采用3.3V供電，其應用電路如圖4所示。

圖6 陀螺儀傳感器電路

3.4 電機驅動電路

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

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

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

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

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

圖8 PID控制流圖

5 實驗結果

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

圖9 傾角值波形

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

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

圖10 平衡測試曲線

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

6 結束語

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