數(shù)字孿生車(chē)間機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)構(gòu)建方法

劉懷蘭 趙文杰 李世壯 岳 鵬 馬寶瑞

華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院，武漢，430074

0 引言

構(gòu)建數(shù)字孿生車(chē)間是實(shí)現(xiàn)制造物理世界和信息世界智能互聯(lián)與交互融合的一種有效途徑[1]，基于設(shè)備層、產(chǎn)線層、車(chē)間層的物理實(shí)體可實(shí)現(xiàn)數(shù)字孿生車(chē)間的層次化構(gòu)建。工業(yè)機(jī)器人是車(chē)間物理實(shí)體設(shè)備層的重要組成元素，快速有效地實(shí)現(xiàn)其數(shù)字孿生虛擬實(shí)體模型的搭建，以及它在虛擬空間中的仿真運(yùn)行模擬和虛實(shí)驅(qū)動(dòng)同步運(yùn)行，具有重要的研究?jī)r(jià)值和應(yīng)用價(jià)值。

虛實(shí)驅(qū)動(dòng)系統(tǒng)通過(guò)采集獲取生產(chǎn)現(xiàn)場(chǎng)設(shè)備的實(shí)時(shí)數(shù)據(jù)，實(shí)現(xiàn)對(duì)應(yīng)設(shè)備虛擬實(shí)體模型的狀態(tài)驅(qū)動(dòng)，使得設(shè)備對(duì)象在物理空間與虛擬空間的狀態(tài)一致，因此該系統(tǒng)同樣需要虛擬實(shí)體模型的精確構(gòu)建。唐堂等[2]提出的數(shù)字孿生模型以數(shù)字化的形式對(duì)實(shí)體模型進(jìn)行行為描述，支持產(chǎn)品設(shè)計(jì)、制造、優(yōu)化等全生命周期的仿真模擬，實(shí)現(xiàn)生產(chǎn)制造全流程的閉環(huán)控制與優(yōu)化。丁凱等[3]研究了虛擬實(shí)體與物理實(shí)體的映射關(guān)系建模方法、制造過(guò)程演化的建模方法、制造過(guò)程關(guān)聯(lián)數(shù)據(jù)的建模方法，構(gòu)建了基于數(shù)字孿生的智能制造空間模型，實(shí)現(xiàn)了制造車(chē)間的虛實(shí)同步運(yùn)行仿真與實(shí)時(shí)動(dòng)態(tài)反饋控制。ZHANG等[4]提出使用數(shù)字孿生快速進(jìn)行中空玻璃生產(chǎn)線個(gè)性化設(shè)計(jì)的方法，通過(guò)融合系統(tǒng)模型、分布式實(shí)時(shí)過(guò)程數(shù)據(jù)構(gòu)建整條生產(chǎn)線的數(shù)字孿生系統(tǒng)。陳黎明等[5]采用.NET技術(shù)開(kāi)發(fā)的碼垛機(jī)器人離線編程系統(tǒng)能在仿真環(huán)境中進(jìn)行軌跡控制指令程序的試運(yùn)行檢驗(yàn)，并能直接控制物理空間的機(jī)器人，實(shí)現(xiàn)仿真系統(tǒng)與實(shí)際系統(tǒng)的同步運(yùn)行。陳煒燦[6]基于機(jī)器人操作系統(tǒng)(robot operating system，ROS)設(shè)計(jì)了一款模塊化可重用的分布式三維仿真平臺(tái)，并通過(guò)KUKA的youbot移動(dòng)機(jī)械臂、雙目視覺(jué)、輪式足球機(jī)器人實(shí)驗(yàn)對(duì)所設(shè)計(jì)的平臺(tái)進(jìn)行了驗(yàn)證。WEI等[7]根據(jù)承擔(dān)任務(wù)實(shí)時(shí)性的不同劃分ROS節(jié)點(diǎn)，將非實(shí)時(shí)部分運(yùn)行在Linux系統(tǒng)中，將實(shí)時(shí)系統(tǒng)部分運(yùn)行在Nuttx的實(shí)時(shí)操作系統(tǒng)中，設(shè)計(jì)的RGMP-ROS軟件框架提高了機(jī)器人運(yùn)動(dòng)控制的實(shí)時(shí)性。目前，數(shù)字孿生車(chē)間系統(tǒng)的基本組成部分已在工業(yè)生產(chǎn)中建立，但包括虛擬實(shí)體模型的其余部分還未有統(tǒng)一的構(gòu)建方法。當(dāng)前的虛擬實(shí)體模型構(gòu)建方法缺乏通用性，數(shù)字孿生車(chē)間系統(tǒng)的配置非常耗時(shí)，且每次修改系統(tǒng)參數(shù)時(shí)都需要重新配置[8]。企業(yè)為快速適應(yīng)市場(chǎng)變化，必須找到一種方法來(lái)縮短數(shù)字孿生車(chē)間系統(tǒng)的建立時(shí)間。

針對(duì)上述現(xiàn)狀，本文首先借助數(shù)字孿生五維模型[9]中虛擬實(shí)體模型的概念，提出數(shù)字孿生車(chē)間工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)的模塊化構(gòu)建方法，結(jié)合ROS與Unity3D平臺(tái)來(lái)構(gòu)建工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)，最后以華數(shù)機(jī)器人為例進(jìn)行工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)的驗(yàn)證。實(shí)例結(jié)果研究表明：系統(tǒng)需重新配置時(shí)，只需修改其對(duì)應(yīng)功能模塊的參數(shù)；系統(tǒng)支持多類(lèi)型的工業(yè)機(jī)器人運(yùn)動(dòng)仿真控制，通用性強(qiáng)，構(gòu)建工業(yè)機(jī)器人孿生體的開(kāi)發(fā)時(shí)間成本較低、難度較小。

1 工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)結(jié)構(gòu)

工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)分為運(yùn)動(dòng)控制與虛擬仿真兩個(gè)部分，其中，運(yùn)動(dòng)控制部分根據(jù)操作指令進(jìn)行動(dòng)作執(zhí)行，虛擬仿真部分用于設(shè)備模型的可視化展示，以及人機(jī)交互指令的處理。

