雙輪自平衡小車控制系統(tǒng)設(shè)計(jì)

蚌埠學(xué)院電子與電氣工程系 孟 帥 常德軍 陳章寶

雙輪自平衡小車控制系統(tǒng)設(shè)計(jì)

蚌埠學(xué)院電子與電氣工程系 孟 帥 常德軍 陳章寶

雙輪自平衡小車是雙輪機(jī)器人研究的基礎(chǔ)，具有多變量、非線性、強(qiáng)耦合等特性。本文以STC15W4K32S4為主控芯片，MPU6050采集車身的角度和角速度，通過(guò)互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合，得到優(yōu)化的角度值，以TB6612FNG作為電機(jī)驅(qū)動(dòng)芯片，霍爾元件進(jìn)行車速測(cè)量，構(gòu)建雙輪小車的姿態(tài)平衡控制和運(yùn)動(dòng)控制系統(tǒng)。通過(guò)姿態(tài)解算和PID控制，實(shí)現(xiàn)了小車的平穩(wěn)運(yùn)行。

雙輪自平衡小車；STC15W4K32S4；MPU6050；TB6612FNG；PID

引言

雙輪自平衡小車因其結(jié)構(gòu)簡(jiǎn)單、轉(zhuǎn)向靈活等特點(diǎn)，能在狹窄空間中行駛，以及在特殊環(huán)境下執(zhí)行特殊任務(wù)，對(duì)變化的地形適應(yīng)性強(qiáng)，引起新一輪研究熱潮和廣泛的應(yīng)用開發(fā)。雙輪自平衡小車作為控制系統(tǒng)的經(jīng)典模型，是驗(yàn)證控制理論和力學(xué)理論的理想實(shí)驗(yàn)平臺(tái)，為科學(xué)理論的實(shí)驗(yàn)研究起到了硬件平臺(tái)的作用，有著巨大的研究?jī)r(jià)值[1]。

1.小車運(yùn)動(dòng)平衡分析

設(shè)小車模型車體重心到底盤重心長(zhǎng)度為l，質(zhì)量為m，當(dāng)車輪產(chǎn)生水平向右的加速度α?xí)r，對(duì)車模受力分析如下圖1所示。

圖1 車模受力分析

根據(jù)小車平衡狀態(tài)受力和“角加速度與曲率半徑的乘積等于切向加速度”可得到車模運(yùn)動(dòng)過(guò)程中的動(dòng)態(tài)數(shù)學(xué)模型[2]：

在初始狀態(tài)時(shí)，車輪加速度α為0，并進(jìn)行工程近似：在θ較小時(shí)，，模型可以簡(jiǎn)化為下式。

對(duì)上式微分方程等式兩邊同時(shí)進(jìn)行拉普拉斯變換，得到靜止?fàn)顟B(tài)下系統(tǒng)傳遞函數(shù)為：

由式可解系統(tǒng)有一個(gè)極點(diǎn)位于復(fù)頻域?qū)嵼S的右半部分，所以在靜止時(shí)車身無(wú)法達(dá)到平衡，為此需引入比例微分控制，對(duì)車模傳遞函數(shù)進(jìn)行校正，提高系統(tǒng)的穩(wěn)定性。在增加比例微分負(fù)反饋后，相當(dāng)于增加了系統(tǒng)的開環(huán)零點(diǎn)，系統(tǒng)兩極點(diǎn)均具有負(fù)實(shí)部，系統(tǒng)穩(wěn)定[3]。

2.硬件電路設(shè)計(jì)

為實(shí)現(xiàn)雙輪自平衡小車的平衡控制和運(yùn)動(dòng)控制，系統(tǒng)硬件設(shè)計(jì)包括主控模塊、姿態(tài)檢測(cè)模塊（陀螺儀和加速度計(jì)）、測(cè)速模塊、直流電機(jī)驅(qū)動(dòng)模塊、電源模塊和人機(jī)交互模塊，控制系統(tǒng)框圖如圖2所示。電壓轉(zhuǎn)換電路采用LM2940-5.0，將電池組的+12V電壓轉(zhuǎn)成+5V，用AMS1117-3.3穩(wěn)壓電路將+5V轉(zhuǎn)成+3.3V，人機(jī)交互模塊使用LCD1602和四個(gè)獨(dú)立按鍵，實(shí)現(xiàn)系統(tǒng)調(diào)試時(shí)的參數(shù)設(shè)置和顯示。

圖2 控制系統(tǒng)方框圖

2.1 主控模塊設(shè)計(jì)

