季鵬飛，朱 燕，程傳統(tǒng)，杜 曉

(鹽城師范學(xué)院物理科學(xué)與電子技術(shù)學(xué)院，江蘇鹽城 224002)

兩輪自平衡小車是一種自然不穩(wěn)定體，其動(dòng)力學(xué)系統(tǒng)具有多變量、非線性、強(qiáng)耦合等特性，是研究各種控制方法的理想平臺(tái)。兩輪小車的自平衡控制過程是利用微控制器對(duì)系統(tǒng)各種狀態(tài)參數(shù)進(jìn)行實(shí)時(shí)分析，使系統(tǒng)在水平方向或垂直方向上的位移和角度的偏移量被控制在允許的范圍以內(nèi)，使之在靜態(tài)平衡點(diǎn)附近保持平衡，或者在一定范圍內(nèi)在運(yùn)動(dòng)過程中使小車保持平衡。因此小車控制的關(guān)鍵是姿態(tài)傾角、角速度及車輪加速度測(cè)量的準(zhǔn)確性和實(shí)時(shí)性，這直接決定小車的控制精度及穩(wěn)定性。針對(duì)這一問題，本文采用高性能單片機(jī)STM32F103C8T6作控制器，通過安裝在車輪軸上的光柵碼盤等實(shí)現(xiàn)小車位移、速度及運(yùn)動(dòng)方向的測(cè)量，用數(shù)字運(yùn)動(dòng)傳感器MPU－6050實(shí)現(xiàn)車體傾角及傾角角速度的測(cè)量，利用卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，精確地計(jì)算出小車的實(shí)時(shí)姿態(tài)信息，再利用PID控制算法實(shí)現(xiàn)小車的自平衡控制［1］。

1 動(dòng)力學(xué)模型

兩輪小車受力分析如圖1所示。圖中小車質(zhì)量為m，車輪半徑為r、車速為v，車體自身水平作用力為H、車輪受到地面摩擦力為Hf，θ為傾角，車輪對(duì)車軸的等效轉(zhuǎn)動(dòng)慣量為J，車體對(duì)車輪吊銷轉(zhuǎn)動(dòng)慣量為I，車輪轉(zhuǎn)速為ω，電機(jī)電磁轉(zhuǎn)矩為Tm，車體質(zhì)心位置坐標(biāo)(x，y)，車體質(zhì)量為ma，車輪對(duì)車身扭矩為T。在傾角θ較小的情況下，分析可得小車動(dòng)力學(xué)方程可簡化為［2］

圖1 小車受力分析方程

式(1)為兩輪自平衡小車車體傾角與角加速度的微分。文獻(xiàn)指出在小車質(zhì)量一定的情況下，地面摩擦系數(shù)越大、電機(jī)扭矩越高，小車自平衡可控角度范圍越大，越容易控制小車的平衡［3］。

2 卡爾曼濾波數(shù)據(jù)融合算法

車體傾角測(cè)量由陀螺儀和加速度傳感器實(shí)現(xiàn)，單獨(dú)使用陀螺儀或加速度傳感器都無法完成傾角的測(cè)量。一方面，陀螺儀能夠測(cè)量車體傾斜的角速度，對(duì)角速度積分得到車體傾角，但在實(shí)際應(yīng)用中陀螺儀輸出的信號(hào)有偏差或漂移，經(jīng)過積分運(yùn)算后會(huì)形成隨時(shí)間增加而增大的積累誤差，導(dǎo)致無法得到有效的傾角信號(hào);另一方面，小車運(yùn)動(dòng)中車身會(huì)不斷擺動(dòng)，導(dǎo)致加速度傳感器輸出的信號(hào)中有很大的干擾，雖然通過數(shù)據(jù)平滑濾波可有效降低干擾，但濾波后的信號(hào)已無法實(shí)時(shí)反映車體傾角的變化，因此僅用加速度傳感器也無法實(shí)現(xiàn)傾角測(cè)量。陀螺儀和加速度傳感器輸出的信號(hào)中含有高斯白噪聲，使用卡爾曼濾波算法將陀螺儀和加速度傳感器輸出的數(shù)據(jù)進(jìn)行優(yōu)化處理，補(bǔ)償加速度傳感器的動(dòng)態(tài)誤差及陀螺儀的漂移誤差，可得到準(zhǔn)確的車體傾角。