1.1 系統(tǒng)架構(gòu)分層方案

工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)開(kāi)發(fā)的目標(biāo)功能為離線狀態(tài)下的仿真運(yùn)行預(yù)演、連接狀態(tài)下的設(shè)備監(jiān)控與反饋控制。因此首先根據(jù)各模塊的功能，將虛擬空間分為人機(jī)交互層、動(dòng)作執(zhí)行層，而將物理空間劃為物理設(shè)備層。

人機(jī)交互層用于虛擬工業(yè)機(jī)器人的運(yùn)動(dòng)控制和設(shè)備運(yùn)行狀態(tài)的顯示，包括用戶操作界面模塊、三維狀態(tài)顯示模塊。動(dòng)作執(zhí)行層根據(jù)控制指令進(jìn)行運(yùn)動(dòng)執(zhí)行。工業(yè)機(jī)器人的常用控制類(lèi)型分為程序指令控制與手動(dòng)控制，因此動(dòng)作執(zhí)行層根據(jù)控制類(lèi)型的不同，可分為程序解析模塊和控制器模塊。物理設(shè)備層主要為工業(yè)機(jī)器人實(shí)體設(shè)備，包括編輯設(shè)備程序的機(jī)器人示教器，以及用于動(dòng)作執(zhí)行的機(jī)器人控制器。工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)功能分層如圖1所示。系統(tǒng)搭建時(shí)，可根據(jù)搭建需求添加設(shè)備的幾何模型功能模塊，設(shè)置各功能塊屬性的參數(shù)，自定義搭建工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)的虛擬空間，實(shí)現(xiàn)對(duì)虛擬設(shè)備的運(yùn)動(dòng)控制。

圖1 工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)功能分層

機(jī)器人虛擬模型控制器作為一個(gè)整體進(jìn)行功能開(kāi)發(fā)會(huì)導(dǎo)致代碼復(fù)用性低、擴(kuò)展性差。本文提出將機(jī)器人控制器的全部功能轉(zhuǎn)化為相互獨(dú)立節(jié)點(diǎn)功能耦合的開(kāi)發(fā)方法。該方法將控制器模塊擴(kuò)展劃分為狀態(tài)隨動(dòng)模塊、運(yùn)動(dòng)控制模塊、反饋控制模塊。運(yùn)動(dòng)控制模塊根據(jù)要求的運(yùn)動(dòng)類(lèi)型及目標(biāo)點(diǎn)位姿參數(shù)進(jìn)行軌跡插補(bǔ)，并輸出插補(bǔ)計(jì)算后的關(guān)節(jié)軸轉(zhuǎn)動(dòng)角度。狀態(tài)隨動(dòng)模塊根據(jù)生產(chǎn)現(xiàn)場(chǎng)設(shè)備的實(shí)時(shí)狀態(tài)數(shù)據(jù)、運(yùn)動(dòng)控制模塊輸出的關(guān)節(jié)狀態(tài)數(shù)據(jù)，控制人機(jī)交互層三維模型的狀態(tài)更新，實(shí)現(xiàn)虛擬空間中的虛擬設(shè)備模型與物理空間中的物理設(shè)備實(shí)體的虛實(shí)驅(qū)動(dòng)同步運(yùn)行，以及模擬仿真狀態(tài)下的虛擬設(shè)備模型獨(dú)立運(yùn)行。反饋控制模塊根據(jù)機(jī)器人的狀態(tài)數(shù)據(jù)、用戶的操作指令，對(duì)物理空間中的工業(yè)機(jī)器人進(jìn)行運(yùn)動(dòng)控制。

綜上，本文最終將虛實(shí)驅(qū)動(dòng)系統(tǒng)劃分為人機(jī)交互層、動(dòng)作執(zhí)行層，其中，人機(jī)交互層包括界面功能操作、設(shè)備模型狀態(tài)顯示；動(dòng)作執(zhí)行層根據(jù)功能需求劃分為程序解析模塊、運(yùn)動(dòng)控制模塊、狀態(tài)隨動(dòng)模塊、反饋控制模塊，系統(tǒng)總體設(shè)計(jì)架構(gòu)如圖2所示。

圖2 工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)結(jié)構(gòu)

1.2 關(guān)鍵功能模塊設(shè)計(jì)

(1)程序解析模塊。用于識(shí)別用戶編輯的工業(yè)機(jī)器人軌跡路徑程序，獲取當(dāng)前命令行的控制指令信息，將解析后的目標(biāo)點(diǎn)位姿數(shù)據(jù)、插補(bǔ)類(lèi)型命令、動(dòng)作參數(shù)傳輸?shù)竭\(yùn)動(dòng)控制模塊。

(2)運(yùn)動(dòng)控制模塊。根據(jù)獲取的目標(biāo)位姿點(diǎn)數(shù)據(jù)和插補(bǔ)類(lèi)型進(jìn)行運(yùn)算，并將運(yùn)算后的關(guān)節(jié)角度傳輸?shù)綘顟B(tài)隨動(dòng)模塊，實(shí)現(xiàn)機(jī)器人的手動(dòng)控制及軌跡程序運(yùn)行。圖3所示為機(jī)器人指令程序數(shù)據(jù)傳輸流程。機(jī)器人處于虛擬仿真狀態(tài)時(shí)，運(yùn)動(dòng)控制模塊輸入的指令數(shù)據(jù)由程序解析模塊或用戶操作界面控制按鈕傳輸。機(jī)器人處于虛實(shí)驅(qū)動(dòng)狀態(tài)時(shí)，運(yùn)動(dòng)控制模塊的輸入數(shù)據(jù)為真實(shí)設(shè)備在工作空間中的位姿數(shù)據(jù)。

圖3 機(jī)器人指令數(shù)據(jù)傳輸

