磁場尋跡機器人導(dǎo)航系統(tǒng)設(shè)計

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖2 單片機控制程序流程圖

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

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

圖3 ARM控制終端圖

尋跡程序中的基本思想是通過對磁場傳感器采集的機器人當(dāng)前所處位置的地磁強度與所給定的磁場強度進行比較，判斷機器人所處位置與所需尋跡的等值線位置相對關(guān)系，從而判斷機器人下一步的運動方向。考慮到起初磁場等值線位置是未知的，在尋跡初期可由人工干預(yù)來控制機器人的運動方向，這有助于機器人快速尋找到需要尋跡的磁場等值線，因此在系統(tǒng)中加入了人工干預(yù)功能。ARM終端控制終端圖如圖3所示。由于單片機與上位機之間通過Zigbee實現(xiàn)無線通訊，該過程是基于串口異步通訊實現(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è)坐標區(qū)3條顏色的圖形分別表示某一時刻采集的地磁信號在空間坐標系X、Y、Z這3個方向上的強度。圖3中右下角區(qū)域可以用來進行人工干預(yù)機器人運動，保證尋跡的效率而添加相應(yīng)功能。因此，通過ARM控制終端圖可以直觀地顯示機器人在尋跡過程中采集到的磁場變化。

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

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

表1 磁場信號比較尋跡

3.3 等值線圖繪制

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

4 結(jié) 語

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

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

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

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

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

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