魏 雅

基于單片機(jī)的兩輪自平衡小車的設(shè)計(jì)?

魏 雅

（陜西工業(yè)職業(yè)技術(shù)學(xué)院 咸陽(yáng) 712000）

系統(tǒng)采用陀螺儀MPU6050構(gòu)成小車姿態(tài)檢測(cè)裝置，使用卡爾曼濾波完成陀螺儀數(shù)據(jù)與加速度計(jì)數(shù)據(jù)的融合，選用STM32F103單片機(jī)為控制核心，完成了傳感器信號(hào)的處理，實(shí)現(xiàn)了車身的控制。整個(gè)系統(tǒng)的各個(gè)模塊能夠正常、協(xié)調(diào)工作，小車可以在無(wú)人干預(yù)條件下實(shí)現(xiàn)自主平衡。同時(shí)在引入適量干擾情況下小車能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)。

兩輪自平衡；陀螺儀；姿態(tài)檢測(cè)；卡爾曼濾波；數(shù)據(jù)融合

AbstractThe system uses gyroscope MPU6050 car attitude detection device，uses the kalman filter to complete the data fu?sion of gyroscope and accelerometer data，uses STM32F103 microcontroller as control core，completes the processing of the sensor signal，realizes the control of the body.Each module of the whole system can work normally and coordinate，and the car can realize independent balance under the condition of no intervention.At the same time，in the case of the introduction of appropriate interfer?ence，the car can be adjusted independently and quickly restored to a stable state.

Key Wordstwo wheel self balancing，gyroscope，attitude detection，kalman filter，data fusion

Class NumberTP3-02

1 引言

近年來(lái)，隨著電子技術(shù)的發(fā)展與進(jìn)步，移動(dòng)機(jī)器人的應(yīng)用范圍越來(lái)越廣泛，但是怎樣讓移動(dòng)機(jī)器人在凹凸不平的地面上行走，或者在較狹窄的地方行走等，逐漸成為研究者關(guān)心的問(wèn)題。系統(tǒng)設(shè)計(jì)的兩輪自平衡機(jī)小車［1］，采用了兩輪共軸、各自獨(dú)立驅(qū)動(dòng)的工作方式，車身的重心位于車輪軸的上方，通過(guò)輪子的前后移動(dòng)來(lái)保持車身的平衡，并且還能夠在直立平衡的情況下行駛。由于特殊的結(jié)構(gòu)，其適應(yīng)地形變化能力強(qiáng)，運(yùn)動(dòng)靈活，可以在復(fù)雜的環(huán)境里工作。

2 系統(tǒng)硬件電路設(shè)計(jì)

系統(tǒng)硬件設(shè)計(jì)總框圖如圖1所示。在硬件電路設(shè)計(jì)時(shí)，對(duì)電路的所有環(huán)節(jié)都進(jìn)行了電磁兼容性設(shè)計(jì)，做好各部分的接地、屏蔽、濾波等工作，將高速數(shù)字電路與模擬電路分開(kāi)，從而大大提高本系統(tǒng)工作的可靠性和高效性。

圖1 硬件設(shè)計(jì)總框圖

2.1 單片機(jī)最小系統(tǒng)電路

系統(tǒng)采用 STM32F103 增強(qiáng)型系列單片機(jī)［2～3］為控制核心，采用的是5V供電，由它構(gòu)成的最小系統(tǒng)［4～5］主要包括單片機(jī)供電、復(fù)位電路、時(shí)鐘電路。由于單片機(jī)內(nèi)部集成了多個(gè)模塊，因此使用方便。

設(shè)計(jì)中時(shí)鐘電路采用的外部晶振為16MHz，電容C11和C12為外部時(shí)鐘的起振電容。在單片機(jī)進(jìn)入睡眠模式時(shí)，時(shí)鐘振蕩電路輸入端被內(nèi)部200Ω的下拉電阻拉低，振蕩電路停振，從而達(dá)到省電的目的。圖2為單片機(jī)最小系統(tǒng)時(shí)鐘電路原理圖。

圖2 時(shí)鐘電路原理圖

圖3 復(fù)位電路

設(shè)計(jì)中采用了由電阻電容構(gòu)成的簡(jiǎn)易復(fù)位電路，如圖3所示。加電后，由于電容的充電，RESET保持低電平，單片機(jī)復(fù)位；一段時(shí)間后，電容電量充滿，RESET端輸出高電平，此時(shí)單片機(jī)運(yùn)行。手動(dòng)復(fù)位時(shí)，按下手動(dòng)復(fù)位按鈕，RESET端保持低電平，單片機(jī)復(fù)位；釋放手動(dòng)復(fù)位按鈕后，RESET端輸出高電平，單片機(jī)工作。

