文旭，仲梁維

（200093 上海 上海理工大學(xué) 機(jī)械工程學(xué)院）

0 引言

近幾年，我國(guó)在機(jī)械工程、計(jì)算機(jī)技術(shù)、自動(dòng)控制以及人工智能等領(lǐng)域快速發(fā)展，機(jī)器人領(lǐng)域也越來(lái)越受關(guān)注，工業(yè)機(jī)器人在制造業(yè)扮演著重要角色。目前，主流的機(jī)器人有瑞典的ABB、德國(guó)的KUKA 以及日本的FANUC 和那智不二越機(jī)器人。我國(guó)在工業(yè)機(jī)器人的應(yīng)用上還處于較低層面，主要是離線(xiàn)示教編程方式，內(nèi)容繁瑣復(fù)雜，精度低。傳統(tǒng)的人工示教效率低、不科學(xué)。隨著計(jì)算機(jī)與通信技術(shù)的快速發(fā)展，離線(xiàn)編程成為當(dāng)前工業(yè)機(jī)器人仿真的主要方式。主流的機(jī)器人都有專(zhuān)門(mén)的離線(xiàn)編程軟件，比如ABB 的RobotStudio、庫(kù)卡的KUKA SIM PRO 以及那智不二越的FD on Desk，缺點(diǎn)是簡(jiǎn)單的示教仿真。目前在線(xiàn)編程以及運(yùn)用算法進(jìn)行路徑規(guī)劃仍不成熟。通過(guò)MATLAB 中的Robotics Toolbox 以及機(jī)器人工具箱對(duì)機(jī)器人仿真，在仿真精度、結(jié)果、與實(shí)際機(jī)器人的匹配都存在相當(dāng)誤差。

本文在VisualStudio 平臺(tái)利用C#編程語(yǔ)言，以Socket 的C/S(客戶(hù)端/服務(wù)端)結(jié)構(gòu)以及那智編程語(yǔ)言為基礎(chǔ)，在FD on Desk 上建立機(jī)器人及工作空間模型，實(shí)現(xiàn)機(jī)器人末端軌跡在線(xiàn)運(yùn)動(dòng)仿真。

1 Socket 通信及控制系統(tǒng)的搭建

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

那智不二越機(jī)器人本體系統(tǒng)與外部設(shè)備（PC、外部傳感器）的數(shù)據(jù)通過(guò)以太網(wǎng)傳輸，支持TCP以及UDP 協(xié)議。UDP 傳輸數(shù)據(jù)，在數(shù)據(jù)的傳輸安全與可靠性方面存在不足，數(shù)據(jù)很容易丟失，本文采用TCP/IP 通訊方式實(shí)現(xiàn)數(shù)據(jù)通訊，保證數(shù)據(jù)傳輸?shù)目煽啃訹1]。將PC 端（機(jī)器人上位機(jī)）作為客戶(hù)端，那智機(jī)器人系統(tǒng)作為服務(wù)端，通過(guò)在那智機(jī)器人系統(tǒng)的KLI 接口和電腦的以太網(wǎng)接口，以XML 文件格式，實(shí)現(xiàn)PC 端對(duì)那智機(jī)器人的遠(yuǎn)程在線(xiàn)控制。

1.2 服務(wù)端程序

本文以機(jī)器人控制系統(tǒng)作為服務(wù)端，根據(jù)那智機(jī)器人系統(tǒng)提供的編程語(yǔ)言，以用戶(hù)程序后端運(yùn)行的方式實(shí)現(xiàn)數(shù)據(jù)的接收與發(fā)送，服務(wù)端程序的連接部分主要包括IP 地址與端口號(hào)。一個(gè)完整的網(wǎng)絡(luò)通信需要的是一對(duì)socket,，一個(gè)在服務(wù)端運(yùn)行，一個(gè)在客戶(hù)端運(yùn)行[2]。服務(wù)端程序是被動(dòng)監(jiān)聽(tīng)狀態(tài)，監(jiān)聽(tīng)是否有客戶(hù)端連接并對(duì)相應(yīng)的信息執(zhí)行相應(yīng)反饋?lái)憫?yīng)。

那智機(jī)器人提供的很多系統(tǒng)函數(shù)，通過(guò)系統(tǒng)函數(shù)的調(diào)用，直接可以獲取那智機(jī)器人的實(shí)時(shí)狀態(tài)，比如機(jī)器人各軸的角度、笛卡爾坐標(biāo)系下的位置、各軸的力矩情況[3]。部分代碼程序如下。

1.3 客戶(hù)端程序

