模塊化蛇形機器人無線控制器開發

陸招蘭， 王坤東， 金 軒(. 三明學院 現代教育技術中心，福建 365004; .上海交通大學 儀器工程系，上海 0040)

?

模塊化蛇形機器人無線控制器開發

陸招蘭1， 王坤東2， 金 軒2
(1. 三明學院 現代教育技術中心，福建 365004; 2.上海交通大學 儀器工程系，上海 200240)

基于無線控制技術和模塊化思想，開發出面向蛇形機器人的控制單元，以克服蛇形機器人龐大關節數量而帶來的通信控制困難。該控制器下位機采用ARM單片機作為內核處理器，利用其PWM波實現兩路舵機的獨立速度控制，基于C1100無線收發芯片實現與上位機的通信，并由鋰電池供電。上位機采用ARM單片機與C1100芯片接收信息，并通過RS232串口連接至計算機。分析了蛇形蜿蜒、直線蠕動步態，提出了實現這些步態的運動控制方法，進行了實驗測試。結果表明，該控制器可靠性、實時性高，便于集成，亦可應用于其他冗余度機器人控制。

蛇形機器人； 控制器； 無線通信； 模塊化

0 引 言

蛇形機器人能適應輪式牽引無法到達的地形，在救災、反恐、偵查中多有應用[1-4]。Hirose開發出第一臺蛇形機器人ACM(Active Cord Mechanism)后，連續推出系列機器人，最新一臺ACM R5為水陸兩棲[5-7]。Gavin從1997年進行蛇形機器人研究，設計并制作了S1～S5五代蛇形機器人[8]。德國GMD國家實驗室也很早進行了蛇形機器人的研究，最近開發了GMD-Snake2[9]。美國NASA研究多關節蛇形機器人，在2000年推出第一代蛇形機器人[10]。國內，上海交通大學開發了我國首臺微小型仿蛇機器人[11]。國防科大也研制了蛇形機器人，安裝有視頻監測器[12]。中科院沈陽自動化所利用可重構的思想研制出了一種模塊化的蛇形機器人，能進行三維運動[13-14]。

在蛇形機器人的研究中，靈巧、有效、可靠的運動機構及相應的控制算法是核心研究內容。但除此之外，構建可靠性高、實時性好的驅動控制系統對于提高機器人的運動可靠性、靈活性和環境適應性也是非常關鍵的。為此，本文基于模塊化思想，利用無線通信技術開發出分布式、單元化的、獨立的無線控制模塊。

1 控制系統設計

1.1 總體控制方案

本研究采用分布式控制方法，每個模塊均具有獨立的控制單元，該控制單元均可接收、發送、處理數據以及控制各自關節的轉動，克服了集中控制的負荷大、走線冗余等問題，具有控制單元負荷小，不存在模塊間通訊的走線問題，實時性高，并對于模塊的自由增減具有靈活性。控制系統組成框圖由圖1所示，系統包括由PC、ARM MCU、CC1101無線模塊構成的上位發送端，以及由ARM MCU、CC1101無線模塊、舵機等構成的下位控制端。下位控制端的電路板放置在每個執行單元上，執行單元通過CC1101無線模塊接收來自上位發送端的信號，在ARM MCU的控制下改變PWM波的占空比，從而控制舵機的轉動方向和速度。

(a) 上位機

(b) 下位機

1.2 分布式下位機驅動控制單元

下位機采用單片機控制，選用意法半導體的STM32F103RBT6，內核為ARM Cortex-M3，用到單片機的SPI接口，2路PWM波輸出，其他資源可以作為后續添加視頻模塊、傳感器模塊的儲備。無線收發模塊采用CC1100，具有體積小、靈敏度高，可編程控制控制數據傳輸率，電流消耗較低，且只需要極少的外部元件就可以組成一個完整的無線傳輸模塊等特點，適合本蛇形機器人設計需要的無線芯片的要求。

驅動單元主要由旋轉舵機和伸縮舵機組成，旋轉舵機產生關節轉動，而伸縮電機經齒輪齒條運動轉換后具有直線伸縮功能，分別采用Futaba S3003 180°舵機和春天SM-S4309R 360°舵機。控制信號為50 Hz的PWM波，在ARM MCU的控制下改變PWM波的占空比，從而控制舵機的轉動方向和速度。

