(華北水利水電學(xué)院機(jī)械學(xué)院，河南 鄭州 450011)

0 引言

根據(jù)第七屆全國大學(xué)生“飛思卡爾”杯智能汽車競賽要求，電磁組比賽所用車模仿照兩輪自平衡車模設(shè)計(jì)，采用雙后輪驅(qū)動，車模在直立的狀態(tài)下沿著賽道行走。直立行走智能車是一個(gè)高階次、強(qiáng)耦合、多變量的非線性系統(tǒng)，它是直線一級倒立擺和環(huán)形一級倒立擺的綜合體。通過分解系統(tǒng)，簡化系統(tǒng)，將系統(tǒng)分為直立、行走、轉(zhuǎn)向三方面控制，三者相互疊加、相互影響，最終控制動力均來自電機(jī)驅(qū)動力。

1 系統(tǒng)整體結(jié)構(gòu)設(shè)計(jì)

智能小車在直立的狀態(tài)下以兩個(gè)后輪著地沿著賽道行進(jìn)，相比四輪著地狀態(tài)，車?？刂迫蝿?wù)更為復(fù)雜。系統(tǒng)硬件部分以MC9S12XS128[1-4]為主控制器，軟件采用卡爾曼濾波和PID控制算法[5-6]進(jìn)行數(shù)據(jù)處理，以實(shí)現(xiàn)最佳控制效果。

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

直立智能小車硬件采用模塊化設(shè)計(jì)，主要由全橋電機(jī)驅(qū)動模塊、傾角測量模塊、光電編碼器、賽道信息檢測模塊、無線通信模塊等構(gòu)成。整個(gè)系統(tǒng)構(gòu)成一個(gè)閉環(huán)控制系統(tǒng)，其原理如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)原理框圖

傾角測量模塊由ENC-03B傾角陀螺儀和MMA7260加速度計(jì)組成，賽道信息檢測模塊由水平陀螺儀和電感線圈組成。MC9S12XS128控制器通過傾角檢測模塊獲得車模傾角信息，通過水平陀螺儀和電感線圈獲得賽道信息，通過光電編碼器測得車速信息，完成原始的數(shù)據(jù)采集。

控制器對采集到的信息進(jìn)行處理，求出小車運(yùn)動的控制參量。同時(shí)通過無線通信模塊將數(shù)據(jù)返回上位機(jī)，通過上位機(jī)直觀顯示小車的各種信息，并實(shí)時(shí)調(diào)整參數(shù)，提高小車的速度和穩(wěn)定性，逐步提高整車性能。

1.2 系統(tǒng)軟件構(gòu)架

系統(tǒng)軟件采用C語言編程[7]，主要包括初始化、參數(shù)設(shè)定、數(shù)據(jù)處理(卡爾曼濾波)、直立控制、方向控制、速度控制等模塊。系統(tǒng)軟件流程圖如圖2所示。

圖2 系統(tǒng)軟件流程圖

主控制器通過片內(nèi)A/D轉(zhuǎn)換將各傳感器采集到的信息轉(zhuǎn)化為數(shù)字量，然后對數(shù)據(jù)進(jìn)行處理：①通過卡爾曼濾波對傾角測量數(shù)據(jù)進(jìn)行信息融合和補(bǔ)償，得到準(zhǔn)確的角度信息；②對實(shí)時(shí)車速信息和賽道信息進(jìn)行處理，采用PID算法，校正控制參量，控制電機(jī)運(yùn)動，實(shí)現(xiàn)直立、速度和方向控制[8]。

2 信息采集和處理

2.1 角度信息的采集及處理

系統(tǒng)采用MMA7260加速度計(jì)測量小車的重力加速度，其輸出值為重力在車體方向的重力加速度，通過反正弦運(yùn)算可獲得車體傾角。小車車體在一個(gè)很小的傾角范圍內(nèi)變化，可以將這種反正弦關(guān)系近似線性化。但是在實(shí)際小車行走時(shí)，車體本身擺動所產(chǎn)生的加速度會產(chǎn)生很大的干擾信號，它疊加在有用測量信號上，使得輸出信號無法準(zhǔn)確反映車體的傾角，因此不能單獨(dú)使用加速度計(jì)測量小車傾角。

系統(tǒng)采用ENC-03B傾角陀螺儀測量小車?yán)@輪軸旋轉(zhuǎn)的角速度，通過對角速度積分獲得車體傾角。如果角速度信息存在較小的偏差，經(jīng)過積分運(yùn)算后誤差會積累，且隨著時(shí)間延長不斷增加，導(dǎo)致測量值完全失真。同時(shí)傾角陀螺儀存在溫度漂移，會進(jìn)一步加大誤差。因此，智能小車系統(tǒng)不能單獨(dú)使用陀螺儀進(jìn)行傾角測量，需要采用加速度傳感器進(jìn)行校正。

