磁場尋跡機器人導航系統設計

肖子玉，徐振平，詹學君敖元元，徐 雄

（長江大學計算機科學與技術學院，湖北 荊州434023）

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

圖1 系統整體構架圖

1 系統整體架構設計

采用C8051F340單片機作為尋跡車的驅動控制模塊，利用Zigbee通信模塊實現單片機與ARM板的通信，并將地磁傳感器MAG3110采集的磁場信號上傳至上位機ARM微處理器 （mini6410）進行數據的處理和分析，再根據分析結果，將尋跡信號通過Zigbee通信模塊傳回給單片機，單片機再做出相應的尋跡動作。同時，ARM微處理器根據處理的信號繪制當前環境的地磁等值線圖。系統設計整體構架圖如圖1所示。

2 單片機控制模塊設計

2.1 機器人驅動設計

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

2.2 單片機控制程序設計

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

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

3 上位機控制模塊設計

3.1 磁場數據的預處理

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

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

3.2 尋跡控制程序設計

圖3 ARM控制終端圖

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

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

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

表1 磁場信號比較尋跡

3.3 等值線圖繪制

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

4 結 語

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

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

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

［3］張毅剛 .新編MCS－51單片機應用設計 ［M］.哈爾濱：哈爾濱工業大學出版社，2003.

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

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