電腦端作為客戶(hù)端，程序在VisualStudio 2017 平臺(tái)上用C#編程語(yǔ)言編寫(xiě)，通過(guò)C#中的IPAddress和IPEndPoint 獲取對(duì)方的IP 與端口號(hào)，并通過(guò)Connect 方法實(shí)現(xiàn)連接，部分代碼如下：

2 機(jī)器人運(yùn)動(dòng)仿真系統(tǒng)的建立

機(jī)器人仿真系統(tǒng)主要包括2 個(gè)模塊：物理仿真模塊和運(yùn)動(dòng)軌跡規(guī)劃模塊。在FD on Desk 仿真軟件中建立相應(yīng)的物理模型，并通過(guò)RRT 算法為機(jī)器人從起始點(diǎn)到終點(diǎn)規(guī)劃出路徑，并將路徑坐標(biāo)存到SQL 數(shù)據(jù)庫(kù)[4]，在VisualStudio 平臺(tái)，通過(guò)套接字通信的方式，實(shí)現(xiàn)對(duì)機(jī)器人的在線(xiàn)控制及路徑規(guī)劃功能。

FD on Desk 是那智機(jī)器人公司開(kāi)發(fā)的一款專(zhuān)門(mén)為那智機(jī)器人仿真的離線(xiàn)仿真軟件（如圖1 所示），其中包括虛擬手柄（Visual TP）、虛擬控制器面板（Visual FD）、虛擬IO(Virtual IO)以及三維模型模塊（Virtual ROBOT）。Virtual ROBOT 模塊具有三維物理建模的功能，同時(shí)也支持外部導(dǎo)入模型的方式。

圖1 FD on Desk 軟件Fig.1 FD on Desk software

2.1 機(jī)器人及相關(guān)模型的建立

本文采用的機(jī)器人模型為那智MZ0701-01 型六自由度機(jī)器人。通過(guò)調(diào)用那智機(jī)器人內(nèi)置MOVE函數(shù)控制機(jī)器人的移動(dòng)，并且調(diào)用它的系統(tǒng)函數(shù)（SYSTEM 函數(shù)），可以直接獲取機(jī)器人姿態(tài)，繞開(kāi)了通過(guò)dh 參數(shù)以及逆解矩陣的方式來(lái)獲取機(jī)器人的姿態(tài)，規(guī)避了dh 及逆解帶來(lái)的仿真誤差。如圖2 所示，機(jī)器人從起點(diǎn)抓取目標(biāo)，通過(guò)RRT 算法繞開(kāi)障礙物的到達(dá)終點(diǎn)。

圖2 工作空間模型Fig.2 Workspace model

2.2 碰撞檢測(cè)

目前廣泛應(yīng)用的碰撞檢測(cè)方法為層次包絡(luò)盒法，主要包括OBB、包圍球以及AABB 等。本文用到的機(jī)器人為六自由度機(jī)械臂，采用的是長(zhǎng)方體包絡(luò)法。為了仿真結(jié)果最優(yōu)化，將機(jī)器人基座、一軸和二軸作為一個(gè)圓柱體包絡(luò)，三軸、四軸以及末端分別用3 個(gè)圓柱體包絡(luò)[5]，那么機(jī)械臂與障礙物之間的碰撞被轉(zhuǎn)換成三維空間下圓柱體間的干涉問(wèn)題。包絡(luò)模型如圖3 所示。

圖3 碰撞模型Fig.3 Collision model

2 個(gè)圓柱體發(fā)生碰撞有3 種碰撞形式：（1）圓柱兩端面之間發(fā)生碰撞；（2）端面與圓柱的側(cè)面發(fā)生碰撞；（3）側(cè)面與端面發(fā)生碰撞。如圖4 所示。

圖4 碰撞形式Fig.4 Collision form

按照碰撞的不同形式，可以依次按照?qǐng)A柱中心距檢測(cè)、檢測(cè)端面圓相交、檢測(cè)側(cè)面相交的順序進(jìn)行判斷。若排除3 種碰撞情況，則可以認(rèn)為這2個(gè)物體不發(fā)生碰撞[6]。

2.3 機(jī)器人路徑規(guī)劃

2.31 RRT 算法

RRT 算法又叫隨機(jī)快速擴(kuò)展隨機(jī)數(shù)算法，如圖5 所示。

圖5 原始RRT 模型Fig.5 Original RRT model

