CN106335057A - 一种基于实时力控的装配机器人全空间柔顺插孔控制方法 - Google Patents

Abstract

本发明公开了一种基于实时力控的装配机器人全空间柔顺插孔控制方法，步骤为：利用六维腕力传感器、实时通信软件包等搭建装配机器人螺栓插孔实时力控系统；采用基于标准位置的重力补偿方法，消除机器人全空间作业时重力分量对六维力传感器读数的影响；通过对插孔任务中接近、寻孔、插入与插入完成四个阶段的受力分析与力/位混合控制策略设计，避免螺栓插孔过程中常见的卡阻与契紧现象，提高机器人插孔作业成功率和效率；在多维力时间序列窗口中采用基于支持向量机的检测器实现寻孔完成状态自动检测，确保寻孔与插孔两个子任务的准确切换。本发明可解决利用常规刚性关节型机器人实现高效率、高准确性的无倒角柔顺插孔装配作业控制问题。

Description

一种基于实时力控的装配机器人全空间柔顺插孔控制方法

技术领域

本发明涉及装配机器人领域，特别涉及一种基于实时力控的装配机器人全空间柔顺插孔控制方法。

背景技术

在机器人进行螺栓插孔等一类精密装配作业时，由于装配环境存在各种不确定性甚至突发状况，常规的刚性关节型机器人已经无法确保孔轴装配任务的完成；若仅采用视觉引导机器人孔-轴对准，且一旦视觉定位导致装配机器人在装配件之间出现些微误差，都有可能会导致装配失败，甚至损坏装配件或机器人(见赵敏，“机器人柔顺抓放技术与应用”，工业控制计算机，2016)。这类装配生产制造领域对工业机器人提出更严苛的要求，该类任务最大的特点是需要与环境接触，这就需要机器人具有一定柔顺机制，即机器人能够通过对环境的顺从来完成某个预定的与环境接触的任务。

实现机器人柔顺作业的两个途径是被动柔顺和主动柔顺技术。被动柔顺并非对机器人本身施加控制，而是通过特殊的被动柔顺装置(如RCC柔顺腕等)来实现柔性，因此仅限于特定的环境和任务。由于RCC无法检测力，所以RCC在进行插孔任务时必须依靠倒角。而若采用主动柔顺力控制方式，则应用范围可以扩展到无倒角的情况下。

主动柔顺控制方法一般通过传感器测量的力信息，进行寻孔和插孔运动控制，摆脱了对倒角的依赖。因此，主动柔顺控制技术通常将力传感器安装在机器人末端和工具之间，通过工具和环境接触直接测量机器人末端受力，从而对机器人进行相应的力控制或者力/位混合控制(见张锟等，“一种基于力/位混合控制进行插孔作业的策略”，机器人，2002)。力传感器具有精度高、分辨率高、灵敏度好、线性度好，可靠性高，使用方便等优点，因而成为研究机器人和环境接触状况时最常用的手段。(见林君健，“基于力传感器的工业机器人主动柔顺装配系统研究”，华南理工大学硕士学位论文，2013)。

经专利检索查新，张峰等人申请了中国发明专利，申请号为CN201410696611.4，名称为“基于EtherCAT总线的机器人柔顺控制系统和方法”。该发明公开了一种实时性强、可靠性高、成本低、体积小的基于EtherCAT总线的模块化机器人柔顺控制方法，适应了机器人在执行接触作业中的要求。该方法为主站将机器人驱动器采集的电机周期性数据与设定的期望值进行叠加并进行阻抗控制，输出参考力矩值至机器人驱动器。因此该发明仅从机器人硬件元部件(驱动器)角度设计了一般阻抗控制的实现机制，并不涉及到任何具体任务中的机器人柔顺装配控制方法。

