屈德華 鄧鵬文 曾渝 魏虎
摘要:本設(shè)計(jì)是在自平衡小車直立的基礎(chǔ)上,設(shè)計(jì)一個(gè)可以控制其移動(dòng)的二輪平衡小車,實(shí)現(xiàn)自主直立平衡和APP控制其移動(dòng)。以STM32F103RCT6單片機(jī)作為主控芯片,使用集成了3軸陀螺儀和3軸加速度計(jì)的六軸運(yùn)動(dòng)處理傳感器MPU6050作為姿態(tài)傳感器,這里用卡爾曼濾波算法對(duì)采集的數(shù)據(jù)進(jìn)行融合處理,來減小數(shù)據(jù)的不準(zhǔn)確性和不穩(wěn)定性,通過TB6612FNG電機(jī)驅(qū)動(dòng)芯片來驅(qū)動(dòng)兩個(gè)大功率直流電機(jī),在直立和移動(dòng)控制方面,采用傾角和角速度反饋,以串級(jí)PID控制來實(shí)現(xiàn)平衡小車的直立平衡和移動(dòng),通過藍(lán)牙模塊,可實(shí)現(xiàn)手機(jī)APP對(duì)其進(jìn)行控制。
關(guān)鍵詞:STM32;藍(lán)牙;PID控制;自平衡小車;卡爾曼濾波
最近幾年,自平衡小車在美國、日本、瑞士等國是研究熱點(diǎn),設(shè)計(jì)出了多個(gè)實(shí)驗(yàn)機(jī)型,提出很多平衡控制方案,對(duì)實(shí)驗(yàn)機(jī)型的平衡性能與運(yùn)動(dòng)特性進(jìn)行了測(cè)試驗(yàn)證。通過在兩輪自平衡系統(tǒng)的基礎(chǔ)上進(jìn)行二次開發(fā),可以在很多環(huán)境中得到應(yīng)用,比如承載、代步、運(yùn)輸、環(huán)境監(jiān)測(cè)等,再加上微電子、集成電路的發(fā)展使得自平衡小車的生產(chǎn)成本降低了很多,所以說自平衡小車系列的商業(yè)化產(chǎn)品有很好的發(fā)展前景。
1 自平衡小車原理
自平衡小車的直立、速度和轉(zhuǎn)向控制都是直接通過控制兩個(gè)直流電機(jī)完成,小車前傾是車輪要往前運(yùn)動(dòng),小車往后傾需要往后運(yùn)動(dòng),讓小車保持平衡。由于轉(zhuǎn)動(dòng)慣量是客觀存在的,為了讓小車靜止在平衡位置附近,不僅需要在電機(jī)上施加和傾角成正比的回復(fù)力,還要增加和角速度成正比的阻尼力,阻尼力和運(yùn)動(dòng)方向相反。因此需要測(cè)量平衡小車的角度偏差和角速度,控制小車的速度來完成直立控制。角速度通過六軸運(yùn)動(dòng)傳感器內(nèi)置陀螺儀測(cè)出,并且不受運(yùn)動(dòng)影響。內(nèi)置加速度計(jì)測(cè)出在X、Y、Z軸的一個(gè)方向上的加速度值通過反正切函數(shù)就可以計(jì)算出小車的傾角。由于無法徹底消除運(yùn)動(dòng)加速度對(duì)角度測(cè)量的影響,需要聯(lián)合陀螺儀測(cè)出的角速度通過積分獲得的傾角,再使用卡爾曼濾波算法,將兩個(gè)傾角值數(shù)據(jù)進(jìn)行融合產(chǎn)生一個(gè)更為準(zhǔn)確和穩(wěn)定的傾角數(shù)據(jù)。將得到的傾角數(shù)據(jù)和角速度來控制小車,并和測(cè)得的電機(jī)的速度信息結(jié)合實(shí)現(xiàn)消除角度偏差形成閉環(huán)控制。
2 系統(tǒng)方案設(shè)計(jì)
本次設(shè)計(jì)采用STM32F103RCT6作為主控芯片,通過獲取MPU6050傳感器獲取的Y軸的角度和加速度信息,并且經(jīng)過用卡爾曼濾波對(duì)角度和加速度信息進(jìn)行濾波來產(chǎn)生控制信號(hào),控制方式采用模糊PID控制,通過將控制信號(hào)PWM波輸送到TB6612去驅(qū)動(dòng)電機(jī),電機(jī)再將自身的運(yùn)動(dòng)數(shù)據(jù)傳回來給STM32,STM32再根據(jù)控制要求對(duì)自平衡小車的姿態(tài)不斷的進(jìn)行調(diào)整,最終形成一個(gè)閉環(huán)的控制。然后手機(jī)通過在APP中藍(lán)牙連接至自平衡小車上藍(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ì)通過主控芯片輸出兩路PWM波給TB6612FNG芯片去驅(qū)動(dòng)直流電機(jī),調(diào)節(jié)PWM波的占空比來控制電機(jī)的速度,同時(shí)主控芯片通過兩個(gè)引腳去控制電機(jī)轉(zhuǎn)向。
3.3無線傳輸模塊
本次設(shè)計(jì)所選取的無線傳輸芯片是藍(lán)牙模塊 HC06,它是主從一體化的串口傳輸?shù)臒o線通訊模塊。通過手機(jī)藍(lán)牙APP連接至HC06藍(lán)牙模塊上,然后手機(jī)產(chǎn)生一系列的控制信號(hào)通過藍(lán)牙傳輸給平衡小車的主控芯片,主控芯片經(jīng)過識(shí)別解碼,再對(duì)平衡小車進(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í)鐘定義。然后采集小車的速度信息和角度信息再進(jìn)行濾波融合處理,通過PID控制輸出相應(yīng)的PWM信號(hào)控制電機(jī)轉(zhuǎn)動(dòng),達(dá)到平衡狀態(tài)。姿態(tài)角檢測(cè)模塊MPU6050與主控芯片是IIC通訊,獲得來自小車的三軸陀螺儀信號(hào)和三軸加速度計(jì)信號(hào)。磁力計(jì)與MPU6050通過IIC連接,與陀螺儀和加速度計(jì)共同作用。小車車速的檢測(cè)是通過編碼器采集到的信息轉(zhuǎn)化成對(duì)應(yīng)的速度值大小實(shí)現(xiàn)的。單片機(jī)通過定時(shí)器捕獲脈沖信號(hào)和輸出電機(jī)驅(qū)動(dòng)PWM波。
系統(tǒng)初始化完成后,系統(tǒng)等待MPU6050初始化完成,直到MPU6050初始化完成時(shí),OLED被點(diǎn)亮顯示卡爾曼濾波模式、左右電機(jī)速度、MPU6050內(nèi)部溫度、電池電壓以及自平衡小車自身傾角。OLED屏幕上顯示的數(shù)據(jù)是實(shí)時(shí)更新的,等待按下啟動(dòng)按鈕,當(dāng)按下啟動(dòng)按鈕時(shí),自平衡小車自動(dòng)進(jìn)入直立平衡模式,保持自身的直立狀態(tài),若有手機(jī)通過藍(lán)牙連接至自平衡小車,則進(jìn)入控制模式,控制模式是在直立模式的基礎(chǔ)上完成的,并且可以完成轉(zhuǎn)彎,前進(jìn)和后退動(dòng)作。
主程序的主循環(huán)里面主要是對(duì)顯示的控制,選擇是否將數(shù)據(jù)更新到連接到小車的手機(jī)APP上面。然后程序中所有任務(wù)的調(diào)度都是在外部中斷服務(wù)程序中執(zhí)行。
4.2 濾波和控制算法
平衡小車獲取姿態(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ì)過程中要考慮到主芯片STM32的資源分配,根據(jù)傳感器的控制端口類型以及所支持的接口協(xié)議,分別來設(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)試。通過調(diào)試前對(duì)PID控制原理和各參數(shù)的物理含義有基本的理解,然后在進(jìn)行PID代碼原理實(shí)現(xiàn)以及PID參數(shù)調(diào)試,在調(diào)試的過程中逐步理解PID。
參考文獻(xiàn):
[1]李紅美,李智,高飛.平衡的杰作——賽格威FIT兩輪平臺(tái)電動(dòng)車[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-),男,漢,四川德陽,本科,研究方向:電氣工程。
魏虎(1995-),男,漢,四川綿陽,本科,研究方向:通信工程。