CN114986515A - 一种位姿解耦的绝缘子更换机器人动态装配方法 - Google Patents

Abstract

本发明涉及一种位姿解耦的绝缘子更换机器人动态装配方法。包括：步骤1：通过深度相机采集绝缘子表面点云图像，定位销孔平面，并提取其法向量作为销孔轴线方向；步骤2：根据销孔轴线方向和右手定则，确定机械臂末端的目标姿态；控制机械臂移动，调整推销工具的目标姿态；步骤3：通过柔顺控制算法对位置进行调整，使得推销工具将定位销推出销孔，完成位姿解耦动态装配。

Description

一种位姿解耦的绝缘子更换机器人动态装配方法

技术领域

本发明涉及柔顺装配领域，具体地说是一种位姿解耦的绝缘子更换机器人动态装配方法。

背景技术

机器人控制技术的发展使机器人可以完成多种精密装配任务。在这些装配任务中，作业平台和机器人底座之间的相对位置固定，机器人在柔顺装配方法的基础上，根据接触力的不同取值，做出不同动作。但当作业平台位于具有柔索特性的环境中时，机器人与作业平台之间会形成紧密的动力学刚柔耦合关系。柔索的位置会因受到机器人的挤压而产生复杂的动态变化，位置的动态变化又会反过来造成接触力具有瞬时变化的动态噪声，进而大大影响装配动作的柔顺性。因此人们将这类装配平台不固定，且位置受机器人运动影响的装配任务定义为动态装配任务，柔索装配环境是其中的典型应用场景。如何在动力学刚柔耦合的环境中保证装配动作的柔顺性，是动态装配任务中的关键科学问题。

在绝缘子更换任务中，需要首先将位于两片绝缘子中间起固定作用的绝缘销推出，在此之前需要将推销工具装配到销孔中。由于绝缘子位于具有柔索特性的输电线路上，因此这一装配任务可以被认为是动态装配任务。将推销工具简化成轴体模型，将销孔简化孔模型后，可以用轴孔装配的控制方法进行分析。在一般的轴孔装配任务中，通常可以采用基于物理建模的轴孔装配方法，基于被动柔顺控制、主动柔顺控制的柔顺装配方法以及力/位混合控制装配方法等。但是在这些装配任务中，装配环境模型一般都固定不变。

发明内容

本发明解决的是使用传统控制方法无法完成动态装配任务的问题。通过使用位姿解耦的装配方法，在深度视觉的参与下，提取点云法向量，进而是实现姿态调整，在这之后再单独对位置进行柔顺控制。

一种位姿解耦的绝缘子更换机器人动态装配方法，用于在具有柔索特性的输电线路上更换绝缘子时，将推销工具与销孔进行位姿解耦动态装配，包括以下步骤：

步骤1：通过深度相机采集绝缘子表面点云图像，定位销孔平面，并提取其法向量作为销孔轴线方向；

步骤2：根据销孔轴线方向和右手定则，确定机械臂末端的目标姿态；控制机械臂移动，调整推销工具的目标姿态；

步骤3：通过柔顺控制算法对位置进行调整，使得推销工具将定位销推出销孔，完成位姿解耦动态装配。

所述深度相机采集的绝缘子表面点云图像中包括定位销孔。

所述定位销孔平面包括：使用深度相机获取销孔一侧绝缘子表面平整区域点云，将该区域点云法向量当作销孔轴线方向。

提取绝缘子表面点云法向量是采用PCA算法。

使用PCA方法对点云的法向量进行估计，包括：

a.定义点集的中心点记为m，则有：

其中，销孔点云集合X＝{x_i}；

b.通过最小二乘法，令点集中的所有点到中心点的向量与法向量n的点积平方和最小，并通过优化的方式求解n，优化目标为：

将x_i-m定义为y_i，可以将上式改写为：

令

y_iy_i ^T＝s_i

得到：

将上式中元素求和矩阵代替后，可以得到：

其中S为点集的协方差矩阵，其表示形式如下：

使用拉格朗日乘子法对带约束问题进行求解，则有：

L(n,λ)＝n^TSn-λ(n^Tn-1)

分别令L对n和λ求偏导，并令其等于0，则有：

求得最终的n为S矩阵中关于特征值λ的特征向量；

对S矩阵进行奇异值分解(SVD)，即：

S＝U∑V^T

计算最小特征值对应的特征向量，即为法向量n。

所述根据销孔轴线方向和右手定则，确定机械臂末端的目标姿态，包括：

a.将提取到的孔轴线方向单位化，将其定义为目标姿态的z轴；

