基于磁導航的兩輪智能車系統(tǒng)設計

魯 云，趙 亮，陳曉東，武 麗

(西南科技大學信息工程學院，中國綿陽 621000)

全國大學生“飛思卡爾”杯智能汽車競賽根據(jù)車模檢測路徑的方案不同分為電磁、光電和攝像頭3 個賽題組.電磁組是通過感應由賽道中心電線產(chǎn)生的交變磁場進行路徑檢測.第七屆全國大學生“飛思卡爾”杯智能汽車競賽規(guī)定電磁組車模直立行走，在比賽過程中只允許車模原有2 個后輪著地.針對此要求，作者提出一套設計方案，實踐證明該方案應用在磁導航的兩輪智能車中具有良好的自主尋跡效果.

1 系統(tǒng)總體框架及工作原理

系統(tǒng)以飛思卡爾公司的32 位單片機PK60X256VLQ100 為核心，設計了電源管理模塊、加速度檢測模塊、角速度檢測模塊、路徑識別模塊、電機驅(qū)動模塊、車速及車體運動方向檢測模塊、無線傳輸模塊、串口調(diào)試模塊和液晶顯示模塊等9 個部分，系統(tǒng)框圖如圖1所示.

圖1 中，首先由加速度和角速度檢測模塊分別測量車模的加速度和角速度，送入單片機計算得到車模初始狀態(tài)的平衡點，然后由互補濾波算法把加速度檢測模塊和角速度檢測模塊輸出的信號進行融合［1-4］，得到一個實時反應車模傾角的信號，再由此信號通過PID 算法控制車模平衡.在車模保持平衡的前提下由路徑識別模塊采集到的信號通過PID 算法控制車模前進速度和方向［5-6］.系統(tǒng)中還設置了串口調(diào)試模塊和無線傳輸模塊，分別用于把車模運行時的數(shù)據(jù)通過有線或無線的方式實時發(fā)送到上位機以便于觀察和調(diào)試.液晶顯示模塊可以把車模運行時的相關(guān)數(shù)據(jù)實時顯示出來.

2 系統(tǒng)硬件設計

2.1 微控制器PK60X256VLQ100

系統(tǒng)的核心控制器為PK60X256VLQ100，PK60X256VLQ100 是飛思卡爾公司生產(chǎn)的一款32 位單片機，片內(nèi)總線時鐘頻率可達100 MHz，片內(nèi)資源包括64 KB RAM、256 KB Flash、256 KB Program Memory 以及內(nèi)部自帶晶振，擁有外部GPIO、ADC、UART、PWM、FTM、PIT、I2C 等模塊接口.

2.2 電源管理模塊

智能車的動力源為一塊容量為2 000 mA，額定電壓為7.2 V 的鎳鎘電池.電源管理模塊為系統(tǒng)各個部分提供所需要的電源，可靠的電源是整個系統(tǒng)正常工作的基礎(chǔ)，系統(tǒng)電源管理模塊框圖如圖2所示.

圖1 系統(tǒng)框圖Fig.1 System block diagram

圖2 電源管理模塊框圖Fig.2 Block diagram of power management

2.3 加速度檢測模塊

加速度傳感器可測量由地球引力作用或者物體運動所產(chǎn)生的加速度，其輸出的是與加速度成正比的模擬電壓信號.選用MMA7260 作為本系統(tǒng)的加速度檢測器件.MMA7260 是一款三軸低重力加速度半導體加速度計，可以同時輸出3 個方向上的加速度模擬信號［7］，加速度傳感器如圖3所示.

圖3 加速度傳感器MMA7260Fig.3 Acceleration sensor MMA7260

通過軟件設置可以使MMA7260 各軸信號輸出靈敏度為800 mV/g，這個信號不需要進行放大，可以直接送到單片機進行AD 轉(zhuǎn)換.本系統(tǒng)只選用Z 軸方向上的加速度值，以便計算出車模相對于豎子在地面時的傾角.當車模發(fā)生傾斜時，重力加速度g 在Z 軸方向形成加速度分量，從而引起該軸輸出電壓變化，其變化規(guī)律為:Δu=kgsin θ，式中，k 為加速度傳感器靈敏度系數(shù)，g 為重力加速度，θ 為車模以豎直于地面為基準的傾角.

