詹長書 邢博坤 樸光宇 巨生龍 蘇德新 馬璐

摘要：本文在總結(jié)和歸納國內(nèi)外兩輪自平衡小車的研究現(xiàn)狀后，在使用現(xiàn)有小車的情況下，選用適當(dāng)?shù)目刂破?、?zhí)行電機(jī)和傳感器，設(shè)計(jì)出兩輪自平衡小車的驅(qū)動電路，實(shí)現(xiàn)了兩輪小車的硬件控制系統(tǒng)。其工作原理是系統(tǒng)以姿態(tài)傳感器（陀螺儀、加速度計(jì)）來監(jiān)測車身所處的俯仰狀態(tài)和狀態(tài)變化率，通過高速中央處理器計(jì)算出適當(dāng)數(shù)據(jù)和指令后，驅(qū)動電動機(jī)產(chǎn)生前進(jìn)或后退的加速度來達(dá)到車體前后平衡的效果。本次實(shí)驗(yàn)將陀螺儀與加速度傳感器集成在一張板上，對傳感器兩者所采集的數(shù)據(jù)進(jìn)行互補(bǔ)濾波的優(yōu)化處理，補(bǔ)償陀螺儀的漂移誤差和加速度計(jì)的動態(tài)誤差，得到一個更優(yōu)的傾角近似值，之后使用PID控制器來進(jìn)行系統(tǒng)設(shè)計(jì)，最終實(shí)現(xiàn)小車的直立。

關(guān)鍵詞：自平衡小車；互補(bǔ)濾波；積分誤差；陀螺儀

中圖分類號：S776.3

文獻(xiàn)標(biāo)識碼：A

文章編號：1001-O05X（2015）04-0073-04

兩輪自平衡小車體積小、結(jié)構(gòu)簡單、運(yùn)動靈活，適用于狹小和危險(xiǎn)的工作空間，擁有廣泛的應(yīng)用前景。兩輪自平衡小車具有多變量、非線性、強(qiáng)耦合、時(shí)變、參數(shù)不確定性等特性。近年來，國內(nèi)外多家科研單位和企業(yè)以及自平衡代步車的愛好者都對自平衡理論的發(fā)展做出了許多有益的貢獻(xiàn)，大大推進(jìn)了自平衡技術(shù)的發(fā)展，同時(shí)也出現(xiàn)了一批有代表性的自平衡機(jī)器人和載人代步車作品。國外的機(jī)器人起步較早，所研制的樣機(jī)比較多，理論也比較完善。和國外比，國內(nèi)的產(chǎn)品尚處于理論研究階段。小車硬件采用MK60單片機(jī)作為主控芯片，陀螺儀和加速度傳感器檢測車身重力方向的傾斜角度和車身輪軸方向上的旋轉(zhuǎn)加速度。由于兩輪自平衡小車存在陀螺儀存在溫漂和積分誤差，加速度傳感器動態(tài)響應(yīng)較慢，而且直立容易受干擾等問題。針對積分誤差的問題，本文將著重使用互補(bǔ)濾波算法將陀螺儀和加速度傳感器數(shù)據(jù)融合，得到一個優(yōu)化的角度近似值。采用雙環(huán)PID控制策略，控制電機(jī)調(diào)整小車狀態(tài)，使小車保持動平衡。

1 系統(tǒng)方案設(shè)計(jì)及改進(jìn)

1.1 主控芯片選擇

采用MK60單片機(jī)作為主控芯片。相對其他單片機(jī)，MK60外設(shè)豐富，主頻高，價(jià)格便宜，有專門的軟件庫，操作簡單，調(diào)試方便，低功耗。基本型時(shí)鐘頻率為36MHz，增強(qiáng)型系列時(shí)鐘頻率達(dá)到72MHz，是同類產(chǎn)品中性能最高的產(chǎn)品。考慮系統(tǒng)的復(fù)雜度，主控芯片需要與傳感器進(jìn)行IIc通訊，輸出靈活可控制的信號，以及進(jìn)行大量的數(shù)學(xué)運(yùn)算。從性能和價(jià)格綜合考慮，即用MK60作為本系統(tǒng)的主控芯片，具體型號為MK60DN512。