整個蛇形機器人系統是由鋰電池供電。參考鋰電池的輸出功率以及儲電量和2個舵機的功率，可工作6 h左右，滿足實驗設計要求。鋰離子電池輸出電壓為9 V，經穩壓后輸出4.8 V驅動舵機，另一路穩壓后為3.3 V，給單片機提供工作電壓。同時，為了判斷機器人模塊是否處于加電工作狀態，在芯片的輸出端放置了一個LED燈。

2 運動控制算法及程序

上位PC機界面采用C#編寫，主要功能是發送自定義的控制信號給上位機發送電路板，包括蛇形機器人的步態選擇，前進方向以及速度控制等。機器人在每次運動開始前需要初始化：當選擇蜿蜒運動時，需要將機器人的機體曲線初始化為蛇形曲線形式；當選擇直線運動時，需要將機器人的每個執行單元設置為自由伸長狀態，以保證運動的可靠性和精確性。

控制信息的長度為15 Byte，字節0為狀態選擇位，分別對應下位機的定時器時間設置、蜿蜒運動、直線運動、蜿蜒運動初始化、直線運動初始化以及停止6種狀態。若前導碼為0xFF，則對應為定時器時間設置，用以設置機器蛇運動速度的快慢，具體分析見蜿蜒運動的運動控制部分，此時字節1、2為定時器3設置位，用以控制直線運動單步之間間隔的時間，字節3、4為定時器2設置位，用以控制蜿蜒運動兩轉角之間PWM占空比轉換時間；若前導碼為0x01，則為蜿蜒運動初始化控制指令，其中需要控制搖擺關節回到零位，并將直線關節收縮；若前導碼為0x1C，則為蜿蜒運動開始標志，則字節1、2對應模塊1搖擺關節初始化角度，字節3、4對應模塊2搖擺關節初始化角度，……；同理，前導碼為0x0A，0x3B，0x4F分別為機器蛇直線運動初始化、直線運動開始以及運動結束控制指令，控制字節格式如圖2所示，其存儲在RxBuf中。

圖2 控制字節格式

2.1 蛇形蜿蜒運動

動態情況下，在任意時間t，機器人每個搖擺關節的關節角函數[8]：

φi(t)=Asin[ωt+(i-1)β]

(1)

式中：ω=bk，決定關節角的頻率大小；A=-2αsinb1幅值控制參數；β=2b1為周期控制參數。蜿蜒運動的速度由舵機轉速決定，改變PWM脈寬時間，用以改變轉速。當機器人搖擺關節轉角變化時，對應著PWM波占空比的變化；當PWM波從上一個狀態的占空比到下一個狀態的占空比持續的時間不同時，就會使得轉角的平均轉速不同，從而使整個蛇形機器人的前進速度不同。控制舵機的PWM信號時基為20 ms，意味著最少需要20 ms的時間才能使舵機的轉角發生變化。實際工作狀態下，蛇形機器人關節轉角值在時間上不可能連續，只能是一些離散值，離散點越多，運動越逼近真實運動，造成的代價是控制的復雜性。但是，離散點過少會造成機器人運動的卡頓。綜合考慮，將關節轉角函數離散成7個值，每個離散點間相位差為60°。在單片機的控制上，固化一個離散表(此表為離散的關節轉角函數值，分別對應不同占空比的PWM波，采用定時器4捕獲/比較寄存器2作為蜿蜒運動PWM波寄存器)，每過一段時間，讀取表中的值用以產生占空比不同的PWM波，控制搖擺關節的轉動。

