基于MEMS傳感器的兩輪平衡小車(chē)設(shè)計(jì)

李詳鵬 陳春 周子文 施娜

摘 ?要：介紹一種基于MEMS加速度傳感器的兩輪平衡小車(chē)設(shè)計(jì)。以STM32F103RBT6 ARM微處理器為主控制器，以一片TB661

2FNG驅(qū)動(dòng)小車(chē)兩輪，采用MPU-6050 MEMS加速度傳感器做為小車(chē)運(yùn)動(dòng)姿態(tài)信息感知的兩輪自平衡系統(tǒng)，通過(guò)直立角度環(huán)和速度環(huán)串級(jí)PID實(shí)現(xiàn)了小車(chē)的動(dòng)平衡和靜平衡控制。

關(guān)鍵詞：平衡小車(chē);MEMS傳感器;串級(jí)控制

中圖分類(lèi)號(hào)：TP273 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：2095-2945（2019）22-0027-03

Abstract： This paper introduces the design of a two-wheel balanced trolley based on MEMS accelerometer. The STM32F103RBT6 ARM microprocessor is used as the main controller， a piece of TB6612FNG is used to drive the two wheels of the car， and the MPU-6050 MEMS accelerometer is used as the two-wheel self-balanced system for the perception of the motion and attitude information of the car. The dynamic balance and static balance control of the car are realized by vertical angle loop and velocity loop cascade PID.

Keywords： balance trolley; MEMS sensor; cascade control

引言

兩輪自平衡小車(chē)是一個(gè)集動(dòng)態(tài)決策和規(guī)劃、環(huán)境感知、行為控制和執(zhí)行等多種功能于一體的綜合復(fù)雜系統(tǒng)，其關(guān)鍵是在解決自平衡的同時(shí)，還能夠適應(yīng)在各種環(huán)境下的控制任務(wù)。通過(guò)運(yùn)用外加速度傳感器、超聲波傳感器、傾角傳感器、防碰撞開(kāi)關(guān)等，可以實(shí)現(xiàn)小車(chē)的跟蹤、路徑規(guī)劃和自主避障等多種復(fù)雜的功能。還可以把GPS和慣性導(dǎo)航設(shè)備配備到小車(chē)上實(shí)現(xiàn)組合導(dǎo)航。所以，兩輪自平衡小車(chē)其實(shí)是一個(gè)實(shí)現(xiàn)起來(lái)相對(duì)簡(jiǎn)單又復(fù)雜系統(tǒng)，受到世界各個(gè)國(guó)家的重視，具有較高的學(xué)術(shù)研究意義。

1 硬件系統(tǒng)設(shè)計(jì)

1.1 硬件系統(tǒng)總體框架

自平衡兩輪小車(chē)控制系統(tǒng)的總體硬件框圖如圖1所示：

整個(gè)硬件控制系統(tǒng)以ST意法半導(dǎo)體的ARM微處理器STM32F103RBT6為核心主控芯片，通過(guò)IIC接口實(shí)時(shí)獲取MPU-6050 MEMS運(yùn)動(dòng)傳感器的姿態(tài)數(shù)據(jù)，通過(guò)卡爾曼濾波算法以及姿態(tài)解耦算法獲得小車(chē)在X、Y、Z三個(gè)方向的姿態(tài)以及加速度信息，兩路轉(zhuǎn)速編碼信號(hào)實(shí)時(shí)獲取左右兩個(gè)減速電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向反饋信息，主控芯片通過(guò)位置環(huán)和速度環(huán)的PID輸出PWM控制信號(hào)，驅(qū)動(dòng)電機(jī)驅(qū)動(dòng)模塊芯片TB6612FNG接收到PWM信號(hào)之后再完成對(duì)電機(jī)的驅(qū)動(dòng)控制，最終實(shí)現(xiàn)平衡小車(chē)的動(dòng)平衡和靜平衡以及動(dòng)態(tài)運(yùn)動(dòng)控制。其中，藍(lán)牙模塊主要實(shí)現(xiàn)小車(chē)運(yùn)動(dòng)狀態(tài)的遠(yuǎn)程設(shè)定與控制。

1.2 姿態(tài)檢測(cè)傳感器及其電路設(shè)計(jì)

