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

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

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

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

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

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

1 設(shè)計(jì)原理

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

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

1.2 原理示意圖

圖1 原理示意圖

1.3 設(shè)計(jì)原理

雙輪平衡車的控制思想是基于一級倒立擺的模型，當(dāng)車體發(fā)生傾斜時，傳感器會檢測到車體角度的變化，控制系統(tǒng)根據(jù)車體角度的變化，控制電機(jī)向傾斜方向轉(zhuǎn)動，從而產(chǎn)生抵抗傾斜的力矩，使車身維持在平衡狀態(tài)。當(dāng)車體不發(fā)生傾斜時，平衡車會在小范圍內(nèi)實(shí)現(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é)小車的傾角來實(shí)現(xiàn)小車速度控制，實(shí)際上最后還是演變成通過控制電機(jī)的轉(zhuǎn)速來實(shí)現(xiàn)車輪速度的控制。

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

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

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

2 設(shè)計(jì)流程

圖2 設(shè)計(jì)流程圖

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

為實(shí)現(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)估計(jì)的算法。在本系統(tǒng)中，測量角度和測量角速度存在著導(dǎo)數(shù)關(guān)系，在這種情況下，我們不直接估計(jì)車體的角速度，而是采用加速度計(jì)的測量值來估算偏差gyro_bias , gyro_bias可以作為狀態(tài)向量的另一個元素。我們把車體的角度和角速度用入下關(guān)系時表示:

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

anglek-1上一時刻加速計(jì)的測量值；

dt為采樣時間。

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

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

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

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

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

圖3 仿真結(jié)果圖

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

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

雙輪玩具小車是非線性、多階次、不穩(wěn)定的系統(tǒng)，要實(shí)現(xiàn)其動態(tài)平衡，控制小車完成相應(yīng)的動作，即給出相應(yīng)的姿態(tài)信息，將控制信號重新寫入存儲器，由于其平衡狀態(tài)是處于動態(tài)，所以需要不斷的進(jìn)行測量更新，針對兩輪自平衡車傾角的測量問題,采用了卡爾曼多傳感融合算法。該算法融合了加速度傳感器和陀螺儀傳感器數(shù)據(jù),實(shí)現(xiàn)了兩輪自平衡車傾角實(shí)時在線估算，控制器給出控制信息后又控制系統(tǒng)驅(qū)動電機(jī)工作，采用了比例微分(PID)控制算法,實(shí)現(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é)。實(shí)現(xiàn)被控量的值與要求的值達(dá)到一致。但為了適應(yīng)不同的過程，需要給出合理的控制規(guī)律，否則，PID控制器達(dá)不到預(yù)定效果?？刂扑惴ǜ鶕?jù)小車的姿態(tài)信息計(jì)算出PWM信號，再通過閉環(huán)反饋控制使小車完成相應(yīng)的動作。根據(jù)角度角速度變化，控制電機(jī)來完成當(dāng)前的平衡，以及在車體在運(yùn)動過程中的各個姿態(tài)。

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

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

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

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

2.4.3 PWM輸出

要產(chǎn)生頻率和脈沖寬度可調(diào)的PWM波形，這可通過FPGA豐富的硬件資源和可以配置I/O引腳來實(shí)現(xiàn)。嵌入式系統(tǒng)中FPGA的應(yīng)用設(shè)計(jì)關(guān)鍵是系統(tǒng)軟硬件功能的劃分。通過計(jì)數(shù)器的方式實(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)行了大量的實(shí)驗(yàn)調(diào)控，得到比較符合要求的實(shí)驗(yàn)數(shù)據(jù)，再通過大數(shù)據(jù)的分析和計(jì)算確定反饋比例系數(shù)。由于本系統(tǒng)采用CME——M7，其具有ARM內(nèi)核，可以做SOC，能將算法級的復(fù)雜程序直接放在ARM內(nèi)核中，降低程序的開發(fā)難度。

3 最終成果展示

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

圖5 平衡小車實(shí)物展示

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

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

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

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

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