经专利检索查新，崔裕翔申请了中国发明专利，申请号为CN201510525775.5，名称为“新型智能泛用型高速拧紧装配机器人”。曲道奎等人申请了中国发明专利，申请号为CN201410632143.4，名称为“螺钉抓取机构和含有该螺钉抓取机构的螺钉装配机器人”。这些专利都是从具体装置的机械设计角度提出具体发明。其中“螺钉抓取机构和含有该螺钉抓取机构的螺钉装配机器人”这一发明公开了一种螺丝抓取与装配的特殊机械机构，在同一设备上连续完成螺钉抓取与拧紧操作，并且在该操作中，螺钉套筒始终和螺帽配合，避免了螺钉放入螺纹孔后的拧紧过程中认帽不完全准确的情况。但是该发明也不涉及机器人寻孔与插孔等具体环节的控制方法问题。

根据以上调研可见，目前尚未有针对机器人实时力控下的全空间、无倒角柔顺插孔装配控制方法方面的相关发明专利。虽然在机器人柔顺力控制方面，已有一些基础的技术方法，并用于机器人柔顺抓取、放置、层叠、对准等典型装配任务，为本发明的提出提供了参考和支持。但是用于机器人全空间、无倒角柔顺插孔装配控制方法，目前尚未有相关发明专利报道。

主动柔顺下的机器人插孔作业控制具有三个难点需要考虑。(1)首先是如何实时根据力测量信息调整机器人位姿，避免插孔过程中常见的卡阻与契紧现象，这是机器人插孔作业控制必须解决的首要问题，否则将极大影响机器人插孔装配的效率，因此需要采用力/位混合控制方法对机器人插孔过程各个阶段的受力情况及控制策略进行分析和设计。(2)其次，以往视觉引导的机器人孔轴装配方法(见)大多解决平面装配问题，难以适应机器人全空间作业需求。而力控下的机器人全空间插孔，最大的问题在于六维力传感器所测量的数据是对外界所有力的总体受力情况的整体反映，然而在装配操作中，力控制的对象是机器人末端与外界的接触力。根据分析，六维力传感器测量得到的数据中包含四个分量：接触力、工具重力、加速度力和传感器的初始值。作为全空间孔轴装配的基础，需要把接触力从六维力传感器测量的数据中分离出来，即解决重力补偿问题。(3)另外一个问题是，如何从力传感器时序数据中判断出插孔各个环节之间的切换，尤其是从寻孔阶段到插孔阶段的切换，难以仅仅采用简单阈值化方法来判断，因此可以利用机器学习的方法从力传感器数据中检测寻孔结束事件的发生。这些是本发明专利考虑并解决的核心问题。

发明内容

本发明的目的是针对如何利用传统刚性机器人实现柔顺接触的插孔(peg-in-hole)作业问题，实现安全、高效率的全空间机器人插孔功能，提出一种基于实时力控的装配机器人全空间柔顺插孔控制方法。

本发明采用的技术方案为：一种基于实时力控的装配机器人全空间柔顺插孔控制方法，具体包括以下步骤：

步骤1、利用六维腕力传感器、实时通信软件包等搭建装配机器人实时力控平台。在步骤1中，所搭建的系统主要由KUKA机器人本体、机器人控制器及末端执行器、力传感器、上位机等几个主要部分组成。力传感器通过转接件加装在机器人末端，采集实时力信号，通过NetBox的解析，得到力控制所需的六维力数据，并通过以太网发送至上位机。上位机与机器人控制器之间采用KUKA RSI实时通信接口进行通信，上位机实时接收机器人发送的位置及姿态、速度、电流、转矩等状态信息，同时实时获得NetBox发送的当前状态六维力信息，经过力控制算法的计算，将机器人下一步的运动的偏移量实时发送给机器人，机器人端程序实时接收偏移量并运行偏移量，从而实现对机器人的实时力控制。

步骤2、通过机器人全空间重力补偿，使得机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0。

精密的孔轴装配属于低速作业，加速度可以忽略。传感器初始值每次都有变化，所以不可忽略，需要通过参考位置补偿。所述步骤2中，重力补偿的目标是机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0。重力与重力矩补偿的基本方法是计算力传感器末端加装工具的重力G与重心{l_x,l_y,l_z}，通过机器人控制器读出的位姿信息预测此时的传感器力与力矩信息，传感器数据减去所预测的信息即为重力补偿后的力数据。

