CN109968361B - 一种基于实时力反馈的变阻抗遥操作控制装置及方法 - Google Patents

Abstract

本发明公开一种基于实时力反馈的变阻抗遥操作控制装置及方法。该装置中，主端姿态传感器设置在主端力反馈设备上；从端姿态传感器和从端力传感器设置在从端机械臂的末端；从端力传感器的信号输出端分别与阻抗控制器的信号输入端和卡尔曼滤波装置的信号输入端连接；卡尔曼滤波装置的信号输出端与主端力反馈设备的信号输入端连接；主端姿态传感器、从端姿态传感器和阻抗参数输入模块的信号输出端均与阻抗控制器的信号输入端连接；阻抗控制器的控制输出端与从端机械臂的控制输入端连接。本发明的基于实时力反馈的变阻抗遥操作控制装置及方法在环境变化时无需重新对阻抗参数进行整定，从而能够适应应用环境的变化。

Description

一种基于实时力反馈的变阻抗遥操作控制装置及方法

技术领域

本发明涉及遥操作领域，特别是涉及一种实时力反馈的变阻抗遥操作控制装置及方法。

背景技术

遥操作系统是一类典型的人机交互系统，广泛应用于核电厂、外太空探索以及远程医疗等领域。一个典型的遥操作系统包括位于主端的操作者以及力反馈设备和位于从端的受控机械臂。主端向从端机械臂发送运动指令并接收从端机械臂反馈的力信息，通过力反馈设备向操作者提供真实的触觉和力觉临场感，机械臂在操作者控制下在远端环境中完成复杂任务，仿佛操作者肢体的延伸。

为了保证从端机械臂的安全以及灵活性，从端机械臂的控制通常会采用阻抗控制的方法。采用阻抗控制，可以保证当从端机械臂与外界环境产生接触力时，使从端机械臂具有一定的柔顺性，从而保证从端机械臂的安全。

目前从端机械臂的阻抗控制方法主要是调整阻抗控制器的阻抗参数为固定阻抗参数，该控制方法是针对某个特定的工作环境，整定好阻抗控制器中的惯量M、阻尼B以及刚度K并固定。这种方法通常能适应该特定的场景，但是当环境改变时，控制器的性能往往会变差，达不到预期的效果。如果要保证工作性能，那么阻抗参数需要针对新的环境重新整定，无法适应应用环境的变化。

发明内容

本发明的目的是提供一种基于实时力反馈的变阻抗遥操作控制装置及方法，在环境变化时无需重新对阻抗参数进行整定，从而适应应用环境的变化。

为实现上述目的，本发明提供了如下方案：

一种基于实时力反馈的变阻抗遥操作控制装置，包括：从端机械臂、阻抗参数输入模块、阻抗控制器、主端力反馈设备、卡尔曼滤波装置、主端姿态传感器、从端姿态传感器和从端力传感器；

所述主端姿态传感器设置在所述主端力反馈设备上；所述从端姿态传感器和所述从端力传感器设置在所述从端机械臂的末端；所述从端力传感器的信号输出端分别与所述阻抗控制器的信号输入端和所述卡尔曼滤波装置的信号输入端连接；所述卡尔曼滤波装置的信号输出端与所述主端力反馈设备的信号输入端连接；所述主端姿态传感器、所述从端姿态传感器和所述阻抗参数输入模块的信号输出端均与所述阻抗控制器的信号输入端连接；所述阻抗控制器的控制输出端与所述从端机械臂的控制输入端连接；

所述主端姿态传感器用于采集所述主端力反馈设备的运动姿态得到主设备姿态；

所述从端姿态传感器用于采集所述从端机械臂的运动姿态，得到从设备姿态；

所述从端力传感器用于采集所述从端机械臂末端与环境的接触力，得到实测接触力；

所述卡尔曼滤波装置用于根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力；

所述主端力反馈设备用于根据所述预测接触力向用户再现所述从端机械臂的与环境的接触力；

所述阻抗参数输入模块用于采集用户选择的阻抗参数；

所述阻抗控制器用于根据所述主设备姿态、所述从设备姿态、所述阻抗参数以及所述实测接触力生成控制所述从端机械臂运动的控制信号；

所述从端机械臂根据所述控制信号执行动作。

可选的，所述主端力反馈设备为Omega.6型力反馈设备。

