基于虛擬現實的探測器布局遙操實驗裝置

陳子晗,劉 儀,孫 平,鄧 瑋

(1. 四川大學原子核科學技術研究所,四川 成都610064;2. 核工業西南物理研究院,四川 成都 610041)

1 引言

核聚變裝置托卡馬克內部的核輻射環境產生的射線會對人產生健康危害,例如放電時等離子體會產生軟X射線。測量軟X射線的空間分布和輻射能譜,能獲得重要的等離子體信息,如電子溫度、電子速率分布、重金屬雜質含量等信息[1],為此需要在托卡馬克的真空室布置多個探測器。遠程操作機械臂定期對探測器進行更換并改變空間布局,需要有極高的工作精度,以及對現場情況真實、快速的反饋[2]。將虛擬現實(Virtual Reality,VR)技術應用于遙操,可提供任意視角的畫面,實現沉浸式操作,有效地提高遠程操作的效率和安全性[3]。除了進行真實的遙操作,VR還可應用于仿真培訓[4]。

基于VR技術的核環境機械臂遙操在國外已有研究,JET(Joint European Torus)使用VR技術輔助機械臂對托卡馬克裝置內部進行遠程維護[5],有兩種工作模式,在線模式下,遠程設備連接到VR系統,并為VR系統提供遠程環境的實時3D顯示;離線模式下,不連接遠程設備,只操作虛擬模型,用于制定遙操策略和檢查操作可行性。ITER(International Thermonuclear Experimental Reactor)同樣使用VR技術研究、改進和驗證停堆維護操作[6]。國內的EAST(Experimental Advanced Superconducting Tokamak)應用VR生成虛擬組件安裝方案,為實際安裝過程提供參考[7],并建立了一個全面的實驗和培訓環境[8]。VR與機器人結合的技術將在核輻射環境應用中發揮重要的作用[9]。

為研究遙操機械臂進行探測器的布局,在無輻射的實驗室搭建了一臺協作式機械臂及操作臺,操作臺設計有不同角度的工作平面,可在水平、垂直或斜坡面進行操作。應用VR技術,遙操機械臂抓取、搬運、安裝位于不同平面的探測器。目的是快速建立機械臂遙操作原型裝置,有助于發現問題,驗證功能及方案,為將來本裝置實際應用于托卡馬克上打下基礎。主要工作包括:系統集成,虛擬模型建立,從端功能實現,主從端網絡通信實現,遙操作功能實現。

2 設計及實現

2.1 系統集成

機械臂遙操測試平臺由主端和從端設備組成,如圖1所示。從端的設備都集成到UR10機械臂的控制柜中,而控制柜通過TCP/IP與主端實現遠程連接,末端執行器選取了DH Robotics AG-95自適應電爪和DP-DXL-001自動螺絲鎖付機。主端設備提供視覺反饋和人機交互功能,包括上位機(遙操作的核心),Oculus Rift虛擬現實設備(包括1個頭戴顯示器,2個Touch手柄,2個傳感器),以及用于實現手勢操作的Leap Motion控制器。

圖1 主從端設備及其通信連接方式

2.2 VR環境建模

建立一個與真實工作環境一致的VR環境,需要較高的建模精度保證虛實環境的一致。首先使用SolidWorks軟件根據CAD圖紙繪制與實物1:1的模型,UR10的模型可從UR機器人官網上下載,其次使用3ds MAX對模型進行優化處理,去除模型不必要的特征,如螺釘、墊片等,可以提升VR系統運行的流暢度。將模型轉為.FBX格式后導入Unity3D,按照真實環境精確擺放各個物體的相對位置,如圖2所示。方形和圓形工件分別表示兩種不同外形的探測器,需要將1～6號抓取點上的探測器搬運至7～12號安裝點,并擰螺絲安裝。圖2中編號13、14、15和16分別為UR10機械臂、電爪、螺絲鎖付機和螺絲供料機。

圖2 建模完成的VR環境

