针对狭窄空间作业的机器人动态视觉系统研究

王 杨,王 犇,麦晓明

（1.广东电网有限公司电力科学研究院人工智能与机器人研究所，广东 广州 510000 2.华北电力大学（保定）能源动力与机械学院，河北 保定 017000）

1 引言

配网带电作业机器人一直是机器人研究的热点和难点[1]。配网作业环境非确定性强，遥操作操控方式能有效提高机器人的适应性。但是，当线路为同塔多回架设时，作业区域内非作业相导线或设备分布密集，安全作业空间狭窄受限，作业机器人本体无法灵活移动，传统的固定式视觉感知系统难以绕过作业空间内障碍遮蔽为操作人员提供空间高覆盖的作业环境感知和目标定位信息。而随着民用VR/AR设备技术成熟、成本降低，将VR/AR设备、串联机器人和遥操作技术相融合，设计可根据操作人员观测需求，避让视场内障碍对作业目标多角度观测和定位、辅助机器人带电作业的感动态视觉感知系统，不仅有现实需求，更有实现基础。

现有文献中利用AR/VR设备搭建视觉遥操作系统的研究较少。目前，遥操作与AR/VR技术相结合的研究主要集中于两个方向。

（1）将AR/VR设备仅作为显示设备向操作人员反馈现场视频图像，提升一定的遥操作临场感[2-5]；（2）利用AR/VR技术构建现场虚拟图像或者虚拟力、虚拟触觉降低系统延时对操作临场感的影响[6]。

在遥操作实现方面，异构型遥操作的逆运动学问题是难点之一，目前常用的方法是采用D-H法建立遥操作机器人主从端的运动学模型，通过对模型求逆的方法求解逆运动学问题[7]。

综上所述，现有针对将遥操作与VR/AR技术相结合设计搭建能跟随操控者头部位姿灵活运动，为操控者实时提供对目标多角度观测的动态视觉感知系统的研究尚处空白。

这里搭建了一套由VR头戴设备、6自由度机器人和双目相机构成的动态视觉感知系统，提出了一种针对该系统的逆运动学求解方法，并基于双目相机采集的视场内深度信息，实现对视场范围内固定小球位置的多角度观测和绝对位置粗定位。

2 系统设计

2.1 系统组成与工作流程

受到人类在多视角观测同一目标时脊柱姿态变化的启发，这里搭建了动态视觉系统。样机测试平台，如图1所示。各组件明细，如表1所示。

表1 系统硬件与主要技术指标Tab.1 System Hardware and Key Parameters

图1 动态视觉系统试验平台Fig.1 Experiment Platform of Dynamic Vision System

动态视觉系统工作流程如下：一方面，VR头戴设备将实时反馈双目相机采集的可见光图像；佩戴人员根据观察需要在一定范围内移动或转动头部；主控制器通过采集VR 设备内置惯导（Inertial Measurement Unit，IMU）及基站信息反馈的VR 设备实时姿态与位置信息，基于逆运动学模型求解末端搭载双目相机的6自由度串联机器人各关节角，并通过伺服模式驱动机器人关节运动，从而实现双目相机对佩戴VR设备的人员头部空间运动的实时跟踪；

另一方面，控制器根据双目相机返回的视场内深度信息和可见光信息，对目标进行识别和绝对空间位置粗定位，并将信息反馈至VR设备中，为操作人员提供目标位置信息[8]。

基于遥操作的动态视觉系统信号传递路径，如图2所示。

图2 系统信号传递路径图Fig.2 Diagram of Signal Transmission

2.2 主从异构型遥操作系统逆运动学求解

由于VR头戴设备（主设备）与6自由度串联机器人（从设备）构型相异，动态视觉系统遥操作需解决主从异构型遥操作的逆运动学问题。采用VR 设备绕笛卡尔坐标系（由标定过程决定）x，y，z轴旋转角（[φ，θ，φ]m）描述VR 设备姿态，以及VR 设备分别在三个轴上的投影[x，y，z]m描述VR设备空间位置；采用与相机（双目相机的左镜头）固连的机器人末端分别绕笛卡尔坐标系的x，y，z轴转角（[φ，θ，φ]s）描述相机空间姿态，以及末端分别在三个轴上的投影[x，y，z]s描述相机空间位置。VR设备和机器人坐标系定义，如图3所示。

