一种基于力反馈的机器人装配自动寻孔方法与流程

本发明属于工业机器人智能控制技术领域，具体涉及一种基于力反馈的机器人装配自动寻孔方法，基于六维力传感器反馈信息，合理估计孔相对于轴的偏移方向和偏移距离，规划寻孔策略。

背景技术：

在工业任务中，轴孔装配是最常见的工业作业之一，而轴孔最初接触过程中，首先面临的问题是“轴孔对中”，即“自动寻孔”问题。随着智能化程度的提高，对于初始位置不精确，轴与孔的中心位置存在一定的误差的情况，仅仅靠单纯的位置控制无法完成装配任务，则需要根据实际情况智能地调整轴孔相对位置。

针对寻孔过程，目前的做法有两种，其一是利用视觉信息，经过图片处理实时计算轴孔的相对位置，但是该方法需要实时处理图片信息，对于工业装配控制器的处理频率要求较高，在普通的工业任务中一般较难满足；另一种是利用六维力传感器的力反馈信息，根据力矩值求得的方向以恒定步长运动，但是这种方法会存在低效率和非通用性问题，低效率性体现在均匀步长的调整过程比较迟缓，而非通用性表现在较小偏差下较大调整步长的情况下无法适应所有的轴孔间隙。

技术实现要素：

鉴于以上问题，本发明的目的是针对现有的轴孔装配中寻孔过程的低效率恒定步长的寻孔方法在时效上和通用性的不足，提出一种基于力反馈的机器人装配自动寻孔方法。该方法采用了变步长方法，通过六维力传感器信息计算轴孔相对移动的偏移量和偏移方向，引入了等效力臂的概念，使得偏移量和等效力臂成一一对应关系，同时通过变步长的控制策略，达到了加速且稳定的轴孔装配寻孔效果，具有较强的通用性和可调节性。

本方法主要是基于力反馈的机器人装配自动寻孔方法，分为以下步骤：

步骤1：对于固定尺寸的圆轴圆孔装配过程，根据力传感器反馈值设计合理的对中方案，假设轴端面和接触孔的端面均匀接触，则轴孔之间受力均匀，接触力与接触面积有关，根据所述轴与所述孔的尺寸，离线地计算不同偏移距离下的等效力臂，并用固定阶数的函数拟合等效力臂和孔尺寸的转换关系；

步骤2：在寻孔过程中，对所述轴与所述孔接触过程的接触力利用力传感器进行信息采集，判断轴孔之间是否已经接触，若是，则继续后续步骤，否则继续采集接触力信息；

步骤3：以所述轴端面圆心为坐标原点建立坐标系，根据力传感器在轴上的安装位置，利用力传感器采集信号，将经过滤波处理的力传感器信号的力矩和力转换为相对于所述轴端面坐标系的力矩和力；

步骤4：根据转换后的力矩值确定孔相对于轴的偏移方向，根据该偏移方向确定微调方向；

步骤5：提取出步骤3中所述六维力传感器转换后的接触力/力矩信息，根据所述轴轴向方向的作用力和另外两个方向的接触力矩，计算当前作用力与作用力矩下的等效力臂；

步骤6：根据步骤1中所述拟合函数的对应关系求得步骤5所述等效力臂下的偏移距离；

步骤7：将步骤6所述偏移距离乘以可选的比例因子，再将所述比例因子与偏移距离的乘积加上固定的较小的数值，使求和结果作为寻孔策略的微调距离；

步骤8：利用机械臂控制将轴从步骤3所述接触点抬起小段距离，之后根据步骤4所述微调方向和步骤7所述微调距离移动，再次将所述轴向下移动，若轴能向下移动超过端面2 毫米，则进入下一步骤；

步骤9：根据导纳控制原理，在轴插入孔中2mm之后，根据所述力传感器力反馈信息，分别将各个方向的力/力矩值调整为相应方向上的位移/旋转变化量；

步骤10：随着轴不断深入孔中，若轴插入深度大于孔深的0.8倍，则停止装配，否则，继续重复步骤9。

所述步骤1中，在控制前根据轴孔的几何关系确定等效力臂与偏移距离的关系，是变步长寻孔方案的重要步骤。