系統(tǒng)主控模塊采用STC15系列MCU，STC15W4K32S4是宏晶公司生產(chǎn)的增強(qiáng)型8051內(nèi)核的單片機(jī)。工作電壓2.5～5.5V，有6路硬件PWM、7個(gè)定時(shí)器、4組串口、8路高速10位ADC，最大有61k程序Flash和4k字節(jié)SRAM，內(nèi)部集成了時(shí)鐘和復(fù)位電路。通過(guò)引腳1、3與MPU6050進(jìn)行I2C通信，采集姿態(tài)數(shù)據(jù)，引腳30～33、34、35連接電機(jī)驅(qū)動(dòng)芯片TB6612，控制電機(jī)的轉(zhuǎn)向和轉(zhuǎn)速，通過(guò)引腳38、39捕獲霍爾傳感器輸入脈沖；通過(guò)引腳21、2、22、4可以對(duì)霍爾傳感器兩相A1(A2)、B1(B2)進(jìn)行外部中斷觸發(fā)和計(jì)數(shù)器計(jì)數(shù)。

2.2 姿態(tài)檢測(cè)電路

姿態(tài)傳感器使用的是MPU6050，內(nèi)部集成了三軸加速度計(jì)和三軸陀螺儀，所以又稱為六軸傳感器[4]，具有體積小、功耗低、分辨率高等優(yōu)點(diǎn)。模塊內(nèi)置三個(gè)16位模數(shù)轉(zhuǎn)換器，加速度計(jì)測(cè)量范圍±2、±4、±8、±16g，陀螺儀測(cè)量范圍±250、±500、±1000、±2000°/sec，可編程控制姿態(tài)傳感器的測(cè)量范圍，來(lái)提高讀取數(shù)據(jù)的最小精度，使用引腳23、24與主控間進(jìn)行I2C通信。姿態(tài)檢測(cè)電路圖如圖3所示。

圖3 姿態(tài)檢測(cè)電路

2.3 電機(jī)驅(qū)動(dòng)電路

電機(jī)驅(qū)動(dòng)芯片選用的是東芝生產(chǎn)的TB6612，它的最大工作電壓高達(dá)15V，平均工作電流可達(dá)1.2A，瞬時(shí)最大電流可以達(dá)到3.2A，而且PWM控制頻率可高達(dá)100KHz[5]。AIN1、AIN2、BIN1、BIN2腳為輸入控制電平，控制電機(jī)正、反轉(zhuǎn)，PWM3、PWM4腳與MCU的PWM輸出口相連，控制電機(jī)加、減速，電機(jī)驅(qū)動(dòng)電路如圖4所示。TB6612具有大電流H橋結(jié)構(gòu)，雙通道電路輸出，可同時(shí)驅(qū)動(dòng)兩個(gè)電機(jī)，AO1(BO1)、和AO2(BO2)之間可分別接電機(jī)。

圖4 電機(jī)驅(qū)動(dòng)電路

2.4 霍爾測(cè)速電路

霍爾傳感器是基于霍爾效應(yīng)的磁場(chǎng)傳感器，主要由磁塊和磁電檢測(cè)裝置組成[5]。將磁鐵粘在非磁性圓盤的邊沿，靠近圓盤邊沿處放置霍爾傳感器，當(dāng)圓盤轉(zhuǎn)動(dòng)時(shí)，霍爾傳感器就輸出脈沖，相應(yīng)的計(jì)數(shù)變量就加1，在單位時(shí)間內(nèi)測(cè)得的脈沖數(shù)越多代表速度越快。脈沖采集電路主要由電壓比較器LM393、霍爾元件3144和磁感應(yīng)探頭組成，采用非接觸檢測(cè)方式，D6為感應(yīng)指示燈，當(dāng)感應(yīng)到磁場(chǎng)時(shí)，輸出低電平，信號(hào)燈D6亮，3144感應(yīng)磁場(chǎng)的強(qiáng)度并輸出數(shù)字信號(hào)，再由LM393轉(zhuǎn)化成為脈沖。

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

采用模塊化的設(shè)計(jì)思想，系統(tǒng)軟件設(shè)計(jì)包括：數(shù)據(jù)采集與傳輸，PID控制，PWM驅(qū)動(dòng)及其他模塊程序設(shè)計(jì)。系統(tǒng)上電初始化后，首先檢測(cè)傾角是否在可控范圍內(nèi)，條件為真時(shí)進(jìn)行數(shù)據(jù)采集與傳輸，通過(guò)角度PD和速度PI控制PWM輸出，調(diào)節(jié)PWM占空比來(lái)調(diào)節(jié)車速，從而保持車模的動(dòng)態(tài)平衡，通過(guò)改變給定的車體傾角θ實(shí)現(xiàn)小車的運(yùn)行。

六軸姿態(tài)傳感器MPU6050內(nèi)置了三軸加速度計(jì)和三軸陀螺儀，知道六個(gè)軸的數(shù)據(jù)寄存器的地址，就能從數(shù)據(jù)寄存器中讀取數(shù)據(jù)，MPU6050中自帶了I2C程序，利用由數(shù)據(jù)線SDA和時(shí)鐘線SCL構(gòu)成的兩線式串行總線，主控單片機(jī)就能通過(guò)I2C通信讀取姿態(tài)傳感器的未經(jīng)過(guò)濾波的角度，通過(guò)互補(bǔ)濾波進(jìn)行數(shù)據(jù)融合，得到小車的傾角值。

