CN114102579B - 一种基于圆锥运动的轴孔力控装配搜孔方法及系统 - Google Patents

Abstract

本申请涉及但不限于工业机器人技术领域，尤其涉及一种基于圆锥运动的轴孔力控装配搜孔方法及系统，应用于轴孔力控装配系统，系统包括：控制器、机械臂、传感器、轴件、以及孔件；轴件和传感器均设置于机械臂的末端，传感器用于检测轴件的六轴力/力矩，传感器与控制器通信连接；该方法包括：控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；控制轴件沿孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功；本申请搜孔时间较短且搜孔节拍稳定的搜孔方法。

Description

一种基于圆锥运动的轴孔力控装配搜孔方法及系统

技术领域

本申请涉及但不限于工业机器人技术领域，尤其涉及一种基于圆锥运动的轴孔力控装配搜孔方法及系统。

背景技术

据相关统计，精密装配作业在整个生产周期中占30％的成本开销、50％的时间开销。现阶段，精密装配作业主要还是通过工人手动装配，产品的装配效率主要受到工人人数和工人熟练度等因素的影响。工人越多，装配熟练度越高，企业的整体生产效率会越高，但随之而来的人工费用、管理费用、维护费用也会越高。工业机器人因其高精高速、环境适应性好、成本低的优点，是未来装配作业的理想载体。

机器人装配按待装配零件的几何特征可分为：圆柱形轴孔件装配、方形轴孔件装配、不规则几何外形轴孔件装配。圆柱形轴孔件在工业生产中最为常见，如轴和轴承、轴和齿轮等，因此对圆柱形轴孔件装配技术的研究对推广机器人装配技术的发展与应用具有巨大的促进、基础作用。而一般需要人工操作的装配场景在采用机器人替代时，都需要机器人也具备相同的外界感知能力，比如力觉、视觉等，无外界感知的传统机械臂进行精密装配作业，容易发生设备损坏。

不管待装配件的几何外形特征如何，应用工业机器人实现装配的一般流程含有三个阶段，分别是：接近阶段、搜孔阶段、插孔阶段。接近阶段和插孔阶段如字面含义所言，分别是轴件与孔件相互接近的过程。搜孔阶段对于高精度装配作业而言是必不可少的。一般情况下，轴件在插孔前与孔件上孔的相对位姿较理想情况下是有一些误差的，造成该误差的因素有很多，比如机器人的运动定位误差、轴件和孔件的安装定位误差。上述误差会造成轴件和孔件间没有“对齐”，此时如若不加调整，直接控制轴件插孔，则装配作业必会失败，严重情况下会损坏机器人。搜孔阶段就是一个让轴件和孔件“对齐”的阶段，也是精密装配过程中非常重要的一个阶段。

因此，有必要针对以上现有技术存在的问题，对装配任务中的搜孔策略进行改进，提供一种搜孔时间较短且搜孔节拍稳定的搜孔方法。

发明内容

以下是对本文详细描述的主题的概述。本概述并非是为了限制权利要求的保护范围。

本申请实施例提供了一种基于圆锥运动的轴孔力控装配搜孔方法及系统，基于圆锥运动的轴孔装配搜孔策略，其速度优于常规搜孔装配方法(螺旋线寻孔与方波寻孔)，且搜孔节拍稳定。

第一方面，本申请实施例提供了一种基于圆锥运动的轴孔力控装配搜孔方法，所述方法应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述方法包括以下步骤：

控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。

在一些实施例中，所述采用圆锥运动方式进行探测，以确定轴件的圆锥运动区，包括：

采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值；

根据数值最小的力所在方向为中心确定轴件的圆锥运动区。

在一些实施例中，所述采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值，包括：

取轴件末端为机器人工具TCP点，记轴件末端在当前工具坐标系下的笛卡尔点位，将该笛卡尔点位作为起始点；

控制轴件从起始点分别朝多个方向做旋转运动，采集得到轴件在多个方位点上沿其轴线方向上力的数值；其中，每个方位点与起始点的姿态角Rx均相差角度为α，相邻方位点之间的姿态角Rx相差角度为β。

在一些实施例中，所述控制轴件从起始点分别朝多个方向做旋转运动，包括：