平衡小車(chē)姿態(tài)檢測(cè)主要是完成角度的測(cè)量和角速度的測(cè)量，這就需要用到加速度計(jì)和陀螺儀，在實(shí)際使用中，同時(shí)使用陀螺儀和加速度計(jì)會(huì)產(chǎn)生軸間差的問(wèn)題，為了避免這個(gè)問(wèn)題，這里選擇了Invensense公司的MPU6050模塊，它首創(chuàng)的整合性6軸運(yùn)動(dòng)處理組件，是全球首例，其中包含三軸角速度，三軸加速度，廠(chǎng)家提供了一個(gè)信號(hào)處理驅(qū)動(dòng)庫(kù)（DMP驅(qū)動(dòng)），用戶(hù)借此能夠方便高效地解決數(shù)據(jù)融合的難題。MPU6050傳感器外圍電路如圖2所示。MPU6050 擁有一個(gè)中斷系統(tǒng)，并可根據(jù)用戶(hù)需求進(jìn)行編程設(shè)定中斷輸出間隔，INT管腳可以產(chǎn)生中斷信號(hào)。

1.3 TB6612FNG電機(jī)驅(qū)動(dòng)電路設(shè)計(jì)

為保持平衡小車(chē)處于動(dòng)態(tài)平衡的狀態(tài)，就要在控制系統(tǒng)中做到根據(jù)小車(chē)姿態(tài)不斷地變換電機(jī)的正反轉(zhuǎn)，這就要求電機(jī)驅(qū)動(dòng)電路擁有大電流輸出使電機(jī)快速切換運(yùn)行狀態(tài)的能力，從而防止在工作過(guò)程中出現(xiàn)堵轉(zhuǎn)的現(xiàn)象。東芝半導(dǎo)體公司TB6612FNG具有兩路電機(jī)驅(qū)動(dòng)輸出通道，每通道能輸出最高1.2A的連續(xù)驅(qū)動(dòng)電流，啟動(dòng)峰值電流達(dá)2A/3.2A（連續(xù)脈沖/單脈沖）;4種電機(jī)控制模式：正轉(zhuǎn)/反轉(zhuǎn)/制動(dòng)/停止;PWM支持頻率高達(dá)100kHz;驅(qū)動(dòng)不需要外加散熱片，而且外圍電路比較簡(jiǎn)單，只需要外接電源濾波電容就能直接驅(qū)動(dòng)電機(jī)，大大簡(jiǎn)化了硬件設(shè)計(jì)難度。圖3所示是電機(jī)驅(qū)動(dòng)電路。

VM1，VM2，VM3直接用12V電池供電，模塊的引腳3、4、9、10、18接地，當(dāng) STBY接高電平時(shí)模塊正常工作。A路電機(jī)的正極和負(fù)極分別接到AO1和AO2。然后通過(guò) PWMA、AIN2、AIN1控制電機(jī)。如表1所示TB6612FNG控制的真值表。

2 軟件設(shè)計(jì)

系統(tǒng)軟件設(shè)計(jì)：

（1）主程序軟件設(shè)計(jì)

主程序主要實(shí)現(xiàn)的工作是各個(gè)變量的定義，各模塊的初始化，中斷的初始化等部分，程序的流程圖如圖4所示。從主函數(shù)開(kāi)始，首先完成對(duì)系統(tǒng)各模塊的初始化設(shè)定，開(kāi)啟MPU6050傳感器5ms定時(shí)輸出中斷，再進(jìn)入主循環(huán)的函數(shù)。

（2）控制軟件設(shè)計(jì)

如圖5所示是控制函數(shù)的主要工作流程。為主函數(shù)提供50ms的精準(zhǔn)延時(shí)，5ms讀取一次陀螺儀和加速度計(jì)的值，這樣的采樣頻率可以改善卡爾曼濾波和互補(bǔ)濾波的效果。

平衡小車(chē)調(diào)試直立控制使用的是PD（比例微分）控制器，如圖6所示是直立調(diào)試的流程。平衡小車(chē)調(diào)試速度控制使用的是PI（比例積分）控制器，如圖7所示是實(shí)現(xiàn)速度調(diào)試流程。

