【摘要】臺(tái)達(dá)高端PC-Based工業(yè)控制器 (PAC) 具有強(qiáng)大的運(yùn)動(dòng)控制功能，其中DDA功能是臺(tái)達(dá)運(yùn)動(dòng)控制卡提供的一種特殊運(yùn)動(dòng)指令，可以滿足各種軌跡運(yùn)動(dòng)的需求。本文以SCARA機(jī)器人控制為例，詳細(xì)講解如何透過DDA功能的應(yīng)用實(shí)現(xiàn)SCARA機(jī)器人直線、圓弧等插補(bǔ)運(yùn)動(dòng)。

[Abstract] Delta Programmable Automation Controller (PAC) is equipped with powerful motion control functions including DDA function (via Delta motion control card) which provides specific  l motion commands to achieve all kinds of track movement. By taking SCARA robot motion control as an example, this paper explains  the application of DDA function and control over SCARA motion interpolation such as straight-line or arc movement.    .

【關(guān)鍵字】Programmable Automation Controller (PAC)產(chǎn)品， DDA功能，SCARA機(jī)器人，插補(bǔ)運(yùn)動(dòng)

[Keywords] PAC product, DDA function, SCARA robot, motion interpolation 

前言

臺(tái)達(dá)高端PC-Based工業(yè)控制器(PAC)產(chǎn)品具備非常強(qiáng)大的DDA功能，雖然與該功能相關(guān)的API指令很少，但想要正確、應(yīng)用存在一定的難度，主要困難點(diǎn)包括：一、運(yùn)動(dòng)模型的建立問題，以SCARA機(jī)器人為例，必須根據(jù)SCARA的機(jī)構(gòu)原理搭建合理的機(jī)器人運(yùn)動(dòng)模型，實(shí)現(xiàn)機(jī)器人的正運(yùn)動(dòng)學(xué)控制與逆運(yùn)動(dòng)學(xué)控制。二、路徑規(guī)劃問題，良好的路徑規(guī)劃演算法可建立更加平滑的運(yùn)動(dòng)效果。本文針對(duì)以上兩個(gè)難點(diǎn)，詳細(xì)講解SCARA機(jī)器人的模型推導(dǎo)，及運(yùn)動(dòng)路徑規(guī)劃，最終通過DDA功能實(shí)現(xiàn)SCARA機(jī)器人平滑運(yùn)動(dòng)控制。

一、 DDA功能簡(jiǎn)介

DDA功能是臺(tái)達(dá)運(yùn)動(dòng)控制卡提供的一種特殊指令，可以同時(shí)針對(duì)12個(gè)軸進(jìn)行運(yùn)動(dòng)控制。當(dāng)DDA功能開啟后，運(yùn)動(dòng)控制卡可以每毫秒執(zhí)行一筆DDA Table中的資料，其中DDA Table最大Buffer為1000筆數(shù)據(jù)，值得注意的是在Buffer執(zhí)行完畢之前，不接受其他指令（如：Sd_Stop等）。

與DDA功能相關(guān)的API指令主要有四個(gè)，包括開啟DDA填表功能 （enable_dda_mode）、向DDA Table中填寫資料（set_dda_data）、以及取得資料（get_dda_cnt）和清除資料（clear_dda_data）的輔助指令。

1.開啟DDA填表功能

_DMC_01_enable_dda_mode(U16 CardNo，U16 enable)

其中enable參數(shù)是unit16類型，每一個(gè)bit位代表一個(gè)從站，其中0表示DDA功能關(guān)閉，1表示DDA功能開啟，如圖1 所示，現(xiàn)需開啟從站1、2、3、4號(hào)四個(gè)軸DDA功能，只需讓Enable=15就可以了。

2.向DDA Table中填寫資料

_DMC_01_set_dda_data(U16 CardNo，U32* abs_pos)