所述步骤2中，首先记录重力补偿6标准位姿下的力传感器数据；接着是预测重力分量，即在获得传感器与工具的重力和重心位置的基础上，通过读取机器人控制器上对末端的位姿描述，计算工具重力在传感器坐标系上三个坐标轴方向上的分量，即为预测的分量。然后是基于参考点的重力补偿，最后是重力矩补偿。

步骤3、机器人无倒角插孔作业过程控制策略。将无倒角插孔的各个阶段分为接近阶段、寻孔阶段、插入阶段与插入完成四个阶段，按照力位混合控制(Hybrid Position/Force Control)思路，对无倒角插孔作业过程各个阶段的自然约束与人工约束进行设计。

步骤4、对步骤3中涉及的接近阶段、寻孔阶段、插入阶段与插入完成四个阶段，分别针对步骤3中提出的人工约束和自然约束条件，进行导纳控制。导纳控制检测机器人与环境的接触力，输出为机器人的位置和速度，从而能够利用传统的位置型机器人控制器实现柔顺的力接触功能。

步骤5、基于支持向量机分类器的机器人寻孔结束检测。利用腕力传感器数据的时间序列及机器学习方法，判断机器人插孔作业过程中是否寻孔结束，从而切换到插孔阶段。

所述步骤5中，样本的收集方法是，首先利用机器人反复重现插孔运动，收集含有插孔结束(正样本)及非含有插孔结束(负样本)的力信号序列。腕力传感器获得的信号序列每帧数据包含三维力和三维力矩信号，即{F_x,F_y,F_z,T_x,T_y,T_z}。正样本为包含一次插孔结束(snap)事件的力/力矩信号序列，其中在snap事件前的数据点有n_pre个，在snap事件后的数据点有n_post个。

所述步骤5中，基于SVM分类器的寻孔结束事件检测器的训练，其具体方法是：利用定义的代价函数，来根据需求选取合适的参数，包括选用六维力/力矩数据中的哪些元素构成特征向量、以及数据点数目参数n_pre和n_post的选择。其结果是，确定采用六维腕力传感器中的F_y、F_z和T_z三个通道的数据，每个通道的数据包含12个数据点，因此拉直后的一个时序样本为1*36维特征向量。训练具有高斯核(Gaussian Kernel)的支持向量机并得到最佳的分类器。

所述步骤5中，机器人寻孔结束在线检测的具体方法是：在线检测中，将具有n_pre＝6和n_post＝6时间窗口的力/力矩特征时序拉直成1*36维特征向量送入该分类器，可以对该时序中是否含有snap事件进行实时检测，从而判断机器人是否完成一次寻孔过程。为了提高寻孔结束检测的准确性，在该分类器之后补充加一个阈值判断，即当机器人末端沿Z轴方向的速度超过某一阈值时，表示从寻孔阶段结束而应该进入插入阶段。

实际工程应用中的机器人插孔作业大多采用RCC柔顺腕等被动柔顺机制，其缺点是仅限于特定的环境和任务，而且使用RCC在进行插孔任务时必须依靠孔的倒角。基于力控的主动柔顺控制技术可用于通用型无倒角插孔装配任务。本发明提出了一种基于实时力控的装配机器人全空间柔顺插孔控制方法，所提出的方法在解决利用传统刚性机器人实现全空间接触式柔顺装配作业问题上具有一定通用性和可借鉴性；在无倒角插孔任务中，能够确保高效率、高准确性得完成机器人插孔任务，并避免插孔过程中常见的卡阻与契紧现象。本发明可配合机器人视觉功能使用，先由视觉引导机器人孔-轴对准，再启动实时力控完成插孔，从而克服视觉引导造成的孔-轴对准误差。

有益效果：

1、本发明借助主动柔顺控制技术，采用常规的刚性机器人就可以实现柔顺的螺栓插孔功能，通过对插孔任务中接近、寻孔、插入与插入完成四个阶段的受力分析与力/位混合控制策略设计，避免插孔过程中常见的卡阻与契紧现象，提高了机器人插孔作业成功率和效率；