2.2 電源模塊設(shè)計(jì)

電源模塊［6］由若干相互獨(dú)立的穩(wěn)壓電路模塊組成，這樣做可以減少各模塊之間的相互干擾。整個(gè)系統(tǒng)需要三種電源：1）7.2V電源，為驅(qū)動(dòng)電機(jī)供電；2）5V電源，為單片機(jī)及相關(guān)外設(shè)供電；3）3.3V電源，為陀螺儀及加速度計(jì)供電。

整個(gè)系統(tǒng)電源來(lái)源為7.2V鎳氫電池，5V電源由LM2940提供，3.3V電源采用AMS1117。電機(jī)供電直接采用電池供電，如圖4所示為電源模塊電路。

圖4 電源模塊電路圖

2.3 傾角傳感器信號(hào)調(diào)理電路

系統(tǒng)采用MPU6050陀螺儀，其輸出為0.67mv/deg/sec。ATD模塊最高采集精度為12bit，AD基準(zhǔn)電壓為3.3V，計(jì)算得出最小分辨電壓為0.8mv，因此不能直接對(duì)陀螺儀輸出信號(hào)進(jìn)行采集，需要設(shè)計(jì)放大電路。系統(tǒng)采用LM358（圖5 U1B）設(shè)計(jì)負(fù)反饋放大電路，放大倍數(shù)為+1，即放大10倍。同時(shí)，由于陀螺儀輸出會(huì)隨溫度而變化，影響系統(tǒng)傾角檢測(cè)精度，為抑制陀螺儀溫漂，需要在放大電路中設(shè)計(jì)零點(diǎn)偏置電壓調(diào)整電路。系統(tǒng)中利用LM358（圖5 U1A）構(gòu)成電壓跟隨器，輸出電壓通過(guò)電位器調(diào)節(jié)，使零點(diǎn)偏置電壓保持在陀螺儀工作電壓的一半（1.65V），這樣可以有效抑制陀螺儀的溫漂，放大電路如圖5。

圖5 放大電路

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

本系統(tǒng)中，根據(jù)功能要求，采用TB6612電機(jī)驅(qū)動(dòng)電路。由于電機(jī)啟動(dòng)瞬間電流很大，會(huì)將整個(gè)系統(tǒng)電壓拉低，造成其他設(shè)備如單片機(jī)的工作不正常，因此要在電池電源輸入側(cè)加上較大濾波電容。

如圖6所示，PWM1和PWM2分別為兩個(gè)半橋的控制端口。當(dāng)PWM1為高電平，PWM2為低電平時(shí)，MOTOR1口即輸出高電壓，MOTOR2輸出低電壓，此時(shí)電機(jī)正轉(zhuǎn)；當(dāng)PWM1為低電平而PWM2為高電平時(shí)，MOTOR1口即輸出低電壓，MOTOR2輸出高電壓，此時(shí)電機(jī)反轉(zhuǎn)。通過(guò)改變PMW 1和PWM2端口的驅(qū)動(dòng)波形占空比改變輸出端電壓，從而實(shí)現(xiàn)電機(jī)調(diào)速的目的。

2.5 速度檢測(cè)模塊設(shè)計(jì)

兩輪自平衡小車的原理［7～9］是利用地面對(duì)車輪的摩擦力抵消車受到的重力，在系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。為實(shí)現(xiàn)速度的閉環(huán)控制，必須加入速度檢測(cè)裝置實(shí)現(xiàn)速度閉環(huán)控制中的反饋環(huán)節(jié)。系統(tǒng)測(cè)速模塊采用旋轉(zhuǎn)霍爾編碼器。由于編碼器采用集電極開(kāi)路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡(jiǎn)單。需要在信號(hào)輸出端接入一個(gè)上拉電阻，即可將信號(hào)提供給單片機(jī)采集數(shù)據(jù)。如圖7所示，PULSE引腳為編碼器A相，接單片機(jī)的脈沖計(jì)數(shù)口，通過(guò)單片機(jī)的PACNT模塊對(duì)輸入脈沖進(jìn)行計(jì)數(shù)從而獲取電機(jī)轉(zhuǎn)速。DIR為編碼器B相輸出，接單片機(jī)I/O口，通過(guò)A、B相位差進(jìn)行軟件鑒相，從而判斷電機(jī)轉(zhuǎn)動(dòng)方向。

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

