畢業(yè)設計（論文）雙足智能機器人的設計與實現(xiàn)

《畢業(yè)設計（論文）雙足智能機器人的設計與實現(xiàn)》由會員分享，可在線閱讀，更多相關《畢業(yè)設計（論文）雙足智能機器人的設計與實現(xiàn)（45頁珍藏版）》請在裝配圖網(wǎng)上搜索。

1、 1 引言 機器人是作為現(xiàn)代高新技術的重要象征和發(fā)展結果，已經(jīng)廣泛應用于國民生產(chǎn)的各個領域，并正在給人類傳統(tǒng)的生產(chǎn)模式帶來革命性的變化，影響著人們生活的方方面面。對于步行機器人來說，它只需要模仿人在特殊情況下(平地或己知障礙物)完成步行動作，這個條件雖然可以使機器人的骨骼機構大大降低和簡化，但也不是說這個系統(tǒng)就不復雜了，其步行動作一樣是高度自動化的運動，需要控制機構進行復雜而巧妙地協(xié)調各個關節(jié)上的動作。 雙足機器人的研究工作開始于上世紀60年代末，只有三十多年的歷史，然而成績斐然。如今已成為機器人領域主要研究方向之一。最早在1968年，英國的Mosher．R試制了一臺名為“Rig”的操縱

2、型雙足步行機器人[1]，揭開了雙足機器人研究的序幕。該機器人只有踝和髖兩個關節(jié)，操縱者靠力反饋感覺來保持機器人平衡。1968~1969年間，南斯拉夫的M.Vukobratovic提出了一種重要的研究雙足機器人的理論方法，并研制出全世界第一臺真正的雙足機器人。雙足機器人的研制成功，促進了康復機器人的研制。隨后，牛津大學的Witt等人也制造了一個雙足步行機器人，當時他們的主要目的是為癱瘓者和下肢殘疾者設計使用的輔助行走裝置。這款機器人在平地上走得很好，步速達0.23米/秒。日本加藤一郎教授于1986年研制出WL－12型雙足機器人。該機器人通過軀體運動來補償下肢的任意運動,在軀體的平衡作用下，實現(xiàn)了

3、步行周期1.3秒，步幅30厘米的平地動態(tài)步行。 法國Poitiers大學力學實驗室和國立信息與自動化研究所INRIA機構共同開發(fā)了一種具有15個自由度的雙足步行機器人BIP2000，其目的是建立一整套具有適應未知條件行走的雙足機器人系統(tǒng)。它們采用分層遞解控制結構，使雙足機器人實現(xiàn)站立、行走、爬坡和上下樓梯等。此外，英國、蘇聯(lián)、南斯拉夫、加拿大、意大利、德國、韓國等國家，許多學者在行走機器人方面也做出了許多工作。 國內(nèi)雙足機器人的研制工作起步較晚。1985年以來，相繼有幾所高校進行了這方面的研究并取得了一定的成果。 哈爾濱工業(yè)大學自1985年開始研制雙足步行機器人，迄今為止已經(jīng)完

4、成了三個型號的研制工作。其中HIT－Ⅲ為12個自由度，實現(xiàn)了靜態(tài)步行和動態(tài)步行，能夠完成前／后行、側行、轉彎、上下臺階及上斜坡等動作。目前，該校正致力于功能齊全的雙足機器人HIT－Ⅳ的研制工作，新機器人包括行走機構、上身及髖部執(zhí)行機構，初步設定32個自由度。國防科技大學也進行了這方面的研究。在1989年研制成功了一臺雙足行走機器人，這臺機器人具有10個自由度，能完成靜態(tài)步行和動態(tài)步行。國防科技大學還將工業(yè)機器人的軌跡示教方法用到了兩足步行機器人的步態(tài)規(guī)劃中，形成了步行機器人的步態(tài)示教規(guī)劃技術。 值得一提的是，北京理工大學研制成功我國首例擬人機器人BRH-01，該機器人身高1.58米，體重76

5、公斤，具有32個自由度，每小時能夠行走1公里，步幅0.33米。除了能打太極拳，這個機器人還會騰空行走，并能根據(jù)自身的平衡狀態(tài)和地面高度變化，實現(xiàn)未知路面的穩(wěn)定行走。它在系統(tǒng)集成、步態(tài)規(guī)劃和控制系統(tǒng)等方面實現(xiàn)了重大突破，標志著我國雙足機器人研究已經(jīng)跨入世界先進行列。 國內(nèi)其它院校如清華大學、上海交通大學、北京航空航天大學等高等院校也在近幾年投入了相當?shù)娜肆?、物力，進行智能雙足機器人的研制工作。我校也開始了這方面的研究工作，不過我們的工作處于研究的初級階段。 為了促進機器人技術在我國的發(fā)展，全國各地尤其是部分高校舉辦了各種類型的機器人大賽。中國機器人大賽是由中國自動化學會機器人競賽工作委員會和

6、科技部高技術研究發(fā)展中心主辦的一個全國性的賽事。其中最為引人矚目的舞蹈機器人項目，足球機器人項目就是為了促進雙足步行機器人的發(fā)展而設立的。由于步行機器人的實現(xiàn)目前還存在很多技術難題，前幾屆全國機器人大賽基本上是以輪式機器人為主，步行機器人參賽才被列入議程不久。由此可見，雙足步行機器人的發(fā)展還有一段很長的路要走。研制雙足步行機器人的重要內(nèi)容是對其進行建模分析、步態(tài)規(guī)劃、控制分析等?；谏鲜鲈?，本課題擬進行雙足步行機器人的基本設計與研究，研制具有高度穩(wěn)定性的雙足步行機器人平臺，為研究得后續(xù)工作和進一步的擬人機器人研制奠定基礎，所設計的機器人以ATmega1280單片機微控制器為核心技術芯片，完成

7、行走、下蹲、倒地、起身、前滾翻、后滾翻等簡單動作，同時通過一些必要的傳感器組件完成其對前方道路情況的探測和判斷,以達到避障效果。 2 雙足智能機器人總體分析 要設計和開發(fā)一個步行機器人，首先應該對其進行總體分析和設計，確定步行機器人的功能、基本結構和系統(tǒng)配置等。 2.1 功能定位 這款機器人不僅能夠滿足實驗室科研的需要，而且應該是一款很適合學生參與、研究、學習的機器人，能夠滿足互相學習的需要?，F(xiàn)在希望制作出一個成本相對較低的機器人，研制雙足步行機器人能夠滿足這方面的要求。 基于上面幾點的考慮，決定開發(fā)一款雙足智能機器人，首先使其能