(3)狀態(tài)隨動(dòng)模塊。狀態(tài)隨動(dòng)模塊根據(jù)獲取的關(guān)節(jié)角度驅(qū)動(dòng)仿真環(huán)境中的虛擬工業(yè)機(jī)器人運(yùn)行。機(jī)器人處于虛擬仿真狀態(tài)時(shí)，狀態(tài)隨動(dòng)模塊輸入數(shù)據(jù)為運(yùn)動(dòng)控制模塊或用戶操作界面關(guān)節(jié)軸控制功能按鈕傳輸?shù)臄?shù)據(jù)。機(jī)器人處于虛實(shí)驅(qū)動(dòng)狀態(tài)時(shí)，運(yùn)動(dòng)控制模塊的輸入數(shù)據(jù)為真實(shí)設(shè)備的關(guān)節(jié)軸角度實(shí)時(shí)數(shù)據(jù)。

(4)反饋控制模塊。用于人機(jī)交互界面與物理設(shè)備控制系統(tǒng)間的數(shù)據(jù)通信，將相關(guān)的控制指令信息下發(fā)給實(shí)體機(jī)器人控制系統(tǒng)，通過(guò)操作仿真環(huán)境中的虛擬設(shè)備模型驅(qū)動(dòng)機(jī)器人本體運(yùn)動(dòng)，便于用戶對(duì)工業(yè)機(jī)器人的遠(yuǎn)程控制。

功能模塊設(shè)計(jì)完成后，根據(jù)模塊特點(diǎn)選擇開(kāi)發(fā)平臺(tái)。程序解析模塊與狀態(tài)隨動(dòng)模塊在執(zhí)行過(guò)程中不參與運(yùn)動(dòng)學(xué)的計(jì)算，為減小機(jī)器人控制器運(yùn)動(dòng)過(guò)程中的資源損耗，該部分與人機(jī)交互層界面功能模塊歸類(lèi)為虛擬仿真部分，使用Unity3D仿真引擎進(jìn)行功能開(kāi)發(fā)。運(yùn)動(dòng)控制部分主要實(shí)現(xiàn)工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)解算、軌跡插補(bǔ)、反饋控制，該部分功能使用ROS開(kāi)發(fā)實(shí)現(xiàn)。

2 基于ROS和Unity3D的機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)設(shè)計(jì)

ROS是處于機(jī)器人交互應(yīng)用軟件和操作系統(tǒng)之間的抽象層工具，屬于開(kāi)源架構(gòu)的軟件生態(tài)系統(tǒng)，提供了多種常用的工具、庫(kù)及協(xié)議，具有支持硬件的抽象描述、底層驅(qū)動(dòng)程序管理、共用功能的執(zhí)行、程序間的消息傳遞、程序發(fā)行包管理等類(lèi)似操作系統(tǒng)的功能[10]。

2.1 工業(yè)機(jī)器人模型文件描述

開(kāi)發(fā)機(jī)器人運(yùn)動(dòng)控制功能時(shí)，首先對(duì)被控機(jī)器人進(jìn)行參數(shù)化描述(包括臂桿參數(shù)、各臂桿間的相對(duì)關(guān)系)，完成機(jī)械臂運(yùn)動(dòng)關(guān)系鏈的構(gòu)建。本文通過(guò)創(chuàng)建URDF(unified robot description format)[11]文件，以模型樹(shù)的結(jié)構(gòu)形式記錄設(shè)備模型的結(jié)構(gòu)參數(shù)[12]。URDF將工業(yè)機(jī)器人各臂解釋為連桿并通過(guò)tag標(biāo)簽中的link進(jìn)行標(biāo)記，記錄當(dāng)前連桿的形狀、尺寸、顏色、慣性矩陣碰撞參數(shù)，使用tag標(biāo)簽中的joint標(biāo)記連桿之間的關(guān)節(jié)，記錄關(guān)節(jié)類(lèi)型、運(yùn)動(dòng)限制值、關(guān)節(jié)關(guān)系等參數(shù)，如圖4所示。

圖4 URDF文件link與joint描述

模型文件創(chuàng)建完成后，導(dǎo)入Rviz可視化工具進(jìn)行文件校驗(yàn)，通過(guò)控制機(jī)器人各關(guān)節(jié)軸的轉(zhuǎn)動(dòng)，對(duì)所設(shè)置的關(guān)節(jié)坐標(biāo)系、旋轉(zhuǎn)中心軸位姿等進(jìn)行檢測(cè)，避免軸中心偏移導(dǎo)致機(jī)器人運(yùn)動(dòng)出現(xiàn)偏差。

2.2 工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)配置

工業(yè)機(jī)器人模型創(chuàng)建完成后，添加運(yùn)動(dòng)學(xué)功能。本文使用MoveIt！工具集進(jìn)行機(jī)器人模型的動(dòng)力學(xué)特性配置及相應(yīng)的運(yùn)動(dòng)規(guī)劃仿真，通過(guò)自碰撞檢測(cè)、創(chuàng)建虛擬關(guān)節(jié)、配置規(guī)劃組、預(yù)定位姿、完善配置文件等步驟最終生成配置好的運(yùn)動(dòng)學(xué)參數(shù)文件包。

2.3 工業(yè)機(jī)器人的運(yùn)動(dòng)控制與軌跡規(guī)劃

工業(yè)機(jī)器人運(yùn)動(dòng)控制分為軌跡規(guī)劃與運(yùn)動(dòng)學(xué)解算。軌跡規(guī)劃根據(jù)用戶指定的運(yùn)動(dòng)方式及目標(biāo)點(diǎn)位進(jìn)行路徑插補(bǔ)，將軌跡線條轉(zhuǎn)化為一系列的中間位姿點(diǎn)，之后控制器驅(qū)動(dòng)機(jī)器人依次到達(dá)生成的點(diǎn)位，按軌跡運(yùn)動(dòng)。運(yùn)動(dòng)學(xué)解算分為正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)兩種類(lèi)型，其中，逆向運(yùn)動(dòng)學(xué)根據(jù)解算方式的不同分為數(shù)值解與解析解。

