諶孫杰，馬 璐，吳秋平

(武漢理工大學(xué) 自動化學(xué)院，武漢 430070)

?

兩自由度并聯(lián)機(jī)器人控制系統(tǒng)設(shè)計

諶孫杰，馬 璐，吳秋平

(武漢理工大學(xué) 自動化學(xué)院，武漢 430070)

針對機(jī)電裝配、醫(yī)藥包裝等行業(yè)自動化抓取和放置實際應(yīng)用的需求，設(shè)計了一種采用直線伺服驅(qū)動技術(shù)的兩自由度高速并聯(lián)機(jī)器人；首先進(jìn)行了機(jī)器人的機(jī)械結(jié)構(gòu)設(shè)計，并且通過運動學(xué)分析，結(jié)合運用蒙特卡洛法與幾何法求解出并聯(lián)機(jī)器人的工作空間；其次采用MCU+CPLD(STM32F103+MaxⅡ)的雙控制架構(gòu)，根據(jù)工作空間結(jié)構(gòu)在MCU中設(shè)計出高效的運動軌跡生成算法，并在CPLD中運用DDS技術(shù)將伺服運動參數(shù)轉(zhuǎn)換成脈沖經(jīng)一級差分后輸出到伺服驅(qū)動器;在雙控制器的通信方面合理利用CPLD中的狀態(tài)機(jī)提出了雙級數(shù)據(jù)緩沖方式，使得MCU寫與CPLD讀交錯進(jìn)行，可以實現(xiàn)數(shù)據(jù)的無縫刷新；最后進(jìn)行了并聯(lián)機(jī)器人系統(tǒng)樣機(jī)的制作與調(diào)試，分析結(jié)果表明所研制的機(jī)器人控制系統(tǒng)能夠使機(jī)器人快速抓放金屬小球，實現(xiàn)穩(wěn)定可靠運行。

并聯(lián)機(jī)器人；雙控制器；運動軌跡；無縫刷新

0 引言

隨著人們對并聯(lián)機(jī)器人的深入研究，其各方面的優(yōu)勢逐步顯現(xiàn)出來。并聯(lián)機(jī)器人可以提供高于串聯(lián)機(jī)構(gòu)的承載能力和剛度，同時還具有高速、高加速度和高精度等優(yōu)點，眾多研究機(jī)構(gòu)和制造企業(yè)均看好其在工業(yè)制造領(lǐng)域的應(yīng)用前景[1]。少自由度并聯(lián)機(jī)器人不僅繼承了并聯(lián)機(jī)器人的固有優(yōu)點，同時還克服了高自由度并聯(lián)機(jī)器人控制困難、解算復(fù)雜的缺點，在工業(yè)上具有很高的實用價值。運動控制器按照期望的機(jī)械運動向電機(jī)驅(qū)動發(fā)出指令，在運動控制器的控制下，整個機(jī)器人系統(tǒng)按照一定的軌跡完成預(yù)期的動作。

傳統(tǒng)伺服接口有模擬速度給定+編碼器反饋以及脈沖+方向兩種形式。前者在運動控制器內(nèi)形成位置閉環(huán)，速度及電流環(huán)在驅(qū)動內(nèi)；后者運動控制器以脈沖形式發(fā)出位置給定，位置、速度和電流閉環(huán)在驅(qū)動內(nèi)。隨著現(xiàn)場總線技術(shù)的快速發(fā)展，近年來采用現(xiàn)場總線的伺服接口也越來越普遍。運動控制行業(yè)的前景被很多公司看好，在國內(nèi)外，已有多種商品化的開放式運動控制器，然而價格通常比較昂貴且功能過于復(fù)雜，在要求實現(xiàn)的功能在相對簡單的小型伺服系統(tǒng)中并不實用[2]?；谝陨戏治?，設(shè)計一款應(yīng)用于低自由度并聯(lián)機(jī)器人系統(tǒng)的運動控制器十分必要。