8、夠完成一些基本動作，既開始時先走3步、立正、然后臥下（身體向前）、向前翻跟斗3次，再起立、向前走3步、立正、然后臥下（身體向后）、再向后翻跟斗2次、再起立、然后以輕快步履走向終點、要在指定5分鐘或少于指定時間內(nèi)完成所有動作，及要走到終點，同時要求其對前方道路情況的探測和判斷，用以避障。 2.2 自由度的配置 機器人可以有很多不同類型的關節(jié)，有線性的、旋轉的、滑動的或球鉸鏈型的。人體的髖關節(jié)和踝關節(jié)類似小運動范圍的球關節(jié)，能夠使人靈活行走，實現(xiàn)前后左右拐彎等行走動作，方向靈活，但需要控制的自由度多、難度大，所以在機器人結構中不常用。但是單純用旋轉關節(jié)來實現(xiàn)多自由度的設計勢必給空間布置和安裝增

9、加了難度，而且同時又考慮到關節(jié)驅動件驅動能力、運動效率和設計成本，以及設計審美性等因素。該雙足智能機器人設計的目的是要實現(xiàn)擬人下肢多自由度得平穩(wěn)行走，在實現(xiàn)這個功能的前提下為降低設計的難度，我們按照目前世界上各研究機構普通采用的下肢10個自由度的關節(jié)配置形式，來實現(xiàn)行走功能所必須的各關節(jié)自由度分布，具體自由度配置為單腿髖關節(jié)2個，膝關節(jié)1個，踝關節(jié)2個。髖關節(jié)用于擺動腿，實現(xiàn)邁步，并起到了輔助平衡作用。膝關節(jié)主要用來調節(jié)重心的高度，及改變擺動腿的著地高度，使之與地形相適應。踝關節(jié)用來和髖關節(jié)相配合實現(xiàn)支撐腿的移動，以及調整與地面的接觸狀態(tài)。 基于鄭元芳博士的理論，來規(guī)劃自己所要設計的雙足機器

10、人的自由度。為了實現(xiàn)這款雙足步行機器人的穩(wěn)步行走，可以規(guī)劃其運動過程，假設機器人行走步驟：先走3步，立正，然后臥下，向前翻跟斗3次，再起立，向前走3步，立正，然后臥下（身體向后），再向后翻跟斗2次，再起立，然后走向終點,遇到障礙物，能向左拐。則從機器人步行步驟可以看出：機器人向前邁步，髖關節(jié)的前向旋轉自由度起作用，同時配合踝關節(jié)可實現(xiàn)支撐腿的移動；這樣，所設計的雙足步行機器人有10個個自由度，每條腿5個自由度，即踝關節(jié)有2個自由度，膝關節(jié)有1個自由度，髖關節(jié)有2個自由度，包括前向、后向自由度。其結構圖見下圖1。 圖1自由度的分配 踝關節(jié)和髖關節(jié)采用十字交叉結構。十字交叉關節(jié)又叫萬向聯(lián)軸

11、節(jié)，常用于汽車方向盤底盤轉向機構，可以實現(xiàn)互相垂直方向的兩個自由度運動，這種機構可以減少關節(jié)耦合程度和非線性。 研究表明：至少要有髖、膝、踝這三個關節(jié)，雙足穩(wěn)定行走才能成為可能。髖、膝、踝關節(jié)對于穩(wěn)定有效的行走來說是不可少的。髖關節(jié)用于擺動腿，實現(xiàn)邁步并使上軀體前傾或者后仰，使之在步行過程中起輔助平衡作用。膝關節(jié)主要用來調整重心的高度、并用來調整擺動腿的著地高度，使之與地形相適應。踝關節(jié)和步行功能有關，它用來和髖關節(jié)相配合實現(xiàn)支撐腿和上軀體的移動，而且還可以調整腳掌與地面的接觸狀態(tài)。如果踝關節(jié)被固定，將會缺乏與地面觸覺感知的能力，前后向穩(wěn)定性很差。 2.3系統(tǒng)結構設計 根據(jù)確定的自由度配

12、置方案以及選用的微型伺服馬達、傳感器、控制板，設計機器人的零件。本著結構簡單、盡量采用通用零件、外形美觀等原則，對機器人的機構及外觀進行優(yōu)化。 2.3.1 布置對稱性 本文設計的機器人機構,其主要特點有以下幾點： (1) 步行運動中普遍存在結構對稱性。Goldberg[3]等人研究了步行運動中的對稱性，發(fā)現(xiàn)機身運動的對稱性和腿機構的對稱性之間存在相互關系。在單足支撐階段，對稱性的機身運動要求腿部機構也是對稱的；在雙足支撐階段，機身對稱性運動未必需要腿部機構的對稱性，除非有額外的約束條件。根據(jù)這一點，我們在結構設計時也采用對稱性布置。 (2) 框架的設計有效地利用了舵機的尺寸大小，并使舵

13、機的活動范圍能盡量符合各關節(jié)的活動范圍。 (3) 采用多關節(jié)型結構。行走機構能實現(xiàn)平地前后、平地側行、轉彎、上下臺階等功能。 (4) 整個結構采用1mm的鋁合金(LY12)鈑金材料。 (5) 由于機器人的各關節(jié)是用舵機驅動為了減小機器人的體積、減輕重量,機器人的結構做成是框架型的?？蚣艿脑O計有效地利用了舵機的尺寸大小,并使舵機的活動范圍能盡量符合各關節(jié)的活動范圍。 實物圖見下圖2所示 。 圖2 狹窄足雙足機器人機械結構簡圖 2.3.2 驅動方案的選擇 實現(xiàn)行走的基本問題是對機器人各關節(jié)位置、速度的伺服控制和協(xié)調控制。如果把連桿以及關節(jié)想象為機器人的骨骼，那么驅動器就

14、起到肌肉的作用，它通過移動或轉動連桿來改變機器人的構型。驅動器必須有足夠的功率對負載加速或者減速。同時，啟動器本身要精確、靈敏、輕便、經(jīng)濟、使用方便可靠且易于維護。 目前機器人的驅動方式主要有液壓驅動、氣壓驅動和電機驅動三種方式。液壓驅動方式雖然具有驅動力矩大、響應速度快等特點，但是成本高、重量大、工藝復雜，且有發(fā)熱問題。氣壓驅動易于高速控制，氣動調節(jié)閥的制造精度要求沒有液壓元件高、無污染，但是位置和速度控制困難，并且其工作穩(wěn)定性差，壓縮空氣需要除水。液壓驅動與氣壓驅動不能實現(xiàn)試驗系統(tǒng)自帶能源的目標，直接決定了這兩種驅動方式難于應用到雙足機器人系統(tǒng)中。電機驅動具有成本低、精度高、可靠且維修方