運動控制程序的流程圖如圖3所示，單片機設置一個定時器用以控制PWM波占空比的變化間隔。程序中設置了一個函數TM4_Config(u16 pwm1,u16 pwm2)，其中pwm1和pwm2分別為180°舵機和360°舵機的PWM控制值。在蜿蜒運動中，只關心180°舵機的值，而將360°舵機的值設置為0x00。由圖2所示，每個pwm1變量由高低字節2 Byte組成，每個循環周期遍歷一遍控制字節(除前導碼)。在開始進行蜿蜒運動前，首先需要將直線平移關節收縮，然后每個模塊將固化表中對應的值寫入TM4_Config()函數，使機器人身體形成蜿蜒曲線。當啟動蜿蜒運動時，定時器開始工作，定時時間到時，判斷標志位是否是控制字節的末尾(此處將標志位設為status，判斷status是否等于7，若是7，在遍歷到了末尾)，若不是，則將status+1，繼續定時，并同時將得到的高低字節寫入TM4_Config()函數，改變PWM波占空比。若遍歷到了末尾，則將status設置為0，重新開始新一輪的遍歷，同時也將得到的字節寫入函數，控制PWM波的變化。

圖3 蛇形蜿蜒運動控制流程圖

2.2 直線運動

直線運動多為蚯蚓等仿生生物所采用，本研究中引入直線關節后，蛇形機器人的運動步態亦可包含該步態[15]。圖4(a)為一種單駐波運動示意圖，其中紅色加深單元為正在運動的模塊，具體運動過程描述如下：假設向右為機器人前進方向，在第0步為初態；t1時刻，向執行單元1的舵機(此處為360°舵機，以下敘述無特殊說明均為360°舵機)輸入占空比為0.15，頻率為50 Hz PWM波，其余執行單元舵機加電，但是處于停止狀態。執行單元1舵機正轉，帶動齒條產生位移，從而使執行單元1前進一個步距，此時機器人處在第1步；在t2時刻，向執行單元1的舵機輸入占空比為0.05的PWM波，同時向執行單元2的舵機輸入占空比為0.15的PWM波，其余執行單元舵機加電，但是處于停止狀態。執行單元1舵機反轉，執行單元2舵機正轉，此時執行單元1保持在原位不動，而執行單元2向前前進1個步距，機器人處在第2步；在t3、t4，…，t9時刻，每個執行單元中舵機的控制與前述相同。定義向舵機輸入占空比為0.15，頻率為50 Hz PWM波，使舵機正轉，以正電平表示；向舵機輸入占空比為0.05，頻率為50 Hz PWM波，使舵機反轉，以負電平表示，上述運動過程如圖4(b)。從第0～第7步，機器人完成一個前進步態，步距等于每個單元前進距離。重復這個過程，機器人就能完成連續前進運動。通過調節輸入PWM波的占空比，即可以控制機器人的前進方向以及前進速度。

(a) 運動過程

(b) 舵機時序

單駐波運動的控制流程圖如圖5所示。首先直線運動前開始初始化，主要是搖擺關節回到零位，直線平移關節伸長。現以模塊1的軟件控制為例，flag初始值為0，直線運動開始時，打開定時器3，flag加1，當定時時間到時，判斷flag是否為1，若為1，則將PWM波占空比設置為1 ms，使360°舵機反轉，相應的直線平移關節收縮。剛開始運動時，模塊1的flag++后flag值為1，則直線平移關節收縮；然后等待第二次定時時間到，flag再次加1，判斷flag是否等于2，若為2(此時為2)，則將PWM波占空比設置為2 ms，使360°舵機正轉，相應的直線平移關節伸長；然后第三次定時到時，flag為3，PWM波占空比設置為1.5 ms，舵機停轉，直線平移關節靜止。至此，模塊1的直線平移關節在一個運動周期內運動結束，當定時器第8次定時時間到時，將flag設置為0，繼續開始另一個周期的運動。后面的其他模塊跟模塊1的控制流程相同。

圖5 單駐波直線運動控制流程圖

3 運動控制測試與分析

3.1 蛇形蜿蜒運動

利用前面確定的數值A=0.7,ω=1,β=0.9，計算各關節的轉角數據，分為7個離散值，求出各個模塊控制電路板ARM芯片定時器4捕獲比較寄存器1需要遍歷的值，固化轉角數據表如表1所示。

表1 實驗條件下各關節初始轉角

