CN104440864A - 一种主从式遥操作工业机器人系统及其控制方法 - Google Patents

Abstract

本发明公开了一种主从式遥操作工业机器人系统及其控制方法，所述系统包括主机器人、与主机器人相连的主从控制系统、及与控制系统相连的从机器人，所述主机器人包括力反馈设备，操作人员将主机器人运动指令通过主从控制系统传递给从机器人，从机器人跟随主机器人运动，进而完成对任务对象的操作，同时从机器人将任务状态反馈给操作人员。本发明通过分析主手和从手的几何构型，设计了主从运动映射策略，采用了基于逆雅克比矩阵主从控制算法，并通过比例-微分控制环节来消除主从位置跟随误差，为了消除操作者手部抖动对手术机器人精度影响，设计了低通数字滤波器来滤除手部颤动。

Description

一种主从式遥操作工业机器人系统及其控制方法

技术领域

本发明涉及时工业机器人技术领域，尤其涉及一种主从式遥操作工业机器人系统及其控制方法。

背景技术

本发明涉及工业用机器人的主从遥操作运动控制及遥操作示教再现方法，主要用于制造加工业、自动化控制及其配套产品生产领域，可以高效准确地生成作业序列，优化操作路径，使得机器人动作过程可以平稳的重现。

现代工业快速多变以及日益复杂性，广泛采用工业机器人，不仅可提高产品的质量与数量，而目对保障人身安全，减轻劳动强度，提高劳动生产率，降低生产成本，有着十分重要的意义。机器人作为能作为一个开放并具有友好的人机交互功能的可编程、可重构制造单元融合到制造业系统中。

示教技术通过示教盒或软件，根据任务需要实现对机器人作业任务的编程。示教再现方法的优点是简单、容易实现。通常机器人采用的示教方法是示教盒，操作人员通过按示教盒上相应的命令按钮移动机器人的每个关节，直至所有关节的运动组合产生所需的目标示教姿态。

中国专利申请201210059636.4公开了一种具备总线通信的机器人示教盒及其控制方法，提出了一种基于总线的可远离机器人本体进行远程操作的、可对机器人运行参数进行监控的机器人示教盒。中国专利申请201120293768.4针对目前国内外使用的有线示教盒的拖线易断及控制距离受限的问题，提出一种应用于机器人系统上的无线遥控示教器由无线遥控示教器，来解决这些问题。

现有技术中的示教方法需要操作人员通过按示教盒上相应的命令按钮移动机器人的每个关节，直至所有关节的运动组合产生所需的目标示教姿态，操作复杂、功能单一、操作灵活性差、示教次数多、步进时冲击大等缺点，对于操作者来说也十分不便，需要记住控制每个关节正反转的按钮，给操作者造成生理和心理上沉重的负担。基于笛卡尔空间的位姿示教需要操作者事先知道机器人的基坐标系，逐个按下各方向的命令按钮，直至各运动组合产生所需的目标姿态，操作非常不便。目前还没有一种直观的控制方式能够使得操作人员在笛卡尔空间轻松的控制从机器人到达期望的示教位姿。

因此，针对上述技术问题，有必要提供一种主从式遥操作工业机器人系统及其控制方法。

发明内容

有鉴于此，本发明的目的在于提供一种主从式遥操作工业机器人系统及其控制方法，通过分析主手和从手的几何构型，设计了主从运动映射策略，采用了基于逆雅克比矩阵主从控制算法，并通过比例-微分控制环节来消除主从位置跟随误差，为了消除操作者手部抖动对手术机器人精度影响，设计了低通数字滤波器来滤除手部颤动。

为了达到上述目的，本发明实施例提供的技术方案如下：

一种主从式遥操作工业机器人系统，所述系统包括主机器人、与主机器人相连的主从控制系统、及与控制系统相连的从机器人，所述主机器人包括力反馈设备，操作人员将主机器人运动指令通过主从控制系统传递给从机器人，从机器人跟随主机器人运动，进而完成对任务对象的操作，同时从机器人将任务状态反馈给操作人员。

作为本发明的进一步改进，所述主从控制系统包括：

工业PC机，通过IEEE 1394接口与主机器人相连；

光学跟踪器，与工业PC机相连，所述工业PC机通过网卡与光学跟踪器进行数据交换；

运动控制器，与工业PC机通过网卡相连，运动控制器接受主机器人的操作命令，并对操作命令进行处理生成运动指令发送给从机器人。

