屈德華 鄧鵬文 曾渝 魏虎

摘要：本設(shè)計(jì)是在自平衡小車(chē)直立的基礎(chǔ)上，設(shè)計(jì)一個(gè)可以控制其移動(dòng)的二輪平衡小車(chē)，實(shí)現(xiàn)自主直立平衡和APP控制其移動(dòng)。以STM32F103RCT6單片機(jī)作為主控芯片，使用集成了3軸陀螺儀和3軸加速度計(jì)的六軸運(yùn)動(dòng)處理傳感器MPU6050作為姿態(tài)傳感器，這里用卡爾曼濾波算法對(duì)采集的數(shù)據(jù)進(jìn)行融合處理，來(lái)減小數(shù)據(jù)的不準(zhǔn)確性和不穩(wěn)定性，通過(guò)TB6612FNG電機(jī)驅(qū)動(dòng)芯片來(lái)驅(qū)動(dòng)兩個(gè)大功率直流電機(jī)，在直立和移動(dòng)控制方面，采用傾角和角速度反饋，以串級(jí)PID控制來(lái)實(shí)現(xiàn)平衡小車(chē)的直立平衡和移動(dòng)，通過(guò)藍(lán)牙模塊，可實(shí)現(xiàn)手機(jī)APP對(duì)其進(jìn)行控制。

關(guān)鍵詞：STM32;藍(lán)牙;PID控制;自平衡小車(chē);卡爾曼濾波

最近幾年，自平衡小車(chē)在美國(guó)、日本、瑞士等國(guó)是研究熱點(diǎn)，設(shè)計(jì)出了多個(gè)實(shí)驗(yàn)機(jī)型，提出很多平衡控制方案，對(duì)實(shí)驗(yàn)機(jī)型的平衡性能與運(yùn)動(dòng)特性進(jìn)行了測(cè)試驗(yàn)證。通過(guò)在兩輪自平衡系統(tǒng)的基礎(chǔ)上進(jìn)行二次開(kāi)發(fā)，可以在很多環(huán)境中得到應(yīng)用，比如承載、代步、運(yùn)輸、環(huán)境監(jiān)測(cè)等，再加上微電子、集成電路的發(fā)展使得自平衡小車(chē)的生產(chǎn)成本降低了很多，所以說(shuō)自平衡小車(chē)系列的商業(yè)化產(chǎn)品有很好的發(fā)展前景。

1 自平衡小車(chē)原理

自平衡小車(chē)的直立、速度和轉(zhuǎn)向控制都是直接通過(guò)控制兩個(gè)直流電機(jī)完成，小車(chē)前傾是車(chē)輪要往前運(yùn)動(dòng)，小車(chē)往后傾需要往后運(yùn)動(dòng)，讓小車(chē)保持平衡。由于轉(zhuǎn)動(dòng)慣量是客觀存在的，為了讓小車(chē)靜止在平衡位置附近，不僅需要在電機(jī)上施加和傾角成正比的回復(fù)力，還要增加和角速度成正比的阻尼力，阻尼力和運(yùn)動(dòng)方向相反。因此需要測(cè)量平衡小車(chē)的角度偏差和角速度，控制小車(chē)的速度來(lái)完成直立控制。角速度通過(guò)六軸運(yùn)動(dòng)傳感器內(nèi)置陀螺儀測(cè)出，并且不受運(yùn)動(dòng)影響。內(nèi)置加速度計(jì)測(cè)出在X、Y、Z軸的一個(gè)方向上的加速度值通過(guò)反正切函數(shù)就可以計(jì)算出小車(chē)的傾角。由于無(wú)法徹底消除運(yùn)動(dòng)加速度對(duì)角度測(cè)量的影響，需要聯(lián)合陀螺儀測(cè)出的角速度通過(guò)積分獲得的傾角，再使用卡爾曼濾波算法，將兩個(gè)傾角值數(shù)據(jù)進(jìn)行融合產(chǎn)生一個(gè)更為準(zhǔn)確和穩(wěn)定的傾角數(shù)據(jù)。將得到的傾角數(shù)據(jù)和角速度來(lái)控制小車(chē)，并和測(cè)得的電機(jī)的速度信息結(jié)合實(shí)現(xiàn)消除角度偏差形成閉環(huán)控制。

2 系統(tǒng)方案設(shè)計(jì)

