一種直立智能循跡小車的設計

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

0 引言

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

1 系統整體結構設計

智能小車在直立的狀態下以兩個后輪著地沿著賽道行進，相比四輪著地狀態，車模控制任務更為復雜。系統硬件部分以MC9S12XS128[1-4]為主控制器，軟件采用卡爾曼濾波和PID控制算法[5-6]進行數據處理，以實現最佳控制效果。

1.1 系統硬件結構

直立智能小車硬件采用模塊化設計，主要由全橋電機驅動模塊、傾角測量模塊、光電編碼器、賽道信息檢測模塊、無線通信模塊等構成。整個系統構成一個閉環控制系統，其原理如圖1所示。

圖1 系統結構原理框圖

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

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

1.2 系統軟件構架

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

圖2 系統軟件流程圖

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

2 信息采集和處理

2.1 角度信息的采集及處理

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

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

為獲得準確的傾角信號，需采用多傳感器信號融合處理，并采用適宜的數據融合方法，以達到最好的測量效果。

卡爾曼濾波是一種最優化自回歸數據處理算法[9-10]，它屬于一種軟件濾波方法，常用于傳感器數據融合。卡爾曼濾波是一種遞歸估計，其通過上一時刻的最佳估計值和當前狀態的觀測值來更新當前狀態的估計值。

已知傾角φ和傾角角速度存在導數關系，φ可作為狀態向量；利用加速度計估計陀螺儀的偏差δ，將該偏差作為另一狀態向量，則有：

(1)

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

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

最小均方誤差為:

(2)

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

時間更新方程：

(3)

狀態更新方程：

(4)

經過卡爾曼濾波融合后的傾角波形平滑，濾波效果明顯，并且完全跟隨加速度計測量角度的變化。卡爾曼濾波同時也消除了加速度計測量中產生的噪聲干擾和過沖現象。

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

2.2 賽道信息的采集及處理

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

系統采用兩個工字型電感線圈檢測導線兩側的磁場強度，并將兩個電感線圈測得的數據之差和兩者之和作商，獲得方向控制的比例偏差。

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

2.3 速度信息檢測

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

3 控制方案

3.1 直立控制

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

圖3 倒立擺受力分析圖

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

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

(5)

式中:θ值很小，可對其進行線性化處理。

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

(6)

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

(7)

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

3.2 速度控制

將直立控制與光電編碼器測得的小車運動速度相加，便構成速度閉環控制。

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

3.3 方向控制

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

4 結束語

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

計與陀螺儀的影響，減小傾角測量誤差，提高運算速度。

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

[1] 張陽.MC9S12XS單片機原理及嵌入式系統開發[M].北京:電子工業出版社，2011:84-132.

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

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

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

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

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

[7] 譚浩強.C程序設計[M].3版.北京:清華大學出版社，2005:30-120.

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

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

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