基于TwinCAT的機器人虛擬控制系統設計

張煒岸 吳世林

(武漢紡織大學，湖北武漢430200)

近年來，隨著《中國制造2025》行動綱領的提出，中國工業正有條不紊的向目標邁進。而如今，工業機器人不僅僅活躍在加工制造、食品包裝等行業，也開始逐步被引入大學校園，成為學業學習的一部分。由于工業機器人技術的普及，且工業機器人的種類繁多，開發一種具有低成本、魯棒性、高兼容性的機器人控制系統迫在眉睫。

其中以六軸的串聯臂機器人應用最為廣泛，而六軸工業機器人市場早已被“四大家族”分割[1］，整套純進口的工業機器人價格是十分高昂的。因此，如果能夠選用國產的驅控系統，便能大大減少預算和額外開支。除開國產驅動器選擇性較多外，作為整個機器人控制系統的核心——控制器的選擇則是重中之重。通過TwinCAT3可以將隨手可見的個人計算機“改造”成一臺性能精良的機器人控制器。

TwinCAT3是德國倍福公司發布的一款基于Windows操作系統的軟件，同時兼容32位系統和64位系統。TwinCAT3將非實時的Windows系統轉換為一個多核的實時控制系統，滿足工業機器人對實時性、穩定性、安全性的要求。除了支持IEC 61131-3國際工業控制編程標準的低級編程語言之外，還支持例如C＋＋等高級語言的混合編寫，為后續開發復雜的算法提供了富裕的空間與方法。

TwinCAT NC軸是通過現場總線與驅動器和測量系統循環交換數據來仿真的控制模塊，由于個人計算機的處理能力十分強大，因此可以輕松地對多個運動軸進行定位和調試，通過優秀的定位算法，對生成輪廓時具有加加速度限制以及對速度和加速度的預控制，可以將跟隨誤差降到最小。

本文通過載有TwinCAT3的個人計算機作為機器人控制器，CPU為酷睿i5多核處理器，內裝Windows 7 64位系統用以兼容Windows平臺的其他軟件。

1 運動學分析

六軸工業機器人通常指的是六關節串聯機器人(圖1)，根據右手定則，在軸一到軸六建立六個坐標系，其中坐標系{B}和坐標系{O}重合。

1.1 位置正解

串聯機器人的運動學正解是已知當前各軸的角度求末端工作坐標。

根據連桿變換矩陣公式:

展開得到:

將這些連桿變換矩陣連乘:

色差儀，CR-400，日本Konica Minolta公司；質構儀，TA.XT Plus，英國SMS公司；手持式糖度計，PAL-1，日本ATAGO；游標卡尺，上海恒量量具有限公司；電熱恒溫鼓風干燥箱，DHG-9075A，上海齊欣科學儀器有限公司；蔬菜分辨器，臺灣天燁集團有限公司。

隨后經過計算便可得到坐標系{T}相對于坐標系{O}的變換矩陣[2］:

再分別計算矩陣中各元素的值即可。值得一提的是，矩陣中px、py及pz便是坐標系{T}相對于坐標系{B}位置參數。

因為運動學正解計算相對簡單，完全可以用st語言進行編寫，直接運行在TwinCAT PLC中，因此，可以把正解算法通過TwinCAT3的PLC模塊封裝成一個單獨的函數功能塊，從而方便用來計算機器人末端的坐標。

1.2 位置逆解

即已知點坐標，反求相應各軸的角度。

對于串聯六軸機器人，求逆解的方式存在多種，在這里采用代數解法進行求解.但是由于求解過程的復雜程度高于正解，并且存在多解的情況。因此，需要根據實際情況對多解進行篩選從而選擇最為合理的逆解角度，比如六軸進行耦合之后的工業機器人各軸運動范圍是受限制的，因此首先要舍棄超出各軸運動范圍的解。

而由于運動學逆解與正解不同，通常不會直接用來使用，而是通過用來實現機器人的軌跡規劃簡介使用，例如直線插補、圓弧插補等功能[3］。

直線插補是基于笛卡爾空間的軌跡規劃，需要反復求解逆運動學方程來求解下個中間點的姿態[4］，如下給出算法基本邏輯:

已知笛卡爾空間中兩點A、B，插補次數為N，則有:

那么對于每個周期坐標系XYZ各軸插補增量有:

對于該規劃的直線上任意一點i的坐標是:

在進行逆解時，有時候在排除了超過運動范圍的解后還會出現不止一個解，這時，可以綜合計算軸的移動距離，從而選擇轉動量最少的一組解，或者也可以遵循“少轉大軸，多轉小軸的”的原則，來對多解進行篩選。這樣，在每一次逆解后，就可以確保能得到一組符合要求的解值。

當然，為了保證整個架構清晰、方便后續開發調用，需要把逆解算法單獨用C＋＋封裝成一個模塊。

圖2是插補的的基本算法流程圖。

2 軟件設計

2.1 用C＋＋編寫逆解算法，生成TcCOM對象

運動學逆解計算復雜，過程中需要進行多次反三角函數計算，如果和正解一樣用st語言編寫會顯得比較麻煩，還會使TwinCAT PLC系統資源被過多占用，而剛好TwinCAT3同時又支持C＋＋編程，因此最好的方法便是通過C＋＋來編寫逆解算法，然后通過PLC來調用。

TC1300是TwinCAT3的一個C＋＋組件，它可以將C＋＋模塊運行在實時環境下。我們首先通過“新建”一個C＋＋模塊并命名“InverseKinematic1”，隨后選擇“TwinCAT Module Class with Cyclic IO”模板，這個模板是一個標準模板，作用是用于建立C＋＋模塊的IO數據接口。隨后點擊項目下的“TMC File”文件夾里面的“InverseKinematic1.tmc”文件，如圖3所示。

在這里需要添加輸入量和輸出量，輸入變量需要添加逆解需要的數據變量和使能信號，輸出變量添加六軸的角度坐標。在建立完需要鏈接的變量后，右鍵工程名稱，點擊“TwinCAT TMC Code Generator”對新建的變量進行代碼生成，這樣一來，我們在編寫程序時，才可以調用這些IO變量。

與編寫C＋＋一般程序不同的是，用于TwinCAT3的C＋＋代碼需要寫在特定的位置。對于需要實時循環的代碼我們放在一個特定方法“CycleUpdate()”內，如圖4所示。

在編寫程序時，首先要注意的是IO變量的調用需要通過訪問成員的方式來調用，比如”m_Inputs.px”、”m_Outputs.theta1”。其次，因為逆解的運算涉及到一些函數方法的使用，比如“sin()”、“atant2()”等，而在 Twin-CAT3的C＋＋模塊中，并不能通過一般包含頭文件“math.h”的方法訪問函數，因為TwinCAT3將“math.h”頭文件更名為“fpu87.h”，其中所有的函數都需要在結尾添加“_”，比如“sin_()”、“atant2_()”。 最后，Windows自帶的API并不能直接運行在內核模式中，如果需要，可以使用TwinCAT提供的SDK來實現。

在編寫完代碼后，還需要做的便是利用TcCOM技術將算法實例化生成TcCOM對象，使逆解算法生成相應的變量接口，再依次與PLC程序里相應的變量進行鏈接。這樣，C＋＋中的變量便能訪問PLC中的變量數值，從而進行實時數據交換。

2.2 PLC編寫主體程序，HMI編寫界面

對于串聯的六軸機器人，需要控制6臺電動機，而各軸最終的運動都是通過TwinCAT NC軸來完成，因此我們先要建立6個NC虛擬軸用來對應機器人的6個軸，在PLC程序中，要建立相應的軸變量“axis_ref”來與NC軸一一鏈接，這樣就可以監測軸的位置狀態和運行狀態。在用PLC封裝的正解功能塊里，我們將六軸的絕對位置作為輸入，那么輸出的則是坐標系{6}在笛卡爾坐標系的位置參數，如圖5所示。程序編寫完后，再通過HMI插件對界面進行繪制。

對于六軸機器人最基礎的功能便是示教，點對點等功能，在TwinCAT平臺上，實現這些功能可以通過調用運動功能庫中的Jog和MC_MoveRelative功能實現，由于軸運動是基于TwinCAT NC軸上的，因此，對于各軸運動的精度和速度規劃是由NC模塊內部自動完成，以每2 ms的周期計算下一次的位置，因此可以十分精準和平滑地運動到所需要的位置。而在軸運動的同時，由于正解功能塊的輸入為各軸的絕對位置，我們同時也設定PLC的運行周期以2 ms的周期運行，因此保證了正解功能塊計算的準確性和實時性。如圖7、8截取了通過示教點動造成的隨機角度所對應的笛卡爾坐標系的坐標和PTP功能中軸1的仿真結果。

3 結語

本文主要介紹了通過TwinCAT3和C＋＋設計了一個基于DH模型的六軸串聯機器人的運動控制系統，在硬件部分，可以任意選用總線的驅動器，僅需要將相關的ESI通信文件放入相關文件夾中，然后將NC虛擬軸與IO實體軸一一鏈接，便能實現通過仿真到實體運行的結果，因此TwinCAT3平臺使得本系統的兼容性得到了極大拓展。在軟件部分，首先通過DH建模實現了正逆解的推導和算法編寫，之后通過TwinCAT3組件TC1300將逆解算法用C＋＋封裝，之后通過接口與TwinCAT3軟件的PLC平臺進行數據交互。最后在PLC平臺上編寫并封裝了正解、點對點、直線插補等基本功能塊，并通過仿真驗證了算法的正確性和實時性。因此，基于TwinCAT3軟件平臺的開發不僅提高了軟件編寫的效率，同時減少了機器人開發的成本與周期。