获取在k时刻，轴件已旋转的角度α_k和初始姿态q₀：

α_k＝-kv_sT；

采用以下公式确定轴件从初始姿态q₀旋转的角度α_k后姿态的变化q_m：

则轴件在k时刻的姿态q_k通过四元数乘法获得：

q_k＝q₀q_m；

其中，k＝0,1,2,...,n；

根据轴件在k时刻的姿态q_k控制轴件做旋转运动。

在一些实施例中，所述控制轴件在圆锥运动区做圆锥运动，以确定孔位方向，包括：

控制轴件在圆锥运动区做圆锥运动，并通过力传感器采集轴件做圆锥运动时的传感数据，所述传感数据包括轴件做圆锥运动时的速度、轴件轴件沿其轴线方向的力序列和方位角序列；

根据所述传感数据确定孔位方向。

在一些实施例中，所述根据所述传感数据确定孔位方向，包括：

在轴件在做圆锥运动时的速度中取多个较大值；

以多个较大值得到的力序列作为纵坐标，以多个较大值的角度序列作为横坐标，进行三次多项式样条插值，得到新力序列和对应的新角度序列；

确定新力序列中的最小值，按照映射关系找到其对应的新角度序列中的角度；

根据所述新角度序列中的角度计算出孔位方向。

第二方面，本申请实施例还提供了一种基于圆锥运动的轴孔力控装配搜孔装置，所述基于圆锥运动的轴孔力控装配搜孔装置应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述装置包括：

粗调模块，用于控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

圆锥运动区确定模块，用于采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

孔位方向确定模块，用于控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

搜孔模块，用于控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。

第三方面，本申请实施例还提供了一种基于圆锥运动的轴孔力控装配搜孔系统，其特征在于，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述控制器包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如第一方面所述的基于圆锥运动的轴孔力控装配搜孔方法。

第四方面，本申请实施例还提供了一种计算机可读存储介质，存储有计算机可执行指令，所述计算机可执行指令用于执行如第一方面所述的基于圆锥运动的轴孔力控装配搜孔方法。

本申请实施例包括：一种基于圆锥运动的轴孔力控装配搜孔方法及系统，所述方法应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；该方法包括：控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；采用圆锥运动方式进行搜孔，以确定孔位方向；控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。

本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请而了解。本申请的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本申请技术方案的进一步理解，并且构成说明书的一部分，与本申请的实施例一起用于解释本申请的技术方案，并不构成对本申请技术方案的限制。

图1是本申请一个实施例提供的基于圆锥运动的轴孔力控装配搜孔方法的流程图；

图2是图1中步骤S200的具体流程图；

图3是本申请一个实施例中轴孔初始相对位置的示意图；

图4是本申请一个实施例中轴件倾斜姿态位置的示意图；

图5a是本申请一个实施例提供的判断孔件孔心方向的示意图；

图5b是本申请一个实施例提供的判断孔件孔心方向的另一示意图；

图5c是本申请一个实施例提供的判断孔件孔心方向的另一示意图；

图5d是本申请一个实施例提供的判断孔件孔心方向的另一示意图；

图6是本申请一个实施例提供的孔件孔心方向的示意图；

图7是本申请一个实施例提供的轴件在新区间做圆锥运动的过程示意图；

图8是本申请一个实施例提供的三次多项式样条插值示意图；

图9本申请一个实施例提供的基于圆锥运动的轴孔力控装配搜孔装置的结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。

需要说明的是，虽然在装置示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于装置中的模块划分，或流程图中的顺序执行所示出或描述的步骤。说明书、权利要求书或上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。

现有技术中，搜孔方法大致分为两类，盲搜孔方法、机器学习智能搜孔方法；盲搜索方法代表性有阿基米德螺线轨迹搜孔法或方波轨迹搜孔法，但此种方法一般搜孔时间较长。且时间波动误差不定，无法保持较短和稳定的搜孔节拍。机器学习搜孔方法大多停留在理论阶段，实际中应用调试复杂度较高、稳定性较差，难以产生落地应用。

