基于CoDeSys平臺的六自由度工業機器人運動控制器設計

王耀東，徐建明，徐勝華

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

0 引言

隨著機器人在現代工廠的廣泛應用，機器人技術得到了迅猛發展，使得傳統工業發生新的變革。在生產、醫療、物流等新興領域，機器人的應用改變了整個行業的發展模式。面對現代工業發展需求的不斷提高，降低生產成本、縮短開發周期和擴大應用領域尤為重要。目前，機器人控制器多采用專用的控制器，成本高，開放性和移植性差。這就需要開發出一套高開放性、可移植性和可擴展性的機器人控制系統。

控制器是機器人控制系統的核心，機器人的運動學求解、運動控制和軌跡規劃等都由控制器完成。控制器大致分為四類，基于單片機的控制器［1］、基于PLC的運動控制器［2］、基于PC運動控制卡的控制器［3］以及軟件型運動控制器［4］。其中基于PCI總線的運動控制卡控制方式和軟件型運動控制器在當下運用最為廣泛。運動控制卡采用高性能微處理器，如DSP和大規模可編程設備，因此具有比較強大的計算能力和邏輯處理能力，大多數控制卡擁有統一的接口，開放性強。而軟件型運動控制器是建立在計算機強大的計算能力基礎之上，只需要將開發好的軟件安裝在PC或者嵌入式開發板上，該平臺就具有運動控制能力，且該系統開放性強，具有邏輯語言編程、CNC多軸同步插補等優點。

目前，任福深等人［5］基于PC機開發了開放式的焊接機器人控制系統，該系統運行穩定，適應性強，可以很好的完成焊接任務。馬瓊雄等人［6］采用基于工控機 (IPC)結合運動控制卡的結構形式，并借鑒ORC(Open Robot Controller)等典型機器人控制系統軟件體系結構設計了分層模塊化的軟件系統，使得現有機器人控制系統只需要做少量修改就可以應用到不同場合。胡鵬等人［7］采用IPC+DSP作為六軸工業機器人的控制器，設計了一種基于可編程多軸控制器 PMAC(Programmable Multi－Axis Controller)的開放式機器人控制系統，門昌華等人［8］以IPC+PMAC作為CIN CINNATI工業機器人的控制器，設計了一種基于PMAC的開放式機器人控制系統。

本文以埃夫特公司的ER50機器人為研究對象，采用基于ARM+CoDeSys架構設計機器人運動控制器，并采用EtherCAT實時以太網通信技術，實時性強，運行速度快。該控制器具有機器人示教、在線編程、建立坐標系、關節點動控制、笛卡爾點動控制、軌跡插補規劃、一鍵回零、各軸位置實時顯示等功能。

1 控制系統結構方案

本文采用在ARM板上安裝CoDeSys軟件作為開發平臺，構成一個支持邏輯與機器人運動控制相結合的可編程邏輯控制器。上位機采用CoDeSys軟件提供的可視化模塊進行設計開發，驅動器采用清能德創的CoolA8驅動器，控制總線選擇EtherCAT通訊總線，機器人本體使用ER50機器人。整個系統采用模塊化、總線式設計，因此整個機器人控制系統具有很強的通用性與開放性。系統架構如圖1所示。

圖1 機器人控制系統整體架構圖

本文在CoDeSys軟件平臺上，基于PLCopen規范［9-10］對工業機器人運動控制器的軟件部分進行設計。設計目標是基于CoDeSys準確搭建ER50機器人的正逆運動學模型，基于該模型開發一款控制精度高，運行穩定，且具有點動、示教、在線編程等功能的機器人運動控制器。首先對根據ER50機器人的DH參數建立連桿坐標系進而進行運動學建模，然后基于該運動學模型實現機器人關節空間點動，笛卡爾空間點動，基本的軌跡規劃以及機器人示教和在線編程等功能，同時針對各功能設計良好的人機交互界面。

2 ER50機器人運動學建模

機器人的機械結構和運動學密切相關。ER50機器人的機械本體屬于控制系統的下層，是控制系統上層機構的控制對象。機器人本體設計和動力學不在本文的研究范圍之內。機器人運動學方程又與本體結構密切相關，其運動學的正解和逆解是控制系統軟件發送控制指令的依據，是工業機器人的軌跡規劃、軌跡跟蹤以及位置伺服控制的基礎。因此本文以六自由度關節型機器人ER50為例，對工業機器人的運動學模型進行分析。