2、相比以往的仅限于平面作业的装配机器人技术，本发明所涉及技术支持机器人全空间作业，通过重力补偿来克服变化姿态下力传感器读数中重力项的影响，从而允许插孔运动中机器人末端处于任意姿态。

3、相比简单的阈值化判别法，本发明在多维力信号时间序列窗口中采用基于支持向量机的检测器实现寻孔完成状态自动检测，确保寻孔与插孔两个子任务的准确切换。以往支持向量机一般用于静态特征向量的分类，而本发明将其用于多维力信号时间序列的分类，通过一系列参数学习过程确定时间窗口的尺寸及特征选取。

附图说明

图1为本发明实施实例中，插孔装配机器人系统结构；

图2为本发明实施实例中，无倒角的插孔过程；

图3为本发明实施实例中，无倒角的插孔控制策略；

图4为本发明实施实例中，导纳控制结构图；

图5为本发明实时实例中，折线寻孔路径图；

图6为本发明实施实例中，无倒角插孔任务各阶段受力数据曲线。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步说明。

一种基于实时力控的装配机器人全空间柔顺插孔控制方法，包括以下步骤：

步骤1、利用六维腕力传感器、实时通信软件包等搭建装配机器人实时通信力控平台。

具体步骤如下：系统主要由KUKA机器人本体、机器人控制器及末端执行器、力传感器、上位机等几个主要部分组成。系统总体的结构如图1所示。力传感器通过转接件加装在机器人末端，采集实时力信号，通过NetBox的解析，得到力控制所需的六维力数据，并通过以太网发送至上位机。上位机与机器人控制器之间采用KUKA RSI实时通信接口进行通信，上位机实时接收机器人发送的位置及姿态、速度、电流、转矩等状态信息，同时实时获得NetBox发送的当前状态六维力信息，经过力控制算法的计算，将机器人下一步的运动的偏移量实时发送给机器人，机器人端程序实时接收偏移量并运行偏移量，从而实现对机器人的实时力控制。执行器为平行二指抓手，经过转接件加装力传感器之下，抓手控制器通过RS232串口接收控制信息。

步骤2、通过机器人全空间重力补偿，使得机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0。

精密的孔轴装配属于低速作业，加速度可以忽略。传感器初始值每次都有变化，所以不可忽略，需要通过参考位置补偿。所述步骤2中，重力补偿的目标是机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0。重力与重力矩补偿的基本方法是计算力传感器末端加装工具的重力G与重心{l_x,l_y,l_z}，通过机器人控制器读出的位姿信息预测此时的传感器力与力矩信息，传感器数据减去所预测的信息即为重力补偿后的力数据。

步骤2中具体包括以下步骤：

步骤111：首先是标准位置数据记录。为了获得重力与重心位置信息，需要完成如下表所示的六个标准位姿，记录力数据。

重力补偿6标准位姿

位姿5与位姿6在Z方向的差值为工件重力的两倍，故有：

$G=\frac{F_{z6}-F_{z5}}{2}$

同理，根据第1-4种位姿力矩的相互差值，可以求出工具的重心位置：

$\begin{array}{ccc}{l}_x=\frac{T_{z4}-T_{z2}}{2G}& l_y=\frac{T_{z3}-T_{z1}}{2G}& l_z=\frac{T_{y3}-T_{y1}}{2G}\end{array}$

步骤112：其次是预测重力分量，即在获得传感器与工具的重力和重心位置的基础上，通过读取机器人控制器上对末端的位姿描述，计算工具重力在传感器坐标系上三个坐标轴方向上的分量，即为预测的分量。

