錢炳鋒+何奉泊+陳嘉裕

【摘 要】雙輪平衡車是一個(gè)高度不穩(wěn)定兩輪機(jī)器人，單獨(dú)使用陀螺儀或者加速度計(jì)都不能提供有效而可靠的信息來保證車體的平衡。介紹了以 STM32單片機(jī)作為核心控制器的雙輪平衡車控制系統(tǒng)設(shè)計(jì)方案，采用 MPU6050 六軸傳感器采集角度和角速度信號(hào)，應(yīng)用卡爾曼融合濾波方式和PID 控制算法，以實(shí)現(xiàn)雙輪平衡車的平衡和簡(jiǎn)單的直立行走。

【關(guān)鍵詞】STM32；陀螺儀；加速度計(jì)；濾波；MPU6050

引言

兩輪自平衡小車，其本質(zhì)上是一個(gè)底端可以自由移動(dòng)的倒立擺系統(tǒng)，屬于同軸平行布置的結(jié)構(gòu)，該系統(tǒng)是非線性、強(qiáng)耦合、多變量和自然不穩(wěn)定的系統(tǒng)，是檢驗(yàn)各種控制理論的理想模型。在控制過程中平衡小車能有效地反映諸如穩(wěn)定性、魯棒性、隨動(dòng)性以及跟蹤性等許多控制中的關(guān)鍵問題。

本文通過對(duì)平衡車系統(tǒng)建立合理的數(shù)學(xué)模型，基于ARM內(nèi)核的32位微控制系統(tǒng)，通過融合陀螺儀、加速度傳感器等信息，實(shí)現(xiàn)平衡車反饋系統(tǒng)的控制。

1. 系統(tǒng)構(gòu)成

1.1機(jī)械結(jié)構(gòu)

平衡車機(jī)械外型結(jié)構(gòu)一般由一個(gè)兩輪同軸，左右平行布置結(jié)構(gòu)，兩輪分別由一個(gè)直流電機(jī)驅(qū)動(dòng)，直流電機(jī)末端都帶有反饋用的編碼器，車身中心位于車輪軸上方，通過控制兩輪的轉(zhuǎn)速差實(shí)現(xiàn)直行和定半徑轉(zhuǎn)彎。

優(yōu)秀的機(jī)械結(jié)構(gòu)需要具備以下特點(diǎn)：質(zhì)量集中、重心低、質(zhì)量輕。質(zhì)量集中為了減少轉(zhuǎn)動(dòng)慣量，這里的控制電池是關(guān)鍵，即電池盡量離車軸近；重心低為了防止過彎側(cè)滑，抬輪等，能夠使過彎更平滑順暢；質(zhì)量輕是便于控制，減少整車慣性。不到萬不得已不要加配重。當(dāng)然，可以看到不少跑得好的車子都加了配重的，這樣是為了控制角度或速度，而不是想要車子變得更“穩(wěn)重”。抓住這九個(gè)字的，如何將傳感器、電池、主板組裝將會(huì)是一個(gè)決定小車速度的要點(diǎn)。

1.2系統(tǒng)工作原理

平衡車基本工作原理如圖1所示。兩個(gè)直流電動(dòng)機(jī)安裝在車體平臺(tái)下面，通過直流電機(jī)分別驅(qū)動(dòng)左右車輪運(yùn)動(dòng)。采用MPU6050檢測(cè)車體平臺(tái)運(yùn)行姿態(tài)。

MPU6050 六軸傳感器芯片為全球首例整合性6軸運(yùn)動(dòng)處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時(shí)間軸之差的問題，減少了大量的封裝空間。其集成了3 軸MEMS陀螺儀和3軸MEMS加速度計(jì)，每個(gè)軸對(duì)應(yīng)有一個(gè)16位AD轉(zhuǎn)換器，測(cè)量范圍達(dá)±16 g，其高分辨率（ 3.9 mg /LSB）能夠測(cè)量不到1.0°的傾斜角度變化。其中陀螺儀檢測(cè)平臺(tái)繞轉(zhuǎn)動(dòng)輪軸轉(zhuǎn)動(dòng)的角速率，加速度計(jì)得到的數(shù)據(jù)與陀螺儀檢測(cè)值，經(jīng)過卡爾曼濾波器，得到補(bǔ)償后的角度值，解決陀螺儀零票問題。采用可編程邏輯器件對(duì)電動(dòng)機(jī)編碼器產(chǎn)生的脈沖量進(jìn)行計(jì)數(shù)，從而檢測(cè)出車體平臺(tái)的位移和速度。跟據(jù)姿態(tài)監(jiān)測(cè)和位置傳感，通過一定的控制算法計(jì)算出控制電壓信號(hào)，再通過驅(qū)動(dòng)電機(jī)，調(diào)整車體平臺(tái)運(yùn)行姿態(tài)，從而使車體平臺(tái)始終保持平衡狀態(tài)。