本文進(jìn)行了一種新型實用的平面二自由度并聯(lián)機(jī)器人的設(shè)計，應(yīng)用于電子材料、機(jī)電裝配、醫(yī)藥包裝等行業(yè)自動化抓取和放置實際需求，采用精密直線電機(jī)直接驅(qū)動方式，取代了傳統(tǒng)旋轉(zhuǎn)伺服通過滾珠絲桿間接獲取直線運動的中間環(huán)節(jié)，大大簡化了系統(tǒng)的機(jī)械結(jié)構(gòu)，降低了制造成本，同時提高了系統(tǒng)響應(yīng)速度和時間精度[3]。設(shè)計過程中進(jìn)行了并聯(lián)機(jī)器人的機(jī)械結(jié)構(gòu)設(shè)計和運動學(xué)分析，選用了MCU+CPLD雙控制架構(gòu)，研制了一種帶有差分式Step+Dir通用伺服接口的運動控制器，編寫了MCU伺服運算程序及CPLD脈沖發(fā)生程序，實現(xiàn)了樣機(jī)的制作與調(diào)試，調(diào)試結(jié)果表明所設(shè)計的并聯(lián)機(jī)器人能夠準(zhǔn)確快速完成抓取和放置要求，具有實用價值和創(chuàng)新性。

1 工作原理與運動學(xué)分析

本文所設(shè)計的兩自由度并聯(lián)機(jī)器人采用直線電機(jī)作為動力源，兩根電機(jī)間距一定距離并且都安裝有導(dǎo)桿，電機(jī)與導(dǎo)軌采用凸臺軸向定位后與擺桿相連如圖1所示。與左側(cè)電機(jī)相連的兩根導(dǎo)桿在正視圖下重合，無論是左側(cè)電機(jī)還是右側(cè)電機(jī)，與其相連的擺桿在運動過程中始終圍成底邊在同一水平線上的平行四邊形，這種結(jié)構(gòu)確保了擺桿末梢鉸鏈處的立方體金屬塊始終保持水平，使得與金屬塊相連的電磁鐵運動到任意位置均可方便的吸放小球。

圖1 并聯(lián)機(jī)器人正視結(jié)構(gòu)圖

并聯(lián)機(jī)器人機(jī)械系統(tǒng)的正常運行離不開精確可靠的控制系統(tǒng)，本文所設(shè)計的兩自由度機(jī)器人用于抓放物體的末端執(zhí)行器的運動狀態(tài)由兩直線電機(jī)共同決定，因此直線電機(jī)的控制是整個系統(tǒng)的控制關(guān)鍵。通常伺服控制系統(tǒng)由伺服電機(jī)、驅(qū)動器、控制器和檢測裝置組成，其中MCU需要完成運動軌跡計算及與上位機(jī)的通信，結(jié)合其控制任務(wù)及控制精度要求可選用STM32F103單片機(jī)。然而要發(fā)出一定帶寬且能實時更新的脈沖去驅(qū)動直線電機(jī)僅采用單片機(jī)很難實現(xiàn)，因此本文所述的運動控制系統(tǒng)采用了MCU+CPLD雙控制架構(gòu)作為運動控制器，系統(tǒng)整體框圖如圖2所示。運動控制器要求能夠根據(jù)運動控制指令，在線規(guī)劃電機(jī)運動軌跡，計算出電機(jī)運行過程中每一時刻的具體位置、速度等參數(shù)；將電機(jī)的位置給定包括位移增量以及移動方向以脈沖形式發(fā)給伺服驅(qū)動器，去驅(qū)動并聯(lián)機(jī)器人，所述的并聯(lián)機(jī)器人由伺服電機(jī)和機(jī)械傳動機(jī)構(gòu)組成。系統(tǒng)運行時，控制器接受檢測裝置檢測到的機(jī)器人實時運行狀態(tài)同時與上位機(jī)進(jìn)行通信，使整個系統(tǒng)能夠?qū)崟r接收上位機(jī)的管理控制，并能在突發(fā)情況下進(jìn)行緊急處理。

圖2 兩自由度并聯(lián)機(jī)器人控制系統(tǒng)整體框圖

