基于虚拟力引导的人机协同目标抓取方法

姜通维，姜勇

（1.沈阳建筑大学 信息与控制工程学院，辽宁 沈阳 110168;2.中国科学院 网络化控制系统重点实验室，辽宁沈阳 110016;3.中国科学院 沈阳自动化研究所，辽宁 沈阳 110016;4.中国科学院 机器人与智能制造创新研究院，辽宁 沈阳 110169）

一些特殊的工作场景如带电作业，由于其危险的工作环境使得工人们在工作时往往处于危险环境中如高压和高空[1]。力反馈遥操作机器人能够让使用者在安全的距离外通过主端设备控制从端设备进行作业，因而被广泛应用于带电作业以及其他危险环境的工作。人机协同技术能够将人的决策能力实时地融入到机器人的自主控制中，提高机器人在复杂环境下作业的能力和可靠度，从而提高其对复杂环境下作业的适应能力[2-3]。

自Rosenberg 首次提出虚拟夹具技术[4]以来，国内外学者对力反馈装置与人机协同系统进行了一系列的研究。主要集中于操作者的控制辅助以及与从端环境的交互和触觉信息的感知方面[5-8]。Lee 等[6]提出了一种机器人控制方法，操作者通过在实时视频图像上绘制所需的参考路径，据此生成机器人移动的实际路径并产生虚拟引导力协助操作者操作移动机器人完成相关动作。Nikolaidis等[9]提出了人机相互适应的人机协同方法，通过MOMDP 计算用户的目标分布概率，在保持人的信任的同时引导操作者向最优目标前进。Wang等[10]提出了一种共享控制遥操作方法，通过利用多自由度机械臂的冗余自由度，在不影响机械臂末端位置的情况下实现机械臂本体对动态障碍物进行避障。

由于主端和从端设备的结构不同，操作者只能对从端设备的一部分进行控制。所以目前这个方向的研究大多将从端视为一个等效质点，注重于从端的位置(如移动机器人和机械臂末端)[11-13]。传统基于人工势场法的虚拟力引导方法会将机械臂末端受到的斥力和引力叠加后通过力反馈设备反馈给操作者[11-14]。这种方法由于目标吸引力的存在导致操作者感受到的力并不适合于引导操作者进行机械臂本体避障任务。此外，对于6 自由度机械臂的目标抓取操作而言，往往需要操作者操作机器人末端以需要的姿态到达目标位置[15]。但在直接遥操作中，操作者往往难以将机械臂移动到某一精确的位置和姿态，并且由于人的手臂和手腕构成了一个耦合系统，二者的运动通常会相互影响，导致在操作过程中经常不可避免地会出现操作偏差。

因此，本文研究了一种基于虚拟力引导的遥操作方法。基于从端机械臂末端、障碍物和目标点的相对位置，通过在预定义路径周围构建管道形虚拟力场，结合障碍物的斥力与虚拟力场的约束力，生成趋近目标的同时进行避障操作的力引导。通过构建锥形虚拟力场并对操作者的输出进行限制，生成协助抓取任务的力引导，同时减少操作者的误操作。

1 基于虚拟力引导目标抓取方法

目标抓取任务可以分为趋近目标点和到达目标点两个阶段。在趋近目标阶段，通过构建管道形虚拟力场(以下简称管形力场)，利用管形力场内部的受约束空间协助操作者避开障碍物后尽可能再回到预定义的路径上。当机械臂末端趋近目标点到达一定的距离后，通过构建锥形虚拟力场(以下简称锥形力场)协助操作者到达目标点实施抓取任务。

1.1 管道形虚拟力场的构建

在本系统中，我们使用forbidden region virtual fixture (FRVF)[8]来限制机械臂的末端不要过多地偏离理想的路径。

假设P是一条预定义的路径(可由任意轨迹规划算法生成)。以P为轴线，rd为半径可以构建出一个管形力场(如图1 所示)。再假设xc是机械臂末端在笛卡尔空间中的坐标，路径P上的点s(xc)是该路径上距离xc最近的点，其定义为

图1 管形虚拟力场示意Fig.1 Virtual force field of pipe

力场中的约束力可以定义为

式中：δ(xc)是约束力的参考方向，由xc和s(xc)的相对位置得出：

1.2 避障任务力引导的构建

针对传统人工势场法不适用于引导操作者进行机械臂本体避障的问题，提出的改进避障引导策略如图2 所示，由目标点对机械臂产生的引力被管形力场所产生的约束力代替，以防止产生错误的力引导信息。值得注意的是，我们所提的方法基于一个前提，即由于遥操作是一个human-inthe-loop 系统，操作者根据人的决策能力结合系统给与的力觉引导做出合理的操作，所以系统反馈给操作者的引导信息并不需要是完美的[2-3]。

图2 避障任务的虚拟力引导Fig.2 Virtual force guidance for obstacle avoidance

对于斥力场的构建，其通常的表达式如式(4)所示。当机械臂本体或者末端进入障碍物的作用范围时，作用在机械臂本体的斥力被映射到机械臂末端，经主端的力反馈设备反馈给操作者。距离障碍物越近，机械臂所受到的斥力越大。