盲搜索方法指代工业机器人基于力觉感知的前提下，进行某种类似“盲人摸象”的轨迹运动，直到感知到“轴”与“孔”对齐，则此阶段完成。一般的策略有：方波搜孔法、阿基米德螺线搜孔法等。盲搜孔方法的缺点是效率低、平均搜孔时间长，优点是使用简单、适用性较高、可发展性较好。通过设定合理的搜孔轨迹，盲搜孔方法的搜孔效率可得到极大提升。

基于机器学习的智能搜孔方法是最近的研究热点，典型代表是基于示教学习的搜孔方法。示教学习主要有三个阶段：感知、编码和再现。感知阶段的目的是记录和解释人体的运动轨迹信号、接触力/力矩信号和轴件的位姿信号，这各阶段需要用到各种传感器，如动作捕捉传感器系统、六维力/力矩传感器、编码器等。为了便于解释，感知阶段还应用了一些数据预处理技术来处理原始数据，如用主成分分析(PCA)进行降维和动态时间规整(DTW)来对来自不同演示的所有样本点进行时间上的对齐。编码阶段涉及到建立观察状态和执行操作之间的映射关系，比较常用的映射方法是动态运动原语(DMPs)、高斯混合回归(GMR)。再现阶段，利用编码阶段建立的映射关系复现相关操作。此类方法的优点是能够有效地解决环境参数未知以及机器人动力学参数精度低带来的负面影响，但是其对环境的适应性较差，每当装配作业环境发生了大的改变，都需要重新采集大量数据训练建模，且目前基于机器学习的智能搜孔方法只能应用于较大装配间隙的装配作业中。

本申请提供了一种基于圆锥运动的轴孔力控装配搜孔方法及系统，所述方法应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；该方法包括：控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。本申请基于力觉感知工业机器人的基础上针对圆柱形轴孔件进行装配策略改进，提供了一种搜孔时间较短且搜孔节拍稳定的搜孔方法。

下面结合附图，对本申请实施例作进一步阐述。

如图1所示，图1是本申请一个实施例提供的一种基于圆锥运动的轴孔力控装配搜孔方法的流程图，所述方法应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；这样，控制器通过通讯可以实时获取传感器采集的传感器数据(轴件的六轴力/力矩)，为机器人决策提供信息输入量，在一实施例中，所述机械臂采用华数HSR-JR603机械臂，所述传感器为六维力/力矩传感器。

在该方法中，包括但不限于有以下步骤：

步骤S100，控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

需要说明的是，在一些实施例中，可采用人工记录轴件工具示教点的方式，控制机械臂运动，使得机械臂的末端靠近孔位。所述轴件工具示教点位需要满足：轴件底端平面大致平行于孔件表面，且轴件底端中心点投影落在孔内。

步骤S200，采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

步骤S300，控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

步骤S400，控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。

在一些实施例中，给运动方向施加牵引力F_g，传感器测量力为F_s，合力为F_t＝F_g+F_s，再将合力F_t采用PID控制方式进行控制运动。控制轴件按照所述孔位方向前进，搜孔的成功标志是轴件在前进过程中，其轴线方向上受到的力在某个时刻小于某个阈值(理论上该阈值是0N，但考虑到力传感器噪声，一般设为0.5N)，即可判断搜孔成功。

另外，参照图2，在一实施例中，所述步骤S200包括但不限于有以下步骤：

步骤S210，采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值；

步骤S220，根据数值最小的力所在方向为中心确定轴件的圆锥运动区。

在一实施例中，将数值最小的力所在方向为中心左右各π/6为轴件的圆锥运动区间。

另外，在一实施例中，所述步骤S210包括但不限于有以下步骤：

步骤S211，取轴件末端为机器人工具TCP点，记轴件末端在当前工具坐标系下的笛卡尔点位，将该笛卡尔点位作为起始点；

步骤S212，控制轴件从起始点分别朝多个方向做旋转运动，采集得到轴件在多个方位点上沿其轴线方向上力的数值；其中，每个方位点与起始点的姿态角Rx均相差角度为α，相邻方位点之间的姿态角Rx相差角度为β。