设机器人的基坐标系为{B}，工具坐标系为{T}，安装在传感器末端的工具重力为G，则重力在基坐标系中的向量通过机器人控制器可以实时读出机器人按照固定坐标系旋转的R(Roll)P(Pitch)Y(Yaw)角，记为α、β、γ。当六维力传感器随着机器人末端发生位姿变化时，重力的向量与工具坐标系的三个坐标轴X_t、Y_t、Z_t会有一个变化的夹角，记为α_t、β_t、γ_t，则工具重力在X_t、Y_t、Z_t轴上的分量分别为G*cos(α_t)、G*cos(β_t)、G*cos(γ_t)。通过RPY角的定义，可求出基坐标系到工具坐标系的传输矩阵{T}为：

$\begin{array}{c}T=RPY(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=Rot(z,\mathrm{\α})Rot(Y,\mathrm{\β})Rot(X,\mathrm{\γ})\\ =\left[\begin{array}{cccc}c\mathrm{\α}c\mathrm{\β}& c\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}-s\mathrm{\α}c\mathrm{\γ}& c\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}+s\mathrm{\α}s\mathrm{\γ}& 0\\ s\mathrm{\α}c\mathrm{\β}& s\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}+c\mathrm{\α}c\mathrm{\γ}& s\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}-c\mathrm{\α}s\mathrm{\γ}& 0\\ -s\mathrm{\β}& c\mathrm{\β}s\mathrm{\γ}& c\mathrm{\β}c\mathrm{\γ}& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& 0\\ r_{21}& r_{22}& r_{23}& 0\\ r_{31}& r_{32}& r_{33}& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$

则：

$cos\left({\mathrm{\α}}_t\right)=\frac{(r_{11},r_{21},r_{31})\·(0,0,-G)}{\sqrt{{r_{31}}^2+{r_{32}}^2+{r_{33}}^2}*G}=\frac{-r_{31}G}{G}=-r_{31}$

重力在工具坐标系X轴的分量为：

G_x＝G·cos(α_t)＝-G·r₃₁

同理得出重力在工具坐标系{T}的三个坐标轴上分量：

G_x，G_y，G_z}＝{-r₃₁·G,-r₃₂·G,-r₃₃·G}

＝{G·sin(β),-G·cos(β)·sin(γ),-G·cos(β)·cos(γ)}

步骤113：然后是基于参考点的重力补偿。在有关重力补偿的相关研究中，一般都使用零位为基准进行重力补偿，然而这种算法需要每次对六个标准位置都进行测量，在实际应用中并不适用。因此本发明使用一种在实际中更便捷的补偿方法，即基于标准位置的重力补偿。

每次运行前，需要记录参考位置的力数据，这里以标准位置6为基准，记测量到的力数据记为{F_refx，F_refy，F_refz}，通过计算预测的数据记为{F_ex，F_ey，F_ez}，根据前文推算，可得到：

F_ex＝F_refx+G·sin(β)

F_ey＝F_refy-G·cos(β)·sin(γ)

F_ez＝F_refz-G-G·cos(β)·cos(γ)

步骤114：最后是重力矩补偿。前已计算出工具重心在工具坐标系{T}下的坐标为{l_x，l_y，l_z}，工具重力在工具坐标下三个方向的力矩分量为{T_x，T_y，T_z}，重力在工具的三个坐标轴上分量{G_x，G_y，G_z}。根据力和力矩的关系，可以得出：

T_x＝G_z·l_y-G_y·l_z

T_y＝G_x·l_z-G_z·l_x

T_z＝G_y·l_x-G_x·l_y

对于大部分工具而言，工件与抓手都相对对称，质心位置在传感器坐标系Z轴上，所以l_x＝l_y＝0。以标准位置6为参考位置，力矩参考值记为{T_refx,T_refy,T_refz}，计算工具在传感器的力矩分量{T_ex,T_ey,T_ez}：

T_ex＝T_refx+G_y·l_z

T_ey＝T_refy-G_x·l_z

T_ez＝T_refz

步骤3、机器人无倒角插孔作业过程控制策略。无倒角插孔的各个阶段如图2所示，分为接近阶段、寻孔阶段、插入阶段与插入完成。机器人无倒角插孔控制策略如图3所示。按照John J.Craig提出的力位混合控制(Hybrid Position/Force Control)思路，无倒角插孔作业过程各个阶段的自然约束与人工约束如下分析。