式中：ηrep是一个斥力的增益因子；ρ是机械臂本体上距离障碍物最近的点；ρo是障碍物的作用范围。

障碍物所产生的斥力大小为斥力场对距离的导数，其表达式为

则操作者受到的引导力为机械臂在工作空间中受到的引力与斥力的合，其表达式为

若存在多个障碍物，则式(6)可变为

式中：Fc为管形力场产生的约束力。当机械臂本体距离障碍物较近时，其系统产生的斥力会大于管形力场的约束力，以确保避障任务的优先级高于趋近目标的任务。

约束力一直指向管形力场的中轴线(预定义路径)，则约束力的方向与斥力的方向的夹角会接近一条直线，所以约束力的加入不会对避障任务造成不利影响。当避障任务完成之后，操作者感受到的力引导只由虚拟力场产生，用于协助操作者跟随预定义的路径移动。

1.3 锥形虚拟力场的构建

当机械臂末端足够接近目标点后，操作者需要调整手臂和手腕来操作机械臂到达理想的位置和姿态。与管形力场的构建方法类似，当机械臂末端与目标点之间的距离小于一个特定的距离(即dc)时，可以建立一个基于机械臂末端位置的锥形力场(如图3 所示)来协助操作者进行精细的操作。对于一个目标点xt，一个初始点xs和一个基本圆锥的开口角度 α，则其对应的圆锥的中轴线可以定义为

图3 锥形虚拟力场示意Fig.3 Conical virtual force field

给定一个机械臂末端的笛卡尔空间位置xa，将它与目标点连线分解为与圆锥轴线相关的正交分量：

定义dcone为锥形力场边界到圆锥中轴线的距离，则如果机械臂末端在圆锥范围内就必须满足：

若xc在当前构建的锥形力场范围之外，即时可以根据当前机械臂末端的位置重新构建锥形力场协助操作者完成操作任务。

根据所建立的锥形力场，可以计算出虚拟引导的力，其表达式为其中k1和k2是两个增益参数，用于调节引导力的大小。从式(14) 可以看出，对于等量的位置偏差，随着机械臂末端位置与目标点的距离的减小，而操作者感受到的引导强度会增加。

对于机械臂末端姿态调整的虚拟力引导，其计算方式与对末端位置的引导一样。假设 θc是机械臂末端当前姿态的旋转角度，θt是目标姿态所对应的机械臂的旋转角度。则对于一个当前机械臂的状态X[xc,θc]，系统对操作者反馈的总引导力信息定义为

1.4 运动约束

如图4 所示，为了实现对操作者输出的限制，以下信息将会被用到：采样时间 ∆T、锥形力场的中轴线(式(8))a；机械臂末端所经过的离散路径P={x1,x2,···,xn}∈R3；在k时刻，机械臂末端位置其中x(k)表示末端位置，θ(k)表示当前姿态。在k时刻，机械臂末端速度其中ω(k)为后3 个关节的角速度。

图4 速度矢量分解Fig.4 Examples of decomposed velocity vectors

对一个机械臂末端速度v(k)，可以将其分解为

式中：vO(k)和vR(k)分别是v(k)垂直于圆锥中轴线和平行于中轴线的分量。

此时，加入一个调整参数µ(k)，则式(16)变为

通过调节参数µ(k)，可以实现对操作者输出的限制。当µ(k)=1时，机械臂末端将完全按照操作者发送的指令移动；当µ(k)=0时，则从端机械臂将完全忽略垂直于中心轴方向的分量。

在人机协作领域，存在一个如何分配机器人自主性能与人的操作权重的问题。许多学者对该问题进行了研究和探讨[2-3,16-18]，但当前还没有一个公认的权重分配方案。结合实际操作需求，本文认为人在大多数时间可以顺从机器人的自主决策和引导，但一旦人想要自己进行操作就应该拥有足够的控制权限。故将参数µ(k)定义为

式中：dcone(k)和d(k)分别由式(9)和式(12)计算得出。图5 描述了参数µ(k)与d(k)之间的关系。机械臂末端越接近目标位置，操作者的输出在垂直圆锥轴线的分量的控制权重越低，以降低误操作的影响。但系统不会忽视操作者的输出，操作者依然对从端有足够的控制权限。对于姿态误操作的限制与末端位置的限制相同，在此不再赘述。

图5 参数 µ(k)与 d(k)之间的关系Fig.5 Relationship between µ (k) andd(k)

2 实验验证

本文所提基于虚拟力引导的遥操作系统由一个主从控制子系统和深度相机组成，如图6 所示。

图6 虚拟引导力遥操作系统结构Fig.6 Virtual force guidance teleoperation system

相机用于获取目标点位置、障碍物位置和从端机械臂与障碍物的相对距离。从端设备为一个UR5 协作机器人，主端为一个Virtuose 6D 力反馈仪，能够与操作者实现力觉交互。在实验中，从端机械臂向主端发送位置和速度信息，而主端力反馈设备结合从端发送的信息和相机发送的距离信息计算出引导力并反馈给操作者；同时向从端发送速度指令用以控制从端设备的运动。