開啟DDA功能后，DDA Table中暫無資料，從站無動(dòng)作。當(dāng)向DDA Table中寫入資料數(shù)據(jù)后，從站按資料信息進(jìn)行動(dòng)作。其中abs_pos是一個(gè)12軸的資料陣列。例如：現(xiàn)要對(duì)從站1、2、3、4 DDA Table發(fā)送數(shù)據(jù)，發(fā)送數(shù)據(jù)格式如圖2所示。

3.輔助指令

_DMC_01_get_dda_cnt(U16 CardNo，U16* dda_cnt)

該指令可以取得目前DDA Table尚存的資料筆數(shù)。

_DMC_01_clear_dda_data(U16 CardNo)

該指令用來將DDA Table中尚存的資料清除，及清空DDA Table中的資料。當(dāng)DDA Table中的資料執(zhí)行結(jié)束前，其他的停止API是無效的，只有通過clear_dda_data指令清空DDA Table中的資料才能使伺服電機(jī)停止下來。

二、SCARA機(jī)器人

SCARA（Selective Compliance Assembly Robot Arm) 中文譯名是選擇順應(yīng)性裝配機(jī)器手臂 ,這是一種圓柱坐標(biāo)型的特殊類型的工業(yè)機(jī)器人。SCARA系統(tǒng)在X、Y方向上具有順從性，而在Z軸方向具有良好的剛度，特別適合于裝配工作。同時(shí)，SCARA串接的兩桿結(jié)構(gòu)，類似人的手臂，可以伸進(jìn)有限空間中作業(yè)然后收回，適合于搬動(dòng)和取放物件。

1.SCARA機(jī)器人運(yùn)動(dòng)學(xué)模型

SCARA機(jī)器人具有四個(gè)軸和四個(gè)運(yùn)動(dòng)自由度，(包括X、Y、Z方向的平動(dòng)自由度和繞Z軸的轉(zhuǎn)動(dòng)自由度)。如圖3為SCARA機(jī)器人運(yùn)動(dòng)學(xué)模型。其中為第一關(guān)節(jié)角度，為第二關(guān)節(jié)角度，為第三關(guān)節(jié)伸縮長(zhǎng)度，為第四關(guān)節(jié)角度。

根據(jù)SCARA機(jī)器人運(yùn)動(dòng)學(xué)模型，在卡迪爾坐標(biāo)系下就可以建立SCARA機(jī)器人數(shù)學(xué)模型：

其中：

、、為SCARA末端點(diǎn)位坐標(biāo)

為第一關(guān)節(jié)臂長(zhǎng)

為第二關(guān)節(jié)臂長(zhǎng)

該模型下，當(dāng)時(shí)，SCARA機(jī)器人工作在右手系；當(dāng)時(shí)，SCARA機(jī)器人工作在左手系。

以臺(tái)達(dá)SCARA機(jī)器人DRS40L機(jī)械結(jié)構(gòu)為例，第一關(guān)節(jié)與第二關(guān)節(jié)伺服電機(jī)帶有減速機(jī)，第三關(guān)機(jī)伺服電機(jī)帶動(dòng)螺紋絲桿，進(jìn)而可以得到伺服電機(jī)編碼器反饋位置與各關(guān)節(jié)角度或長(zhǎng)度關(guān)系：

其中：

為關(guān)節(jié)角度

為關(guān)節(jié)長(zhǎng)度

為編碼器反饋位置

為電子齒輪比分子

為電子齒輪比分母

為減速器減速比

為絲桿螺紋間距

通過以上推導(dǎo)，就可以將伺服電機(jī)編碼器位置量與SCARA機(jī)器人末端空間位置坐標(biāo)一一對(duì)應(yīng)起來。

2.SCARA機(jī)器人逆運(yùn)動(dòng)學(xué)模型

根據(jù)上節(jié)給出的SCARA機(jī)器人運(yùn)動(dòng)模型，反推逆運(yùn)動(dòng)學(xué)模型為：

計(jì)算出各關(guān)節(jié)量后就能反推出各關(guān)節(jié)伺服電機(jī)編碼器位置量：