图3 遥操作系统坐标系定义Fig.3 Coordinate System Definition of Teleoperation System

该遥操作系统的逆运动学问题可描述为：根据VR设备的空间位姿求解相应的机器人关节角。即：

式中：f—由VR设备位姿到机器人末端位姿的映射模型；g—由机器人的末端位姿到机器人各关节角大小的映射模型，q=[q1，q2，…，q6]—机器人关节角。

2.2.1 VR设备与动态视觉系统的位姿映射模型

在位置映射方面，VR设备空间位置与动态视觉系统末端相机镜头的位置映射采用比例映射，即：

式中：a—主从设备之间位置映射的比例系数。为实现高临场感，a=1。[x0，y0，z0]m和[x0，y0，z0]s分别为VR设备和机器人在末端初始状态空间位置。

姿态映射是主从异构型遥操作系统逆运动学问题求解的关键。为保证佩戴者的高临场感体验，需保证VR头戴设备的姿态与机器人末端相机的姿态一一映射。由于主从设备的相对坐标系定义不同，首先引入旋转矩阵实现对VR头戴设备和机器人末端空间姿态的唯一表达，即：

其次，在映射关系下，机器人末端相对坐标系的n，o，a轴分别与VR设备的a，o，n对应且在笛卡尔坐标系z-轴上的投影等大反向，即：

2.2.2 机器人末端位姿与关节角度映射模型

6自由度串联机器人机构图，如图4所示。机器人的正运动学模型可描述为：

图4 机器人坐标系定义Fig.4 Coordinate System Definition of Robot

因此，g的求解就是根据机器人理想末端位姿M(q)求解其各关节角大小q=[q1，q2，…，q6]的问题。

为方便描述求解过程，定义关节4回转轴线和关节5回转轴线的交点为机器人“颈”。这里采用求解方法的关键是利用机器人的几何约束，首先根据末端位姿确定“颈”部空间位置，从而将一个描述机器人末端位姿和机器人6个关节角度关系的六元方程组求解问题，转化成为分别描述机器人“颈”部位置与机器人[q1，q2，q3]等3个关节角度之间关系，以及描述机器人末端姿态与剩余[q4，q5，q6]等3个关节角度之间关系的两个三元方程组求解问题。具体的求解过程请见参考文献［9］。

2.3 被观测目标绝对空间位置粗定位

基于刚体的坐标变换原理，观测目标在相机中的位置与在笛卡尔坐标系中的位置存在以下转换关系：

式中：pin_eye—观测目标在相机坐标系中的位置；pin_cartesian—观测目标在笛卡尔坐标系中的位置。

3 样机试验及结果分析

采用样机试验的方法验证这里搭建的动态视觉系统的感知和定位功能有效性。

3.1 样机系统参数设定

系统的软件编写采用Visual Studio 2015 并在工作站运行。工作站为主控制器，通过调用配套API函数以125Hz的工作频率采集HTC VIVE（主端设备）位姿信号，并将经逆运动学模型计算后的机器人关节角度由socket建立的TCP通讯以125Hz工作频率发送至UR3（从端设备）的30003端口。UR3采用关节伺服控制，控制指令参考文献［10］中的相关命令格式编写（具体参看servoj命令，这里将控制时间参数设置为t=0.1）。

试验过程图，如图5所示。UR3机器人的详细尺寸参数，及机器人本身的逆运动学问题求解精度在参考文献［9］中详述。

图5 试验过程Fig.5 Testing Process

3.2 环境多方位动态感知遥操作试验

初始状态下，人员佩戴VR设备后，VR设备初始空间位姿为：

动态视觉系统在工作空间和关节空间位姿跟踪试验结果分别，如图6、图7所示。

图6 遥操作位姿轨迹跟踪Fig.6 Trajectory Tracking of Teleoperation

图7 关节跟踪试验结果Fig.7 Trajectory Tracking of Robot Joints Position

图6中，VR设备生成的参考信号已经换算为在UR3绝对坐标系下的表达，下标ref表示VR头戴设备生成的末端参考位姿轨迹信号，下标act表示与UR3末端固连的双目相机实际空间位姿信号。系统在工作空间和关节空间的跟踪平均误差和标准差统计分别，如表2、表3所示。

由试验结果可知，在位姿跟踪精度方面：这里动态视觉系统及遥操作模型能实现串联6自由度机器人末端及搭载双目相机对VR 头戴设备的空间位姿跟踪。位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差＜2.36°。动态视觉系统在对环境多方位感知方面具有较高的临场感。

