CN111660307B - 一种机器人操作高辅精度的虚拟夹具控制方法及系统 - Google Patents

Abstract

本发明属于人机交互相关技术领域，并具体公开了一种机器人操作高辅精度的虚拟夹具控制方法及系统。所述方法包括：对牵引机器人过程中的人手操作力进行前处理，得到与环境交互力；获取机器人末端的实际位姿与虚拟夹具引导路径最近的参考点位姿，生成机器人末端位置轮廓误差补偿量和方向轮廓误差补偿量，以获取误差补偿修正力；对虚拟夹具引导路径的曲线参数进行限位处理；对与环境交互力进行约束，生成约束后的操作力，并根据误差补偿修正力对机器人末端的路径偏差进行在线补偿。所述系统包括虚拟夹具下的动觉示教模块以及轮廓误差估计与分量补偿模块。本发明使机器人能够在虚拟夹具引导路径上往复运动，具有辅助示教精度高、适应性强等特点。

Description

一种机器人操作高辅精度的虚拟夹具控制方法及系统

技术领域

本发明属于人机交互相关技术领域，更具体地，涉及一种机器人操作高辅精度的虚拟夹具控制方法及系统。

背景技术

虚拟夹具也被称为虚拟约束，从功能上可以分为引导型虚拟夹具与禁止区域型虚拟夹具。目前，其被广泛应用在工业、医疗、人机日常协作等领域。当前，常用的虚拟夹具控制方法有代理法、势场法、参考方向法等，其中参考方向法由于其简单性被广泛应用。然而在现有参考方向法虚拟夹具控制结构中普遍存在辅助示教精度低、尚不能高效高精度获得机器人末端与虚拟夹具路径上的最邻近点的问题，因此无法满足需要高辅助示教精度的虚拟夹具辅助操作任务例如手术机器人术中引导操作、辅助遥操作机器人装配以及辅助机器人动觉示教等。

现有技术中，虚拟夹具辅助机器人动觉示教控制结构的作用下，虽然在一定程度上能够满足辅助机器人动觉示教的要求，但是仍存在如下问题：(1)当机器人初始状态不在虚拟夹具引导路径上时，则会导致机器人末端跟踪一条平行于虚拟夹具引导路径的参考曲线，而不是构造的虚拟夹具引导路径；(2)当操作者施加的操作力过大时，机器人末端会偏离虚拟夹具引导路径的参考曲线，示教路径偏差也随之增大。

基于现有技术中存在的上述问题，需要对传统虚拟夹具控制结构及方法作出改进，对示教路径偏差进行补偿，以提高辅助示教精度。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种机器人操作高辅精度的虚拟夹具控制方法及系统，其中结合虚拟夹具构建自身的特征及其人机交互的工艺特点，相应设计了机器人操作高辅助精度的虚拟夹具控制方法及结构，其根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量生成误差补偿修正力，然后根据约束后的操作力和误差补偿修正力对机器人末端的路径偏差进行在线补偿，并生成机器人末端执行操作的期望速度和期望位姿，使机器人末端能够在虚拟夹具引导路径上往复运动，使虚拟夹具辅助操作精度能够直观合理的表示，具有辅助示教精度高、适应性强等特点。

为实现上述目的，本发明提出了一种机器人操作高辅精度的虚拟夹具控制方法，包括以下步骤：

S1对牵引机器人过程中的人手操作力进行前处理，得到与环境交互力，并将该与环境交互力转换为机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿；

S2获取机器人末端执行操作过程中的实际位姿与虚拟夹具引导路径最近的参考点位姿，并以此生成机器人末端位置轮廓误差补偿量和方向轮廓误差补偿量；

S3根据机器人末端的实际位姿对虚拟夹具引导路径的曲线参数进行限位处理，生成边界约束模型；

S4根据虚拟夹具引导路径以及边界约束模型对所述与环境交互力进行约束，生成约束后的操作力；

S5根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量生成误差补偿修正力，然后根据约束后的操作力和误差补偿修正力对机器人末端的路径偏差进行在线补偿，并生成机器人末端执行操作的期望速度和期望位姿，重复步骤S2至步骤S5，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

