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

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

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

1 引言

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

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

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

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

2.1 系統(tǒng)集成

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

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

2.2 VR環(huán)境建模

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

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

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

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

圖3 UR10機(jī)械臂連桿坐標(biāo)系

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

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

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

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

2.4 主從端通信

2.4.1 機(jī)械臂通信

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

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

void Update()

{

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

qd=URobj.GetComponent().qd1;

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

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

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

}

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

2.4.2 工具端通信

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

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

2.5 遙操作實(shí)現(xiàn)

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

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

2.5.1 鍵盤控制

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

2.5.2 手柄控制

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

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

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

2.5.3 Leap Motion手勢(shì)控制

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

palmpos=palm.transform.position;

palmrot=palm.transform.rotation;

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

此外,通過(guò)Leap Motion還可用手勢(shì)識(shí)別來(lái)定義多種控制功能,因?yàn)槌终仆?Leap Motion會(huì)返回每個(gè)手指和關(guān)節(jié)的參數(shù),通過(guò)處理這些手指和關(guān)節(jié)的數(shù)據(jù),可以定義一些簡(jiǎn)單的手勢(shì),如握拳和張開手掌分別對(duì)應(yīng)電爪的張開和夾緊。

2.5.4 碰撞檢測(cè)

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

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

3 結(jié)果

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

圖5 虛實(shí)機(jī)械臂的位姿對(duì)比

(2)

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

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

將機(jī)械臂移動(dòng)到約200個(gè)不同的工作點(diǎn),對(duì)使用標(biāo)準(zhǔn)參數(shù)和刻度參數(shù)建模的位姿同步精度進(jìn)行分析,誤差的分布見圖6,誤差取絕對(duì)值后的數(shù)據(jù)見表2。

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

圖6 Unity3D模型與UR10的TCP誤差

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

4 結(jié)語(yǔ)

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

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

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