主要思路如下：（1）將抓取物的坐標(biāo)作為起始節(jié)點(diǎn)Xinit，并以此節(jié)點(diǎn)作為根節(jié)點(diǎn)，產(chǎn)生隨機(jī)樹(shù)；（2）設(shè)定一個(gè)概率P值引導(dǎo)隨機(jī)數(shù)搜索方向，在此方向上隨機(jī)產(chǎn)生一個(gè)采樣點(diǎn)Xrand作為隨機(jī)數(shù)實(shí)際擴(kuò)展方向；（3）在產(chǎn)生的2 個(gè)節(jié)點(diǎn)中選取離Xrand最近的節(jié)點(diǎn)Xnearest；（3）通過(guò)擴(kuò)展函數(shù)Extend（Xnearest，Xrand），以某一步長(zhǎng)沿XnearestXrand方向產(chǎn)生一個(gè)新的節(jié)點(diǎn)Xnew，如果在隨機(jī)擴(kuò)展中檢測(cè)到發(fā)生碰撞，則舍去此節(jié)點(diǎn)，重新回到（2）重復(fù)執(zhí)行[7]。

原始的RRT 算法在搜索的方向存在一定的盲目性，導(dǎo)致實(shí)際規(guī)劃的路徑不夠理想，除了設(shè)定一個(gè)搜索方向概率P對(duì)路徑進(jìn)行優(yōu)化，同時(shí)可以采用RRT*對(duì)規(guī)劃的路徑再次優(yōu)化。RRT*與RRT 算法的不同在于，在人為設(shè)定的某個(gè)區(qū)域內(nèi)，在區(qū)域內(nèi)的節(jié)點(diǎn)重新為Xnew選擇父節(jié)點(diǎn)，重布線(xiàn)隨機(jī)樹(shù)[8]。如圖6 所示。

圖6 改進(jìn)后的RRT 模型Fig.6 Improved RRT model

圖6 中的圓圈部分表示節(jié)點(diǎn)，兩節(jié)點(diǎn)之間的數(shù)字表示路徑的長(zhǎng)度，對(duì)橢圓包圍的區(qū)域內(nèi)進(jìn)行路徑優(yōu)化[9]。從圖6（a）得知，原始路徑為0-4-9-6，路徑長(zhǎng)度為16。重新為Xnew尋找父節(jié)點(diǎn)，分別以4、5、8 節(jié)點(diǎn)作為父節(jié)點(diǎn)，則路徑分別為0-1-5-9，0-4-9，0-1-5-8-9，路徑總長(zhǎng)度為14，11，12。選取長(zhǎng)度最小的節(jié)點(diǎn)5 作為新的父節(jié)點(diǎn)，優(yōu)化后的路徑為圖圖6（b）[10]。

偽代碼如下[11]：

3 仿真結(jié)果分析

在Virtual ROBOT 建立工作空間，起點(diǎn)坐標(biāo)為（600，251，385），終點(diǎn)坐標(biāo)為（600，-250，360）。設(shè)置RRT 的搜索步長(zhǎng)為50?？梢钥闯?，在沒(méi)有障礙物的情況下，機(jī)械臂末端軌跡近似沿著一條直線(xiàn)的方向，說(shuō)明這個(gè)方法是接近最短路徑的[12]。

在原來(lái)的工作空間設(shè)置若干障礙物，障礙物恰好在起點(diǎn)和終點(diǎn)中間，仿真結(jié)果如圖9、圖10 所示。

圖7 無(wú)障礙物下路徑規(guī)劃狀態(tài)1Fig.7 Path planning status 1 under no obstacles

圖8 無(wú)障礙物下路徑規(guī)劃狀態(tài)2Fig.8 Path planning status 2 under no obstacles

圖9 有障礙物下路徑規(guī)劃狀態(tài)1Fig.9 Path planning status 1 under obstacles

圖10 有障礙物下路徑規(guī)劃狀態(tài)2Fig.10 Path planning status 2 under obstacles

4 結(jié)束語(yǔ)

本文用C#語(yǔ)言在visualstudio 平臺(tái)、基于工業(yè)仿真軟件進(jìn)行系統(tǒng)仿真，通過(guò)Socket 通信以及機(jī)器人本體語(yǔ)言實(shí)現(xiàn)了對(duì)機(jī)器人的在線(xiàn)控制以及路徑規(guī)劃，一定程度擺脫了當(dāng)前工業(yè)軟件在離線(xiàn)以及簡(jiǎn)單示教編程上的瓶頸。開(kāi)發(fā)一個(gè)將在線(xiàn)控制、工業(yè)仿真軟件、智能路徑規(guī)劃融為一體的系統(tǒng)，具有一定的現(xiàn)實(shí)意義。