需要说明的是，本步骤为孔件孔心初筛运动，以运动一个方位的数据采集为例，机器人控制轴件从P0运动到P1(P1与P0的姿态角Rx相差α角度)，在P1点处完成传感器数据采集，然后回到P0点，即完成一个方位的数据采集。同上所述，共完成八个方位的数据采集，将需要依次到达的方位点分别记为P1，P2…，P8，相邻方位点之间的姿态角Rx相差β角度。在一实施例中，控制轴件以倾斜姿态对起始点圆周进行数据采集，完成八个方位的数据采集，需要依次到达的方位点为P1，P2…，P8，其中，每个方位点与起始点的姿态角Rx均相差角度为α，相邻方位点之间的姿态角Rx相差β角度。

另外，在一实施例中，所述步骤S212包括但不限于有以下步骤：

获取在k时刻，轴件已旋转的角度α_k和初始姿态q₀：

α_k＝-kv_sT；

采用以下公式确定轴件从初始姿态q₀旋转的角度α_k后姿态的变化q_m：

则轴件在k时刻的姿态q_k通过四元数乘法获得：

q_k＝q₀q_m；

其中，k＝0,1,2,...,n；

根据轴件在k时刻的姿态q_k控制轴件做旋转运动。

另外，在一实施例中，所述步骤S300包括但不限于有以下步骤：

步骤S310，控制轴件在圆锥运动区做圆锥运动，并通过力传感器采集轴件做圆锥运动时的传感数据，所述传感数据包括轴件做圆锥运动时的速度、轴件轴件沿其轴线方向的力序列和方位角序列；

步骤S320，根据所述传感数据确定孔位方向。

另外，在一实施例中，所述步骤S320包括但不限于有以下步骤：

步骤S321，在轴件在做圆锥运动时的速度中取多个较大值；

步骤S322，以多个较大值得到的力序列作为纵坐标，以多个较大值的角度序列作为横坐标，进行三次多项式样条插值，得到新力序列和对应的新角度序列；

步骤S323，确定新力序列中的最小值，按照映射关系找到其对应的新角度序列中的角度；

步骤S324，根据所述新角度序列中的角度计算出孔位方向。

需要说明的是，所述孔位方向即为轴件在后续运动的前进方向。

另外，在另一实施例中，所述步骤S320包括但不限于有以下步骤：

步骤S3211，采集得到轴件沿其轴线方向的力序列f、以及力序列f所对应的方位角序列；确定力序列f中最小的三个值fa,fb,fc所对应的方位角。

在一实施例中，在P1->P8共8个点位处，采集得到轴件Fz方向的力序列f＝{f_k|k＝1,2,3,...,8}，以及力序列f所对应的方位角序列θ＝{θ_k|k＝1,2,...,8},即θ代表P1->P8的8个点位处的姿态角Rz。寻找力序列f中最小的三个值fa,fb,fc所对应的方位角θ1,θ2,θ3，那么θ1～θ3即是孔心所在方位。

步骤S3212，在方位角θ1～θ3的姿态角Rz的角度方位，以设定的角度步长做圆锥运动，采集得到圆锥区的力序列f′，以及力序列f2所对应的方位角θ′。

在一实施例中，在方位角θ1～θ3的姿态角Rz的角度方位，以设定的角度步长做圆锥运动，从起始Rz＝θ1，运动到Rz＝θ3，采集得到圆锥区的力序列f′＝{f_k′|k′＝1,2,3,...,n}，以及力序列f2所对应的方位角θ′＝{θ_k′|k′＝1,2,...,n}。

步骤S3213，对力序列f′与对应的方位角θ′进行密集化插补或者拟合，生成新序列f_new以及与所述新序列f_new对应的新方位角θ_new，将新序列f_new中最小值所对应的方位角作为孔心所在的方位角孔位方向。

在一些实施例中，所述密集化插补或者拟合的方法采用多项式插补或拟合方法，孔心所在的方位角

下面以一具体示例对上述实施例中基于圆锥运动的轴孔力控装配搜孔方法进行举例说明：

步序1.孔件孔心方向粗筛运动：

首先，假定系统经过接近阶段后，轴孔初始相对位置如图3所示，A表示初始状态下轴件中轴线位置，O表示孔件中轴线位置。要求轴件A状态下底面与孔件平面基本平行且轴件底面中心点O_p投影落在孔所在圆内。