針對(duì)機(jī)器人軌跡規(guī)劃功能的實(shí)現(xiàn)，使用ROS中支持多種運(yùn)動(dòng)控制功能的MoveIt！工具包。OMPL(open motion planning library)是 MoveIt！提供的開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)。運(yùn)動(dòng)學(xué)逆解部分，MoveIt！常用的算法有通過(guò)數(shù)值解方式計(jì)算的KDL、TRAC_IK，以及通過(guò)解析解方式計(jì)算的IKFast。MoveIt！支持的運(yùn)動(dòng)規(guī)劃和運(yùn)動(dòng)學(xué)算法都通過(guò)插件的形式進(jìn)行加載。此外，ROS還提供了TF工具包，用于記錄設(shè)備模型間坐標(biāo)系關(guān)系，支持坐標(biāo)系的查看、添加以及修改功能，同時(shí)也能對(duì)位姿信息進(jìn)行正逆運(yùn)動(dòng)學(xué)校驗(yàn)。

控制機(jī)器人運(yùn)動(dòng)時(shí)，用戶通過(guò)人機(jī)交互界面調(diào)用MoveIt!中的move_group提供的API接口函數(shù)連接需要的規(guī)劃組，發(fā)送運(yùn)動(dòng)規(guī)劃請(qǐng)求到OMPL。move_group作為MoveIt!的核心，具有不同開(kāi)發(fā)語(yǔ)言的控制器接口，包括用于 C++語(yǔ)言開(kāi)發(fā)的 move_group_interface、用于 Python 語(yǔ)言開(kāi)發(fā)的 moveit_commander等。運(yùn)動(dòng)請(qǐng)求包含用戶指定的目標(biāo)點(diǎn)位姿(關(guān)節(jié)空間或笛卡兒空間)，以及當(dāng)前軌跡路徑中的約束條件，如位姿約束、方向約束、用戶自定義約束等，通過(guò)OMPL進(jìn)行規(guī)劃后，生成一條到達(dá)目標(biāo)點(diǎn)位的軌跡路徑。之后，通過(guò)插補(bǔ)算法將運(yùn)動(dòng)軌跡轉(zhuǎn)為連續(xù)的運(yùn)動(dòng)點(diǎn)，并通過(guò)逆向運(yùn)動(dòng)學(xué)解算出路徑點(diǎn)下的機(jī)器人關(guān)節(jié)軸狀態(tài)。最后，將關(guān)節(jié)角度及用戶設(shè)置的運(yùn)動(dòng)速度、加速度等參數(shù)發(fā)送到驅(qū)動(dòng)控制器進(jìn)行軌跡運(yùn)行，完成機(jī)器人在ROS環(huán)境中的運(yùn)動(dòng)控制，機(jī)器人運(yùn)動(dòng)學(xué)的解算流程如圖5所示。

圖5 機(jī)器人運(yùn)動(dòng)學(xué)解算

2.4 狀態(tài)隨動(dòng)模塊設(shè)計(jì)

狀態(tài)隨動(dòng)模塊通過(guò)接收關(guān)節(jié)角度的數(shù)據(jù)驅(qū)動(dòng)仿真空間中的幾何模型進(jìn)行動(dòng)作執(zhí)行，創(chuàng)建虛擬三維模型時(shí)，需要對(duì)設(shè)備零部件的動(dòng)作腳本進(jìn)行編輯描述。工業(yè)機(jī)器人通過(guò)關(guān)節(jié)軸的旋轉(zhuǎn)到達(dá)目標(biāo)位姿點(diǎn)，因此編輯動(dòng)作腳本時(shí)，主要描述關(guān)節(jié)軸的旋轉(zhuǎn)運(yùn)動(dòng)。

本文在Unity3D仿真引擎中使用C+語(yǔ)言編輯控制腳本，開(kāi)發(fā)狀態(tài)隨動(dòng)功能模塊。Unity3D仿真引擎的Transform類(lèi)為用戶提供了執(zhí)行旋轉(zhuǎn)運(yùn)動(dòng)的API接口。仿真環(huán)境中的虛擬工業(yè)機(jī)器人根據(jù)單軸關(guān)節(jié)的旋轉(zhuǎn)角度運(yùn)動(dòng)，關(guān)節(jié)軸執(zhí)行的旋轉(zhuǎn)運(yùn)動(dòng)是相對(duì)于零部件模型本體坐標(biāo)系的轉(zhuǎn)動(dòng)。本節(jié)選用Transform.localRotaton控制機(jī)器人關(guān)節(jié)軸的旋轉(zhuǎn)運(yùn)動(dòng)，該接口使用四元數(shù)描述方法描述物體相對(duì)于父物體坐標(biāo)系的旋轉(zhuǎn)變換運(yùn)動(dòng)。狀態(tài)隨動(dòng)模塊根據(jù)控制器輸入的關(guān)節(jié)軸數(shù)據(jù)驅(qū)動(dòng)虛擬設(shè)備模型的運(yùn)行，因此需要設(shè)定數(shù)據(jù)采集的循環(huán)時(shí)間T1。

控制虛擬機(jī)器人運(yùn)動(dòng)時(shí)，首先判斷運(yùn)動(dòng)時(shí)間T2。T2≤0表示控制虛擬空間中的機(jī)器人進(jìn)行初始化操作，機(jī)器人直接運(yùn)動(dòng)到目標(biāo)位姿狀態(tài)。T2>0時(shí)，將工業(yè)機(jī)器人的當(dāng)前位姿標(biāo)記為初始位姿，獲取目標(biāo)位姿狀態(tài)后，使用Quaternion.Lerp函數(shù)進(jìn)行旋轉(zhuǎn)角度插補(bǔ)，控制虛擬工業(yè)機(jī)器人逐幀運(yùn)動(dòng)到目標(biāo)位姿。為保證運(yùn)動(dòng)過(guò)程的準(zhǔn)確，設(shè)置累加時(shí)間Tc進(jìn)行運(yùn)動(dòng)時(shí)間判斷，當(dāng)運(yùn)動(dòng)累加時(shí)間Tc到達(dá)設(shè)定時(shí)間T3時(shí)，將目標(biāo)位姿的旋轉(zhuǎn)角度強(qiáng)制賦給虛擬設(shè)備，實(shí)現(xiàn)對(duì)工業(yè)機(jī)器人動(dòng)作過(guò)姿態(tài)的校準(zhǔn)，機(jī)器人獲取關(guān)節(jié)軸數(shù)據(jù)、進(jìn)行軌跡運(yùn)行的功能邏輯如圖6所示。