作为本发明的进一步改进，所述从机器人包括若干一一相连的伺服放大器和伺服电机，所述伺服放大器接受运动控制器发送的运动指令，放大后发送至相应的伺服电机，伺服电机根据运动指令完成相应的动作。

作为本发明的进一步改进，所述从机器人还包括若干与伺服放大器相连的编码器，主从控制系统还包括信号转换单元，信号转换单元用于对编码器的初始值进行设定。

作为本发明的进一步改进，所述信号转换单元通过RS232接口与工业PC机相连，信号转换单元通过RS422接口与伺服放大器相连。

相应地，一种主从式遥操作工业机器人控制方法，所述方法包括：

S1、获取主机器人中主手各关节的位置信息和速度信息，经过主手正运动学计算得到主手各关节在笛卡尔空间的位置信息和速度信息；

S2、经过主从比例映射后得到从机器人从手末端在笛卡尔空间的速度信息，再经过从手逆雅可比矩阵变换后得到从手各关节的速度信息；

S3、经过关节速度限制运算后输出控制信号控制各伺服电机运动；

S4、引入比例-微分控制环节来消除主从手位置跟随误差。

作为本发明的进一步改进，所述步骤S2中，经过主从比例映射后，主从手位置跟随误差e为：

e＝(X_m-X_m0)-K(X_s-X_s0)；

主从手末端速度误差为：

$\stackrel{\·}{e}={\stackrel{\·}{X}}_m-K{\stackrel{\·}{X}}_s;$

其中，X_m和X_s分别为主手和从手末端执行器在笛卡尔空间坐标中的位置坐标，X_m0和X_s0分别为主从手末端执行器的初始位置，和分别为主从手末端执行器的笛卡尔速度矢量，K为3×3主从映射对角系数矩阵。

作为本发明的进一步改进，所述步骤S4中“消除主从手位置跟随误差”通过误差控制方程实现，相应的误差控制方程为：

$K_{p}e+K_d\stackrel{\·}{e}=0;$

K_p和K_d分别为3×3比例、微分系数矩阵。

作为本发明的进一步改进，所述步骤S2中从手末端在笛卡尔空间的速度信息为：

${\stackrel{\·}{X}}_s=K^{-1}{\stackrel{\·}{X}}_m+{\left(K_{d}K\right)}^{-1}{K}_p((X_m-X_{m0})-K(X_s-X_{s0}));$

从机器人关节角速度信息为：

${\stackrel{\·}{\mathrm{\θ}}}_s=J_s^{-1}\left({\mathrm{\θ}}_s\right){\stackrel{\·}{X}}_s;$

其中，θ_s和分别表示从机器人关节角度和关节角速度矢量；为从机器人逆雅克比矩阵。

作为本发明的进一步改进，所述方法还包括：

主从控制过程中，实时检测从手雅可比矩阵的奇异性，当从机器人某位形灵活性下降时则切换到关节映射模式：

${\stackrel{\·}{\mathrm{\θ}}}_s=K_r{\stackrel{\·}{\mathrm{\θ}}}_m;$

K_r为5×5主从关节速度映射系数矩阵。

本发明具有以下有益效果：

操作人员可以通过主机器人末端操作杆控制从机器人运动到期望的示教位姿，这种方式符合操作者的习惯，不需要通过间接控制各个关节使机器人到达示教位姿；

能够用于制造业自动化生产，示教模块实时记录机器人运行轨迹，优化操作路径，高效准确地生成控制指令序列与示教文件，使得机器人动作过程可以平稳的重现。

附图说明

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

图1为本发明一种主从式遥操作工业机器人系统的原理图。

图2为本发明一具体实施方式中主从式遥操作工业机器人系统的结构示意图。

图3为本发明一具体实施方式中主手和6R机器人几何构型及连杆坐标系示意图。

图4为本发明一具体实施方式中主从式遥操作工业机器人控制方法的原理图。

具体实施方式

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

本发明主从式遥操作工业机器人系统由控制端和操作端两大部分组成。控制端主要由PC机和主机器人构成，主机器人采用通用的力反馈设备；操作端主要指完成示教再现功能的从机器人，从手为通用的工业机器人。通过分析主手和从手的几何构型，设计了主从运动映射策略，采用了基于逆雅克比矩阵主从控制算法，并通过比例-微分控制环节来消除主从位置跟随误差，为了消除操作者手部抖动对手术机器人精度影响，设计了低通数字滤波器来滤除手部颤动。工作时，操作人员在控制端完成操作端从机器人的运动控制和状态监控，从机器人直接与操作对象进行交互。