进一步，如图4所示，轴件从状态A开始，选定一个方向绕轴件底面中心点O_p旋转α角度。到达位置B。然后采集当前传感器在Fz轴的力数据，再返回位置A。重复上述步骤，完成8个方位的数据采集。

8个方位的数据采集工作步骤如图5a、图5b、图5c和图5d所示，图5a、图5b、图5c和图5d中所述符号说明如下：

U1，U2，U3，U4：{P0}的X-Y平面内的4个用作旋转轴的单位向量，它们与{P0}的X轴夹角分别为：0、其中，{P0}为初始坐标系；

1，2，3：分别表示轴件绕旋转轴(U1、U2、U3、U4)逆时针旋转α、顺时针旋转2α、逆时针旋转α这3个步骤，执行这三步后，轴件会回到初始姿态；

s1，s2，s3：分别表示执行1、2、3步时，轴件的运动方向(以轴件的轴线向量的运动简化表示轴件本体的运动)；

f0，f1，f2，f3，f4，f5，f6，f7，f8：轴件在绕U1、U2、U3、U4分别执行完1、2步时，由传感器采集到的轴件沿其轴线方向上的力(传感器Z方向上力的读数)；

f0-f7共8个力，代表了8个方向，根据图5a、图5b、图5c和图5d，再结合上文中总结的客观规律和事实，可知f7应该是最小力，则孔件孔心的大致方向是图6中所示的7方向，即以这个方向(7方向)左右各π/6为轴件的圆锥运动区间。至此，本次孔件孔心方向粗筛运动结束，输出圆锥运动区，其孔位方向必定在此初筛区域中。

如图5a、图5b、图5c和图5d所示，轴件需要分别绕U1、U2、U3、U4执行1、2、3步骤，最后得到8个力f0-f7。U1、U2、U3、U4为单位向量，设其通用形式为：

(x_i,y_i,0) (4)

x_i、y_i取值如下：

由于轴件绕U1、U2、U3、U4的运动情况相似，下面仅以U1为例进行说明。

设轴件绕U1的旋转速度为v_s；轴件初始姿态为q₀(四元数形式)；步骤1、2、3中轴件的旋转角度分别为-α、+2α、-α；轴件运动时的时间以控制周期数k表示，简称为时刻k，k初始值为0；系统控制周期为T。

<1>步骤1中，在k时刻，轴件理论上已旋转的角度是：

α_k＝-kv_sT,k＝0,1,2,…,αmod v_sT (7)

轴件从初始姿态q₀转α_k后姿态的变化可以用一个四元数表示：

则轴件在k时刻的姿态可以通过四元数乘法获得：

q_k＝q₀q_m (9)

k的取值是0,1,2,...,αmod v_sT，则相应可得到一系列q_k，将这些q_k依次下发给机器人控制器，则可控制轴件做相应的旋转运动。

<2>步骤2中，先将k清零。在k时刻，轴件理论上已旋转的角度是：

α_k＝-α+kv_sT,k＝0,1,2,…,2αmod v_sT (10)

轴件从初始姿态q₀转α_k后姿态的变化可以用公式(8)中的四元数表示，轴件在k时刻的姿态可以用公式(9)表示。

k的取值是0,1,2,...,2αmod v_sT，则相应可得到一系列q_k，将这些q_k依次下发给机器人控制器，则可控制轴件做相应的旋转运动。

<3>步骤3中，先将k清零。在k时刻，轴件理论上已旋转的角度是：

α_k＝α-kv_sT,k＝0,1,2,...,αmod v_sT (11)

轴件从初始姿态q₀转α_k后姿态的变化可以用公式(8)中的四元数表示，轴件在k时刻的姿态可以用公式(9)表示。

k的取值是0,1,2,...,αmod v_sT，则相应可得到一系列q_k，将这些q_k依次下发给机器人控制器，则可控制轴件做相应的旋转运动。

轴件分别绕U1、U2、U3、U4执行完1、2、3步骤后，得到f0-f7共8个力。根据图6，设f0-f7中最小力对应的方向记为d，其中，d取值范围为{0,1,2,3,4,5,6,7}。

