CN101116968A - 六维力传感器装配机械手姿态及力的控制方法 - Google Patents

Abstract

一种机电控制技术领域的六维力传感器装配机械手姿态及力的控制方法，步骤为：第一步，建立三联杆并联操作平台：包括上平台、下平台、调整作动臂，上平台和下平台为平板，上平台固定为机架，下平台上安装一个六维力传感器；第二步，根据微动开关和六维力传感器检测的工件姿态及装配力传送到工业控制机，经工业控制机处理后变为控制指令发送到运动控制卡，运动控制卡发出的运动指令经放大器驱动直流电机，在工件姿态调整过程中作动臂的调节位置反馈给工业控制机，经PID控制实现作动臂动作的精密控制，这样，根据六维力传感器的输出不断调整并联平台三个作动臂的长度，实现工件装配姿态精密自动调整。

Description

六维力传感器装配机械手姿态及力的控制方法

技术领域

本发明涉及的是一种机械工程技术领域的控制方法，具体是一种六维力传感器装配机械手姿态及力的控制方法。

背景技术

汽车尾气净化器生产过程中，净化器陶瓷芯载体与金属外壳装配尺寸间隙小，稍有不甚陶瓷芯就会破碎，在装配过程中需要根据陶瓷芯载体受力状态自动调整装配姿态，目前国内都是通过人工装配，每个工人每天要装配几千个，长时间工作易产生厌倦疲劳，造成不和各产品出现。在其它一些对尺寸及变形、受力要求比较严格的场合机械自动化生产中，也经常出现因没有满足要求的自动化装配装置而必须进行人工装配，降低了生产效率及质量。在自动化生产过程中急需姿态自适应、带力觉的高精度装配机械手，并在装配过程中对工件装配姿态和装配力进行实时控制。

经对现有技术的文献检索发现，中国发明专利“机器人及机器人的姿态控制方法”，申请号200410087422.3，公开号CN1590039，该专利公开了一种机器人姿态控制方法：一个或多个相位发生器用于机器人系统，且根据产生的相位来选择多个控制器中的一个，控制器根据连续相位信息控制可移动部分的驱动。该发明的实质仍是通过控制精度提高各运动部件的精度，实现机器人姿态的精密控制。存在以下不足：[1]姿态调整属于传统调整方法，不能从原理上获得更高的精度；[2]采用串联机构存在姿态调整累积误差。[3]采用采用串联机构系统本身刚度，从而导致多自由度装配精度低。

发明内容

本发明的目的在于针对现有技术的不足，针对生产线精密装配过程中缺乏精密工件姿态及力控制，提出了一种六维力传感器装配机械手姿态及力的控制方法，提高工件自动化装配的质量。

本发明是通过以下技术方案实现的，本发明包括以下步骤：

第一步，建立三联杆并联操作平台

所述三联杆并联操作平台中，上平台和下平台为平板，调整作动臂为可轴向伸缩的套筒机构，调整作动臂通过球铰链与上平台和下平台联接。上平台固定为机架，通过调整作动臂的伸缩变化实现下平台的姿态调整。下平台上安装有六维力传感器，六维力传感器下边安装指爪式夹持机械手，夹持机械手上安装微动开关作为工件夹持感知系统；

第二步，根据三联杆并联操作平台中六维力传感器的输出通过调整控制下平台位姿实现对工件姿态及装配力的控制

机械手夹牢工件后，微动开关和六维力传感器检测工件姿态及装配力并传送到工业控制机，经工业控制机处理后变为控制指令发送到运动控制卡，运动控制卡发出的运动指令经放大器驱动直流电机，在工件姿态调整过程中作动臂的调节位置反馈给工业控制机，经P工D控制实现作动臂动作的精密控制，这样，根据六维力传感器的输出不断调整并联平台三个作动臂的长度，实现工件装配姿态精密自动调整。

