基于虛擬現(xiàn)實的探測器布局遙操實驗裝置

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

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

1 引言

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

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

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

2 設(shè)計及實現(xiàn)

2.1 系統(tǒng)集成

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

圖1 主從端設(shè)備及其通信連接方式

2.2 VR環(huán)境建模

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

圖2 建模完成的VR環(huán)境

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

表1 UR10機械臂DH參數(shù)

圖3 UR10機械臂連桿坐標系

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

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

2.3 從端功能實現(xiàn)

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

2.4 主從端通信

2.4.1 機械臂通信

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

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

void Update()

{

double qd; ∥關(guān)節(jié)角速度

qd=URobj.GetComponent().qd1;

∥從UR10對象中得到關(guān)節(jié)1的角速度

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

∥轉(zhuǎn)動模型的Base關(guān)節(jié)

}

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

2.4.2 工具端通信

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

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

2.5 遙操作實現(xiàn)

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

圖4 基于VR的機械臂遙操實現(xiàn)流程

2.5.1 鍵盤控制

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

2.5.2 手柄控制

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

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

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

2.5.3 Leap Motion手勢控制

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

palmpos=palm.transform.position;

palmrot=palm.transform.rotation;

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

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

2.5.4 碰撞檢測

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

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

3 結(jié)果

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

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

(2)

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

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

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

表2 Unity3D模型與UR10的TCP誤差統(tǒng)計

圖6 Unity3D模型與UR10的TCP誤差

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

4 結(jié)語

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

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

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