步序2.控制轴件在圆锥运动区做圆锥运动并采集力数据：

如图7所示。根据步序1中得到的圆锥运动区间，轴件做相应的圆锥运动，图7中各名词解释说明如下：

Us：1步骤用到的作为旋转轴的单位向量，在{P0}的X-Y平面内；

Ue：3步骤用到的作为旋转轴的单位向量，在{P0}的X-Y平面内；

1：轴件从初始姿态绕Us逆时针旋转α后到达圆锥运动起始姿态的过程；

2：轴件从圆锥运动起始姿态运动到圆锥运动终止姿态的过程；

3：轴件从圆锥运动终止姿态绕Ue顺时针旋转α后回到初始姿态的过程；

L：建立的轴件轴线向量，用来简化表示轴件本体的运动状态；

Ls：1步骤结束后的轴件轴线向量状态；

Le：2步骤结束后的轴件轴线向量状态；

轴件在执行第2步骤时，通过力传感器采集轴件沿其轴线方向上的受力，记为：

f＝{f_k|k＝0,1,2,...,n} (1)

同时也要记录在采集轴件沿其轴线方向上的受力时，轴件的轴线向量和圆锥运动起始姿态时的轴线向量在{P0}的X-Y平面内的投影间的夹角：

θ＝{θ_k＝kv_cT|k＝0,1,2,...,n} (2)

式中n的大小取决于轴件做圆锥运动时的速度v_c和轴孔力控装配系统的控制周期T，即：

轴件做圆锥运动运动学说明：

上一步序中最后得到了方向d，则根据图7，轴件做圆锥运动过程中步骤1和步骤3要用到的Us和Ue为：

m＝(d+7)mod8 (14)

同样的，在分析说明轴件运动学前，解释一些参数含义。设轴件绕Us、Ue的旋转速度为v_s，旋转角分别为-α、+α；轴件初始姿态为q₀(四元数形式)；子步骤2中轴件做圆锥运动的速度为v_c；轴件运动时的时间以控制周期数k表示，简称为时刻k，k初始值为0；系统控制周期为T；轴件做圆锥运动时的圆锥运动区间为

<1>步骤1中，轴件绕Us逆时针旋转，k时刻轴件理论上已旋转角度为：

α_k＝-kv_sTk＝0,1,2,...,αmod v_sT (15)

轴件从初始姿态q₀转α_k后姿态的变化可以用一四元数表示：

m含义见公式(14)，则轴件在k时刻的姿态可以通过四元数乘法获得：

q_k＝q₀q_m (17)

k的取值是0,1,2,...,αmod v_sT，则相应可得到一系列q_k，将这些q_k依次通过上位机下发给机器人控制器，则可控制轴件做相应的旋转运动，完成步骤1。

<2>步骤2中，首先k要清零。设k时刻时，轴件轴线向量为Lk。已知轴件做圆锥运动时起始姿态的轴线向量为L0，L0就是步骤1完成后的轴线向量。圆锥运动过程中，轴线向量Lk与初始的L间的夹角是固定的，也就是步骤1中提到的α。根据这个事实，可以得知既然L0是轴件绕Us逆时针旋转α得到的，那么Lk就是轴件绕Uk逆时针旋转α得到的，Uk与U0都是{P0}的X-Y平面内的单位向量。

将k时刻，Uk与U0间的夹角记为：

则Uk为：

m含义见公式(14)。轴件从初始姿态q₀绕Uk逆时针旋转α后姿态的变化可以用一四元数表示：

则轴件在k时刻的姿态可以通过四元数乘法获得：

q_k＝q₀q_m (21)

k的取值是则相应可得到一系列q_k，将这些q_k依次下发给机器人控制器，则可控制轴件做圆锥运动，完成步骤2。

<3>步骤3中，首先k清零。轴件绕Ue顺时针旋转，k时刻轴件理论上已旋转角度为：

α_k＝-α+kv_sTk＝0,1,2,...,αmod v_sT (22)

轴件从初始姿态q₀转α_k后姿态的变化可以用一四元数表示：