所述的微动开关和六维力传感器检测工件姿态及装配力，是指：抓取机械手安装微动开关，当机械手紧密抓取工件时，微动开关动作，输出信号，表示机械手与工件成为一体，通过调整并联平台三个作动臂可实现工件姿态的调整；装配过程中工件与装配壳体之间的作用力及变形通过下平台上安装的六维力传感器输出，六维力传感器输出表示三个扭矩和三个力分量，分别表示装配过程中工件所受外部扭矩及力。

所述工件装配姿态精密自动调整，是指：

(1)确定工件姿态调整方向。工件姿态方向与所受力及扭矩失量方向相反，工件所收力及扭矩由六维力传感器检测，六维力传感器输出X，Y，Z三个方向的力及绕X轴、Y轴和Z轴三个方向的扭矩，工件姿态调整方向为工件所受力及扭矩失量的反方向。

(2)确定工件姿态调整实现的并联平台作动臂单位调整量。记忆当前姿态下并联平台各作动臂长度以及并联平台下平台当前姿态参数，加上工件姿态调整所需单位变形姿态得到新的并联平台下平台的目标姿态。通过矩阵变换确定得到并联平台下平台的目标姿态所需的并联平台各作动臂长度，与记忆中的并联平台各作动臂长度相减，即可得到并联平台各作动臂的调整量。

(3)各作动臂长度按(2)所述调整量进行调整。控制器对长度需要调整的作动臂驱动电机发出控制指令，通过位置-速度双闭环实现作动臂长度调整的精确控制。

(4)检测六维力传感器输出，重复(1)到(3)直到工件姿态及受力满足要求。

与现有技术相比具有的优点，本发明采用并联机构和六维力传感器进行工件装配姿态和力的控制，通过三个平台作动臂可实现工件六自由度姿态调整，调整机构相对简单；在姿态调整过程中没有累积误差，刚度大，从而调整精度高，同时采用同样的驱动元件，可达到比采用迪卡尔平面直角坐标系统的装配机械手高5-10倍的精度。

附图说明

图1为本发明实施例实现步骤框图；

图2为本发明实施例实现结构示意图；

图3为本发明实施例实现的信息及动作传递示意图。

具体实施方式

下面结合附图对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，本实施例通过工件夹持及装配力感知检测→工件姿态调整方向确定→并联平台作动臂单位调整量确定→各作动臂长度调整→六维力传感器检测输出→装配姿态及装配力检测→姿态及装配力满足要求，姿态调整及装配完成/姿态及装配力不满足要求，重新调整实现工件姿态及装配力的精密控制。

如图2所示，本实施例采用的控制平台包括三联杆并联平台、六维力传感系统、工件夹持感知系统。所述三联杆并联平台主要包括上平台、下平台、调整作动臂，直流电机等。调整作动臂通过球铰链与上平台、下平台联接，调整作动臂为可沿轴向伸缩的套筒机构，调整作动臂的轴向伸缩由直流电机驱动，上平台固定为机架，通过调整作动臂的伸缩变化实现下平台的姿态调整。所述工件夹持感知系统包括指爪式夹持机械手及安装在上面的微动开关，机械手夹持好工件后微动开关输出信号。上平台坐标系O₀-X₀Y₀Z₀为参考坐标系，O₁-X₁Y₁Z₁为下平台、六维力传感器及工件坐标系，α，β，γ分别表示工件及下平台姿态调整的三个转动量。

如图3所示，本实施例实现过程中信息流程为：首先根据微动开关和六维力传感器检测的工件姿态及装配力传送到工业控制机，经工业控制机处理后变为控制指令发送到运动控制卡，运动控制卡发出的运动指令经放大器驱动直流电机，在工件姿态调整过程中作动臂的调节位置反馈给工业控制机，经PID控制实现作动臂动作的精密控制。

本实施实例中工业PC选用台湾研华的IPPC-9120，运动控制卡采用凌华公司的四轴运动控制卡PCI-8164，可同时控制四个伺服轴。并联平台轴联杆作动臂直流电机采用日本沙迪克(SODICK)的直线电机构成全闭环控制。六维力传感器采用安徽旭宁科技有限公司的Smart300六维力传感器。工件夹持感知系统采用OMRON公司的D2SW-3超小型微动开关实现。

