線束插植機械臂控制系統(tǒng)設(shè)計

，，，

(浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023)

0 引言

線束需要經(jīng)過送線、裁線、端子壓接、預(yù)裝工藝以及總裝工藝等一系列工序制作完成[1]，將壓接好的線束裝配到接插件中的過程成為插植。目前，國內(nèi)外對于線束的送線、裁線、壓接自動化技術(shù)研究較多，相關(guān)的自動化產(chǎn)品也都已經(jīng)推向了市場。而線束裝配[2]工藝較為特殊，再加上傳統(tǒng)剛性機械臂一般以位置控制為主，很難實現(xiàn)對線束這類柔性材料的精確控制。因此，線束插植大多仍然依靠人工進行。另外，線束裝配時機械臂末端與接插件存在接觸力，需要對插植接觸力進行控制。近年來，國內(nèi)外學(xué)者紛紛對線束裝配過程中的接觸力進行了研究。2005年，Liu[3]等人通過對線束插孔過程中接觸力的分析，調(diào)整機械手指的運動軌跡，實現(xiàn)了機器人的線插孔操作。2010年，Sun[4]等人采用攝像機初步檢測兩接插件的位置和姿態(tài)，并在接觸過程中根據(jù)力傳感器檢測出的接觸力大小，時時調(diào)整機械臂末端的姿態(tài)，成功實現(xiàn)了兩個接插件的對接。本次設(shè)計利用柔性結(jié)構(gòu)——SEA[5-7]串聯(lián)彈性執(zhí)行器，使得機械臂末端與環(huán)境相接觸時具有較好的柔順性。再結(jié)合閉環(huán)PD力控制方法，可以實現(xiàn)對插植過程中接觸力的控制，避免出現(xiàn)由于接觸力過小而導(dǎo)致線束未插緊，出現(xiàn)線束松動、脫落，或者接觸力過大而造成接插件損壞、導(dǎo)線變形的狀況。SEA串聯(lián)彈性執(zhí)行器是以電機為驅(qū)動源，在電機輸出軸和負(fù)載之間加入彈簧等彈性結(jié)構(gòu)而構(gòu)成的柔性單元，具有低阻抗、體積小、能量密度高、力輸出穩(wěn)定以及對外部沖擊載荷可以起到較好的緩存的特點[8]，已經(jīng)廣泛應(yīng)用于仿生機械臂、康復(fù)機器人、柔順機械臂等領(lǐng)域。2016年，張秀麗[9]等人設(shè)計了一款基于串聯(lián)彈性執(zhí)行器的柔順機械臂-SoftArm II，在機械臂的關(guān)節(jié)處，電機通過串聯(lián)彈性執(zhí)行器與連桿相連，在采用被動柔順裝置SEA的基礎(chǔ)上，柔順機械臂外部采用位置PID控制方法，使得機械臂末端接觸力處于一定的范圍內(nèi)。

本文首先針對線束插植工藝流程和要求，搭建了包含SEA串聯(lián)彈性執(zhí)行器柔性結(jié)構(gòu)的直角坐標(biāo)機械臂，并對其控制系統(tǒng)結(jié)構(gòu)進行設(shè)計。然后，開發(fā)了專用的線束插植控制器，并對其坐標(biāo)系示教、多線束自動插植編程、插植力控制功能以及相關(guān)的可視化界面進行了編程實現(xiàn)。最后，利用所研制的線束插植控制器，進行了手動插植實驗，實現(xiàn)了機械臂的插植力控制。同時，通過在線編程，進行了線束自動插植實驗，實現(xiàn)了多線束可編程插植。

1 線束插植機械臂控制系統(tǒng)設(shè)計

1.1 線束插植的工藝流程

線束插植的工藝流程如下：首先，對接插件所在坐標(biāo)系進行標(biāo)定。然后，根據(jù)接插件的規(guī)格，確定接插孔的坐標(biāo)。最后，利用氣爪抓取線束，進行線束插植。由于線束為柔性材料，且氣爪在抓取線束時，由于抓取的位置與線束端子的距離不同，導(dǎo)致插植距離也不相同。為了防止出現(xiàn)第一次插植距離過短，線束未完全插入接插件中的情況。在第一次插植完成之后，打開氣爪，松開線束，并令在插植方向的Y軸后退一定距離后，重新抓取線束，進行“二次插植”，以此提高插植的成功率，如圖1所示。

圖1 線束插植工藝流程圖

由以上線束插植工藝流程圖可知，本次設(shè)計的線束插植機械臂控制系統(tǒng)必須解決三個問題：

1)多線束的可編程插植的實現(xiàn)；

2)插植過程的插植力控制；

3)進行二次插植，提高插植成功率。