為獲得準(zhǔn)確的傾角信號，需采用多傳感器信號融合處理，并采用適宜的數(shù)據(jù)融合方法，以達(dá)到最好的測量效果。

卡爾曼濾波是一種最優(yōu)化自回歸數(shù)據(jù)處理算法[9-10]，它屬于一種軟件濾波方法，常用于傳感器數(shù)據(jù)融合。卡爾曼濾波是一種遞歸估計(jì)，其通過上一時(shí)刻的最佳估計(jì)值和當(dāng)前狀態(tài)的觀測值來更新當(dāng)前狀態(tài)的估計(jì)值。

已知傾角φ和傾角角速度存在導(dǎo)數(shù)關(guān)系，φ可作為狀態(tài)向量；利用加速度計(jì)估計(jì)陀螺儀的偏差δ，將該偏差作為另一狀態(tài)向量，則有：

(1)

式中：ωgyro為陀螺儀測量值；ωg為陀螺儀的測量噪聲；φacce為加速度計(jì)測量值轉(zhuǎn)化后的角度；ωa為加速度計(jì)的測量噪聲，且ωg和ωa為相互獨(dú)立的零均值高斯白噪聲。

令X=[φδ]T,T為采樣周期，則可得到離散系統(tǒng)的狀態(tài)方程X(k)和測量方程Z(k)分別為：

最小均方誤差為:

(2)

式中:qacce為加速度計(jì)測量協(xié)方差;qgyro為陀螺儀測量協(xié)方差，其數(shù)值越小，表明卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度越高;racce為觀測噪聲協(xié)方差。

時(shí)間更新方程：

(3)

狀態(tài)更新方程：

(4)

經(jīng)過卡爾曼濾波融合后的傾角波形平滑，濾波效果明顯，并且完全跟隨加速度計(jì)測量角度的變化?？柭鼮V波同時(shí)也消除了加速度計(jì)測量中產(chǎn)生的噪聲干擾和過沖現(xiàn)象。

在實(shí)際操作過程中，卡爾曼濾波還能有效抑制陀螺儀的溫漂。

2.2 賽道信息的采集及處理

根據(jù)麥克斯韋電磁場理論，交變電流的周圍會產(chǎn)生交變的電場；又根據(jù)畢奧-薩法爾定律，通有穩(wěn)恒電流的導(dǎo)線周圍會產(chǎn)生按一定規(guī)律分布的磁場，該磁場分布是以導(dǎo)線為軸的一系列同心圓，并且距離導(dǎo)線越遠(yuǎn)磁場強(qiáng)度越弱。導(dǎo)線周圍變化的磁場可以近似為緩變的磁場，按照檢測靜態(tài)磁場的方法獲取導(dǎo)線周圍的磁場分布，從而檢測出賽道信息。

系統(tǒng)采用兩個(gè)工字型電感線圈檢測導(dǎo)線兩側(cè)的磁場強(qiáng)度，并將兩個(gè)電感線圈測得的數(shù)據(jù)之差和兩者之和作商，獲得方向控制的比例偏差。

由于小車具有較大的轉(zhuǎn)動慣量，在方向調(diào)整過程中會出現(xiàn)小車轉(zhuǎn)向過沖而沖出賽道。為了消除小車方向控制中的過沖現(xiàn)象，需要增加微分控制。利用水平陀螺儀測得車體左右轉(zhuǎn)動角速度，作為方向控制的微分控制量。

2.3 速度信息檢測

系統(tǒng)通過光電編碼器分別測得小車左右輪的實(shí)時(shí)速度。由于MC9S12XS128的脈沖累加器只有一個(gè)I/O接口，因此要通過分時(shí)測速對左右兩輪進(jìn)行測速。

3 控制方案

3.1 直立控制

在直立控制中，將車模近似為倒立擺，其受力分析如圖3所示。

圖3 倒立擺受力分析圖

為使倒立擺能夠像單擺一樣穩(wěn)定在垂直位置，必須使恢復(fù)力和位移方向相反，這樣倒立擺所受的恢復(fù)力為：

F=mgsinθ-macosθ≈mgθ-mk1θ

(5)

式中:θ值很小，可對其進(jìn)行線性化處理。

另外,為使車模盡快穩(wěn)定下來，還需要增加控制阻尼力，增加的控制阻尼力與偏角速度成正比，所以式(5)轉(zhuǎn)化為：

(6)

這樣就可以將倒立擺模型轉(zhuǎn)化為單擺模型處理，從而得到直立的條件：

