基于以太網(wǎng)通信和OpenGL的庫(kù)卡機(jī)器人在線控制及仿真

杜必強(qiáng) 陳佳楠

（華北電力大學(xué) 能源動(dòng)力與機(jī)械工程學(xué)院，保定 071000）

目前工業(yè)機(jī)器人應(yīng)用范圍的越來(lái)越廣，示教編程作為大部分機(jī)器人設(shè)計(jì)的編程方式，存在過(guò)程繁瑣、精度較低的，且示教過(guò)程存在安全隱患并占用較多機(jī)器人工作時(shí)間，導(dǎo)致傳統(tǒng)機(jī)器人示教編程方式已經(jīng)無(wú)法滿足現(xiàn)代化生產(chǎn)制造的要求。隨著計(jì)算機(jī)圖形技術(shù)和可視化技術(shù)的發(fā)展，離線編程逐漸成為一種主流編程方式，大部分國(guó)外離線編程軟件通用性差，且價(jià)格昂貴，國(guó)內(nèi)在機(jī)器人離線編程仿真領(lǐng)域也進(jìn)行了相關(guān)的研究，但和國(guó)外相比仍有一定的差距[1]。

本文利用C#編程語(yǔ)言，基于socket的C/S（客戶端/服務(wù)器）結(jié)構(gòu)以及庫(kù)卡編程語(yǔ)言開(kāi)發(fā)了一種通信模塊和在線運(yùn)動(dòng)控制界面，以此在NET框架下對(duì)OpenGL圖形庫(kù)進(jìn)行調(diào)用，建立機(jī)器人三維模型，實(shí)現(xiàn)機(jī)器人末端軌跡在線運(yùn)動(dòng)控制以及運(yùn)動(dòng)仿真功能。

1 機(jī)器人通信及在線控制界面搭建

1.1 基于以太網(wǎng)的數(shù)據(jù)通信

庫(kù)卡的Ethernet KRL可以實(shí)現(xiàn)庫(kù)卡控制系統(tǒng)和外部系統(tǒng)之間的以太網(wǎng)通信，并支持TCP協(xié)議以及UDP協(xié)議。UDP/IP是無(wú)連接的網(wǎng)絡(luò)協(xié)議，在通信過(guò)程中可能丟失數(shù)據(jù)，因此本文采用面向連接的TCP/IP通訊協(xié)議，并通過(guò)C/S模式將PC端作為通信服務(wù)器端，而庫(kù)卡機(jī)器人控制器作為通信客戶端，通過(guò)EthernetKRL以太網(wǎng)通信連接庫(kù)卡機(jī)器人控制系統(tǒng)上的KLI接口以及PC機(jī)的以太網(wǎng)接口，并通過(guò)XML文件格式進(jìn)行數(shù)據(jù)的傳輸，從而實(shí)現(xiàn)庫(kù)卡機(jī)器人的在線運(yùn)動(dòng)控制[2]。

1.2 服務(wù)器程序

socket是一組接口，主要由IP地址和端口號(hào)組成，建立網(wǎng)絡(luò)通信連接需要一對(duì)socket，其中一個(gè)運(yùn)行于服務(wù)器端，另一個(gè)運(yùn)行于客戶端。其中服務(wù)器端的socket用于網(wǎng)絡(luò)監(jiān)聽(tīng)，處于等待連接狀態(tài)，實(shí)時(shí)監(jiān)控網(wǎng)絡(luò)狀態(tài)，一旦有客戶端發(fā)出連接請(qǐng)求，立即響應(yīng)[3-4]。

C#中的socket為網(wǎng)絡(luò)通信提供了可行的方法，針對(duì)TCP/IP通信協(xié)議，使用Listen方法監(jiān)聽(tīng)網(wǎng)絡(luò)狀態(tài)，并通過(guò)Bind方法指定IP地址以及端口號(hào)，收到客戶端連接請(qǐng)求時(shí)，使用Accept方法進(jìn)行處理，并且返回一個(gè)可與客戶端進(jìn)行數(shù)據(jù)通信的socket，部分代碼如下：