作為整個兩自由度并聯(lián)機(jī)器人控制系統(tǒng)的核心，運動控制器主要功能是根據(jù)提前規(guī)劃的軌跡做運算，并將運算結(jié)果以脈沖方式輸出位置與方向給定去分別驅(qū)動兩個直流電機(jī)。要做軌跡規(guī)劃就必須對并聯(lián)機(jī)器人進(jìn)行運動學(xué)分析，建立運動學(xué)方程。如圖1所示，左右兩側(cè)電機(jī)的動子下部端點分別為M1、M2，A、B為動子的延長線與擺桿的交點，兩轉(zhuǎn)動傳動副末梢閉鏈結(jié)構(gòu)交點為C，并聯(lián)機(jī)器人搬運的小球下半球面與水平面的切點為點D。根據(jù)這些關(guān)鍵點建立坐標(biāo)系，結(jié)合實際距離參數(shù)，可以確定各關(guān)鍵點坐標(biāo)。本文中涉及的運動學(xué)分析為運動學(xué)逆問題，即根據(jù)末端執(zhí)行器在參考坐標(biāo)系下的位置和姿態(tài)求解關(guān)節(jié)的位置，也就是已知擺桿L1、L2交點C的位置，求解兩個電機(jī)動子下部端點M1、M2的具體位置，機(jī)器人運動學(xué)的逆解方程為：

(1)

式中,L為擺桿的長度，x、y分別表示橫坐標(biāo)和縱坐標(biāo)。與此同時，還要確定機(jī)器人關(guān)節(jié)變量與末端執(zhí)行器之間位置隨時間的變化率，跟據(jù)所求的運動學(xué)逆解方程，對其進(jìn)行時間微分，即可求得速度雅克比矩陣逆為：

(2)

進(jìn)行機(jī)器人的軌跡規(guī)劃時，要考慮機(jī)器人末端參考點所能到達(dá)的實際空間，還要進(jìn)行機(jī)器人工作空間的求解。本文運用MATLAB仿真軟件，使用蒙特卡洛法求得機(jī)器人理想工作空間，再考慮機(jī)器人避障問題(實際情況下受臺階限制)結(jié)合幾何法[4-6]，求得實際工作空間(出去陰影部分)如圖3所示。

圖3 并聯(lián)機(jī)器人實際工作空間

2 雙控制器系統(tǒng)設(shè)計

本文所描述的控制系統(tǒng)采用單片機(jī)+可編程邏輯器件雙控制器的設(shè)計，總體系統(tǒng)如圖2所示。單片機(jī)負(fù)責(zé)與上位機(jī)通信、開關(guān)量的控制以及根據(jù)預(yù)設(shè)運動軌跡，計算出相應(yīng)的脈沖頻率給定，每1 ms(伺服周期)刷新一次頻率給定值；可編程邏輯器件在接收到單片機(jī)的給定后，將其轉(zhuǎn)化為對應(yīng)頻率的方波信號，這種雙控制器架構(gòu)，充分發(fā)揮了單片機(jī)的靈活性與可編程邏輯器件的高速性。結(jié)合系統(tǒng)的需求選用了具有豐富的存儲及外設(shè)資源和強(qiáng)大處理能力的STM32F103RET6單片機(jī)作為主控制器。主控制器單片機(jī)和MaxIIEPM570最小系統(tǒng)的應(yīng)用已經(jīng)很成熟，此處不再贅述[7]。

單片機(jī)作為控制器系統(tǒng)的核心，需要根據(jù)第2節(jié)中所述的運動學(xué)分析，進(jìn)行運動軌跡的合理規(guī)劃和軟件算法實現(xiàn)。機(jī)器人的所有運行軌跡均被限制在如圖3所示的工作空間內(nèi)，機(jī)器人利用電磁鐵吸放金屬小球的過程來模擬實際工業(yè)生產(chǎn)過程，其運動軌跡包括直線運動與圓弧運動。其中軌跡1、3、4為不包含x方向的直線運動，電機(jī)動子端點M1、M2的運動軌跡完全相同且為豎直方向直線運動，進(jìn)行軟件編程時，需根據(jù)預(yù)設(shè)給定位移，求解出運動軌跡中的加速、勻速(部分軌跡存在)和減速3個階段之間的轉(zhuǎn)折時間點及各時間段內(nèi)運動方向；包含x方向的運動包括直線運動軌跡2和圓弧運動軌跡5，運用運動學(xué)分析方程分別求出軌跡2、5下的M1、M2位移時間曲線如圖4所示，這種情況下若采用軟件解算法遠(yuǎn)比不含x分量的直線軌跡復(fù)雜，因此本文采用一種在位移時間曲線上均勻采樣去擬合逼近原曲線的方法。