可选的，所述从端机械臂为UR3型多自由度串联机械臂。

可选的，所述从端力传感器为FT300型六维力传感器。

本发明还公开一种基于实时力反馈的变阻抗遥操作控制方法，应用于上述的基于实时力反馈的变阻抗遥操作控制装置；

该控制方法包括：

阻抗控制器和卡尔曼滤波装置获取从端力传感器采集的从端机械臂末端与环境的实测接触力；

阻抗控制器获取从端姿态传感器采集的从端机械臂的运动姿态，得到从设备姿态；

卡尔曼滤波装置根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力；

主端力反馈设备根据所述预测接触力再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户；

阻抗控制器获取主端姿态传感器采集的主端力反馈设备在用户的带动下运动时的运动姿态，得到主设备姿态；

阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数；

阻抗控制器根据所述主设备姿态、所述从设备姿态、所述阻抗参数和所述实测接触力生成对从端机械臂进行控制的控制指令；

从端机械臂根据所述控制指令运动。

可选的，在卡尔曼滤波装置根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力之后，在主端力反馈设备根据所述预测接触力再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户之前，还包括：

根据所述主端力反馈设备与所述从端机械臂的结构差异建立所述从端机械臂与所述主端力反馈设备之间的坐标映射关系。

可选的，所述阻抗参数包括期望惯性、期望阻尼和理想刚度。

可选的，所述阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数，具体包括：

阻抗参数输入模块采集用户选择的阻抗参数类型中的一种，得到选中类型；所述阻抗参数类型包括阻抗值由低到高的第一阻抗类型、第二阻抗类型、第三阻抗类型、第四阻抗类型、第五阻抗类型和第六阻抗类型；

阻抗参数输入模块将所述选中类型对应的阻抗参数值输出到所述阻抗控制器。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明所公开的基于实时力反馈的变阻抗遥操作控制方法，通过获取用户输入的阻抗参数来实现阻抗参数的选择和变化，从而能够根据用户的意图来选择适当的阻抗参数，当环境发生变化时，只需用户的意图发生变化即可适应环境的变化，因此在环境变化时无需重新对阻抗参数进行整定，从而适应应用环境的变化。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例1基于实时力反馈的变阻抗遥操作控制装置的结构框图；

图2为本发明实施例2基于实时力反馈的变阻抗遥操作控制方法的控制过程的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1：

图1为本发明实施例1基于实时力反馈的变阻抗遥操作控制装置的结构框图。

参见图1，该基于实时力反馈的变阻抗遥操作控制装置，包括：主端设备、阻抗控制器7、卡尔曼滤波装置8和从端设备。主端设备包括阻抗参数输入模块1、主端姿态传感器2和主端力反馈设备3；从端设备包括：从端姿态传感器4、从端机械臂5和从端力传感器6；

所述主端姿态传感器2设置在所述主端力反馈设备3上；所述从端姿态传感器4和所述从端力传感器6设置在所述从端机械臂5的末端；所述从端力传感器6的信号输出端分别与所述阻抗控制器7的信号输入端和所述卡尔曼滤波装置8的信号输入端连接；所述卡尔曼滤波装置8的信号输出端与所述主端力反馈设备3的信号输入端连接；所述主端姿态传感器2、所述从端姿态传感器4和所述阻抗参数输入模块1的信号输出端均与所述阻抗控制器7的信号输入端连接；所述阻抗控制器7的控制输出端与所述从端机械臂5的控制输入端连接；

所述主端姿态传感器2用于采集所述主端力反馈设备3的运动姿态得到主设备姿态；

所述从端姿态传感器4用于采集所述从端机械臂5的运动姿态，得到从设备姿态；

所述从端力传感器6用于采集所述从端机械臂5末端与环境的接触力，得到实测接触力；

所述卡尔曼滤波装置8用于根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力；

所述主端力反馈设备3用于根据所述预测接触力向用户再现所述从端机械臂5的与环境的接触力；

所述阻抗参数输入模块1用于采集用户选择的阻抗参数；

所述阻抗控制器7用于根据所述主设备姿态、所述从设备姿态、所述阻抗参数以及所述实测接触力生成控制所述从端机械臂5运动的控制信号；

所述从端机械臂5根据所述控制信号执行动作。

可选的，所述主端力反馈设备3为Omega.6型力反馈设备。

可选的，所述从端机械臂5为Universal Robot公司的UR3型多自由度串联机械臂。