步骤211：接近阶段，螺栓处于位置自由空间，因此自然约束为：

F＝0

这种情况下，人工约束构成了整个运行轨迹，使螺栓竖直向下运动接近工件表面。

$v=\left[\begin{array}{c}0\\ 0\\ v_{\mathrm{qpproach}}\\ 0\\ 0\\ 0\end{array}\right]$

式中，v_approach为接近工件表面的速度。在图2(b)中，螺栓已经接触工件表面，为了检测这一状态，需要检测Z轴方向的力F_z。当检测的力达到某一阈值，则认为螺栓与工件发生了接触。至此，进入了下一个子任务。

步骤212：寻孔阶段，机器人按折线轨迹进行寻孔。图4为折线形式进行寻孔轨迹，寻孔的间距为c。若要搜索到在平面内任意位置内的孔。采用螺线或者折线寻孔时，需要满足下式：

2πb＜2(R_k-R_z)

d＜2(R_k-R_z)

其中，R_k为孔的半径，R_z为轴的半径。即在寻孔过程中，搜索的半径应小于轴与孔直径之间的间隙差。寻孔阶段的自然约束和人工约束为：螺栓在竖直方向不能自由移动，也不能绕X轴或Y轴旋转。在另外三个方向，不能施加任意的力，自然约束描述如下式：

v_z＝0,w_x＝0,w_y＝0,F_x＝0,F_y＝0,T_z＝0

人工约束条件则描述了控制螺栓在X方向沿工件表面滑动，同时在Z方向施加一个较小的力来维持螺栓与工件表面接触：

v_x＝v_side,v_y＝0,w_z＝0,F_z＝F_contact,T_x＝0,T_y＝0

步骤213：插入阶段，从寻孔阶段开始检测Z轴方向的速度，当判断出寻孔结束时(具体方法见后)，表示从寻孔阶段进入了插入阶段。自然约束再次发生变化，重新改变装配策略的人工约束。新的自然约束为：

v_x＝0,v_y＝0,w_x＝0,w_y＝0,F_z＝0,T_z＝0

选择人工约束为：

v_z＝v_insert,w_z＝0,F_x＝0,F_y＝0,T_x＝0,T_y＝0

式中，v_insert为螺栓向孔中插入的速度。最后，当Z轴方向的力超过某一阈值时，检测得到进入装配完成阶段。

步骤4、对步骤3中涉及的接近阶段、寻孔阶段、插入阶段与插入完成四个阶段，分别针对步骤3中提出的人工约束和自然约束条件，进行导纳控制。

导纳控制检测机器人与环境的接触力，输出为机器人的位置和速度。导纳控制的结构图如图5所示。图5中F₀为力期望值,F_int为经过重力补偿后的接触力，ΔF为力偏差信号，ΔX为位置纠正量。导纳控制器输出若为位置信号，此时属于电纳控制。

ΔX＝MΔF

式中，表示机器人在各个方向上的偏移量。ΔF＝[ΔF_x,ΔF_y,ΔF_z,ΔT_z,ΔT_y,ΔT_x]^T，表示机器人各个方向接触力的偏差信号。M是一个6阶对角矩阵，对角线上的元素代表了机器人进行力控的反馈系数。

$M=\left[\begin{array}{cccccc}{m}_1& 0& 0& 0& 0& 0\\ 0& m_2& 0& 0& 0& 0\\ 0& 0& m_3& 0& 0& 0\\ 0& 0& 0& m_4& 0& 0\\ 0& 0& 0& 0& m_5& 0\\ 0& 0& 0& 0& 0& m_6\end{array}\right]$

M中对角线上的元素m₁～m₆表示在机器人各个方向上的力控系数，改变m₁～m₆的取值可以设定机器人在各个方向上力信号与位置信号之间的反馈系数。

若导纳控制器输出机器人的速度信号，此时属于电导控制：

ΔV＝NΔF

若同时转换为速度与位置信号则属于导纳控制：