1.2 姿態(tài)檢測方案選擇

姿態(tài)檢測采用加速度傳感器與陀螺儀傳感器。通過融合算法，提取出加速度傳感器的靜態(tài)效果和陀螺儀的動態(tài)效果。融合算法比較復(fù)雜，優(yōu)點(diǎn)是能測出準(zhǔn)確穩(wěn)定的傾角，但是由于準(zhǔn)確穩(wěn)定的傾角正是本文要討論的話題，因此最終選擇此方案，即加速度傳感器與陀螺儀傳感器數(shù)據(jù)融合測量傾角。并為了簡化電路，最終選擇了均為nc接口的陀螺儀與加速器一體的ENC -03RC +MMA7361板。

1.3 電機(jī)選擇

直流有刷電機(jī)具有機(jī)械特性硬，響應(yīng)速度快，調(diào)速范圍寬的特點(diǎn)，滿足兩輪自平衡小車對靈敏性、快速性等要求。雖然電機(jī)的電刷會使電機(jī)的壽命縮短，還會引發(fā)電磁干擾。但是由于本設(shè)計(jì)負(fù)載較輕，換向器和電刷的損耗較低。小車布置采用多層機(jī)械結(jié)構(gòu)，電機(jī)驅(qū)動電路與其他電路分離，有效降低電磁干擾。因此，使用兩個6V帶有減速齒輪的直流有刷電機(jī)驅(qū)動兩輪自平衡小車。

2 主要芯片與驅(qū)動電路

2.1 加速度傳感器的選擇

加速度傳感器MMA7361檢測的是它受到的慣性力。加速度傳感器實(shí)際上是用MEMS技術(shù)檢測慣性力造成的微小形變。所以把加速度傳感器水平靜止放在桌子上，它的Z軸輸出的是1g的加速度，因?yàn)閆軸方向被重力向下拉出了一個形變。從剛才的分析可以發(fā)現(xiàn)重力在不產(chǎn)生加速度的情況下對加速度傳感器造成形變，在產(chǎn)生加速度的時(shí)候不造成形變，而其他力都做不到。

所以，當(dāng)系統(tǒng)在三維空間做變速運(yùn)動時(shí)，它的輸出就不正確了或者說它的輸出不能表明物體的姿態(tài)和運(yùn)動狀態(tài)。所以說，只靠加速度傳感器來估計(jì)小車的姿態(tài)是不可取的。

2.2 陀螺儀傳感器ENC-03RC

陀螺儀ENC-03RC可以測量角速度，具有高動態(tài)特性。但是它是一個間接測量器件，它測量的是角度的導(dǎo)數(shù)，角速度，顯然我們要將角速度對時(shí)間積分才能得到角度。

因此綜合考慮，加速度計(jì)是極易受外部干擾的傳感器，但是測量值隨時(shí)間的變化相對較小。陀螺儀可以積分得到角度關(guān)系，動態(tài)性能好，受外部干擾小，但測量值隨時(shí)間變化比較大。它們優(yōu)缺點(diǎn)互補(bǔ)，結(jié)合起來才能準(zhǔn)確地測量小車傾角。

2.3 電機(jī)驅(qū)動電路BTN 7971b

本設(shè)計(jì)中使用減速直流有刷電機(jī)作為兩輪自平衡車的驅(qū)動電機(jī)，電機(jī)采用H橋驅(qū)動方式，使用脈寬調(diào)制方式調(diào)節(jié)電機(jī)兩端電壓有效值，達(dá)到調(diào)速的目的。

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

3.1 互補(bǔ)濾波器的設(shè)計(jì)

由于加速度計(jì)對小車的加速度比較敏感，取瞬時(shí)值計(jì)算傾角誤差比較大；而陀螺儀積分得到的角度不受小車加速度的影響，但是隨著時(shí)間的增加積分漂移和溫度漂移帶來的誤差比較大。所以這兩個傳感器正好可以彌補(bǔ)相互的缺點(diǎn)?；パa(bǔ)濾波就是在短時(shí)間內(nèi)采用陀螺儀得到的角度作為最優(yōu)，定時(shí)對加速度采樣來的角度進(jìn)行取平均值來校正陀螺儀得到的角度，由圖可看出該濾波器的輸入有兩個數(shù)據(jù)，一是加速度計(jì)測得的角度，二是陀螺儀測得的角速度，兩者經(jīng)過一定的融合運(yùn)算后得出一個穩(wěn)定可靠的傾角。以下將介紹數(shù)據(jù)的融合過程。首先建立一個以主控板為基準(zhǔn)的空間坐標(biāo)系，如圖1所示。