PID（比例、積分、微分）控制以車身傾角為設(shè)定值[6]，以偏差e(t)為輸入，構(gòu)建雙閉環(huán)PID調(diào)節(jié)控制器，通過(guò)第一級(jí)角度PD控制計(jì)算出小車保持動(dòng)態(tài)平衡需要的車輪速度值，然后以直流電機(jī)上霍爾測(cè)速裝置構(gòu)成第二級(jí)的速度PI控制。第一級(jí)角度反饋不斷去補(bǔ)償角度偏差直至最后接近零，第二級(jí)速度反饋調(diào)節(jié)加到電機(jī)上保持小車以特定速度運(yùn)行。

PWM即脈沖寬度調(diào)制，它是指將輸出信號(hào)的最小周期固定，通過(guò)調(diào)整最小周期內(nèi)工作周期的大小來(lái)控制輸出電壓的方法。本設(shè)計(jì)中，通過(guò)PWM調(diào)寬可以快速地控制輸入直流電機(jī)的電壓大小來(lái)達(dá)到調(diào)速的目的，設(shè)最小周期固定時(shí)，輸出電壓最大為，設(shè)占空比為，則平均輸出電壓為，當(dāng)改變占空比α?xí)r，就可以得到不同大小的輸出電壓，從而達(dá)到調(diào)速的目的。

4.總結(jié)

本次設(shè)計(jì)以STC15W4K32S4為主控芯片，通過(guò)互補(bǔ)濾波和PID控制算法，設(shè)計(jì)出能夠自平衡的兩輪直立前進(jìn)后退、左右轉(zhuǎn)向的控制系統(tǒng)。通過(guò)互補(bǔ)濾波算法，濾波后的角度，既能在靜態(tài)時(shí)緊緊趨近于加速度計(jì)的角度，也能在動(dòng)態(tài)時(shí)隨陀螺儀角度的變化而變化，角度PD控制算法通過(guò)互補(bǔ)濾波輸出優(yōu)化的角度并乘以比例增益，再相加未經(jīng)濾波的角速度乘以微分增益的值，兩者共同輸出角度PD控制下的電機(jī)驅(qū)動(dòng)PWM信號(hào)；速度PI控制算法將設(shè)定與實(shí)測(cè)速度值的偏差作為輸入，先積分后乘以積分增益，再結(jié)合輸入乘以比例增益得到的值，兩者共同輸出速度PI作用下的電機(jī)驅(qū)動(dòng)PWM信號(hào)，角度與速度輸出的PWM值相加得到最終電機(jī)驅(qū)動(dòng)PWM信號(hào)。實(shí)際運(yùn)行結(jié)果表明，小車上電后的動(dòng)態(tài)特性良好，能夠有效實(shí)現(xiàn)對(duì)小車整體平穩(wěn)運(yùn)行。本設(shè)計(jì)實(shí)現(xiàn)了小車在平整路面上的運(yùn)動(dòng)控制，如何實(shí)現(xiàn)小車在不平整及障礙路面執(zhí)行特定的任務(wù)，需在以后的實(shí)踐中進(jìn)一步進(jìn)行研究。

[1]徐國(guó)保,尹怡欣等.智能移動(dòng)機(jī)器人技術(shù)現(xiàn)狀及展望[J].機(jī)器人技術(shù)與應(yīng)用,2010.3:29-32.

[2]上海交通大學(xué)物理教研室.大學(xué)物理學(xué)(第四版)[M].上海:上海交通大學(xué)出版社,2010:115-117.

[3]高金源,夏潔.計(jì)算機(jī)控制系統(tǒng)[M].北京:清華大學(xué)出版社,2010:159-164.

[4]賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計(jì)[J].河南工程學(xué)院學(xué)報(bào),2014,26(1):53-57.

[5]劉江.兩輪自平衡機(jī)器人系統(tǒng)設(shè)計(jì)及控制研究[D].北京:北京工業(yè)大學(xué)控制科學(xué)與工程系,2007.

[6]孟慶明.自動(dòng)控制原理(非自動(dòng)化類)(第二版)[M].北京:高等教育出版社,2008:179-183.

孟帥（1995－），男，本科，電子信息工程，研究方向：嵌入式系統(tǒng)開發(fā)，機(jī)器人運(yùn)動(dòng)控制。

大學(xué)生創(chuàng)新創(chuàng)業(yè)計(jì)劃項(xiàng)目（AH201411305036），2016蚌埠學(xué)院教學(xué)研究項(xiàng)目（2016JYXM28）。