智能循跡小車設(shè)計(jì)＊

朱建華

（中國冶金地質(zhì)總局山東局二隊(duì)，山東 濰坊 261021）

智能循跡小車是在沒有人工干預(yù)的情況下，能夠沿設(shè)定路徑自動行駛的安全、穩(wěn)定運(yùn)輸車。它是以電磁、光學(xué)等作為引導(dǎo)裝置，普遍采用充電蓄電池為行駛動力，通過設(shè)計(jì)計(jì)算機(jī)程序來控制它的行駛狀態(tài)和滿足其它要求。工業(yè)上普遍用來裝載和搬運(yùn)貨物。例如，在規(guī)定路徑上鋪設(shè)一條通電漆包線，小車可以通過電磁軌道所帶來的路徑信息進(jìn)行自動行駛，將工廠貨物自動沿著路徑運(yùn)送到目的地，大大提高了工作效率。

為保證兩輪小車的直立行走，設(shè)計(jì)了兩輪小車的硬件和軟件；通過鋪設(shè)跑道，即鋪有一條漆包線，漆包線內(nèi)通有20KHz的方波交流信號，通過對其周圍交變磁場的檢測以實(shí)現(xiàn)循跡行駛［1］。

1 直立原理

車模直立的控制可以簡化為倒立擺直立的控制。倒立擺的平衡原理又類似于單擺。單擺能夠穩(wěn)定在一個(gè)位置的原因是當(dāng)物體離開垂直的平衡位置之后，便會受到重力與懸線的作用合力，驅(qū)動重物恢復(fù)到平衡位置。在回復(fù)力作用下，單擺進(jìn)行周期運(yùn)動，而此時(shí)的空氣阻力提供了阻尼使其最后穩(wěn)定在平衡位置。

圖1 兩輪車受力示意圖

若要使倒立擺平衡，只需對其增加額外受力，使恢復(fù)力與位移方向相反即可?？刂频沽[底部，使得它作加速運(yùn)動。在受力點(diǎn)上分析倒立擺受力，它就會受到額外的慣性力，該力與倒立擺底部的加速度方向相反，大小成正比。倒立擺所受到的回復(fù)力為（式中，θ為車模傾角；θ′為角速度）：

增加阻尼力后變?yōu)椋?/p>

控制算法簡化為

只需調(diào)整兩個(gè)K 參數(shù)即可使倒立擺直立。通過類比，即可得到車模的直立控制方案。假設(shè)倒立車模簡化成高度為L，質(zhì)量為m 的簡單倒立擺，它放置在可以左右移動的車輪上。假設(shè)外力干擾引起車模產(chǎn)生角加速度x（t）。沿著垂直于車模地盤方向進(jìn)行受力分析，可以得到車模傾角與車輪運(yùn)動加速度a（t）以及外力干擾加速度x（t）之間的運(yùn)動方程。兩輪車直立受力如圖1所示。

車模運(yùn)動方程：

在θ很小時(shí)，簡化為：

車模靜止時(shí)a（t）=0：

2 硬件設(shè)計(jì)

2.1 機(jī)械設(shè)計(jì)

為了保證直立控制的穩(wěn)定性，盡量保證車模在豎直位置時(shí)能夠到達(dá)平衡狀態(tài)。

前瞻設(shè)計(jì)：前瞻用來檢測賽道上的交變磁場，實(shí)現(xiàn)循跡行駛。其材料采用了碳纖維管。碳纖維管質(zhì)量輕，具有較強(qiáng)的剛性，為確保兩個(gè)電感在一條直線上，將電感塞進(jìn)碳纖維管兩端。為了方便后期調(diào)試，用鐵絲纏繞在縱向桿上，可以方便調(diào)整前瞻的長度。在前瞻的橫向桿中間安裝了一個(gè)插頭，為后期調(diào)試提供方便。前瞻如圖2所示。

圖2 前瞻

電池和電機(jī)布置：為了循跡運(yùn)行，需要將前瞻安裝在車模前面20-40cm 處，為了保證車模在靜止?fàn)顟B(tài)時(shí)，車身盡量接近豎直狀態(tài)以方便傳感器的布置，需要將電池安裝在車模背面，電機(jī)安裝在底部。

傳感器安裝：傳感器包括編碼器、陀螺儀、加速度傳感器。其中陀螺儀和加速度傳感器用雙面膠粘到底盤上，一方面方便以后改變位置，另一方面可以在一定程度上減小振動的影響。編碼器包括碼盤和光點(diǎn)開光兩部分，將碼盤先安裝在塑料柱上，然后用AB 膠粘到主動齒輪上，光電開關(guān)用鐵絲固定，方便調(diào)整。傳感器安裝如圖3所示。

圖3 傳感器安裝

2.2 電路設(shè)計(jì)