UR10機械臂有基座、肩部、肘部、手腕1～3共六個關節。參照UR10的關節結構,在Unity3D的物體列表中,將UR10機械臂模型中的所有子關節順序設為層級關系,從頂層到底層依次為UR10→Base→Shoulder→Elbow→Wrist1→Wrist2→Wrist3,這樣可實現子關節跟隨父關節的連桿整體傳動。UR10機械臂的運動學也是用Denavit-Hartenberg(DH)參數來設置,見表1,建立的連桿坐標系見圖3,則表1中的關節角θn是繞zn-1軸,從xn-1到xn軸的旋轉角度,初始值為0;連桿長度an是沿xn軸,從zn-1到zn軸之間的距離;連桿偏距dn是沿zn-1軸,從xn-1到xn軸的長度;連桿扭轉角αn是繞xn軸,從zn-1到zn軸之間的夾角,夾角定義采用右手法則。

表1 UR10機械臂DH參數

圖3 UR10機械臂連桿坐標系

由表1和圖3,在Unity3D中設置虛擬機械臂各關節的坐標系原點和坐標軸方向,對模型進行標定。標定完成后,將真實機械臂關節角θ1～θ6賦值給虛擬機械臂對應關節,可實現虛擬機械臂和真實機械臂的位姿同步,此時Unity3D可計算得到模型TCP相對于基座的坐標值。

當完成某一探測器的抓取后,通過函數transform.SetParent()設置該探測器模型的父層級為電爪,而電爪是與機械臂TCP綁定的,這樣探測器就會跟隨機械臂TCP一同移動;當放置探測器后,設置該探測器模型的父層級為操作平臺,此時工件將不再跟隨機械臂移動。

2.3 從端功能實現

在從端的機械臂示教器上用URScript編寫urp程序,將遙操機械臂必須經過的抓取、放置及擰螺絲的工作點記錄在程序中,并將重復固定的動作寫入子程序,比如“下降-抓取-上升”、“移動-取螺釘-回位”或“下降-擰螺釘-上升”等頻繁使用的動作。在操作員遙操機械臂移動到某工作點,將觸發Unity3D的碰撞檢測事件,隨后可調用該工作點關聯的子程序,精確地移動至工作點并自動抓取,可以減小遙操的難度和操作效率。

2.4 主從端通信

2.4.1 機械臂通信

UR10提供了URSDK(.Net 4.0)開發包,本工作采用了DashBoard和Realtime兩個接口,網絡端口分別是29999和30003。其中,使用Dashboard接口可發送命令字符串給機械臂,執行預先編好的urp程序,并接收返回結果字符串。Realtime接口以125Hz的頻率獲取機械臂的詳細狀態信息,如電壓、電流、速度、加速度、關節角、TCP坐標及旋轉矢量等參數,此外還可以發送movej,movel等腳本命令,控制機械臂的運行。URSDK命名空間Communication下的類RTClientObj包含了機械臂運行時的主要參數,可使用類RTClient的getRTClientObj()函數得到該RTClientObj對象,實時獲取UR10機械臂各關節的參數,賦值給VR環境中的機械臂模型。

在Unity3D虛擬模型中,根據收到的真實關節角,計算出角速度,再修改每個關節的Update()函數,轉動虛擬機械臂,實現虛實機械臂的位姿同步,Base關節的Update()函數示例如下:

void Update()

{

double qd; ∥關節角速度

qd=URobj.GetComponent().qd1;

∥從UR10對象中得到關節1的角速度

transform.Rotate(0,0,(float)qd*Time.deltaTime, Space.Self);

∥轉動模型的Base關節

}

由于Update()函數會在虛擬界面刷新的每幀調用,在上位機中就可看到機械臂的同步運動。

2.4.2 工具端通信

工具端通信包括電爪通信和螺絲鎖付機通信。電爪通信協議為CAN2.0A,配備通信轉接盒后,可支持TCP/IP,設置獨立的IP地址。在Unity3D中,使用System.Net.Sockets類下面的TcpClient的函數Client.Send(),根據電爪的通信協議發送初始化電爪和控制電爪開度的命令。

螺絲鎖付機有兩種控制方式,IO方式通過UR10控制柜的IO輸出端口與螺絲鎖付機相連,控制螺絲機的正轉和反轉,以及螺桿的伸縮,RS485模式使上位機可以通過數字信號遠程控制螺絲機,采用的協議是ModbusRTU。使用Unity3D的System.IO.Ports的SerialPort類,根據螺絲鎖付機的通信協議發送切換控制方式、正轉、反轉和自由驅動等命令。