圖6 機(jī)器人運(yùn)動(dòng)控制邏輯

2.5 程序解析模塊設(shè)計(jì)

工業(yè)機(jī)器人程序指令解釋器將用戶輸入的程序指令文本按照指定的語(yǔ)法規(guī)則轉(zhuǎn)化為執(zhí)行指令及路徑點(diǎn)數(shù)據(jù)。根據(jù)工作流程可將程序指令解釋器分解為詞法分析模塊、語(yǔ)法分析模塊、中間數(shù)據(jù)結(jié)構(gòu)、錯(cuò)誤處理模塊、執(zhí)行機(jī)構(gòu)，其工作邏輯如圖7所示。

圖7 工業(yè)機(jī)器人程序指令解釋器工作邏輯

以工業(yè)機(jī)器人動(dòng)作程序指令為例描述解釋器的工作邏輯：指令解釋器獲取文本中的機(jī)器人程序，將指令代碼以字符串的形式傳輸?shù)皆~法分析器，檢查程序指令中的單詞，對(duì)無(wú)法識(shí)別的單詞進(jìn)行錯(cuò)誤處理。將處理后輸出的詞法單元序列傳輸?shù)秸Z(yǔ)法分析器，進(jìn)行指令語(yǔ)句文法檢查，對(duì)不符合文法規(guī)范的指令語(yǔ)句進(jìn)行錯(cuò)誤處理。接著，將程序指令中存儲(chǔ)的數(shù)據(jù)信息，按照中間數(shù)據(jù)表的數(shù)據(jù)結(jié)構(gòu)分類(lèi)存儲(chǔ)。最后，由執(zhí)行機(jī)構(gòu)輸出運(yùn)動(dòng)指令、相關(guān)路徑點(diǎn)位數(shù)據(jù)、運(yùn)動(dòng)參數(shù)。

目前常見(jiàn)的指令解釋器創(chuàng)建方式是采用FLEX詞法分析器和BISON語(yǔ)法分析器進(jìn)行開(kāi)發(fā)創(chuàng)建。本節(jié)內(nèi)容采用自定義編程方式進(jìn)行功能實(shí)現(xiàn)，重點(diǎn)進(jìn)行機(jī)器人運(yùn)動(dòng)指令及邏輯指令的解析與傳輸。

2.5.1詞法分析

詞法分析是將所輸入文本字符串中的空格、換行符、程序注釋等與指令含義無(wú)關(guān)的字符段剔除，生成具有特定意思標(biāo)記符(token)的過(guò)程[13]。標(biāo)記符可根據(jù)需求自定義所代表的含義，能代表字母、數(shù)字、符號(hào)等。源程序中，標(biāo)記符對(duì)應(yīng)的文本內(nèi)容稱(chēng)為“詞素”。

構(gòu)建詞法分析器的第一步就是定義程序中每個(gè)字母或字符串的含義，并定義標(biāo)記符的數(shù)據(jù)結(jié)構(gòu)。筆者定義Token類(lèi)時(shí)，使用code表示詞素類(lèi)型，使用value表示token的值。

詞法分析器的工作就是逐個(gè)匹配獲取的軌跡程序與所定義的詞素，該過(guò)程可采用正則表達(dá)式[14]實(shí)現(xiàn)，也可編寫(xiě)程序進(jìn)行匹配。

2.5.2語(yǔ)法分析

語(yǔ)法分析就是按照指定的文法規(guī)則，對(duì)詞法分析器輸出的標(biāo)記符序列進(jìn)行語(yǔ)法成分識(shí)別，并檢測(cè)其語(yǔ)法結(jié)構(gòu)的正確性。語(yǔ)法分析器開(kāi)發(fā)過(guò)程中，常使用由終結(jié)符號(hào)集合、非終結(jié)符號(hào)集合、產(chǎn)生式集合、開(kāi)始符號(hào)組成的“上下文無(wú)關(guān)文法”進(jìn)行語(yǔ)法規(guī)則描述[15]。開(kāi)發(fā)語(yǔ)法分析模塊功能時(shí)，首先使用擴(kuò)展巴科斯(EBNF)范式描述程序指令。EBNF是一種用于“上下文無(wú)關(guān)文法”表達(dá)描述的元語(yǔ)法符號(hào)，常見(jiàn)的符號(hào)及功能解釋如表1所示。實(shí)際使用時(shí)，各廠家為其生產(chǎn)的機(jī)器人設(shè)計(jì)相應(yīng)的文本規(guī)則。

表1 EBNF范式主要符號(hào)及功能描述

根據(jù)分析流程的不同，分析語(yǔ)法分為自頂而下和自底向上兩種方法。自頂而下的語(yǔ)法分析即分析過(guò)程由根節(jié)點(diǎn)(開(kāi)始符)觸發(fā)，逐步向葉節(jié)點(diǎn)(終止符)方向進(jìn)行分析。這種語(yǔ)法分析方法的邏輯關(guān)系容易理解，因此本文采用該語(yǔ)法分析技術(shù)描述EBNF范式，完成自定義編寫(xiě)的語(yǔ)法分析器模塊的實(shí)現(xiàn)。

2.5.3中間數(shù)據(jù)表

機(jī)器人程序指令解釋器的各個(gè)模塊在工作過(guò)程中需要進(jìn)行多種數(shù)據(jù)的處理，因此本文根據(jù)工業(yè)機(jī)器人程序指令的特點(diǎn)，將指令解釋器的中間數(shù)據(jù)按照點(diǎn)位表、指令表、參數(shù)表的形式進(jìn)行功能設(shè)計(jì)。