15、便等特點，容易和計算控制系統(tǒng)相連接，目前的雙足機器人大都采用這種方式。 舵機是一種最早應用在航模運動中的動力裝置，是一種微型伺服馬達，它的控制信號是一個寬度可調的方波脈沖信號，所以很方便和數(shù)字系統(tǒng)進行接口。只要能產(chǎn)生標準的控制信號的數(shù)字設備都可以用來控制舵機，比如PLC、單片機和DSP等。而且舵機體積緊湊、便于安裝、輸出力矩大、穩(wěn)定性好、控制簡單。根據(jù)所需的驅動力矩要求和性價比方面的考慮，我們決定選用輝盛公司生產(chǎn)的12公斤大扭力全金屬齒輪舵機。該類型舵機價格適中且規(guī)格參數(shù)能夠滿足雙足機器人的各項性能要求。因此在綜合了開銷，性能等一系列因素后我們選擇了MG945。經(jīng)過嘗試，我們得出了舵機轉動角

16、度與脈沖的對應關系：90度對應的差不多是1.6ms的脈沖，也就是說一個1.6ms的脈沖MG945就轉到90度，而0度對應的是0.8ms脈沖，180度對應的是2.1ms脈沖。 2.3.3 避障傳感器原理 智能關節(jié)機器人為了能在未知或時變環(huán)境下自主地工作.應具有感受作業(yè)環(huán)境和規(guī)劃自身動作的能力。為此.必須提高機器人對當前感知環(huán)境的快速理解識別及實時避障的能力。實時避障是實現(xiàn)智能化機器人自主工作能力的關鍵技術.也是國內(nèi)外智能機器人近期發(fā)展的一個熱點.其顯著特征是具有傳感器信息反饋.可以實現(xiàn)很好的智能行為。 紅外傳感器是一種比較有效的接近覺傳感器，經(jīng)常被國內(nèi)外學者應用在多關節(jié)機器人避障系統(tǒng)中，用

17、來構成大面積機器人“敏感皮膚”，覆蓋在機器人手臂表面，可以檢測機器人運行過程中的各種物體。傳感器發(fā)出的光的波長大約在幾百納米范圍內(nèi)，是短波長的電磁波。紅外傳感器具有以下特點：不受電磁波的干擾、非噪聲源、可實現(xiàn)非接觸性測量。另外，紅外線（指中、遠紅外線）不受周圍可見光的影響，故可在晝夜進行測量。 同聲納傳感器相似，紅外線傳感器工作處于發(fā)射/接收狀態(tài)。這種傳感器由同一發(fā)射源發(fā)射紅外線，并用兩個光檢測器測量反射回來的光量。由于這些儀器測量光的差異，它們受環(huán)境的影響非常大，物體的顏色、方向、周圍的光線都能導致測量誤差。但由于發(fā)射光線是光而不是聲音，可以希望在相當短的時間內(nèi)獲得較多的紅外線傳感器測量值

18、。測距范圍較近，大致為30cm以內(nèi)。 3 雙足智能機器人步態(tài)規(guī)劃 步態(tài)規(guī)劃是雙足智能機器人研究中的一項重要工作，步態(tài)規(guī)劃的好壞將直接影響機器人行走過程中的穩(wěn)定性、所需驅動力矩的大小以及姿態(tài)的美觀性等多個方面，同它也直接影響到控制方法及其實現(xiàn)的難易程度。 3.1步態(tài)規(guī)劃的基本原則 步態(tài)規(guī)劃[4]工作既可以在關節(jié)空間內(nèi)也可以在直角坐標空間內(nèi)進行，無論在哪個空都有很多不同的規(guī)劃方法。事實上，許多方法可以在兩種空間內(nèi)通用。直角坐標空間的軌跡規(guī)劃比較實用和直觀，可以得到一條可控且可預知的路徑，人們很容易看到機人末端執(zhí)行器的軌跡，但計算復雜不易規(guī)劃。而且，難以確保不存在奇異點(發(fā)生機器軌跡穿入自身

19、，及軌跡到達工作空間之外等)。顯然，對于指定的像直線運動那樣的徑，必須在直角坐標空間內(nèi)進行規(guī)劃，才能生成直線。如果不要求機器人跟蹤指定的徑，那么關節(jié)空間的軌跡規(guī)劃更容易計算從而產(chǎn)生出實際的運動。實際上，所有用于節(jié)空間軌跡規(guī)劃的方法都可用于直角坐標空間的規(guī)劃。最根本的差別在于，直角坐標間軌跡規(guī)劃必須反復求解逆運動學方程來計算關節(jié)角。對于關節(jié)空間軌跡規(guī)劃，規(guī)劃數(shù)生成的值就是關節(jié)量，而直角坐標空間軌跡規(guī)劃函數(shù)生成的值是機器人末端執(zhí)行器位姿，它們需要通過求解逆運動學方程才能轉化為關節(jié)量。雙足步行機器人的步態(tài)規(guī)劃，是指機器人行走過程中其各組成部分運動軌跡的劃，比如說，腳掌何時離開地面、擺動相中整個腳掌在

20、空中的軌跡、何時落地等。關鍵之就在于所規(guī)劃的軌跡必須滿足零力矩點（ZMP）穩(wěn)定條件，否則，機器人不能穩(wěn)定步行步態(tài)規(guī)劃要解決的問題主要有: 1. 保證機器人本體不和環(huán)境或者自身發(fā)生干涉,從而導致無法實現(xiàn)預定的軌跡。 2. 保證機器人的穩(wěn)定性。機器人的穩(wěn)定性問題一直是困擾兩足步行機器人發(fā)展的重要問題，由于各個關節(jié)間的藕合作用，很難設計出理想的ZMP軌跡[5]。現(xiàn)在使用的步態(tài)規(guī)劃方法主要有如下幾種: (1) 基于實驗的規(guī)劃方法 這種規(guī)劃方法基于力學的相似原理，基本過程如下:讓人模仿機器人行走(如果機器人有幾個自由度，那么人在模仿行走的時候也盡量只動相應的關節(jié))，同時對此人的行走過程進行正面和