可选的，所述从端力传感器6为FT300型六维力传感器。

实施例2：

实施例2公开一种基于实时力反馈的变阻抗遥操作控制方法，该控制方法应用于实施例1的基于实时力反馈的变阻抗遥操作控制装置。

该控制方法包括：整定过程和控制过程。

一、整定过程：

整定过程为在控制过程执行之前对阻抗控制器的阻抗参数进行预整定。该整定过程完成后将不再进行整定。

对从端机械臂进行控制的阻抗控制器的控制表达式为

式中：

表示期望轨迹。

X_r为期望的从端机械臂末端的位置坐标。

X,

反映从端机械臂的末端位置、速度以及加速度。

M_d,B_d,K_d均为阻抗参数，分别表示期望惯性、期望阻尼和理想刚度。

F表示从端机械臂的末端与环境的接触力。

阻抗控制本质是通过输入接触力F，计算出所需的机械臂末端的位置偏差X-X_r。采用阻抗控制首先要选择适宜的阻抗参数，从而赋予机械臂期望的柔顺性能。M_d表示期望惯性，该项对具有较大加速度的运动表现出较大惯性；B_d为期望阻尼，速度越大，该项影响越大；K_d为理想刚度，在偏离期望位置时能够表现出弹簧特性。这些参数可以被设置为定值或者是变参数来调整机械臂的柔顺性能。而本发明正是要根据外界环境的不同由用户主动地调整这三个参数，使从端机械臂在不同的环境中根据任务需求表现出不同的柔顺性能。

预先整定六个类型的阻抗参数，六个阻抗参数类型包括阻抗值由低到高的第一阻抗类型、第二阻抗类型、第三阻抗类型、第四阻抗类型、第五阻抗类型和第六阻抗类型。这六个阻抗参数类型能够使从端机械臂的柔顺特性涵盖极低阻抗，低阻抗，较低阻抗，较高阻抗，高阻抗，极高阻抗六种。这六个阻抗参数类型约束了阻抗控制器的阻抗参数变化的范围，不同的外界环境对应阻抗控制器中不同的阻抗参数类型。这样保证用户能够根据外界环境的不同而主动地调整从端机械臂的阻抗参数，使从端机械臂更加灵活地完成复杂任务。

二、控制过程：

图2为本发明实施例2基于实时力反馈的变阻抗遥操作控制方法的控制过程的流程图。

参见图2，该控制过程包括以下控制步骤：

步骤201：阻抗控制器和卡尔曼滤波装置获取从端力传感器采集的从端机械臂末端与环境的实测接触力。

步骤202：阻抗控制器获取从端姿态传感器采集的从端机械臂的运动姿态，得到从设备姿态。

步骤203：卡尔曼滤波装置根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力。

本发明中从端机械臂的反馈力需要进行处理以提高力反馈的实时性。基于离散卡尔曼滤波算法对下一时刻的接触力进行估计，以降低时延的影响。首先建立从端机械臂的空间直角坐标系，并建立从端机械臂末端接触力的状态空间方程，从端机械臂末端的运动模型如式(2)所示。

式中，z_s是从端机械臂末端的Z轴位置，v_s是从端机械臂末端的速度，a_s是从端机械臂末端的加速度，T_s为采样周期。设预测区间为N_p，那么从第k+1时刻到第k+N_p时刻的预测序列可由式(3)描述。

式中Σ_zs＝[1 … 1]^T，

Γ_zs＝[T_s 2T_s … N_pT_s]^T，

从端机械臂末端运动的状态空间模型，可以由式(4)与式(5)描述。式中z指的是从端机械臂的运动状态；f_sl是测量到的从端机械臂与环境的接触力；z_p＝[z_sp 0]是目标状态矢量，z_sp是从端机械臂发生接触作用的目标的位置。

z[k+1]＝[z_s[k+1] v_s[k+1]]^T＝Az[k]+Bu[k] (4)

结合式(4)与式(5)，可以得到

K是环境刚度。

接触力可以通过从端力传感器测量得到，卡尔曼滤波装置通过已接收到的接触力数据预测下一时刻的状态，并以此预测值作为实时的力反馈值。定义从端机械臂末端运动状态矢量z作为用于估计离散时间过程的状态变量，f_sl为待观测变量，则预测接触力的离散卡尔曼算法表达式如式(6)和式(7)所示。