2.1 卡爾曼濾波器一般形式

卡爾曼濾波算法是一種求最優(yōu)估計(jì)值的遞推算法，是利用前一狀態(tài)的估計(jì)值和當(dāng)前的測(cè)量值來更新對(duì)狀態(tài)變量的預(yù)測(cè)和估計(jì)，最終可得到當(dāng)前狀態(tài)的最優(yōu)估計(jì)值。對(duì)本系統(tǒng)而言，將陀螺儀輸出的角速度作為預(yù)測(cè)值的控制量，加速度傳感器輸出的角度作為測(cè)量值，對(duì)車體當(dāng)前傾角進(jìn)行最優(yōu)估計(jì)。記是 k時(shí)刻對(duì)狀態(tài)的先驗(yàn)估計(jì)，是k時(shí)刻得到觀測(cè)值后對(duì)狀態(tài)的后驗(yàn)估計(jì)，是先驗(yàn)估計(jì)協(xié)方差，Pk為后驗(yàn)估計(jì)協(xié)方差，Kk為卡爾曼增益，則卡爾曼濾波方程如式(2)～式(6)所示［3］。其中式(2)～式(3)為預(yù)測(cè)方程;式(4)～式(6)為校正方程。其中矩陣A、B是系統(tǒng)結(jié)構(gòu)確定的矩陣，Q、R分別是系統(tǒng)噪聲和觀測(cè)噪聲的協(xié)方差矩陣，H是測(cè)量系統(tǒng)的參數(shù)

2.2 加速度傳感器和陀螺儀數(shù)據(jù)融合算法

記θk為車體k時(shí)刻的傾角，陀螺儀k時(shí)刻測(cè)得的角速度記為，加速度傳感器k時(shí)刻測(cè)量到的車體傾角為θakccel，為k時(shí)刻角速度是k時(shí)刻角速度誤差，采樣時(shí)間間隔為 d t，則預(yù)測(cè)方程為式(7)～式(8)，更新方程為式(9)～式(11)［4］

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

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

兩輪自平衡小車硬件電路主要由微控制器、測(cè)量模塊、直流電機(jī)驅(qū)動(dòng)模塊、藍(lán)牙模塊等構(gòu)成。其中測(cè)量模塊包括傳感器MPU－6050、光柵編碼器兩部分。MPU－6050用于測(cè)量車體傾角，同時(shí)也完成加速度測(cè)量;安裝在左右兩個(gè)車輪軸上的增量型光柵碼盤經(jīng)光柵編碼器處理后，分別輸出兩路AB相脈沖信號(hào)，經(jīng)單片機(jī)處理后可得到每個(gè)車輪的轉(zhuǎn)速及方向;藍(lán)牙模塊接收手機(jī)或電腦發(fā)出的信號(hào)，控制小車的運(yùn)動(dòng)。

小車的硬件系統(tǒng)結(jié)構(gòu)如圖2所示。選用意法半導(dǎo)體公司的STM32F103C8T6作為控制器，該控制器是一種基于Cortex M3內(nèi)核的高性能MCU，運(yùn)行時(shí)鐘最高為72 MHz，運(yùn)算速度高達(dá) 1.25 DMIPS/MHz，片上64 kB Flash ROM、20 kB RAM，內(nèi)含7個(gè)定時(shí)器，可輸出多路PWM信號(hào)又可配置成增量型編碼器接口［5］，尤其適合于車輪測(cè)速傳感器輸出的AB相脈沖信號(hào)的處理。片上集成的I2C總線模塊，方便了對(duì)傳感器MPU－6050的控制。STM32F103C8T6完成傳感器MPU－6050輸出的加速度信號(hào)和陀螺儀信號(hào)的采集;對(duì)加速度信號(hào)和陀螺儀信號(hào)進(jìn)行數(shù)據(jù)融合，計(jì)算出車體實(shí)時(shí)傾角;處理兩個(gè)車輪上光柵編碼器輸出的兩路AB相信號(hào)，得到小車運(yùn)動(dòng)速度及方向信息;利用數(shù)字PID算法輸出PWM信號(hào)控制電機(jī)驅(qū)動(dòng)模塊，實(shí)現(xiàn)小車姿態(tài)調(diào)整從而實(shí)現(xiàn)小車的自平衡控制。