示教再现模块通过操作端PC机示教再现界面上完成，操作人员通过控制端主机器人驱动从机器人完成相应动作，示教时通过主手上的使能按钮切换工作模式，自动记录机器人作业位姿、运动参数等，经过平滑及插值后优化操作路径，生成控制指令序列与示教文件，使得机器人动作过程可以平稳的重现。

参图1所示，一个典型的遥操作系统包括操作人员、主机器人、控制系统、从机器人和操作环境五部分。本发明主从遥操作示教再现机器人系统由控制端和操作端两大部分组成。控制端主要由PC机和主机器人构成，操作端主要指完成示教功能的从机器人。操作者将主手运动指令通过主从控制系统传递给从执行器，从手快速、精确地跟随主手运动，进而完成对任务对象的操作，同时监控设备将任务状态反馈给操作者，有效地指导操作。6R机器人控制系统以台式PC机为开发平台，采用上、下位机的控制方式。

结合图2所示，本发明的一具体实施方式中的一种主从式遥操作工业机器人系统，包括主机器人、与主机器人相连的主从控制系统、及与控制系统相连的从机器人，主机器人包括力反馈设备，操作人员将主机器人运动指令通过主从控制系统传递给从机器人，从机器人跟随主机器人运动，进而完成对任务对象的操作，同时从机器人将任务状态反馈给操作人员。本实施方式中主机器人采用Sensable Technology公司的Phantom Omni力反馈设备，在其他实施方式中也可以替换为desktop设备或ForceDimension公司的Omega和Sigma系列的产品

其中，主从控制系统包括：

工业PC机，通过IEEE 1394接口与主机器人相连；

光学跟踪器，与工业PC机相连，工业PC机通过网卡与光学跟踪器进行数据交换；

运动控制器，与工业PC机通过网卡相连，运动控制器接受主机器人的操作命令，并对操作命令进行处理生成运动指令发送给从机器人。

从机器人包括若干一一相连的伺服放大器和伺服电机，伺服放大器接受运动控制器发送的运动指令，放大后发送至相应的伺服电机，伺服电机根据运动指令完成相应的动作。从机器人还包括若干与伺服放大器相连的编码器，主从控制系统还包括信号转换单元，信号转换单元用于对编码器的初始值进行设定。信号转换单元通过RS232接口与工业PC机相连，信号转换单元通过RS422接口与伺服放大器相连。本实施方式中的从机器人采用的教学型的6R串联机器人，在其他实施方式中也可以是ABB、FANUC、Yaskawa、KUKA等机器人、新松机器人等。

控制系统的上位机通过外围硬件接口和总线将主机器人、运动控制器、数据采集卡等联系起来，同时通过网卡与光学跟踪器之间进行数据交换。控制系统的上位机将主机器人的操作命令传递给主控机，主控机接收机器人运动状态并通过控制算法对操作命令进行处理；生成的机器人运动指令通过以太网发送给运动控制卡；控制系统下位机的运动控制卡接收主控机的控制命令，最终通过驱动伺服系统使得从机器人完成相应的动作；下位机将当前从机器人各关节的编码器值等上行数据发送到主控端PC机；此外，PC机通过RS232/RS422端口与伺服放大器连接并对绝对编码器的初始值进行设定，以此作为机器人的零点位置。

针对本系统，主从控制实质是指通过操作Phantom主手来控制从手执行器末端，使得从机器人位置能够跟随主手运动。图3为本实施方式中主机器人和从机器人(主手和从手)的几何构型，Phantom主手为6R结构，基座三个关节控制腕部中心位置，执行末端3个旋转关节控制姿态，从机器人为6R结构。由于主从机器人在机械机构、运动学及动力学行为等方面存在很大的差异，即主手与从手存在明显的“异构”问题。异构机器人的主从控制包括主从手操作空间的匹配，主从手之间运动信息的交互反馈控制。主从控制策略的选取必须满足运动一致性原则，实现手术工具末端的运动与操作者所操作的手柄运动方向完全一致，姿态完全对应的直觉运动控制。

本发明的主从式遥操作工业机器人控制方法包括：

S1、获取主机器人中主手各关节的位置信息和速度信息，经过主手正运动学计算得到主手各关节在笛卡尔空间的位置信息和速度信息；

