基于虚拟现实的探测器布局遥操实验装置

陈子晗,刘 仪,孙 平,邓 玮

(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开发虚拟现实应用,能较为精确地遥操机械臂,在水平、垂直或斜坡面上布置探测器的基本不会出现失误。本研究方法积累可以应用于包括核环境在内的危险场景遥操作中,下一步将实际应用于托卡马克装置的遥操作上。