圖7 編碼器接口電路

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

軟件設(shè)計(jì)［10～11］是兩輪自平衡小車設(shè)計(jì)中的關(guān)鍵。根據(jù)系統(tǒng)要求，軟件需要完成單片機(jī)初始化、姿態(tài)信息采集、卡爾曼濾波、速度檢測(cè)、直流電機(jī)PID控制算法等。系統(tǒng)軟件流程如圖8所示。由于篇幅問(wèn)題，在此重點(diǎn)介紹姿態(tài)信息采集軟件設(shè)計(jì)。姿態(tài)信息采集包括陀螺儀與加速度計(jì)輸出值轉(zhuǎn)換及卡爾曼濾波器軟件實(shí)現(xiàn)兩部分。

圖8 系統(tǒng)總體軟件流程圖

3.1 陀螺儀與加速度計(jì)輸出值轉(zhuǎn)換

本系統(tǒng)采用的慣性傳感器為模擬量輸出式，利用單片機(jī)自帶ATD模塊進(jìn)行模數(shù)轉(zhuǎn)換，從而檢測(cè)出傳感器輸出電壓。此外還需要對(duì)陀螺儀及加速度計(jì)的輸出進(jìn)行轉(zhuǎn)換運(yùn)算，使最終送入卡爾曼濾波器的為陀螺儀檢測(cè)的角速度以及加速度計(jì)檢測(cè)的角度。

陀螺儀輸出電壓與檢測(cè)角速度轉(zhuǎn)換公式如下

式中：G為陀螺儀檢測(cè)的角速度；Vout為陀螺儀輸出電壓；Voffest為陀螺儀靜止時(shí)的輸出電壓；Vsen為陀螺儀靈敏度；K為放大電路放大系數(shù)。加速度計(jì)輸出電壓與檢測(cè)的角度轉(zhuǎn)換公式如下

式中：A為加速度計(jì)檢測(cè)的重力加速度；Vout為加速度計(jì)輸出電壓；Voffest為加速度計(jì)在平衡位置的輸出電壓；Vsen為加速度計(jì)靈敏度。再利用反三角函數(shù)求出與重力方向的傾角。

根據(jù)式（1）將陀螺儀的輸出電壓轉(zhuǎn)換為角速度；根據(jù)式（2）求出加速度計(jì)的輸出角度acc（此時(shí)得出的角度為弧度），為了將加速度計(jì)的輸出角度與陀螺儀的積分角度單位統(tǒng)一，需要對(duì)加速度計(jì)的輸出角度進(jìn)行單位換算，通過(guò)式（3）進(jìn)行換算得出角度。

3.2 卡爾曼濾波器的軟件實(shí)現(xiàn)

由卡爾曼濾波［12］原理可知，卡爾曼濾波器是一種高效率的遞歸濾波器（自回歸濾波器）。其主要原理是利用前一狀態(tài)的估計(jì)值和當(dāng)前狀態(tài)的觀測(cè)值更新對(duì)狀態(tài)變量的估計(jì)，從而得到當(dāng)前狀態(tài)的估計(jì)值。卡爾曼濾波器的工作流程如圖9所示。

圖9 卡爾曼濾波器預(yù)估-更新流程圖

通過(guò)卡爾曼增益計(jì)算出最優(yōu)估計(jì)值及預(yù)測(cè)值偏差，得到最優(yōu)角度值及角速度值。

4 結(jié)語(yǔ)

通過(guò)相應(yīng)硬件與軟件的設(shè)計(jì)，最終實(shí)現(xiàn)了兩輪自平衡小車的平衡控制及運(yùn)動(dòng)控制。小車可以在無(wú)人干預(yù)條件下實(shí)現(xiàn)自主平衡。同時(shí)在引入適量干擾情況下小車能夠自主調(diào)整并迅速恢復(fù)穩(wěn)定狀態(tài)。

［1］季鵬飛，朱燕，程傳統(tǒng)，等.基于STM32的兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)［J］.電子科技，2014，27（11）：96-99.

JI Pengfei，ZHU Yan，CHENG Jing，et al.Design of con?trol system for two wheeled self balancing vehicle based on［J］.Electronic Science and technology，2014，27（11）：96-99.

［2］郝蕓，楊奇，佟皓萌.基于STM32的四軸飛行器設(shè)計(jì)［J］.電子測(cè)試，2015（18）：6-8.