平衡小車(chē)獲取角度使用的算法有三種：四元數(shù)、互補(bǔ)濾波和卡爾曼濾波。平衡小車(chē)的卡爾曼濾波器設(shè)計(jì)關(guān)鍵要素是選取狀態(tài)向量，平衡小車(chē)的車(chē)體傾斜角度和車(chē)體的傾斜角速度是倒數(shù)關(guān)系，所以狀態(tài)向量的元素可以選擇為車(chē)體的角速度（Q_gyro）、車(chē)體的傾角（Q_angle）和選取加速度計(jì)來(lái)估計(jì)陀螺儀的零位偏（Q_bias），卡爾曼濾波采樣時(shí)間為dt=0.005，觀(guān)查噪聲協(xié)方差矩陣（R_angle）。

平衡小車(chē)的調(diào)試主要分為三個(gè)環(huán)節(jié)：直立控制調(diào)試、速度控制調(diào)轉(zhuǎn)向控制調(diào)試。

a.平衡小車(chē)直立控制的調(diào)試使用的是PD（比例微分）控制器。根據(jù)直立控制的調(diào)試過(guò)程，得到如表2，表3所示的結(jié)論。

可以看出P、D參數(shù)的最大值是kp=500，kd=1.7。根據(jù)實(shí)踐經(jīng)驗(yàn)，對(duì)每個(gè)參數(shù)乘以0.6，取整數(shù)部分得到我們最終需要的理想?yún)?shù) kp=300，kd=1。

b.平衡小車(chē)速度控制的調(diào)試中使用的是PI（比例積分）控制器。根據(jù)速度控制的調(diào)試過(guò)程，得到如表4所示的結(jié)論。

從表4中我們能看到速度控制P、I參數(shù)的理想值是kp=80，ki=0.4。

c.平衡小車(chē)轉(zhuǎn)向控制調(diào)試中使用的是P（比例）控制器，根據(jù)轉(zhuǎn)向控制的調(diào)試過(guò)程，得到如表5所示的結(jié)論。

根據(jù)表5中的數(shù)據(jù)，我們可以確定轉(zhuǎn)向控制 P 參數(shù)的理想值是kp=1。綜上所述，PID參數(shù)的調(diào)試就到達(dá)了設(shè)計(jì)要求，平衡小車(chē)能穩(wěn)定的做自平衡運(yùn)動(dòng)。

3 結(jié)束語(yǔ)

平衡小車(chē)與倒立擺系統(tǒng)很相似，是一種多變量、非線(xiàn)性、強(qiáng)耦合、參數(shù)不確定的復(fù)雜系統(tǒng)，它體積小、簡(jiǎn)單的結(jié)構(gòu)、靈活的運(yùn)動(dòng)性能，非常適合在狹小的空間內(nèi)工作，因此成為檢驗(yàn)各種控制方法處理能力的理想裝置。通過(guò)對(duì)小車(chē)軟硬件系統(tǒng)分析，研究平衡小車(chē)在自平衡基礎(chǔ)上的其他性能，例如在特殊環(huán)境下的導(dǎo)航、拐角的轉(zhuǎn)彎等問(wèn)題，對(duì)解決在工業(yè)生產(chǎn)和社會(huì)生活中的許多問(wèn)題有很大的幫助。

參考文獻(xiàn)：

[1]王曉宇.兩輪自平衡機(jī)器人的研究[D].哈爾濱工業(yè)大學(xué)，2007.

[2]聞雙云.兩輪自平衡小車(chē)控制算法的研究與優(yōu)化[D].吉林大學(xué)，2017.

[3]王泉.兩輪自平衡機(jī)器人自主移動(dòng)系統(tǒng)設(shè)計(jì)和應(yīng)用[D].武漢科技大學(xué)，2018.

[4]范淇元，覃羨烘，李洪毅.基于STM32的雙輪平衡機(jī)器人的控制系統(tǒng)設(shè)計(jì)[J].自動(dòng)化與儀表，2018（03）：20-21+41.

[5]潘二偉.基于STM32的兩輪自平衡車(chē)設(shè)計(jì)與實(shí)現(xiàn)[D].黑龍江大學(xué)，2018.