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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

系統(tǒng)電源輸入采用12V電源進行供電，LM2596開關(guān)電壓調(diào)節(jié)器是降壓型電源管理單片集成電路，能夠輸出3A的驅(qū)動電流，同時具有很好的線性和負載調(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軸加速度計，該加速度傳感器電路的應用電路如圖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 電機驅(qū)動電路

使用減速直流電機減速比為1:30的GB37電機作為兩輪自平衡機器人的驅(qū)動電機，電機采用H橋驅(qū)動方式。電機驅(qū)動器件采用TB6612FNG，具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時驅(qū)動兩個GB37電機，外圍驅(qū)動電路簡單[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)用于轉(zhuǎn)速測量和控制，控制電機輸出，使平衡機器人車輪轉(zhuǎn)速與角度環(huán)的輸出值保持一致；外環(huán)是角度環(huán)，始終要保持小車傾斜角度盡可能等于0。圖中U為平衡機器人角度的目標值（始終為0），作為系統(tǒng)的輸入。X0是角度環(huán)PID的輸出值，也是電機轉(zhuǎn)速目標值。X1為車輪的轉(zhuǎn)速，Y為實際的傾斜角度。Z為擾動輸入。PID控制器控制流圖如圖8所示。

圖8 PID控制流圖

5 實驗結(jié)果

為了方便測試，將傳感器數(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ì)量變好。

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

圖10 平衡測試曲線

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

6 結(jié)束語

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