天津理工大學(xué) 王孟軒 陳子娟 汪 煊 李秀順

基于FPGA平衡小車的設(shè)計

天津理工大學(xué) 王孟軒 陳子娟 汪 煊 李秀順

隨著科技進(jìn)步，生活水平的提高，人們追求智能與舒適的愿望也日益強(qiáng)烈。從而催生了許多智能化的產(chǎn)品。如智能電視、智能小車等。如何實現(xiàn)小車的自動快捷駕駛，也成為人們心中的向往與疑問，基于這種趨勢與需求，著眼于實際情況。本文介紹了基于MPU6050的自平衡小車系統(tǒng)的設(shè)計。系統(tǒng)基于陀螺儀等傳感器采集姿態(tài)信息，通過對比分析一階互補(bǔ)濾波和卡爾曼濾波的優(yōu)缺點(diǎn)，采用卡爾曼濾波算法，并利用PID平衡算法，對小車的速度傾斜角度平衡狀態(tài)來進(jìn)行檢測，并通過FPGA來控制電機(jī)來實現(xiàn)雙輪小車自如平衡地運(yùn)動。從而實現(xiàn)小車智能自主控制的目的。本設(shè)計獲得了2016年“京微雅格杯FPGA應(yīng)用大賽”一等獎。

FPGA；平衡車；PID算法；卡爾曼濾波

1 設(shè)計原理

1.1 產(chǎn)品調(diào)研

目前，電動平衡車主控芯片大多數(shù)是使用ST的一款32單片機(jī)——STM32F103系列，單片機(jī)由于是單線程，程序語句需要等待單片機(jī)周期才能執(zhí)行。因此基于單片機(jī)設(shè)計的程序運(yùn)行速度較慢，而FPGA由于是硬件電路，運(yùn)行速度直接取決于晶振速度，速度較快且系統(tǒng)穩(wěn)定。所以本設(shè)計擬采用基于FPGA的CME—M7系列芯片和TB6612FNG、HC-05、MPU6050完成雙輪電動車的設(shè)計。

1.2 原理示意圖

圖1 原理示意圖

1.3 設(shè)計原理

雙輪平衡車的控制思想是基于一級倒立擺的模型，當(dāng)車體發(fā)生傾斜時，傳感器會檢測到車體角度的變化，控制系統(tǒng)根據(jù)車體角度的變化，控制電機(jī)向傾斜方向轉(zhuǎn)動，從而產(chǎn)生抵抗傾斜的力矩，使車身維持在平衡狀態(tài)。當(dāng)車體不發(fā)生傾斜時，平衡車會在小范圍內(nèi)實現(xiàn)動態(tài)平衡。所以，傾角越大加速度越快。在此策略下，平衡車能夠?qū)崿F(xiàn)靜止、前進(jìn)、后退等各種運(yùn)動狀態(tài)，從而保證平衡車的穩(wěn)定運(yùn)行。小車作為一個控制對象，它的輸出量是兩個電機(jī)的轉(zhuǎn)動速度[1]。小車的運(yùn)動控制任務(wù)可以分解成以下三個基本任務(wù)：

控制小車平衡：通過控制兩個電機(jī)正反向運(yùn)動保持小車直立平衡狀態(tài)；

控制小車速度：通過調(diào)節(jié)小車的傾角來實現(xiàn)小車速度控制，實際上最后還是演變成通過控制電機(jī)的轉(zhuǎn)速來實現(xiàn)車輪速度的控制。

控制小車方向：通過控制兩個電機(jī)之間的轉(zhuǎn)動差速實現(xiàn)小車轉(zhuǎn)向控制。

小車直立和方向控制任務(wù)都是直接通過控制小車兩個電機(jī)完成的。在實際控制中，是將控制小車直立和方向的控制信號疊加在一起加載電機(jī)上，只要電機(jī)處于線性狀態(tài)就可以同時完成上面兩個任務(wù)。小車的速度是通過調(diào)節(jié)小車傾角來完成的。小車不同的傾角會引起小車的加減速的變化，從而達(dá)到對于速度的控制。

由上述原理分析可知，車身需要采集的主控芯片信息有車身加速度、車身角加速度、電機(jī)速度和位置信息等。因此本設(shè)計以陀螺儀和加速度計作為傳感器，采用FPGA作為主控芯片，通過電機(jī)驅(qū)動電路驅(qū)動直流電機(jī)實現(xiàn)電動雙輪玩具小車的姿態(tài)控制?？刂破魍ㄟ^IIC串口通信協(xié)議讀取傳感器的姿態(tài)信息，經(jīng)過濾波算法后進(jìn)行數(shù)據(jù)的融合分析，得到小車的姿態(tài)，再通過姿態(tài)更新后，進(jìn)入控制模塊，控制算法根據(jù)小車的姿態(tài)計算出驅(qū)動電機(jī)所需的PWM信號，電機(jī)控制部分主要算法采用PID算法，再通過雙閉環(huán)反饋控制（速度閉環(huán)控制和位置閉環(huán)控制），使小車穩(wěn)定運(yùn)行達(dá)到動態(tài)平衡。

