摘 要：為提高遙操作機器人的示教編程效率和在線示教的安全性，提出了面向遙操作機器人在線示教的數字孿生框架，建立了機器人及其工作場景的虛擬模型，實現了物理實體到數字孿生模型的映射；設計並實現了基於RGB-D圖像和姿態示教器的遙示教方式，通過滑鼠和姿態示教器規劃虛擬機器人末端執行器的路徑和姿態，並利用增強現實技術，實現了虛實融合示教，提高了遙操作機器人在線示教的人機互動性；構建了機器人及其工作場景的八叉樹模型，用以預測規劃路徑和姿態下機器人和工作場景之間的碰撞干涉；系統將未發生碰撞干涉的規劃路徑和姿態指令發送給物理機器人控制器控制物理機器人運動，從而實現基於數字孿生遙操作機器人在線示教功能，提高在線示教系統的安全性。
關鍵詞:遙操作機器人;在線示教;數字孿生;增強現實;人機互動;碰撞檢測;
Online teaching system of teleoperation robot based on digital twin model
CHEN Chengjun DING Xutong PAN Yong ZHANG Hao HONGJun
School of Mechanical and Automotive Engineering, aingdao University of Technology School of Mechanical Engineering, Xi an Jiaotong University
Abstract：In order to improve the teaching efficiency and safety of teleoperation robot, a digital twin framework for teleoperation robot is proposed. To map physical entity to the digital twin model, virtual models of both the robot and its working scene are established. A teleoperation method based on RGB-D image and posture teaching device is designed and implemented. In this method, the paths and postures of the virtual robot end-effector are controlled by mouse and posture teaching device. In addition, the augmented reality (AR) technology is used to combine the virtual modes with physical scene, and the AR-based teaching of teleoperation robot is realized. The octree models of the virtual robot and its working scene are also constructed to predict the collision interference between the virtual robot and the working scene. By collision detection technology, the system sends the planned paths and postures without collision interference to the physical robot controller to control the movement of the physical robot, so as to realize the robot online teaching of the teleoperation robot.
Keyword：teleoperation robot; online teaching; digital twin; augmented reality; human-computer interaction; collision detection;
0 引言
工業機器人廣泛應用於加工製造行業，工業機器人的推廣應用率是衡量一個國家工業自動化水平的重要標誌。目前機器人編程方式主要分為在線示教編程、離線示教編程和自主示教編程[1]，在線示教編程的過程繁瑣，效率低；離線示教編程對專業性要求高，建模工作量大，操作複雜；自主編程的場景適應能力和抗干擾能力差、成本高。
遙操作機器人[2]多應用於高溫、高輻射等惡劣環境或者人無法到達的非可及環境，當前主要採用主從式控制方式，示教人員與從端機器人位於不同的物理空間，通過操作主端機器人控制從端機器人執行示教任務。由於機器人環境感知能力不足、人機互動性差等問題制約，導致遙操作機器人示教編程效率低且安全性難以保證。為提高遙操作機器人示教編程效率和安全性，Wang等人[3]提出了一種3D模型驅動的遠程機器人裝配方法，通過機器視覺方法建立遠程端裝配環境的3D虛擬模型，示教人員實時監測遠程端狀態，並在3D虛擬環境中利用虛擬機器人模擬裝配，用以驅動遠程端物理機器人運動，提高裝配效率。Liu等人[4]提出了一種基於信息物理融合系統（CPS）的遠程人機協作系統，利用本地端機器人控制遠程端機器人，為操作員提供了遠程端實時的生產環境。Nikolaos等人[5]面向人機協作，採用CPS技術，提出了基於安全距離實時評估和觸發碰撞預防措施的人機協作閉環控制系統，用以保障人機協作中示教人員的人身安全。Sung等人[6]設計了一種基於深度學習和數字孿生的人機安全協作的混合現實系統，該系統通過實時測量最小安全距離，並通過混合現實眼鏡向示教人員提供可視化信息，保障了人機互動的安全性。Liu等人[7]提出了一種基於上下文感知的無碰撞遠程人機協作系統，該系統利用碰撞感知模塊可以避免機器人與示教人員的碰撞，並保證機器人可以及時到達目標位置。上述研究多關注遙操作機器人離線示教，對遙操作機器人在線示教以及示教過程中的安全性問題研究較少。
數字孿生（Digital Twin）是指在產品或系統的整個生命周期中，通過數據交互、信息融合、疊代計算和指令優化等方法，使物理實體與虛擬模型相互關聯，實現生產周期內模型、數據與技術的集成[8,9,10]。數字孿生可以利用虛擬模型、傳感器數據、運行數據等，在產品的全生命周期各階段將產品開發、產品製造、產品服務等各個環節數據在產品數字孿生體中進行關聯映射，通過數字孿生體反映物理實體的功能、實時運行狀態及運行發展趨勢，為物理實體的全生命周期提供更加實時、高效、智能的服務[11]。Bilberg等人[12]建立了人機協作的工作場景和裝配工作的數字孿生體，數字孿生體在生產系統的生命周期內通過不斷地鏡像物理系統來保持更新，以便快速、安全地嵌入並持續改進，提高了的人機協作的設計、構建和控制的效率。Tipary等人[13]提出了一種基於數字孿生的柔性機器人工作單元開發方法，允許在數字空間中改進工作單元，以指導柔性機器人的抓取和放置動作，加快了整體調試或重新配置的過程。Szybicki等人[14]提出了一種基於數字孿生和虛擬現實技術的工業機器人示教編程方法，通過虛擬現實技術生成機器人運動路徑，通過物理實體和數字孿生模型的交互來執行示教任務，提高了工業機器人示教的人機互動性。楊艷芳等人[15]針對斷路器柔性裝配單元機器人，結合數字孿生技術，提出了一種支持機器人運動規劃和控制的柔性裝配機器人數字孿生框架，通過對裝配過程中的物理實體的數字孿生建模以及與機器人運動控制器間的實時通信，優化了裝配生產過程。上述研究表明，數字孿生技術可以實現機器人示教過程中物理實體和虛擬模型間的實時同步映射，為示教提供實時可視化的示教依據，可以通過數字孿生模型與物理實體的交互提高示教效率，在提高機器人示教效率和示教安全性方面具有很大的潛力。
為提高遙操作機器人在線示教的編程效率和安全性，本文提出面向遙操作機器人在線示教的數字孿生框架，建立遙操作機器人及其工作場景數字孿生模型，實現了物理實體到數字孿生模型的映射，為後續的位姿規劃提供實時的可視化依據；設計基於RGB-D圖像和姿態示教器的遙示教方式，結合增強現實技術，通過滑鼠和姿態示教器實時規劃機器人末端執行器的路徑和姿態，實現了虛實融合示教，提高遙操作機器人的示教效率；構建機器人及其工作場景的八叉樹模型，預測規劃位姿下機器人和工作場景之間的碰撞，系統將未發生碰撞干涉的規劃路徑和姿態指令發送至物理機器人，控制物理機器人運動，提高遙操作機器人在線示教的安全性。
1遙操作機器人在線示教數字孿生體系架構
本研究使用數字孿生技術在線預測遙操作機器人按照示教路徑和姿態運動時是否存在碰撞干涉，並以此決定是否控制物理機器人運動。因此，本文提出了如圖1所示的基於數字孿生的遙操作機器人在線示教框架，該框架主要由物理單元和數字孿生單元兩部分組成。其中物理單元由物理機器人及工作場景、機器人控制器和RGB-D相機等組成，數字孿生單元包括機器人數字模型單元和遙操作機器人示教單元。物理單元利用RGB-D相機等傳感器獲取機器人及其工作場景的狀態信息，將傳感器獲取的實時數據發送至數字模型單元；數字模型單元對獲取的幾何數據、關節數據、RGB-D圖像數據以及位姿數據等進行分析處理，建立機器人及其工作場景的數字孿生模型。實時數據與孿生模型相結合，預測示教過程中可能存在的碰撞干涉。遙操作機器人示教單元將數據、孿生模型以及碰撞檢測結果等在上位機中實時呈現，為示教人員規劃機器人位姿提供可視化依據。示教人員實時監測示教平台規劃機器人的位姿，將規劃的位姿數據發送至數字模型單元進行預測驗證，無碰撞干涉時生成控制指令，通過ROS控制機器人運動。
本研究提出的基於數字孿生的遙操作機器人在線示教系統工作原理如下：數字模型單元根據物理機器人的尺寸參數建立機器人三維模型，根據當前姿態時物理機器人的位姿數據，利用機器人逆運動學求解，得出機器人各關節數據，使用關節數據驅動機器人三維模型運動；根據RGB-D相機拍攝工作場景的RGB-D圖像進行三維重建；通過八叉樹建模建立機器人及其工作場景的碰撞檢測模型；根據手眼標定獲得機器人基坐標系與相機坐標系的轉換矩陣，將虛擬機器人模型疊加到工作場景點雲圖像中，從而建立機器人及其工作場景的數字孿生模型。遙操作機器人示教單元使用增強現實註冊技術將虛擬機器人模型疊加到工作場景的RGB圖像中，實現虛實位置一致；示教人員利用滑鼠點擊工作場景的RGB圖像獲取機器人末端執行器運動路徑關鍵點的三維坐標，將姿態示教器姿態作為機器人末端執行器姿態的輸入，在虛實融合的示教環境中規劃機器人的路徑和姿態；將規劃的路徑和姿態數據發送至碰撞檢測模型，進行基於八叉樹模型的碰撞檢測，驗證示教人員規劃的機器人路徑和姿態的可行性，若無碰撞干涉，再將示教路徑和姿態指令傳輸至物理機器人，控制機器人完成在線示教操作。
與傳統遙操作機器人在線示教過程相比，本研究建立了機器人及其工作場景的數字孿生模型，可以實時反饋物理機器人的位姿數據以及工作場景的實時變化，實現了數字孿生模型對實體機器人及其工作場景的實時動作、行為和狀態的映射；基於八叉樹模型建立了機器人及其工作場景的碰撞檢測模型，可以實時監測機器人在示教過程中是否存在碰撞干涉。本研究通過數字孿生模型預測機器人是否與工作場景產生碰撞干涉，提高了示教工作的安全性；通過增強現實技術，在虛實融合的場景中交互規劃機器人的路徑和姿態，提高了在線示教的人機互動性和示教效率。
2機器人及工作場景數字孿生模型的構建
本研究使用ROS（Robot Operating System）建立遙操作機器人在線示教的數字孿生模型，實現物理單元與數字孿生單元之間數據信息的交互，主要用於預測遙操作機器人示教單元輸出的路徑和姿態是否存在碰撞干涉。基於對孿生模型數據信息分析和處理，向遙操作機器人在線示教提供智能化的支持與服務。
2.1 虛擬機器人模型的構建
建立與物理實體相一致的數字化虛擬模型是數字孿生建模的關鍵之一。本研究選用ABB_IRB120型號機器人，建立虛擬機器人模型，並添加虛擬機器人模型各關節坐標系。根據當前位姿下機器人的位姿數據，對機器人進行正逆運動學分析，計算機器人關節數據，使虛擬機器人模型與物理機器人的運動保持一致。建立的虛擬機器人模型如圖2所示。
2.2 機器人工作場景數字孿生模型的構建
為了構建機器人工作場景數字孿生模型，本文使用RGB-D相機採集機器人工作場景的深度圖像，ROS將深度圖像轉換成點雲圖像，實現三維重建。相機的深度值Z已知，根據式（1）所示的深度相機成像模型可以得到深度圖像坐標與點雲坐標的轉換矩陣[18]：
⎧⎩⎨⎪⎪X=(x−cx)ZfxY=(y−cy)Zfy(1)
其中，(X,Y,Z)為點雲的三維坐標，(x,y)為深度圖像坐標，fx、fy為深度相機在X軸和Y軸方向上的焦距，cx、cy為深度相機主點坐標。當相機在工作場景中移動時，ROS系統採集到不同的視角的工作場景深度圖像，通過採用圖像幀間的點雲配准算法，使用新的深度圖像視頻幀將前序深度圖像視頻幀中的空洞進行填充，最終融合拼接出整個工作場景的點雲圖像。
如圖3所示，由於相機獲得的點雲圖像視野範圍過大,會採集到與機器人工作空間無關的場景點雲圖像，導致點雲更新速率慢且點雲圖像邊緣過於粗糙，不利於後續的碰撞檢測。因此，本研究採用直通濾波、體素濾波和半徑濾波對點雲圖像進行處理，縮小點雲圖像的視野範圍，使點雲圖像的邊緣更加清晰。具體步驟如下：
（1）直通濾波處理：在相機坐標系各坐標軸上設置一個閾值範圍（0~1.3m），將閾值範圍之外的離群點去除，消除與機器人工作場景無關的點雲，縮小點雲圖像的視野範圍；
（2）體素濾波處理：將直通濾波處理後的點雲圖像均分成若干個大小為0.005m的三維體素網格，該體素網格內的所有點都通過體素網格的一個重心點來表示，實現點雲圖像下採樣，即體素濾波處理，使得採樣點對應的曲面更為準確，點雲邊緣更加清晰；
（3）半徑濾波處理：即以點雲圖像的某個點雲點為球心，在半徑為0.002m的球體內，若與球心相鄰的點雲點個數小於30個，則將該點雲點和所有相鄰的點雲都剔除；若大於或等於30個，則保留所有點雲點。以此進一步消除與機器人工作場景無關的離群點。
經過直通濾波、體素濾波和半徑濾波處理後的點雲圖像如圖4所示。
2.3 機器人手眼標定
為了將虛擬機器人模型與工作場景三維點雲模型融合，實現位置一致，需要對機器人和RGB-D相機進行手眼標定，得到相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣TCW。本研究將相機安裝在機器人外一固定位置，相機坐標系相對於機器人基坐標系固定不變，機器人末端執行器坐標系和標識卡坐標系隨機器人位姿的變化而變化，故採用Eye-To-Hand手眼標定方法[19]，標定示意圖如圖5所示。
由圖5可知，機器人基坐標系、末端執行器坐標系、標識卡坐標系和相機坐標系的轉換矩陣如下[20]：
(TCA)(i)=TCW⋅(TWM)(i)⋅TMA,i∈[1,n](2)
其中，TCA表示相機坐標系O-XCYCZC到標識卡坐標系O-XAYAZA的轉換矩陣，TCW表示相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣，TWM表示機器人基坐標系O-XWYWZW到機器人末端執行器坐標系O-XMYMZM的轉換矩陣,TMA表示機器人末端執行器坐標系O-XMYMZM到標識卡坐標系O-XAYAZA的轉換矩陣，n表示機器人位姿變化次數。將TMA消去得到：
(TCA)(i+1)⋅(TCA)−1(i)⋅TCW=TCW⋅(TWM)(i+1)⋅(TWM)−1(i),i∈[1,n−1](3)
令A=(TCA)(i+1)⋅(TCA)−1(i)，B=(TWM)(i+1)⋅(TWM)−1(i)，則式（3）可簡化為：
A⋅TCW=TCW⋅B(4)
其中，TCA可通過相機識別標識卡直接測得，TWM可通過機器人正運動學分析得到。故使用式（4）可求得相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW間的轉換矩陣TCW。根據轉換矩陣TCW，在虛擬工作場景中導入機器人三維模型，建立如圖6所示的機器人與其工作場景融合後的三維模型。上述機器人及其工作場景的數字孿生建模方法同樣適用於移動機器人，若將機器人和RGB-D相機安裝在移動平台上，只需保證操作過程中RGB-D相機與工業機器人之間的相對位置不變。
通過建立機器人及其工作場景的數字孿生模型，實現了物理實體和虛擬模型的實時動作、行為和狀態的同步映射，示教人員可以實時監測機器人的運行狀態以及工作場景的變化，為遙操作機器人示教提供可視化依據。
3碰撞檢測
為提高遙操作機器人在線示教的安全性，需要在物理機器人執行運動指令前先對規劃的路徑和姿態數據進行碰撞檢測。為此，本研究採用OBB (Oriented Bounding Box)碰撞檢測方法[21]，建立機器人及其工作場景的八叉樹模型，並進行碰撞檢測，提高遙操作機器人在線示教的安全性。建立碰撞檢測模型的具體步驟如下：
（1）如圖7（a）所示，將機器人及其工作場景的點雲圖像進行分割，建立八叉樹模型，並定義八叉樹模型的每個立方體空間的大小。如圖7（b）所示，首先將整個工作場景點雲圖像的立方體空間作為八叉樹模型的第一層（即根節點）；然後將整個點雲圖像平均分成8個立方體子空間，構成八叉樹模型第二層，每個立方體為第一層的子節點；將第二層的8個立方體分割成更小的8個立方體，形成第三層；依次類推，直至最後一層葉子節點，從而建立工作場景的八叉樹模型。八叉樹分割的層數越多，葉子層節點的立方體子空間越小，碰撞檢測精度越高；反之，八叉樹分割的層數越少，則葉子層節點的立方體子空間越大，碰撞檢測精度越低。
（2）為提高八叉樹模型的精度和更新速率，並將物理機器人的八叉樹模型從工作場景的八叉樹模型中剔除，需要修改ROS系統中點雲圖像轉換八叉樹模型的插件文件。首先設置八叉樹模型最大分割層次和最大更新頻率，在保證點雲圖像轉換八叉樹模型的速率的前提下，提高八叉樹模型精度；之後結合手眼標定得到的相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣TCW，將虛擬機器人模型置入工作場景八叉樹模型中；再將八叉樹模型填充抵消率和八叉樹模型填充比例設置為1，ROS系統可以將與虛擬機器人模型大小相同體積內的八叉樹模型剔除，避免將物理機器人誤判成障礙物進行碰撞檢測。根據如圖8（a）所示的真實工作場景，將虛擬機器人模型與工作場景八叉樹模型融合生成如圖8（b）所示的碰撞檢測模型。
建立機器人及其工作場景的碰撞檢測模型後即可進行兩者之間的碰撞檢測，具體步驟如下：示教人員使用滑鼠點擊工作場景RGB圖像確定虛擬機器人路徑點，使用姿態示教器規劃虛擬機器人末端執行器的姿態；遙操作機器人在線示教單元將規劃好的示教路徑與姿態數據發送至碰撞檢測模型，更新虛擬機器人模型；RGB-D相機拍攝工作場景實時的深度圖像，對工作場景進行八叉樹建模；使用OBB碰撞檢測算法對虛擬機器人模型和工作場景的八叉樹模型進行碰撞檢測，檢測示教路徑與姿態下虛擬機器人是否與周圍環境碰撞。如圖9所示，若虛擬機器人某關節與工作場景有碰撞干涉的情況出現，則將該關節標紅，並在示教平台輸出報警信息，同時控制物理機器人停止運動；若不產生碰撞則將示教路徑和姿態數據發送至機器人控制器，控制機器人運動。本碰撞檢測方法在機器人執行示教任務的過程中也可進行實時碰撞檢測。
4基於RGB-D圖像和姿態示教裝置的遙操作機器人在線示教
為了提高遙操作機器人的編程效率和人機互動性，本團隊提出了一種基於RGB-D圖像和姿態示教裝置的機器人示教方法[22]，該方法通過RGB-D相機獲取機器人工作場景實時的RGB圖像和深度圖像，通過增強現實註冊將虛擬機器人模型疊加到機器人工作場景的RGB圖像中，示教人員使用滑鼠點擊選取工作場景的RGB圖像路徑點，使用姿態示教器規劃虛擬機器人末端執行器的姿態，將規劃的路徑與姿態數據保存並發送至物理機器人，控制其運動，實現機器人的路徑和姿態示教。該方法雖然提供了一種高效便捷的機器人示教方式，但是規劃的路徑與姿態數據未經驗證就發送至物理機器人，使得物理機器人的示教過程存在安全隱患。因此本研究在遙操作機器人數字孿生模型的基礎上，採用基於RGB-D圖像和姿態示教裝置的示教方法，將規劃的路徑與姿態數據先發送至ROS系統中的機器人碰撞檢測模型進行碰撞檢測，無碰撞發生時再將運動指令發送至物理機器人完成遙示教編程，降低遙操作機器人在線示教的安全隱患，具體示教流程如圖10所示。
（1）在進行機器人位姿規劃前，RGB-D相機拍攝工作場景實時RGB圖像，根據手眼標定得到的相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣TCW轉換，將虛擬機器人模型疊加到真實工作場景中，使虛擬機器人基坐標系和物理機器人基坐標系重合，實現增強現實註冊。
（2）如圖11（a）和（b）所示，示教人員使用滑鼠點擊選取工作場景的RGB圖像確定遙操作機器人在線示教的路徑點，路徑點會同時顯示在RGB圖像和深度圖像中。如圖12所示，根據RGB圖像與深度圖像之間的映射關係以及相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣，來計算示教路徑點在物理機器人基坐標系O-XWYWZW下的三維坐標P(xw,yw,zw)，具體步驟如下：
①已知圖像上路徑點P在RGB圖像中的像素坐標為p(uc,vc)，深度相機的深度值為d，根據RGB圖像坐標系O-UcVc到深度圖像坐標系O-UdVd的轉換矩陣可以得到路徑點P在深度圖像坐標系O-UdVd中的像素坐標p(ud,vd,d);
②根據深度相機的內部參數矩陣Mt和深度相機的焦距f，可以得到深度圖像中任意一點p(ud,vd,d)，在深度相機坐標系O-XdYdZd中的三維坐標(xd,yd,zd)：
③根據深度相機坐標系O-XdYdZd到RGB相機坐標系O-XcYcZc的轉換矩陣，計算得到路徑點P在RGB相機坐標系中的三維坐標系(xc,yc,zc)；
④根據手眼標定得到的RGB相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣TCW，計算到的路徑點P在物理機器人基坐標系O-XWYWZW中的三維坐標P(xw,yw,zw)。
計算出路經點在物理機器人基坐標系O-XWYWZW中的三維坐標P(xw,yw,zw)後，經機器人逆運動學模型求解，得到虛擬機器人各關節的旋轉角度，驅動虛擬機器人末端執行器以默認的姿態移動至相應的路徑點，如圖11（c）所示。
（3）路徑規劃結束後，使虛擬機器人模型的末端執行器跟隨路徑點運動，在運動的過程中，示教人員使用姿態示教器對虛擬機器人末端執行器的姿態進行規劃。如圖13（a）所示，姿態示教器採用慣性傳感器模塊在地理坐標系中輸出姿態數據，並通過藍牙模塊將姿態數據傳輸至計算機。在姿態示教過程中，示教人員通過調節姿態示教器的姿態來規劃虛擬機器人末端執行的姿態，並通過AR可視化介面實時觀察，以達到預期姿態，如圖13（b）所示。
（4）在進行位置和姿態規劃的同時，實時將規劃的位姿數據發送至ROS系統，更新機器人碰撞檢測模型，通過機器人逆運動學計算虛擬機器人各關節的運動角度，控制虛擬機器人模型運動。RGB-D相機採集工作場景實時深度圖像，生成點雲圖像並進行八叉樹建模。虛擬機器人模型與工作場景的八叉樹模型進行碰撞檢測，驗證位姿規劃的合理性。若無碰撞發生，則將生成機器人運動指令發送至機器人控制器，控制物理機器人運動。本研究在控制物理機器人運動前，先預測機器人與工作場景之是否存在碰撞干涉，降低了碰撞的風險，提高了遙操作機器人在線示教的安全性。
5實驗驗證與分析
為驗證基於數字孿生的遙操作機器人在線示教方法的可行性和安全性，本文設計了如圖14所示的實驗系統。實驗系統由本地端和遠程端組成，其中本地端包括機器人、機器人控制器、機器人工作檯和RGB-D相機等；遠程端包括示教人員、兩台計算機和姿態示教器等。示教人員在1號計算機（CPU：i7-9700，RAM：16GB，顯卡：RTX2060，系統：Ubuntu16.04，ROS：kinetic）中運行ROS系統，進行數字孿生建模，建立機器人及其工作場景的數字孿生模型；在2號計算機（CPU：i5-9300，RAM：16GB，顯卡：GTX1650，系統：Windows 10）中基於VS2013開發環境，搭建AR示教環境，採用滑鼠和姿態示教器進行機器人的位姿規劃，實現虛實融合示教。
具體實驗流程如下：
（1）在進行遙操作機器人在線示教前，首先需要對機器人及其工作場景進行數字孿生建模。示教人員在1號計算機對物理機器人進行手眼標定，計算RGB-D相機在機器人基坐標系中的位姿；根據物理機器人的幾何和關節數據以及RGB-D相機採集的工作場景的RGB-D圖像，建立機器人及其工作場景數字孿生模型，之後基於RGB-D相機坐標繫到機器人基坐標系的轉換矩陣，將機器人數字孿生模型和工作場景數字孿生模型融合，如圖15（a）所示；最後根據工作場景的深度圖像建立工作場景的八叉樹模型，基於RGB-D相機的位姿，將機器人數字孿生模型與工作場景八叉樹模型融合，建立如圖15（b）所示的碰撞檢測模型。
（2）示教人員在2號計算機根據手眼標定得到相機位姿，將虛擬機器人模型疊加到工作場景的RGB圖像中，搭建如圖16（a）所示的AR示教環境；其中路徑規劃窗口分為RGB圖像窗口和深度圖像窗口，如圖16（b）和（c）所示，深度圖像中藍色區域表示距離，顏色越深，距離越近。
（3）如圖17（a）和（b）所示，示教人員使用滑鼠點擊工作場景的RGB圖像，獲取機器人末端執行器的路徑點，點擊的路徑點會顯示在工作場景的RGB圖像和深度圖像上。示教系統根據圖12所示的轉換矩陣計算路徑點在機器人基坐標系的三維坐標，驅動虛擬機器人以默認的姿態運動至該路徑點，如圖17（c）所示。
（4）路徑規劃結束後，示教人員使用姿態示教器規劃虛擬機器人末端執行器在各路徑點的姿態。根據機器人當前的位姿數據，通過機器人逆運動學模型計算機器人各關節轉動的角度，驅動虛擬機器人模型運動。如圖18所示，在姿態規划過程中，虛擬機器人模型的姿態實時的顯示在AR示教窗口中，示教人員根據AR示教窗口調節虛擬機器人模型末端執行器的姿態。
（5）在進行機器人位姿規劃的同時，2號計算機將規劃的位姿數據實時發送至1號計算機進行碰撞檢測。如表1中碰撞檢測窗口中的虛擬運動軌跡所示，當機器人運動路徑上沒有碰撞干涉時，ROS將位姿數據發送至機器人控制器，控制物理機器人按規劃的姿態移動至路徑點，完成示教操作，如表1中機器人實際運動軌跡所示。
為驗證示教精度，本文以矩形軌跡示教為例，將規劃的路徑數據保存，使用最小二乘法將所有路徑點擬合為矩形，然後計算示教路徑點到矩形輪廓的最小距離，作為軌跡誤差，該誤差包括人的示教誤差、標定算法誤差、相機成像誤差等。實驗結果表明基於RGB-D圖像和姿態示教裝置的遙操作機器人示教方法的平均軌跡誤差為0.5050 mm, 最大誤差為 3.3261 mm[22]。因此示教軌跡精度可以滿足機器人噴塗、打磨等工作要求。
根據實驗結果可知，基於數字孿生的遙操作機器人在線示教方式可以實時地傳遞數據，可以準確地控制虛擬機器人的位姿，可以有效地檢測機器人的運動狀態，增強了機器人對場景的感知能力，提高了在線示教的人機互動性。此外，本研究成果還可應用於機器人實時碰撞檢測。在步驟（5）生成的機器人運動軌跡上設置障礙物，將步驟（5）生成路徑和姿態數據再次發送至ROS系統進行碰撞檢測。RGB-D相機實時拍攝工作場景，ROS系統根據障礙物的深度圖像構建障礙物的八叉樹模型。碰撞檢測模型識別到障礙物，ROS系統對虛擬機器人的運動路徑進行重新規劃，如表2中虛擬運動軌跡所示。碰撞檢測窗口中的虛擬機器人的運動路徑由於存在碰撞干涉而發生了改變，但依然以規劃的姿態達到了目標路徑點。ROS系統將碰撞檢測後重新規劃位姿數據發送至機器人控制器，機器人的運動軌跡如表2中實際運動軌跡所示。
綜上可以看出，本文設計的基於數字孿生的遙操作機器人在線示教系統無需人工建立遙操作機器人所在環境的3D模型，通過滑鼠和姿態示教器規劃機器人末端執行器的位置和姿態，採用AR技術實現了示教結果的實時可視化，並且可以實時檢驗示教位置和姿態是否可行，可以提高遙操作機器人示教的安全性和示教效率，示教精度滿足機器人噴塗、打磨等應用。本文的研究成果對遙操作機器人在線示教和控制具有一定的參考價值。
6 結束語
為提高遙操作機器人在線示教的編程效率和安全性，本文設計了基於數字孿生的遙操作機器人在線示教系統。通過建立機器人及其工作場景的數字孿生模型，實現了虛擬模型和物理實體的實時映射；採用基於RGB-D圖像和姿態示教器的示教方式，實現了在虛實結合的場景中進行機器人路徑和姿態規劃；構建了機器人及其工作場景的八叉樹模型，可提前預測示教路徑和姿態下機器人和工作場景之間是否存在碰撞干涉，提高了遙操作機器人在線示教的可靠性、安全性和示教效率，為遙操作機器人在線示教提供了新思路。
參考文獻
[1] ONG S, THANIGAIVEL N, NEE A,et al. Augmented reality-assisted robot programming system for industrial applications, Robot. Comput. Integr. Manuf. 2020,61:101820.
[2] SHERIDANT B. Tele-robotics, automation, and human supervisory control[M]. Massachusetts: the MIT Press, 1992.
[3] WANG L, MOHAMMED A, ONORI M. Remote robotic assembly guided by 3d models linking to a real robot. CIRP Ann-Manuf Technol 2014,63(1):1–4.
[4] LIU H, WANG L. Remote human–robot collaboration: A cyber–physical system application for hazard manufacturing environment, J. Manuf. Syst. 2020,54:24–34.
[5] NIKOLAKIS N, MARATOS V, MAKRIS S. A cyber physical system (CPS) approach for safe human-robot collaboration in a shared workplace. Robot. Comput. Integr. Manuf. 56(2019)233–243.
[6] SUNG H, PARK K, DONG H R,et al. An integrated mixed reality system for safety-aware human-robot collaboration using deep learning and digital twin generation. Robot. Comput. Integr. Manuf.2022,73:102258.
[7] SCHLEICH B, ANWER N, MATHIEU L,et al. Shaping the Digital Twin for Design and Production Engineering. CIRP Annals—Manufacturing Technology 66:141–144.
[8] TAO Fei, ZHANG Meng, CHENG Jiangfeng,et al. Digital twin workshop:a new paradigm for future workshop[J].Computer Integrated Manufacturing Systems,2017,23(01):1-9. [陶飛,張萌,程江峰,等.數字孿生車間——一種未來車間運行新模式[J].計算機集成製造系統,2017,23(01):1-9.]
[9] ELISA N, LUCA F, MARCO M. A review of the roles of Digital twin in CPS-based production system[J]. Procedia Manufacturing,2017,11:939-948.
[10] TAO F,ZHANG M. Digital Twin Shop-Floor: A New Shop-Floor Paradigm Towards Smart Manufacturing[J].IEEE Access, 2017,5:20418-20427.
[11] TAO Fei, LIU Weiran, ZHANG Meng,et al. Five-dimension digital twin model and its ten applications[J]. Computer Integrated Manufacturing Systems, 2019,25(01):1-18. [陶飛,劉蔚然,張萌,等.數字孿生五維模型及十大領域應用[J].計算機集成製造系統,2019,25(01):1-18.]
[12] WU Pengxing, GUO Yu, HUANG Shaohua,et al. Visual real-time monitoring method for discrete manufacturing workshop based on digital twin[J]. Computer Integrated Manufacturing Systems, 2021,27(06):1605-1616. [吳鵬興,郭宇,黃少華,等.基於數字孿生的離散製造車間可視化實時監控方法[J].計算機集成製造系統,2021,27(06):1605-1616.]
[13] T地址ARY B, ERDS G. Generic development methodology for flexible robotic pick-and-place workcells based on Digital Twin[J]. Robot. Comput. Integr. Manuf. 2021,71:102140.
[14] BURGHARDT, SZYBICKI D, GIERLAKP,et al. Programming of Industrial Robots Using Virtual Reality and Digital Twins[J]. Applied sciences, 2020,10(2):486.
[15] YANG Yanfang, HE Huan,SHU Liang,et al.Digital twin robot and its motion control for flexible assembly of circuit breake[J/OL]. Computer Integrated Manufacturing Systems.2020, 26(11):2915-2926. DOI:10.13196/j.cims. 2020.11.002.[楊艷芳, 賀煥, 舒亮, 等. 斷路器柔性裝配數字孿生機器人及其運動控制[J]. 計算機集成製造系統, 2020,26(11):2915-2926. DOI:10.13196/j.cims. 2020.11.002.]
[16] LV Yongjun, LIU Feng, ZHENG Liaomo,et al. Application Analysis of Generalized and Modified D-H Method in Kinematic Modeling[J]. Computer system application,2016,25(05):197-202. [呂永軍,劉峰,鄭飂默,等.通用和修正D-H法在運動學建模中的應用分析[J].計算機系統應用,2016,25(05):197-202.]
[17] ZHU Heng, YANG Dongchao, CHANG Xu,et al. Calibration of Joint Parameters of Mainpulator Based on Partiale Swarm Optimization Method[J]. Machinery Design ＆ Manufacture, 2021(07):285-290. [朱衡,楊東超,常旭,等.利用粒子群優化方法標定機械臂關節參數[J].機械設計與製造,2021(07):285-290.]
[18] BETHENCOURT A, JAULIN L. 3D reconstruction using interval methods on the Kinect device coupled with an IMU [J].International Journal of Advanced Robotic Systems, 2013,10(2):1-10．
[19] STROBL K, HIRZINGERi G. Optimal Hand-Eye Calibration[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2007.
[20] CHEN Baocun,WU Wei, GUO Yu,et al. Automatic Hand – Eye Calibration System of Robot based on ROS[J]. Computer Simulation,2020,37(02):343-348. [陳寶存,吳巍,郭毓,等.基於ROS的機器人自動手眼標定系統設計[J].計算機仿真,2020,37(02):343-348.]
[21] ZHOU Junwei, WAN Yu, WAN Wanggen,et al.A Method of Collision Detection with OBB based on Octree[J]. Computer Applications and Software,2009,26(04):75-77. [周俊瑋,萬宇,萬旺根,等.一種基於八叉樹的OBB包圍盒碰撞檢測方法[J].計算機應用與軟體,2009,26(04):75-77.]
[22] PAN Y, Chen C, Li D,et al. Augmented reality-based robot teleoperation system using RGB-D imaging and attitude teaching device. Robot. Comput. Integr. Manuf. 2021,71:102167.
關鍵詞:遙操作機器人;在線示教;數字孿生;增強現實;人機互動;碰撞檢測;
Online teaching system of teleoperation robot based on digital twin model
CHEN Chengjun DING Xutong PAN Yong ZHANG Hao HONGJun
School of Mechanical and Automotive Engineering, aingdao University of Technology School of Mechanical Engineering, Xi an Jiaotong University
Abstract：In order to improve the teaching efficiency and safety of teleoperation robot, a digital twin framework for teleoperation robot is proposed. To map physical entity to the digital twin model, virtual models of both the robot and its working scene are established. A teleoperation method based on RGB-D image and posture teaching device is designed and implemented. In this method, the paths and postures of the virtual robot end-effector are controlled by mouse and posture teaching device. In addition, the augmented reality (AR) technology is used to combine the virtual modes with physical scene, and the AR-based teaching of teleoperation robot is realized. The octree models of the virtual robot and its working scene are also constructed to predict the collision interference between the virtual robot and the working scene. By collision detection technology, the system sends the planned paths and postures without collision interference to the physical robot controller to control the movement of the physical robot, so as to realize the robot online teaching of the teleoperation robot.
Keyword：teleoperation robot; online teaching; digital twin; augmented reality; human-computer interaction; collision detection;
0 引言
工業機器人廣泛應用於加工製造行業，工業機器人的推廣應用率是衡量一個國家工業自動化水平的重要標誌。目前機器人編程方式主要分為在線示教編程、離線示教編程和自主示教編程[1]，在線示教編程的過程繁瑣，效率低；離線示教編程對專業性要求高，建模工作量大，操作複雜；自主編程的場景適應能力和抗干擾能力差、成本高。
遙操作機器人[2]多應用於高溫、高輻射等惡劣環境或者人無法到達的非可及環境，當前主要採用主從式控制方式，示教人員與從端機器人位於不同的物理空間，通過操作主端機器人控制從端機器人執行示教任務。由於機器人環境感知能力不足、人機互動性差等問題制約，導致遙操作機器人示教編程效率低且安全性難以保證。為提高遙操作機器人示教編程效率和安全性，Wang等人[3]提出了一種3D模型驅動的遠程機器人裝配方法，通過機器視覺方法建立遠程端裝配環境的3D虛擬模型，示教人員實時監測遠程端狀態，並在3D虛擬環境中利用虛擬機器人模擬裝配，用以驅動遠程端物理機器人運動，提高裝配效率。Liu等人[4]提出了一種基於信息物理融合系統（CPS）的遠程人機協作系統，利用本地端機器人控制遠程端機器人，為操作員提供了遠程端實時的生產環境。Nikolaos等人[5]面向人機協作，採用CPS技術，提出了基於安全距離實時評估和觸發碰撞預防措施的人機協作閉環控制系統，用以保障人機協作中示教人員的人身安全。Sung等人[6]設計了一種基於深度學習和數字孿生的人機安全協作的混合現實系統，該系統通過實時測量最小安全距離，並通過混合現實眼鏡向示教人員提供可視化信息，保障了人機互動的安全性。Liu等人[7]提出了一種基於上下文感知的無碰撞遠程人機協作系統，該系統利用碰撞感知模塊可以避免機器人與示教人員的碰撞，並保證機器人可以及時到達目標位置。上述研究多關注遙操作機器人離線示教，對遙操作機器人在線示教以及示教過程中的安全性問題研究較少。
數字孿生（Digital Twin）是指在產品或系統的整個生命周期中，通過數據交互、信息融合、疊代計算和指令優化等方法，使物理實體與虛擬模型相互關聯，實現生產周期內模型、數據與技術的集成[8,9,10]。數字孿生可以利用虛擬模型、傳感器數據、運行數據等，在產品的全生命周期各階段將產品開發、產品製造、產品服務等各個環節數據在產品數字孿生體中進行關聯映射，通過數字孿生體反映物理實體的功能、實時運行狀態及運行發展趨勢，為物理實體的全生命周期提供更加實時、高效、智能的服務[11]。Bilberg等人[12]建立了人機協作的工作場景和裝配工作的數字孿生體，數字孿生體在生產系統的生命周期內通過不斷地鏡像物理系統來保持更新，以便快速、安全地嵌入並持續改進，提高了的人機協作的設計、構建和控制的效率。Tipary等人[13]提出了一種基於數字孿生的柔性機器人工作單元開發方法，允許在數字空間中改進工作單元，以指導柔性機器人的抓取和放置動作，加快了整體調試或重新配置的過程。Szybicki等人[14]提出了一種基於數字孿生和虛擬現實技術的工業機器人示教編程方法，通過虛擬現實技術生成機器人運動路徑，通過物理實體和數字孿生模型的交互來執行示教任務，提高了工業機器人示教的人機互動性。楊艷芳等人[15]針對斷路器柔性裝配單元機器人，結合數字孿生技術，提出了一種支持機器人運動規劃和控制的柔性裝配機器人數字孿生框架，通過對裝配過程中的物理實體的數字孿生建模以及與機器人運動控制器間的實時通信，優化了裝配生產過程。上述研究表明，數字孿生技術可以實現機器人示教過程中物理實體和虛擬模型間的實時同步映射，為示教提供實時可視化的示教依據，可以通過數字孿生模型與物理實體的交互提高示教效率，在提高機器人示教效率和示教安全性方面具有很大的潛力。
為提高遙操作機器人在線示教的編程效率和安全性，本文提出面向遙操作機器人在線示教的數字孿生框架，建立遙操作機器人及其工作場景數字孿生模型，實現了物理實體到數字孿生模型的映射，為後續的位姿規劃提供實時的可視化依據；設計基於RGB-D圖像和姿態示教器的遙示教方式，結合增強現實技術，通過滑鼠和姿態示教器實時規劃機器人末端執行器的路徑和姿態，實現了虛實融合示教，提高遙操作機器人的示教效率；構建機器人及其工作場景的八叉樹模型，預測規劃位姿下機器人和工作場景之間的碰撞，系統將未發生碰撞干涉的規劃路徑和姿態指令發送至物理機器人，控制物理機器人運動，提高遙操作機器人在線示教的安全性。
1遙操作機器人在線示教數字孿生體系架構
本研究使用數字孿生技術在線預測遙操作機器人按照示教路徑和姿態運動時是否存在碰撞干涉，並以此決定是否控制物理機器人運動。因此，本文提出了如圖1所示的基於數字孿生的遙操作機器人在線示教框架，該框架主要由物理單元和數字孿生單元兩部分組成。其中物理單元由物理機器人及工作場景、機器人控制器和RGB-D相機等組成，數字孿生單元包括機器人數字模型單元和遙操作機器人示教單元。物理單元利用RGB-D相機等傳感器獲取機器人及其工作場景的狀態信息，將傳感器獲取的實時數據發送至數字模型單元；數字模型單元對獲取的幾何數據、關節數據、RGB-D圖像數據以及位姿數據等進行分析處理，建立機器人及其工作場景的數字孿生模型。實時數據與孿生模型相結合，預測示教過程中可能存在的碰撞干涉。遙操作機器人示教單元將數據、孿生模型以及碰撞檢測結果等在上位機中實時呈現，為示教人員規劃機器人位姿提供可視化依據。示教人員實時監測示教平台規劃機器人的位姿，將規劃的位姿數據發送至數字模型單元進行預測驗證，無碰撞干涉時生成控制指令，通過ROS控制機器人運動。
本研究提出的基於數字孿生的遙操作機器人在線示教系統工作原理如下：數字模型單元根據物理機器人的尺寸參數建立機器人三維模型，根據當前姿態時物理機器人的位姿數據，利用機器人逆運動學求解，得出機器人各關節數據，使用關節數據驅動機器人三維模型運動；根據RGB-D相機拍攝工作場景的RGB-D圖像進行三維重建；通過八叉樹建模建立機器人及其工作場景的碰撞檢測模型；根據手眼標定獲得機器人基坐標系與相機坐標系的轉換矩陣，將虛擬機器人模型疊加到工作場景點雲圖像中，從而建立機器人及其工作場景的數字孿生模型。遙操作機器人示教單元使用增強現實註冊技術將虛擬機器人模型疊加到工作場景的RGB圖像中，實現虛實位置一致；示教人員利用滑鼠點擊工作場景的RGB圖像獲取機器人末端執行器運動路徑關鍵點的三維坐標，將姿態示教器姿態作為機器人末端執行器姿態的輸入，在虛實融合的示教環境中規劃機器人的路徑和姿態；將規劃的路徑和姿態數據發送至碰撞檢測模型，進行基於八叉樹模型的碰撞檢測，驗證示教人員規劃的機器人路徑和姿態的可行性，若無碰撞干涉，再將示教路徑和姿態指令傳輸至物理機器人，控制機器人完成在線示教操作。
與傳統遙操作機器人在線示教過程相比，本研究建立了機器人及其工作場景的數字孿生模型，可以實時反饋物理機器人的位姿數據以及工作場景的實時變化，實現了數字孿生模型對實體機器人及其工作場景的實時動作、行為和狀態的映射；基於八叉樹模型建立了機器人及其工作場景的碰撞檢測模型，可以實時監測機器人在示教過程中是否存在碰撞干涉。本研究通過數字孿生模型預測機器人是否與工作場景產生碰撞干涉，提高了示教工作的安全性；通過增強現實技術，在虛實融合的場景中交互規劃機器人的路徑和姿態，提高了在線示教的人機互動性和示教效率。
2機器人及工作場景數字孿生模型的構建
本研究使用ROS（Robot Operating System）建立遙操作機器人在線示教的數字孿生模型，實現物理單元與數字孿生單元之間數據信息的交互，主要用於預測遙操作機器人示教單元輸出的路徑和姿態是否存在碰撞干涉。基於對孿生模型數據信息分析和處理，向遙操作機器人在線示教提供智能化的支持與服務。
2.1 虛擬機器人模型的構建
建立與物理實體相一致的數字化虛擬模型是數字孿生建模的關鍵之一。本研究選用ABB_IRB120型號機器人，建立虛擬機器人模型，並添加虛擬機器人模型各關節坐標系。根據當前位姿下機器人的位姿數據，對機器人進行正逆運動學分析，計算機器人關節數據，使虛擬機器人模型與物理機器人的運動保持一致。建立的虛擬機器人模型如圖2所示。
2.2 機器人工作場景數字孿生模型的構建
為了構建機器人工作場景數字孿生模型，本文使用RGB-D相機採集機器人工作場景的深度圖像，ROS將深度圖像轉換成點雲圖像，實現三維重建。相機的深度值Z已知，根據式（1）所示的深度相機成像模型可以得到深度圖像坐標與點雲坐標的轉換矩陣[18]：
⎧⎩⎨⎪⎪X=(x−cx)ZfxY=(y−cy)Zfy(1)
其中，(X,Y,Z)為點雲的三維坐標，(x,y)為深度圖像坐標，fx、fy為深度相機在X軸和Y軸方向上的焦距，cx、cy為深度相機主點坐標。當相機在工作場景中移動時，ROS系統採集到不同的視角的工作場景深度圖像，通過採用圖像幀間的點雲配准算法，使用新的深度圖像視頻幀將前序深度圖像視頻幀中的空洞進行填充，最終融合拼接出整個工作場景的點雲圖像。
如圖3所示，由於相機獲得的點雲圖像視野範圍過大,會採集到與機器人工作空間無關的場景點雲圖像，導致點雲更新速率慢且點雲圖像邊緣過於粗糙，不利於後續的碰撞檢測。因此，本研究採用直通濾波、體素濾波和半徑濾波對點雲圖像進行處理，縮小點雲圖像的視野範圍，使點雲圖像的邊緣更加清晰。具體步驟如下：
（1）直通濾波處理：在相機坐標系各坐標軸上設置一個閾值範圍（0~1.3m），將閾值範圍之外的離群點去除，消除與機器人工作場景無關的點雲，縮小點雲圖像的視野範圍；
（2）體素濾波處理：將直通濾波處理後的點雲圖像均分成若干個大小為0.005m的三維體素網格，該體素網格內的所有點都通過體素網格的一個重心點來表示，實現點雲圖像下採樣，即體素濾波處理，使得採樣點對應的曲面更為準確，點雲邊緣更加清晰；
（3）半徑濾波處理：即以點雲圖像的某個點雲點為球心，在半徑為0.002m的球體內，若與球心相鄰的點雲點個數小於30個，則將該點雲點和所有相鄰的點雲都剔除；若大於或等於30個，則保留所有點雲點。以此進一步消除與機器人工作場景無關的離群點。
經過直通濾波、體素濾波和半徑濾波處理後的點雲圖像如圖4所示。
2.3 機器人手眼標定
為了將虛擬機器人模型與工作場景三維點雲模型融合，實現位置一致，需要對機器人和RGB-D相機進行手眼標定，得到相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣TCW。本研究將相機安裝在機器人外一固定位置，相機坐標系相對於機器人基坐標系固定不變，機器人末端執行器坐標系和標識卡坐標系隨機器人位姿的變化而變化，故採用Eye-To-Hand手眼標定方法[19]，標定示意圖如圖5所示。
由圖5可知，機器人基坐標系、末端執行器坐標系、標識卡坐標系和相機坐標系的轉換矩陣如下[20]：
(TCA)(i)=TCW⋅(TWM)(i)⋅TMA,i∈[1,n](2)
其中，TCA表示相機坐標系O-XCYCZC到標識卡坐標系O-XAYAZA的轉換矩陣，TCW表示相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣，TWM表示機器人基坐標系O-XWYWZW到機器人末端執行器坐標系O-XMYMZM的轉換矩陣,TMA表示機器人末端執行器坐標系O-XMYMZM到標識卡坐標系O-XAYAZA的轉換矩陣，n表示機器人位姿變化次數。將TMA消去得到：
(TCA)(i+1)⋅(TCA)−1(i)⋅TCW=TCW⋅(TWM)(i+1)⋅(TWM)−1(i),i∈[1,n−1](3)
令A=(TCA)(i+1)⋅(TCA)−1(i)，B=(TWM)(i+1)⋅(TWM)−1(i)，則式（3）可簡化為：
A⋅TCW=TCW⋅B(4)
其中，TCA可通過相機識別標識卡直接測得，TWM可通過機器人正運動學分析得到。故使用式（4）可求得相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW間的轉換矩陣TCW。根據轉換矩陣TCW，在虛擬工作場景中導入機器人三維模型，建立如圖6所示的機器人與其工作場景融合後的三維模型。上述機器人及其工作場景的數字孿生建模方法同樣適用於移動機器人，若將機器人和RGB-D相機安裝在移動平台上，只需保證操作過程中RGB-D相機與工業機器人之間的相對位置不變。
通過建立機器人及其工作場景的數字孿生模型，實現了物理實體和虛擬模型的實時動作、行為和狀態的同步映射，示教人員可以實時監測機器人的運行狀態以及工作場景的變化，為遙操作機器人示教提供可視化依據。
3碰撞檢測
為提高遙操作機器人在線示教的安全性，需要在物理機器人執行運動指令前先對規劃的路徑和姿態數據進行碰撞檢測。為此，本研究採用OBB (Oriented Bounding Box)碰撞檢測方法[21]，建立機器人及其工作場景的八叉樹模型，並進行碰撞檢測，提高遙操作機器人在線示教的安全性。建立碰撞檢測模型的具體步驟如下：
（1）如圖7（a）所示，將機器人及其工作場景的點雲圖像進行分割，建立八叉樹模型，並定義八叉樹模型的每個立方體空間的大小。如圖7（b）所示，首先將整個工作場景點雲圖像的立方體空間作為八叉樹模型的第一層（即根節點）；然後將整個點雲圖像平均分成8個立方體子空間，構成八叉樹模型第二層，每個立方體為第一層的子節點；將第二層的8個立方體分割成更小的8個立方體，形成第三層；依次類推，直至最後一層葉子節點，從而建立工作場景的八叉樹模型。八叉樹分割的層數越多，葉子層節點的立方體子空間越小，碰撞檢測精度越高；反之，八叉樹分割的層數越少，則葉子層節點的立方體子空間越大，碰撞檢測精度越低。
（2）為提高八叉樹模型的精度和更新速率，並將物理機器人的八叉樹模型從工作場景的八叉樹模型中剔除，需要修改ROS系統中點雲圖像轉換八叉樹模型的插件文件。首先設置八叉樹模型最大分割層次和最大更新頻率，在保證點雲圖像轉換八叉樹模型的速率的前提下，提高八叉樹模型精度；之後結合手眼標定得到的相機坐標系O-XCYCZC到機器人基坐標系O-XWYWZW的轉換矩陣TCW，將虛擬機器人模型置入工作場景八叉樹模型中；再將八叉樹模型填充抵消率和八叉樹模型填充比例設置為1，ROS系統可以將與虛擬機器人模型大小相同體積內的八叉樹模型剔除，避免將物理機器人誤判成障礙物進行碰撞檢測。根據如圖8（a）所示的真實工作場景，將虛擬機器人模型與工作場景八叉樹模型融合生成如圖8（b）所示的碰撞檢測模型。
建立機器人及其工作場景的碰撞檢測模型後即可進行兩者之間的碰撞檢測，具體步驟如下：示教人員使用滑鼠點擊工作場景RGB圖像確定虛擬機器人路徑點，使用姿態示教器規劃虛擬機器人末端執行器的姿態；遙操作機器人在線示教單元將規劃好的示教路徑與姿態數據發送至碰撞檢測模型，更新虛擬機器人模型；RGB-D相機拍攝工作場景實時的深度圖像，對工作場景進行八叉樹建模；使用OBB碰撞檢測算法對虛擬機器人模型和工作場景的八叉樹模型進行碰撞檢測，檢測示教路徑與姿態下虛擬機器人是否與周圍環境碰撞。如圖9所示，若虛擬機器人某關節與工作場景有碰撞干涉的情況出現，則將該關節標紅，並在示教平台輸出報警信息，同時控制物理機器人停止運動；若不產生碰撞則將示教路徑和姿態數據發送至機器人控制器，控制機器人運動。本碰撞檢測方法在機器人執行示教任務的過程中也可進行實時碰撞檢測。
4基於RGB-D圖像和姿態示教裝置的遙操作機器人在線示教
為了提高遙操作機器人的編程效率和人機互動性，本團隊提出了一種基於RGB-D圖像和姿態示教裝置的機器人示教方法[22]，該方法通過RGB-D相機獲取機器人工作場景實時的RGB圖像和深度圖像，通過增強現實註冊將虛擬機器人模型疊加到機器人工作場景的RGB圖像中，示教人員使用滑鼠點擊選取工作場景的RGB圖像路徑點，使用姿態示教器規劃虛擬機器人末端執行器的姿態，將規劃的路徑與姿態數據保存並發送至物理機器人，控制其運動，實現機器人的路徑和姿態示教。該方法雖然提供了一種高效便捷的機器人示教方式，但是規劃的路徑與姿態數據未經驗證就發送至物理機器人，使得物理機器人的示教過程存在安全隱患。因此本研究在遙操作機器人數字孿生模型的基礎上，採用基於RGB-D圖像和姿態示教裝置的示教方法，將規劃的路徑與姿態數據先發送至ROS系統中的機器人碰撞檢測模型進行碰撞檢測，無碰撞發生時再將運動指令發送至物理機器人完成遙示教編程，降低遙操作機器人在線示教的安全隱患，具體示教流程如圖10所示。
（1）在進行機器人位姿規劃前，RGB-D相機拍攝工作場景實時RGB圖像，根據手眼標定得到的相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣TCW轉換，將虛擬機器人模型疊加到真實工作場景中，使虛擬機器人基坐標系和物理機器人基坐標系重合，實現增強現實註冊。
（2）如圖11（a）和（b）所示，示教人員使用滑鼠點擊選取工作場景的RGB圖像確定遙操作機器人在線示教的路徑點，路徑點會同時顯示在RGB圖像和深度圖像中。如圖12所示，根據RGB圖像與深度圖像之間的映射關係以及相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣，來計算示教路徑點在物理機器人基坐標系O-XWYWZW下的三維坐標P(xw,yw,zw)，具體步驟如下：
①已知圖像上路徑點P在RGB圖像中的像素坐標為p(uc,vc)，深度相機的深度值為d，根據RGB圖像坐標系O-UcVc到深度圖像坐標系O-UdVd的轉換矩陣可以得到路徑點P在深度圖像坐標系O-UdVd中的像素坐標p(ud,vd,d);
②根據深度相機的內部參數矩陣Mt和深度相機的焦距f，可以得到深度圖像中任意一點p(ud,vd,d)，在深度相機坐標系O-XdYdZd中的三維坐標(xd,yd,zd)：
③根據深度相機坐標系O-XdYdZd到RGB相機坐標系O-XcYcZc的轉換矩陣，計算得到路徑點P在RGB相機坐標系中的三維坐標系(xc,yc,zc)；
④根據手眼標定得到的RGB相機坐標系O-XcYcZc到物理機器人基坐標系O-XWYWZW的轉換矩陣TCW，計算到的路徑點P在物理機器人基坐標系O-XWYWZW中的三維坐標P(xw,yw,zw)。
計算出路經點在物理機器人基坐標系O-XWYWZW中的三維坐標P(xw,yw,zw)後，經機器人逆運動學模型求解，得到虛擬機器人各關節的旋轉角度，驅動虛擬機器人末端執行器以默認的姿態移動至相應的路徑點，如圖11（c）所示。
（3）路徑規劃結束後，使虛擬機器人模型的末端執行器跟隨路徑點運動，在運動的過程中，示教人員使用姿態示教器對虛擬機器人末端執行器的姿態進行規劃。如圖13（a）所示，姿態示教器採用慣性傳感器模塊在地理坐標系中輸出姿態數據，並通過藍牙模塊將姿態數據傳輸至計算機。在姿態示教過程中，示教人員通過調節姿態示教器的姿態來規劃虛擬機器人末端執行的姿態，並通過AR可視化介面實時觀察，以達到預期姿態，如圖13（b）所示。
（4）在進行位置和姿態規劃的同時，實時將規劃的位姿數據發送至ROS系統，更新機器人碰撞檢測模型，通過機器人逆運動學計算虛擬機器人各關節的運動角度，控制虛擬機器人模型運動。RGB-D相機採集工作場景實時深度圖像，生成點雲圖像並進行八叉樹建模。虛擬機器人模型與工作場景的八叉樹模型進行碰撞檢測，驗證位姿規劃的合理性。若無碰撞發生，則將生成機器人運動指令發送至機器人控制器，控制物理機器人運動。本研究在控制物理機器人運動前，先預測機器人與工作場景之是否存在碰撞干涉，降低了碰撞的風險，提高了遙操作機器人在線示教的安全性。
5實驗驗證與分析
為驗證基於數字孿生的遙操作機器人在線示教方法的可行性和安全性，本文設計了如圖14所示的實驗系統。實驗系統由本地端和遠程端組成，其中本地端包括機器人、機器人控制器、機器人工作檯和RGB-D相機等；遠程端包括示教人員、兩台計算機和姿態示教器等。示教人員在1號計算機（CPU：i7-9700，RAM：16GB，顯卡：RTX2060，系統：Ubuntu16.04，ROS：kinetic）中運行ROS系統，進行數字孿生建模，建立機器人及其工作場景的數字孿生模型；在2號計算機（CPU：i5-9300，RAM：16GB，顯卡：GTX1650，系統：Windows 10）中基於VS2013開發環境，搭建AR示教環境，採用滑鼠和姿態示教器進行機器人的位姿規劃，實現虛實融合示教。
具體實驗流程如下：
（1）在進行遙操作機器人在線示教前，首先需要對機器人及其工作場景進行數字孿生建模。示教人員在1號計算機對物理機器人進行手眼標定，計算RGB-D相機在機器人基坐標系中的位姿；根據物理機器人的幾何和關節數據以及RGB-D相機採集的工作場景的RGB-D圖像，建立機器人及其工作場景數字孿生模型，之後基於RGB-D相機坐標繫到機器人基坐標系的轉換矩陣，將機器人數字孿生模型和工作場景數字孿生模型融合，如圖15（a）所示；最後根據工作場景的深度圖像建立工作場景的八叉樹模型，基於RGB-D相機的位姿，將機器人數字孿生模型與工作場景八叉樹模型融合，建立如圖15（b）所示的碰撞檢測模型。
（2）示教人員在2號計算機根據手眼標定得到相機位姿，將虛擬機器人模型疊加到工作場景的RGB圖像中，搭建如圖16（a）所示的AR示教環境；其中路徑規劃窗口分為RGB圖像窗口和深度圖像窗口，如圖16（b）和（c）所示，深度圖像中藍色區域表示距離，顏色越深，距離越近。
（3）如圖17（a）和（b）所示，示教人員使用滑鼠點擊工作場景的RGB圖像，獲取機器人末端執行器的路徑點，點擊的路徑點會顯示在工作場景的RGB圖像和深度圖像上。示教系統根據圖12所示的轉換矩陣計算路徑點在機器人基坐標系的三維坐標，驅動虛擬機器人以默認的姿態運動至該路徑點，如圖17（c）所示。
（4）路徑規劃結束後，示教人員使用姿態示教器規劃虛擬機器人末端執行器在各路徑點的姿態。根據機器人當前的位姿數據，通過機器人逆運動學模型計算機器人各關節轉動的角度，驅動虛擬機器人模型運動。如圖18所示，在姿態規划過程中，虛擬機器人模型的姿態實時的顯示在AR示教窗口中，示教人員根據AR示教窗口調節虛擬機器人模型末端執行器的姿態。
（5）在進行機器人位姿規劃的同時，2號計算機將規劃的位姿數據實時發送至1號計算機進行碰撞檢測。如表1中碰撞檢測窗口中的虛擬運動軌跡所示，當機器人運動路徑上沒有碰撞干涉時，ROS將位姿數據發送至機器人控制器，控制物理機器人按規劃的姿態移動至路徑點，完成示教操作，如表1中機器人實際運動軌跡所示。
為驗證示教精度，本文以矩形軌跡示教為例，將規劃的路徑數據保存，使用最小二乘法將所有路徑點擬合為矩形，然後計算示教路徑點到矩形輪廓的最小距離，作為軌跡誤差，該誤差包括人的示教誤差、標定算法誤差、相機成像誤差等。實驗結果表明基於RGB-D圖像和姿態示教裝置的遙操作機器人示教方法的平均軌跡誤差為0.5050 mm, 最大誤差為 3.3261 mm[22]。因此示教軌跡精度可以滿足機器人噴塗、打磨等工作要求。
根據實驗結果可知，基於數字孿生的遙操作機器人在線示教方式可以實時地傳遞數據，可以準確地控制虛擬機器人的位姿，可以有效地檢測機器人的運動狀態，增強了機器人對場景的感知能力，提高了在線示教的人機互動性。此外，本研究成果還可應用於機器人實時碰撞檢測。在步驟（5）生成的機器人運動軌跡上設置障礙物，將步驟（5）生成路徑和姿態數據再次發送至ROS系統進行碰撞檢測。RGB-D相機實時拍攝工作場景，ROS系統根據障礙物的深度圖像構建障礙物的八叉樹模型。碰撞檢測模型識別到障礙物，ROS系統對虛擬機器人的運動路徑進行重新規劃，如表2中虛擬運動軌跡所示。碰撞檢測窗口中的虛擬機器人的運動路徑由於存在碰撞干涉而發生了改變，但依然以規劃的姿態達到了目標路徑點。ROS系統將碰撞檢測後重新規劃位姿數據發送至機器人控制器，機器人的運動軌跡如表2中實際運動軌跡所示。
綜上可以看出，本文設計的基於數字孿生的遙操作機器人在線示教系統無需人工建立遙操作機器人所在環境的3D模型，通過滑鼠和姿態示教器規劃機器人末端執行器的位置和姿態，採用AR技術實現了示教結果的實時可視化，並且可以實時檢驗示教位置和姿態是否可行，可以提高遙操作機器人示教的安全性和示教效率，示教精度滿足機器人噴塗、打磨等應用。本文的研究成果對遙操作機器人在線示教和控制具有一定的參考價值。
6 結束語
為提高遙操作機器人在線示教的編程效率和安全性，本文設計了基於數字孿生的遙操作機器人在線示教系統。通過建立機器人及其工作場景的數字孿生模型，實現了虛擬模型和物理實體的實時映射；採用基於RGB-D圖像和姿態示教器的示教方式，實現了在虛實結合的場景中進行機器人路徑和姿態規劃；構建了機器人及其工作場景的八叉樹模型，可提前預測示教路徑和姿態下機器人和工作場景之間是否存在碰撞干涉，提高了遙操作機器人在線示教的可靠性、安全性和示教效率，為遙操作機器人在線示教提供了新思路。
參考文獻
[1] ONG S, THANIGAIVEL N, NEE A,et al. Augmented reality-assisted robot programming system for industrial applications, Robot. Comput. Integr. Manuf. 2020,61:101820.
[2] SHERIDANT B. Tele-robotics, automation, and human supervisory control[M]. Massachusetts: the MIT Press, 1992.
[3] WANG L, MOHAMMED A, ONORI M. Remote robotic assembly guided by 3d models linking to a real robot. CIRP Ann-Manuf Technol 2014,63(1):1–4.
[4] LIU H, WANG L. Remote human–robot collaboration: A cyber–physical system application for hazard manufacturing environment, J. Manuf. Syst. 2020,54:24–34.
[5] NIKOLAKIS N, MARATOS V, MAKRIS S. A cyber physical system (CPS) approach for safe human-robot collaboration in a shared workplace. Robot. Comput. Integr. Manuf. 56(2019)233–243.
[6] SUNG H, PARK K, DONG H R,et al. An integrated mixed reality system for safety-aware human-robot collaboration using deep learning and digital twin generation. Robot. Comput. Integr. Manuf.2022,73:102258.
[7] SCHLEICH B, ANWER N, MATHIEU L,et al. Shaping the Digital Twin for Design and Production Engineering. CIRP Annals—Manufacturing Technology 66:141–144.
[8] TAO Fei, ZHANG Meng, CHENG Jiangfeng,et al. Digital twin workshop:a new paradigm for future workshop[J].Computer Integrated Manufacturing Systems,2017,23(01):1-9. [陶飛,張萌,程江峰,等.數字孿生車間——一種未來車間運行新模式[J].計算機集成製造系統,2017,23(01):1-9.]
[9] ELISA N, LUCA F, MARCO M. A review of the roles of Digital twin in CPS-based production system[J]. Procedia Manufacturing,2017,11:939-948.
[10] TAO F,ZHANG M. Digital Twin Shop-Floor: A New Shop-Floor Paradigm Towards Smart Manufacturing[J].IEEE Access, 2017,5:20418-20427.
[11] TAO Fei, LIU Weiran, ZHANG Meng,et al. Five-dimension digital twin model and its ten applications[J]. Computer Integrated Manufacturing Systems, 2019,25(01):1-18. [陶飛,劉蔚然,張萌,等.數字孿生五維模型及十大領域應用[J].計算機集成製造系統,2019,25(01):1-18.]
[12] WU Pengxing, GUO Yu, HUANG Shaohua,et al. Visual real-time monitoring method for discrete manufacturing workshop based on digital twin[J]. Computer Integrated Manufacturing Systems, 2021,27(06):1605-1616. [吳鵬興,郭宇,黃少華,等.基於數字孿生的離散製造車間可視化實時監控方法[J].計算機集成製造系統,2021,27(06):1605-1616.]
[13] T地址ARY B, ERDS G. Generic development methodology for flexible robotic pick-and-place workcells based on Digital Twin[J]. Robot. Comput. Integr. Manuf. 2021,71:102140.
[14] BURGHARDT, SZYBICKI D, GIERLAKP,et al. Programming of Industrial Robots Using Virtual Reality and Digital Twins[J]. Applied sciences, 2020,10(2):486.
[15] YANG Yanfang, HE Huan,SHU Liang,et al.Digital twin robot and its motion control for flexible assembly of circuit breake[J/OL]. Computer Integrated Manufacturing Systems.2020, 26(11):2915-2926. DOI:10.13196/j.cims. 2020.11.002.[楊艷芳, 賀煥, 舒亮, 等. 斷路器柔性裝配數字孿生機器人及其運動控制[J]. 計算機集成製造系統, 2020,26(11):2915-2926. DOI:10.13196/j.cims. 2020.11.002.]
[16] LV Yongjun, LIU Feng, ZHENG Liaomo,et al. Application Analysis of Generalized and Modified D-H Method in Kinematic Modeling[J]. Computer system application,2016,25(05):197-202. [呂永軍,劉峰,鄭飂默,等.通用和修正D-H法在運動學建模中的應用分析[J].計算機系統應用,2016,25(05):197-202.]
[17] ZHU Heng, YANG Dongchao, CHANG Xu,et al. Calibration of Joint Parameters of Mainpulator Based on Partiale Swarm Optimization Method[J]. Machinery Design ＆ Manufacture, 2021(07):285-290. [朱衡,楊東超,常旭,等.利用粒子群優化方法標定機械臂關節參數[J].機械設計與製造,2021(07):285-290.]
[18] BETHENCOURT A, JAULIN L. 3D reconstruction using interval methods on the Kinect device coupled with an IMU [J].International Journal of Advanced Robotic Systems, 2013,10(2):1-10．
[19] STROBL K, HIRZINGERi G. Optimal Hand-Eye Calibration[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2007.
[20] CHEN Baocun,WU Wei, GUO Yu,et al. Automatic Hand – Eye Calibration System of Robot based on ROS[J]. Computer Simulation,2020,37(02):343-348. [陳寶存,吳巍,郭毓,等.基於ROS的機器人自動手眼標定系統設計[J].計算機仿真,2020,37(02):343-348.]
[21] ZHOU Junwei, WAN Yu, WAN Wanggen,et al.A Method of Collision Detection with OBB based on Octree[J]. Computer Applications and Software,2009,26(04):75-77. [周俊瑋,萬宇,萬旺根,等.一種基於八叉樹的OBB包圍盒碰撞檢測方法[J].計算機應用與軟體,2009,26(04):75-77.]
[22] PAN Y, Chen C, Li D,et al. Augmented reality-based robot teleoperation system using RGB-D imaging and attitude teaching device. Robot. Comput. Integr. Manuf. 2021,71:102167.
收藏