S2、经过主从比例映射后得到从机器人从手末端在笛卡尔空间的速度信息，再经过从手逆雅可比矩阵变换后得到从手各关节的速度信息；

S3、经过关节速度限制运算后输出控制信号控制各伺服电机运动；

S4、引入比例-微分控制环节来消除主从手位置跟随误差。

本发明的主从控制映射过程为：主手控制器获取获取主机器人中主手各关节的位置信息和速度信息，经过主手正运动学计算，得到主手各关节在笛卡尔空间的位置信息和速度信息，经主从比例映射后得到从机器人从手末端在笛卡尔空间的速度信息，再经过从手逆雅可比矩阵变换后得到从手各关节的速度信息，经过关节速度限制运算后输出控制信号控制各伺服电机运动，并引入比例-微分控制环节来消除主从手位置跟随误差。

参图4所示，在本发明的一具体实施方式中，X_m和X_s分别表示主手和从手末端执行器在笛卡尔空间坐标中的位置坐标；和分别为主从手末端执行器的笛卡尔速度矢量；θ_m和分别表示主机器人关节角度和关节角速度矢量；θ_s和分别表示从机器人关节角度和关节角速度矢量；为从机器人逆雅克比矩阵；K为3×3主从映射对角系数矩阵；LPF为低通数字滤波器，用于滤除主手操作颤动。

经主从比例映射后，位置跟随误差e为：

e＝(X_m-X_m0)-K(X_s-X_s0)   (1)

式中X_m0和X_s0分别为主从手执行器末端的初始位置。

主从手末端速度误差可以表示为：

$\stackrel{\·}{e}={\stackrel{\·}{X}}_m-K{\stackrel{\·}{X}}_s---\left(2\right)$

当采用开环控制时，主从跟随误差就会不断积累，从而降低位置跟随精度，为此设计比例-微分控制器来消除跟随误差，相应的误差控制方程为：

$K_{p}e+K_d\stackrel{\·}{e}=0---\left(3\right)$

其中K_p和K_d分别为3×3比例、微分系数矩阵，穿刺机器人末端速度控制率为：

${\stackrel{\·}{X}}_s=K^{-1}{\stackrel{\·}{X}}_m+{\left(K_{d}K\right)}^{-1}{K}_p((X_m-X_{m0})-K(X_s-X_{s0}))---\left(4\right)$

主手笛卡尔速度经逆雅可比矩阵映射后得到各关节速度：

${\stackrel{\·}{\mathrm{\θ}}}_s=J_s^{-1}\left({\mathrm{\θ}}_s\right){\stackrel{\·}{X}}_s---\left(5\right)$

通过主从比例控制可以增加对主从操作空间适应性，比例系数K＝diag(kx,ky,kz)取值越大，从手末端操作精细程度越高，但有可能造成从手工作空间过小，导致操作失败；反之比例系数值越小，操作精细程度越低，有可能超出操作空间。因此在主从控制过程中，需要实时检测从手雅可比矩阵的奇异性，当机器人某位形灵活性下降时就切换到关节映射模式

${\stackrel{\·}{\mathrm{\θ}}}_s=K_r{\stackrel{\·}{\mathrm{\θ}}}_m---\left(6\right)$

K_r为5×5主从关节速度映射系数矩阵，一般为对角阵。通过关节速度映射控制，从机器人可以渡过局部的奇异位型。

在主从操作过程中，操作者手部不免都会发生抖动，而且在长时间的手术过程中医生疲劳度增加，手部抖动会更加突出，而且主手关节传感器的测量噪声也会通过主从映射会反映到从手的运动上，最终会影响机器人执行精度。本系统通过在主手关节角速度求解阶段对各关节角度数据进行低通滤波，去除这些无关的抖动。低通滤波器设计为滑动平均值滤波，该滤波器满足实时性控制要求，窗口长度的选择与采样率有关。

示教过程是依靠操作员观察机器人及其夹持工具相对于作业对象的位姿，通过主从操作反复调整示教点处机器人的作业位姿，通过主手上的使能按钮切换工作模式，自动记录机器人作业位姿、运动参数等，经过平滑及插值后，增加工艺参数就生成了机器人的控制指令序列示教文件。

本实施方式中的主从运动映射采用了基于逆雅克比矩阵主从控制算法，并通过比例-微分控制环节来消除主从位置跟随误差，在其他实施方式中也可以采用主从机器人各关节一对一的比例缩放进行映射。

有以上实施方式可以看出，本发明具有以下有益效果：