所属步骤3中，将六维力传感器信息根据传感器安装位置，通过坐标转换将六维力信号转换为轴端坐标系的六维力信息，其中，所属坐标转换关系为三维力根据检测力在轴坐标中的坐标系分量发生转换，而三维力矩根据检测检测力矩在轴坐标中的坐标系分量发生转换，同时加上三维力所产生的力矩的矢量和。

所属步骤5和步骤6中，利用所述转换后的力矩除以轴向作用力为等效力臂，在离线计算中，所述等效力臂只与轴孔尺寸和偏移距离有关，则利用等效力臂与偏移距离的一一对应关系的离线计算的拟合函数求得偏移距离。

所述步骤7中，所述微调距离的值由两部分之和组成，其一为固定的较小的偏移量数值，其二为所述求得偏移距离与比例系数的乘积，这两部分共同形成了变步长的微调距离。

如上所述，本发明基于力反馈的机器人装配自动寻孔方法的优点在于：

引入离线计算等效力臂的概念，等效力臂的计算只与轴孔尺寸和偏移距离有关，在实际运用中等效力臂与偏移距离一一对应，当接触力任意变化时具有通用性；

变步长的微调方案避免了恒定步长下低效率和非通用性问题，在实际应用中增大了寻孔效率，对不同尺寸的轴孔具有较强的适应性，同时计算过程简单，适用于实际工业任务中。

附图说明

图1为自动寻孔方案流程图；

图2为寻孔方案示意图；

图3为轴孔接触二维视图模型图；

图4为偏移距离计算简化模型图；

图5为等效力臂计算流程图；

图6为等效力臂-偏移距离曲线图；

图7为拟合曲线与原等效力臂-偏移距离曲线对比图；

图8为轴孔寻孔方案仿真结果图；

具体实施方式

本发明在于提供一种基于等效力臂的变步长寻孔方法，应用于工业装配过程，解决孔的对中问题，加速寻孔过程。其中，采用了结合离线在线综合计算手段，在轴孔接触过程中变步长的自动寻孔方案，利用六维力传感器信息，面向实际操作中目标位置的不精确条件下轴和孔之间的目标点位置存在未知误差情况下，使轴能根据受力方向和偏移方向做出相应的调整，达到变步长自动寻孔的效果，具有增快寻孔过程的优势，在工业装配中面向不精确目标下的寻孔过程提供了一种新的具有可实践性的方案，在不精确的目标位置下仍然能够实现轴孔装配过程，具有较强的现实意义。

以下结合实施例及附图对本发明作进一步详细的说明，但本发明实施方式不限于此。本发明可应用于基于位置控制方式的多自由度机械臂机器人上，针对大部分普通的工业机器人均具有较强的通用性。对于不精确的目标位置下的轴孔装配过程，本发明主要解决轴孔之间的主动调整运动过程，如图1所示自动寻孔方案过程分为以下步骤：

步骤1：在控制方案之前根据轴孔尺寸离线计算等效力臂与偏移距离的数学关系。

在步骤1中，对于固定尺寸的圆轴圆孔装配过程，根据力传感器反馈值设计合理的对中方案，假设轴端面和接触孔的端面均匀接触，则轴孔之间受力均匀，接触力与接触面积有关，根据所述轴与所述孔的尺寸，离线地计算不同偏移距离下的等效力臂，并用固定阶数的函数拟合等效力臂和孔尺寸的转换关系。

假设轴端面和接触孔的端面均匀接触，则轴孔之间受力均匀，接触力分布与接触面积有关，如图2所示。建立二维接触模型如图3所示。

其中，根据二维模型，已知条件为轴端受到的力矩和力，求解对象为轴末端在当前状态下应对中平移的距离和方向。本方法首先计算出轴端应该移动距离，然后在根据X和Y方向的力矩值确定移动方向。

首先，为了方便计算偏移距离，如图4所示简化二维模型。设定在接触点作用力均匀分布的条件下，孔端面对轴在Z轴负方向的作用力Fz恒定条件下，Fz均匀作用在接触面积S上，如图4所示，在考虑偏移距离时，不考虑偏移方向，则将轴孔放置在如下图4所示的坐标系中X轴线上，之后根据轴端与孔圆心的偏差距离，在作用力F0下，计算在接触面积(如图4 所示阴影部分)产生的的力矩。

首先，如图3所示，根据力矩关系可知：

其中，总力矩用M表示，可分解为机械臂末端轴X和Y方向的转动力矩，d表示平均距离，Fz表示Z方向上的作用力，可以推导出

