肖子玉，徐振平，詹學(xué)君敖元元，徐 雄

（長江大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，湖北 荊州434023）

地磁場是地球固有的物理場，地磁導(dǎo)航技術(shù)是利用地磁固有的穩(wěn)定可靠性進(jìn)行導(dǎo)航的一種新型制導(dǎo)技術(shù)，其利用地磁模型和地磁圖與實(shí)時航跡進(jìn)行地磁匹配，精確定位，從而達(dá)到制導(dǎo)效果［1］。地磁導(dǎo)航屬于無源導(dǎo)航，導(dǎo)航定位誤差不隨時間積累。在機(jī)器人導(dǎo)航方面，從生產(chǎn)應(yīng)用及國內(nèi)機(jī)器人比賽中使用的機(jī)器人尋跡系統(tǒng)情況來看，多數(shù)采用的是視覺導(dǎo)航系統(tǒng)，其導(dǎo)航抗干擾能力較弱。為此，筆者將控制技術(shù)和地磁導(dǎo)航技術(shù)結(jié)合起來，按照地磁場等值線進(jìn)行尋跡，取得了較好的效果。

圖1 系統(tǒng)整體構(gòu)架圖

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

采用C8051F340單片機(jī)作為尋跡車的驅(qū)動控制模塊，利用Zigbee通信模塊實(shí)現(xiàn)單片機(jī)與ARM板的通信，并將地磁傳感器MAG3110采集的磁場信號上傳至上位機(jī)ARM微處理器 （mini6410）進(jìn)行數(shù)據(jù)的處理和分析，再根據(jù)分析結(jié)果，將尋跡信號通過Zigbee通信模塊傳回給單片機(jī)，單片機(jī)再做出相應(yīng)的尋跡動作。同時，ARM微處理器根據(jù)處理的信號繪制當(dāng)前環(huán)境的地磁等值線圖。系統(tǒng)設(shè)計(jì)整體構(gòu)架圖如圖1所示。

2 單片機(jī)控制模塊設(shè)計(jì)

2.1 機(jī)器人驅(qū)動設(shè)計(jì)

在單片機(jī)控制模塊中，機(jī)器人的尋跡運(yùn)動由伺服電機(jī)與驅(qū)動電機(jī)組合的模式完成，即利用伺服電機(jī)控制整個機(jī)器人的運(yùn)動方向，驅(qū)動電機(jī)提供驅(qū)動力，使機(jī)器人沿著某條直線作前進(jìn)后退的運(yùn)動。通過單片機(jī)產(chǎn)生PWM脈寬信號，可以控制伺服電機(jī)轉(zhuǎn)動到所需要的角度，考慮到實(shí)際中一定區(qū)域地磁變化很小的實(shí)際情況，將伺服電機(jī)的可控角度定在0～180°之間，可控精度限定為3°［2］。

2.2 單片機(jī)控制程序設(shè)計(jì)

單片機(jī)模塊控制機(jī)器人的運(yùn)動行為，包括加速、減速及轉(zhuǎn)向，其次通過機(jī)器人搭載的地磁傳感器對機(jī)器人當(dāng)前所處地域地磁信號進(jìn)行采集，發(fā)送至上位機(jī)ARM微處理器。ARM微處理器接受單片機(jī)上傳的地磁信號，對當(dāng)前地磁信號進(jìn)行數(shù)據(jù)的處理分析，由此繪制出地磁信號的時間－強(qiáng)度圖和等值線圖。同時，根據(jù)數(shù)據(jù)分析結(jié)果，判斷機(jī)器人的下一步運(yùn)動方向，并發(fā)送給單片機(jī)相應(yīng)的控制指令。

機(jī)器人的各種運(yùn)動狀態(tài)控制指令由上位機(jī)ARM尋跡程序產(chǎn)生或人工發(fā)送，單片機(jī)對指令信號解析后控制機(jī)器人的運(yùn)動。關(guān)于舵機(jī)控制和車速控制，可采用魯棒性強(qiáng)的PID算法分別進(jìn)行位置閉環(huán)控制和車速閉環(huán)控制［3］。單片機(jī)控制程序流程圖如圖2所示。

3 上位機(jī)控制模塊設(shè)計(jì)

3.1 磁場數(shù)據(jù)的預(yù)處理

圖2 單片機(jī)控制程序流程圖

由于采集的數(shù)據(jù)存在一定外界干擾，且在小范圍內(nèi)地磁信號的變化不明顯，這樣，采集的數(shù)據(jù)就不可能直接用于尋跡機(jī)器人的位置判斷和尋跡控制，需要進(jìn)行一定處理后才能作為尋跡程序的判定值。設(shè)計(jì)中采用5階二次平滑和傅里葉變換算法對采集的數(shù)據(jù)分析預(yù)處理［4］，再進(jìn)行尋跡判斷。

3.2 尋跡控制程序設(shè)計(jì)

圖3 ARM控制終端圖

