兩輪自平衡智能車(chē)系統設計

摘要：針對智能汽車(chē)競賽中的電磁組參賽要求，提出了兩輪自平衡智能車(chē)系統的設計方案。主控芯片采用飛思卡爾公司的MC9S12XS128，選用加速度傳感器檢測車(chē)模的傾角，陀螺儀檢測車(chē)模的角加速度；通過(guò)控制兩個(gè)電機的加減速實(shí)現車(chē)模的自平衡控制。闡述了卡爾曼濾波法在陀螺儀和加速度傳感器信號融合方面的應用，提出了針對閉環(huán)速度控制的PI算法。實(shí)驗表明：該處理方法實(shí)現簡(jiǎn)單，能夠準確、快速地實(shí)現車(chē)模的自平衡控制。
關(guān)鍵詞：自平衡；智能車(chē)；卡爾曼濾波；PI算法

引言
兩輪自平衡智能車(chē)控制技術(shù)在諸如航空、航海、安保等許多領(lǐng)域有著(zhù)廣泛的應用，各種類(lèi)型的傾角傳感器和數據處理方法應運而生。隨著(zhù)現代科技的發(fā)展，對于自平衡的響應速度和精確度提出了更高的要求，以此為背景，第七屆全國大學(xué)生智能車(chē)競賽電磁組參賽要求設計的智能車(chē)為兩輪直立行走的自平衡循跡智能車(chē)。
針對該問(wèn)題，本文選用MMA7260加速度傳感器和NEC-03陀螺儀共同檢測車(chē)模的角度信息，通過(guò)卡爾曼濾波器得到車(chē)模準確的實(shí)時(shí)角度信息，以在主控芯片MC9S12XS128中實(shí)現的PI控制算法和兩個(gè)直流電機為智能車(chē)的控制核心，實(shí)現了兩輪智能車(chē)的自平衡控制。該系統的優(yōu)點(diǎn)是適應性強、響應迅速并有很強的抗干擾能力。

1 設計原理
控制直立車(chē)模平衡的直觀(guān)經(jīng)驗來(lái)自于人們日常生活經(jīng)驗。一般人通過(guò)簡(jiǎn)單練習就可以讓一個(gè)直木棒在手指尖上保持直立。這需要兩個(gè)條件：一個(gè)是托著(zhù)木棒的手掌可以移動(dòng)；另一個(gè)是眼睛可以觀(guān)察到木棒的傾斜角度和傾斜趨勢(角度和角速度)。通過(guò)手掌移動(dòng)抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。這兩個(gè)條件缺一不可，實(shí)際上就是控制中的負反饋機制。
車(chē)模的平衡控制也是通過(guò)負反饋來(lái)實(shí)現的，與上面保持木棒直立比較相對簡(jiǎn)單。因為車(chē)模有兩個(gè)輪子著(zhù)地，車(chē)體只會(huì )在輪子滾動(dòng)的方向上發(fā)生傾斜?？刂戚喿愚D動(dòng)，抵消在一個(gè)維度上傾斜的趨勢便可以保持車(chē)體平衡了，如圖1所示。

由圖1可知，為了保持車(chē)模的直立自平衡狀態(tài)，需要滿(mǎn)足以下兩個(gè)條件：
①能夠準確測量車(chē)模的傾角和角加速度的大小，以得到車(chē)模的狀態(tài)和趨勢；
②可以控制車(chē)輪的速度和加速度，使車(chē)模保持直立狀態(tài)。

2 自平衡智能車(chē)系統結構
自平衡智能車(chē)系統主要包括主控模塊、角度信息采集模塊、電機驅動(dòng)模塊和電源模塊等，自平衡智能車(chē)系統結構框圖如圖2所示。

主控模塊選用Freescale公司16位主控芯片Mc9S12XS128具有高精度高采樣頻率的內部A／D轉換器，同時(shí)具有內部定時(shí)器模塊和輸入捕捉功能，能夠很好地實(shí)現對車(chē)模電機的測速；對車(chē)模角度信息的處理，主控芯片的時(shí)鐘頻率最高可達80 MHz，可以迅速響應車(chē)模角度的變化，控制車(chē)模的自平衡。

