兩輪自平衡小車的控制策略設計

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

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

關鍵詞：自平衡小車；互補濾波；積分誤差；陀螺儀

中圖分類號：S776.3

文獻標識碼：A

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

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

1 系統(tǒng)方案設計及改進

1.1 主控芯片選擇

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

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

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

1.3 電機選擇

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

2 主要芯片與驅動電路

2.1 加速度傳感器的選擇

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

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

2.2 陀螺儀傳感器ENC-03RC

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

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

2.3 電機驅動電路BTN 7971b

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

3 系統(tǒng)軟件設計

3.1 互補濾波器的設計

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

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

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

Rest（0） =Racc（0）。

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

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

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

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

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

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

再由以下公式可得：

同理得：

到此，計算出了兩個單位為1的向量：Racc（n），Rgyro（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控制器的設計

PID控制器的輸入輸出關系為：

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

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

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

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

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

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

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

3.3

PID測試結果

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

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

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

4 結論

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