21、側面的錄像，然后對這些錄像進行分析，得到此人在步行過程各個主要關節(jié)的角度變化與時間的函數(shù)，然后根據(jù)力學相似原理把這些函數(shù)相似地推廣到機器人的關節(jié)變化上。 (2) 基于能量原理的規(guī)劃方法 這種方法來源于一個生物學假設:人經(jīng)過千百萬年的進化，其行走方式是能量消耗最低的，而且還能保持步行的穩(wěn)定性。如果機器人也能滿足這個假設，則其行走方式將與人一樣或很接近。根據(jù)能耗最小原則可以建立一個變分方程，并最終得到機器人的軌跡方程。 (3) 基于力學穩(wěn)定性的規(guī)劃方法 在機器人行走過程中，其ZMP點必須落在某個區(qū)域范圍之內(nèi)，只有這樣才能保證步行機器人穩(wěn)定地行走。實現(xiàn)方法有兩種: a. 計算出理想的ZMP

22、軌跡，然后推導出各個關節(jié)的運動函數(shù)以實現(xiàn)理想行走。 b. 先大致規(guī)劃出雙足和軀干的運動軌跡，然后進行ZMP計算，最后選出穩(wěn)定性最好的結果作為控制方程。 相比前一種方法，后一種可以較快地得到規(guī)劃結果。在保證了機器人基本性能的前提下，盡量減少控制的工作量，降低成本。所以本文將采用第二種方法，結合三次樣條插值和機器人的逆運動學規(guī)劃進行機器人步態(tài)的參數(shù)化設計。 3.2步態(tài)規(guī)劃的具體方法 合適的步態(tài)設計是機器人實現(xiàn)動態(tài)行走的關鍵。在計算各關節(jié)軌跡之前，首先要建立機器人所在的空間坐標系。 坐標系的 x軸指向機器人的正面,y軸由右側指向左側,z軸垂直向上，原點0位于后腳完全落地時踝關節(jié)在地面上的

23、垂直投影處。當機器人沿著直線往前走時，由于只考慮前向運動，雙腳和 腰部在側方向( y軸)上的位置是不變的，因為設定條件為機器人在水平地面行走，且腳面抬起高度為0，所以z軸位置不變。因此，僅僅討論在 x平面內(nèi)的軌跡。機器人所在的空間坐標系如圖3 所示 。 圖3 機器人的空間坐標系 雙足機器人完整的步行過程包括三個階段[6] (1) 起步階段:由初始的雙腿并立靜止狀態(tài)變化到行走狀態(tài)，一條腿向前跨出了半步距離，髖部速度從零上升到恒定值； (2) 整步階段：兩條腿交替地向前跨出一步距離，髖部速率保持不變； (3) 落步階段：后腿向前跨出半步，落在與另一條腿并行的部位，髖部速度減

24、少到零，恢復成雙腿并立靜止狀態(tài)。 主要討論整步的軌跡計算。前向運動的規(guī)劃步驟如下： (1) 確定步速和步長； (2) 設定初始參數(shù)，計算踝關節(jié)和髖關節(jié)軌跡 ； (3) 根據(jù)踝關節(jié)和髖關節(jié)軌跡計算膝關節(jié)軌跡； (4) 計算ZMP軌跡； (5) 改變參數(shù)值，返回第 2步； (6) 選取具有最大穩(wěn)定性的軌跡。 步態(tài)規(guī)劃的流程圖如圖4所示。 圖4 雙足機器人的步態(tài)規(guī)劃流程圖 雙足步行機器人行走控制的關鍵是根據(jù)踝關節(jié)和髖關節(jié)的運動軌跡確定膝關節(jié)的運動軌跡。在腳面抬起高度為0的情況下，膝關節(jié)的軌跡由步幅唯一決定，而步幅和步速則可唯一確定出髖關節(jié)的軌跡，其

25、步態(tài)周期圖如圖5所示。 圖5 步態(tài)周期圖 因為單支撐和雙支撐是一個交替的過程，雙腿支撐需要有一定時間來保證ZMP點由后腳移動到前腳，所以我們?nèi)‰p支撐的時間間隔為20%個周期。 從上圖可以看出在一個完整的步態(tài)周期里包括了兩個雙支撐和兩個單支撐時間段，雙支撐占20%個周期，單支撐占80%個周期。對于單腿來說整個周期里只有一個擺動周期，占40%個周期，支撐周期由一個單支撐和兩個雙支撐組成，占60%個周期。 根據(jù)機器人自身結構特點，在行走時采用“平行步態(tài)[7]”，如圖所示。機器人采用這種步態(tài)行走時，兩腳的落腳痕跡是相互平行的，實現(xiàn)這種步態(tài)行走的機器人對其自由度要求較少，但對穩(wěn)定平衡提出了較

26、高的要求。 4 控制系統(tǒng)的硬件設計 4.1 主控制板 主板選用南陽理工學院自動化實驗室自行開發(fā)的Arduino MEGA控制板，該主板是以ATmega1280單片機微控制器為核心,包括電源模塊、USB下載模塊、ISP下載模塊、電機驅動模塊、外部晶振以及各種I/O接口。主板如圖6所示。 圖6 主控制板 為了滿足對機器人主控制平臺的需求，我們設計了基于Arduino開發(fā)平臺的機器人主控制板。機器人主控制板的原理圖如附錄1所示。進行PCB設計時的技巧和方法如下： (1) 傳輸線拐角要采用45角，以降低回損。 (2) 要采用絕緣常數(shù)值按層次嚴格受控的高性能絕緣電路板。這種方

27、法有利于對絕緣材料與鄰近布線之間的電磁場進行有效管理。 (3) 要完善有關高精度蝕刻的PCB設計規(guī)范。要考慮規(guī)定線寬總誤差為+/-0.0007英寸、對布線形狀的下切(undercut)和橫斷面進行管理并指定布線側壁電鍍條件。對布線(導線)幾何形狀和涂層表面進行總體管理，對解決與微波頻率相關的趨膚效應問題及實現(xiàn)這些規(guī)范相當重要。 (4) 突出引線存在抽頭電感，要避免使用有引線的組件。高頻環(huán)境下，最好使用表面安裝組件。 (5) 對信號過孔而言，要避免在敏感板上使用過孔加工(pth)工藝，因為該工藝會導致過孔處產(chǎn)生引線電感。如一個20層板上的一個過孔用于連接1至3層時，引線電感可影響4到

