基于STM32的兩輪自平衡小車控制系統(tǒng)設(shè)計

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

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

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

1 動力學模型

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

圖1 小車受力分析方程

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

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

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

2.1 卡爾曼濾波器一般形式

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

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

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

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

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

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

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

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

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

圖3 單片機及傳感器接口電路

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

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

圖4 MPU－6050電路

圖5 電機驅(qū)動電路

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

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

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

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

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

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

5 結(jié)束語

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

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

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

［3］程剛，屈勝利，劉學超.兩輪自平衡小車可控角度的推導研究［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.