本次設(shè)計(jì)采用STM32F103RCT6作為主控芯片，通過(guò)獲取MPU6050傳感器獲取的Y軸的角度和加速度信息，并且經(jīng)過(guò)用卡爾曼濾波對(duì)角度和加速度信息進(jìn)行濾波來(lái)產(chǎn)生控制信號(hào)，控制方式采用模糊PID控制，通過(guò)將控制信號(hào)PWM波輸送到TB6612去驅(qū)動(dòng)電機(jī)，電機(jī)再將自身的運(yùn)動(dòng)數(shù)據(jù)傳回來(lái)給STM32，STM32再根據(jù)控制要求對(duì)自平衡小車(chē)的姿態(tài)不斷的進(jìn)行調(diào)整，最終形成一個(gè)閉環(huán)的控制。然后手機(jī)通過(guò)在APP中藍(lán)牙連接至自平衡小車(chē)上藍(lán)牙模塊進(jìn)行通信，傳輸運(yùn)動(dòng)信息和控制信息。

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

3.1主控芯片

本系統(tǒng)選用STM32F103RCT6作為主控芯片，該芯片內(nèi)核是ARM 32位的Cortex-M3，芯片集成多個(gè)定時(shí)器，可輸出多路PWM信號(hào)是的電機(jī)驅(qū)動(dòng)模塊獲得穩(wěn)定的PWM波形，另外還集成了SPI，I2C，USART等多種通信接口，方便與MPU6050、藍(lán)牙以及OLED屏進(jìn)行通信。

3.2電機(jī)驅(qū)動(dòng)芯片

TB6612FNG是東芝半導(dǎo)體公司生產(chǎn)的一款直流電機(jī)驅(qū)動(dòng)器件，它具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)2個(gè)電機(jī)。本設(shè)計(jì)通過(guò)主控芯片輸出兩路PWM波給TB6612FNG芯片去驅(qū)動(dòng)直流電機(jī)，調(diào)節(jié)PWM波的占空比來(lái)控制電機(jī)的速度，同時(shí)主控芯片通過(guò)兩個(gè)引腳去控制電機(jī)轉(zhuǎn)向。

3.3無(wú)線傳輸模塊

本次設(shè)計(jì)所選取的無(wú)線傳輸芯片是藍(lán)牙模塊 HC06，它是主從一體化的串口傳輸?shù)臒o(wú)線通訊模塊。通過(guò)手機(jī)藍(lán)牙APP連接至HC06藍(lán)牙模塊上，然后手機(jī)產(chǎn)生一系列的控制信號(hào)通過(guò)藍(lán)牙傳輸給平衡小車(chē)的主控芯片，主控芯片經(jīng)過(guò)識(shí)別解碼，再對(duì)平衡小車(chē)進(jìn)行前進(jìn)、后退、轉(zhuǎn)彎控制。

3.4 姿態(tài)檢測(cè)傳感器

姿態(tài)檢測(cè)是整個(gè)控制系統(tǒng)中比較重要的環(huán)節(jié)，它的數(shù)據(jù)的即時(shí)性和準(zhǔn)確性直接影響著整個(gè)系統(tǒng)的穩(wěn)定情況。在本次設(shè)計(jì)中姿態(tài)檢測(cè)芯片選擇的是一款集成三軸加速度和三軸角速度的六軸傳感器 MPU6050，并且MPU6050內(nèi)部集成一個(gè)內(nèi)部 DMP。

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

4.1系統(tǒng)主程序設(shè)計(jì)

首先在主程序中進(jìn)行初始化聲明、系統(tǒng)時(shí)鐘定義。然后采集小車(chē)的速度信息和角度信息再進(jìn)行濾波融合處理，通過(guò)PID控制輸出相應(yīng)的PWM信號(hào)控制電機(jī)轉(zhuǎn)動(dòng)，達(dá)到平衡狀態(tài)。姿態(tài)角檢測(cè)模塊MPU6050與主控芯片是IIC通訊，獲得來(lái)自小車(chē)的三軸陀螺儀信號(hào)和三軸加速度計(jì)信號(hào)。磁力計(jì)與MPU6050通過(guò)IIC連接，與陀螺儀和加速度計(jì)共同作用。小車(chē)車(chē)速的檢測(cè)是通過(guò)編碼器采集到的信息轉(zhuǎn)化成對(duì)應(yīng)的速度值大小實(shí)現(xiàn)的。單片機(jī)通過(guò)定時(shí)器捕獲脈沖信號(hào)和輸出電機(jī)驅(qū)動(dòng)PWM波。

