光学运动捕捉在机械臂遥操作控制中的应用研究

张伯强，方宇，杨皓，杨蕴杰，黄子健

（上海工程技术大学 机械与汽车工程学院，上海 201620）

机器人遥操作技术兼具主端人的规划、决策能力以及从端机器人自动化两方面优势，目前是机器人研究领域的热门方向之一[1-3]。

传统的遥操作系统主端一般采用摇杆手柄、按键、手势等作为指令输入方式。王朝阳等[4]利用Kinect 传感器作为遥操作系统主端，搭建了基于Motoman 类人机器人的实验平台，用于实现机器人直接示教。卢明林等[5]使用双操作杆和Motoman-UP6 机器人，研发了以远程遥控操作方式进行示教取点的焊接机器人连续轨迹控制系统。倪涛等[6]设计了能够实现主从位移控制的手控器,通过融合操作者与机器人的视觉引导，实现机器人遥操作中主从两端的信息共享和相互引导。上述类型的机器人遥操作系统普遍缺乏直观性，临场感不强，同时对操作人员的技术水平要求很高。

很多学者在遥操作主端使用了可穿戴式设备来提高操作的灵活性和临场感。李家霖等[7]开发了能够精确获取人体上肢运动信息的外骨骼式遥操作主手，并通过异构映射算法，实现对六自由度协作机械臂的遥操作。Rebelo 等[8]使用七自由度外骨骼手臂主机，实现了具有直观用户界面的双边机器人遥操作。李海源等[9]设计了基于力和位置的阻抗控制方法，使得操作者与关节系统可以实现协同运动。但穿戴式设备会限制操作人员的活动范围，同时也存在着操作繁琐复杂的问题。

针对上述遥操作方法中存在的不足，本文提出了一种能够将光学运动捕捉系统与ROS[10]之间数据实现无隙链接和实时传输的遥操作控制方法，并搭建了六自由度机械臂遥操作系统。该方法充分发挥了光学运动捕捉系统的低延时、高精度和ROS可复用性高、可拓展性强的优势，有望在远距离或非结构化等复杂环境下的机器人应用领域实现较高的应用价值[11-13]。

1 系统构成及实现原理

本文在Ubuntu16.04 上利用ROS-kinetic 搭建了由光学运动捕捉系统、六自由度机械臂组成的数据传输以及运动规划平台，机械臂遥操作系统由Optitrack 运动捕捉系统和ROS 组成。

1.1 光学运动捕捉系统

采用NaturalPoint 公司开发的Optitrack 运动捕捉系统，可以实现最优0.02 mm 的定位精度，即使在环境不理想的情况下也能稳定输出位置误差低于0.3 mm、角度误差低于0.05°的六自由度数据。运动捕捉系统示意图如图1 所示。

图1 运动捕捉系统示意图Fig.1 Schematic diagram of motion capture system

通过多个标记点构建出刚体，可利用动作捕捉软件解算出目标刚体在空间中的位姿和运动轨迹等信息，实现对目标的追踪。刚体的质心位置可通过软件进行调整，从而消除刚体构建引起的随机误差。由于构建的刚体只需要部分暴露在红外相机的视野中即可，因此在一定程度上降低了工作空间中存在遮挡物的影响，使得对目标的捕捉更具有稳定性。红外相机及标记点如图2 所示。

图2 红外相机及标记点Fig.2 Infrared Camera and Marking Points

1.2 机器人操作系统

ROS 是一种用于编写机器人软件的灵活框架，它是工具、库和约定的集合，旨在简化各种机器人平台上创建复杂而稳健的机器人行为的任务。ROS 提供了大量的软件资源，集成了多样化的功能包[14]，具有很强的可复用性和扩展性。

1.3 遥操作实现原理

