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

基于FPGA平衡小車的設計

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

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

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

1 設計原理

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

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

1.2 原理示意圖

圖1 原理示意圖

1.3 設計原理

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

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

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

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

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

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

2 設計流程

圖2 設計流程圖

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

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

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

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

2.2 卡爾曼濾波器

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

卡爾曼濾波（Kalman filtering）一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。在本系統(tǒng)中，測量角度和測量角速度存在著導數(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)〔煌闹荡砹瞬灰粯拥男湃纬潭?。如果認為陀螺儀的可信度更高，可以把Q-gyro設置的小一些，如果認為加速度計的可信度更高可以把Q_ angle設置的小一些。R矩陣代表觀測噪聲的協(xié)方差，加入R很大，說明系統(tǒng)具有很大的噪聲。

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

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

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

圖3 仿真結(jié)果圖

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

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

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

2.4 電機驅(qū)動控制

2.4.1 雙輪平衡車控制特點

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

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

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

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

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

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

2.4.3 PWM輸出

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

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

由上文中PID控制的原理可知，比例環(huán)節(jié)輸出的控制信號與系統(tǒng)誤差e(t)成比例，比例系數(shù)越大，系統(tǒ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)變化做出預警，產(chǎn)生提前的調(diào)節(jié)。微分控制可以使系統(tǒng)相位超前，抵消積分作用造成的相位滯后，同時可以減小超調(diào)，但微分系數(shù)過大，可能造成系統(tǒng)不穩(wěn)定[4]。

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

3 最終成果展示

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

圖5 平衡小車實物展示

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

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

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

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

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