由于車模在運動時本身擺動所產(chǎn)生的向前或向后的加速度會帶來很大的干擾信號，它疊加在上述測量信號上使得輸出信號無法準確反映車模的傾角.加速度傳感器安裝在車模上，距離車輪軸高度為h，車模擺動具有的加速度為β，車模運動向前或向后加速度為α，那么在加速度傳感器Z 軸上實際產(chǎn)生的加速度為hβ+α.為了減少車模擺動引起的干擾信號，加速度傳感器在車模上安裝得盡量低，但是仍然無法徹底消除車模擺動帶來的影響.因此對于車模直立平衡控制所需要的傾角信息還需要通過另外一種器件獲得，那就是角速度傳感器——陀螺儀.

2.4 角速度檢測模塊

陀螺儀可以用來測量物體的旋轉(zhuǎn)角速度［8］.選用ENC-03 作為本系統(tǒng)的角速度檢測器件.ENC-03 角速度傳感器放大電路嚴格按照其官方數(shù)據(jù)手冊中的參考電路設計，這里不再贅述.

在車模上安裝陀螺儀，可以測量車模傾斜角速度，將角速度信號進行積分得到車模的傾角.因為陀螺儀輸出的角速度信號要經(jīng)過積分運算得到車模傾斜角度，如果角速度信號存在微小的偏差和漂移，經(jīng)過積分運算后會形成累計誤差.這個誤差隨著時間的推移逐漸增加，最終導致電路飽和，無法得到正確的角度信號.針對此問題，本文利用前述加速度傳感器獲得的角度信號與陀螺儀輸出信號經(jīng)積分后獲得的角度α 進行互補濾波，得到車模傾角θ，車模角度控制框圖如圖4所示.

圖4 車模角度控制框圖Fig.4 Model car's angle control block diagram

2.5 路徑識別模塊

在比賽中，兩輪智能車通過感應賽道中心下鋪設的電線進行路徑識別，電線規(guī)格是直徑為0.1～0.8 mm的漆包線，其中通有20 kHz(范圍為(20 ±1)kHz)，100 mA(范圍為(100 ±20)mA)的交變電流.以電感線圈作為路徑檢測傳感器，在智能車行駛的過程中，電感線圈輸出的感應電動勢是幅值和相位隨著線圈與載流導線相對位置變化而變化的正弦波，然后將此正弦波信號經(jīng)過正弦波峰值檢測電路轉(zhuǎn)換成對應的直流電壓信號，供給單片機進行A/D 采樣轉(zhuǎn)換，以獲取電感線圈與賽道的當前位置關(guān)系.路徑識別模塊包括路徑檢測模塊和正弦波峰值檢測電路.利用路徑檢測模塊的輸出信號，通過正弦波峰值檢測電路獲得車模當前的位置信息.正弦波峰值檢測電路詳細設計參見文獻［9］.

路徑檢測模塊采用4 個均勻分布在一條檢測直桿上的電感線圈構(gòu)成，其中左右2 個電感線圈平行于信號檢測桿，用于判斷車模與賽道的相對位置關(guān)系;中間2 個電感線圈分別向信號檢測桿中間傾斜45°，用于預測車模前方的路徑信息.

2.6 電機驅(qū)動模塊

本系統(tǒng)選用BTS7960 作為電機驅(qū)動芯片［10］.BTS7960 是應用于電機驅(qū)動的大電流半橋高集成芯片，它帶有1 個P 溝道的高邊MOSFET、1 個N 溝道的低邊MOSFET 和1 個驅(qū)動IC.P 溝道高邊開關(guān)省去了電荷泵的需求，因而減小了EMI.集成驅(qū)動IC 具有邏輯電平輸入、電流診斷、斜率調(diào)節(jié)、死區(qū)時間產(chǎn)生和過溫、過壓、欠壓、過電流及短路保護的功能.BTS7960 通態(tài)電阻典型值為16 mΩ，驅(qū)動電流可達43 A.因此完全能勝任本設計.電機驅(qū)動模塊［11］電路圖嚴格按照官方資料設計，這里不再贅述.

2.7 車速及車體運動方向檢測模塊

車模在運行過程中需要實時監(jiān)控其速度和運動方向，本系統(tǒng)在左右電機上各安裝一個500 線兩相光電編碼器.利用單片機內(nèi)部計數(shù)器在固定時間間隔內(nèi)測量由編碼器返回的脈沖信號個數(shù)可以獲得車模的運動速度.由于A、B 兩相相位差為90°，可通過比較A、B 兩相信號先后順序來判斷編碼器的正反轉(zhuǎn)，即車模的運動方向.