1.2 線束插植機械臂控制系統(tǒng)的方案設(shè)計

本次設(shè)計采用直角坐標(biāo)機械臂為本體，在被動柔順結(jié)構(gòu)——SEA串聯(lián)彈性執(zhí)行器的基礎(chǔ)上，結(jié)合閉環(huán)PD力控制方法，實現(xiàn)對線束的插植力控制。同時，開發(fā)專用的線束插植控制器，通過數(shù)控形式，實現(xiàn)多線束的可編程插植以及“二次插植”。整個控制系統(tǒng)的設(shè)計要求插植力可調(diào)，PD控制器的參數(shù)分別取：Kp=8.5，Kd=0.01，單根線束插植總時間控制在4 s左右。

線束插植機械臂控制系統(tǒng)的設(shè)計包括：機械結(jié)構(gòu)設(shè)計和控制系統(tǒng)結(jié)構(gòu)設(shè)計兩個方面。在機械結(jié)構(gòu)設(shè)計方面：線束插植機械臂的X、Z軸采用上銀的KK60高精度型半封閉式滾珠絲桿線性模組，Y軸采用KK40型線性模組，采用L型角鐵將其兩兩固定，構(gòu)成XYZ三軸直角坐標(biāo)機械臂。在線束插植方向上的Y軸的滑塊下方，設(shè)計安裝SEA串聯(lián)彈性執(zhí)行器。其由法蘭盤、導(dǎo)向桿、U型固定支座、固定擋板、彈簧以及滑動支座構(gòu)成。兩根導(dǎo)向桿穿過通孔，插在U型固定支座和滑動支座內(nèi)，滑動支座的前后各安裝一組彈簧，分別用固定擋板進行固定，使得彈簧保持一定初始壓縮量，滑動支座內(nèi)部放置兩個彈簧自復(fù)位線性位移傳感器。整個線束插植機械臂放置于移動平臺上，平臺的另一側(cè)設(shè)置有用于安放不同規(guī)格接插件的固定架。

在控制系統(tǒng)結(jié)構(gòu)設(shè)計方面：機械臂每個軸所配的伺服驅(qū)動器以及IO模塊作為EtherCAT從站，通過EtherCAT總線以線性拓?fù)浣Y(jié)構(gòu)[10]的方式，掛載在EtherCAT主站——線束插植控制器下。線束插植機械臂控制系統(tǒng)結(jié)構(gòu)如圖2所示。

圖2 線束插植機械臂控制系統(tǒng)結(jié)構(gòu)圖

2 線束插植控制器設(shè)計

2.1 線束插植控制器總體設(shè)計

結(jié)合線束插植的工藝要求，其控制器的設(shè)計主要包括：坐標(biāo)系示教、多線束插植可編程以及插植力控制功能設(shè)計。其中，所利用的CoDeSys自帶的功能塊如表1所示。

表1 所調(diào)用的功能塊

2.1.1 坐標(biāo)系示教功能設(shè)計

針對多線束插植可編程的要求，必須對接插件坐標(biāo)系進行標(biāo)定，進而確定接插孔的坐標(biāo)。本次采用三點示教法進行坐標(biāo)系示教。首先，移動機械臂至接插件原點(第一排最右端接插孔處)并記錄該點，為A(Ox,Oy,Oz)；然后，將機械臂水平向左移動一段距離，記錄下第二點坐標(biāo)，為B(Xx,Xy,Xz)；最后，將機械臂豎直向上移動一段距離，記錄下第三點坐標(biāo)，為C(Yx,Yy,Yz)。

記三軸線束插植機械臂的基座標(biāo)系為{A}，通過三點示教，計算接插件坐標(biāo)系{B}。則接插件坐標(biāo)系{B}相對于{A}，X軸方向的矢量：

(1)

Z軸方向的矢量為：

(2)

Y軸方向的矢量為：AYB=AZB×AXB。

分別進行歸一化處理得：

(3)

2.1.2 多線束插植可編程功能設(shè)計

本次設(shè)計的線束插植控制器要求能夠進行在線編寫程序，通過對編寫的指令進行解析，控制機械臂實現(xiàn)多線束的可編程插植。因此，多線束插植可編程功能設(shè)計包括：插植指令開發(fā)和指令解析。本次開發(fā)的所有插植指令如表2所示。

表2 線束插植控制指令

下面以movel指令的開發(fā)和解析為例：

movel直線運動控制指令對應(yīng)的程序由定時器TON模塊、輸入位置模塊PositionIn以及直線運動模塊MC_MoveLinearAbsolute組成，如圖3所示。

圖3 直線運動控制程序

