王娜　孫思遠(yuǎn)

【摘 要】?jī)奢喿云胶庑≤?chē)是一種不穩(wěn)定系統(tǒng)，只有通過(guò)調(diào)節(jié)兩輪的運(yùn)動(dòng)快慢和方向才能使其達(dá)到動(dòng)態(tài)的平衡。本文基于stm32系列單片機(jī)，從硬件原理開(kāi)始分析說(shuō)明，闡述了控制系統(tǒng)的硬件設(shè)計(jì)，對(duì)陀螺儀和加速度傳感器測(cè)量實(shí)際傾角的過(guò)程和方法做了仔細(xì)分析說(shuō)明，并設(shè)計(jì)了雙閉環(huán) PID 控制器，同時(shí)對(duì)系統(tǒng)中涉及的濾波算法和PID控制算法做了理論分析，給出了整個(gè)系統(tǒng)的軟件控制流程。通過(guò)實(shí)驗(yàn)表明兩輪自平衡小車(chē)基本能夠?qū)崿F(xiàn)直立控制。

【關(guān)鍵詞】平衡控制；PID控制器；卡爾曼濾波；STM32；傳感器

0 引言

移動(dòng)機(jī)器人技術(shù)是目前科學(xué)技術(shù)發(fā)展最活躍的領(lǐng)域之一，而兩輪自平衡小車(chē)又是移動(dòng)機(jī)器人研究中的一個(gè)重要領(lǐng)域[1]，兩輪自平衡小車(chē)系統(tǒng)具有本質(zhì)不穩(wěn)定、多變量、非線性、強(qiáng)耦合等特點(diǎn)，并且涉及到復(fù)雜的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)系統(tǒng)和很多控制算法[2]。本文在對(duì)該控制理論分析的基礎(chǔ)上通過(guò)PID控制和卡爾曼濾波，最終實(shí)現(xiàn)對(duì)平衡車(chē)的系統(tǒng)設(shè)計(jì)。

1 自平衡兩輪電動(dòng)車(chē)的運(yùn)動(dòng)控制原理

由于該模型是單軸雙輪，所以在車(chē)體與豎直方向產(chǎn)生傾角的時(shí)候，小車(chē)就會(huì)在車(chē)體重力沿水平方向的分力作用下運(yùn)動(dòng)而不能保持靜止，但是我們可以通過(guò)電機(jī)控制兩個(gè)輪子的轉(zhuǎn)速和轉(zhuǎn)向，從而使其抵消其傾斜的趨勢(shì)，這樣便可以使兩輪小車(chē)保持自平衡。[3]

兩輪自平衡小車(chē)的總體控制原理如圖1所示。整個(gè)系統(tǒng)的核心是STM32F103C8T6單片機(jī)，該系統(tǒng)主要由運(yùn)動(dòng)處理傳感器、電機(jī)驅(qū)動(dòng)、電源管理電路、串口通信、直流無(wú)刷電機(jī)和磁電編碼器組成。MPU6050內(nèi)部集成陀螺儀和加速度計(jì)，分別用于測(cè)量角速度和角加速度，并將采集到的數(shù)據(jù)存放到FIFO寄存器，通過(guò)I2C協(xié)議傳輸給單片機(jī)后進(jìn)行卡爾曼濾波，就可以得到正確的傾角。而電機(jī)的轉(zhuǎn)速則是由磁電編碼器返回到單片機(jī)并計(jì)算出一定時(shí)間內(nèi)的計(jì)數(shù)來(lái)判斷車(chē)速的大小和方向。電機(jī)驅(qū)動(dòng)則選用TB6612FNG來(lái)控制兩個(gè)直流電機(jī)的正轉(zhuǎn)、反轉(zhuǎn)、制動(dòng)和調(diào)速功能。串口通信主要是進(jìn)行程序的燒寫(xiě)。電源模塊的主要作用是提供各電路工作所需的電源。

2 系統(tǒng)的硬件設(shè)計(jì)及實(shí)現(xiàn)

2.1 單片機(jī)選型