2.1 ER50機器人連桿坐標系建立

對于ER50機器人相鄰坐標系之間的坐標變換，一般先對全部連桿從基座到機器人末端執行器依次建立相鄰連桿i－ 1 與 i之間的變換關系。坐標系 Oi－1Xi－1Yi－1Zi－1的兩次旋轉和兩次平移可以變換到坐標系OiXiYiZi，圖2為相鄰連桿坐標系的建立:

1) 繞軸 Zi－1旋轉 θi角，使得 Xi－1軸與軸 Xi處于同一個平面內;

2) 沿Zi－1軸平移di個距離，使得軸Xi－1與軸Xi共線;

3) 沿 Xi－1軸 平 移 ai個 距 離，使 得 坐 標 系Oi－1Xi－1Yi－1Zi－1和 OiXiYiZi坐標原點重合;

4)繞Xi軸旋轉αi角，使兩坐標系重合。

圖2 相鄰連桿坐標系與參數

齊次變換矩陣可表示為:

其中:Trans為平移變換矩陣，Rot為旋轉變換矩陣，具體可表示為:

ER50機器人的D－H參數如表1所示，其中di為關節軸i上的連桿偏距，ai－1為關節軸i－1和關節軸i之間的公垂線長度 (連桿長度)，αi－1表示關節軸i－1和關節軸i之間連桿轉角。由于機器人本體的機械傳動耦合關系，四關節的轉動將影響五、六關節的轉動，五關節的轉動又會帶動六關節的旋轉，關節耦合比如表2所示，本文設計的機器人運動控制器通過軟件編程補償了關節耦合對機器人控制產生的誤差。由機器人的連桿參數和關節的旋轉參數確定機器人操作臂末端坐標系在基坐標中位姿關系，建立ER50機器人連桿坐標系如圖3所示，根據該連桿坐標系關系建立ER50機器人的正逆運動學模型。

表1 ER50機器人D－H參數表

表2 ER50機器人關節耦合比

圖3 ER50機器人連桿坐標系

2.2 ER50機器人運動學模塊封裝

2.2.1 正運動學功能塊封裝

本文正逆運動學求解方式借鑒《機器人學導論》［11］書中所提出的方法。將正運動學公式用程序代碼表述并封裝成功能塊，程序功能塊如圖4(a)所示，config為機器人的D－H參數配置，drive0至drive5為關節角當前位置信息，Position是經過正運動學得出的笛卡爾空間位姿。armState、elbowState、handState分別是第一關節、第三關節和第五關節的狀態。

2.2.2 逆運動學功能塊封裝

機器人逆運動學是指已知機器人工具末端的位置和姿態，通過逆運動學變換求出機器人各個關節軸對應的角度。由于本文設計對象為六軸串聯型機器人，所以根據逆運動學方程變換最多可求出八組逆解，根據關節變化量最小的原則選取最優逆解，逆運動學變換功能塊在調用時的實例圖如圖4(b)所示。模塊中_pre表示上一次變換后各關節軸對應的關節角，pos表示需要變換的機器人末端位姿。dq表示經功能塊變換后得出的各關節軸的角度。

3 機器人運動控制器功能開發

圖4 運動學功能封裝

在CoDeSys軟件平臺上開發機器人運動控制器的各個功能，本文主要從示教模式、點動模式以及在線編程模式幾個方面來闡述該運動控制器的軟件開發過程。

3.1 示教模式

3.1.1 示教流程

通過使用笛卡爾空間點動或關節空間點動功能控制機器人工具末端運動到需要示教的位置點，記錄關節坐標以及笛卡爾空間末端執行器位姿信息。示教編程時，可根據不同運動類型選擇不同運動模式，供選擇的模式有MoveL、MoveJ以及MoveC三種運動模式，在文件程序運行時判斷指令的ASCLL碼值，然后再調用對應運動功能塊。示教流程如圖6所示。控制流程實現了Wait()函數，采用定時器TON功能塊實現，定時時間可通過參數傳入。