2.5 遙操作實現

在實現主從端通信的基礎上,實現了通過鍵盤、手柄和手勢識別來遠程控制機械臂,遙操機械臂的流程如圖4所示。

圖4 基于VR的機械臂遙操實現流程

2.5.1 鍵盤控制

UR機器人采用6個元素的向量來表示TCP位姿,如p=[x,y,z,Rx,Ry,Rz],分別表示位置和旋轉矢量。參照示教器的控制方式,控制TCP位姿需要12個按鍵,其中6個控制TCP的位置,分別是x,y,z坐標的增加和減少;6個控制TCP的旋轉矢量,分別是Rx,Ry,Rz的增加和減少。此外,還需要12個按鍵增減6個關節的角度,因此共需要24個按鍵來操控機械臂。在計算機鍵盤上定義24個鍵,Unity3D中,用Input.GetKey()函數檢測到按鍵后,得到相應的增量,分別用getRTClientObj().actual＿q和getRTClientObj().actual＿TCP＿pose函數獲取當前關節角和當前位姿,加上增量后,得到目標關節角或位姿,生成并發送movej或movel命令,分別控制機械臂轉動關節或改變TCP位姿。當檢測到表示按鍵松開的GetKeyUp()消息,立即發送停止機械臂移動的命令。

2.5.2 手柄控制

手柄控制包括手柄按鍵與手柄位姿控制。手柄按鍵控制與鍵盤控制同理,共定義左右兩個手柄的22個按鍵狀態,需要把按鍵組合定義才夠用。通過OVRInput.Get()函數,檢測手柄按鍵的輸入狀態,當按住按鍵時令機械臂移動,當松開按鍵時令機械臂停止。手柄具有振動功能,通過函數OVRInput.SetControllerVibration()設置,可以給操作員帶來反饋和提醒。

傳感器可跟蹤得到手柄的位姿,以左手手柄為例,在Unity3D中通過函數GameObject.Find("hand＿left")找到左手手柄對象,得到該對象transform組件中的position和rotation,它們會隨手柄的移動和旋轉而變化,計算一定時間間隔內position的增量Δp和rotation的增量Δr,再把手柄的位姿增量轉換為機械臂的位姿增量。轉換中要注意,Unity3D采用左手坐標系,而UR10采用右手坐標系;Unity3D中的rotation是用四元數Quaternion保存的,而UR10中用的是旋轉矢量。此外,位姿增量轉換時也可以根據需要設定一定的比例系數,不一定按1:1的比例映射。

轉換后的位姿增量與當前機械臂位姿相加,即可得到目標位姿,UR機器人位姿相加要使用RobMath.pose＿trans()函數。這樣,就可以把手柄的位姿變化轉換為機械臂的位姿變化,從而通過手柄位姿控制機械臂的TCP位姿。

2.5.3 Leap Motion手勢控制

基于Leap Motion的人手位姿控制的原理與手柄位姿控制相似,圖像傳感器以120Hz的頻率,捕獲人手的位姿變化,識別出數據后,傳遞給Unity3D中的人手骨骼模型,取手掌的位姿作為計算的依據:

palmpos=palm.transform.position;

palmrot=palm.transform.rotation;

在程序中跟蹤palm位姿的變化,計算一定時間間隔內的位姿增量,與手柄位姿控制類似,也可實現手勢控制機械臂TCP位姿。

此外,通過Leap Motion還可用手勢識別來定義多種控制功能,因為除手掌外,Leap Motion會返回每個手指和關節的參數,通過處理這些手指和關節的數據,可以定義一些簡單的手勢,如握拳和張開手掌分別對應電爪的張開和夾緊。

2.5.4 碰撞檢測