點(diǎn)位表用于存儲(chǔ)路徑點(diǎn)的位姿數(shù)據(jù)。本文使用Dirctionary〈int，float[]〉進(jìn)行數(shù)據(jù)存儲(chǔ)，其中，int為路徑點(diǎn)數(shù)據(jù)的存儲(chǔ)位置，float[]以浮點(diǎn)數(shù)組的形式表示目標(biāo)點(diǎn)位的位姿信息。

指令表用于存儲(chǔ)程序中控制指令的數(shù)據(jù)結(jié)構(gòu)。該類(lèi)數(shù)據(jù)記錄機(jī)器人的軌跡路徑，但不處理路徑點(diǎn)的信息。本文使用List〈Token〉存儲(chǔ)數(shù)據(jù)，例如將“J”指令描述為T(mén)oken{“J”，100}，其中，其中100表示關(guān)鍵的ID。

參數(shù)表存儲(chǔ)程序指令中相關(guān)屬性參數(shù)的信息。以運(yùn)動(dòng)程序指令為例，數(shù)據(jù)表中的該類(lèi)型數(shù)據(jù)的信息為路徑點(diǎn)序號(hào)“P[1]”、速度參數(shù)“VEL=100”等，本文使用Dirctionary〈String，int〉存儲(chǔ)數(shù)據(jù)。

工業(yè)機(jī)器人指令程序代碼解釋器在實(shí)際使用時(shí)，解釋器中的執(zhí)行模塊按照設(shè)置的執(zhí)行順序解析中間數(shù)據(jù)表中的信息，輸出相應(yīng)的指令類(lèi)型及點(diǎn)位數(shù)據(jù)，前行指令解析完成后，跳轉(zhuǎn)到下一行內(nèi)容，直到執(zhí)行完整個(gè)軌跡程序。解析過(guò)程中出現(xiàn)錯(cuò)誤時(shí)，錯(cuò)誤信息將會(huì)傳輸?shù)藉e(cuò)誤模塊并顯示，同時(shí)停止指令的解析任務(wù)。

2.5.4錯(cuò)誤模塊

編輯工業(yè)機(jī)器人軌跡程序時(shí)，經(jīng)常會(huì)出現(xiàn)各種各樣的問(wèn)題，因此需要?jiǎng)?chuàng)建錯(cuò)誤模塊檢測(cè)和處理程序指令中的錯(cuò)誤內(nèi)容。筆者根據(jù)工業(yè)機(jī)器人常見(jiàn)問(wèn)題，將錯(cuò)誤分為以下幾種類(lèi)型：

(1)詞法錯(cuò)誤，即在軌跡程序指令中出現(xiàn)的詞法分析器不能識(shí)別的指令數(shù)據(jù)內(nèi)容。

(2)語(yǔ)法錯(cuò)誤，即當(dāng)前位置的程序指令語(yǔ)句含有不符合規(guī)定文法規(guī)則的語(yǔ)句內(nèi)容，語(yǔ)法分析器無(wú)法識(shí)別該指令語(yǔ)句的相關(guān)參數(shù)。

(3)其他錯(cuò)誤，包括邏輯指令語(yǔ)句和特殊指令語(yǔ)句的使用錯(cuò)誤。邏輯指令語(yǔ)句是指軌跡程序中用于執(zhí)行邏輯判斷的語(yǔ)句。

2.6 反饋控制功能模塊設(shè)計(jì)

本文將華數(shù)機(jī)器人作為研究對(duì)象，通過(guò)廠商提供的二次開(kāi)發(fā)接口獲取功能模塊對(duì)應(yīng)的信息。在與實(shí)際機(jī)器人進(jìn)行通信時(shí)，采用TCP協(xié)議與實(shí)際機(jī)器人控制器進(jìn)行連接，根據(jù)機(jī)器人控制類(lèi)型調(diào)用對(duì)應(yīng)的二次開(kāi)發(fā)接口函數(shù)。連接華數(shù)3型機(jī)器人控制器時(shí)，調(diào)用二次開(kāi)發(fā)庫(kù)接口中的connect(strIP，uPort)()函數(shù)，輸入目標(biāo)設(shè)備的IP地址及端口號(hào)完成連接關(guān)系的創(chuàng)建。檢查通信連接的狀態(tài)時(shí)，調(diào)用isConnected()函數(shù)，信息文本框會(huì)根據(jù)返回值的不同顯示不同的連接狀態(tài)。通信連接的工作方式如圖8所示。

圖8 機(jī)器人控制器通信

控制類(lèi)型用于選擇Unity3D中的數(shù)據(jù)傳輸主體。本文將控制類(lèi)型分為手動(dòng)控制與自動(dòng)控制，其中，手動(dòng)控制用于示教操作，自動(dòng)控制用于程序運(yùn)行。手動(dòng)控制模式下，可進(jìn)行關(guān)節(jié)軸控制與笛卡兒線性控制的轉(zhuǎn)換，還可調(diào)節(jié)機(jī)器人運(yùn)動(dòng)速度，其功能邏輯如圖9所示。

圖9 功能邏輯圖

2.7 虛實(shí)驅(qū)動(dòng)系統(tǒng)通信方案設(shè)計(jì)

模塊化的工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)開(kāi)發(fā)需要實(shí)現(xiàn)不同功能模塊和不同平臺(tái)間的通信，本次研究模塊之間的通信為ROS內(nèi)部節(jié)點(diǎn)間的通信，不同平臺(tái)間的通信采用ROSbridge功能包來(lái)實(shí)現(xiàn)。