$\mathrm{\Δ}V=M\mathrm{\Δ}\stackrel{\·}{F}+N\mathrm{\Δ}F$

在实际的任务中根据工件的几何特征和任务的具体要求，需要增加一个选择矩阵S。如式下所示：

$\mathrm{\Δ}V=S(M\mathrm{\Δ}\stackrel{\·}{F}+N\mathrm{\Δ}F)$

式中S为一对角矩阵，并且对角线元素只能为0或者1。元素设置为0的方向上的位置偏差量不受力偏差信号影响，只进行位置控制。设置为1的方向进行力控。通过设置合理的S矩阵可以进行力位混合控制。

步骤5、基于支持向量机分类器的机器人寻孔结束检测。利用腕力传感器数据及机器学习方法，判断机器人插孔作业过程中是否寻孔结束，从而切换到插孔阶段。具体步骤如下：

步骤311：首先利用机器人反复重现插孔运动，收集含有插孔结束(正样本)及非含有插孔结束(负样本)的力信号序列。腕力传感器获得的信号序列每帧数据包含三维力和三维力矩信号，即{F_x,F_y,F_z,T_x,T_y,T_z}。正样本为包含一次插孔结束(snap)事件的力/力矩信号序列，其中在snap事件前的数据点有n_pre个，在snap事件后的数据点有n_post个。

步骤312：训练基于SVM分类器的寻孔结束事件检测器。利用定义的代价函数，来根据需求选取合适的参数，包括选用六维力/力矩数据中的哪些元素构成特征向量、以及数据点数目参数n_pre和n_post的选择。

首先定义分类代价函数：

J＝10n_fp+n_fn

由于对寻孔结束事件的检测，漏检风险比误检风险小，因此在该代价函数中误检事件(n_fp)的惩罚代价是漏检事件(n_fn)惩罚代价的10倍，分类器训练目的是使得训练样本和测试样本上获得的代价函数值最小。

然后确定最佳的候选特征通道。从仅选用六维力/力矩信号中的一维力信号数据开始，不断增加候选特征，观察分类器在训练样本和测试样本上获得的代价结果值变化情况，当增加一个力信号而代价函数值下降比率低于10％时，结束最佳候选特征的训练过程。经过实验比较，最终选取三个通道F_y、F_z和T_z数据来组成特征向量。

最后确定数据点数目参数n_fp和n_fn。从另n_fp＝n_fn＝1开始，逐渐增加n_fp和n_fn的值，观察分类器在训练样本和测试样本上获得的代价值变化情况，当n_fp和n_fn增加1而代价函数值下降比率低于10％时，结束数据点数目参数的训练过程。经过实验比较，最终选择n_pre＝6和n_post＝6，即在正样本发生snap事件前后各选取6个数据点来组成特征向量，可以获得理想的分类效果。

步骤313：机器人寻孔结束在线检测。最终确定采用六维腕力传感器中的F_y、F_z和T_z三个通道的数据，每个通道的数据包含12个数据点，因此拉直后的一个样本为1*36维特征向量。训练具有高斯核(Gaussian Kernel)的支持向量机并得到最佳的分类器。在线检测中，将具有n_pre＝6和n_post＝6时间窗口的力/力矩特征时序拉直成1*36维特征向量送入该分类器，可以检测该时序中是否含有snap事件，从而判断机器人是否完成一次寻孔过程。为了提高寻孔结束检测的准确性，在该分类器之后补充加一个阈值判断，即当机器人末端沿Z轴方向的速度超过某一阈值时，表示从寻孔阶段结束而应该进入插入阶段。图6为具体实施中机器人插孔过程六维力测量数据，其中包含了该方法判断出来的寻孔—插孔状态切换。

应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (4)

1.一种基于实时力控的装配机器人全空间柔顺插孔控制方法，其特征在于：具体包括以下步骤：

步骤1、利用六维腕力传感器、实时通信软件包等搭建装配机器人实时力控平台；

步骤2、通过机器人全空间重力补偿，使得机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0；