1、操作人员可以通过主机器人末端操作杆控制从机器人运动到期望的示教位姿，这种方式符合操作者的习惯，不需要通过间接控制各个关节使机器人到达示教位姿；

2、能够用于制造业自动化生产，示教模块实时记录机器人运行轨迹，优化操作路径，高效准确地生成控制指令序列与示教文件，使得机器人动作过程可以平稳的重现。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (10)

1.一种主从式遥操作工业机器人系统，其特征在于，所述系统包括主机器人、与主机器人相连的主从控制系统、及与控制系统相连的从机器人，所述主机器人包括力反馈设备，操作人员将主机器人运动指令通过主从控制系统传递给从机器人，从机器人跟随主机器人运动，进而完成对任务对象的操作，同时从机器人将任务状态反馈给操作人员。

2.根据权利要求1所述的系统，其特征在于，所述主从控制系统包括：

工业PC机，通过IEEE 1394接口与主机器人相连；

光学跟踪器，与工业PC机相连，所述工业PC机通过网卡与光学跟踪器进行数据交换；

运动控制器，与工业PC机通过网卡相连，运动控制器接受主机器人的操作命令，并对操作命令进行处理生成运动指令发送给从机器人。

3.根据权利要求2所述的系统，其特征在于，所述从机器人包括若干一一相连的伺服放大器和伺服电机，所述伺服放大器接受运动控制器发送的运动指令，放大后发送至相应的伺服电机，伺服电机根据运动指令完成相应的动作。

4.根据权利要求3所述的系统，其特征在于，所述从机器人还包括若干与伺服放大器相连的编码器，主从控制系统还包括信号转换单元，信号转换单元用于对编码器的初始值进行设定。

5.根据权利要求4所述的系统，其特征在于，所述信号转换单元通过RS232接口与工业PC机相连，信号转换单元通过RS422接口与伺服放大器相连。

6.一种如权利要求1所述的主从式遥操作工业机器人控制方法，其特征在于，所述方法包括：

S1、获取主机器人中主手各关节的位置信息和速度信息，经过主手正运动学计算得到主手各关节在笛卡尔空间的位置信息和速度信息；

S2、经过主从比例映射后得到从机器人从手末端在笛卡尔空间的速度信息，再经过从手逆雅可比矩阵变换后得到从手各关节的速度信息；

S3、经过关节速度限制运算后输出控制信号控制各伺服电机运动；

S4、引入比例-微分控制环节来消除主从手位置跟随误差。

7.根据权利要求6所述的方法，其特征在于，所述步骤S2中，经过主从比例映射后，主从手位置跟随误差e为：

e＝(X_m-X_m0)-K(X_s-X_s0)；

主从手末端速度误差为：

$\stackrel{.}{e}={\stackrel{.}{X}}_m-K{\stackrel{.}{X}}_s;$

其中，X_m和X_s分别为主手和从手末端执行器在笛卡尔空间坐标中的位置坐标，X_m0和X_s0分别为主从手末端执行器的初始位置，和分别为主从手末端执行器的笛卡尔速度矢量，K为3×3主从映射对角系数矩阵。

8.根据权利要求7所述的方法，其特征在于，所述步骤S4中“消除主从手位置跟随误差”通过误差控制方程实现，相应的误差控制方程为：

$K_{p}e+K_d\stackrel{.}{e}=0;$

K_p和K_d分别为3×3比例、微分系数矩阵。

9.根据权利要求8所述的方法，其特征在于，所述步骤S2中从手末端在笛卡尔空间的速度信息为：

${\stackrel{.}{X}}_s=K^{-1}{\stackrel{.}{X}}_m+{\left(K_{d}K\right)}^{-1}{K}_p((X_m-X_{m0})-K(X_s-X_{s0}));$

从机器人关节角速度信息为：

${\stackrel{.}{\mathrm{\θ}}}_s=J_s^{-1}\left({\mathrm{\θ}}_s\right){\stackrel{.}{X}}_s;$

其中，θ_s和分别表示从机器人关节角度和关节角速度矢量；为从机器人逆雅克比矩阵。

10.根据权利要求9所述的方法，其特征在于，所述方法还包括：

主从控制过程中，实时检测从手雅可比矩阵的奇异性，当从机器人某位形灵活性下降时则切换到关节映射模式：

${\stackrel{.}{\mathrm{\θ}}}_s=K_r{\stackrel{.}{\mathrm{\θ}}}_m;$

K_r为5×5主从关节速度映射系数矩阵。