b.找到任意一个与z轴垂直的单位向量，将其定义为目标姿态的y轴；

c.根据右手定则得到目标姿态x轴。

所述控制机械臂移动调整推销工具的目标姿态包括：

计算机械臂末端连杆坐标系的当前姿态到目标姿态之间的期望旋转矩阵；

利用姿态控制算法按照期望旋转矩阵实现姿态调整；所述姿态控制算法包括角度控制，速度控制；

当机械臂末端到达目标姿态时，其末端的推销工具刚好对准销孔，推销工具轴线与销孔轴线一致。

所述对位置进行调整是采用柔顺控制算法实现推销工具向销孔方向的进给位置控制。

所述推销工具为在机械臂末端安装的推杆。

一种位姿解耦的绝缘子更换机器人动态装配系统，包括：深度相机、控制器、机械臂；

所述深度相机，用于采集绝缘子销孔图像发送给控制器；

所述控制器，用于根据销孔位置计算销孔轴线方向，获取机械臂末端的目标姿态和位置，控制机械臂带动末端的推销工具移动，将定位销推出销孔；

所述机械臂，末端安装有用于将定位销推出的推销工具。

本发明具有的有益效果及优点：

本发明方法通过位姿解耦控制，可以控制机械臂模块的相机采集销孔图像，控制器根据销孔轴线位置确定机械臂末端的目标姿态，从而控制机械臂末端带动推销工具移动至目标姿态，再采用柔顺控制算法对进给位置进行调整，将推销工具插入绝缘子上的销孔中，推出定位销，本发明方法提高了动态环境下装配的成功率。使控制流程更加透明化。

附图说明

图1孔装配中的绝缘子表面图。

图2为位姿解耦的动态装配方法框图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方法做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

除非另有定义，本文所使用的所有技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

如图1所示，为孔装配中的绝缘子表面图。

本发明需要的硬件设备为：机械臂、控制器、深度相机，深度相机用于拍摄绝缘子表面销孔图像发送给控制器，机械臂末端连接有推销装置(推杆)，推销装置用于将销孔中的销子推出，控制器根据深度相机的机器视觉识别定位销孔的姿态并设置为目标姿态，并由目标姿态和机械臂基坐标系下末端坐标系姿态得到期望旋转矩阵，进而利用姿态控制算法，使得推销工具轴向和销孔轴向一致，最后在位置柔顺控制作用下，将推销工具插入绝缘子上的销孔中，完成装配推出销子。

如图2所示，本发明是一种位姿解耦的绝缘子更换机器人动态装配方法，其具体实施步骤如下：

步骤1：通过深度相机采集包括销孔的绝缘子表面点云，并提取其法向量看作孔轴线方向。

步骤2：将提取到的孔轴线方向作为机械臂目标姿态的z轴，并根据右手定则确认目标姿态中其他坐标轴的方向，定义为目标姿态。最后通过目标姿态与当前机械臂末端连杆坐标系之间的坐标变换得到期望旋转矩阵，进而实现姿态控制。

步骤3：然后通过柔顺控制算法对位置进行调整，使得机械臂末端的推销工具插入销孔，将定位销推出，完成装配。

步骤1在于使用深度相机获取销孔一侧绝缘子表面平整区域点云，然后鉴于此区域平面近乎与销孔轴线方向垂直，因此将此处点云法向量当作销孔轴线方向。点云法向量提取通过PCA算法得到。

使用PCA方法对点云的法向量进行估计。首先设点集X＝{x_i}，将点集的中心点记为m，则有：

由于一个平面的法向量与平面中所有的向量都是相互垂直的关系，因此根据相互垂直的向量点积等于0这个特征，我们可以找到点集所表示平面的法向量，并将其定义为n。在这里，通过最小二乘的思路，令点集中的所有点到中心点的向量与法向量n的点积平方和最小，并通过优化的方式求解n，优化目标为:

将x_i-m定义为y_i，可以将上式改写为:

令

y_iy_i ^T＝s_i

得到:

将上式中元素求和矩阵代替后，可以得到:

其中S为点集的协方差矩阵，其表示形式如下:

使用拉格朗日乘子法对带约束问题进行求解，则有:

L(n,λ)＝n^TSn-λ(n^Tn-1)

分别令L对n和λ求偏导，并令其等于0，则有:

求得最终的n为S矩阵中关于特征值λ的特征向量。因此我们对S矩阵进行奇异值分解(SVD)，即:

S＝U∑V^T

U中的列向量即为对角线位置特征值对应的特征向量，也就是矩阵S对应的特征向量。其中最大的特征值对应的特征向量为此点集中的点的主方向，而最小特征值对应的特征向量表示了与所有样本点所代表的方向都无关的方向。也就是我们需要找的法向量。