3 自平衡智能車(chē)硬件設計
控制車(chē)模自平衡所需的位置信息(即角度和角速度信息)，可以通過(guò)加速度傳感器和陀螺儀獲得；由于主控模塊I／O口驅動(dòng)能力有限，智能車(chē)需要使用全橋驅動(dòng)來(lái)驅動(dòng)電機；為了實(shí)現對車(chē)速精準的閉環(huán)控制，需要測速模塊。
3．1 加速度傳感器
加速度傳感器可以測量由地球引力作用或者物體運動(dòng)所產(chǎn)生的加速度。自平衡智能車(chē)系統所采用的是Freescale系列的加速度傳感器MMA7260。該系列的傳感器采用了半導體表面微機械加工和集成電路技術(shù)，體積小、重量輕。MMA7260是一款三軸低g半導體加速度計，可以同時(shí)輸出3個(gè)方向上的加速度模擬信號，同時(shí)電路實(shí)現簡(jiǎn)單，角度輸出精度高。
3．2 陀螺儀
自平衡智能車(chē)系統的角速度傳感器采用的是陀螺儀，陀螺儀可以用來(lái)測量物體的旋轉角速度。根據精度需要選用了村田公司出品的ENC-03系列的加速度傳感器。它利用了旋轉坐標系中的物體會(huì )受到科里奧利力的原理，在器件中利用壓電陶瓷做成振動(dòng)單元。當旋轉器件時(shí)會(huì )改變振動(dòng)頻率從而反映出物體旋轉的角速度。
3．3 電機驅動(dòng)
自平衡智能車(chē)系統選用了電機驅動(dòng)芯片BTS7960作為電機驅動(dòng)。該芯片在工作時(shí)，阻抗典型值為16 mΩ(IOUT=9 A，Ti=25℃)，可提供的最大驅動(dòng)電流為43 A。當芯片過(guò)熱時(shí)可自動(dòng)關(guān)閉或鎖定；在過(guò)電流的情況下，開(kāi)關(guān)模式可限制電流，可降低功耗；欠壓時(shí)會(huì )自動(dòng)關(guān)閉，過(guò)壓時(shí)會(huì )鎖定。驅動(dòng)電路圖如圖3所示。

3．4 測速模塊
本系統采用固定在電機輸出軸上的光碼盤(pán)以及相互配合的光電對管器件實(shí)現對車(chē)輪的測速，如圖4所示。

由于智能車(chē)所采用的主控芯片MC9S12XS128只有一個(gè)脈沖累加器端口，故需要采用數據選擇器控制其在同一時(shí)間只測量一個(gè)電機的速度而在下一時(shí)刻測量另一輪子的速度。同時(shí)，程序根據選通信號和測得的速度分別計算兩輪的速度，以便給出精準的控制信號，使車(chē)模保持良好的直立狀態(tài)，以及完成加減速和轉向。

4 車(chē)模信息采集與處理
4．1 位置信息的處理
自平衡智能車(chē)位置信息主要是通過(guò)加速度傳感器和陀螺儀獲得的。加速度傳感器MMA7260是一款三軸低g半導體加速度計，可以同時(shí)輸出3個(gè)方向上的加速度模擬信號。通過(guò)軟硬件設置可以使得MMA7260各軸信號最大輸出靈敏度為800 mV／g，這個(gè)信號無(wú)需再進(jìn)行放大，可以直接送到單片機進(jìn)行A／D轉換。
實(shí)際應用中，只需要測量其中一個(gè)方向上的加速度值，就可以計算出車(chē)模傾角，比如使用Z軸方向上的加速度信號。車(chē)模直立時(shí)，固定加速度器在Z軸水平方向，此時(shí)輸出信號為零偏電壓信號。當車(chē)模發(fā)生傾斜時(shí)，重力加速度g便會(huì )在Z軸方向形成加速度分量，從而引起該軸輸出電壓變化。變化的規律為：
△u=k·g·sinθ≈k·g·θ (1)
式中，g為重力加速度，θ為車(chē)模傾角，k為加速度傳感器靈敏度系數。當傾角θ比較小的時(shí)候，輸出電壓的變化可以近似與傾角成正比。
理論上只需要加速度傳感器就可以獲得車(chē)模的傾角，再對此信號進(jìn)行微分便可以獲得傾角速度。但在實(shí)際車(chē)模運行過(guò)程中，由于車(chē)模本身的擺動(dòng)所產(chǎn)生的加速度會(huì )產(chǎn)生很大的干擾信號，它疊加在上述測量信號上，使得輸出信號無(wú)法準確反映車(chē)模的傾角。
下面分析一下運動(dòng)所產(chǎn)生的干擾信號。加速度傳感器安裝在車(chē)模上，距離車(chē)軸高度為h。車(chē)模轉動(dòng)具有角加速度α，運動(dòng)加速度a。那么，在加速度傳感器Z軸上出現由于車(chē)模運動(dòng)引起的加速度為h·α+a，如圖5所示。為了減小運動(dòng)引起的干擾，加速度傳感器安裝的高度越低越好，但是無(wú)法徹底消除車(chē)模運動(dòng)的影響。