作为进一步优选的，步骤S1中，所述与环境交互力通过导纳控制转换为机器人末端的期望速度和期望位姿。

作为进一步优选的，步骤S2具体包括以下步骤：

S21获得距离机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_c，构建机器人末端实际位置轮廓误差模型：

ε_p＝P_c-P_a

其中，ε_p为机器人位置轮廓误差补偿量，其方向由点P_a指向参考点P_c；

S22利用等效转轴和等效转角构建机器人末端实际位置方向轮廓误差模型：

ε_o＝r_cε_o

其中，r_c＝[r_cx,r_cy,r_cz]^T为单位旋转轴向量，ε_o为绕轴r_c旋转的角度；

S23根据所述机器人末端实际位置轮廓误差模型以及机器人末端实际位置方向轮廓误差模型，获取机器人末端实际位姿与虚拟夹具引导路径最近的参考点位姿，并以此生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量。

作为进一步优选的，步骤S23具体包括以下步骤：

S231获得离当前机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_l以及指定步长内虚拟夹具引导路径上与参考点P_l相邻的两点P_l-1和P_l+1；

S232利用参考点P_l-1、P_l与P_l+1的二阶多项式曲线描述虚拟夹具引导路径的局部几何特征P(s)，其中s为曲线参数；

S233根据机器人末端实际位置点P_a以及局部几何特征P(s)求解机器人末端估计的轮廓点P_c，该估计的轮廓点P_c的计算模型为：

其中，δ为曲线参数s的取值区间；

S234根据所述估计的轮廓点P_c计算估计的位置轮廓误差

S235根据估计的轮廓点P_c、虚拟夹具引导路径上的参考位置点P_l-1、P_l和P_l+1以及参考方向Q_l-1、Q_l和Q_l+1以及四元数球面线性插值公式求解轮廓点方向

该轮廓点方向

的计算模型为：

其中，λ为距离系数，Slerp()为四元数球面线性插值公式；

S236根据轮廓点方向

生成方向轮廓误差补偿量模型：

其中，

是估计的机器人末端方向轮廓误差的大小，

是估计的机器人末端方向轮廓误差的方向。

作为进一步优选的，步骤S3中，对所述与环境交互力进行约束的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵。

作为进一步优选的，步骤S5具体包括以下步骤：

S51根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量对机器人末端的实际速度和实际位姿进行在线补偿，获取误差补偿修正力；

S52所述约束后的操作力和误差补偿修正力通过导纳控制转换为修正后的机器人末端的期望速度和期望位姿，重复步骤S2至步骤S5，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

作为进一步优选的，步骤S52中，所述误差补偿修正力f_c的计算模型为：

所述导纳控制的计算模型为：

其中，K为刚度矩阵，B为对角刚度矩阵，X_p为虚拟夹具引导路径参考点的位姿，X_e机器人末端实际位置点的位姿，

为虚拟夹具引导路径参考点的速度，

机器人末端实际位置点的速度，f′_e为约束后的操作力，f_e是人手操作力经前处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

作为进一步优选的，步骤S4中所述边界约束模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。

按照本发明的另一个方面，还提供了一种机器人操作高辅精度的虚拟夹具控制系统，包括虚拟夹具下的动觉示教模块以及轮廓误差估计与分量补偿模块，其中，

所述虚拟夹具下的动觉示教模块包括依次连接的动觉示教力输入单元、虚拟约束生成器、导纳控制器，其用于输入人手操作力f_h，最后输出机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿；

所述虚拟夹具下的动觉示教模块还包括逆运动学计算器和PID控制器，其用于根据期望速度和期望位姿控制机器人末端执行操作，并输出机器人末端的实际速度和实际位姿；