28、19層。 (6) 要提供豐富的接地層。要采用模壓孔將這些接地層連接起來防止3維電磁場對電路板的影響。 (7) 要選擇非電解鍍鎳或浸鍍金工藝，不要采用HASL法進行電鍍。這種電鍍表面能為高頻電流提供更好的趨膚效應此外，這種高可焊涂層所需引線較少，有助于減少環(huán)境污染。 (8) 阻焊層可防止焊錫膏的流動。但是，由于厚度不確定性和絕緣性能的未知性，整個板表面都覆蓋阻焊材料將會導致微帶設計中的電磁能量的較大變化。一般采用焊壩(solder dam)來作阻焊層。 4.2硬件電路總體結構 (1) 主芯片 Arduino MEGA控制板采用的是Atmega1280 [9]AVR芯片（如圖7所示

29、）， 圖7 Atmega1280內(nèi)部結構圖 該芯片具有如下特點： 128字節(jié)的系統(tǒng)內(nèi)可編程Flash(具有在寫的過程中還可以讀的能力，即RWW)、 4K字節(jié)的EEPROM、8K字節(jié)的SRAM。 54個通用I/O口、32個通用工作寄存器、實時時鐘RTC。 6個靈活的具有比較模式和PWM功能的定時器/計數(shù)器（T/C）。 4個USART、面向字節(jié)的兩線接口TW、SPI串行端口I。 16通道10位ADC（具有可選的可編程增益）。 具有片內(nèi)振蕩器的可編程看門狗定時器。 與IEEE 1149.1規(guī)范兼容的JTAG測試接口（此接口同時還可以用于片上調試） 六種可以

30、通過軟件選擇的省電模式。空閑模式時CPU停止工作，而SRAM、 T/C、SPI端口以及中斷系統(tǒng)繼續(xù)工作；掉電模式時晶體振蕩器停止振蕩，所有功能除了中斷和硬件復位之外都停止工作，寄存器的內(nèi)容則一直保持；省電模式時異步定時器繼續(xù)運行，以允許用戶維持時間基準，器件的其他部分則處于睡眠狀態(tài)； ADC噪聲抑制模式時PU和所有的I/O模塊停止運行，而異步定時器和AD繼 續(xù)工作，以減少ADC轉換時的開關噪聲。 Standby模式時振蕩器工作而其他部分睡眠，使得器件只消耗極少的電流，同時 具有快速啟動能力；擴展Standby模式則允許振蕩器和異步定時器繼續(xù)工作。 器件是以Atmel的高密度非易

31、失性內(nèi)存技術生產(chǎn)的。片內(nèi)ISP Flash可以通過SPI接口、通用編程器或引導程序多次編程。引導程序可以使用任何接口來下載應用程序到應用Flash存儲器。在更新應用Flash存儲器時引導Flash區(qū)的程序繼續(xù)運行，實現(xiàn)RWW操作。通過8位RISC CPU與系統(tǒng)內(nèi)可編程的Flash集成在一個芯片內(nèi)，ATmega1280為多嵌入式控制應用提供了靈活而低成本的方案。下圖8為ATmega1280實物圖。 圖8 ATmega1280芯片 上述這些特點使采用AVR單片機的應用系統(tǒng)不僅具有運行速度快、結構簡單、功能強大的特點，而且具有高可靠性和良好的經(jīng)濟型。Arduino軟件可以直接進行C/C++

32、語言程序開發(fā)，繞開了復雜難懂的匯編，從而可以進行更有效率的系統(tǒng)設計開發(fā)工作。 (2) 外部晶振 Arduino MEGA控制板采用外部16MHZ晶振。通過MXTAL1和MXTAL2與主芯片相連接。晶振兩端分別使用22pF電容與GND連接。 (3) ISP下載模塊 ISP(in-system programming，在線系統(tǒng)編程)，是指電路板上的空白器件可以編程寫入最終用戶代碼，而不需要從電路板上取下器件，已經(jīng)編程的器件也可以用ISP方式擦除或再編程。 其優(yōu)點在于，不需要編程器就可以進行單片機的實驗和開發(fā)，單片機芯片可以直接焊接到電路板上，只要留出和上位機接口的這個串口，就可以實現(xiàn)芯片

33、內(nèi)部存儲器的改寫，而無需再取下芯片。ISP是Flash存儲器的固有特性（通常無需額外的電路），F(xiàn)lash幾乎都采用這種方式編程。因此通過ISP可以對芯片進行熔絲位的配置及Flash編程,EPROM的燒錄等操作。 (4) USB下載模塊 Arduino MEGA控制板采用USB下載方式，通過FT232RL芯片與主芯片進行通信連接及程序的下載，實現(xiàn)USB轉串口功能。 硬件接口電路如下圖9所示。 圖9 USB下載接口電路 (5) 各種I/O接口 Arduino MEGA控制板包括數(shù)字I/O接口，ADC模擬量接口，PWM接口，通信接口。其I/O口有：端口A(PA7..PA0) 、端口

34、B(PB7..PB0) 、端口C(PC7..PC0) 、端口D(PD7..PD0)、端口E(PE7..PE0) 、端口F(PF7..PF0) 、端口G(PG4..PG0) 。 除端口G之外其余端口為8 位雙向I/O 口，并具有可編程的內(nèi)部上拉電阻。其輸出緩沖器具有對稱的驅動特性，可以輸出和吸收大電流。作為輸入使用時，若內(nèi)部上拉電阻使能，則端口被外部電 路拉低時將輸出電流。復位發(fā)生時端口為三態(tài)。端口也可以用做其他不同的特殊功能。 (6) 電源模塊 Arduino MEGA控制板采用電池供電，一路通過LM2576-5.0進行電壓轉換，轉換后的電壓為5.0V，供主芯片以及各種傳感器使用，另外一

35、路直接從電源引出到直流電機驅動模塊以供電機工作。 LM2576的特點： 最大輸出電流：3A; 最大輸入電壓：LM2576為40V的，LM2576HV至60V; 輸出電壓：3.3V，5V或12V的，15V和形容詞（可調），可選; 振東頻率：52kHz; 轉換效率：75％?88％（即不同的輸出電壓差的效率）; 控制方式：脈寬調制; 工作溫度范圍：-40℃+125℃ 工作模式：低功耗/正常模式，可以是外部的控制; 工作模式控制：TTL電平兼容; 所需外部元件：僅四個（不可調）或六個（可調）; 保護裝置：熱停機和電流限制; 包裝：為了- 220或TO