步骤3、机器人无倒角插孔作业过程控制策略；将无倒角插孔的各个阶段分为接近阶段、寻孔阶段、插入阶段与插入完成四个阶段，按照力位混合控制思路，对无倒角插孔作业过程各个阶段的自然约束与人工约束进行设计；

步骤4、对步骤3中涉及的接近阶段、寻孔阶段、插入阶段与插入完成四个阶段，分别针对步骤3中提出的人工约束和自然约束条件，进行导纳控制；导纳控制检测机器人与环境的接触力，输出为机器人的位置和速度，从而能够利用传统的位置型机器人控制器实现柔顺的力接触功能；

步骤5、基于支持向量机分类器的机器人寻孔结束检测；利用腕力传感器数据的时间序列及机器学习方法，判断机器人插孔作业过程中是否寻孔结束，从而切换到插孔阶段。

2.根据权利要求1所述的一种基于实时力控的装配机器人全空间柔顺插孔控制方法，其特征在于：所述步骤1中，力传感器通过转接件加装在机器人末端，采集实时力信号，通过NetBox的解析，得到力控制所需的六维力数据，并通过以太网发送至上位机；上位机与机器人控制器之间采用KUKA RSI实时通信接口进行通信，上位机实时接收机器人发送的位置及姿态、速度、电流、转矩等状态信息，同时实时获得NetBox发送的当前状态六维力信息，经过力控制算法的计算，将机器人下一步的运动的偏移量实时发送给机器人，机器人端程序实时接收偏移量并运行偏移量，从而实现对机器人的实时力控制。

3.根据权利要求1所述的一种基于实时力控的装配机器人全空间柔顺插孔控制方法，其特征在于：所述步骤2中，重力补偿的目标是机器人无论在何种与外界不接触的姿态下，测量的六维力数据都为0；重力与重力矩补偿的基本方法是计算力传感器末端加装工具的重力G与重心{l_x,l_y,l_z}，通过机器人控制器读出的位姿信息预测此时的传感器力与力矩信息，传感器数据减去所预测的信息即为重力补偿后的力数据；

所述步骤2中，首先记录重力补偿6标准位姿下的力传感器数据；接着是预测重力分量，即在获得传感器与工具的重力和重心位置的基础上，通过读取机器人控制器上对末端的位姿描述，计算工具重力在传感器坐标系上三个坐标轴方向上的分量，即为预测的分量；然后是基于参考点的重力补偿，最后是重力矩补偿。

4.根据权利要求1所述的一种基于实时力控的装配机器人全空间柔顺插孔控制方法，其特征在于：所述步骤5中，样本的收集方法是，首先利用机器人反复重现插孔运动，收集含有插孔结束及非含有插孔结束的力信号序列；腕力传感器获得的信号序列每帧数据包含三维力和三维力矩信号，即{F_x,F_y,F_z,T_x,T_y,T_z}；正样本为包含一次插孔结束事件的力/力矩信号序列，其中在插孔结束事件前的数据点有n_pre个，在插孔结束事件后的数据点有n_post个；

所述步骤5中，基于SVM分类器的寻孔结束事件检测器的训练，其具体方法是：利用定义的代价函数，来根据需求选取合适的参数，包括选用六维力/力矩数据中的哪些元素构成特征向量、以及数据点数目参数n_pre和n_post的选择；其结果是，确定采用六维腕力传感器中的F_y、F_z和T_z三个通道的数据，每个通道的数据包含12个数据点，因此拉直后的一个时序样本为1*36维特征向量；训练具有高斯核的支持向量机并得到最佳的分类器；

所述步骤5中，机器人寻孔结束在线检测的具体方法是：在线检测中，将具有n_pre＝6和n_post＝6时间窗口的力/力矩特征时序拉直成1*36维特征向量送入该分类器，可以对该时序中是否含有snap事件进行实时检测，从而判断机器人是否完成一次寻孔过程；为了提高寻孔结束检测的准确性，在该分类器之后补充加一个阈值判断，即当机器人末端沿Z轴方向的速度超过某一阈值时，表示从寻孔阶段结束而应该进入插入阶段。