本发明采用基于并联平台的工件装配姿态和力的控制方法，首先机械手夹持工件，工件夹牢后工件夹持感知系统的D2SW-3超小型微动开关动作，输出信号，表示机械手已夹持住工件，与工件成为一体，通过调整并联平台三个作动臂可实现工件姿态的调整，同时工件装配过程中受力传递到六维力传感器Smart300；装配过程中工件与装配壳体之间的作用力及及变形通过六维力传感器Smart300输出，六维力传感器Smart300输出Tx，Ty，Tz表示装配过程中工件所受外部扭矩，六维力传感器Smart300输出Fx，Fy，Fz表示装配过程中工件所受外力，六维力传感器输出信号以RS232方式传递到工业控制机IPPC-9120。工业控制机IPPC-9120根据六维力传感器输出的Tx，Ty，Tz，Fx，Fy，Fz确定工件姿态调整方向，使Tx，Ty，Tz，Fx，Fy，Fz减小的方向为正确的姿态调整方向。

根据确定的姿态调整方向，进行工件姿态调整，首先记忆当前姿态并联平台各作动臂长度l₀₁，l₀₂，l₀₃及并联平台下平台当前姿态矩阵： $T=\left[\begin{array}{cccc}{d}_{11}& d_{21}& d_{31}& \mathrm{dx}\\ d_{12}& d_{22}& d_{32}& \mathrm{dy}\\ d_{13}& d_{23}& d_{33}& \mathrm{dy}\\ 0& 0& 0& 1\end{array}\right]$ ，其中第1，2，3列分别表示下下平台坐标系坐标轴O₁X₁，O₁Y₁，O₁Z₁在上平台坐标系的方向余弦；[dx，dy，dz]^T为下平台坐标原点O₁在上平台坐标系的位置向量。根据工件姿态调整所需单位变形姿态得到新的并联平台下平台的目标姿态：

$T_N=\left[\begin{array}{cccc}{k}_x{k}_x\mathrm{vers\θ}+\mathrm{c\θ}& k_y{k}_x\mathrm{vers\θ}-k_z\mathrm{s\θ}& k_z{k}_x\mathrm{vers\θ}+k_y\mathrm{s\θ}& \mathrm{\δx}\\ k_x{k}_y\mathrm{vers\θ}+k_z\mathrm{s\θ}& k_y{k}_y\mathrm{vers\θ}+\mathrm{c\θ}& k_z{k}_y\mathrm{vers\θ}-k_z\mathrm{s\θ}& \mathrm{\δy}\\ k_x{k}_z\mathrm{vers\θ}-k_y\mathrm{s\θ}& k_y{k}_z\mathrm{vers\θ}+k_z\mathrm{s\θ}& k_z{k}_z\mathrm{verx\θ}+\mathrm{c\θ}& \mathrm{\δz}\\ 0& 0& 0& 1\end{array}\right]T.$

其中姿态调整后下平台及工件位姿矩阵，具体有以下六种情况：当下平台绕自身坐标轴O₁X₁旋转θ时，k_x＝1，k_y＝k_z＝0；下平台绕自身坐标轴O₁Y₁旋转θ时，k_y＝1，k_x＝k_z＝0；下平台绕自身坐标轴O₁Z₁旋转θ时，k_z＝1，k_x＝k_y＝0；下平台沿自身坐标轴O₁X₁移动δx时，k_x＝k_y＝k_z＝θ＝0，δy＝δz＝0；下平台沿自身坐标轴O₁Y₁移动δy时，k_x＝k_y＝k_z＝θ＝0，δx＝δz＝0；下平台沿自身坐标轴O₁Z₁移动δz时，k_x＝k_y＝k_z＝θ＝0，δx＝δy＝0。