36、- 263。 電池供電電壓理論上為7-40V DC，但由于電源模塊采用的電容耐壓值均為25V，所以盡量使用25V以下電池進行，電路圖如圖10所示。 圖10 電源電路 (7) 整體電路結構圖如下圖所示。 硬件系統(tǒng)以ATmega1280為控制核心，通過在PC上把編程軟件編譯好的程序經(jīng)過FT232下載到ATmega1280上。主芯片通過10路PWM控制雙足共10個伺服舵機完成行走和前后翻動作，并利用紅外避障傳感器實現(xiàn)避障功能。最終實現(xiàn)需要完成的各項動作。硬件結構圖如圖11所示。 圖11 硬件結構圖 4.3避障傳感器設計 紅外傳感器主要是為了機器人避障用，具體工作過程：LM567的中

37、心頻率為38kHz，為紅外發(fā)射管提供38kHz的脈沖信號，使紅外發(fā)射管發(fā)出38kHz的光波；當有障礙物時，紅外接收管輸出38kHz頻率的壓值變化信號（無障礙物時輸出高電平），由LM741電壓放大后輸入LM567，當LM567接收到38kHz頻率的數(shù)據(jù)時，輸出低電平，從而判斷出前方的障礙物，如下圖12所示。 圖12紅外傳感器檢測過程 如下圖13所示，CKr是單片機輸出的38KHz的方波（使用其硬件PWM），通過調節(jié)PWM的占空比，從而實現(xiàn)調節(jié)感知障礙物距離的功能。U2是紅外線接收頭，使用紅外線接收頭具有相當?shù)撵`敏度，可以檢測到200mm以外的物體，確保機器人不會撞向障礙物。 圖13

38、 紅外避障傳感器 4.4 舵機選型 本設計所選舵機為輝盛公司生產(chǎn)的12公斤大扭力全金屬齒輪舵機。該類型舵機價格適中且規(guī)格參數(shù)能夠滿足雙足機器人的各項性能要求。因此在綜合了開銷，性能等一系列因素后我們選擇了MG945。經(jīng)過嘗試，我們得出了舵機轉動角度與脈沖的對應關系：90度對應的差不多是1.6ms的脈沖，也就是說一個1.6ms的脈沖MG945就轉到90度，而0度對應的是0.8ms脈沖，180度對應的是2.1ms脈沖。 MG945參數(shù)如表1所示。 表1 MG945參數(shù)表 參數(shù)名稱 工作速度 工作電壓 扭矩 重量 尺寸 參數(shù)值 0.24秒/60度 3.0V-7.2V

39、 12kg/cm 62g 40.8X20X37.3mm 本設計需要使用10個舵機，其接口為硬件開發(fā)板PWM1-PWM10接口，接口引腳見圖16總電路圖所示。 一個舵機內(nèi)部包括了一個小型直流馬達、一組變速齒輪組、一個反饋可調電位器和一塊電子控制板。其中，高速轉動的直流馬達提供了原始動力，帶動變速齒輪組，使之產(chǎn)生高扭力的輸出，齒輪組的變速比愈大，舵機的輸出扭力也愈大，也就是說越能承受更大的重量，單轉動的速度也愈低。 一個微型伺服馬達是一個典型閉環(huán)反饋系統(tǒng)，其原理可由下圖14所示： 圖14 舵機的工作原理示意圖 減速齒輪組由馬達驅動，其輸出端帶動一個線性的比例電位器作位置檢

40、測，該電位器把轉角坐標轉換為一比例電壓反饋給控制單元，控制單元將其與輸入的控制脈沖信號比較，產(chǎn)生糾正脈沖，并驅動馬達正向或反向地轉動，使齒輪組的輸出位置與期望值相符，從而達到舵機精確定位的目的。 舵機的控制信號是脈沖位置調制信號，周期一般為20ms，當方波的脈沖寬度改變時，舵機轉軸的角度發(fā)生變化，角度變化與脈沖寬度的變化成正比。一般舵機的輸出軸轉角與輸入信號的脈沖寬度之間的關系可用下圖15所示。 圖15 控制信號脈沖寬度與輸出軸轉角的關系圖 伺服馬達的瞬時運動速度是由其內(nèi)部的直流馬達和變速齒輪組的配合決定的，除了和電壓有關外，還和轉動的角度和負載有關。但其平均運動速度可通

41、過分段停頓的控制方式來改變，例如，需要讓舵機的輸出端從目前的A位置轉到B位置，可以在兩個位置之間，選取n個中間位置（C、D、E、F、G……），然后，讓舵機從A開始，經(jīng)過CDEFG……最終轉到B；通過選取中間位置的數(shù)量和每個位置的停頓時間，可以比較準確地控制舵機的轉速曲線。 4.5整體電路原理圖 根據(jù)本章以上的設計選用最終的電路圖為下圖16所示。 圖16 系統(tǒng)整體電路圖 5 控制系統(tǒng)的軟件設計 為了實現(xiàn)模型樣機的實驗調試，在硬件基礎上進行了系統(tǒng)軟件設計，通過對實際機械系統(tǒng)運動控制理論研究，規(guī)劃了一套簡便的調試方案以驗證樣機設計的合理性，并進行對預訂步態(tài)的相應關節(jié)控制調試和可行性驗

42、證。 5.1 Arduino-0017簡介 Arduino是一個能夠讓你的計算機更好地感知和控制外世界的物理計算（physical computing）平臺，它基于一個源碼開放的微控制器電路板，并提供了相應的集成開發(fā)環(huán)境來進行軟件的開發(fā)。Arduino能夠被用來進行交互式對象（interactive object） 的開發(fā)，例如從一組開關或者傳感器中獲得用戶輸入，或是控制一組燈光、馬達或其他物理輸出設備。用Arduino開發(fā)的項目即能夠單獨運行，也能夠同運行在計算機上的其他軟件（如Flash、Processing或Max/MSP）進行通信，從而共同完成相應的任務。Arduino具有自己的編

43、程語言，它是基于Processing的多媒體編程環(huán)境Wiring（另一個非常類似的物理計算平臺）的一種實現(xiàn)。 Arduino-0017軟件主界面包括4部分，從上到下依次為：菜單、編輯窗口、信息窗口和提示欄，其操作主界面如圖17所示?；趧?chuàng)用CC開放原始碼的電路圖設計，基于創(chuàng)用CC開放原始碼的程式開發(fā)環(huán)境，也可依需求自己修改，遵照姓名標示。Arduino可使用ICSP線上燒入器，將[bootloader]燒入新的IC晶片。可依據(jù)官方電路圖，簡化Arduino模組，完成獨立運作的微處理控制?？珊唵蔚嘏c感測器，各式各樣的電子元件連接(EX:紅外線，超音波，熱敏電阻，光敏電阻，伺服馬達，…等)支援多

44、樣的互動程式(EX: Macromedia Flash、Max/Msp、VVVV、PD、C、Processing、、、等)，使用低價格的微處理控制器(ATMEGA 8-168) NT$120~NT$150，USB介面，不需外接電源，另外有提供9VDC輸入。應用方面，利用Arduino，突破以往只能使用滑鼠，鍵盤，CCD等輸入的裝置的互動內(nèi)容，可以更簡單地達成單人或多人游戲互動。 Arduino管腳說明： (1) Digital I/O數(shù)字式輸入/輸出端共1～13。（13個數(shù)字輸入輸出口DIO，其中10個PWM輸出，可做模擬輸出口使用AO）l (2) Analog I/O模擬式輸入/輸出端

45、共0～5。（5個模擬輸入口AI） (3) 支持USB接頭傳輸數(shù)據(jù)及供電(不需額外電源)。 (4) 支持ICSP在線刻錄功能。支援TX/RX端子。 (5) 支持外部ADC電壓基準AREF端子輸入。 (6) 支持6組PWM端子(Pin11,Pin10,Pin9,Pin6,Pin5,Pin3)。 (7) 輸入電壓：USB接口供電，無須外部供電。5V～12V DC外部供電。l (8) 輸出電壓：5V DC輸出。 (9) 采用Atmega168-20PI/PU，最新的Arduino 2009支持Atmega328-20PI/PU。 圖17 Arduino-0017 操作界面 5.

46、2 程序設計 在程序設計過程中，首先需要對端口進行設置，以及進行開發(fā)板的選擇。創(chuàng)建、命名、保存后，對熔絲位進行設置。下面為PonyProg2000 軟件中的“熔絲位和程序及數(shù)據(jù)存儲器鎖定位”設置窗口問題說明。 進行熔絲位設置時應注意一些問題： (1) 在SPI 串行編程模式下SPIEN 熔絲位不可訪問； (2) CKOPT 熔絲功能由CKSEL 位設置決定； (3) BOOTSZ1-0 默認為最大Boot 大小 ； (4) 當對RSTDISBL熔絲位編程，并行編程使用其他熔絲位或執(zhí)行其他編程模式； (5) 對于默認時鐘源，SUT1-0的默認值給出最大的啟動時間； (6) C

47、KSEL3-0的默認設置導致了片內(nèi)RC振蕩器運行于1MHz CKOPT、CKSEL3.0、SUT1.0 為系統(tǒng)時鐘配置（包括喚醒脈沖和復位延時時間選擇設定）熔絲位。 設置好熔絲位后就可以進行程序的編寫、調試、編譯，具體程序見附錄。程序設計采取模塊化設計思想，按照圖18示的流程圖，每個模塊完成各自指定的功能。 5.2.1主程序 主程序按照規(guī)則要求完成對各個功能模塊的邏輯調用和時序的調用開始時在可以躲在障礙物的情況下，先走3步、立正、然后臥下（身體向前）、向前翻跟斗3次，再起立、向前走3步、立正、然后臥下（身體向后）、再向后翻跟斗2次、再起立、然后以輕快步履走向終點、要在指定5分鐘或少于

48、指定時間內(nèi)完成所有動作，及要走到終點。當系統(tǒng)開始后進行初始化，機器人呈現(xiàn)立正狀態(tài)。緊接著紅外傳感器檢測是否有障礙物，如有障礙物則執(zhí)行避障子程序，否則執(zhí)行正常的行走子程序（其中行走子程序包括前進和前后翻動作），然后判斷是否走完所需要的動作，如完成則程序結束，否則返回紅外檢測處開始執(zhí)行，程序重復執(zhí)行最終完成設定的動作。其主程序流程圖見圖18所示。 圖18 主程序流程圖 5.2.2避障程序 避障是系統(tǒng)重要的一項功能，當紅外傳感器檢測到有障礙物時則開始執(zhí)行避障子程序，在子程序中判斷是哪個方向有障礙物，如果前方有障礙物且機器人執(zhí)行前進則要向左邁一步，然后接著檢測是否有障礙物，如有則重復剛才的

49、動作直到?jīng)]有障礙物，然后跳出避障子程序繼續(xù)執(zhí)行主程序。傳感器檢測的狀態(tài)和要執(zhí)行如何避障的策略見避障子程序流程圖，如圖19所示。 圖19 避障程序流程圖 5.2.3行走程序 機器人要完成的行走任務為：先走3步、立正、向前翻跟斗3次、向前走3步、再向后翻跟斗2次、然后以輕快步履走向終點。行走的各階段都有其標志位，當進入行走子程序時，開始判斷此時要執(zhí)行的動作是前進、向前翻還是向后翻，判斷完后執(zhí)行相應的動作并返回主程序并繼續(xù)檢測障礙物。其行走子程序流程圖如下圖20所示。 圖20 行走程序流程圖 通過程序的編寫和聯(lián)機調試并最終在場地上行走，機器人能夠很好的完成所設定的任務，當加入適當?shù)?/p>

50、障礙物時，機器人也能夠檢測到并最終避開障礙物完成設計任務。 結束語 隨著科技的發(fā)展，雙足智能步行機器人的研究登上了一個新的臺階。ASIMO、SDR-4X的研制成功標志著雙足步行機器人已經(jīng)從研究領域邁進了家用領域。而ROBOCUP雙足步行機器人比賽的順利舉行，又為大學、科研機構進行雙足機器人的研究開辟了一個新的方向。我國雙足機器人研究重點放在大型的雙足步行機器人上，為此，研制開發(fā)能夠參加國際性比賽的雙足步行機器人，提高我國雙足機器人理論研究和樣機研制水平，特別是機構學、運動學和動力學等基礎理論的研究水平，為國爭光，十分有意義。為使理論成果具有更強的

51、實用性，課題研究過程中做了大量機器人資料搜集和整理工作，在此基礎上構建起雙足機器人總體研究框架，并做了一系列雙足機器人基礎性研究工作，包括動力學建模、步態(tài)規(guī)劃和控制分析等。具體內(nèi)容包括： (1).通過分析國內(nèi)外雙足機器人的樣機研制狀況和理論研究水平，掌握國內(nèi)外雙足機器人的研究現(xiàn)狀及發(fā)展趨勢，明確了雙足步行機器人的研究意義，確定課題的研究方向和研究內(nèi)容。 (2).確定了課題的研究任務是設計可以完成智能行走，下蹲、倒地、起身、前滾翻、后滾翻等動作的雙足機器人，在此基礎上確定其所需關節(jié)、自由度，并對其進行結構分析。 為了讓設計出的機器人完成預定的行走，下蹲、倒地、起身、前滾翻、后滾翻等動作，在

52、設計和調試過程中會遇到一些問題，我們就需要針對其穩(wěn)定性，平衡性，易調性，避障性，干擾等方面進行簡要分析和改進。有一種隱患會導致機器人在行進過程中反復出現(xiàn)斷電又接通的情況[11]，使得機器人不能正常的連貫的工作，這多數(shù)是因為焊接的電路板上的零件有松動，主要是電源連接方面。這就要求細致的焊接板子，防止一切斷路和短路情況的發(fā)生。 參考文獻 [1] 陳立新,兩足步行機器人步態(tài)及運動穩(wěn)定性分析[D].現(xiàn)代機械,1995 (3):30-35 [2] 謝濤,徐建峰.仿人機器人的研究歷史、現(xiàn)狀及展望[J].機器人,2002(7):26-35 [3] J.Knani,

53、Dynamic modelling of flexible robotic mechanisms and adaptive robust control of trajectory computer simulation–Part I[J].Applied Mathematical Modelling ,2002:16-22 [4] 張學允,雙足步行機器人動態(tài)步行研究[D].哈爾濱工業(yè)大學,2001:34-45 [5] Ishida T,KurokiY.Sensor system of a small biped entertainment robot[J].Advanced Roboti

54、cs ,2004 (10) [6] 劉志遠,兩足機器人的動態(tài)行走研究[D].哈爾濱工業(yè)大學,1991:37-56 [7] 譚冠政,朱劍英,尉忠信.國內(nèi)外兩足步行機器人研究的歷史、現(xiàn)狀及發(fā)展趨勢[J].機器人,1992, (3):32-45 [8] 吳 玉,姜復光.機器人工程導論[M].哈爾濱工業(yè)大學出版社,1999:123-145 [9] 徐元呂,工業(yè)機器人[M].中國輕工業(yè)出版社.1999:234-256 [10] 周伯英,工業(yè)機器人設計[M].北京機械土業(yè)出版社.2005:124-137 [11] 嚴冰,李曉翠.基于雙足機器人的設計實現(xiàn)以及相關改進[J].科協(xié)論壇. 2009(

55、5) [l2] 李元宗,機器人學講義[D].太原工業(yè)大學. 2001:83-115 [13] 蹼良貴,紀各剛.機械設計[M].高等教育出版社. 1996:126-148 [14] 余達太,馬香峰.工業(yè)機器人應用[M].冶金工業(yè)出版社. 2003:78-95 [15] 蔡白興,機器人學[M].清華大學出版社.2002:168-245 [16] 劉鴻文,材料力學[M].高等教育出版社.2003:55-68 附錄 (1) .主板電路圖 (2). 程序清單 #include #define number

56、 6 #define first 4 MegaServo servo[number]; int a=13,b=13; int pos1=90,pos2=90,pos3=90 ,pos4=90,pos5=90,pos6=90,k=0; int be=5; int tm=10; int flag=0; void setup() { int i; for(i=0;i

57、); to_pos4(0,1,3,4,2,64,63,68,68,89,b);//68-70 // to_pos3(0,1,3,4,70,68,68,68,13); //偏右 to_pos1(2,5,89,95,a); /* to_pos1(2,5,60,105,13); to_pos4(0,1,3,4,2,68,68,68,68,89,14);//70-68 to_pos1(2,5,89,95,13);*/ /* to_pos1(2,5,60,106,15);//107 to_angle(2,89,1); t

58、o_pos3(0,1,3,4,68,65,64,64,14); to_pos1(2,5,92,93,15);*/ } void first_step21() { to_pos1(2,5,60,105,a); to_pos4(0,1,3,4,2,64,63,70,68,90,b);//68-70//0.65-64 // to_pos3(0,1,3,4,70,68,68,68,13); //偏右 to_pos1(2,5,89,95,a); /* to_pos1(2,5,60,105,14); to_po

59、s4(0,1,3,4,2,70,70,70,68,89,12);//0.70-68 to_pos1(2,5,89,95,14);*/ } void second_step20() { to_pos1(2,5,103,60,a); to_pos4(0,1,3,4,5,107,109,108,105,94,b); to_pos1(2,5,89,95,a); } void second_step() { to_pos1(2,5,103,65,a); to_pos4(0,1,3,4,5,108,109,108,105,94,16)

60、;//0-106-104 // to_pos3(0,1,3,4,106,109,108,105,13); //偏右 to_pos1(2,5,89,95,a); /* to_pos1(2,5,102,65,13); to_pos4(0,1,3,4,5,106,109,108,105,94,18); to_pos1(2,5,89,95,13);*/ /*to_pos1(2,5,105,65,15); to_angle(5,95,5); to_pos3(0,1,3,4,103,104,105,102,15); to_pos1(2,5,

61、91,95,20); */ } void init_end() { to_pos1(2,5,105,65,14); to_pos3(0,1,3,4,87,88,91,89,13); to_pos1(2,5,89,94,15); } void three_step() { first_step1(); second_step(); first_step1(); init_end(); } /***************************初始化角度*********************************/

62、 void init_start() { to_pos6(0,1,2,3,4,5,87,90,89,91,89,94,10); // to_pos2(1,2,4,88,89,89,10); //to_pos2(0,3,5,87,91,94,10); } /*********************************************************************/ void loop() { if(k==0) { init_start(); delay(500); three_step(); zheng

63、fang(); three_step(); delay(100); daofang(); first_step1(); k=1; } second_step(); first_step1(); } void zhengfang() { flag=1; int i; for(i=0;i<3;i++) { init_start(); to_pos3(0,3,1,4,52,129,115,60,10);//12-10

64、 to_pos3(0,3,1,4,44,139,135,40,10); to_pos3(0,3,1,4,24,159,155,20,10); to_pos3(0,3,1,4,4,179,175,0,10); to_pos1(3,4,150,0,3); to_pos1(3,4,30,173,3); to_pos1(3,4,0,173,5);

65、 to_pos1(0,1,30,173,3); to_pos1(0,1,135,2,3); to_pos1(0,1,162,2,5); to_pos3(0,3,1,4,87,91,90,89,10);//11-10 /* to_pos1(0,3,

66、4,179,10); //較好，速度慢 to_pos1(1,4,175,0,10); // to_pos3(0,3,1,4,4,179,175,0,12); to_pos1(3,4,150,0,3); to_pos1(3,4,30,175,3); to_pos1(3,4,0,175,3); to_pos1(0,1,30,173,3); to_pos1(0,1,135,2,3);