圖4 軌跡2、5下M1、M2位移時間曲線

對曲線進(jìn)行擬合時，可均勻取n個采樣點，每兩點之間直線連接如圖5所示，曲線S在t時間內(nèi)位移與曲線S′在t′時間內(nèi)位移相同，只是所用時間不同。因此所設(shè)計的算法在時域上具有伸縮性，對某具體軌跡均勻采樣n個點，無論這條軌跡需要多長時間完成，都可用同一組數(shù)據(jù)表示完成運算，大大簡化了軟件解算程序。需要注意的是，進(jìn)行軟件編程時，一段連續(xù)的曲線運動軌跡在計算機(jī)內(nèi)無法實現(xiàn)連續(xù)變化，因此我們需要將連續(xù)時域的軌跡作離散化的處理。以一次曲線為例，若運動控制器的伺服周期設(shè)為1 ms，其軌跡離散處理方法如圖6所示。伺服周期決定了計算機(jī)更新伺服給定的最小分辨率，并且一個伺服周期內(nèi)直線電機(jī)的速度保持最新給定值不變，那么在每1 ms的伺服周期中，要使用一個恒定的速度等效替代連續(xù)域中變化的速度，并保證最終完成的位移效果相同，可以遵行面積相等的等效替代原則。

圖5 軌跡時域伸縮擬合示意圖

圖6 軌跡離散處理示意圖

然而，通過MCU的軟件解算得到的各類軌跡伺服參數(shù)仍然無法直接去驅(qū)動電機(jī)，還需要通過CPLD將控制參數(shù)轉(zhuǎn)化為脈沖信號輸出到驅(qū)動器，這里就涉及到MCU與CPLD之間的通信程序設(shè)計。

一個伺服周期中，MCU要向CPLD傳輸80 bit數(shù)據(jù)，如果采用并行通信方式需要大量IO資源，因此本文采用SPI同步串行的通信方式。利用CPLD內(nèi)部的邏輯塊設(shè)計生成一個SPI接口，它的功能相當(dāng)于一個串入并出的移位寄存器。每根電機(jī)的運動參數(shù)信息為40 bit，因此可以開發(fā)一個40位的移位寄存器。并聯(lián)機(jī)器人伺服系統(tǒng)對實時性要求很高，每個伺服周期內(nèi)必須在前一組數(shù)據(jù)脈沖發(fā)送完成前完成下一組數(shù)據(jù)更新，本文設(shè)計了一種雙級數(shù)據(jù)緩沖方式，其數(shù)據(jù)更新時序圖如圖7所示。

圖7 雙緩沖數(shù)據(jù)更新時序圖

在CPLD脈沖發(fā)生周期進(jìn)行到一半時，MCU通過SPI發(fā)送數(shù)據(jù)到一級緩存，一個周期脈沖發(fā)完時CPLD更新頻率給定信號FRE和計數(shù)值CMP等參數(shù)，且通過CPLD中狀態(tài)機(jī)的合理調(diào)度使得MCU的伺服運算周期與CPLD的脈沖更新周期在長度上相等，僅僅是錯開了半個周期。這種將MCU傳輸數(shù)據(jù)與CPLD取數(shù)據(jù)在時間上交錯的雙級緩存方式，消除了伺服周期內(nèi)由于脈沖更新產(chǎn)生的死區(qū)的影響，完成了數(shù)據(jù)的無縫刷新，提高了系統(tǒng)實時性。CPLD接收到MCU發(fā)送的實時數(shù)據(jù)，運用其高速的時序邏輯處理能力，需將這些參數(shù)轉(zhuǎn)化為頻率隨時間動態(tài)變化的脈沖，這就需要DDS信號發(fā)生模塊。DDS信號發(fā)生模塊具有基準(zhǔn)時鐘頻率fDDS、查表計數(shù)器cnt和相位步進(jìn)量M3個重要參數(shù)，每來一個基準(zhǔn)時鐘沿，cnt的值加上M，cnt通過查表得出一個對應(yīng)的具體數(shù)值作為輸出如表1所示。