2.1 机械臂避障实验

在机械臂本体避障实验中，我们仅考虑障碍物和目标物都为静态且唯一的情况。根据机械臂末端、障碍物和目标物的位置建立起斥力场和管形力场，并计算出虚拟力引导(由式(6)得出)，通过主端反馈给操作者。操作者根据这些力反馈信息对从端机械臂进行操作，最终避过障碍物并接近目标点。当机械臂避过障碍物后，机械臂末端距离目标点10 cm 内即可判定为完成实验。当机械臂本体碰到障碍物则实验失败。

实验在两种场景中进行(如图7 所示)，两个场景的目标点位置和机械臂初始位置相同，但障碍物的位置不同，两场景中障碍物分别作用于机械臂的末端和连杆。我们分别在两个场景中使用本文所提虚拟力引导方法、文献[7]所提力感知方法和无力引导操作方法分别进行多次实验，实验结果如表1 和表2 所示。

图7 机械臂避障实验的实验环境Fig.7 Experimental environment for obstacle avoidance

表1 障碍物作用于机械臂末端避障实验结果Table 1 Experimental results of the obstacle approaching the end

表2 障碍物作用于机械臂连杆避障实验结果Table 2 Experimental results of the obstacle approaching the links

图8(a) 和图8(b)～(d) 分别是一次避障实验中，在3 种方法协助下的机械臂末端的轨迹和其末端与预定义路径的偏差。可见，在虚拟力引导的情况下，当机械臂在障碍物的作用范围内时，障碍物对机械臂的斥力对操作者的引导占主要作用从而避免碰撞发生。操作者可以在虚拟力的引导下完成避障任务。同时，相较于文献[7]所提力感知引导方法，由于管形力场的作用，机械臂末端轨迹更接近障碍物表面，使得末端与预定义路径的最大偏差大幅减小。并且在完成了避障任务后，末端与预定义路径的偏差能快速缩小。此外，即使没有受到指向目标点的力引导，操作者依然能够通过自身的决策能力结合管形力场提供的约束力快速完成实验任务，缩短耗时，且机械臂末端的与预定于路径的偏差在较小的范围之内。图8(e) 是一次当障碍物作用于机械臂连杆时3 种方法的机械臂末端轨迹对比，其中红色路径为机械臂与障碍物发生碰撞的点。可见基于人工势场的力感知方法没有考虑到机械臂连杆的作用，导致会产生错误的引导信息，使得与障碍物发生碰撞的概率增加。而虚拟力能够正确地引导操作者完成操作任务。

图8 避障实验中机械臂末端的轨迹及偏差Fig.8 Trajectory and deviation of the end of the manipulator in obstacle avoidance experiment

2.2 目标抓取验证

与避障实验相似，在精细操作的实验中，目标点的位置和目标姿态不变，而机械臂末端的初始位置和姿态不同。实验在有引导力、无引导力和仅有运动约束3 种条件下进行。操作者操作机械臂末端从初始点移动到目标点，并完成姿态的调整。当机械臂末端与目标点的距离小于1 cm 且后3 个关节旋转角度与目标角度相差小于 5◦则判定实验成功，当操作时间超过70 s 则判定操作失败。实验结果如表3 所示。

表3 目标抓取实验结果Table 3 Results of the object grasping experiment

图9(a)和图9(b)～(d)分别是有力引导、无力引导以及仅运动约束作用下的一次目标抓取操作实验中机械臂末端的轨迹对比和与圆锥轴线的偏差对比。可以看出，相较于在仅有运动约束的协助方法，在锥形力场的引导下操作者能够在实验开始阶段便快速接近圆锥轴线并向目标点接近。表明虚拟引导力的加入能够提高操作者对从端设备的操作效率。同时大幅减轻操作者的操作负担。而相较于无力引导的直接遥操作方法，运动约束的加入能够减小在机械臂末端接近轴线后路径偏差的波动，有效低降低在精细操作时由于人手的生理构造造成的操作偏差。

图9 不同协助下的机械臂末端轨迹Fig.9 Trajectory and deviation of the end of the manipulator with different assistance

3 结束语

本文，主要关注于力反馈遥操作系统中的两个问题：1)在进行避障任务时从端的机械臂应被视为一个整体而非一个等效质点来控制；2)由于人手构造的问题，在进行目标抓取任务时操作者往往难以将机械臂移动到较为精确的位置和姿态。

针对第一个问题我们提出了一个改进方案，基于机械臂和障碍物的位置信息，结合人工势场法和虚拟夹具构建对障碍物的斥力场和管形力场，能够为操作者提供相应的导向力，协助操作者完成机械臂的整体避障任务，接近目标点。对于第二个问题，当机械臂末端需要到达目标点时，构建出基于位置和旋转角的锥形力场来辅助操作者到达目标点并调整姿态。此外，使用了一种输出限制方法以减少操作者在精细操作时的操作失误。最后通过相关的实验验证了该方法能有效提高远程操作的效率，降低操作失误的发生频率。