m含义见公式(14)，则轴件在k时刻的姿态可以通过四元数乘法获得：

q_k＝q₀q_m (24)

k的取值是0,1,2,...,αmod v_sT，则相应可得到一系列q_k，将这些q_k依次通过上位机下发给机器人控制器，则可控制轴件做相应的旋转运动，完成步骤3。

轴件按步骤1、2、3做完圆锥运动时，在步骤2阶段，轴件轴线每到一个Lk时，要通过力传感器采集轴件轴线方向上的受力(力传感器Z轴读数)，则步骤2结束后我们能得到一个力序列，记为：

步骤2中的θ_k也要记录，称其为角度序列，记为：

步序3.计算孔位方向：

孔位计算方式提出两种计算方法。一种方法为：将步序2中圆锥运动过程中采集到的力序列f＝{f_k|k＝0,1,2,...,n}中取最小值，最小力所对应的θ方向即是孔位方向。

然而，采用这种方式存在一种问题：由于控制系统属于离散系统，若v_c取较大值，那么Δθ就会较大，n就会很小，也就是运动速度高但力数据采样点很稀疏，判别出来的方向误差较大，失败率较高。反之，若v_c取较小值，那么Δθ就会较小，n就会很大，也就是运动速度低，但力数据采样点很密集，判别出来的方向误差较小但速度慢。

为了兼顾速度问题与误差问题，本发明又提出另一种方法，即多项式插补方法。

首先，轴件在做圆锥运动时的v_c取较大值，以由此得到的力序列作为纵坐标，以角度序列作为横坐标，再做三次多项式样条插值。插值步长可以选得小一些，则插值后得到新力序列和对应的新角度序列。找到新力序列中的最小值，按照映射关系找到其对应的新角度序列中的角度θ，由此θ可计算出V，此V即为轴件之后的前进方向。按上述方法既能保证较小的搜孔时间开销，又能在一定程度上保证搜孔成功率。

采用多项式插补判别孔位方向的具体方法如下：

根据式(25)和式(26)，可大致画出轴件圆锥运动时其轴线方向上受力情况，如图8所示。原力序列f中最小值附近11个点做三次多项式样条插值，插值步长记为：

s＝ηv_cT (27)

η一般取0.001到0.01。

插值后得到的新力序列为：

f＝{f_m|m＝0,1,2,...} (28)

插值后得到的新角度序列为：

θ＝{θ_m|m＝0,1,2,…} (29)

插值后如图8所示得到新的最小力，其对应的θ是孔件孔心的方向。

一些实施例中提到的参数取值见下表。

表1参数表

另外，参考图9，在一实施例中，还提供了一种基于圆锥运动的轴孔力控装配搜孔装置，所述基于圆锥运动的轴孔力控装配搜孔装置应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述装置包括：

粗调模块100，用于控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

圆锥运动区确定模块200，用于采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

孔位方向确定模块300，用于控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

搜孔模块400，用于控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功。

以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

另外，本申请的一个实施例还提供了一种基于圆锥运动的轴孔力控装配搜孔系统，其特征在于，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述控制器包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如第一方面所述的基于圆锥运动的轴孔力控装配搜孔方法。

此外，本申请的一个实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个处理器或控制器执行，例如，被上述电子设备实施例中的一个处理器执行，可使得上述处理器执行上述实施例中的基于圆锥运动的轴孔力控装配搜孔方法。

本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统可以被实施为软件、固件、硬件及其适当的组合。某些物理组件或所有物理组件可以被实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存储器技术、CD-ROM、数字多功能盘(DVD)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。

以上是对本申请的较佳实施进行了具体说明，但本申请并不局限于上述实施方式，熟悉本领域的技术人员在不违背本申请精神的前提下还可作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (6)

1.一种基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述方法应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述方法包括以下步骤：

控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功；

所述采用圆锥运动方式进行探测，以确定轴件的圆锥运动区，包括：

采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值；

根据数值最小的力所在方向为中心确定轴件的圆锥运动区；

所述采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值，包括：

取轴件末端为机器人工具TCP点，记轴件末端在当前工具坐标系下的笛卡尔点位，将该笛卡尔点位作为起始点；