所述轮廓误差估计与分量补偿模块包括轮廓误差估计器以及边界约束生成器，其中所述轮廓误差估计器根据机器人末端实际速度和实际位姿生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量，并将该位置轮廓误差补偿量和方向轮廓误差补偿量生成差补偿修正力，输入至导纳控制器，所述边界约束生成器根据虚拟夹具引导路径所确定的曲线参数的上限位以及机器人末端的位置轮廓误差补偿量对虚拟约束生成器进行迭代更新，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

作为进一步优选的，所述动觉示教力输入单元用于对人手操作力f_h进行重力补偿和滤波处理，生成与环境交互力f_e；

所述虚拟约束生成器用于根据虚拟夹具引导路径以及虚拟约束生成器对所述与环境交互力f_e进行约束，生成约束后的操作力；

所述边界约束生成器的计算模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数；

所述虚拟约束生成器的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵；

所述导纳控制器的计算模型为：

其中，f′_e为约束后的操作力，f_e是人手操作力经进行重力补偿和滤波处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明通过机器人轮廓误差描述虚拟夹具辅助操作精度，使虚拟夹具辅助操作精度能够直观合理的表示。

2.本发明所提出的虚拟夹具控制结构中的局部迭代轮廓误差估计方法能够高效高精度获得机器人末端与虚拟夹具引导路径的最邻近点。

3.本发明所提出的虚拟夹具控制结构，相较于传统参考方向法虚拟夹具控制结构，辅助示教精度有了显著提升，同时能在虚拟夹具引导路径上往复运动。

附图说明

图1是本发明优选实施例涉及的一种机器人操作高辅精度的虚拟夹具控制方法的流程图；

图2是本发明优选实施例涉及的一种机器人操作高辅精度的虚拟夹具控制方法的结构控制图；

图3是本发明优选实施例所构建的虚拟夹具辅助示教精度定义示意图，图3中的a为位置轮廓误差定义示意图，图3中的b为方向轮廓误差定义示意图；

图4是本发明优选实施例所构建的轮廓误差估计器中位置估计方法示意图；

图5是本发明优选实施例所构建的轮廓误差估计器中方向估计方法示意图，图5中的a为机器人末端坐标系相对于最近的虚拟夹具引导路径参考点的参考位姿，图5中的b为轮廓点

位于P_l与P_l-1的情况，图5中的c为轮廓点

位于P_l与P_l+1的情况。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1以及图2所示，本发明实施例提供的一种机器人操作高辅精度的虚拟夹具控制方法具体包括以下步骤：

步骤一，对牵引机器人过程中的人手操作力进行前处理，得到与环境交互力，并将该与环境交互力转换为机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿，机器人末端则根据期望速度和期望位姿沿虚拟夹具引导路径执行操作，输出机器人末端的实际速度和实际位姿。

具体而言，对牵引机器人过程中的人手操作力进行前处理的实现过程为，通过在机器人末端设置力传感器以及重力补偿与滤波器，其中，力传感器用于检车牵引机器人过程中的人手操作力，重力补偿与滤波器接收力传感器发送的信号，并对该信号进行滤波处理和重力补偿处理，以获取与环境交互力f_e。该与环境交互力f_e依次输入导纳控制器、逆运动学计算器、PID控制器，最后输出机器人末端实际速度和实际位姿。

更具体的，该力传感器为六维力传感器。

其中，未经虚拟约束前，导纳控制器的计算模型为：

其中，M为质量矩阵，D为阻尼矩阵，K为刚度矩阵，f_e是人手操作力f_h通过六维力传感器经平滑滤波与末端重力补偿获得的与环境交互力，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿。

步骤二，如图3以及图4所示，获取机器人末端执行操作过程中的实际位姿与虚拟夹具引导路径最近的参考点位姿，并以此生成机器人末端位置轮廓误差补偿量和方向轮廓误差补偿量。

具体而言，(1)获得距离机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_c，构建机器人末端实际位置轮廓误差模型：

ε_p＝P_c-P_a

其中，ε_p为建机器人位置轮廓误差补偿量，其方向由点P_a指向参考点P_c。

(2)利用等效转轴和等效转角构建机器人末端实际位置方向轮廓误差模型：

ε_o＝r_cε_o