設(shè)計其有效指令形式為：Movel(3,4,5,6)，前三個參數(shù)分別對應(yīng)程序中的X_Lin、Y_Lin、Z_Lin變量，最后一個參數(shù)對應(yīng)速度Velocity變量。在在線編程界面中輸入該指令，進行編譯，控制器將對該指令進行解析，其解析流程如圖4所示。

圖4 指令解析程序流程圖

首先進行語法檢查，語法正確則對指令進行解碼判斷。然后根據(jù)解碼結(jié)果，讀取參數(shù)后調(diào)用相應(yīng)的控制程序。其中，語法檢查包括：檢查指令字母、檢查左右括號、檢查逗號以及檢查句末分號。首先檢測輸入的字符前5個是否為有效字母(設(shè)計規(guī)定控制指令有效字母數(shù)為5)，如果正確，則繼續(xù)檢測第6個字符是否為“(”。然后，檢測字符“(”之后的字符是否為數(shù)字或者“，”。如果檢測到數(shù)字或者“，”后，則繼續(xù)檢測是否出現(xiàn)字符“)”。最后，如果檢測到字符“)”，則繼續(xù)檢測是否有字符“；”。若出現(xiàn)字符“；”，則語法正確；否則，語法錯誤。語法檢測流程如圖5所示。

圖5 語法檢測流程圖

進行指令解碼時，依次讀取前5個字符的ASCII值，進行指令判斷。如果檢測到前5個字符的ASCII值對應(yīng)的字符依次為m→o→v→e→l，則將n賦值為2。指令解碼程序流程圖如圖6所示。指令與n對應(yīng)關(guān)系如表3所示。

2.1.3 插植力控制功能設(shè)計

針對插植力可控的工藝要求，在設(shè)計被動柔順機構(gòu)——串聯(lián)彈性執(zhí)行器的基礎(chǔ)上，采用閉環(huán)PD力控制方法，實現(xiàn)對插植力的控制。插植力控制程序由PD功能塊和MC_ReadActualPosition讀取電機實際位置模塊構(gòu)成。利用IO模塊的12位精度ADC實時檢測KS1-15彈簧自復(fù)位直線位移傳感器的電壓值，將檢測出的電壓V轉(zhuǎn)化為實際接觸力f實際，再與設(shè)定的期望接觸力f期望作比較后，輸出給 PD功能塊。經(jīng)過PD功能塊，得出位置增量，再與MC_ReadActualPosition功能塊讀取的電機實時位置相加，作為

圖6 指令解碼程序流程圖

名稱n的值名稱n的值movej1plant6movel2uplan7delay3state8opend4coord9close5其他10

輸出信號輸出給電機，從而控制電機運行的位置，使得彈簧的壓縮量保持期望的值，實現(xiàn)對插植力的控制。閉環(huán)PD插植力控制原理如圖7所示。

圖7 插植力控制原理圖

2.2 線束插植控制器界面設(shè)計

2.2.1 坐標(biāo)系示教界面

坐標(biāo)系示教界面包括單軸點動控制按鍵、系統(tǒng)狀態(tài)顯示、界面跳轉(zhuǎn)功能鍵、回零按鍵、示教點及坐標(biāo)系顯示、示教功能鍵六個部分。利用控制器操作機械臂到三個示教點O、X、Y后記錄其點的坐標(biāo)，調(diào)用坐標(biāo)系示教程序即可生成所示教的坐標(biāo)系。坐標(biāo)系示教界面如圖8所示。

2.2.2 多線束插植編程界面

多線束插植編程界面包括5個部分：系統(tǒng)信息狀態(tài)欄、程序編輯文本框、界面跳轉(zhuǎn)功能鍵、程序文件處理鍵以及文件信息欄。通過編輯程序文本框可在線編寫插植程序，當(dāng)編譯正確時點擊運行按鈕即可執(zhí)行線束插植。多線束插植編程界面如圖9所示。

圖9 多線束插植編程界面

2.2.3 力控制插植界面

力控制插植界面包括：插植力設(shè)置、狀態(tài)顯示、單軸點動控制按鍵、界面跳轉(zhuǎn)功能鍵、手動插植按鈕以及機械臂速度位置信息顯示6個部分，如圖10所示。

圖10 力控制插植界面圖

在力控制插植界面，能夠?qū)ζ谕逯擦M行設(shè)置并實時顯示實際的插植力，并且在插植結(jié)束之后，通過插植狀態(tài)顯示欄中的指示燈，能夠反映當(dāng)前接插件已經(jīng)插植完成的接插孔狀態(tài)。同時，設(shè)置有手動插植功能鍵，結(jié)合單軸點動控制按鍵，可以方便地進行手動插植調(diào)試。

3 實驗結(jié)果與分析