圖5 示教流程圖

手動操作機器人到各個示教點后保存示教點。再應用示教點指令生成示教程序，保存程序到.txt文件中，示教采集點和示教編程界面分別如圖6(a)、6(b)所示。本系統根據機器人語言常用規范實現了一些基本指令，組成本系統示教再現模塊的機器人指令集。機器人的指令集包括動作指令集、控制指令集。動作指令集包括 MoveC，MoveL，MoveJ，控制指令集有 Wait、Jump。MoveC、MoveL是圓弧插補和直線插補，MoveJ是笛卡爾空間點到點的軌跡規劃。在選擇插補方式時設定對應運動速度v=5，執行語句“MoveL［p0，p1］;”則機器人末端將從一個點p0沿直線軌跡運動到當前點位置p1，其速度為5 mm/s。通過Read按鈕可以讀取到示教文件上的各個示教點信息，可以在線編輯修改示教文件，以及插入、刪除示教點。如果機器人在運行過程中出現問題，為了安全保障，可以選擇Stop按鈕，緊急停止所有操作。

圖6 示教界面

3.2 點動模式

3.2.1 關節空間點動

關節空間點動模式采用MC_Jog功能塊實現，可以單獨控制關節軸的正反轉，以及運行時的速度、加速度，也可以根據需要獨立設置，Axis為電機軸對應的控制字。機器人控制系統單軸模式的程序圖如圖7所示。MC_Power功能塊用于控制驅動器的使能狀態，是控制伺服驅動器電源模塊，上電使能是電機軸運動的必要前提。該模塊中，Axis為關節軸控制字，對應于具體的關節軸電機驅動器，通過置Enable、bRegulatorOn和bDriveStart三個輸入控制字為true即可使能伺服驅動器電源。MC_Reset實現電機軸的復位操作，用于電機軸從ErrorStop狀態到StandStill狀態的轉換。

圖7 單軸模式程序圖

3.2.2 笛卡爾空間點動

笛卡爾空間點動，設計此功能的目的在于使用示教器示教或需要平移末端工具點時，能更加快速到達目標點，使用時更加方便和快捷。此功能設計有6個功能按鍵，分為X+、X－、Y+、Y－、Z+、Z－，分別控制機器人工具末端沿著世界坐標系中的X、Y、Z坐標軸移動。以X軸為例，當點擊X+時，工具末端會沿著X軸正方向移動。此功能的設計原理是通過功能按鍵來控制末端位置的坐標值增加或減小，然后再根據機器人坐標系模型中的逆運動學方程，將末端位置轉換為各個關節角度，再將轉換出的各個關節角度作為各軸連接的電機的設定位置，從而控制各軸同步運動到相應的位置，達到預期目的。

3.2.3 點動模式界面設計

主要包含常用增刪改查的操作，另包括：數據資源災備(自動備份，異常情況下，保證數據資源安全)、數據庫信息標識、日志信息查看、索引自動創建查看、授權信息、執行計劃、服務性能、SQL審核、故障錯誤信息、系統升級、系統調用及部署等。

點動界面包含兩種點動模式，分別為關節空間點動和笛卡爾點動，每種模式單獨設計一個界面，界面包含點動控制的各功能按鍵、保障安全的急停按鍵、模式之間相互切換的切換按鍵、顯示軸運動狀態的狀態顯示燈、實時顯示各軸關節位置或工具末端位姿數據的數據狀態欄等。在關節空間下，通過控制各個關節來實現手動運行如圖8(a);在運行速度上為了安全沒有提供在線設置運行速度，而是提供了三種運行速度模式，分為Slow、Middle、High三種，這樣操作者可以根據需求在線選擇運行速度。另一種是在笛卡爾空間進行手動操作如圖8(b)，可以手動移動在空間中的位置以及姿態，通過三維坐標的修改按鈕，以及歐拉角的旋轉按鈕進行增加歐拉角度或者減少，從而改變末端姿態，使其運行到期望位置。

圖8 點動界面

3.3 在線編程模式

3.3.1 在線編程實現