其中，r_c＝[r_cx,r_cy,r_cz]^T为单位旋转轴向量，ε_o为绕轴r_c旋转的角度。

(3)根据所述机器人末端实际位置轮廓误差模型以及机器人末端实际位置方向轮廓误差模型，获取机器人末端实际位姿最近的参考位姿，并以此生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量。

更具体的，步骤(3)中，首先，获得离当前机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_l以及指定步长内虚拟夹具引导路径上与参考点P_l相邻的两点P_l-1和P_l+1。

其次，利用参考点P_l-1、P_l与P_l+1的二阶多项式曲线描述虚拟夹具引导路径的局部几何特征P(s)，其中s为曲线参数。

接着，根据机器人末端实际位置点P_a以及局部几何特征P(s)求解机器人末端估计的轮廓点P_c，该估计的轮廓点P_c的计算模型为：

其中，δ为曲线参数s的取值区间。

然后，根据所述估计的轮廓点P_c计算估计的机器人末端位置轮廓误差

然后，根据估计的轮廓点P_c、虚拟夹具引导路径上的参考位置点P_l-1、P_l和P_l+1以及参考方向Q_l-1、Q_l和Q_l+1以及四元数球面线性插值公式求解轮廓点方向

该轮廓点方向

的计算模型为：

其中，λ为距离系数，Slerp()为四元数球面线性插值公式。

最后，根据轮廓点方向

生成方向轮廓误差补偿量模型：

其中，

是估计的机器人末端方向轮廓误差的大小，

是估计的机器人末端方向轮廓误差的方向。

步骤三，根据机器人末端的实际位姿对虚拟夹具引导路径的曲线参数进行限位处理，生成边界约束模型。

其中，所述边界约束模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。

步骤四，根据虚拟夹具引导路径以及边界约束模型对所述与环境交互力进行约束，生成约束后的操作力。

其中，对与环境交互力进行约束的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵。

步骤五，根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量生成误差补偿修正力，然后根据约束后的操作力和误差补偿修正力对机器人末端的路径偏差进行在线补偿，并生成机器人末端执行操作的期望速度和期望位姿，重复步骤二至步骤五，使机器人能够在虚拟夹具引导路径上往复运动。

具体而言，首先根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量对机器人末端的实际速度和实际位姿进行在线补偿，获取误差补偿修正力；然后所述约束后的操作力和误差补偿修正力通过导纳控制转换为修正后的机器人末端的实际速度和实际位姿，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

其中，所述误差补偿修正力f_c的计算模型为：

所述导纳控制的计算模型为：

其中，K为刚度矩阵，B为对角刚度矩阵，X_p为虚拟夹具引导路径参考点的位姿，X_e机器人末端实际位置点的位姿，

为虚拟夹具引导路径参考点的速度，

机器人末端实际位置点的速度，f′_e为约束后的操作力，f_e是人手操作力经前处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

作为发明的优选方案，如图1所示是本文所提出的基于机器人轮廓误差补偿的虚拟夹具控制结构示意图，该控制结构分为虚拟夹具下的动觉示教，轮廓误差估计与分量补偿两部分，其具体包括如下步骤：

(1)构建人-机器人遥操作牵引系统，将人手操作力f_h通过导纳控制转换为机器人末端的位置或速度指令，使机器人完成牵引运动，在这个过程中，导纳控制模型为：

其中，M为质量矩阵，D为阻尼矩阵，K为刚度矩阵，f_e是人手操作力f_h通过六维力传感器经平滑滤波与末端重力补偿获得的力值，即为与环境交互力，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿。

(2)虚拟约束生成，通过虚拟夹具引导路径构造理想运动方向矩阵D_r与禁止运动方向矩阵D_t，其具体表示如下

D_r＝J(J^TJ)^-1J^T (2)

D_t＝I-D_r (3)

其中，J_s为虚拟夹具引导路径的运动学模型，是关于曲线参数s的函数，||·||为求取二范数运算操作。