2.8 調(diào)試模塊

為便于系統(tǒng)參數(shù)的觀察和調(diào)試，需要把車模運行時的數(shù)據(jù)發(fā)送到上位機.設計了無線傳輸模塊和串口調(diào)試模塊，分別用于把車模運行時的數(shù)據(jù)以無線和有線方式發(fā)送到上位機.本系統(tǒng)選用NRF905 作為無線傳輸模塊［12］，采用RS-232 作為串口調(diào)試模塊.

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

本系統(tǒng)控制軟件使用C 語言編程，采用模塊化程序設計思想，以主程序為核心，設置了單片機初始化模塊、串口發(fā)送模塊、無線發(fā)送模塊、液晶顯示模塊、車模平衡控制模塊、車模速度控制模塊和車模轉(zhuǎn)向模塊等.

3.1 系統(tǒng)主函數(shù)

主函數(shù)主要有執(zhí)行系統(tǒng)初始化、發(fā)送系統(tǒng)相關(guān)參數(shù)到上位機、液晶顯示、判斷車模傾角是否大于25°的功能.其流程圖如圖5所示.

3.2 系統(tǒng)中斷程序

系統(tǒng)中斷函數(shù)主要負責處理控制車模平衡、速度和轉(zhuǎn)向程序.進入中斷函數(shù)時首先檢測車模平衡點，然后采用互補濾波算法把由加速度傳感器和陀螺儀分別得到的角度信號進行濾波，再利用PID 算法將濾波后得到的車模傾角信號通過電機驅(qū)動模塊控制車模平衡.在車模保持平衡的前提下，利用PID 算法將路徑識別模塊返回的信號通過電機驅(qū)動模塊分3 步控制車模的前進速度、轉(zhuǎn)向和加減速.系統(tǒng)中斷函數(shù)如圖6所示.

圖5 系統(tǒng)主函數(shù)流程圖Fig.5 Flowchart of system main function

圖6 系統(tǒng)中斷函數(shù)流程圖Fig.6 Flowchart of system interrupt function

3.3 互補濾波算法

互補濾波算法公式為

angle_new=A_gyro*(angle_old+Gyroangle)+B_acc*Accangle)，

式中:angle_new 為第n 次濾波后的角度，angle_old 為第n 次濾波前的角度，A_gyro 為陀螺儀濾波權(quán)重系數(shù)，B_acc 為加速度濾波權(quán)重系數(shù)，Gyroangle 為第n 次陀螺儀采樣的角速度值，Accangle 為第n 次加速度采樣的角度值［13-14］.

兩濾波權(quán)重系數(shù)之間存在關(guān)系為A_gyro+B_acc=1.利用公式A_gyro=t/(t+T)，t 為時間常數(shù)，它決定了兩者的各自權(quán)重;對于時間周期小于t 的，陀螺儀將會占主要作用，其加速度計的噪聲將被過濾掉;相反，加速度計將起主要作用而抑制陀螺儀的積分漂移.得到A_gyro 后，即得到B_acc，然后再進行微調(diào)就得到理想的濾波參數(shù).

3.4 車模平衡控制算法

車模平衡控制采用了PD 算法，其公式為

acceleration=kp*angle_new+kd*Gyroangle，

式中:angle_new 為最近一次車模傾斜角度，Gyroangle 為最近一次車模的角速度kd.調(diào)參數(shù)方法:先將比例控制作用由小變到大，觀察各次響應，直至得到反應快、超調(diào)小的響應曲線.然后置微分參數(shù)kd=0，逐漸加大kd，同時相應調(diào)整比例系數(shù)，反復試湊至獲得滿意的控制效果和PD 控制參數(shù).

3.5 車模行駛控制算法

車模行駛控制采用了PI 算法，其公式為:

run_speed_TempNew=nDeltaValue *P+run_speed_Tempkeep*I，

式中:nDeltaValue 為最近一次速度期望與實際速度的偏差，run_speed_Tempkeep 為速度偏差的累積.調(diào)參數(shù)方法:先將比例控制作用P 由小變到大，觀察各次響應，直至得到反應快、超調(diào)小的響應曲線，再調(diào)節(jié)積分控制作用I 參數(shù)來消除控制穩(wěn)態(tài)誤差.