2 設(shè)計流程

圖2 設(shè)計流程圖

2.1 姿態(tài)信息采集模塊

為實現(xiàn)小車的直立行走，需要利用MPU6050姿態(tài)傳感器和霍爾正交編碼器采集以下信息：

（1）小車傾角速度陀螺儀信號，獲得小車的傾角和角速度。（2）重力加速度信號（z軸信號），補(bǔ)償陀螺儀的漂移。

（3）小車電機(jī)轉(zhuǎn)速脈沖信號，獲得小車運(yùn)動速度和位置信息，進(jìn)行速度控制。

2.2 卡爾曼濾波器

兩輪自平衡小車系統(tǒng)具有多變量、非線性、強(qiáng)耦合、參數(shù)不確定等特性，是一個集動態(tài)決策和規(guī)劃、環(huán)境感知、行為控制和執(zhí)行等多種功能于一體的綜合復(fù)雜系統(tǒng).因此，采集小車的姿態(tài)信息很困難，所以我們必須增加濾波模塊以排除信號噪聲和動態(tài)加速度的干擾[2]。通常濾波分為一階濾波、互補(bǔ)濾波和卡爾曼濾波。

卡爾曼濾波（Kalman filtering）一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。在本系統(tǒng)中，測量角度和測量角速度存在著導(dǎo)數(shù)關(guān)系，在這種情況下，我們不直接估計車體的角速度，而是采用加速度計的測量值來估算偏差gyro_bias , gyro_bias可以作為狀態(tài)向量的另一個元素。我們把車體的角度和角速度用入下關(guān)系時表示:

式中：anglek為估算的k時刻傾角；

anglek-1上一時刻加速計的測量值；

dt為采樣時間。

過程噪聲的協(xié)方差矩陣形式如下：

R=[R_angle]為觀測噪聲的協(xié)方差矩陣，且車體的角度與角速度中含有的噪聲為白噪聲加速度計測得的車體角度的過程噪聲的協(xié)方差為Q_ angle，陀螺儀測得的車體的角速度的過程噪聲協(xié)方差為Q-gyro。它們?nèi)〔煌闹荡砹瞬灰粯拥男湃纬潭取Ｈ绻J(rèn)為陀螺儀的可信度更高，可以把Q-gyro設(shè)置的小一些，如果認(rèn)為加速度計的可信度更高可以把Q_ angle設(shè)置的小一些。R矩陣代表觀測噪聲的協(xié)方差，加入R很大，說明系統(tǒng)具有很大的噪聲。

仿真數(shù)據(jù)分析：

由式(2-1)我們可以編寫具體的卡爾曼濾波算法，并加以仿真。

在仿真過程中，通過調(diào)整協(xié)方差系數(shù)，可以控制濾波效果，仿真結(jié)果如圖3所示。通過圖3可以看出，加上噪音后，輸入卡爾曼濾波器的信號品質(zhì)很差，通過卡爾曼濾波器濾波后，消除了輸出信號的噪聲，輸出信號可以很好地跟蹤理想信號。

圖3 仿真結(jié)果圖

通過分析仿真及實測數(shù)據(jù)，可以看出，所設(shè)計的控制系統(tǒng)能夠很好地控制自平衡兩輪車直立，且自平衡車前后擺動較小，這證明了該控制器設(shè)計合理，控制器各參數(shù)選擇合適，硬件系統(tǒng)工作正常。

2.3 姿態(tài)信息更新

雙輪玩具小車是非線性、多階次、不穩(wěn)定的系統(tǒng)，要實現(xiàn)其動態(tài)平衡，控制小車完成相應(yīng)的動作，即給出相應(yīng)的姿態(tài)信息，將控制信號重新寫入存儲器，由于其平衡狀態(tài)是處于動態(tài)，所以需要不斷的進(jìn)行測量更新，針對兩輪自平衡車傾角的測量問題,采用了卡爾曼多傳感融合算法。該算法融合了加速度傳感器和陀螺儀傳感器數(shù)據(jù),實現(xiàn)了兩輪自平衡車傾角實時在線估算，控制器給出控制信息后又控制系統(tǒng)驅(qū)動電機(jī)工作，采用了比例微分(PID)控制算法,實現(xiàn)了兩輪自平衡車姿態(tài)的穩(wěn)定控制[3]。

2.4 電機(jī)驅(qū)動控制

2.4.1 雙輪平衡車控制特點(diǎn)

兩輪平衡車需要解決的問題是在前進(jìn)、靜止、退過程中都要保持車體的平衡。它使一級倒立擺的運(yùn)動方式變得更加靈活，更加多樣化。對于不完整非線性的系統(tǒng)，考慮下面介紹的PID控制方法:

PID控制是最常見的控制算法，它主要是對被控系統(tǒng)進(jìn)行偏差調(diào)節(jié)。實現(xiàn)被控量的值與要求的值達(dá)到一致。但為了適應(yīng)不同的過程，需要給出合理的控制規(guī)律，否則，PID控制器達(dá)不到預(yù)定效果。控制算法根據(jù)小車的姿態(tài)信息計算出PWM信號，再通過閉環(huán)反饋控制使小車完成相應(yīng)的動作。根據(jù)角度角速度變化，控制電機(jī)來完成當(dāng)前的平衡，以及在車體在運(yùn)動過程中的各個姿態(tài)。

2.4.2 速度、位置雙閉環(huán)控制

我們一般在速度閉環(huán)控制系統(tǒng)里面，使用增量式PI控制，而在我們的微處理器里面，因為控制器是通過軟件實現(xiàn)其控制算法的，所以必須對模擬調(diào)節(jié)器進(jìn)行離散化處理，這樣它只需根據(jù)采樣時刻的偏差值計算控制量。因此，我們需要使用離散的差分方程連續(xù)的差分方程。如下部分給出了小車的雙閉環(huán)控制原理

速度、位置雙環(huán)控制由以上兩個控制器進(jìn)行嵌套而成因為雙閉環(huán)控制過程較復(fù)雜，故給出其控制原理圖4如下：

圖4 雙環(huán)控制原理圖

2.4.3 PWM輸出

要產(chǎn)生頻率和脈沖寬度可調(diào)的PWM波形，這可通過FPGA豐富的硬件資源和可以配置I/O引腳來實現(xiàn)。嵌入式系統(tǒng)中FPGA的應(yīng)用設(shè)計關(guān)鍵是系統(tǒng)軟硬件功能的劃分。通過計數(shù)器的方式實現(xiàn)PWM的輸出。

2.4.4 電機(jī)PID控制算法的反饋系數(shù)的確定

由上文中PID控制的原理可知，比例環(huán)節(jié)輸出的控制信號與系統(tǒng)誤差e(t)成比例，比例系數(shù)越大，系統(tǒng)反應(yīng)越靈敏，精度越高，但也會使調(diào)節(jié)震蕩更劇烈，甚至造成系統(tǒng)不穩(wěn)定。積分環(huán)節(jié)輸出的控制信號與誤差的積分成比例，

積分系數(shù)大，可以更快地消除穩(wěn)態(tài)誤差，但如果積分系數(shù)太大，則會產(chǎn)生較大的超調(diào)和長時間的波動。微分環(huán)節(jié)輸出的控制信號與誤差的變化率成比例，微分調(diào)節(jié)主要作用是對系統(tǒng)變化做出預(yù)警，產(chǎn)生提前的調(diào)節(jié)。微分控制可以使系統(tǒng)相位超前，抵消積分作用造成的相位滯后，同時可以減小超調(diào)，但微分系數(shù)過大，可能造成系統(tǒng)不穩(wěn)定[4]。

根據(jù)這一原理，進(jìn)行了大量的實驗調(diào)控，得到比較符合要求的實驗數(shù)據(jù)，再通過大數(shù)據(jù)的分析和計算確定反饋比例系數(shù)。由于本系統(tǒng)采用CME——M7，其具有ARM內(nèi)核，可以做SOC，能將算法級的復(fù)雜程序直接放在ARM內(nèi)核中，降低程序的開發(fā)難度。

3 最終成果展示

該設(shè)計采用PID控制算法對角度，角速度進(jìn)行控制，使角速度可控，使系統(tǒng)反應(yīng)時間大大縮短，電機(jī)驅(qū)動的PWM輸出，如圖5所示。實物小車展示表明小車確實可以平衡穩(wěn)定的運(yùn)行，當(dāng)小車具有一定傾角的時候小車能保持平衡并能根據(jù)傾角完成相應(yīng)的動作，以及展示小車抗干擾能力和對復(fù)雜路面的適應(yīng)能力。

圖5 平衡小車實物展示

[1]楊世勇,徐麗萍,王培進(jìn).單級倒立擺的PID控制研究[J].控制工程,2007(B05):23-24.

[2]蔡述庭.“飛思卡爾”杯智能汽車競賽設(shè)計與實踐:基于S12XS和Kineris K10[M].北京:北京航空航天大學(xué)出版社,2012:248-251.

[3]張志強(qiáng).基于STM32的雙輪平衡車[A].電子設(shè)計工程,2011:103-106.

[4]袁澤睿.兩輪自平衡機(jī)器人控制算法的研究 [D].哈爾濱 :哈爾濱工業(yè)大學(xué),2006:22-31.

王孟軒（1993-），男，寧夏銀川人，現(xiàn)就讀于天津理工大學(xué)電子信息工程學(xué)院。