在PCB 繪制時(shí)，考慮到電機(jī)對單片機(jī)的影響，將單片機(jī)安裝到離電機(jī)盡量遠(yuǎn)的地方，使單片機(jī)能夠穩(wěn)定運(yùn)行，單片機(jī)運(yùn)行需要5V 電源，其供電采用了7805方案，可以將電池7.2V 的電壓轉(zhuǎn)換成穩(wěn)定的5V 電壓，為單片機(jī)提供穩(wěn)定的工作環(huán)境。

編碼器使用了光電開關(guān)和碼盤的方案，節(jié)約了成本；速度信號的采集使用了計(jì)數(shù)器芯片CD4520，對片內(nèi)兩路計(jì)數(shù)器進(jìn)行級聯(lián)，以滿足計(jì)數(shù)要求；車輪方向的鑒別使用了D 觸發(fā)器74LS74，保證了速度反饋的準(zhǔn)確性；電機(jī)驅(qū)動采用MC33883 加四個(gè)IRF3205組成H 橋方案，其內(nèi)阻小，發(fā)熱量?。?］。

信號采集電路是將賽道上的電磁信息放大、濾波處理后，將信息經(jīng)過A／D 轉(zhuǎn)換傳遞到單片機(jī)中，經(jīng)過單片機(jī)的運(yùn)算處理。信號放大采用的是運(yùn)算法大器實(shí)現(xiàn)的。然后，利用2個(gè)二極管把交變信號變成直流信號。接到單片機(jī)A／D 口。信號采集電路如圖4所示。

圖4 信號采集電路

電機(jī)驅(qū)動采用了MC33883來驅(qū)動H 橋。H 橋使用的是四個(gè)IRF3205，其導(dǎo)通內(nèi)阻僅有8 mΩ，最大電流可達(dá)110A。配合MC33883，IRF3205 的發(fā)熱量很小。這為車模的平衡控制創(chuàng)造了良好的條件電路原理如圖5所示。

圖5 電機(jī)驅(qū)動電路

3 軟件設(shè)計(jì)

車?？刂齐娐分谱髋c安裝均已完畢。車模是否能夠正常高速穩(wěn)定運(yùn)行，需要通過編寫控制軟件和車模調(diào)試來完成。

控制程序的編寫主要包括信號采集及處理、直立控制、速度控制、方向控制。

信號的采集：加速度傳感器和陀螺儀輸出的信號為模擬量，通過單片機(jī)的ATD 端口轉(zhuǎn)換為數(shù)字量進(jìn)行歸一化計(jì)算和卡爾曼濾波，電感信號同樣為模擬信號，但由于其信號微弱，需要經(jīng)過運(yùn)放電路放大再輸入到單片機(jī)的ATD 端口。輪速信號由編碼器采集，編碼器產(chǎn)生兩路相差90度的方波型號，通過CD4520對一路方波進(jìn)行計(jì)數(shù)，得到方波個(gè)數(shù)，由74LS74根據(jù)兩個(gè)方波判斷車輪的轉(zhuǎn)動方向。這樣就完成了信號的采集。

程序的總體框架：主函數(shù)初始化完成后，開啟1ms中斷，中斷服務(wù)程序中分為測速、AD 信號轉(zhuǎn)化及濾波、直立控制、速度控制、轉(zhuǎn)向控制5個(gè)片段，每產(chǎn)生一個(gè)中斷執(zhí)行一個(gè)程序片段，5ms執(zhí)行完一個(gè)循環(huán)。程序總體框架如圖6所示。

圖6 程序總體框架

中斷服務(wù)程序：在程序中使用9S12 XS128 的RTI定時(shí)中斷，產(chǎn)生一毫秒的周期中斷。中斷服務(wù)程序的任務(wù)被均勻分配在0-4 的中斷片段中，每個(gè)中斷片段中的任務(wù)執(zhí)行的周期為5 毫秒，頻率為200 Hz。將任務(wù)分配到不同的中斷片段中，一方面防止這些任務(wù)累積執(zhí)行時(shí)間超過1 毫秒擾亂一毫秒中斷的時(shí)序，同時(shí)也考慮到這些任務(wù)之間的時(shí)間先后順序。這些任務(wù)包括［4］：

（1）電機(jī)測速脈沖計(jì)數(shù)器讀取與清除。累積電機(jī)轉(zhuǎn)動角度。累積電機(jī)速度，為后面車模速度控制提供平均數(shù)；

（2）啟動AD 轉(zhuǎn)換。對加速度和角速度信號進(jìn)行卡爾曼濾波。

（3）車模直立控制過程。包括車模角度計(jì)算、直立控制計(jì)算、電機(jī)PWM 輸出等。

（4）車模速度控制：在這個(gè)時(shí)間片段中，又進(jìn)行0-19計(jì)數(shù)。在其中第0片段中，進(jìn)行速度PID 調(diào)節(jié)。因此，速度調(diào)節(jié)的周期為100 毫秒。也就是每秒鐘調(diào)節(jié)10 次。

（5）車模方向控制：根據(jù)前面讀取的電磁場檢波數(shù)值，計(jì)算偏差數(shù)值，然后融合陀螺儀的信號進(jìn)行PID控制。