//創(chuàng)建一個(gè)新的線程

secondThread = new Thread()；

//啟動(dòng)該線程

secondThread.Start()；

//創(chuàng)建socket

Socket handler；

Socket listener；

EndPoint = new IPEndPoint(ipAddress, Port)；

//綁定IP地址及端口號(hào)

listener.Bind(EndPoint)；

//開(kāi)始對(duì)網(wǎng)絡(luò)狀態(tài)進(jìn)行監(jiān)聽(tīng)

listener.Listen(1)；

//綁定收到的連接請(qǐng)求

handler = listener.Accept()

socket命名空間下定義的NetworkStream作為一個(gè)網(wǎng)絡(luò)數(shù)據(jù)流通道，可以對(duì)數(shù)據(jù)進(jìn)行發(fā)送和讀取，特別是在處理不是很復(fù)雜的字符型數(shù)據(jù)時(shí)，傳輸效率很高，部分代碼如下：

//創(chuàng)建一個(gè)用于存儲(chǔ)數(shù)據(jù)的字符串

StringBuilder message；

//創(chuàng)建一個(gè)網(wǎng)絡(luò)數(shù)據(jù)流對(duì)象

NetworkStream stream = new NetworkStream(handler)；

//將收到的數(shù)據(jù)添加到字符串

Num = stream.Read()；

message.AppendFormat()；

//將發(fā)送的數(shù)據(jù)寫(xiě)入數(shù)據(jù)通道

stream.Write()

1.3 客戶端程序

機(jī)器人控制系統(tǒng)作為客戶端，內(nèi)布設(shè)置為KRL語(yǔ)言（庫(kù)卡機(jī)器語(yǔ)言），并通過(guò)EthernetKRL指令進(jìn)行數(shù)據(jù)接收和發(fā)送。另外，庫(kù)卡機(jī)器人控制器讀取和發(fā)送的數(shù)據(jù)主要為XML文件，所以預(yù)先設(shè)置好一個(gè)XML文件結(jié)構(gòu)，然后每次發(fā)送數(shù)據(jù)之前，在XML文件的各個(gè)節(jié)點(diǎn)中將待發(fā)數(shù)據(jù)添加進(jìn)去，部分指令如下：

//打開(kāi)連接通道

EKX_Open()

//發(fā)送字符串

EKX_Write()

EKX_WriteLine()

//發(fā)送XML結(jié)構(gòu)數(shù)據(jù)

EKX_Send()

EKX_WriteReal()

//等待要讀取的XML結(jié)構(gòu)數(shù)據(jù)

EKX_WaitForSensorData()

//讀取數(shù)據(jù)

EKX_GetIntegerElement()

//關(guān)閉通道

EKX_Close()

1.4 在線控制界面

機(jī)器人控制器與外部系統(tǒng)進(jìn)行通信是實(shí)現(xiàn)在線控制的前提，兩者之間通訊系統(tǒng)建立之后，就可以由外部系統(tǒng)定義運(yùn)動(dòng)相關(guān)信息，從而對(duì)機(jī)器人運(yùn)動(dòng)進(jìn)行控制。運(yùn)動(dòng)相關(guān)信息包括角度空間軸坐標(biāo)、笛卡爾空間坐標(biāo)以及運(yùn)動(dòng)方式，如果是圓弧運(yùn)動(dòng)還需要給出角度值。這些信息通過(guò)C#中的XmlDocument的根節(jié)點(diǎn)以及子節(jié)點(diǎn)來(lái)定義，將每一個(gè)運(yùn)動(dòng)坐標(biāo)點(diǎn)的所有相關(guān)信息都保存在同一個(gè)根節(jié)點(diǎn)下，最后生成XML文件并通過(guò)連接通道發(fā)送。在機(jī)器人控制系統(tǒng)運(yùn)行客戶端中，若程序接收到由客戶端發(fā)來(lái)的數(shù)據(jù)，即可搜索XML文件中各個(gè)節(jié)點(diǎn)元素獲得運(yùn)動(dòng)坐標(biāo)點(diǎn)以及運(yùn)動(dòng)方式，從而實(shí)現(xiàn)了外部系統(tǒng)對(duì)機(jī)器人的在線運(yùn)動(dòng)控制，機(jī)器人運(yùn)動(dòng)控制界面如圖1所示。