将图4的阴影部分算作轴的受力面积，根据实际情况，我们要计算受力点的整体相对于轴中心产生的力矩，由图4所示，在Y轴方向上作用力对轴心产生作用力矩的面积是对称的，则在作用力均匀分布的条件下产生的力矩会相互抵消，则力矩的计算公式为

其中，S阴表示阴影部分的面积，F0表示Z轴方向上的作用力，xi表示每个微分点相对于坐标原点的x坐标，会随着点的移动而变化，dSi表示对应微分点的面积。

根据如图4所示几何关系，在固定的偏移距离d下，阴影部分面积形状固定，则公式(3) 可以推得

其中，表示等效力臂，由积分中值定理可知，在固定的偏移距离d下，存在且只与如图4所示接触面积的形状有关。

其中，由受力分析图3可知，轴端面对力传感器产生的力矩均由轴向作用力Fz产生，随着Fz增大，对应产生的力矩值成正比增大，由实际接触过程中受力模型可知，可知无论作用力Fz对环境(孔端面)嵌入深度多大，或者产生的力多大，在接触力作用均匀的前体下，只要是同样的平均偏移距离，对应产生的力矩值均为成比例变化。由于力传感器只能检测力矩和作用力的值，则由公式(2)可以推得，在轴端面形状和孔端面形状一定的条件下，偏移距离 d与M/Fz相关。

由公式(4)可得，偏移距离d对应固定的等效力臂求得，而等效力臂表示M/Fz，则根据轴孔的尺寸在插孔实验开始前先离线计算轴端对中过程中应该偏移距离d和等效力臂的关系。

在实际运用过程中，复杂的积分算法不适用于常见的编程环境且计算过程复杂，不具有通用性。所以选择通过随机大量取点的方法来计算等效力臂计算流程图如图5所示。

如流程图5所示，可计算在不同的偏移距离下等效力臂的值。为了验证寻孔方案效果，在离线求解过程中，根据不同的轴心与孔圆心偏移距离d，设置轴半径r＝1cm,孔半径R＝1.1cm，则求出不同偏移距离下偏移距离d与等效力臂曲线图，如图6所示。

在实际使用过程中，根据如上图6所示的等效力臂与偏移距离的关系，直接从已知力传感器采集的作用力和力矩值数据由公式(4)计算得到得到等效力臂，再从图6所示曲线中取值偏移距离d作为移动量。

其中，为了更好的得到偏移距离的计算值dc，在经过离线计算得到如图6中等效力臂- 偏移距离曲线之后，通过多项式拟合的方式得到等效力臂-偏移距离的关系，则表达式为：

其中n表示多项式阶数，Ci表示多项式常数矩阵，在实际计算中，可将等效力臂-偏移距离曲线拟合成n阶多项式，用解析式求解偏移距离dc来代替离散点取值的方法。为了验证拟合效果，取值阶数n＝4，计算得到拟合的多项式参数值Ci为[3.2303-8.7561 8.48558-4.9281 2.0636]，拟合曲线与原等效力臂-偏移距离曲线如图6所示。

如图6所示，在四阶模型下，拟合的等效力臂-偏移距离曲线与原曲线的偏差值最大为 0.0071cm，偏差绝对值的均值为0.0035cm，误差值较小，符合实际控制要求，证明拟合曲线已经基本吻合原曲线，能满足dc求解要求，求得等效力臂与偏移距离的数学关系，主要的求解过程流程图如图5所示。

步骤2：在寻孔过程中，对所述轴与所述孔接触过程的接触力利用力传感器进行信息采集，判断轴孔之间是否已经接触，若是，则继续后续步骤，否则继续采集接触力信息。

步骤3：以所述轴端面圆心为坐标原点建立坐标系，根据力传感器在轴上的安装位置，利用力传感器采集信号，将经过滤波处理的力传感器信号的力矩和力转换为相对于所述轴端面坐标系的力矩和力。

其中，所属坐标转换关系为三维力根据检测力在轴坐标中的坐标系分量发生转换，而三维力矩根据检测检测力矩在轴坐标中的坐标系分量发生转换，同时加上三维力所产生的力矩的矢量和。

步骤4：根据转换后的力矩值确定孔相对于轴的偏移方向，根据该偏移方向确定微调方向。