固化轉角數據表內編號為1～7寄存器的值分別對應1～7模塊初始化時，需要寫入捕獲比較寄存器1的值。蛇形機器人每個模塊均會遍歷表1內的寄存器值，蜿蜒運動PWM波占空比變化規律為每7個單位時間循環1次。即蛇形機器人每一個模塊相比與前一個模塊的轉角滯后1位，同時也都隨著時間的推移，每隔單位時間(200 ms)相應向后循環把寄存器數據寫入定時器4捕獲比較寄存器1中改變180°舵機的脈沖占空比(到達第7位后繼續回到第1位取值)，從而形成完整的蜿蜒運動過程。

只要舵機原始的轉角速度與單位時間內通過脈沖占空比改變而得到的轉角速度平均值大致相同或成一定比例，就可以保證蛇形機器人運動的整體平滑性。舉例說明如下：我們設置單位時間為200 ms，表1內相鄰2位寄存器的值造成的PWM波占空比變化對應的轉角差為60°，則實際舵機轉速為375～300°/s，蛇形機器人雖然每一步運行時間與舵機實際轉速有些許出入，但整體上不會有明顯的卡頓現象。在前述數據下，我們進行了蜿蜒運動的實驗，如圖6所示。在蜿蜒運動中，機器人的運動曲線準確，由于機體與地板表面的摩擦系數縱向與軸向幾乎一致，機器人運動時會有些打滑。分析原因是研制的蛇形機器人外殼不具有真實蛇類皮膚表面摩擦系數各向異性的特點。通過在機器人底部增加被動輪的經典方式，可以有效防止打滑出現。

圖6 蜿蜒運動實驗

3.2 直線運動

在直線運動過程中，綜合考慮舵機運行速度、齒輪齒條模數比、齒條長度以及模塊各部分干涉問題，將一個加電周期設置為1.4 s。實驗時在狹窄通道中機器人運行平穩，采用圖4運動學分析的步態時，機器人運動速度約為4.3 mm/s。通過在一個運動周期內增加多個駐波的形式，可以有效提高機器人的前進速度，采用2個駐波時，速度可以達到約8.6 mm/s。在平坦地面上直線運動與在狹窄通道中運動狀態參數類似。直線運動的實驗結果如圖7所示，運動方向為從左向右移動，駐波在圖中用白色方框圈出。可以看出，右邊第1個模塊開始運動，駐波隨后相繼向左傳遞，當駐波到達左邊第一個模塊，1個周期并未結束時，右邊第1個模塊繼續開始運動，從而構成了多駐波直線運動。

圖7 直線運動實驗

4 結 語

超冗余度蛇形機器人控制系統的難點之一就是其龐大的控制系統，模塊化分布式控制方式是其發展的必然趨勢。本研究基于模塊化思想，開發上位機、下位機及其控制驅動模塊的軟硬件系統。研究開發了電源穩壓、無線收發、舵機控制等電路，開發了上位機PC程序、無線收發程序以及舵機控制程序。對蛇形蜿蜒運動及直線蠕動的運動控制進行了算法分析，并實現了這兩種運動形式。運動測試表明，本研究構建的無線控制與驅動系統切實可行、穩定性高。但是，目前各個關節模塊之間仍然沒有橫向信息傳遞，沒有完全發揮分布式控制系統的優勢，另外，一旦其中某一模塊單片機宕機，則會產生運動的不協調，且不利于機器人的自主運動，這些需要在后續的開發中予以改進。

[1] Maruyama H, Ito K. Semi-autonomous snake-like robot for search and rescue[C]//Safety Security and Rescue Robotics (SSRR). 2010 IEEE International Workshop on. Bremen: IEEE, 2010: 1-6.

[2] Meister E, Stepanenko S, Kernbach S. Dynamics and Control of Modular and Self-Reconfigurable Robotic Systems[J]. International Journal on Advances in Intelligent Systems. 2013, 6(1): 66-78.

[3] Fukushima H, Satomura S,Kawai T,etal. Modeling and Control of a Snake-Like Robot Using the Screw-Drive Mechanism[J]. IEEE Transactions on Robotics, 2012, 28(3): 541-554.