主控芯片采用STM32F103C8T6單片機(jī)，它作為一種高性能、低成本、低功耗的ARM Cortex-3內(nèi)核單片機(jī)，比傳統(tǒng)的51單片機(jī)應(yīng)用更加廣泛，體積更加小巧。最高時(shí)鐘頻率可達(dá)72MHz，而工作電壓只有3.3V，它的外設(shè)包括10個(gè)定時(shí)器、4路PWM、兩個(gè)12位模數(shù)轉(zhuǎn)換器、兩個(gè)12位數(shù)模轉(zhuǎn)換器、兩個(gè)I2C接口、五個(gè)USART接口和三個(gè)SPI端口。新產(chǎn)品外設(shè)共有12條DMA通道，可以用ARM內(nèi)核的Keil工具進(jìn)行編程。

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

本系統(tǒng)的電源共涉及三個(gè)電壓值，分別為12V、5V、3.3V。12V用于驅(qū)動(dòng)電機(jī)，3.3V作為單片機(jī)和傳感器的邏輯電平，5V則作為驅(qū)動(dòng)電機(jī)的電平，而且在供電設(shè)計(jì)時(shí)的壓降也不宜過(guò)大，否則容易使芯片發(fā)熱量過(guò)大。利用軟包電池組或者其他電池作為12V直流電源，先通過(guò)LM2596穩(wěn)壓芯片將12V轉(zhuǎn)換為5V，再通過(guò)AMS1117-3.3將5V轉(zhuǎn)換為3.3V電源。其應(yīng)用方式均可在相應(yīng)芯片資料中得到。

2.3 測(cè)速系統(tǒng)的設(shè)計(jì)

測(cè)速設(shè)計(jì)采用的是AB相增量式霍爾編碼器，其原理是采用磁阻或者霍爾元件對(duì)變化的磁性材料的角度或者位移值進(jìn)行測(cè)量，磁性材料角度或者位移的變化會(huì)引起一定電阻或者電壓的變化，通過(guò)放大電路對(duì)變化量進(jìn)行放大，通過(guò)單片機(jī)處理后輸出脈沖信號(hào)或者模擬量信號(hào)，達(dá)到測(cè)量的目的。

其核心代碼如下：

Encoder_TIM= （short）TIM2 -> CNT； TIM2 -> CNT=0；

通過(guò)一段時(shí)間內(nèi)計(jì)數(shù)器的計(jì)數(shù)個(gè)數(shù)來(lái)判斷速度大小，其具體速度值還需要進(jìn)一步處理。

2.4 電機(jī)控制芯片選型

電機(jī)控制采用芯片TB6612FNG，它有6路輸入8路輸出，可控制兩個(gè)直流電機(jī)，邏輯電平輸入為5V，驅(qū)動(dòng)電壓為12V。每四個(gè)輸出控制一個(gè)電機(jī)，每三個(gè)輸入都可以實(shí)現(xiàn)一組電機(jī)的正轉(zhuǎn)、反轉(zhuǎn)、制動(dòng)。而速度的調(diào)節(jié)則由單片機(jī)的計(jì)時(shí)器提供的PWM波實(shí)現(xiàn)。

3 車(chē)身傾角的獲取

3.1 運(yùn)動(dòng)處理傳感器的選取和編碼

對(duì)于車(chē)身傾角的獲取采用MPU6050運(yùn)動(dòng)處理傳感器，它內(nèi)部包含GYRO（陀螺儀）用于采集角速度和ACCEL（加速度計(jì)）用于采集加速度，由于陀螺儀的溫漂還集成了溫度傳感器。采集后的數(shù)據(jù)存入FIFO寄存器，可以輸出6軸旋轉(zhuǎn)矩陣，分別對(duì)應(yīng)xyz軸的角度和角加速度。其并不需要AD轉(zhuǎn)換的步驟，而是利用I2C同步通訊與單片機(jī)傳輸數(shù)據(jù)，提高了系統(tǒng)運(yùn)行效率。

其采集數(shù)據(jù)的核心代碼為：

MPU6050_FIFO[i][j-1]=MPU6050_FIFO[i][j]；

該數(shù)組表示對(duì)第i個(gè)軸上的第j-1個(gè)數(shù)據(jù)進(jìn)行更新，通過(guò)循環(huán)即可得到一段時(shí)間內(nèi)的i*j的矩陣。為了減小噪聲干擾，可以取多組數(shù)據(jù)平均值即為該時(shí)刻的運(yùn)動(dòng)狀態(tài)。

3.2 傳感器數(shù)據(jù)的處理