在機械臂的遙操中,若操作員隨意控制機械臂,有可能會產生物理碰撞,帶來機械臂和設備的損傷。為解決此問題,在Unity3D中設置了碰撞檢測功能。對所有在操作過程中可能會產生碰撞和需要進行人機交互的物體模型添加碰撞體,賦予其碰撞檢測功能,即圖2里模型外部的綠色方框。碰撞體尺寸大于真實物體尺寸,即當虛擬環境中檢測到碰撞,在物理碰撞發生前,系統會收到碰撞消息,如OnTriggerEnter()、OnTriggerStay()和OnTriggerLift()函數,分別對應夾具進入、停留及離開某一探測器碰撞檢測范圍的三個過程。在程序中可通過文字或手柄振動的方式,提醒操作者注意,并拒絕執行會發生物理碰撞的操作。圖2中物體模型外部的綠色方框即為設置的碰撞檢測范圍,用于實現碰撞檢測并在遙操作中避免機械臂和工件發生碰撞。

此外,碰撞檢測還能識別到工作點的編號,而工作點號和具體的碰撞檢測函數和機器人示教器編寫的程序是綁定的。當觸發工作點的碰撞檢測功能時,使用類DashBoard下的函數loadProgram()、play()和stop()分別控制示教器加載、運行和停止對應點的urp程序,實現與工作點編號對應的抓取、放置和擰螺絲的任務。當機械臂尚未進入碰撞檢測范圍時,表明機械臂當前位置與目標工作點相距較遠,將無法加載示教器的程序。

3 結果

圖5是虛擬機械臂和真實機械臂的對比,虛擬機械臂除了省略一些螺釘、線纜、固線器等配件,和真實機械臂的位姿基本一致。戴上頭盔后,可以通過頭部的移動轉換視角,近距離觀察觀察內部結構或剖面,便于掌握復雜裝置的結構,充分發揮虛擬現實的優勢,這在托卡馬克環境中是非常有用的。

圖5 虛實機械臂的位姿對比

(2)

DH參數中的an、dn和αn是常量,將關節角變量θn代入式(2)中能得到TCP坐標的理論值[10]。理論值與Unity3D數值接近,與UR10的讀值仍然差別較大。

在示教器的系統文件夾中找到UR10的刻度文件Calibration.conf,該文件給出了UR10的DH偏差,把刻度后的DH參數代入式(2)計算后,與UR10讀值的誤差大大減小,說明UR10讀值采用了刻度參數,Unity3D建模也應采用刻度后的DH參數。

將機械臂移動到約200個不同的工作點,對使用標準參數和刻度參數建模的位姿同步精度進行分析,誤差的分布見圖6,誤差取絕對值后的數據見表2。

表2 Unity3D模型與UR10的TCP誤差統計

圖6 Unity3D模型與UR10的TCP誤差

使用刻度后的DH參數后,虛實機械臂的誤差大大減小,位姿同步的精度得到提高,最大誤差降到0.4mm,絕對值平均為0.2mm左右。仍有的誤差主要來源于角度采樣時的波動(<0.1°),以及Unity3D中,為實現動畫采用的計算轉換中,會帶來一定的誤差。

4 結語

本研究基于Unity3D,實現了鍵盤、手柄、手勢識別等方式的機械臂遙操作。Unity3D提供了較完善的軟硬件集成方式,大大節省了開發時間。在各種遙操方式中,按鍵命令清晰明確,但機械臂遙操按鍵過多,不符合人的自然操作習慣,且機械臂運行顯得緩慢而笨拙。使用手柄姿態或手勢動作的方式控制機械臂,省去了繁多的鍵位設置,多維度的旋轉和平移可同時進行,操作方式更為直觀,但也有操作范圍小、命令不明確、控制準確度低等問題。

采用主端遙操作與從端固定子程序結合的方式,能降低操作精度和難度,并仍具有一定的靈活性,組合后可完成更復雜的操作。由于虛擬現實需要對真實工作環境進行建模,因此只適用于工作環境是已知且固定的場景中,如果工作環境在操作過程中發生變化,則需要結合增強現實進行識別和調整。

結果表明,使用Unity3D開發虛擬現實應用,能較為精確地遙操機械臂,在水平、垂直或斜坡面上布置探測器的基本不會出現失誤。本研究方法積累可以應用于包括核環境在內的危險場景遙操作中,下一步將實際應用于托卡馬克裝置的遙操作上。