ROSbridge是實(shí)現(xiàn)ROS系統(tǒng)與非ROS系統(tǒng)之間數(shù)據(jù)傳輸?shù)墓δ馨?，其借助ROS及節(jié)點(diǎn)分布式通信的特點(diǎn)，將節(jié)點(diǎn)間的通信轉(zhuǎn)化為client節(jié)點(diǎn)與代理節(jié)點(diǎn)間的C/S通信，使用代理節(jié)點(diǎn)將請(qǐng)求轉(zhuǎn)發(fā)給server節(jié)點(diǎn)。ROS中的數(shù)據(jù)通過(guò)該方式轉(zhuǎn)化為JSON數(shù)據(jù)進(jìn)行傳輸。ROSbridge協(xié)議規(guī)范的核心思想是，任何支持JSON語(yǔ)句的平臺(tái)都可通過(guò)與ROSbridge協(xié)議的對(duì)話實(shí)現(xiàn)與ROS的數(shù)據(jù)交互。筆者在設(shè)計(jì)方案時(shí)，使用ROSbridge協(xié)議實(shí)現(xiàn)ROS與Unity3D平臺(tái)及實(shí)體機(jī)器人的數(shù)據(jù)傳輸，整體數(shù)據(jù)通信框架如圖10所示。

圖10 平臺(tái)間的通信

仿真運(yùn)行虛實(shí)驅(qū)動(dòng)系統(tǒng)軟件時(shí)，Unity3D將用戶輸入的運(yùn)動(dòng)軌跡控制指令傳輸?shù)街噶罱忉屍?，解析處理后，將指令中的運(yùn)動(dòng)方式及路徑點(diǎn)信息傳輸?shù)絉OS。ROS根據(jù)獲取的數(shù)據(jù)進(jìn)行運(yùn)動(dòng)學(xué)規(guī)劃解算，并將解算完成的結(jié)果通過(guò)話題的形式輸出到Unity3D平臺(tái)。仿真環(huán)境中的虛擬設(shè)備根據(jù)獲取的數(shù)據(jù)進(jìn)行動(dòng)作執(zhí)行，實(shí)現(xiàn)工業(yè)機(jī)器人的運(yùn)動(dòng)控制。

ROS與Unity3D平臺(tái)使用ROSbridge中的WebSocket通道協(xié)議傳輸數(shù)據(jù)。進(jìn)行WebSocket通信連線時(shí)，客戶端瀏覽器首先發(fā)送連線請(qǐng)求，服務(wù)器根據(jù)接收到的請(qǐng)求進(jìn)行回應(yīng)，實(shí)現(xiàn)數(shù)據(jù)通道的建立。通道建立后，兩者之間即可以進(jìn)行信息的傳輸，如圖11所示。

圖11 WebSocket通信

ROS與Unity3D平臺(tái)使用ROSbridge進(jìn)行數(shù)據(jù)通信前，要在Unity3D平臺(tái)安裝ROSbridge功能插件。Unity3D使用C+語(yǔ)言進(jìn)行功能開(kāi)發(fā)，因此與ROS的通信需要依靠對(duì)應(yīng)的軟件庫(kù)與工具ROS#。在Unity3D中創(chuàng)建節(jié)點(diǎn)的過(guò)程如圖12所示。

圖12 Unity3D節(jié)點(diǎn)創(chuàng)建

通信關(guān)系建立后，Unity3D創(chuàng)建的節(jié)點(diǎn)會(huì)在ROS的ROSmaster注冊(cè)，通過(guò)rosbridge_websocket節(jié)點(diǎn)完成ROS與Unity3D平臺(tái)節(jié)點(diǎn)間話題內(nèi)容的數(shù)據(jù)傳輸。

實(shí)現(xiàn)虛擬控制器工業(yè)機(jī)器人模塊功能時(shí)，ROS與Unity3D平臺(tái)之間需要建立3個(gè)通信話題：①robot_style話題，其數(shù)據(jù)內(nèi)容為機(jī)器人類(lèi)型，根據(jù)機(jī)器人類(lèi)型啟動(dòng)對(duì)應(yīng)的launch文件；②robot_control話題，其數(shù)據(jù)內(nèi)容為指令類(lèi)型及點(diǎn)位數(shù)據(jù)，根據(jù)控制指令進(jìn)行機(jī)器人的路徑插補(bǔ)及軌跡規(guī)劃；③Joint_State話題，其數(shù)據(jù)內(nèi)容為機(jī)器人關(guān)節(jié)軸數(shù)據(jù)，用于驅(qū)動(dòng)虛擬機(jī)器人的仿真運(yùn)行；④IK_State話題，其數(shù)據(jù)內(nèi)容為機(jī)器人在笛卡爾空間中的位姿數(shù)據(jù)，用于顯示機(jī)器人的狀態(tài)。圖13所示為機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)中ROS與Unity3D通信節(jié)點(diǎn)圖。

圖13 ROS與Unity3D通信節(jié)點(diǎn)圖

3 案例驗(yàn)證

根據(jù)對(duì)數(shù)字孿生車(chē)間中工業(yè)機(jī)器人功能的分析，抽象出不同的基礎(chǔ)功能模塊，基于數(shù)字孿生車(chē)間虛實(shí)驅(qū)動(dòng)系統(tǒng)模塊化的構(gòu)建方法，建立工業(yè)機(jī)器人的虛擬實(shí)體模型，并對(duì)虛擬實(shí)體的隨動(dòng)功能、軌跡程序同步運(yùn)行功能及反饋控制功能進(jìn)行驗(yàn)證。

使用ROS開(kāi)發(fā)機(jī)器人虛擬控制器時(shí)，工業(yè)機(jī)器人的屬性參數(shù)以文件的形式存儲(chǔ)，因此不同類(lèi)型工業(yè)機(jī)器人控制器的添加通過(guò)啟動(dòng)對(duì)應(yīng)的launch文件實(shí)現(xiàn)。筆者在Unity3D平臺(tái)中定義的robot_style話題發(fā)布機(jī)器人的型號(hào)參數(shù)，ROS端接收到型號(hào)參數(shù)后，通過(guò)調(diào)用函數(shù)啟動(dòng)launch文件。

3.1 虛實(shí)驅(qū)動(dòng)系統(tǒng)隨動(dòng)功能驗(yàn)證