确定得到并联平台下平台的目标姿态，得到所需的并联平台各作动臂长度 $\left|l_{1i}\right|=\sqrt{{(x_{b_{i0}}-x_{b_{i1}})}^2+{(y_{b_{i1}}-y_{b_{i1}})}^2+{(z_{b_{i0}}-z_{b_{i1}})}^2}$ ，(k_bi0，y_bi0，z_bi0)为并联平台上平台上各作动臂端点的坐标，(x_bi1，y_bi1，z_bi1)为并联平台下平台上各作动臂端点的坐标。得到的各作动臂长度l_1i与记忆中的并联平台各作动臂原长度l_0i相减，即可得到并联平台各作动臂的作动量。工业控制机IPPC-9120根据各作动臂的作动量向运动控制卡PCI-8164发出电机驱动指令，经位置PID控制实现各作动臂长度精确调整。继续检测六维力传感器Smart300的输出，重复上述过程，直到Tx，Ty，Tz，Fx，Fy，Fz小于各自域值，完成工件姿态及装配力的调整。

本实施例可实现净化器陶瓷芯载体与壳体的快速精密自动化装配，代替了原来的手工装配。每分钟装配工件30个，装配力控制≤3N，装配合格率≥97％。

Claims (7)

1.一种六维力传感器装配机械手姿态及力的控制方法，其特征在于，包括以下两个步骤：

第一步，建立三联杆并联操作平台

所述三联杆并联操作平台包括上平台、下平台、调整作动臂，上平台和下平台为平板，调整作动臂为可轴向伸缩的套筒机构，调整作动臂通过球铰链与上平台和下平台联接，上平台固定为机架，通过调整作动臂的伸缩变化实现下平台的姿态调整，在三联杆并联操作平台的下平台上安装一个六维力传感器，六维力传感器下边安装指爪式夹持机械手，夹持机械手上安装微动开关作为工件夹持感知系统；

第二步，根据三联杆并联操作平台中六维力传感器的输出通过调整控制下平台位姿实现对工件姿态及装配力的控制

机械手夹牢工件后，微动开关和六维力传感器检测工件姿态及装配力并传送到工业控制机，经工业控制机处理后变为控制指令发送到运动控制卡，运动控制卡发出的运动指令经放大器驱动直流电机，在工件姿态调整过程中作动臂的调节位置反馈给工业控制机，经PID控制实现作动臂动作的精密控制，这样，根据六维力传感器的输出不断调整并联平台三个作动臂的长度，实现工件装配姿态精密自动调整。

2.根据权利要求1所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述的微动开关和六维力传感器检测工件姿态及装配力，是指：当机械手紧密抓取工件时，机械手上安装的微动开关动作，输出信号，表示机械手与工件成为一体，通过调整并联平台三个作动臂可实现工件姿态的调整；装配过程中工件与装配壳体之间的作用力及变形通过下平台上安装的六维力传感器输出，六维力传感器输出表示三个扭矩和三个力分量，分别表示装配过程中工件所受外部扭矩及力。

3.根据权利要求1所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述工件装配姿态精密自动调整，是指：

(1)确定工件姿态调整方向；

(2)确定工件姿态调整实现的并联平台作动臂单位调整量；

(3)各作动臂长度按(2)所述调整量进行调整；

(4)检测六维力传感器输出，重复(1)到(3)直到工件姿态及受力满足要求。

4.根据权利要求3所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述的工件姿态，其方向与所受力及扭矩失量方向相反，工件所收力及扭矩由六维力传感器检测，六维力传感器输出X，Y，Z三个方向的力及绕X轴、Y轴和Z轴三个方向的扭矩，工件姿态调整方向为工件所受力及扭矩失量的反方向。

5.根据权利要求3所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述的工件姿态调整，记忆当前姿态下并联平台各作动臂长度以及并联平台下平台当前姿态参数，加上工件姿态调整所需单位变形姿态得到新的并联平台下平台的目标姿态。

6.根据权利要求3所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述的并联平台作动臂单位调整量，是指：通过矩阵变换确定得到并联平台下平台的目标姿态所需的并联平台各作动臂长度，与记忆中的并联平台各作动臂长度相减，即可得到并联平台各作动臂的调整量。

7.根据权利要求1所述的六维力传感器装配机械手姿态及力的控制方法，其特征是，所述的步骤(3)，是指：控制器对长度需要调整的作动臂驱动电机发出控制指令，通过位置-速度双闭环实现作动臂长度调整的精确控制。

Images