線束插植機械臂控制系統設計

，，，

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

0 引言

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

本文首先針對線束插植工藝流程和要求，搭建了包含SEA串聯彈性執行器柔性結構的直角坐標機械臂，并對其控制系統結構進行設計。然后，開發了專用的線束插植控制器，并對其坐標系示教、多線束自動插植編程、插植力控制功能以及相關的可視化界面進行了編程實現。最后，利用所研制的線束插植控制器，進行了手動插植實驗，實現了機械臂的插植力控制。同時，通過在線編程，進行了線束自動插植實驗，實現了多線束可編程插植。

1 線束插植機械臂控制系統設計

1.1 線束插植的工藝流程

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

圖1 線束插植工藝流程圖

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

1)多線束的可編程插植的實現；

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

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

1.2 線束插植機械臂控制系統的方案設計

本次設計采用直角坐標機械臂為本體，在被動柔順結構——SEA串聯彈性執行器的基礎上，結合閉環PD力控制方法，實現對線束的插植力控制。同時，開發專用的線束插植控制器，通過數控形式，實現多線束的可編程插植以及“二次插植”。整個控制系統的設計要求插植力可調，PD控制器的參數分別取：Kp=8.5，Kd=0.01，單根線束插植總時間控制在4 s左右。

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

在控制系統結構設計方面：機械臂每個軸所配的伺服驅動器以及IO模塊作為EtherCAT從站，通過EtherCAT總線以線性拓撲結構[10]的方式，掛載在EtherCAT主站——線束插植控制器下。線束插植機械臂控制系統結構如圖2所示。

圖2 線束插植機械臂控制系統結構圖

2 線束插植控制器設計

2.1 線束插植控制器總體設計

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

表1 所調用的功能塊

2.1.1 坐標系示教功能設計

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

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

(1)

Z軸方向的矢量為：

(2)

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

分別進行歸一化處理得：

(3)

2.1.2 多線束插植可編程功能設計

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

表2 線束插植控制指令

下面以movel指令的開發和解析為例：

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

圖3 直線運動控制程序

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

圖4 指令解析程序流程圖

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

圖5 語法檢測流程圖

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

2.1.3 插植力控制功能設計

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

圖6 指令解碼程序流程圖

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

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

圖7 插植力控制原理圖

2.2 線束插植控制器界面設計

2.2.1 坐標系示教界面

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

2.2.2 多線束插植編程界面

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

圖9 多線束插植編程界面

2.2.3 力控制插植界面

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

圖10 力控制插植界面圖

在力控制插植界面，能夠對期望插植力進行設置并實時顯示實際的插植力，并且在插植結束之后，通過插植狀態顯示欄中的指示燈，能夠反映當前接插件已經插植完成的接插孔狀態。同時，設置有手動插植功能鍵，結合單軸點動控制按鍵，可以方便地進行手動插植調試。

3 實驗結果與分析

3.1 手動插植力控制實驗

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

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

圖11 插植力控制實驗結果圖

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

3.2 自動插植以及監控測試

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

圖12 自動插植監控測試結果圖

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

部分“二次插植”程序如圖12(a)所示，通過線束插植控制器在線編寫插植程序，能夠實現多線束的可編程插植。由圖12(b)監控結果可知，當所需插植的接插孔，插植完成時，其對應的插植狀態指示燈被點亮，表明所設計的控制器能夠實現對插植狀態的實時監控。

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

4 結論

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