圖3為單片機(jī)及傳感器接口電路，P1、P2分別接左右車輪的光柵傳感器檢測(cè)電路，檢測(cè)后的AB相脈沖分別從PA0PA1、PA6PA7送入單片機(jī);PA9PA10用以與藍(lán)牙模塊進(jìn)行串行通信;PA5為電池電壓檢測(cè)輸入;PA2PA3、PB0PB1分別為左右車輪的控制信號(hào);PB6、PB7分別是左右車輪的PWM調(diào)速控制信號(hào);PB10PB11與傳感器MPU－6050相連，實(shí)現(xiàn)加速度和傾角的測(cè)量。

圖2 系統(tǒng)硬件結(jié)構(gòu)框圖

圖3 單片機(jī)及傳感器接口電路

圖4為傳感器MPU－6050電路。MPU－6050是世界上首款9軸運(yùn)動(dòng)傳感器，它將3軸MEMS陀螺儀、3軸MEMS加速度計(jì)和一個(gè)可擴(kuò)展的運(yùn)動(dòng)數(shù)字處理器(DPM)集成在一個(gè)芯片上，其內(nèi)部對(duì)陀螺儀和加速度計(jì)分別使用3個(gè)16位ADC，檢測(cè)輸出的16位數(shù)字量通過I2C總線接口輸出，為實(shí)現(xiàn)精確跟蹤快速和慢速運(yùn)動(dòng)，陀螺儀和加速度傳感器的測(cè)量范圍都可設(shè)定［6］。

圖5為直流電機(jī)驅(qū)動(dòng)電路。TB6612FNG是一款新型驅(qū)動(dòng)器件，能獨(dú)立控制2個(gè)直流電機(jī)，具有集成度高、輸出驅(qū)動(dòng)能力強(qiáng)等特點(diǎn)，每通道輸出最高1.2 A的連續(xù)驅(qū)動(dòng)電流，啟動(dòng)峰值電流達(dá)2 A/3.2 A，有正轉(zhuǎn)/反轉(zhuǎn)/制動(dòng)/停止4種電機(jī)控制模式，PWM支持頻率高達(dá)100 kHz［7］，在集成化、小型化的電機(jī)控制系統(tǒng)中，是一種理想的電機(jī)驅(qū)動(dòng)器件。

圖4 MPU－6050電路

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

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

軟件主要由傳感器數(shù)據(jù)采集程序、卡爾曼濾波程序、車速及運(yùn)動(dòng)方向檢測(cè)程序、PID控制程序、直流電機(jī)PWM控制程序、藍(lán)牙串口模塊數(shù)據(jù)處理程序等組成。STM32F103C8T6內(nèi)含PWM模塊及I2C總線接口模塊，因此直流電機(jī)PWM控制和傳感器MPU－6050數(shù)據(jù)采集通過讀寫單片機(jī)內(nèi)部的寄存器就可實(shí)現(xiàn)，將單片機(jī)內(nèi)的定時(shí)器配置成增量型編碼器接口，車速及運(yùn)動(dòng)方向的檢測(cè)也很容易實(shí)現(xiàn)，不需要CPU過多干預(yù)。因此，卡爾曼濾波算法及PID控制算法的實(shí)現(xiàn)是軟件編程的關(guān)鍵。

根據(jù)式(7)～式(11)卡爾曼濾波數(shù)據(jù)融合過程分成以下步驟:(1)設(shè)定數(shù)據(jù)更新頻率。具體編程時(shí)由定時(shí)器定時(shí)產(chǎn)生所需要的時(shí)間間隔d t。(2)設(shè)定參數(shù)。主要是矩陣Q的確定，可取般由實(shí)驗(yàn)確定 q1、q2、q3，式(11)中矩陣 P^k取單位矩陣。(3)計(jì)算更新后的協(xié)方差矩陣。(4)將陀螺儀的更新后的協(xié)方差融入到加速度計(jì)的傾角誤差估算。(5)求加速計(jì)新的協(xié)方差矩陣。(6)計(jì)算傾角誤差、角速度誤差、輸出角速度。