[4] 魏 武, 朱紅山. 蛇形機器人在纜索檢測中螺旋滾動步態的研究[J]. 計算機工程與設計, 2011, 32(2): 700-702.

[5] Kouno K, Yamada H, Hirose S. Development of Active-Joint Active-Wheel High Traversability Snake-Like Robot ACM-R4. 2[J]. Journal of Robotics and Mechatronics, 2013, 25(3): 559-566.

[6] Hirose S. Biologically Inspired Robots: Snake-Like Locomotors and Manipulators[M]. London: Oxford University Press, 1993.

[7] Mori M, Hirose S. Three-dimensional serpentine motion and lateral rolling by active cord mechanism ACM-R3[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems, Lausanne, Switzerland, 2002: 829-834.

[8] Miller G S. 13 Snake Robots for Search and Rescue[J]. Neurotechnology for Biomimetic Robots, 2002, 30(2): 271-275.

[9] Klaassen B, Paap K L. GMD-SNAKE2: a snake-like robot driven by wheels and a method for motion control[C]//IEEE International Conference on Robotics and Automation, Detroit, Michigan, 1999: 3014-3019.

[10] John B. Nasa Developing "SNAKEBOT" to Explore and Build in Space[EB/OL]. (2000-10-03) http://www.nasa.gov/centers/ames/news/releases/2000/00_66AR.html

[11] Liu H, Yan G, Ding G. Research on the locomotion mechanism of snake-like robot[C]//Proceedings of 2001 International Symposium on Micromechatronics and Human Science, Nagoya, Japan, 2001: 183-188.

[12] 周旭升, 潘獻飛, 譚紅力. 一種蛇形機器人的研制[J]. 機器人, 2002, 24(7): 684-687.

[13] 郁樹梅, 馬書根, 李 斌. 蛇形機器人步態產生及步態分析[J]. 機器人, 2011, 33(3): 371-378.

[14] Wang Z, Ma S, Li B,etal. Experimental study of passive creeping for a snake-like robot[C]//IEEE/ICME International Conference on Complex Medical Engineering, 2011: 382-387.

[15] 簡小剛, 王葉鋒, 楊鵬春, 基于蚯蚓蠕動機理的仿生機器人研究進展[J]. 中國工程機械學報, 2012, 10(3): 359-363.

法治是實現教育現代化的可靠保障。全面深化改革、全面依法治教如車之兩輪，共同推動教育現代化不斷向前發展。

社會主義核心價值觀是興國之魂。我們要實現的教育現代化是具有中國特色的現代化，社會主義核心價值觀是中國特色的內核，是培養社會主義建設者和接班人的核心要義。

摘自《教育部袁貴仁部長在2015年全國教育工作會議上的講話》

Development of Modularized Wireless Controller for Snake-like Robot

LUZhao-lan1,WANGKun-dong2,JINXuan2
(1. Modem Education Technology Center, Sanming University, Sanming 365004, China; 2. Department of Instrument Engineering, Shanghai Jiaotong University, Shanghai 200240, China)

This research developed a controller for snake-like robot based on wireless technology and modularization. This design overcame the communication difficulty come from its large number of articulations. ARM MCU was used as kernel processor with PWM module to control two steering engine. C1100 chip realized the wireless communication with the upper computer composed of ARM MCU, C1100. The upper computer was connected with the PC by RS232. Snake-like serpentine, linear peristalsis were analyzed. The control methods were proposed to realize these gaits, and the tests were performed to verify these methods. It was demonstrated that this controller had a high reliability, and it was easy to integrated into the robot. Wireless technology and modularization design have made a foundation for the multi-gaits of the snake-like robot, and these could be implemented into the other redundant robots.

snake-like robot; controller; wireless communication; modularizing

2014-04-21

上海科技啟明星計劃項目(13QA1402200)資助

陸招蘭(1966-)，女，上海人，實驗師，主要研究方向為通信與網絡技術。Tel.:13960568830；E-mail:csc5711@163.com

王坤東(1978-)，男，陜西長安人，副教授，主要從事仿生機器人技術研究。Tel.:021-34207229；E-mail: kdwang@sjtu.edu.cn

TP 242

A

1006-7167(2015)02-0062-05