遥操作实现原理如图3 所示，首先利用被动式反光标记点构建刚体，由光学运动捕捉系统获取刚体的质心位姿并通过Vrpn（Virtual-reality peripheral network）协议[15]实时发送给ROS；在ROS 下搭建服务器和客户端，由服务器利用数据处理方法将订阅到的刚体位姿信息实时转换到机械臂的坐标系下，并以服务的形式发送给客户端；使用运动学求解器反解出关节角度，利用Moveit 进行运动规划，通过实现机械臂对刚体的实时位姿跟踪，建立遥操作主从端映射关系。

图3 遥操作实现原理Fig.3 Implementation principle of remote operation

2 系统模型建立

2.1 运动学建模

采用UR5 六自由度协作机械臂进行研究，利用传统的D-H 参数法对其进行运动学建模，建立了各个关节坐标系，其相邻坐标系下的齐次变换矩阵为

式中：i为关节序号；ai为连杆长度；αi为连杆i相对于连杆i-1 的转角；di为第i个关节的偏置距离；θi为第i个关节的转角范围；i=1,2,···,6。

UR5 机械臂本体以及关节连杆简图如图4 所示。UR5 机械臂D-H 参数如表1 所示。

表1 UR5 机械臂D-H 参数Tab.1 D-H parameters of UR5 robotic arm

图4 UR5 机械臂及关节连杆简图Fig.4 Schematic diagram of UR5 joint connecting rod

将表1 中所示的D-H 参数代入式（1），可以得到每个连杆之间的齐次变化矩阵，将矩阵连续右乘，可得到机械臂末端相对于基座坐标系的转换矩阵为

2.2 系统标定模型

UR5 机械臂与Optitrack 运动捕捉系统分别有着各自独立的坐标系，因此在数据传输以及机械臂运动规划的过程中需要进行坐标系转换。标定模型以及坐标转换示意图如图5 所示。Optitrack 采用了L 型标定杆的特殊标定形式，其长边指向Z轴正向，短边指向X轴正向，Y轴正向向上，构成Y-up 右手坐标系。为方便后续的坐标系转换及保证标定精度，L 型标定杆与机械臂的基座坐标系位于同一平面内且坐标轴互相平行。图5 中：base 为UR5 机械臂安装基座；opt 为L 型标定尺；tool 为UR5 末端；Cbase为基坐标系；Ctool为末端工具坐标系（从端坐标系）；Copt为Optitrack 系统坐标系（主端坐标系）。

图5 标定模型示意图Fig.5 Schematic diagram of calibration model

坐标系Copt到坐标系Cbase的映射可以用一个矩阵算子来表示，即

坐标系Cbase到坐标系Ctool的映射则通过变换矩阵T实现，建立遥操作系统主从端之间的映射关系为

式中：a、b、c分别为主从端在3 个空间方向的设定操作距离。

3 数据处理方法

由于Optitrack 运动捕捉系统和ROS 系统是完全独立的，拥有不同的控制方式以及数据格式，需要在这两系统间建立一个平台，实现两系统数据间的转换与实时传输，完成机械臂对刚体六自由度位姿信息的获取。

为了提高数据传输的实时性，在Ubuntu16.04系统下的ROS-kinetic 中，采用ROS 特有的服务（service）机制建立服务器与客户端节点，从而通过网络来完成系统数据间的发送与接收。将服务器与客户端搭建在同一台计算机的ROS 中，运动捕捉系统软件Motive 安装在另一台计算机中作为远程服务器实时监测刚体的运动数据并将其发布。当服务器和客户端连通时，可以在ROS 下获得刚体的信息，包括刚体的名称、请求的坐标系名称、初始位置等信息。图6 所示为系统运行时的节点关系图，其中sub-node 为服务器节点，myclient_node为客户端节点。

图6 节点关系图Fig.6 Node relationship diagram

move_group 作为Moveit 的重要节点之一，通过消息和服务的方式接收机械臂发布的信息以及机械臂的tf 坐标变换，并提供用户接口。

数据处理算法的伪代码如下：

4 试验及结果分析

4.1 试验准备方案

将被动式反光标记点粘贴至作为遥操作主端的AUBO 机械臂末端，构建目标刚体并调整质心至合适位置。