式中，Q为过程噪声协方差矩阵，

与

为不同时刻状态变量的估计值，u_k为控制变量，P_k∣k-1与P_k-1∣k-1不同时刻的协方差；式(6)与式(7)描述了将状态估计以及协方差估计从k-1时刻推进到k时刻的过程。同时令Φ_k＝A，B_k＝B，测量更新方程如式(8)和式(9)描述。

P_k∣k＝P_k∣k-1-K_kH_kP_k∣k-1 (9)

式中，f_k是测量到的远端接触力，矩阵H_k＝C_z，P_k∣k是协方差估计值，

为末端位置估计值，K_k是Kalman增益，可通过式(10)计算得到。

求解式(8)，得到当前状态的最佳矢量估计，R_k是信号噪声协方差通过式(11)得到下一时刻的状态矢量的估计值，反馈的接触力的预测值可由式(5)计算得到。

在进行反馈力预测时，首先，确定Kalman算法在零时刻的状态变量z₀，协方差p₀与Q与R_k的值；接下来根据式(6)与式(7)和上一时刻的z_k-1、p_k-1计算出下一时刻的状态估计与协方差估计。然后计算卡尔曼增益K_k，根据时间更新方程的结果计算P_k∣k，结合从端力传感器得到的力的测量值，可以计算该时刻的最优力的估计值，通过更新k时刻的协方差估计P_k∣k使得算法进入下一个循环，并递推运算下去。

步骤204：根据所述主端力反馈设备与所述从端机械臂的结构差异建立所述从端机械臂与所述主端力反馈设备之间的坐标映射关系。

该步骤为主端设备与从端设备的工作空间映射。从端力传感器反馈的力作为主端力反馈设备的输入，主端力反馈设备通过设备上的力矩电机以及高性能的触觉渲染计算，能够为用户提供高度逼真的力反馈体验，用户通过握住主端力反馈设备的末端获得真实的力反馈。

因为主端力反馈设备与从端机械臂的机械设计结构不同，需要对两者进行工作空间映射。映射关系为

式中，x_s、y_s和z_s代表映射后的位置坐标，x_m、y_m和z_m代表主端力反馈设备末端的坐标，k_x、k_y和k_z为映射系数，b_x、b_y和b_z为偏移量。可根据Omega.6与UR3的实际参数，分别计算X,Y,Z三轴的主从坐标映射系数与偏移量。

步骤205：主端力反馈设备根据所述预测接触力再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户。

用户根据力反馈的信息，对远端环境的刚度做出判断，同时根据环境刚度调整阻抗控制器的阻抗参数。如果远端的环境被用户判断为高刚度，那么用户可以将阻抗参数确定为低阻抗的类型；反之如果远端的环境是低刚度的，用户可以将阻抗参数确定为有高阻抗的类型。根据用户对远端环境刚度的判断，确定阻抗控制器中的惯量M_d、阻尼B_d以及刚度K_d。

步骤206：阻抗控制器获取主端姿态传感器采集的主端力反馈设备在用户的带动下运动时的运动姿态，得到主设备姿态；

步骤207：阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数；

步骤208：阻抗控制器根据所述主设备姿态、所述从设备姿态、所述阻抗参数和所述实测接触力生成对从端机械臂进行控制的控制指令；

步骤209：从端机械臂根据所述控制指令运动。

可选的，所述阻抗参数包括期望惯性、期望阻尼和理想刚度。

可选的，所述阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数，具体包括：

阻抗参数输入模块采集用户选择的阻抗参数类型中的一种，得到选中类型；所述阻抗参数类型包括阻抗值由低到高的第一阻抗类型、第二阻抗类型、第三阻抗类型、第四阻抗类型、第五阻抗类型和第六阻抗类型；

阻抗参数输入模块将所述选中类型对应的阻抗参数值输出到所述阻抗控制器。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明所公开的基于实时力反馈的变阻抗遥操作控制方法，通过获取用户输入的阻抗参数来实现阻抗参数的选择和变化，从而能够根据用户的意图来选择适当的阻抗参数，当环境发生变化时，只需用户的意图发生变化即可适应环境的变化，因此在环境变化时无需重新对阻抗参数进行整定，从而适应应用环境的变化。