4 車模運動控制總框圖

車模運動控制總框圖如圖7所示.由互補濾波得到的車模傾角θ 和陀螺儀輸出的角速度ω 進行PD 運算，獲得控制車模平衡的電機驅(qū)動信號.利用左右電感線圈信號差值按照PD 算法進行運算，得到控制車模運動方向所需的電機驅(qū)動信號.通過中間2 個電感線圈信號差值進行P 運算，獲得車模減速所需的信號.由車模給定速度與速度反饋信號做差后按照PI 算法進行運算，得到車模實際運行速度所需的電機驅(qū)動信號.因此，電機驅(qū)動模塊在車模平衡控制信號、車模速度控制信號和車模運行方向控制信號的分時作用下驅(qū)動2個直流電機來控制車模的平衡、速度和方向.

圖7 車模運動控制總框圖Fig.7 Total block diagram of model car's motion control

5 結(jié)論

以PK60X256VLQ100 單片機作為控制核心，設計磁導航的兩輪智能車控制系統(tǒng).該系統(tǒng)是一種典型的快速、多變量、非線性、強耦合和自不穩(wěn)定系統(tǒng).在系統(tǒng)控制上主要研究了基于互補濾波算法的加速度傳感器與陀螺儀信號融合方法和控制車模平衡及行駛的先進PID 算法.實驗證明:該磁導航兩輪智能車能在任意的規(guī)范賽道上穩(wěn)定、準確、快速地行駛，尋跡效果良好.系統(tǒng)信號及控制響應速度快，系統(tǒng)的穩(wěn)定性和抗干擾能力強.該系統(tǒng)方案也可以用于其他類似的輪式機器人中，應用前景廣闊.

［1］郜園園，阮曉鋼，宋洪軍，等.兩輪自平衡機器人慣性傳感器濾波問題的研究［J］.傳感技術(shù)學報，2010，23(5):696-700.

［2］張吉昌，程 凱，鄭榮兒.單軸雙輪自平衡車姿態(tài)檢測方案設計［J］.中國海洋大學學報:自然科學版，2009，39(增):467-470.

［3］馮智勇，曾 瀚，張 力，等.基于陀螺儀及加速度計信號融合的姿態(tài)角度測量［J］.西南師范大學學報:自然科學版，2011，36(4):136-141.

［4］楊興明，余忠宇.自平衡控制系統(tǒng)的平衡性仿真［J］.計算機工程與應用，2011，47(24):245-248.

［5］張志強.基于STM32 的雙輪平衡車［J］.電子設計工程，2011，19(13):103-106.

［6］李涵武，崔宏耀，張娜偉，等.基于電磁場檢測的智能尋跡模型車設計［J］.黑龍江工程學院學報，2011，25(2):23-25.

［7］楊繼志，郭 敬.基于MMA7260 兩輪自平衡小車控制系統(tǒng)設計［J］.機電產(chǎn)品開發(fā)與創(chuàng)新，2011，24(3):144-146.

［8］吳曉鵬，趙祚喜，黃 健，等.CRS10 陀螺儀及其在角速率與轉(zhuǎn)角測量中的應用［J］.電子設計工程，2010，18(8):39-41.

［9］趙 亮，魯 云，陳曉東，等.磁導航智能車中的正弦波峰值檢測電路設計［J］.電子世界，2012(7):137-139.

［10］陳 軍，楊數(shù)強，王軍強.一種新型智能車電機驅(qū)動電路的設計與實現(xiàn)［J］.電子質(zhì)量，2011，32(3):32-34.

［11］肖文英，黃守道.基于PR 控制器的永磁同步電機弱磁控制研究［J］.湘潭大學學報自然科學版，2011，33(4):108-112.

［12］張孝云，江小華.基于nRF905 的無線加速度測量系統(tǒng)設計［J］.現(xiàn)代電子技術(shù)，2011，34(17):153-155.

［13］趙 莉，陳庚華，張利華，等.互補型自適應濾波器在心磁信號處理中的應用［J］.物理學報，2004，53(12):4420-4427.

［14］費萬民，呂征宇，錢 挺.基于互補策略的新型單周控制有源濾波器［J］.電力系統(tǒng)自動化，2003，27(3):50-53.