在線編程支持新建.txt編程文件，或者加載之前已有的編程文件并再此基礎上進行修改，然后保存。運動指令主要有MoveJ、MoveC和MoveL三種，當在線編程時目標位置以及中間點位置要在機器人運動范圍內以及運動指令要符合特定語法，如 MoveJ(10，20，30，10，10，5，5);并以 “;”結束，前6個參數表示目標位姿，最后一個參數表示運動速度。如果不符合這種語法在編譯時會報錯。錯誤類型主要有:指令不符合規范、文件為空、指令沒有以分號結束等。編譯完成無誤情況下，可以通過運行按鈕執行文件中的指令。控制指令有Wait指令，暫停一定時間。在線編程流程圖如圖9所示。

圖9 在線編程流程圖

3.3.2 在線編程界面

在線編程界面如圖10所示，在線編程界面包含當前坐標系顯示欄、機器人運動狀態顯示欄、程序編譯錯誤顯示欄、程序文件管理模塊、程序文本編輯及修改欄、界面切換功能鍵、程序操作功能鍵及文件操作功能鍵。程序操作功能鍵包括對程序編譯和運行，文件操作功能鍵包括文件新建、打開、保存、關閉和刪除。通過編程界面可在線管理程序代碼，存儲在控制器中的程序文件也會在文件管理模塊中顯示，也可以新建.txt類型的程序文件。通過對程序文件修改保存后，可點擊編譯按鈕，對程序進行編譯，觀察編寫的程序是否有語法錯誤，編譯無誤后，可點擊運行按鈕，機器人就會按照程序功能控制機器人按照預定的軌跡到達目標位置點。通過查看機器人狀態顯示欄查看機器人運動狀態。

圖10 在線編程界面

4 實驗結果與分析

在ER50六軸工業機器人上對設計的運動控制的在線示教、在線編程功能進行測試。實驗對象如圖11。

使機器人姿態保持不變，通過運動控制器的示教功能示教機器人位置，保存4個點如下:

P0(496．213，170．285，612．158，16，23，121)

P1(728．610，296．926，786．103，16，23，121)

P2(950，196．734，1723．02，16，23，121)

P3(1246．207，287．654，1810．143，16，23，121)

圖11 ER50機器人本體+運動控制器

再通過在線編程功能執行語句“MoveL［P0，P1］;”實現點 P0到 P1的直線插補，以及 “MoveC［P1，P2，P3］;”實現點P1、P2、P3之間的圓弧插補。機器人末端位置的三維坐標記錄如圖12所示。將機器人運行數據保存起來并在matlab中對插值數據進行繪圖，得到規劃的軌跡曲線圖如圖13所示，將實際定位的P0、P1、P2、P3點的坐標與示教定位點坐標進行誤差對比，數據如表3所示。

圖12 機器人末端坐標位置

圖13 機器人末端三維軌跡

表3 定位精度表

通過本文設計的控制器示教功能和在線編程功能實現了點P0到P1的直線插補，點P1，P2和P3的圓弧插補，從圖12、13可以看出P0到P1點保持嚴格的直線運行，P1、P2、P3點間的軌跡平滑，足以說明該控制器具有很好的示教、在線編程功能以及運行穩定性;通過表3可以看出，運行點P0、P1、P2、P3與其示教點誤差分別為0.1312 mm、0.0947 mm、0.1466 mm、0.1341 mm，完全滿足工業生產中的加工精度要求，可見該控制器對ER50機器人的正逆運動學模型建立準確。

該機器人控制器功能眾多，本文僅對直線、圓弧插補功能進行測試，從測試過程可以看出，本文設計的這款基于CoDeSys軟件平臺的運動控制器運行穩定、控制精度高，同時具備了豐富的使用功能和人機交互界面，滿足大多數機器人平臺的運動控制要求。

5 結束語

本文研究了一種基于ARM+CoDeSys架構的運動控制器。以ER50六自由度工業機器人為控制對象，結合Ether-CAT通信技術和PLCopen規范，研發了一款具備在線示教、在線編程、關節空間與笛卡爾空間點動以及軌跡插補規劃等功能于一體的機器人運動控制器。該款控制器控制精度高，運行穩定，具有很好的開放性、可移植性和低成本等優勢，已在六軸工業機器人、SCARA機器人以及三軸機器人等平臺工廠化使用，并取得了預期控制效果。