工業(yè)機(jī)器人虛擬模型的隨動(dòng)功能模塊需與創(chuàng)建的機(jī)器人控制器傳輸數(shù)據(jù)，驅(qū)動(dòng)仿真空間中工業(yè)機(jī)器人虛擬模型的運(yùn)行。在關(guān)節(jié)空間控制關(guān)節(jié)軸或在笛卡兒空間中基于選擇的坐標(biāo)系控制增量時(shí)，需要在Unity3D平臺(tái)中定義robot_control話題，發(fā)布運(yùn)動(dòng)類(lèi)型及增量值。增量值在定義過(guò)程中設(shè)為最大值，在實(shí)際使用過(guò)程中根據(jù)選擇的運(yùn)動(dòng)倍率調(diào)整發(fā)布參數(shù)。ROS接收到運(yùn)動(dòng)類(lèi)型及增量值后，進(jìn)行運(yùn)動(dòng)學(xué)逆解及插補(bǔ)運(yùn)算，將解算后的機(jī)器人關(guān)節(jié)角度通過(guò)JointState話題進(jìn)行發(fā)布。Unity3D通過(guò)訂閱JointState話題接收機(jī)器人數(shù)據(jù)，并將數(shù)據(jù)傳輸?shù)教摂M機(jī)器人控制腳本，實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制。機(jī)器人運(yùn)動(dòng)過(guò)程中，借助ROS的TF工具包提供的tf：：transformPose()函數(shù)對(duì)機(jī)器人各坐標(biāo)系在空間中的相對(duì)位姿關(guān)系進(jìn)行轉(zhuǎn)換，并將轉(zhuǎn)換結(jié)果通過(guò)定義的IK_State話題發(fā)布。Unity3D訂閱接收數(shù)據(jù)后，將坐標(biāo)系相對(duì)位姿數(shù)據(jù)顯示在界面中。機(jī)器人隨動(dòng)功能節(jié)點(diǎn)間關(guān)系如圖14所示。

圖14 機(jī)器人隨動(dòng)功能節(jié)點(diǎn)間關(guān)系

連接華數(shù)機(jī)器人控制器時(shí)，通過(guò)調(diào)用二次開(kāi)發(fā)庫(kù)接口的proVar.getJR()與proxyVar.getLR()函數(shù)，實(shí)時(shí)獲取機(jī)器人關(guān)節(jié)軸角度及末端執(zhí)行器的坐標(biāo)軸數(shù)據(jù)，并將獲取的數(shù)據(jù)傳輸?shù)讲煌墓δ苣K進(jìn)行計(jì)算。該部分功能的控制邏輯如圖15所示，連接物理空間中的機(jī)器人控制器獲取關(guān)節(jié)軸角度，并將其傳輸?shù)綘顟B(tài)隨動(dòng)模塊的控制腳本，驅(qū)動(dòng)仿真設(shè)備運(yùn)行。

圖15 軸數(shù)據(jù)獲取控制邏輯

3.2 軌跡程序同步運(yùn)行功能的驗(yàn)證

程序指令解析功能模塊按照所選機(jī)器人，解析獲取的軌跡指令程序，輸出運(yùn)動(dòng)類(lèi)型及目標(biāo)點(diǎn)位姿參數(shù)。輸入程序文本并解析運(yùn)行后，仿真空間中的虛擬機(jī)器人按照編輯的軌跡程序指令運(yùn)行，解釋器檢查機(jī)器人指令程序的正確性。出現(xiàn)錯(cuò)誤時(shí)，機(jī)器人程序會(huì)在錯(cuò)誤段落暫停，并在消息框中顯示出錯(cuò)誤內(nèi)容，如圖16所示。

圖16 動(dòng)作程序指令同步運(yùn)行驗(yàn)證

3.3 反饋控制模塊功能驗(yàn)證

與實(shí)際機(jī)器人通信時(shí)，采用TCP協(xié)議與機(jī)器人控制器進(jìn)行通信，根據(jù)機(jī)器人控制類(lèi)型調(diào)用對(duì)應(yīng)的二次開(kāi)發(fā)接口函數(shù)。添加虛擬設(shè)備模型后，通過(guò)匹配IP地址及端口連接實(shí)際機(jī)器人的控制器，選擇創(chuàng)建的機(jī)器人連接節(jié)點(diǎn)并導(dǎo)入的仿真模型。創(chuàng)建的示教連接通信成功后，實(shí)際機(jī)器人的位置信息文本框會(huì)顯示機(jī)器人的關(guān)節(jié)軸角度和坐標(biāo)系參數(shù)。同時(shí)，操作界面會(huì)出現(xiàn)機(jī)器人的控制面板，機(jī)器人控制面板中的“運(yùn)動(dòng)類(lèi)型”按鈕用于關(guān)節(jié)軸的控制與笛卡兒線性控制之間的轉(zhuǎn)換；“運(yùn)動(dòng)倍率”按鈕用于調(diào)節(jié)機(jī)器人的運(yùn)動(dòng)速度；“控制類(lèi)型”按鈕用于 “手動(dòng)模式”及“自動(dòng)模式”之間的切換；“坐標(biāo)系”按鈕用于工具坐標(biāo)系、基坐標(biāo)系、工件坐標(biāo)系的創(chuàng)建、設(shè)置與切換，如圖17所示。

圖17 機(jī)器人控制面板

本文提出的工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)模塊化的構(gòu)建方法，不僅可以快速便捷地完成虛實(shí)驅(qū)動(dòng)系統(tǒng)的構(gòu)建，所構(gòu)建的系統(tǒng)能夠支持仿真狀態(tài)以及同步運(yùn)行狀態(tài)下的機(jī)器人軌跡運(yùn)行。

4 結(jié)語(yǔ)

本文提出的工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)模塊化的構(gòu)建方法根據(jù)對(duì)工業(yè)機(jī)器人的功能分析，抽象出不同的基礎(chǔ)功能模塊，可對(duì)功能模塊參數(shù)進(jìn)行自定義，提高了構(gòu)建數(shù)字孿生車(chē)間工業(yè)機(jī)器人虛實(shí)驅(qū)動(dòng)系統(tǒng)的效率。