另外，采用基于卡尔曼滤波算法的反馈力预测，有效地解决了由于通信时延造成的系统实时性下降的问题；从端机械臂采用参数可变的阻抗控制，更加的灵活和更好地适应各种环境，从而根据外部环境的刚度进行切换。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (5)

1.一种基于实时力反馈的变阻抗遥操作控制装置，其特征在于，包括：从端机械臂、阻抗参数输入模块、阻抗控制器、主端力反馈设备、卡尔曼滤波装置、主端姿态传感器、从端姿态传感器和从端力传感器；

所述主端姿态传感器设置在所述主端力反馈设备上；所述从端姿态传感器和所述从端力传感器设置在所述从端机械臂的末端；所述从端力传感器的信号输出端分别与所述阻抗控制器的信号输入端和所述卡尔曼滤波装置的信号输入端连接；所述卡尔曼滤波装置的信号输出端与所述主端力反馈设备的信号输入端连接；所述主端姿态传感器、所述从端姿态传感器和所述阻抗参数输入模块的信号输出端均与所述阻抗控制器的信号输入端连接；所述阻抗控制器的控制输出端与所述从端机械臂的控制输入端连接；

所述主端姿态传感器用于采集所述主端力反馈设备的运动姿态得到主设备姿态；

所述从端姿态传感器用于采集所述从端机械臂的运动姿态，得到从设备姿态；

所述从端力传感器用于采集所述从端机械臂末端与环境的接触力，得到实测接触力；

所述卡尔曼滤波装置用于根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力；

所述主端力反馈设备用于根据所述预测接触力向用户再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户，用户根据力反馈的信息，对远端环境的刚度做出判断，同时根据环境刚度调整所述阻抗控制器的阻抗参数；

所述阻抗参数输入模块用于采集用户选择的阻抗参数；

所述阻抗控制器用于根据所述主设备姿态、所述从设备姿态、所述阻抗参数以及所述实测接触力生成控制所述从端机械臂运动的控制信号；

所述从端机械臂根据所述控制信号执行动作；

所述主端力反馈设备为Omega.6型力反馈设备；所述从端机械臂为UR3型多自由度串联机械臂；所述从端力传感器为FT300型六维力传感器。

2.一种基于实时力反馈的变阻抗遥操作控制方法，其特征在于，该方法应用于如权利要求1所述的基于实时力反馈的变阻抗遥操作控制装置；

该控制方法包括：

阻抗控制器和卡尔曼滤波装置获取从端力传感器采集的从端机械臂末端与环境的实测接触力；

阻抗控制器获取从端姿态传感器采集的从端机械臂的运动姿态，得到从设备姿态；

卡尔曼滤波装置根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力；

主端力反馈设备根据所述预测接触力再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户；

阻抗控制器获取主端姿态传感器采集的主端力反馈设备在用户的带动下运动时的运动姿态，得到主设备姿态；

阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数；

阻抗控制器根据所述主设备姿态、所述从设备姿态、所述阻抗参数和所述实测接触力生成对从端机械臂进行控制的控制指令；

从端机械臂根据所述控制指令运动。

3.根据权利要求2所述的基于实时力反馈的变阻抗遥操作控制方法，其特征在于，在卡尔曼滤波装置根据所述实测接触力通过卡尔曼滤波算法对下一时刻的接触力进行预测，得到预测接触力之后，在主端力反馈设备根据所述预测接触力再现所述从端机械臂的与环境的接触力，从而将所述预测接触力反馈给用户之前，还包括：

根据所述主端力反馈设备与所述从端机械臂的结构差异建立所述从端机械臂与所述主端力反馈设备之间的坐标映射关系。

4.根据权利要求2所述的基于实时力反馈的变阻抗遥操作控制方法，其特征在于，所述阻抗参数包括期望惯性、期望阻尼和理想刚度。

5.根据权利要求4所述的基于实时力反馈的变阻抗遥操作控制方法，其特征在于，所述阻抗控制器获取阻抗参数输入模块采集的用户选择的阻抗参数，具体包括：

阻抗参数输入模块采集用户选择的阻抗参数类型中的一种，得到选中类型；所述阻抗参数类型包括阻抗值由低到高的第一阻抗类型、第二阻抗类型、第三阻抗类型、第四阻抗类型、第五阻抗类型和第六阻抗类型；

阻抗参数输入模块将所述选中类型对应的阻抗参数值输出到所述阻抗控制器。

Images