圖1 機(jī)器人運(yùn)動(dòng)控制界面

2 工業(yè)機(jī)器人運(yùn)動(dòng)仿真界面的設(shè)計(jì)

OpenGL為跨編程、跨平臺(tái)的三維圖形程序接口，其功能強(qiáng)大，且可以方便地調(diào)用底層圖形庫(kù)[5]。由于C#開(kāi)發(fā)環(huán)境中不支持直接對(duì)OpenGL進(jìn)行調(diào)用，所以通常需要借助第三方OpenGL開(kāi)源庫(kù)來(lái)實(shí)現(xiàn)[6]。這些開(kāi)源庫(kù)包括有csgl、SharpGL、OpenTK以及Tao.OpenGL等。其中SharpGL庫(kù)支持的OpenGL版本比較高，且安裝配置方便，所以本文利用SharpGL庫(kù)對(duì)機(jī)器人進(jìn)行三維建模，并設(shè)計(jì)仿真界面。

在C#開(kāi)發(fā)環(huán)境中，需要調(diào)用SharpGL.dll、SharpGL.SceneGraph.dll、SharpGL.WinForms.dll三 個(gè) OpenGL動(dòng)態(tài)鏈接庫(kù)。其中SharpGL.dll包含OpenGL的主要對(duì)象，并包括了OpenGL所有函數(shù)以及擴(kuò)展內(nèi)容，SharpGL.SceneGraph.dll主要包含了OpenGL中的場(chǎng)景元素，如燈光、材質(zhì)及紋理等，而SharpGL.WinForms.dll則提供了窗體控件，模型構(gòu)建及仿真模擬顯示功能。

對(duì)機(jī)器人進(jìn)行三維建模過(guò)程中，需要在OpenGLControl控件上對(duì)以上三個(gè)標(biāo)準(zhǔn)事件進(jìn)行編程。在OpenGLInitialized事件中，對(duì)光照及材料屬性等進(jìn)行編程；在Resized事件中，對(duì)視點(diǎn)變換、模型變換以及投影變換等進(jìn)行編程；在OpenGLDraw事件中對(duì)機(jī)器人進(jìn)行建模及仿真。其中機(jī)器人各關(guān)節(jié)坐標(biāo)系平移以及旋轉(zhuǎn)是通過(guò)gl.Translate（）以及gl.Rotate（）來(lái)完成，各坐標(biāo)系之間齊次變換定義如下：

gl.Translate（a，0，0）

gl.Rotate（α，1，0，0）

gl.Translate（0，0，d）

gl.Rotate（θ，0，0，1）

在OpenGL中生成的旋轉(zhuǎn)矩陣計(jì)算量很大，因此將各連桿間的齊次變換通過(guò)函數(shù)gl.NewList（）、gl.EndList（）以及gl.CallList（）進(jìn)行存儲(chǔ)與顯示。機(jī)器人各個(gè)連桿進(jìn)行建模時(shí)，需要通過(guò)函數(shù)gl.PushMatrix（）以及gl.PopMatrix（）來(lái)操作矩陣堆棧，同時(shí)對(duì)各個(gè)桿件進(jìn)行建模以及坐標(biāo)系之間的變換。完成上述步驟后，機(jī)器人模型構(gòu)建完成，并以機(jī)器人各關(guān)節(jié)角作為控制變量實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)仿真，本文所設(shè)計(jì)的機(jī)器人運(yùn)動(dòng)仿真界面如圖2所示。

圖2 運(yùn)動(dòng)仿真界面