(3)构建虚拟约束下的人-机器人自由牵引系统，将虚拟约束矩阵D_r与D_t与步骤(1)中的导纳控制模型结合，为操作者操作机器人过程中提供辅助。此系统控制模型为：

其中，f′_e为力f_e经虚拟约束生成模块后的操作力，k_t为虚拟夹具刚柔性系数，它的取值范围为0到1。本实例中k_t为0，操作者操作时，机器人末端只能沿着约束路径运动。

S4构建虚拟夹具辅助操作精度性能指标即机器人轮廓误差，具体包括机器人末端位置轮廓误差ε_p和方向轮廓误差ε_o

S401如图3中的a所示，定义机器人末端位置轮廓误差ε_p大小：

ε_p＝P_c-P_a (6)

其中，P_a表示机器人末端实际位置点，P_c表示虚拟夹具引导路径参考位置轮廓上离P_a最近的位置点，也称为轮廓点。

S402如图3中的b所示，定义机器人末端方向轮廓误差ε_o通过角/轴表示：

ε_o＝r_cε_o (7)

其中，r＝[r_cx,r_cy,r_cz]^T为关于坐标系O-xyz的单位旋转轴向量，表征方向，ε_o为绕轴r旋转的角度，表征大小。ε_o和r_c表达式为：

其中，r_mn是旋转矩阵R(ε_o,r)第m行第n列的元素，R(ε_o,r_c)的计算模型为：

其中，R_a表示机器人末端实际位置坐标系的旋转矩阵，R_c表示为参考位置点P_c所对应的参考方向。

S5构建机器人轮廓误差估计器，获得离机器人末端实际位姿最近的参考位姿，具体包括位置轮廓误差估计和方向轮廓误差估计：

S501如图4所示，通过全向查表的方式获得离当前机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_l，轮廓点P_c必定位于最近参考点P_l的前后即P_l-1与P_l之间或P_l与P_l+1之间。

S502利用参考点P_l-1、P_l与P_l+1的二阶多项式曲线近似描述路径的局部几何特征P(s)。其中，s为曲线参数。

S503求解估计机器人末端的轮廓点P_c，通过下式计算：

其中，δ为曲线参数s的取值区间，通过黄金分割法对式(11)进行求解，获得轮廓点

S504计算估计的位置轮廓误差

S505根据轮廓点P_c、虚拟夹具引导路径参考位置点P_l-1,P_l和P_l+1与参考方向(以单位四元数表征)Q_l-1,Q_l和Q_l+1以及四元数球面线性插值公式Slerp求解轮廓点方向

如图5所示。具体如下

其中，λ为距离系数。

S506计算估计的方向轮廓误差

其中

是估计的机器人末端方向轮廓误差的大小，

是估计的机器人方向轮廓误差的方向，它们通过式(8)与式(9)和旋转矩阵

联立解得，轮廓点处对应的旋转矩阵

由轮廓点方向

获得，即

其中q_c,w,q_c,i,q_c,j,q_c,k为轮廓点方向

的四个四元数参数。

S6将轮廓误差估计器融入上述虚拟夹具约束人-机器人自由牵引系统，即在机器人末端与虚拟夹具引导路径轮廓点之间增加一个弹簧阻尼系统，生成误差补偿修正力f_c经导纳控制器转换成机器人末端实际位置控制量输入，对虚拟夹具辅助操作过程中的路径偏差进行在线补偿。误差补偿修正力f_c与导纳控制的计算模型分别为：

其中，K是弹簧阻尼系统的对角刚度矩阵，B是弹簧阻尼系统的对角刚度矩阵,X_p与

分别为虚拟夹具引导路径参考点的位姿与速度，

与

分别表示当前机器人末端的实际位姿与速度。

S7加入虚拟夹具引导路劲的边界约束，使机器人能够在虚拟夹具引导路径上往复运动，曲线参数s满足规律具体为：

其中，s_max为由构造的虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。

本发明还提供了一种机器人操作高辅精度的虚拟夹具控制系统，包括虚拟夹具下的动觉示教模块以及轮廓误差估计与分量补偿模块，其中，