步骤2在于将提取到的孔轴线方向单位化后作为机械臂目标姿态的z轴，进而确定目标姿态坐标系方位。然后找到任意一个与z轴垂直的单位向量，将其定义为目标姿态的y轴，最后根据右手定则得到目标姿态x轴的表示形式。得到目标姿态的表示之后，通过坐标变换得到目标姿态与当前机械臂末端连杆坐标系之间期望旋转矩阵，然后利用姿态控制算法实现姿态调整。姿态控制算法有角度控制，速度控制等。

步骤3在于使用位置柔顺控制方法对轴体前进方向的进给位置进行控制。由于对销孔姿态在动态环境中变化不大，因此可以利用位姿解耦控制，即首先根据点云处理结果进行姿态控制，使得机械臂末端带动推销工具移动至销孔前端，使得推销工具轴线与销孔轴线一致。当机械臂末端姿态调整至目标姿态后，再单独对进给位置进行调整，使推销工具箱销孔方向前进，将定位销推出销孔。

柔顺控制算法可以采用例如：阻抗控制、导纳控制、力/位混合控制、强化学习控制等现有算法。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，用于在具有柔索特性的输电线路上更换绝缘子时，将推销工具与销孔进行位姿解耦动态装配，包括以下步骤：

步骤1：通过深度相机采集绝缘子表面点云图像，定位销孔平面，并提取其法向量作为销孔轴线方向；

步骤2：根据销孔轴线方向和右手定则，确定机械臂末端的目标姿态；控制机械臂移动，调整推销工具的目标姿态；

步骤3：通过柔顺控制算法对位置进行调整，使得推销工具将定位销推出销孔，完成位姿解耦动态装配。

2.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述深度相机采集的绝缘子表面点云图像中包括定位销孔。

3.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述定位销孔平面包括：使用深度相机获取销孔一侧绝缘子表面平整区域点云，将该区域点云法向量当作销孔轴线方向。

4.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，提取绝缘子表面点云法向量是采用PCA算法。

5.根据权利要求4所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，使用PCA方法对点云的法向量进行估计，包括：

a.定义点集的中心点记为m，则有：

其中，销孔点云集合X＝{x_i}；

b.通过最小二乘法，令点集中的所有点到中心点的向量与法向量n的点积平方和最小，并通过优化的方式求解n，优化目标为：

将x_i-m定义为y_i，可以将上式改写为：

令

y_iy_i ^T＝s_i

得到：

将上式中元素求和矩阵代替后，可以得到：

其中S为点集的协方差矩阵，其表示形式如下：

使用拉格朗日乘子法对带约束问题进行求解，则有：

L(n,λ)＝n^TSn-λ(n^Tn-1)

分别令L对n和λ求偏导，并令其等于0，则有：

求得最终的n为S矩阵中关于特征值λ的特征向量；

对S矩阵进行奇异值分解(SVD)，即：

S＝U∑V^T

计算最小特征值对应的特征向量，即为法向量n。

6.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述根据销孔轴线方向和右手定则，确定机械臂末端的目标姿态，包括：

a.将提取到的孔轴线方向单位化，将其定义为目标姿态的z轴；

b.找到任意一个与z轴垂直的单位向量，将其定义为目标姿态的y轴；

c.根据右手定则得到目标姿态x轴。

7.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述控制机械臂移动调整推销工具的目标姿态包括：

计算机械臂末端连杆坐标系的当前姿态到目标姿态之间的期望旋转矩阵；

利用姿态控制算法按照期望旋转矩阵实现姿态调整；所述姿态控制算法包括角度控制，速度控制；

当机械臂末端到达目标姿态时，其末端的推销工具刚好对准销孔，推销工具轴线与销孔轴线一致。

8.根据权利要求1所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述对位置进行调整是采用柔顺控制算法实现推销工具向销孔方向的进给位置控制。

9.根据权利要求1-8任意一项所述的一种位姿解耦的绝缘子更换机器人动态装配方法，其特征在于，所述推销工具为在机械臂末端安装的推杆。

10.一种位姿解耦的绝缘子更换机器人动态装配系统，其特征在于，包括：深度相机、控制器、机械臂；

所述深度相机，用于采集绝缘子销孔图像发送给控制器；

所述控制器，用于根据销孔位置计算销孔轴线方向，获取机械臂末端的目标姿态和位置，控制机械臂带动末端的推销工具移动，将定位销推出销孔；

所述机械臂，末端安装有用于将定位销推出的推销工具。

Images