R代表電路板的加速度矢量，Rn，（n可以是x，y，z）代表R在各個軸的分量，Ann，（A代表角度an：gle，nn，可以是xz，yz，如Axz）代表R與各個平面的夾角。因此，從加速度計(jì)測出來的RxAcc，Ry-Acc，RzAcc（ac代表accelerometer）便可合成矢量Racc，即Racc=[Raccx，Raccy，Raccz]。由Racc的分量即可計(jì)算出Axz，Ayz，但如前文討論的，此時(shí)的Axz，Ayz并不穩(wěn)定可靠。

假設(shè)最后計(jì)算出的穩(wěn)定可靠的矢量（實(shí)際上是單純的重力加速度矢量）為Rest=[RxEst，RyEst，RzEst]（Est代表：estimate）。程序中，第一次計(jì)算Rest直接取值于加速度計(jì)的Racc，即：

Rest（0） =Racc（0）。

此后的Rest（如Rest（1），Rest（2），Rest（3）…Rest（n））則由陀螺儀參與調(diào)整。在計(jì)算Rest（n）時(shí)，可以通過RxEst（n-1）和RzEst（n-1）計(jì)算出Axz（n-1），即：

Axz（n-1）= atan2（RxEst（n-1），RzEst（n-1））。

接著由Axz（n-1）和RateAxz（n）可計(jì)算出Axz（n），即：

Axz（n）=Axz（n-1）+RateAxz（n）×T，

其中RateAxz（n）為繞y軸的陀螺儀角速度，T為計(jì)算Axz（n-1）到計(jì)算Axz（n）的時(shí)間差值。同樣道理可得：

Ayz（n）=Ayz（n-1）+RateAyz（n）×T。

再由以下公式可得：

同理得：

到此，計(jì)算出了兩個單位為1的向量：Racc（n），Rgyro（n），通過一個加權(quán)平均公式便可得到Rest（n），即：

Rest（n）=（RaccXwl+RgyroXw2）/（wl+ w2）。

令w2/wl=wGyro，可得

Rest（n）=（Racc +Rgyro XwGyro）/（1+wGyro）。

即：

RxEst（n）=（RxAcc+ RxGyro×wGyro）/

（1+WGyro）。

RyEst（n）=（RyAcc+ RyGyro×wGyro）/

（1+wGyro）。

RzEst（n）=（RzAcc+ RzGyro×wGyro）/

（1+wGyro）。

而小車的傾角為：

RzEst（n）=（RzAcc+ RzGyro×+wGyro）/

（1+wGyro）。

3.2

PID控制器的設(shè)計(jì)

PID控制器的輸入輸出關(guān)系為：

PID控制的模擬和數(shù)宇控制：模擬電子電路調(diào)節(jié)器，調(diào)節(jié)器，所測量的信號與給定值進(jìn)行比較，然后將致動器之后比較PID電路動作之間的差來改變供給量，以達(dá)到調(diào)整的目。PID數(shù)字由計(jì)算機(jī)操作時(shí)，其結(jié)果將被轉(zhuǎn)換成一個模擬輸出，以控制致動器。

比例控制系統(tǒng)的動態(tài)性能影響：當(dāng)增加Kp時(shí)，會使系統(tǒng)的響應(yīng)速度增加，當(dāng)Kp過大，會使系統(tǒng)的振蕩頻率調(diào)整時(shí)間變長；當(dāng)Kp為過小，則系統(tǒng)響應(yīng)緩慢。確定Kp的選擇，以產(chǎn)生輸出響應(yīng)為4：1衰減處理是適當(dāng)?shù)?。在系統(tǒng)穩(wěn)定，增加的Kp可以減少穩(wěn)態(tài)誤差，但它不能消除穩(wěn)態(tài)誤差。