为了减少从端UR5 机械臂逆运动学求解的时间，提高路径规划效率，在UR5 机械臂的URDF 文件中设置了障碍物墙壁并应用至真实环境，将机械臂的运动范围限制在空间第一象限中。

图7 为3D 可视化模块rviz 界面下展示的机械臂运动仿真平台。图8 为所构建刚体的标记点分布，保证至少有3 个标记点能够同时被两台相机捕捉。

图7 Rviz 界面Fig.7 Rviz interface

图8 末端刚体构建Fig.8 End rigid body construction

4.2 直线跟踪试验

通过带有标记点的AUBO 机械臂的随动试验，验证本文提出遥操作方法的跟踪精度。AUBO 机械臂通过编程实现空间直线运动，运动速度10 mm/s，加速度10 mm/s；使用Optitrack 运动捕捉系统对末端刚体运动轨迹进行采集；同时利用rosbag 工具记录UR5 机械臂运动过程中的6 个关节角度数据。

通过正运动学求解获得UR5 末端空间位置，进行坐标转换后画出空间散点图，并利用最小二乘法分别对两组数据进行空间直线拟合。由于点的数量密集，为方便观察，绘制了局部直线效果图。图9 为直线跟踪试验结果图。

图9 直线跟踪试验Fig.9 Linear tracking test

设AUBO 机械臂和UR5 机械臂的空间拟合直线方程分别为l1、l2，即：

UR5 机械臂在跟踪时存在一定的响应时间，导致ROS 的采样频率与运动捕捉系统不一致，因此采样点的数量较少。但是通过数据拟合结果可以看出，两条空间直线的方向向量差距较小，经计算可得夹角为0.003 9°，整体跟踪精度较高。受安装条件和环境因素影响，机械臂在运动过程中不可避免地存在抖动，本试验中运动误差均可控制在1.5 mm 以内。

4.3 复杂图形跟踪试验

通过复杂图形跟踪试验，验证本文提出遥操作方法的跟踪性能。编程控制AUBO 机械臂末端运动“W”型和“0”型轨迹，运动速度10 mm/s，加速度10 mm/s，并使UR5 机械臂进行末端跟踪，其中“W”型试验过程如图10 所示。

图10 “W”轨迹追踪实物实验Fig.10 Physical experiment on "W" trajectory tracking

试验结果如图11 所示，可以看出UR5 机械臂末端的跟随轨迹与AUBO 机械臂基本保持一致。

4.4 响应时间分析

本文随机选取了10 个空间点进行试验，具体的响应时间如表2 所示，运动捕捉系统具有低延时性，使得系统间数据传输时间大幅减小，算法的计算时间可以达到毫秒级。由于机械臂的运动响应时间根据计算机性能不同而有所不同，使用高性能计算机运行程序，更换逆运动学求解器等将会有效减少响应时间。本试验中的运动响应时间可以基本控制在0.8 s 以内，具有较好的实时性。

表2 运动响应时间Tab.2 Motion response time

5 结论

1）提出了一种将Optitrack 光学运动捕捉系统与ROS 结合的机械臂遥操作方法，并搭建了机械臂遥操作系统，实现了二者数据之间无隙链接与实时传输；

2）建立了Optitrack 光学运动捕捉系统与UR5 机械臂的标定模型，实现了坐标系间的转换，并在此基础上建立了遥操作主从端的映射关系；

3）在真实机械臂上进行了跟踪试验，直线跟踪角度差为0.003 9°，运动响应时间小于0.8 s。结果表明该方法能够实现机械臂遥操作任务，精度高且具有良好的实时性。

受场地限制，本文试验将引导机械臂与测试机械臂布置在同一实验场景内。后续当工作空间超出运动捕捉相机视野范围时，可考虑在机械臂上安装传感器来间接获取其运动状态与周围环境信息，同时标记点也可以粘贴至人手或其他引导工具上来构建刚体，从而实现机械臂的远程控制。