在遥操作的位姿跟踪误差来源方面：首先，误差主要是由主、从设备之间的固有工作频率差异导致。这在误差统计方面，表现为标准差远大于误差均值，如表2、表3所示。而在跟踪效果上表现为参考轨迹变化快速发生变化后，跟踪误差会有大幅上升并逐渐回落，如图7中虚线及图6和图7中圆圈标识区域曲线变化所示。VR头戴设备质量轻、惯性小且由操作人员直接驱动运动，工作频率较高；串联机器人关节质量较大且由电机驱动，工作频率较低，这导致该主从遥操作系统本身是一个低通滤波器，从端的机器人始终无法跟踪主端VR 设备生成的轨迹信号中的高频成分。其次，6自由度机器人本身的逆运动学求解精度，也产生了位姿跟踪的部分系统误差。

表2 末端跟踪误差统计Tab.2 Tracking Error of Teleoperation

表3 关节跟踪误差统计Tab.3 Error of Robot Joint Position

3.3 目标多方位观测粗定位试验

在目标识别与定位方面，系统采用颜色和边缘检测的方法实现对目标小球的识别，并利用双目相机采集的深度信息完成对目标小球球心的三维空间定位。在图像反馈方面，VR设备向操作人员展示经三维渲染后的双目相机采集的可见光图像、识别目标高亮选定框、识别目标绝对空间位置等信息。小球识别方法、双目视觉定位原理和VR 图像显示方法不在这里详述。视觉定位试验取样（5次），如图8所示。

图8 视觉定位试验取样（5次）Fig.8 Samples of Vision Location Testing（5 Times）

试验过程包含5次观测，记录机器人处于不同位姿状态下，其末端双目相机采集到的目标小球在相机坐标系中的位置，经（4）式所示模型计算，求得小球在笛卡尔坐标系（机器人绝对坐标系）下的位置。5次观测对应的机器人姿态，如表4所示。经双目相机反馈的小球在相机坐标系中位置，如图9、表5所示。笛卡尔坐标系下小球的实际位置及经计算所得的小球位置，如表6 所示。试验结果表明，这里所设计的动态视觉系统能够实现在不同观测位姿下对小球绝对位置的粗定位，球心定位误差约为1%（小球与机器人底座水平距离为1.1m）。

表4 观测取样对应的机器人关节角度Tab.4 Joint Positions Corresponding to the Samples for Vision Location Testing

表5 观测取样对应的小球在相机坐标系中定位Tab.5 Location of the Ball in the Coordination System of Camera Corresponding to the Samples for Vision Location Testing

表6 观测取样对应小球在笛卡尔坐标系中定位Tab.6 Location of the Ball in the Cartesian Coordination System Corresponding to the Samples for Vision Location Testing

图9 定位结果对比（5次）Fig.9 Comparison of the Calculated Position of the Ball in Cartesian Coordinate System

4 结论

这里设计并搭建了包含VR头戴设备、六自由度串联机器人和双目相机的动态视觉系统，并提出了针对该系统的异构型主从遥操作逆运动学模型，实现了在笛卡尔坐标系下双目相机对VR头戴设备的空间位姿跟踪。其中，位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差小于2.36°；另一方面，这里提出了被观测目标在相机中位置与在笛卡尔坐标系中位置映射模型，实现了相机在不同位姿状态下对一个固定小球在笛卡尔坐标系下的空间粗定位，定位精度优于1%。这里研究成果可应用于配网带电作业机器人，以及其他在高危环境下开展作业的机器人系统研发。以应对作业狭窄，无法通过移动作业机器人本体绕开视场内障碍对作业环境进行空间高覆盖的感知和作业目标粗定位的问题。

这里的研究内容主要集中在系统的基本功能实现，尚未对从设备的运动规划和控制、多种目标识别与定位等内容开展研究。因此，未来需做三点研究。（1）引入机器人的路径规划方法研究，提高从设备对主设备的动态跟踪性能；（2）将遥操作感知与机器人自主感知相结合，研究机器人自主绕障观测目标，并指导作业设备（作业臂）的路径规划；（3）研究针对配网作业相线路金具户外环境识别与定位方法，提高设备实用性；（4）用户体验上，需增加空气墙，防止从设备超出安全工作空间。