直立控制：根據(jù)前面采集并濾波后的加速度傳感器和陀螺儀信號對電機(jī)進(jìn)行PID 控制。陀螺儀得到的是角速度信號，可直接作為車模傾角的微分量，對其積分可得到車模的傾斜角度，將車模角度和角速度乘以各自相應(yīng)的系數(shù)就可以得到車模電機(jī)的控制量。為了避免陀螺儀信號的漂移帶來的問題，通過加速度傳感器得到的Z 軸加速度信號計(jì)算出的車模傾角，對陀螺儀信號積分進(jìn)行修正。

速度控制：根據(jù)當(dāng)前速度和目標(biāo)速度進(jìn)行PID 控制。當(dāng)前速度和目標(biāo)速度差作為基本偏差，對這個(gè)偏差進(jìn)行積分作為累計(jì)偏差，然后分別乘以參數(shù)D 和參數(shù)P，得到PID 計(jì)算的結(jié)果。

速度控制周期為100毫秒。讀取兩個(gè)電極的光碼盤脈沖進(jìn)行累加，利用100毫秒的脈沖數(shù)量可以反映車模的電機(jī)的轉(zhuǎn)速。車模速度采用兩個(gè)電極速度的平均值。為了使得速度具有一定的物理含義，對于所取得的電機(jī)速度值需要進(jìn)行單位轉(zhuǎn)換。根據(jù)定義單位的物理含義，可以確定速度單位轉(zhuǎn)換的比例值。在本程序中，定義速度的單位是電機(jī)每秒鐘的轉(zhuǎn)動圈數(shù)。

為了減少速度控制對于車模直立控制的影響，速度控制輸出量的變化被函數(shù)speed control out 均勻分配在20個(gè)角度控制函數(shù)周期內(nèi)，這樣可以保證車模的穩(wěn)定性。

轉(zhuǎn)向控制：根據(jù)兩個(gè)電感信號的差值和陀螺儀進(jìn)行PID 控制，通過兩輪的差速實(shí)現(xiàn)車模的轉(zhuǎn)向。將兩個(gè)電感信號求差并求和，兩者之差除以兩者之和，即為基本偏差，陀螺儀信號直接作為偏轉(zhuǎn)角度的微分量，分別乘以參數(shù)P 和參數(shù)D，得到方向控制的結(jié)果。根據(jù)車模采集到的左右兩個(gè)電磁傳感器的數(shù)值計(jì)算出角度控制的量。該函數(shù)每10毫秒調(diào)用一次。

為了能夠看到程序是否正常執(zhí)行，安裝一個(gè)發(fā)光二極管作為指示燈。當(dāng)發(fā)光二極管閃爍時(shí)，則程序正常執(zhí)行。在中斷的最后一個(gè)片段里，設(shè)置一個(gè)變量，最后一個(gè)片段每執(zhí)行一次對這個(gè)變量進(jìn)行加一，當(dāng)這個(gè)數(shù)大于某個(gè)數(shù)是則進(jìn)行置零，每次置零時(shí)對發(fā)光二極管的控制引腳取反。這樣，實(shí)現(xiàn)了二極管的閃爍［5］。

通過實(shí)車調(diào)試，當(dāng)直立控制參數(shù)P 為50，參數(shù)D 為2.5時(shí)，車?？梢蕴峁┳銐蚧謴?fù)力；速度控制參數(shù)P 為70，D 為0.05時(shí)，車?？梢苑€(wěn)定在一定范圍內(nèi)來回?cái)[動；方向控制參數(shù)P 為350，D 為1.5時(shí)，車?？梢钥焖倩謴?fù)正確位置，且沒有過沖現(xiàn)象，在急轉(zhuǎn)彎的路況下小車可以在漆包線跑道內(nèi)達(dá)到1M／s的速度。

4 結(jié)束語

分析了兩輪自平衡電磁循跡小車的設(shè)計(jì)思路，提出了該車的總體設(shè)計(jì)方案。設(shè)計(jì)了小車的硬件和軟件，鋪設(shè)了試車跑道，通過調(diào)試，實(shí)現(xiàn)了小車的快速、穩(wěn)定的循跡行駛。

［1］邱關(guān)源.電路［M］.北京：高等教育出版社，2006.

［2］卓晴.電磁組直立行車參考設(shè)計(jì)方案［K］，2012.

［3］張陽.MC9S12XS單片機(jī)原理及嵌入式系統(tǒng)開發(fā)［M］.北京：電子工業(yè)出版社，2011.

［4］雷貞勇，謝光驥.飛思卡爾智能車舵機(jī)和測速的控制設(shè)計(jì)與實(shí)現(xiàn)［J］.電子設(shè)計(jì)工程，2010，18（2）：91-92.

［5］王宜懷，曹金華.嵌入式系統(tǒng)設(shè)計(jì)實(shí)戰(zhàn)：基于飛思卡爾S12X 微控制器［M］.北京：航空航天大學(xué)出版社，2011.