機械臂控制系統(tǒng)的設(shè)計

1、機械臂控制系統(tǒng)的設(shè)計1 引言近年來，隨著制造業(yè)在我國的高速發(fā)展， 工業(yè)機器人技術(shù)也得到了迅速的發(fā) 展。根據(jù)負載的大小可以將機械臂分為大型、中型、小型三類。大型機械臂主要 用于搬運、碼垛、裝配等負載較重的場合；中小型機械臂主要用于焊接、噴漆、 檢測等負載較小的場合。 隨著國外工業(yè)機器人技術(shù)的不斷發(fā)展， 尤其是一些中小 型機器人，它們具有體積小、質(zhì)量輕、精度高、控制可靠的特點，甚至研發(fā)出更 為輕巧的控制箱，可以在工作區(qū)域隨時移動，這樣大大方便了工作人員的操作。 在工業(yè)機器人的應用中最常見的是六自由度的機械臂。 它是由 6 個獨立的旋轉(zhuǎn)關(guān) 節(jié)串聯(lián)形成的一種工業(yè)機器人，每個關(guān)節(jié)都有各自獨立的控制系統(tǒng)。

2、2 機械臂硬件系統(tǒng)設(shè)計2.1 機械臂構(gòu)型的選擇要使機器臂的抓持器能夠以準確的位置和姿態(tài)移動到給定點， 這就要求機器 人具有一定數(shù)量的自由度。 機器臂的自由度是設(shè)計的關(guān)鍵參數(shù)， 其數(shù)目應該與所 要完成的任務相匹配。 為了使安裝在雙輪自平衡機器人上的機械臂能夠具有完善 的功能，能夠完成復雜的任務， 將其自由度數(shù)目定為 6 個，這樣抓持器就可以達 到空間中的任意位姿， 并且不會出現(xiàn)冗余問題。 在確定自由度后， 就可以合理的 布置各關(guān)節(jié)來分配這些自由度了。由于計算數(shù)值解遠比封閉解費時，數(shù)值解很難用于實時控制，這樣，后 3 個關(guān)節(jié)就確定了末端執(zhí)行器的姿態(tài)， 而前 3 個關(guān)節(jié)確定腕關(guān)節(jié)原點的位置。 采用

3、這種方法設(shè)計的機械臂可以認為是由定位結(jié)構(gòu)及其后面串聯(lián)的定向結(jié)構(gòu)或手腕 組成的。這樣設(shè)計出來的機器人都具有封閉解。 另外，定位結(jié)構(gòu)都采用簡單結(jié)構(gòu) 連桿轉(zhuǎn)角為 0或 90的形式，連桿長度可以不同， 但是連桿偏距都為 0，這樣的 結(jié)構(gòu)會使推倒逆解時計算簡單。定位機構(gòu)是涉及形式主要有以下幾種：SCAR型機械臂，直角坐標型機械臂， 圓柱坐標型機械臂，極坐標型機械臂，關(guān)節(jié)坐標型機械臂等。SCAR機械臂是平面關(guān)節(jié)型，不能滿足本文對機械臂周邊3維空間任意抓取 的要求；直角坐標型機械臂投影面積較大， 工作空間??； 極坐標方式需要線性移 動，機械臂如需較大的工作空間， 則臂長較長； 和其他類型相比關(guān)節(jié)型機械臂在