車(chē)模運動(dòng)產(chǎn)生的加速度使得輸出電壓在實(shí)際傾角電壓附近波動(dòng)，這些波動(dòng)噪聲可以通過(guò)數據平滑濾波將其濾除。但是，平滑濾波一方面會(huì )使得信號無(wú)法實(shí)時(shí)反映車(chē)模傾角變化，從而減緩對于車(chē)模車(chē)輪的控制，另一方面也會(huì )將車(chē)模角速度變化信息濾掉。上述兩方面的濾波效果使得車(chē)模無(wú)法保持平衡，因此，對于車(chē)模直立控制所需要的傾角信息需要通過(guò)另外一種器件獲得，那就是角速度傳感器——陀螺儀。
由于陀螺儀輸出的是車(chē)模的角速度，將角速度信號進(jìn)行積分便可以得到車(chē)模的傾角，且不會(huì )受到車(chē)體運動(dòng)的影響，因此，該信號中噪聲很小。此外，車(chē)模的角度又是通過(guò)對角速度積分得到，可進(jìn)一步平滑信號，從而使得角度信號更加穩定。因此，車(chē)?？刂扑枰慕嵌群徒撬俣瓤梢允褂猛勇輧x所得到的信號。由于這個(gè)角度信號是通過(guò)積分獲得的，如果角速度信號存在微小的偏差和漂移，經(jīng)過(guò)積分運算之后，變化會(huì )形成積累誤差。這個(gè)誤差會(huì )隨著(zhù)時(shí)間延長(cháng)逐步增加，最終導致電路飽和，無(wú)法形成正確的角度信號。
為了消除這個(gè)累積誤差，所采用的方法是通過(guò)上面的加速度傳感器獲得的角度信息對此進(jìn)行校正，通過(guò)對比積分所得到的角度與重力加速度所得到的角度，使用它們之間的偏差來(lái)改變陀螺儀的輸出，從而使積分的角度逐步跟蹤到加速度傳感器所得到的角度。
利用加速度計所獲得的角度信息θg與陀螺儀積分后的角度θ進(jìn)行比較，將比較的誤差信號經(jīng)過(guò)1／Tg比例放大，再與陀螺儀輸出的角速度信號疊加之后進(jìn)行積分。對于加速度計給定的角度θg，經(jīng)過(guò)比例、積分環(huán)節之后產(chǎn)生的角度θ必然最終等于θg。
為了避免輸出角度θ跟蹤時(shí)間過(guò)長(cháng)，可以采取以下兩個(gè)方面的措施：
①仔細調整陀螺儀的放大電路，使得它的零點(diǎn)偏置盡量接近于設定值，并且穩定。
②在控制電路和程序運行的開(kāi)始，盡量保持車(chē)模處于直立狀態(tài)，這樣一開(kāi)始就能使輸出角度θ與θg相等，此后，加速度計的輸出只是消除積分的偏移，輸出角度不會(huì )出現太大的偏差。

卡爾曼濾波模型如圖6所示，以陀螺儀測量的角速度作為預測值的控制量，加速度傳感器測量的角度作為觀(guān)測值。公式如下，Z(k)為k時(shí)刻加速度積分得到的角度，U(k)為k時(shí)刻的角速度，X(k|k)為k時(shí)刻經(jīng)卡爾曼濾波后的角度，X(k|k-1)為系統利用k-1時(shí)刻的狀態(tài)得到的k時(shí)刻的預測值，Kg(k)為k時(shí)刻的卡爾曼增益。