表1 M=1時DDS查表對應(yīng)

表1表示當(dāng)DDS的基準(zhǔn)時鐘頻率fDDS=1且相位步進(jìn)量M=1時，每間隔一秒來一個時鐘沿，查表計數(shù)器加1，再通過表中對應(yīng)得到某一時刻的輸出值d，由表中數(shù)據(jù)可知輸出信號的頻率與相位步進(jìn)量M成正比。任意設(shè)一個頻率給定值FRE=1 001 Hz，本文所設(shè)計的DDS模塊的輸出通過示波器實測的顯示結(jié)果如圖8所示，示波器顯示采樣瞬間脈沖頻率為1 002 Hz，頻率平均值為1 000.99 Hz，由此可見實際輸出頻率與理想給定頻率十分接近。

圖8 FRE=1 001 Hz時DDS輸出實測波形

DDS信號發(fā)生模塊的輸出經(jīng)由一級差分處理后，送伺服驅(qū)動器，驅(qū)動電機(jī)完成控制器軟件運算規(guī)劃的運動軌跡，最終實現(xiàn)小球的搬運工作?？刂破飨到y(tǒng)設(shè)計軟件部分還包括MCU與上位機(jī)通信程序設(shè)計及簡單的IO口開關(guān)量控制[8-9],由于篇幅原因這里不做詳細(xì)說明。

3 實驗樣機(jī)調(diào)試與結(jié)果分析

通過前面兩個部分的分析，分別運用C語言和VHDL語言對運動控制器中的MCU(STM32F103RET6)和CPLD(MAXIIEPM570)編程實現(xiàn)控制功能，還同時通過在PC機(jī)上運用LinMot-Talk軟件對伺服驅(qū)動進(jìn)行配置，設(shè)置驅(qū)動的工作模式并能在線實時顯示位置反饋曲線，這使得驅(qū)動器PID參數(shù)的調(diào)節(jié)更加直觀。將Step+Dir伺服接口接至顯波器即可清晰地觀察機(jī)器人走每個動作時，脈沖信號的具體發(fā)生狀態(tài)。加速度aacc值為正數(shù)、零和負(fù)數(shù)時，分別表示運動學(xué)分析中的加速、勻速和減速運動過程。表示圖9(a)所示為加速度aacc=6 m/s2時，DDS輸出的脈沖波形，脈沖頻率在快速逐漸增大；圖9(b)所示為加速度aacc=-1 m/s2時，DDS輸出脈沖波形，脈沖的頻率在逐漸的減小。分析實驗波形可知，當(dāng)加速度為正值時，機(jī)器人的動子端點做加速運動，脈沖頻率是一個遞增的過程，且加速度的值越大，加速過程也就越快，加速時間越短。加速度的絕對值越大，脈沖頻率的變化(增大或減小)就越快，加速或減速的過程也就越短。所得到的脈沖信號經(jīng)過差分驅(qū)動芯片進(jìn)行差分處理，幅值也被一定程度地放大，使得控制器最終的輸出電平為與伺服驅(qū)動器兼容的422差分電平。

以上分析結(jié)果表明，本文所設(shè)計的雙架構(gòu)運動控制器能滿足伺服過程中的各種加減速要求，并為伺服系統(tǒng)提供穩(wěn)定可靠的脈沖給定，所述的并聯(lián)機(jī)器人運動控制系統(tǒng)，在實際應(yīng)用中，只需要通過設(shè)置幾個伺服參數(shù)，根據(jù)具體情況，可以直接調(diào)用所設(shè)計的指令，實現(xiàn)對所有帶Step+Dir接口的伺服電機(jī)的控制，具有較強(qiáng)的通用性。

