兩輪自平衡智能車(chē)系統(tǒng)設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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