所述虚拟夹具下的动觉示教模块包括依次连接的动觉示教力输入单元、虚拟约束生成器、导纳控制器，其用于输入人手操作力f_h，最后输出机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿。

所述虚拟夹具下的动觉示教模块还包括逆运动学计算器和PID控制器，其用于根据期望速度和期望位姿控制机器人末端执行操作，并输出机器人末端的实际速度和实际位姿。

所述轮廓误差估计与分量补偿模块包括轮廓误差估计器以及边界约束生成器，其中所述轮廓误差估计器根据机器人末端实际速度和实际位姿生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量，并将该位置轮廓误差补偿量和方向轮廓误差补偿量生成差补偿修正力，输入至导纳控制器，所述边界约束生成器根据虚拟夹具引导路径所确定的曲线参数的上限位以及机器人末端的位置轮廓误差补偿量对虚拟约束生成器进行迭代更新，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

所述动觉示教力输入单元用于对人手操作力f_h进行重力补偿和滤波处理，生成与环境交互力f_e。

所述虚拟约束生成器用于根据虚拟夹具引导路径以及虚拟约束生成器对所述与环境交互力f_e进行约束，生成约束后的操作力。

所述边界约束生成器的计算模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。

所述虚拟约束生成器的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵。

所述导纳控制器的计算模型为：

其中，f′_e为约束后的操作力，f_e是人手操作力经进行重力补偿和滤波处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，包括以下步骤：

S1对牵引机器人过程中的人手操作力进行前处理，得到与环境交互力，并将该与环境交互力转换为机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿；

S2获取机器人末端执行操作过程中的实际位姿与虚拟夹具引导路径最近的参考点位姿，并以此生成机器人末端位置轮廓误差补偿量和方向轮廓误差补偿量；

S3根据机器人末端的实际位姿对虚拟夹具引导路径的曲线参数进行限位处理，生成边界约束模型；

S4根据虚拟夹具引导路径以及边界约束模型对所述与环境交互力进行约束，生成约束后的操作力；

S5根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量生成误差补偿修正力，然后根据约束后的操作力和误差补偿修正力对机器人末端的路径偏差进行在线补偿，并生成机器人末端执行操作的期望速度和期望位姿，重复步骤S2至步骤S5，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

2.根据权利要求1所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S1中，所述与环境交互力通过导纳控制转换为机器人末端的期望速度和期望位姿。

3.根据权利要求1所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S2具体包括以下步骤：

S21获得距离机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_c，构建机器人末端实际位置轮廓误差模型：

ε_p＝P_c-P_a

其中，ε_p为机器人位置轮廓误差补偿量，其方向由点P_a指向参考点P_c；

S22利用等效转轴和等效转角构建机器人末端实际位置方向轮廓误差模型：

ε_o＝r_cε_o

其中，r_c＝[r_cx,r_cy,r_cz]^T为单位旋转轴向量，ε_o为绕轴r_c旋转的角度；

S23根据所述机器人末端实际位置轮廓误差模型以及机器人末端实际位置方向轮廓误差模型，获取机器人末端实际位姿与虚拟夹具引导路径最近的参考点位姿，并以此生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量。

4.根据权利要求3所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S23具体包括以下步骤：

S231获得离当前机器人末端实际位置点P_a最近的虚拟夹具引导路径参考点P_l以及指定步长内虚拟夹具引导路径上与参考点P_l相邻的两点P_l-1和P_l+1；

S232利用参考点P_l-1、P_l与P_l+1的二阶多项式曲线描述虚拟夹具引导路径的局部几何特征P(s)，其中s为曲线参数；

S233根据机器人末端实际位置点P_a以及局部几何特征P(s)求解机器人末端估计的轮廓点P_c，该估计的轮廓点P_c的计算模型为：

其中，δ为曲线参数s的取值区间；

S234根据所述估计的轮廓点P_c计算估计的位置轮廓误差

S235根据估计的轮廓点P_c、虚拟夹具引导路径上的参考位置点P_l-1、P_l和P_l+1以及参考方向Q_l-1、Q_l和Q_l+1以及四元数球面线性插值公式求解轮廓点方向