PID控制的原理是根據(jù)系統(tǒng)的被調(diào)量實(shí)測(cè)值與設(shè)定值之間的偏差，利用偏差的比例、積分、微分3個(gè)環(huán)節(jié)的組合計(jì)算出控制量，程序設(shè)計(jì)中采用PD組合控制方式，利用卡爾曼濾波器輸出的車體傾角及角速度計(jì)算電機(jī)PWM信號(hào)的占空比，同時(shí)利用小車位移及小車速度計(jì)算PWM信號(hào)占空比增量值，如式(13)、式(14)所示

4 系統(tǒng)調(diào)試

兩輪自平衡小車系統(tǒng)中變量多且相互耦合，因此調(diào)試過程比較繁瑣，主要包括(1)確定零偏。關(guān)閉電機(jī)驅(qū)動(dòng)，使車體垂直于水平桌面并保持靜止，讀取若干組MPU－6050中各通道的偏移量到PC機(jī)中，求取陀螺儀的零偏值及加速度傳感器z軸最小值。(2)靜止倒立小車確定加速度傳感器z軸最大值，從而可確定加速度傳感器z軸輸出值變化范圍，進(jìn)而確定加速度計(jì)的比例因子。(3)陀螺儀比例因子的確定。確定的目的是使由陀螺儀積分得到的車體傾角值與加速度計(jì)計(jì)算到的值一致。(4)PID控制中按照先比例后微分的步驟確定比例因子和微分因子。

5 結(jié)束語

介紹了一種基于微控制器STM32F103C8T6、采用卡爾曼數(shù)據(jù)融合算法的兩輪自平衡小車的設(shè)計(jì)。實(shí)驗(yàn)測(cè)試表明，文中設(shè)計(jì)的小車在普通水磨石地面上自平衡控制范圍約±15°，在外加一定干擾的情況下小車仍能保持平衡，小車在藍(lán)牙設(shè)備發(fā)出的指令下可實(shí)現(xiàn)前進(jìn)、倒退、轉(zhuǎn)彎、停止等動(dòng)作，控制過程穩(wěn)定、可靠。測(cè)試結(jié)果表明，利用卡爾曼濾波算法能有效消除陀螺儀漂移及車體擺動(dòng)等對(duì)加速度計(jì)的影響，準(zhǔn)確計(jì)算出姿態(tài)傾角，結(jié)合數(shù)字PID控制算法可有效實(shí)現(xiàn)小車的自平衡控制;采用高性能微控制器，使得卡爾曼濾波算法及PID控制算法得以快速完成，小車的狀態(tài)信息采集實(shí)時(shí)性好，提高了自平衡小車穩(wěn)定性和抗干擾能力。

［1］魏文，段晨東，高精隆，等.基于數(shù)據(jù)融合的兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)［J］.電子技術(shù)應(yīng)用，2013，39(5):79 －81.

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

［3］程剛，屈勝利，劉學(xué)超.兩輪自平衡小車可控角度的推導(dǎo)研究［J］.機(jī)床與液壓，2009，37(5):115－116.

［4］Rodrigo da Silva Guerra.Kalman filter tutorial for balancing robot［EB/OL］.(2014 －01 －28)［2014 －04－06］http://wenku.baidu.com/view/7cfe5fef102de2bd960588cb.html.

［5］STMicroelectronics Group of Companies.STM32F103Cx8 datasheet［EB/OL］.(2011 －08 －28)［2014 －04 －06］http://ishare.iask.sina.com.cn/f/17966465.html.

［6］InvenSense Inc.MPU －6050 datasheet［EB/OL］.(2013 －08 －31)［2014 －04 －21］http://ishare.iask.sina.com.cn/f/36111609.html.

［7］Toshiba Corporation.TB6612FNG motor driver datasheet［M］.Tokyo:Toshiba Corporation，2011.