(7)

比例參數(shù)能增強(qiáng)小車回復(fù)到平衡位置的回復(fù)力，但是比例參數(shù)過大會引起小車抖動；而微分參數(shù)相當(dāng)于阻尼力，能夠有效抑制小車的抖動。通過不斷的參數(shù)校正，可以維持車模在很短一段距離內(nèi)直立，但是隨著時(shí)間的延長，小車擺動的范圍會變大，最終會朝著某一方向加速運(yùn)動，這時(shí)需要增加速度閉環(huán)控制。

3.2 速度控制

將直立控制與光電編碼器測得的小車運(yùn)動速度相加，便構(gòu)成速度閉環(huán)控制。

在速度控制中，采用比例和積分控制。當(dāng)給定速度為零時(shí)，增加積分控制能迅速使小車恢復(fù)到原來位置，但是積分值過大會導(dǎo)致車體發(fā)生抖動，此時(shí)加入比例控制能抑制這種抖動，增加小車穩(wěn)定性；當(dāng)給定速度不為零時(shí)，小車能穩(wěn)定地按給定速度勻速行進(jìn)。通過加入速度閉環(huán)控制，可以補(bǔ)償陀螺儀和重力加速度的漂移和誤差，提高小車傾角和速度控制的穩(wěn)定性。

3.3 方向控制

方向控制在直立控制和速度控制的基礎(chǔ)上完成。主控制器將通過電感線圈獲得的比例偏差平均分配到左右電機(jī)，形成左右輪差動電壓，通過差速進(jìn)行轉(zhuǎn)向，由水平陀螺儀測得車模左右擺動的角速度。由于角度和角速度存在導(dǎo)數(shù)關(guān)系，因此可以通過比例和微分控制實(shí)現(xiàn)方向控制。其中比例控制控制轉(zhuǎn)向；微分控制對其進(jìn)行修正，消除由比例控制引起的過沖，防止車模沖出賽道，以達(dá)到最佳的控制效果。

4 結(jié)束語

本文設(shè)計(jì)了一種基于卡爾曼濾波的直立智能循跡小車系統(tǒng)，在原始動態(tài)數(shù)據(jù)采集的基礎(chǔ)上，通過卡爾曼濾波有效補(bǔ)償傳感器漂移與測量噪聲等因素對加速度

計(jì)與陀螺儀的影響，減小傾角測量誤差，提高運(yùn)算速度。

通過對整個(gè)系統(tǒng)進(jìn)行分解，分解后獨(dú)立控制，簡化了車模的數(shù)學(xué)模型，最終小車能夠穩(wěn)定地直立，平滑地過彎，響應(yīng)速度快。在控制方面利用閉環(huán)控制，有效地提高了車模運(yùn)行的穩(wěn)定性。該智能循跡小車參加2012年第七屆“飛思卡爾”杯全國大學(xué)生智能汽車競賽，循跡速度為2.5 m/s，并獲得二等獎(jiǎng)，證明了循跡小車設(shè)計(jì)的正確性與合理性，應(yīng)用前景廣闊。

[1] 張陽.MC9S12XS單片機(jī)原理及嵌入式系統(tǒng)開發(fā)[M].北京:電子工業(yè)出版社，2011:84-132.

[2] 卓晴，黃開勝，邵貝貝.學(xué)做智能車—挑戰(zhàn)“飛思卡爾”杯[M].北京:北京航空航天大學(xué)出版社，2007：32-146.

[3] 孫同景.Freescale 9S12十六位單片機(jī)原理及嵌入式開發(fā)技術(shù)[M].北京:機(jī)械工業(yè)出版社，2008:28-112.

[4] 王威.HCS12微控制器原理及應(yīng)用[M].北京:北京航空航天大學(xué)出版社，2011:52-96.

[5] 劉擷捷，楊智，范正平.基于LabVIEW的PID參數(shù)自整定控制器設(shè)計(jì)[J].自動化儀表，2009(11):38-40.

[6] 咸慶信，類延法.PID控制[M].北京:機(jī)械工業(yè)出版社，2011:18-128.

[7] 譚浩強(qiáng).C程序設(shè)計(jì)[M].3版.北京:清華大學(xué)出版社，2005:30-120.

[8] 翁卓，熊承義，李丹婷.基于光電傳感器的智能車控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測量與控制，2010,18(8)：1789-1791.

[9] 馮智勇，曾瀚，張力,等.基于陀螺儀及加速度計(jì)信號融合的姿態(tài)角度測量[J].西南師范大學(xué)學(xué)報(bào):自然科學(xué)版，2011，36(4):137-140.

[10] 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)刊，2009,8(11)：32-34.