根据如图3所示的轴与孔的相对位置，可以得出偏移方向与轴末端在X与Y方向上受到的力矩值的比例相关。可得得出，移动方向如下公式：

步骤5：提取出步骤3中所述六维力传感器转换后的接触力/力矩信息，根据所述轴轴向方向的作用力和另外两个方向的接触力矩，计算当前作用力与作用力矩下的等效力臂。

利用公式(4)，带入X与Y方向上受到的合力矩值作为力矩输入，将Z方向上受到的作用力值为力输入，计算当前接触下的等效力矩。

步骤6：根据步骤1中所述拟合函数的对应关系求得步骤5所述等效力臂下的偏移距离；

利用所述转换后的力矩除以轴向作用力为等效力臂，在离线计算中，所述等效力臂只与轴孔尺寸和偏移距离有关，则利用等效力臂与偏移距离的一一对应关系的离线计算的拟合函数求得偏移距离。

步骤7：将步骤6所述偏移距离乘以可选的比例因子，再将所述比例因子与偏移距离的乘积加上固定的较小的数值，使求和结果作为寻孔策略的微调距离；

为了解决在实际算法中对实验条件要求较高的问题，结合直接计算法和接近法的优点，将直接计算结果转化为变步长的移动偏移量，经过多次移动来解决问题，以减小计算量的误差对系统的影响。

首先需要计算离散的如图6所示等效力臂-偏移距离曲线图作为每次控制离散点取值的基础。在具体执行过程中，离散点轴孔装配变步长微调方法将直接计算寻孔方案和接近法相结合，每次运行循环移动距离的数学表达式如下所示：

Δd＝d0+dc·km (7)

其中，d0表示预先设定的最小移动距离，保证在计算系统完全崩溃的时候该方法还能继续运行，di表示根据事先运算的从如图6中等效力臂-偏移距离曲线中，通过当前反馈值通过 M/Fz计算得到等效力臂值选取对应的偏移距离dc，满足dc∈(02)，km表示距离比例因子，取值范围一般为km∈(01)，根据实际情况确定，表示直接计算的离散点取值在变步长控制中的影响程度。

步骤8：利用机械臂控制将轴从步骤3所述接触点抬起小段距离，之后根据步骤4所述微调方向和步骤7所述微调距离移动，再次将所述轴向下移动，若轴能向下移动超过端面2 毫米，则进入下一步骤。

其中，若轴能向下移动超过端面2毫米，则对应表示寻孔过程结束，轴已经插入孔中一小段距离。

步骤9：根据导纳控制原理，在轴插入孔中2mm之后，根据所述力传感器力反馈信息，分别将各个方向的力/力矩值调整为相应方向上的位移/旋转变化量。利用导纳控制方法作为主动柔顺控制插孔过程的控制算法，将已经插入孔中一点的轴继续向下运动，实现柔顺控制。

其中，导纳控制部分根据控制原理，将机械臂末端的力反馈作为末端工作空间内位置控制的输入，对应导纳控制对目标运动的反馈作为输出，完成机械臂柔顺控制系统的力反馈外环的控制基础。根据控制器和上位机的控制周期，对应机械臂各个关节的控制为周期性更新的离散控制模式，则对应的控制原理为：

其中，偏移方向期望运动速度期望位置为初始位置xd＝x0，由周期控制下离散的变化过程，将公式变形可得

变形得到

其中k＝1,2,3…，则对应下一时刻的控制速度为

其中，k表示运行周期，随着运行周期，由公式(9)～(10)可通过当前位置及速度信息，通过导纳控制率计算得到下一时刻的运动速度，即可实现主动柔顺控制。

步骤10：随着轴不断深入孔中，若轴插入深度大于孔深的0.8倍，则停止装配，否则，继续重复步骤9。

为了验证本发明提出的寻孔方案的有效性，设置参数为轴半径r＝1cm，孔半径R＝1.1cm，以轴端面中心为原点建立坐标系，第一步建立轴孔之间等效力臂与偏移量的曲线数据关系，第二步根据接触力矩和接触力预计移动方向，之后利用公式(7)中变步长的寻孔方模拟轴孔自动寻孔方案，运算结果如图8所示。

如图8所示，以轴圆心为原点建立坐标轴，如图四组数据表示在轴与孔之间存在随机产生的位置偏差下，该算法在四种情况下均能使轴顺利移动到正确的孔的位置，说明该方案正确有效。