该轮廓点方向

的计算模型为：

其中，λ为距离系数，Slerp()为四元数球面线性插值公式；

S236根据轮廓点方向

生成机器人末端的方向轮廓误差补偿量模型：

其中，

是估计的机器人末端方向轮廓误差的大小，

是估计的机器人末端方向轮廓误差的方向。

5.根据权利要求1所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S3中，对所述与环境交互力进行约束的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵。

6.根据权利要求2所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S5具体包括以下步骤：

S51根据机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量对机器人末端的实际速度和实际位姿进行在线补偿，获取误差补偿修正力；

S52所述约束后的操作力和误差补偿修正力通过导纳控制转换为修正后的机器人末端的期望速度和期望位姿，重复步骤S2至步骤S5，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

7.根据权利要求6所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S52中，所述误差补偿修正力f_c的计算模型为：

所述导纳控制的计算模型为：

其中，K为刚度矩阵，B为对角刚度矩阵，X_p为虚拟夹具引导路径参考点的位姿，X_e机器人末端实际位置点的位姿，

为虚拟夹具引导路径参考点的速度，

机器人末端实际位置点的速度，f_e′为约束后的操作力，f_e是人手操作力经前处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

8.根据权利要求1所述的一种机器人操作高辅精度的虚拟夹具控制方法，其特征在于，步骤S4中所述边界约束模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数。

9.一种机器人操作高辅精度的虚拟夹具控制系统，其特征在于，包括虚拟夹具下的动觉示教模块以及轮廓误差估计与分量补偿模块，其中，

所述虚拟夹具下的动觉示教模块包括依次连接的动觉示教力输入单元、虚拟约束生成器、导纳控制器，其用于输入人手操作力f_h，最后输出机器人末端沿虚拟夹具引导路径执行操作的期望速度和期望位姿；

所述虚拟夹具下的动觉示教模块还包括逆运动学计算器和PID控制器，其用于根据期望速度和期望位姿控制机器人末端执行操作，并输出机器人末端的实际速度和实际位姿；

所述轮廓误差估计与分量补偿模块包括轮廓误差估计器以及边界约束生成器，其中所述轮廓误差估计器根据机器人末端实际速度和实际位姿生成机器人末端的位置轮廓误差补偿量和方向轮廓误差补偿量，并将该位置轮廓误差补偿量和方向轮廓误差补偿量生成差补偿修正力，输入至导纳控制器，所述边界约束生成器根据虚拟夹具引导路径所确定的曲线参数的上限位以及机器人末端的位置轮廓误差补偿量对虚拟约束生成器进行迭代更新，以此方式，使机器人能够在虚拟夹具引导路径上往复运动。

10.根据权利要求9所述的一种机器人操作高辅精度的虚拟夹具控制系统，其特征在于，所述动觉示教力输入单元用于对人手操作力f_h进行重力补偿和滤波处理，生成与环境交互力f_e；

所述虚拟约束生成器用于根据虚拟夹具引导路径以及虚拟约束生成器对所述与环境交互力f_e进行约束，生成约束后的操作力；

所述边界约束生成器的计算模型为：

其中，s_max为由虚拟夹具引导路径所确定的曲线参数的上限位，s′为经边界约束后获得的曲线参数；

所述虚拟约束生成器的计算模型为：

D_r＝J(J^TJ)^-1J^T

D_t＝I-D_r

其中，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵，J_s为虚拟夹具引导路径的运动学模型，I为单位矩阵；

所述导纳控制器的计算模型为：

其中，f_e′为约束后的操作力，f_e是人手操作力经进行重力补偿和滤波处理后获得的与环境交互力，k_t为虚拟夹具的刚柔性系数，其取值范围为0到1，M为质量矩阵，D为阻尼矩阵，

为机器人末端的加速度，

为机器人末端的速度，x为机器人末端的位姿，D_r为理想运动方向矩阵，D_t为禁止运动方向矩阵。

Images