圖9 不同加速度值時的加減速脈沖波形

4 結(jié)束語

通過對兩自由度并聯(lián)機(jī)器人控制系統(tǒng)的理論研究，在實際樣機(jī)測試達(dá)到了預(yù)期的控制效果。本設(shè)計僅對兩自由度做出了研究，對多自由度的研究是并聯(lián)機(jī)器人系統(tǒng)的發(fā)展方向。代替MCU+CPLD雙控制架構(gòu)的解決方案是采用單顆FPGA芯片作為控制器的架構(gòu)，使系統(tǒng)的控制板更加細(xì)小，節(jié)約成本。隨著工業(yè)化進(jìn)程的不斷發(fā)展，兩自由度甚至多自由度的并聯(lián)機(jī)器人在工業(yè)領(lǐng)域?qū)玫礁嗟膽?yīng)用，能極大地提高生產(chǎn)效率，具有很高的實用價值。

[1] 田 濤. 一種高速拾取并聯(lián)機(jī)器人的設(shè)計與實現(xiàn) [D]. 大連: 大連理工大學(xué), 2013.

[2] 田家林. 基于ARM與FPGA的運動控制系統(tǒng)的設(shè)計[D]. 成都:西南石油大學(xué),2006.

[3] 施 俊. 直線感應(yīng)電機(jī)雙邊驅(qū)動電梯的控制系統(tǒng)研究 [D]. 杭州: 浙江大學(xué), 2004.

[4] Stan S, Maties V, Balan R. New Methodology and Evaluation of Micro Parallel Robots for Optimal Design[A]. The 16th IEEE International Symposium on Robot and Human interactive Communication[C].2007: 894-899.

[5] Stan S, Maties V, Balan R. Workspace optimal design of a 2 DOF micro parallel robot using genetic algorithms and simulated annealing optimization methods[A]. International Conference on Mechatronics and Automation[C].2007: 1108-1113.

[6] 洪乃剛,等. 電力電子和電力拖動控制系統(tǒng)的MATLAB仿真[M]. 北京: 機(jī)械工業(yè)出版社,2006.

[7] 齊 珊. 多軸運動控制系統(tǒng)設(shè)計[D]. 天津: 天津大學(xué),2008.

[8] 蒙博宇. STM32自學(xué)筆記[M]. 北京: 北京航空航天大學(xué)出版社, 2012.

[9] 黃 任. AVR單片機(jī)與CPLD/FPGA綜合應(yīng)用入門[M].北京: 北京航空航天大學(xué)出版社,2004.

Design of Control System for Parallel Robot with Two Degree of Freedom

Chen Sunjie，Ma Lu，Wu Qiuping

(School of Automation,Wuhan University of Technology, Wuhan 430070,China)

To fit the demand of automatic pick and place in the manufacture process of medicine packaging and electromechanical assembling,a high-speed 2-DOF parallel robot using linear servo technology is designed in this thesis. Firstly, the mechanical structure design of the robot is introduced, according to the results of kinematics analysis, the workspace of the 2-DOF parallel robot is obtained via Monte-Carlo method and geometrical method. Secondly, a control system consist of MCU and CPLD(STM32F103+MaxII)is adopted, an efficient approach to generating motion curve programs in MCU is proposed based on workspace. And we use DDS(Direct Digital Frequency Synthesizer) to generate square wave in accordance with motion parameters, then we transform square wave to differential signals which will be sent to servo driver. In the design of communication between the two controllers, a two stage data buffer is presented by rational use of state machine in CPLD, which can realize the interleave of MCU read and CPLD write to achieve seamless refresh of data. Finally we complete the manufacture and adjustment of the robot demo, the results shows that the performance of the 2-DOF parallel robot system is stable and reliable, at the same time the robot can pick and place the prill quickly.

parallel robot;dual controller;motion curve;seamless refresh

2015-12-08；

2016-01-07。

諶孫杰(1991-)，男，貴州凱里人，碩士研究生，主要從事嵌入式方向的研究。

1671-4598(2016)06-0110-03DOI：10.16526/j.cnki.11-4762/tp

C

A