4、其工作空間內(nèi)干涉是最小的， 是一種較為優(yōu)良的結(jié)構(gòu)。 所以初步確定本文機械臂 構(gòu)型為關(guān)節(jié)型。2.2 臂桿長度的確定機械臂的臂桿設(shè)計如表2-1所示：表2-1機械臂臂桿長度臂體名稱大臂L1小臂L2機械手長度(mm5505001502.3機械臂結(jié)構(gòu)設(shè)計關(guān)節(jié)結(jié)構(gòu)方案為了便于機械臂關(guān)節(jié)的模塊化涉及和簡化結(jié)構(gòu)， 本設(shè)計使用電機直接連接減 速器，減速器連接臂體連接結(jié)構(gòu)。圖 2-1是關(guān)節(jié)結(jié)構(gòu)動力傳遞方案。圖2-1關(guān)節(jié)結(jié)構(gòu)動力傳遞方案使用這種聯(lián)接方式因中間零件少，故形變量與回程間隙都較小，且能保持較 高的結(jié)構(gòu)剛度。2.4關(guān)鍵部件的選型關(guān)節(jié)負載的估算各關(guān)節(jié)的動態(tài)參數(shù)是驅(qū)動元件的選擇和關(guān)節(jié)傳動零件選擇的重要依據(jù)。由機

5、器人動力學相關(guān)知識可知完整的機器人動力學方程為：式中一般使用靜力學方法和動力學方法計算機器人的動力參數(shù)，速度較低的機 械，在運行過程中，慣性引起的動載荷較小，一般使用靜力學方法，忽略C和F的影響。而對于運行速度較高機械，其動載荷也較大，即C項的影響較大，甚至超過靜載荷；且粘滯摩擦也較大，同時考慮靜載荷和動載荷，需使用動力學計算。 本文的設(shè)計要求是一款可以安裝在全向移動平臺上的輕型機械臂，對關(guān)節(jié)的旋轉(zhuǎn)速度要求不高，因此估算機械臂力矩時采用靜力學方法。圖2-2機械臂受力簡圖估計關(guān)節(jié)力矩之前，首先假設(shè)每個關(guān)節(jié)的重力作用集中在中心， 將連桿的重 量均分于各關(guān)節(jié)，機械臂受力簡圖如圖 2-2所示，使用靜力

6、學方法計算關(guān)節(jié)所 受力矩的最大值。六自由度機械臂三維靜態(tài)仿真圖如圖2-3所示：圖2-3三維靜態(tài)仿真圖關(guān)節(jié)驅(qū)動系統(tǒng)電機的選型機械臂的驅(qū)動系統(tǒng)，有三種基本類型，即電動驅(qū)動、液壓驅(qū)動和氣動驅(qū)動， 也可以根據(jù)需要組合成為復合式的驅(qū)動系統(tǒng)。(1)電機驅(qū)動目前機械臂上使用最多的一種驅(qū)動方式是電動驅(qū)動， 它利用各種電機產(chǎn)生的 力和力矩，直接或通過機械傳動裝置來驅(qū)動執(zhí)行機構(gòu)。這類系統(tǒng)效率比液壓驅(qū)動 和氣動驅(qū)動系統(tǒng)高，且電源方便，所以在機器人中得到了廣泛的應用。(2) 液壓驅(qū)動 液壓驅(qū)動的主要優(yōu)點是功率密度大。 液壓缸也可直接作為臂體的一部分， 因 而結(jié)構(gòu)緊湊，剛性好。由于液壓油液的不可壓縮性，系統(tǒng)的固有頻率較

7、高，快速 響應好，可實現(xiàn)頻繁平穩(wěn)的變速和換向。 液壓系統(tǒng)易于實現(xiàn)過載保護， 動作平穩(wěn)、 耐沖擊、耐振動、防爆性好。(3) 氣動驅(qū)動 氣動驅(qū)動系統(tǒng)通常由氣缸、氣閥、氣罐和空壓機組成，其特點是氣源方便、 結(jié)構(gòu)簡單、 造價較低、維修方便。與液壓驅(qū)動系統(tǒng)相比， 同體積條件下功率較小， 也難以進行速度控制，多用于中、小負荷且精度要求不高的機器人控制系統(tǒng)中。 綜上，本設(shè)計決定使用電動驅(qū)動方式為機械臂提供動力， 步進電機為驅(qū)動電機。驅(qū)動系統(tǒng)減速器的選型 結(jié)合上文， 本文將使用步進電機為驅(qū)動電機為機械臂提供動力， 結(jié)合各關(guān)節(jié) 受力和機械臂關(guān)節(jié)傳動機構(gòu)組合方式， 應在驅(qū)動電機和機械臂關(guān)節(jié)間安裝減速器 做扭矩適配

8、，降低輸出軸的速度，增大輸出扭矩。一般行星齒輪減速器、蝸輪蝸 桿減速器、諧波減速器、齒輪減速器等可以和步進電機適配。1、行星齒輪減速器 行星齒輪減速器通常由一個或者多個外部齒輪圍繞著一個中心齒輪旋轉(zhuǎn)， 就 像行星繞著太陽公轉(zhuǎn)一樣。 在工作狀態(tài)中多個行星齒輪協(xié)同工作， 因而承載能力 大，屬純扭矩傳動，工作平穩(wěn)。單級行星齒輪減速器的減速比一般較小，需要增 加減速比時只需增加行星輪系的級數(shù)即可，而整體體積變化較小。2、蝸輪蝸桿減速器 蝸輪蝸桿減速器的傳動比大，一般為 10-80 ，也可以達到 80 以上。此外， 蝸輪蝸桿減速器機械結(jié)構(gòu)緊湊、熱交換性能好、工作平穩(wěn)、噪聲小、具備機械自 鎖能力，安全性高

9、。3、諧波減速器 波發(fā)生器， 柔輪，剛輪是諧波減速器的三大部分， 諧波齒輪減速器傳動結(jié)構(gòu) 簡單，減速比高，同時嚙合的齒數(shù)多，運行平穩(wěn)、傳動承載力大，齒側(cè)間隙小， 傳動精度高，傳動誤差只有普通圓柱齒輪傳動的 1/4 左右，傳動空程小，適用 于反向轉(zhuǎn)動， 在機器人領(lǐng)域有著廣泛應用。 但對柔輪材料有較高的強度要求， 工 藝復雜。4、齒輪減速器 圓柱齒輪減速機構(gòu)為定傳動比齒輪機構(gòu)， 其傳動準確， 平穩(wěn)高效， 傳動功率 范圍和速度范圍大， 廣泛用于各種儀器儀表中， 但其制造和安裝精度要求高， 高減速比時結(jié)構(gòu)較為復雜，體積一般較大綜上，初步去確定使用諧波齒輪減速器，減速比大，傳動精度高，體積小巧， 輸入軸

10、與輸出軸軸線重合，可很方便地與步進電機組合安裝成為機械臂關(guān)節(jié)的一 部分，同時便于機械臂的模塊化設(shè)計。 本文將采用Harmonic Drive CSF-mini系 列組合型諧波減速器，其中腰關(guān)節(jié)采用型號為CSF-14-100-2XH-F ;肘關(guān)節(jié)俯仰和肘關(guān)節(jié)旋轉(zhuǎn)采用 CSF-11-100-2XHF，腕俯仰采用CSF-8-100-2XH-F。 電機驅(qū)動器的選型雖然步進電機廣泛地應用于各行各業(yè)，但步進電機并不能像普通的直流電機 那樣通過控制輸入的等效電壓就可以驅(qū)動和調(diào)速。它必須利用電子電路，將直流電變成分時多相時序控制電流，用這種電流為步進電機供電，步進電機才能正常 工作。常見的有單片機I/O 直接

11、控制，步進電機驅(qū)動芯片、運動控制卡。1、單片機I/O直接控制方式使用單片機內(nèi)部的鎖存器、計數(shù)/定時器，和并行I/O 接口，可以實現(xiàn)對步 進電機的控制，脈沖環(huán)形分配器的功能由單片機系統(tǒng)實現(xiàn)完成，通過軟件中斷方 式實現(xiàn)步進電機的變速控制，改變通電順序則可改變轉(zhuǎn)向。2、步進電機專用驅(qū)動芯片步進電機專用驅(qū)動芯片一般集成度較高，外圍電路簡單,一般有ENABLESTEP和DIR三個輸入端，ENABLE為使能端，使能有效時方可驅(qū)動步進電機； STEP為脈沖輸入，輸入一個脈沖，即可驅(qū)動步進電機產(chǎn)生微動；DIR為方向，改變DIR邏輯電平即可換向。3、運動控制卡驅(qū)動控制通過計算機可直接控制步進電機，運動控制卡是專

12、用于步進電機控制的PC插卡，是應對復雜系統(tǒng)的控制而出現(xiàn)的，一般可同時控制十幾臺甚至幾十臺步進 電機的運動，一般價格很高。綜上，本設(shè)計將使用步進電機專用驅(qū)動芯片來驅(qū)動步進電機。 其中肩關(guān)節(jié)和 肘關(guān)節(jié)俯仰有自鎖需求，使用東芝 THB7128 3A 128高細分步進電機專用驅(qū)動芯 片驅(qū)動，其他軸選用A4988微步驅(qū)動器。表2-3步進電機驅(qū)動芯片相關(guān)參數(shù)型號最咼耐壓電流使用溫度自鎖性能細分模式THB712840VDC 3.3A-40-85 (C)半流鎖定1-128 (8 種)A498835VDC 2A-20-85 (C)無1-16 (5 種)傳感器的選型本文將使用步進電機和諧波齒輪減速器為機械臂提供動

13、力，步進電機只需要 通控制驅(qū)動脈沖的數(shù)量，即可簡單實現(xiàn)較高精度的定位，并使工作物在精確地停在目標位置。步進電機以細分后的步距角為基本單位進行定位。以兩相電機為例，其步距角為1.8。，使用1/16細分方式進行驅(qū)動，那么每給驅(qū)動器一個脈沖步 進電機轉(zhuǎn)子旋轉(zhuǎn)的角度為角度=1.8 *1/16=0.1125。，轉(zhuǎn)子旋轉(zhuǎn)一周需要脈沖數(shù) 為360 /0.1125 =3200,需要旋轉(zhuǎn)到其他任意角度的計算方式與上式相同。本文使用限位開關(guān)的型號為Omron微動開關(guān)SS-5擺桿型限位開關(guān)。表2-4微動限位開關(guān)參數(shù)型號按鍵力度接觸規(guī)格觸發(fā)精度耐熱溫度SS-51.47N1C （雙投型） 0.5mm85下位機的選型對于

14、機器臂控制，需要對多臺電機進行聯(lián)動控制。為了實現(xiàn)多臺電機之間的 通信和控制，必須建立一套數(shù)據(jù)通信系統(tǒng)來完成主計算機與各運動控制單元間的 數(shù)據(jù)交換?；诂F(xiàn)場總線的分布式控制技術(shù)能夠解決這些問題。但常見的分布式控制系統(tǒng)又有USB總線，SERCO總線，RS-485總線和CAN總線等這幾種。本 設(shè)計將采用RS-485總線來實現(xiàn)機械臂的分布式控制。本設(shè)計選用了 TI公司的2000系列DSPTMS320LF2407乍為控制單元。其時 鐘頻率可達40MHz具有高速的處理能力，片內(nèi)資源豐富，特別是它特有兩個內(nèi) 置事件管理器模塊（EVA EVB。通過JTAG接口可以方便的對DSP進行全速的 在系統(tǒng)調(diào)試仿真。TM

15、S320LF2407勺電源電壓為3.3V，正常下作電流為80m A左 右，抗干擾能力較強。關(guān)節(jié)控制器硬件電路關(guān)節(jié)控制器是以DSP芯片為核心，芯片本身及其外圍電路的性能直接決定了系統(tǒng)的性能。故芯片的選擇及其外圍電路的設(shè)計，也就顯得十分的重要。下面將通過單個模塊電路的方式分別介紹控制器硬件電路。（1）電源電路通過開關(guān)電源，接入B0505LS模塊產(chǎn)生穩(wěn)定的的5V電壓作為TPS7333芯 片的供電電壓，管腳8做為2407的上電復位信號。管角5,6通過濾波電容輸 出作為2407的供電電壓（3.3V）。如圖2-5。圖2-4電源電路時鐘電路TMS320LF2407勺時鐘源可以來自外部有源晶振也可以用晶體，禾

16、用內(nèi)部振 蕩器。一般經(jīng)常使用外部時鐘輸入，因為使用外部時鐘時，時鐘的精度高、信號 比較穩(wěn)定，外部時鐘電路和鎖相環(huán)電路如圖 2-6所示。圖2-5時鐘電路（3） JTAG接口電路仿真接口電路如圖 2-7所示.目標層次的TI調(diào)試標準使用 5個標準的IEEE1149.1(JTAG)信號(TRST TCK TMS TDI、TDO 和兩個 TI 擴展口( EMUOEMU1。JTAG目標器件通過專用的仿真端口支持仿真， 此端口由仿真器直接訪問 并提供仿真功能。 JTAG 接口電路為仿真器與微機的接口電路，便于系統(tǒng)進行在 線調(diào)試。圖 2-6 JTAG 電路外接SRAM&路TMS320LF240最多可尋址64K

17、的外部程序空間和64K的外部數(shù)據(jù)空間。由于控制算法的需要，本系統(tǒng)需擴充外部 RAM TMS320LF240片內(nèi)的Flash 可用作程序存儲器，但在開發(fā)階段使用 Flash 作為程序存儲器極為不便，因為 每一次程序的修改都需要對 Flash 進行清除、擦除和編程操作，而且進行 CCS 調(diào)試時只能設(shè)置硬件斷點，故從調(diào)試的角度考慮，應擴充程序RAM這里用的是 CY7C1021V3芯片，它是64K*16bit的SRAM存取時間為15ns,故不需要插入 等待周期，可保證系統(tǒng)全速運行。圖 2-8為外接SRA M擴展電路圖。圖 2-7 SRAM 擴展電路圖(5) 編碼器處理電路 增量式編碼器信號處理電路如圖

18、 2-9 所示。圖 2-8 增量式編碼器信號處理電路(6) 霍爾接近開關(guān)電路本設(shè)計選用 A31443E 常開型霍爾接近開關(guān)。 其接法如圖 2-10 ，提供電壓為 5V,由于輸出采用了集電極開路門，必須通過10K的上拉電阻接到5V電源上。 當磁源的某一極與霍爾傳感器的距離達到一定范圍以內(nèi)時， 輸出低電平， 否則輸 出高電平，不需要外接放大電路。一套關(guān)節(jié)控制器將采用 3 支霍爾接近開關(guān)。 HALL1、HALL2 分別固定在關(guān)節(jié)控制器運動的極限位置， 其信號通過 IOPE5、IOPE6 不斷查詢。 HALL3 用于絕對零位檢測，采用中斷的方式。圖 2-9 霍爾接近開關(guān)的接法2.5 機械臂的模塊化設(shè)計

19、機械臂的大小臂體和關(guān)節(jié)在整個機械臂中具有高度的相似性， 同時機械臂是 機電一體化的典型， 其主體結(jié)構(gòu)和聯(lián)接結(jié)構(gòu)都有一定的復雜性， 而采用模塊化設(shè) 計思想，可以一定程度上簡化設(shè)計流程， 只需對不同的應用對象進行少量修改便 可完成組合適配。旋轉(zhuǎn)關(guān)節(jié)的設(shè)計機械臂的基本單元有旋轉(zhuǎn)關(guān)節(jié)和俯仰關(guān)節(jié)， 其結(jié)構(gòu)具有相似性， 本文重點介 紹旋轉(zhuǎn)關(guān)節(jié)的設(shè)計。旋轉(zhuǎn)關(guān)節(jié)包含有電機、減速器、編碼器、制動器以及其他附 件，本文使用步進電機直連諧波減速器的驅(qū)動方式， 使用限位微動開關(guān)確定機械 臂初始定位零點， 以計步進電機已發(fā)脈沖數(shù)為關(guān)節(jié)相對旋轉(zhuǎn)角度參考， 是一個開 環(huán)的運動控制系統(tǒng)，動力傳遞鏈路為：電機 - 波發(fā)生器-

20、柔輪- 剛輪輸出軸。 機械臂的旋轉(zhuǎn)關(guān)節(jié)模塊在運行過程中會受到來自機械臂末端的彎矩， 因此需要對 輸出軸做軸向和徑向卸荷， 減少輸出軸的負載， 保證系統(tǒng)的剛度， 延長使用壽命。 一般來說， 一根軸需要兩個支點， 每個支點由一個或一個以上的軸承組成， 每組 軸承間有以下三種常用的配置方法。1、雙支點各單向固定 這種軸承配置常用兩個反向安裝的圓錐滾子軸承或角接觸球軸承， 兩個軸承 各限制軸向一個方向的軸向移動。 這種配置方式軸向移動限制比較精確、 也便于 調(diào)整軸承的預緊程度。 另外深溝球軸承也可用于雙支點各單向固定， 通過調(diào)整外 殼與軸承端蓋端面的厚度來補償軸的受熱伸長， 因而這種配置方式不適合需要

21、對 軸做精確軸向定位的場合。2、一支點雙向固定，另一端支點游動 對于熱伸長量較大的軸， 這種軸一般跨距較大且工作溫度較高， 應該采用一 支點雙向固定， 另一支點游動的支撐結(jié)構(gòu)。 其雙向固定端需要使用能承受雙向軸 向載荷的軸承，內(nèi)外圈都需要固定。3、兩端游動支撐 對于人字齒輪軸， 由于本身具有相互間的軸向限位作用， 其中只需保證一根 軸與機座有相對固定的軸向位置， 另一根軸上的兩個軸承必須游動， 防止人字齒 輪卡死或兩側(cè)受力不均勻。 本文將采用一支點雙向固定， 另一端支點游動的方式 組合成卸荷軸承組， 其中雙向固定端使用深溝球軸承， 游動端使用滾針軸承。 其 中腰關(guān)節(jié)使用型號為HRB 61809

22、-2Z、SKF HK5O20肘關(guān)節(jié)旋轉(zhuǎn)使用型號為 HRB 61806-2Z、 SKF HK3512。2.5.2 旋轉(zhuǎn)關(guān)節(jié)的設(shè)計各運動軸基于模塊化設(shè)計， 設(shè)計時已考慮各運動軸的聯(lián)接， 使用簡單的金屬 板件便能將各運動軸連接可靠， 并具有一定的剛度。 本文各運動軸均為法蘭端面 輸出，與金屬板材間通過螺釘聯(lián)接固定， 依靠金屬板與法蘭端面的摩擦力傳遞扭 矩到金屬板材。圖 2-11 為肩關(guān)節(jié)聯(lián)接金屬板材機械加工工程圖：圖 2-10 肩關(guān)節(jié)聯(lián)接金屬板2.5.3 抓手的設(shè)計一款通用型的機械臂應該具有抓取多種物體的能力，也為某一目標物體專門設(shè)計一款抓手， 因而在設(shè)計機械臂臂體與機械抓手時， 需要設(shè)計一個合理的

23、聯(lián) 接結(jié)構(gòu)，以便機械抓手能夠快速更換。 機械抓手在抓取物體時需要保持一定的夾 持力，本文將使用舵機為機械手爪提供動力， 舵機在旋轉(zhuǎn)到位后能持續(xù)提供一定 的扭矩以保持夾持狀態(tài)。使用舵機型號為 TowerPro MG945 全金屬齒舵機， 表 2-5 舵機型號及相關(guān)參數(shù)型號舵機類型機械尺寸（mm扭力工作電壓工作死區(qū)MG945模擬舵機40.7*19.7*42.9：12kg/cm(6V)4.8-6V5us圖2-12為和機械抓手配合的臂體聯(lián)接結(jié)構(gòu)。同理只要末端執(zhí)行器設(shè)計為可與該聯(lián)接結(jié)構(gòu)配合，即可實現(xiàn)更換，以完成對不同物體的抓取任務。圖2-11機械臂執(zhí)行器聯(lián)接結(jié)構(gòu)3機械臂軟件系統(tǒng)設(shè)計整個機械臂控制系統(tǒng)軟件

24、包括主計算機監(jiān)控軟件和各關(guān)節(jié)控制器軟件。主計算機接收目標位姿數(shù)據(jù)，完成路徑規(guī)劃算法。另外，主計算機要實時讀取關(guān)節(jié)控 制器的反饋數(shù)據(jù)，記錄機械臂的當前位姿，并顯示在屏幕上。不僅直線運動、點 到點運動、復位動作的完成需要主計算機監(jiān)控軟件協(xié)調(diào)，而且像各關(guān)節(jié)絕對位置的確定過程、各關(guān)節(jié)運動范圍是否越界也必須由主計算機實時參與，及時做出決策。主計算機還要提供給用戶友好的人機交互界面，方便于用戶輸入各種命令， 存儲設(shè)置好的參數(shù)，允許數(shù)據(jù)以表格或者曲線等形式導出。3.1關(guān)節(jié)控制器軟件設(shè)計控制算法關(guān)節(jié)控制器TMS320LF240來實現(xiàn)，完成電機的位置環(huán)控制和速度環(huán)控制， 如圖3-1所示。位置環(huán)的控制周期設(shè)為 2

25、ms速度環(huán)的控制周期也設(shè)為 2ms由 于采用了速度環(huán)，系統(tǒng)的動態(tài)性性能可以得到顯著提高。 兩個閉環(huán)都采用積分分 離PD控制，根據(jù)實際調(diào)試情況，可以對控制律進行適當?shù)幕?。零位霍爾接近開關(guān)在系統(tǒng)上電時用于較粗略的確定電機的絕對位置，再結(jié)合增量編碼器的Z通道的信號，就可以較精確的確定出電機的絕對位置。 主計算機 路徑規(guī)劃求得的目標位置，應該換算成增量碼盤的脈沖數(shù)后，再發(fā)給關(guān)節(jié)控制器。 關(guān)節(jié)控制器利用它和從增量編碼器實際測得的脈沖數(shù)進行比較，利用積分分離 PID算法求解位置環(huán)的控制量。關(guān)節(jié)控制器程序流程主程序的流程圖如3-2所示：圖3-1主程序流程圖寄存器初始化操作主要包括：設(shè)置 CPUCL為外部晶振的2倍頻，即16MHZ 設(shè)置串口通訊波特率為：38.4kbPs ;設(shè)置定時器/計數(shù)器相關(guān)寄存器；設(shè)置 QEP 電路單元相關(guān)寄存器；設(shè)置中斷控制寄存器等等。串口數(shù)據(jù)接收中斷服務程序流 程圖如3-3所示。在中斷服務程序中，讀取數(shù)據(jù)接收寄存器中的數(shù)據(jù)，存入數(shù)據(jù) 接收區(qū)，而并不作任何進一步分析和處理。 數(shù)據(jù)接收區(qū)是內(nèi)存中暫時存放數(shù)據(jù)的 區(qū)域，當存滿一條完整指令信息后，由主程序分析和處理。圖 3-2 串口數(shù)據(jù)接收中斷服務程序流程圖控制周期2ms定時中斷服務程序的流程見圖 3-4。定時