系統(tǒng)初始化完成后，系統(tǒng)等待MPU6050初始化完成，直到MPU6050初始化完成時(shí)，OLED被點(diǎn)亮顯示卡爾曼濾波模式、左右電機(jī)速度、MPU6050內(nèi)部溫度、電池電壓以及自平衡小車(chē)自身傾角。OLED屏幕上顯示的數(shù)據(jù)是實(shí)時(shí)更新的，等待按下啟動(dòng)按鈕，當(dāng)按下啟動(dòng)按鈕時(shí)，自平衡小車(chē)自動(dòng)進(jìn)入直立平衡模式，保持自身的直立狀態(tài)，若有手機(jī)通過(guò)藍(lán)牙連接至自平衡小車(chē)，則進(jìn)入控制模式，控制模式是在直立模式的基礎(chǔ)上完成的，并且可以完成轉(zhuǎn)彎，前進(jìn)和后退動(dòng)作。

主程序的主循環(huán)里面主要是對(duì)顯示的控制，選擇是否將數(shù)據(jù)更新到連接到小車(chē)的手機(jī)APP上面。然后程序中所有任務(wù)的調(diào)度都是在外部中斷服務(wù)程序中執(zhí)行。

4.2 濾波和控制算法

平衡小車(chē)獲取姿態(tài)角的濾波算法一般為卡爾曼濾波和一階互補(bǔ)濾波。但是 MPU6050 內(nèi)置的 DMP 可以直接輸出和姿態(tài)相關(guān)的四元數(shù)。所以，常用的有三種方法可以獲取角度。根據(jù)長(zhǎng)時(shí)間的實(shí)踐，以DMP 輸出的四元數(shù)表示的角度和卡爾曼濾波最為穩(wěn)定。互補(bǔ)濾波的效果稍差，但也是很不錯(cuò)的。最終采用的是卡爾曼濾波。

直立控制：PD控制，這是最核心的控制，其他的控制都是相對(duì)直立控制而言都是干擾。速度控制：PI控制對(duì)編碼器信息進(jìn)行低通濾波可以削弱電機(jī)控制的比重，提高系統(tǒng)穩(wěn)定性。轉(zhuǎn)向控制：PD控制結(jié)合了Z軸陀螺儀PD控制。

5 設(shè)計(jì)總結(jié)

系統(tǒng)設(shè)計(jì)過(guò)程中要考慮到主芯片STM32的資源分配，根據(jù)傳感器的控制端口類型以及所支持的接口協(xié)議，分別來(lái)設(shè)計(jì)各個(gè)傳感器與主芯片STM32的端口對(duì)接。硬件調(diào)試的思路，首先是分模塊調(diào)試，再完成系統(tǒng)實(shí)現(xiàn)。難點(diǎn)有：MPU6050的數(shù)據(jù)的采集和處理，PID控制方式的理解和實(shí)現(xiàn)以及PID參數(shù)的調(diào)試。通過(guò)調(diào)試前對(duì)PID控制原理和各參數(shù)的物理含義有基本的理解，然后在進(jìn)行PID代碼原理實(shí)現(xiàn)以及PID參數(shù)調(diào)試，在調(diào)試的過(guò)程中逐步理解PID。

參考文獻(xiàn)：

[1]李紅美，李智，高飛.平衡的杰作——賽格威FIT兩輪平臺(tái)電動(dòng)車(chē)[J].電器工業(yè).2002， （6）：19-21.

[2]屠運(yùn)武，徐俊艷，張培仁.自平衡控制系統(tǒng)的建模與仿真[J].系統(tǒng)仿真學(xué)報(bào).2004（04）.

[3]王曉宇，閆繼宏，臧希喆等.兩輪白平衡機(jī)器人多傳感器數(shù)據(jù)融合方法研究[J].傳感器技術(shù)學(xué)報(bào).2007.

[4]趙杰，王曉宇，秦勇等.基于UKF的兩輪自平衡機(jī)器人姿態(tài)最優(yōu)估計(jì)研究[J].機(jī)器人.2006： （11）

[5]付夢(mèng)印，鄧志紅，張繼偉.Kalman 濾波理論及其在慣性導(dǎo)航系統(tǒng)中的應(yīng)用[M].第二版.北京：科學(xué)出版社，2010.

作者簡(jiǎn)介：

屈德華（1995-），男，漢，四川瀘州，本科，研究方向：信息工程。

鄧鵬文（1998-），女，漢，四川達(dá)州，本科，研究方向：信息工程。

曾渝（1998-），男，漢，四川德陽(yáng)，本科，研究方向：電氣工程。

魏虎（1995-），男，漢，四川綿陽(yáng)，本科，研究方向：通信工程。