控制轴件从起始点分别朝多个方向做旋转运动，采集得到轴件在多个方位点上沿其轴线方向上力的数值；其中，每个方位点与起始点的姿态角Rx均相差角度为α，相邻方位点之间的姿态角Rx相差角度为β；

所述控制轴件从起始点分别朝多个方向做旋转运动，包括：

获取在k时刻，轴件已旋转的角度α_k和初始姿态q₀：

α_k＝-kv_sT；

采用以下公式确定轴件从初始姿态q₀旋转的角度α_k后姿态的变化q_m：

则轴件在k时刻的姿态q_k通过四元数乘法获得：

q_k＝q₀q_m；

其中，k＝0,1,2,K,n；

根据轴件在k时刻的姿态q_k控制轴件做旋转运动。

2.根据权利要求1所述的基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述控制轴件在圆锥运动区做圆锥运动，以确定孔位方向，包括：

控制轴件在圆锥运动区做圆锥运动，并通过力传感器采集轴件做圆锥运动时的传感数据，所述传感数据包括轴件做圆锥运动时的速度、轴件沿其轴线方向的力序列和方位角序列；

根据所述传感数据确定孔位方向。

3.根据权利要求2所述的基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述根据所述传感数据确定孔位方向，包括：

在轴件在做圆锥运动时的速度中取多个较大值；

以多个较大值得到的力序列作为纵坐标，以多个较大值的角度序列作为横坐标，进行三次多项式样条插值，得到新力序列和对应的新角度序列；

确定新力序列中的最小值，按照映射关系找到其对应的新角度序列中的角度；

根据所述新角度序列中的角度计算出孔位方向。

4.一种基于圆锥运动的轴孔力控装配搜孔装置，其特征在于，所述基于圆锥运动的轴孔力控装配搜孔装置应用于轴孔力控装配系统，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述装置包括：

粗调模块，用于控制机械臂的末端靠近孔位，以使轴件底面与孔件平面平行且轴件底面中心点投影落在孔件所在圆内；

圆锥运动区确定模块，用于采用圆锥运动方式进行探测，以确定轴件的圆锥运动区；

孔位方向确定模块，用于控制轴件在圆锥运动区做圆锥运动，以确定孔位方向；

搜孔模块，用于控制轴件沿所述孔位方向进行直线运动，直至检测到轴件在其轴线方向的力小于第一阈值，确定搜孔成功；

所述采用圆锥运动方式进行探测，以确定轴件的圆锥运动区，包括：

采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值；

根据数值最小的力所在方向为中心确定轴件的圆锥运动区；

所述采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值，包括：

取轴件末端为机器人工具TCP点，记轴件末端在当前工具坐标系下的笛卡尔点位，将该笛卡尔点位作为起始点；

控制轴件从起始点分别朝多个方向做旋转运动，采集得到轴件在多个方位点上沿其轴线方向上力的数值；其中，每个方位点与起始点的姿态角Rx均相差角度为α，相邻方位点之间的姿态角Rx相差角度为β；

所述控制轴件从起始点分别朝多个方向做旋转运动，包括：

获取在k时刻，轴件已旋转的角度α_k和初始姿态q₀：

α_k＝-kv_sT；

采用以下公式确定轴件从初始姿态q₀旋转的角度α_k后姿态的变化q_m：

则轴件在k时刻的姿态q_k通过四元数乘法获得：

q_k＝q₀q_m；

其中，k＝0,1,2,K,n；

根据轴件在k时刻的姿态q_k控制轴件做旋转运动。

5.一种基于圆锥运动的轴孔力控装配搜孔系统，其特征在于，所述轴孔力控装配系统包括：控制器、机械臂、传感器、轴件、以及孔件；所述轴件和传感器均设置于所述机械臂的末端，所述传感器用于检测所述轴件的六轴力/力矩，所述传感器与所述控制器通信连接；

所述控制器包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至3中任意一项所述的基于圆锥运动的轴孔力控装配搜孔方法。

6.一种计算机可读存储介质，存储有计算机可执行指令，所述计算机可执行指令用于执行如权利要求1至3中任意一项所述的基于圆锥运动的轴孔力控装配搜孔方法。