3.1 手動插植力控制實驗

通過ATI公司的Mini45 Net六維力/力矩傳感器，多次測量接觸力與彈簧形變量Δl以及Δl與位移傳感器電壓之間的關(guān)系，得出：k3=0.013 6 N/V。

設(shè)定PD控制器的參數(shù)分別為：Kp=8.5，Kd=0.01，設(shè)定期望插植力F=5N。本次插植實驗所使用的線束，其前部是帶有卡槽的銅端子，寬度2 mm。所使用的線束接插件共有三排接插孔，每一排共有10個接插孔。每個接插孔寬度約2.5 mm，高度約5 mm，孔間距不到0.5 mm。通過線束插植控制器的手動插植功能鍵，進行插植力控制實驗，測試結(jié)果如圖11所示。

圖11 插植力控制實驗結(jié)果圖

由實驗結(jié)果可知，運行至3.5 s左右，插植力逐漸增大至設(shè)定的期望插植力，由此可知該段曲線與線束插入接插孔內(nèi)部的過程相對應(yīng)。當(dāng)實際插植力的大小到達5 N后，在5 N附近振蕩，表明此時線束端子已經(jīng)與接插孔完全卡牢即插植完成，實現(xiàn)了對插植力的控制。當(dāng)實驗運行至9.3 s時，實際插植力為4.519 2 N，此時實際插植力與期望插植力的偏差最大為-0.480 8 N，最大誤差不到±10%。考慮自復(fù)位線性位移傳感器自身的檢測精度以及該應(yīng)用場合的力控制精度要求不高，達到了預(yù)期的插植力控制要求。

3.2 自動插植以及監(jiān)控測試

自動插植實驗要求：對接插件第一排四個接插孔(從右往左排序)進行插植，并顯示其插植狀態(tài)。根據(jù)實際接插件的規(guī)格，記第一排最右端的接插孔對應(yīng)坐標(biāo)(0,0,0)，則所需插植的接插孔對應(yīng)坐標(biāo)為(9,0,0)。

圖12 自動插植監(jiān)控測試結(jié)果圖

首先，操作線束插植運動控制器，對接插件的坐標(biāo)系進行標(biāo)定，進而確定每一個接插孔對應(yīng)的坐標(biāo)。然后，在多線束插植編程界面中，調(diào)用插植指令，編寫“二次插植”程序。最后，編譯無誤后，點擊運行按鍵，執(zhí)行“二次插植”程序。自動插植監(jiān)控測試結(jié)果如圖12所示。

部分“二次插植”程序如圖12(a)所示，通過線束插植控制器在線編寫插植程序，能夠?qū)崿F(xiàn)多線束的可編程插植。由圖12(b)監(jiān)控結(jié)果可知，當(dāng)所需插植的接插孔，插植完成時，其對應(yīng)的插植狀態(tài)指示燈被點亮，表明所設(shè)計的控制器能夠?qū)崿F(xiàn)對插植狀態(tài)的實時監(jiān)控。

從圖12(c)中可知，系統(tǒng)完成二次插植時間較長，整個過程耗時21 s左右。其中，除去系統(tǒng)運行之前，監(jiān)控示波器自身走過的時間3 s，以及設(shè)置的二次插植間隔時間7 s，整體二次插植時間大約10 s左右。考慮到本系統(tǒng)處于樣機開發(fā)階段，故設(shè)置整體運行速度和插植速度均較慢。而且為了觀察插植力是否實現(xiàn)有效控制，在每一次插植完成之后，均進行了一定程度的延時。因此實際進行“二次插植”的時間為5～6 s，基本滿足預(yù)期的設(shè)計要求。后期可以通過優(yōu)化插植動作的時間分配和提升插植樣機的運行速度，來降低線束插植時間，進一步提高插植效率。

4 結(jié)論

針對線束插植機械臂系統(tǒng)設(shè)計需要實現(xiàn)的目標(biāo)：多線束可編程插植、插植力控制、“二次插植”，設(shè)計了一種基于SEA的線束插植機械臂控制系統(tǒng)。介紹了線束插植的工藝流程，提出了線束插植機械臂的設(shè)計方案。并且研制了專用的線束插植控制器，對控制器的坐標(biāo)系示教、多線束自動插植編程、插植力控制功能以及對應(yīng)的可視化人機界面進行了設(shè)計，實現(xiàn)了插植力的控制。并且，利用設(shè)計的控制器在線編寫“二次插植”程序，實現(xiàn)了多線束的可編程插植。本次設(shè)計的線束插植機械臂控制系統(tǒng)能夠應(yīng)用于自動化線束裝配領(lǐng)域，但還需進一步優(yōu)化插植動作的時間分配，提高插植的效率。