只需要將計(jì)算得到的各關(guān)節(jié)伺服位置、、、寫入DDA Table中，SCARA機(jī)器人末端就可以到達(dá)指定坐標(biāo)點(diǎn)位。

如果想用DDA功能實(shí)現(xiàn)對(duì)SCARA機(jī)器人的運(yùn)動(dòng)控制，需要知道SCARA機(jī)器人運(yùn)動(dòng)過程中每毫秒各關(guān)節(jié)伺服位置應(yīng)是多少，將連續(xù)的伺服位置量寫入DDATable中，就可以實(shí)現(xiàn)想要的SCARA機(jī)器人運(yùn)動(dòng)控制。

三、PC-Based工業(yè)控制器 (PAC) 產(chǎn)品控制SCARA機(jī)器人運(yùn)動(dòng)控制

在生產(chǎn)制造過程中，機(jī)器人可以取代工人實(shí)現(xiàn)物品的搬運(yùn)、設(shè)備的組裝、質(zhì)量檢測(cè)等。在實(shí)際應(yīng)用中，經(jīng)常會(huì)要求機(jī)器人安裝規(guī)定的路徑、額定的速度等來適應(yīng)復(fù)雜的現(xiàn)場(chǎng)需求，如：直線運(yùn)動(dòng)、圓弧運(yùn)動(dòng)、P to P等；

1.控制系統(tǒng)架構(gòu)

在此采用臺(tái)達(dá)SCARA機(jī)器人DRS40L、主機(jī)型PC-Based 工業(yè)控制器MH1-S30D、伺服驅(qū)動(dòng)器&馬達(dá)ASDA-A2，搭建測(cè)試平臺(tái)，并設(shè)計(jì)MovL、MovP、MovArc、V_Move、Emg_Stop、Sd_Stop、HandType_Change等運(yùn)動(dòng)控制指令，Get_Position、Get_Joint_Ang、Get_Real_Speed、Get_Servo_Spd等狀態(tài)監(jiān)控指令。控制系統(tǒng)架構(gòu)如圖4所示：

由于SCARA運(yùn)動(dòng)控制指令較多，但實(shí)現(xiàn)方法相同，在此我們以MovL指令為例，詳細(xì)說明DDA功能如何實(shí)現(xiàn)SCARA機(jī)器人MovL運(yùn)動(dòng)。為了方便說明DDA功能的使用，在設(shè)計(jì)運(yùn)動(dòng)控制指令時(shí)以T型加減速設(shè)計(jì)。

2.SCARA機(jī)器人直線插補(bǔ)指令

Scara_MovL(P,StartVel,MaxVel,EndVel,AccL,DecL)

其中：P為空間點(diǎn)位坐標(biāo)。

MovL程序流程圖如圖5，該指令會(huì)觸發(fā)Timer_Event事件，Timer_Event事件主要負(fù)責(zé)完成運(yùn)動(dòng)過程中的插補(bǔ)運(yùn)算，并將計(jì)算得到的各關(guān)節(jié)放入DDATable中，如圖6。

3.直線插補(bǔ)運(yùn)動(dòng)整機(jī)測(cè)試

完成直線插補(bǔ)程序后進(jìn)行整機(jī)測(cè)試，如圖7所示，為SCARA機(jī)器人從移動(dòng)至?xí)r與關(guān)節(jié)的回授位置曲線和速度曲線：

結(jié)束語

依據(jù)上述程序流程設(shè)計(jì)，采用DDA功能實(shí)現(xiàn)SCARA機(jī)器人直線插補(bǔ)指令、圓弧插補(bǔ)指令、減速停止指令、單軸運(yùn)動(dòng)指令等，并整機(jī)測(cè)試通過。PAC產(chǎn)品具備的DDA功能可以作為一種解決特殊路徑、奇異運(yùn)動(dòng)模型的方法，大幅提升PAC產(chǎn)品的功能屬性，使PAC產(chǎn)品的應(yīng)用領(lǐng)域更加廣泛。