2.信號(hào)處理

2.1 姿態(tài)信號(hào)

在雙輪平衡車姿態(tài)檢測(cè)系統(tǒng)中，加速度計(jì)、陀螺儀主要用于檢測(cè)車體傾斜角和傾斜角的變化速度，其中加速度計(jì)屬于測(cè)量線性運(yùn)動(dòng)的傳感器，其靜態(tài)響應(yīng)好，能夠準(zhǔn)確測(cè)量車體靜態(tài)時(shí)的角度，但對(duì)震動(dòng)較敏感，受動(dòng)態(tài)加速度的影響較大。在角度測(cè)量時(shí)，除了小車角度變化的信號(hào)外，還伴隨著因車體運(yùn)動(dòng)而產(chǎn)生的噪聲，這個(gè)噪聲會(huì)隨著車體運(yùn)動(dòng)速度的增加而增大。陀螺儀屬于測(cè)量旋轉(zhuǎn)運(yùn)動(dòng)的傳感器，其輸出值圍繞某個(gè)軸向的旋轉(zhuǎn)角速率，通過角速率對(duì)時(shí)間積分即可得到角度值。 系統(tǒng)采用微控制器循環(huán)采樣獲取陀螺儀的角速率信息，即每隔一段很短的時(shí)間采樣一次，采用累加的方法實(shí)現(xiàn)積分的功能來計(jì)算角度值，所以存在累積漂移誤差，不適合長(zhǎng)時(shí)間單獨(dú)工作。

2.2 濾波融合

為獲得準(zhǔn)確的車體傾角值，需對(duì)加速度計(jì)和陀螺儀的輸出值進(jìn)行融合，系統(tǒng)采用一種簡(jiǎn)易的卡爾曼濾波方法進(jìn)行數(shù)據(jù)融合?？柭鼮V波器是一種遞歸的估算，例如要估算K時(shí)刻車體的實(shí)際角度值，首先要根據(jù)K-1時(shí)刻的角度值預(yù)測(cè)得到K時(shí)刻的角度值，再根據(jù)K時(shí)刻的預(yù)測(cè)角度值和高斯噪聲的方差，進(jìn)行遞歸運(yùn)算，直到獲得最優(yōu)的車體角度值當(dāng)雙輪平衡車車體產(chǎn)生傾斜時(shí)，系統(tǒng)采用PID控制算法，通過整合車體角度、角速度、車體速度和位置等參數(shù)值，輸出PWM信號(hào)驅(qū)動(dòng)電機(jī)，產(chǎn)生相應(yīng)的力矩，從而保持車體的動(dòng)態(tài)平衡，其結(jié)構(gòu)框圖如圖2 所示，圖中的 Kd，Kp，Ksp，Kis 是PID控制器的相關(guān)參數(shù)。

3.軟件設(shè)計(jì)

軟件的主要功能包括有：

（1）車模運(yùn)行狀態(tài)檢測(cè)；

（2）電機(jī) PWM 輸出；

（3）車模運(yùn)行控制：直立控制、速度控制、方向控制；

（4）車模運(yùn)行流程控制：程序初始化、車模啟動(dòng)與結(jié)束；

（5）系統(tǒng)界面：狀態(tài)顯示、上位機(jī)監(jiān)控、參數(shù)設(shè)定等。

主程序框架如圖3所示。

4.結(jié)語

雙輪平衡車控制系統(tǒng)以STM32單片機(jī)為控制芯片，采用卡爾曼融合濾波方式對(duì)MPU6050傳感器的角度和角速度信號(hào)進(jìn)行融合處理，從而得到實(shí)時(shí)、穩(wěn)定的角度和角速度信號(hào)，應(yīng)用實(shí)驗(yàn)測(cè)試法確定 PID算法中的各個(gè)參數(shù)，從而實(shí)現(xiàn)對(duì)雙輪平衡車的控制。經(jīng)多次測(cè)試，能初步實(shí)現(xiàn)車體的平衡和簡(jiǎn)單的直立行走。

參考文獻(xiàn)：

[1]賴義漢，王 凱 基于 MPU6050 的雙輪平衡車控制系統(tǒng)設(shè)計(jì) 河南工程學(xué)院學(xué)報(bào)（ 自然科學(xué)版） 第 26 卷 第 1 期 2014 年 3 月.

[2]王素青， 熊維堂 基于 STM32 的兩輪自平衡小車系統(tǒng)設(shè)計(jì) 實(shí) 驗(yàn) 室 研 究 與 探 索第 35 卷 第 5 期 2016 年 5 月.

基金資助：

本項(xiàng)目受上海電機(jī)學(xué)院車輛工程學(xué)科建設(shè)項(xiàng)目資助，項(xiàng)目編號(hào)：16TSXK01；上海市大學(xué)生創(chuàng)新計(jì)劃資助。endprint