積分時(shí)間T_i影響系統(tǒng)性能：積分控制通常會影響系統(tǒng)的穩(wěn)定性。T_i太小，使系統(tǒng)不穩(wěn)定和沖擊更頻繁；T_i太大，對系統(tǒng)的影響將減弱；T在適當(dāng)?shù)臅r(shí)候，系統(tǒng)更加理性的過程的過度性質(zhì)。積分控制系統(tǒng)有助于消除穩(wěn)態(tài)誤差。

微分時(shí)間T_D對系統(tǒng)性能的影響：增強(qiáng)的微分作用可以改善動態(tài)特性，如減少超調(diào)，縮短調(diào)整時(shí)間，適當(dāng)增加比例控制，它可以減少穩(wěn)態(tài)誤差，提高控制精度，另一方面微分動作將放大系統(tǒng)噪聲和降低系統(tǒng)抵制干擾?？梢詫?shí)現(xiàn)鑒別程序錯誤，或在瞬間，通過檢驗(yàn)偏移控制的偏差，得出盡早糾正的措施，有助于提高系統(tǒng)的穩(wěn)定性。

數(shù)字PID算法為用計(jì)算機(jī)實(shí)現(xiàn)，用數(shù)值逼近和連續(xù)信號離散化實(shí)現(xiàn)的PID控制規(guī)律，有兩種實(shí)現(xiàn)方式：位置性數(shù)字PID、增量型數(shù)字PID。

對于位置式PID算法，由于公式 位置性數(shù)字PID控制算法可以由公式（1）得到。

結(jié)合公式（1）和公式（2），可以得到增量式數(shù)字PID控制算法公式（3）：

3.3

PID測試結(jié)果

在實(shí)現(xiàn)小車的動態(tài)平衡的過程中，互補(bǔ)濾波器輸出值將繼續(xù)根據(jù)身體姿勢偏轉(zhuǎn)調(diào)節(jié)數(shù)字PID輸出值，調(diào)整PWM占空比，從而控制電機(jī)的轉(zhuǎn)向和速度，以控制適當(dāng)?shù)木獾哪康?，系統(tǒng)控制過程如圖2所示。

在兩輪自平衡車的功能中，最重要的是保證車體的動態(tài)平衡。通過對PID控制器的調(diào)試，可以以實(shí)驗(yàn)的方式確定控制系統(tǒng)的性能是否穩(wěn)定以及算法是否有效，平衡測試曲線如圖3所示。

從圖3兩輪自平衡車平衡曲線中可以看出，從開始時(shí)的20。傾角的平衡，迅速由趨于平衡到振動，然后汽車的傾斜不繼續(xù)廣泛震蕩，證明了PID控制器的有效性。要驗(yàn)證的兩輪自平衡車輛的魯棒性來控制外部影響的能力的作用下，PID控制器，外力施加到小車，其響應(yīng)干擾圖的姿態(tài)如圖4所示。自平衡小車在受到外力施加后，3s內(nèi)立即向初始平衡點(diǎn)響應(yīng)，保持動態(tài)平衡，說明PID控制器能滿足設(shè)計(jì)要求，防干擾系統(tǒng)的性能得到了檢驗(yàn)。圖4中的數(shù)據(jù)可以看到，在當(dāng)兩輪自平衡小車被外力干擾時(shí)，陀螺儀將在第一時(shí)間來改變汽車的角度，根據(jù)陀螺儀反饋到主控制器檢測到的信號，兩輪自平衡車輛通過角度變化來調(diào)整車體回到平衡。

4 結(jié)論

首先完成了兩輪自平衡小車硬件電路的設(shè)計(jì)，通過實(shí)驗(yàn)驗(yàn)證了硬件電路基本滿足系統(tǒng)的要求。其次設(shè)計(jì)了互補(bǔ)濾波器，通過對加速度傳感器和陀螺儀傳感器數(shù)據(jù)的融合處理，得到一個小車傾角的最優(yōu)近似解，便于控制器處理。最后設(shè)計(jì)了雙環(huán)PID控制器，通過實(shí)驗(yàn)的方式驗(yàn)證了算法基本滿足設(shè)計(jì)要求，完成小車的自平衡控制。