在傳感器采集到數(shù)據(jù)后，就會(huì)通過(guò)I2C傳輸給單片機(jī)進(jìn)行下一步處理，此時(shí)就可以得到FIFO寄存器中的數(shù)據(jù)。加速度計(jì)可以測(cè)量動(dòng)態(tài)和靜態(tài)的線性加速度，則加速度計(jì)輸出的加速度與車(chē)身傾角的關(guān)系為：

？茲=tan-1（Ax/Ay）

其中Ax為x軸方向上的加速度，Ay為y軸方向上的加速度。在C程序中可以用math.h里面的函數(shù)計(jì)算，其代碼為：

angle_Y=atan2（Accel_X，Accel_Z）*180/PI；

根據(jù)小車(chē)模型及焊接方向即可確定傾角的旋轉(zhuǎn)軸，此處以y軸為例，為了計(jì)算簡(jiǎn)便最好在設(shè)計(jì)之初確定好MPU6050的軸向。

陀螺儀的直接輸出值是相對(duì)靈敏軸的角速率，角速率對(duì)時(shí)間積分即可得到圍繞靈敏軸旋轉(zhuǎn)過(guò)的角度值，在C程序中利用累加即可完成。

4 整體的控制流程

首先對(duì)整個(gè)電路的工作方式進(jìn)行設(shè)置，包括系統(tǒng)時(shí)鐘初始化、中斷初始化、ADC初始化、傳感器初始化等。然后傳感器開(kāi)始工作，采集到的加速度經(jīng)過(guò)公式轉(zhuǎn)換為角度值，而角速度經(jīng)過(guò)積分得到角度或者直接進(jìn)行濾波，這里采用互補(bǔ)濾波。

其中Ti表示積分時(shí)間常數(shù)；Td表示微分時(shí)間常數(shù)。經(jīng)過(guò)濾波后的角度值為PD控制，而對(duì)編碼器返回的速度值為PI控制，最后輸出的增量值賦值給相應(yīng)寄存器來(lái)調(diào)節(jié)PWM占空比，最后驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)，使小車(chē)達(dá)到平衡。

5 結(jié)論

實(shí)際試驗(yàn)表明我們的設(shè)計(jì)基本實(shí)現(xiàn)了控制小車(chē)防傾倒和行動(dòng)的目的，對(duì)濾波算法和PID算法進(jìn)行了說(shuō)明，同時(shí)降低了硬件成本和能耗。我們將會(huì)在此基礎(chǔ)上做更進(jìn)一步的深入研究，相信未來(lái)兩輪自平衡車(chē)技術(shù)也將會(huì)得到更大的發(fā)展。

【參考文獻(xiàn)】

[1]徐國(guó)華，譚民.移動(dòng)機(jī)器人的發(fā)展現(xiàn)狀及其趨勢(shì)[J].機(jī)器人技術(shù)與應(yīng)用，2001，14（3）：7-14.

[2]鄭曉偉，陳萬(wàn)米.基于DSP的兩輪自平衡小車(chē)系統(tǒng)的研究[J].工業(yè)控制計(jì)算機(jī)，2012，25（9）：33-35.

[3]楊亞龍，劉金棟，孫玉環(huán)，丁婷.基于 STC12C5410AD 單片機(jī)的兩輪自平衡小車(chē)的系統(tǒng)設(shè)計(jì)[J].電子設(shè)計(jì)工程，2013，10（20）：162-164.

[4]張洋，劉軍，嚴(yán)漢宇.原子教你玩STM32（庫(kù)函數(shù)版）[M].北京：北京航天航空大學(xué)出版社，2013.

[5]李寧.ARM MCU開(kāi)發(fā)工具M(jìn)DK使用入門(mén)[M].北京：北京航天航空大學(xué)出版社，2012.

[6]歐陽(yáng)駿，李英芬，王小強(qiáng)，等.瘋狂STM32實(shí)戰(zhàn)講學(xué)錄[M].北京：中國(guó)水利水電出版社，2013.

[7]陳海宴.51單片機(jī)原理及應(yīng)用——基于Keil C與Proteus（第2版）[M].北京：北京航天航空大學(xué)出版社，2010.

[8]何欽銘，顏暉.C語(yǔ)言程序設(shè)計(jì)[M].北京：高等教育出版社，2012.

[責(zé)任編輯：王偉平]