3 運(yùn)動(dòng)仿真及軌跡規(guī)劃

建立機(jī)器人運(yùn)動(dòng)仿真模型主要以關(guān)節(jié)角為控制變量，并根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)，通過(guò)齊次變換得到機(jī)器人末端坐標(biāo)系相對(duì)于基坐標(biāo)下的位姿。當(dāng)機(jī)器人進(jìn)行軌跡規(guī)劃時(shí)，需要根據(jù)機(jī)器人逆運(yùn)動(dòng)學(xué)，給定初始點(diǎn)以及終點(diǎn)位置，然后對(duì)軌跡進(jìn)行插值，得到一系列中間點(diǎn)位置信息，然后再利用逆運(yùn)動(dòng)學(xué)求解方法求出各個(gè)中間點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角，即可實(shí)現(xiàn)離線軌跡運(yùn)動(dòng)仿真。本文通過(guò)離線軌跡規(guī)劃，對(duì)機(jī)器人末端執(zhí)行機(jī)構(gòu)進(jìn)行矩形軌跡規(guī)劃，得到仿真結(jié)果如圖3所示。

通過(guò)給出運(yùn)動(dòng)坐標(biāo)信息，利用上述建立機(jī)器人控制界面控制機(jī)器人運(yùn)動(dòng)，庫(kù)卡機(jī)器人觸發(fā)器可以獲取每段運(yùn)動(dòng)路徑中間點(diǎn)關(guān)節(jié)角，利用這些關(guān)節(jié)角信息可以對(duì)實(shí)際運(yùn)動(dòng)軌跡進(jìn)行運(yùn)動(dòng)仿真。按機(jī)器人控制器返回的參量進(jìn)行實(shí)際運(yùn)動(dòng)軌跡的仿真，可以得到與規(guī)劃的軌跡相同的結(jié)果，對(duì)兩個(gè)軌跡的關(guān)節(jié)變量進(jìn)行對(duì)比，可以發(fā)現(xiàn)兩者之間的誤差很小，比較結(jié)果如表1所示。

圖3 機(jī)器人運(yùn)動(dòng)仿真結(jié)果

對(duì)于復(fù)雜路徑，可以在matlab中采用機(jī)器人工具箱進(jìn)行路徑規(guī)劃，通過(guò)運(yùn)動(dòng)控制界面將數(shù)據(jù)發(fā)送給機(jī)器人控制器控制其運(yùn)動(dòng)，并采集其中節(jié)點(diǎn)關(guān)節(jié)角，通過(guò)C#中matlab函數(shù)對(duì)中間插值點(diǎn)進(jìn)行計(jì)算，將關(guān)節(jié)變量作為參數(shù)對(duì)軌跡進(jìn)行運(yùn)動(dòng)仿真。

4 結(jié)論

本文用C#語(yǔ)言在Visual Studio平臺(tái)上開(kāi)發(fā)了機(jī)器人在線控制系統(tǒng)，并通過(guò)以太網(wǎng)通信實(shí)現(xiàn)了機(jī)器人控制系統(tǒng)與外部PC的數(shù)據(jù)交換功能，通過(guò)服務(wù)器和客戶端程序?qū)崿F(xiàn)了在線控制，使得對(duì)機(jī)器人編程脫離了現(xiàn)場(chǎng)環(huán)境以及復(fù)雜KRL編程語(yǔ)言的限制。最后利用OpenGL圖形庫(kù)建立了機(jī)器人模型，開(kāi)發(fā)了機(jī)器人運(yùn)動(dòng)仿真界面，以及進(jìn)行軌跡規(guī)劃以及運(yùn)動(dòng)仿真，對(duì)機(jī)器人離線規(guī)劃軌跡及實(shí)際運(yùn)動(dòng)軌跡進(jìn)行比較，結(jié)果證明了在線運(yùn)動(dòng)控制有效性和仿真結(jié)果的正確性。

表1 關(guān)節(jié)參量對(duì)比