HAO Yun，YANG Qi，TONG Haomeng.four axis aircraft design based on STM32［J］.Electronic Test，2015（18）：6-8.

［3］常國(guó)權(quán)，戴國(guó)強(qiáng).基于STM32的四軸飛行器飛控系統(tǒng)設(shè)計(jì)［J］.單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2015，15（2）：29-32.

CHANG Guoquan，DAI Guoqiang.STM32 four axis air?craft flight control system design of［J］.microcontroller and embedded systems based on STM32，2015，15（2）：29-32.

［4］王素青，熊維堂.基于STM32的兩輪自平衡小車系統(tǒng)設(shè)計(jì)［J］.實(shí)驗(yàn)室研究與探索，2016，35（5）：146-150.

WANG Suqing，XIONG Weitang.Two STM32 based on self research and exploration，the design of［J］.car system laboratory balance，2016，35（5）：146-150.

［5］張志強(qiáng).基于STM32的雙輪平衡車［J］.電子設(shè)計(jì)工程，2011，19（13）：103-106.

ZHANG Zhiqiang.STM32.The two wheeled balancing car［J］.Electronic design project，based on the，2011，19（13）：103-106.

［6］王振運(yùn).基于RT-Thread和STM32的雙輪自平衡機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)［D］.太原：中北大學(xué)，2016.

WANG Zhenyun.A RT-Thread and STM32 based on the design and implementation of the self balancing robot［D］.Taiyuan：North Central University，2016.

［7］梁華，李曉虹，楊光祥.兩輪自平衡機(jī)器人動(dòng)力學(xué)模型分析及PID控制方法研究［J］.重慶師范大學(xué)學(xué)報(bào)（自然科學(xué)版），2016（1）：163-167.

LIANG Hua，LI Xiaohong，YANG Guangxiang.Study on dynamic model of two wheeled self balancing robot and PID control method［J］.Journal of Chongqing Normal Uni?versity（Natural Science Edition），2016（1）：163-167.

［8］馮智勇，曾瀚，張力，等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量［J］.西南師范大學(xué)學(xué)報(bào)自然科學(xué)版，2011，36（4）：137-141.

FENG Zhiyong，ZENG Han，ZHANG Li，et al.The gyro?scope and accelerometer based on attitude angle measure?ment signal fusion［J］.Natural Science Journal of South?western Normal University，2011，36（4）：137-141.

［9］阮曉鋼，任紅格.兩輪自平衡機(jī)器人動(dòng)力學(xué)建模及其平衡控制［J］.計(jì)算機(jī)應(yīng)用研究，2009，26（1）：99-101.

RUAN Xiaogang，REN Hongge.Dynamic modeling and balance control of two wheeled self balancing robot［J］.Computer Application Research，2009，26（1）：99-101.

［10］華瑾，高嵩，秦剛，等.兩輪自平衡機(jī)器人動(dòng)力學(xué)建模與運(yùn)動(dòng)控制［J］.西安工業(yè)大學(xué)學(xué)報(bào)，2014（10）：851-855.

HUA Jin，GAO Song，QIN Gang，et al.The two wheeled self balancing robot dynamics modeling and motion con?trol［J］.Journal of Xi’an Technological University，2014（10）：851-855.

［11］張洪偉.基于STM32的平衡車設(shè)計(jì)［J］.科技創(chuàng)新與應(yīng)用，2016（2）：78-79.

ZHANGHongwei.Design of balance car based on STM32［J］.Science and Technology Innovation and Applica?tion，2016（2）：78-79.

［12］王學(xué)斌，徐建宏，張章.卡爾曼濾波器參數(shù)分析與應(yīng)用方法研究［J］.計(jì)算機(jī)應(yīng)用與軟件，2012，29（6）：212-215.

WANG Xubin，WU Jianhong，ZHANG Zhang.On Analy?sis and Application Approach for kalman Filtar Parame?ters［J］.Computer Application and Software，2012，29（6）：212-215.

Design of Two W heeled Self Balancing Car Based on MCU

WEI Ya
（Shanxi Polytechnic Institute，Xianyang 712000）

TP3-02

10.3969/j.issn.1672-9722.2017.09.038

2017年3月9日，

2017年4月22日

國(guó)家自然科學(xué)基金項(xiàng)目“基于空間打壓矢量的異步電動(dòng)機(jī)軟啟動(dòng)控制理論與方法研究”（編號(hào)：51577110）資助。

魏雅，女，碩士，副教授，研究方向：電子信息和計(jì)算機(jī)。