由于自平衡智能車(chē)系統為單輸入單輸出系統，因此令A、B、H、K不為矩陣，且值都為1。自平衡智能車(chē)系統卡爾曼濾波參數有3個(gè)，P是卡爾曼系統參數A的協(xié)方差，P的初值P(0)的設定需要根據系統效果來(lái)調試確定，初值一般在1附近。Q是預測值Z(k)的協(xié)方差，R是測量值U(k)的協(xié)方差。對Q和R的設定原則是，Q／(Q+R)的值就是卡爾曼增益的收斂值。例如其值為0．2，那么卡爾曼濾波后的增益會(huì )向0．2收斂，即如果預測角度值是5°，角度測量值是10°，那么最優(yōu)化角度為：5+0．2*(10-5)=6°。因此，卡爾曼增益越小，說(shuō)明預測值越可靠，最優(yōu)化角度越接近預測值；相反，卡爾曼增益越大，說(shuō)明測量值越可靠，最優(yōu)化角度越接近測量值。P／(Q+R)反映收斂的快慢程度，該值設定越小，收斂越快，該值越大，收斂越慢。由于卡爾曼增益收斂總的來(lái)說(shuō)是很快的，因此該值的設置影響較小。

卡爾曼濾波后的波形圖7所示，包括加速度計輸出的信號、陀螺儀輸出的信號和卡爾曼濾波后的波形。實(shí)驗表明，卡爾曼濾波可以很好地還原信號，并可以克服加速度計及陀螺儀的溫漂，能夠準確地提供車(chē)模的角度信息。
4．2 速度信息的處理
本系統的處理方法是根據光電碼盤(pán)的脈沖信息，通過(guò)單片機的輸入捕捉功能實(shí)現測速。速度閉環(huán)控制方面，采用的則是增量型的PID控制算法。所需公式為：

Kp——起比例調節作用，系統一旦出現了偏差，比例調節立即產(chǎn)生調節作用以減少偏差。
Ki——使系統消除穩態(tài)誤差，提高誤差度，積分調節的引入會(huì )使系統穩定性下降，動(dòng)態(tài)響應變慢。
Kd——反映了系統偏差信號的變化速率，可以預見(jiàn)偏差的變化趨勢，能提前對偏差進(jìn)行控制。
針對車(chē)模的電機，結合實(shí)際的調試經(jīng)驗，本車(chē)模的電機調速采用的方案是PI控制，將對目標速度和當前速度再次進(jìn)行PI運算，得到電機控制信息并交給外部設備控制模塊處理，以達到控制速度的目的。
4．3 軟件設計
系統中主要利用主控芯片的A／D轉換模塊讀取陀螺儀和加速度傳感器的值，從而通過(guò)卡爾曼濾波得到車(chē)模的角度信息。由于控制目標是保持車(chē)模直立，因此，以直立時(shí)車(chē)模的角度為控制目標，以所測量的車(chē)模角度信息為當前反饋，通過(guò)PID控制算法，計算出需要給出的電機速度，通過(guò)電機閉環(huán)控制使車(chē)模產(chǎn)生相應的運動(dòng)狀態(tài)以保持車(chē)模的直立平衡狀態(tài)。車(chē)模的系統流程圖如圖8所示。

結語(yǔ)
實(shí)驗結果表明，本系統方法簡(jiǎn)單實(shí)用，可以很好地實(shí)現預期的功能，即實(shí)現小車(chē)的動(dòng)態(tài)自平衡，采用該自平衡方法的磁導航智能車(chē)在第七屆“飛思卡爾”杯全國大學(xué)生智能車(chē)比賽中取得了非常優(yōu)異的成績(jì)。該方法卡爾曼濾波的應用上，公式(2)～(6)可以進(jìn)一步簡(jiǎn)化，同時(shí)也可以通過(guò)優(yōu)化各個(gè)參數更好地實(shí)現車(chē)模自平衡，進(jìn)一步提高車(chē)模的穩定性。