尋跡程序中的基本思想是通過對磁場傳感器采集的機(jī)器人當(dāng)前所處位置的地磁強(qiáng)度與所給定的磁場強(qiáng)度進(jìn)行比較，判斷機(jī)器人所處位置與所需尋跡的等值線位置相對關(guān)系，從而判斷機(jī)器人下一步的運(yùn)動方向?？紤]到起初磁場等值線位置是未知的，在尋跡初期可由人工干預(yù)來控制機(jī)器人的運(yùn)動方向，這有助于機(jī)器人快速尋找到需要尋跡的磁場等值線，因此在系統(tǒng)中加入了人工干預(yù)功能。ARM終端控制終端圖如圖3所示。由于單片機(jī)與上位機(jī)之間通過Zigbee實(shí)現(xiàn)無線通訊，該過程是基于串口異步通訊實(shí)現(xiàn)的，因此圖3中左側(cè)串口參數(shù)設(shè)置功能區(qū)主要是修改串口號和波特率以建立兩者間的連接，而圖中數(shù)據(jù)設(shè)置區(qū)的功能主要是將中間數(shù)據(jù)接收區(qū)數(shù)據(jù)以一定頻率保存或者將發(fā)送數(shù)據(jù)區(qū)數(shù)據(jù)按一定格式和頻率發(fā)送出去。此外，圖3中右側(cè)坐標(biāo)區(qū)3條顏色的圖形分別表示某一時刻采集的地磁信號在空間坐標(biāo)系X、Y、Z這3個方向上的強(qiáng)度。圖3中右下角區(qū)域可以用來進(jìn)行人工干預(yù)機(jī)器人運(yùn)動，保證尋跡的效率而添加相應(yīng)功能。因此，通過ARM控制終端圖可以直觀地顯示機(jī)器人在尋跡過程中采集到的磁場變化。

1）傳感器陣列設(shè)計(jì) 基于尋跡的基本思想［5］，理論上可以在機(jī)器人左右2邊各搭載一個磁場傳感器，這樣當(dāng)給定的磁場強(qiáng)度值處在2個傳感器采集值之間時，可以理解為機(jī)器人正好處在該強(qiáng)度磁場等值線上，機(jī)器人便可以繼續(xù)向前行進(jìn)。但是，考慮到實(shí)際環(huán)境因素、傳感器誤差等影響，采用7個傳感器一字排開的陣列設(shè)計(jì)，使其具有很強(qiáng)的抗干擾能力。

2）算法設(shè)計(jì) 由傳感器的陣列設(shè)計(jì)可知，當(dāng)7個傳感器呈直線排列時，可測出從左至右的7個不同點(diǎn)的磁場強(qiáng)度值，并與固定強(qiáng)度的磁場強(qiáng)度等值線進(jìn)行匹配分析，由此可以準(zhǔn)確判斷出當(dāng)前機(jī)器人與該強(qiáng)度地磁等值線的相對位置，最終達(dá)到尋跡目的。設(shè)計(jì)算法思想如表1所示。

表1 磁場信號比較尋跡

3.3 等值線圖繪制

由于尋跡機(jī)器人按照給定強(qiáng)度的磁場等值線進(jìn)行循跡運(yùn)動，因而可以認(rèn)為機(jī)器人的運(yùn)動軌跡即給定強(qiáng)度的地磁等值線圖。當(dāng)改變尋跡等值線強(qiáng)度時，機(jī)器人的尋跡過程中會形成一條新的運(yùn)動軌跡。根據(jù)機(jī)器人運(yùn)動的地理位置，將一系列運(yùn)動軌跡在地圖上繪制出來，即為該地域范圍內(nèi)的地磁等值線圖。

4 結(jié) 語

以C8051F340單片機(jī)和上位機(jī)mini6410ARM微處理機(jī)作為控制模塊，利用MAG3110地磁傳感器采集的信號，采用匹配算法實(shí)現(xiàn)尋跡機(jī)器人的地磁等值線行走。通過尋跡小車的實(shí)際運(yùn)動效果、地磁信號與時間的變化曲線的仿真試驗(yàn)，證明利用該方法設(shè)計(jì)的地磁導(dǎo)航系統(tǒng)能夠?qū)崿F(xiàn)機(jī)器人的地磁等值線的尋跡導(dǎo)航。

［1］［1］劉穎，吳美平 .基于等值線的地磁約束匹配方法 ［J］.空間科學(xué)學(xué)報，2007，27（6）：505－511.

［2］黃仲良 .石油重·磁·電法勘探 ［M］.北京：石油工業(yè)出版社，2008.

［3］張毅剛 .新編MCS－51單片機(jī)應(yīng)用設(shè)計(jì) ［M］.哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2003.

［4］求是科技 .單片機(jī)典型模塊設(shè)計(jì)實(shí)例導(dǎo)航 ［M］.北京：人民郵電出版社，2004.

［5］譚浩強(qiáng).C程序設(shè)計(jì) ［M］.北京：清華大學(xué)出版社，1991.