[发明专利]一种基于圆锥运动的轴孔力控装配搜孔方法及系统

权利要求书

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

所述方法包括以下步骤：

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

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

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

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

2.根据权利要求1所述的基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述采用圆锥运动方式进行探测，以确定轴件的圆锥运动区，包括：

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

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

3.根据权利要求2所述的基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述采用圆锥运动方式进行探测，通过传感器采集轴件在多个方位点上沿其轴线方向上力的数值，包括：

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

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

4.根据权利要求3所述的基于圆锥运动的轴孔力控装配搜孔方法，其特征在于，所述控制轴件从起始点分别朝多个方向做旋转运动，包括：

获取在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控制轴件做旋转运动。

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

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

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

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

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

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

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

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