CN104729455A - 一种基于测量数据的管路柔性装焊机器人位姿计算方法 - Google Patents

Abstract

本发明提出一种针对基于视觉测量和机器人的管路柔性装焊方案的机器人位姿计算方法，其特征在于：包括下述步骤：(1)计算机器人回归零位时导管两端中心点坐标；(2)确定导管固联坐标系，计算机器人回归零位时导管位姿；(3)计算导管对接装配时，机器人TCP位姿；(4)计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。本发明借助视觉测量技术和工业机器人技术进行管路的柔性生产，省去了繁多的样管和夹具，提升了通用性，降低了成本，提升了管路生产效率。

Description

一种基于测量数据的管路柔性装焊机器人位姿计算方法

技术领域

本发明涉及一种基于测量数据的管路柔性装焊机器人位姿计算方法，是基于装焊系统的测量数据及设备位置，计算导管装配时机器人位姿的方法。属于计算机辅助设计制造领域。

背景技术

管路系统在几乎所有机电产品中都有广泛的应用，其功能主要是输送介质的通道。特别是在航空航天领域，导管都是飞机或者各类航天器的重要组成部分，管路系统能够正常运行是决定产品性能和质量的一个很重要的条件。

目前，航空航天类管路类型繁多、接口复杂而且多以单件或小批量的生产形式存在。传统的导管焊接需要设计专用的组合夹具，效率很低，且夹具通用性低，准备周期长。针对不同的焊接导管，需要设计大量不同的专用夹具，这大大增加了航空航天产品的研制时间和成本，同时也给生产管理带来极大不便。而且在生产过程中，由于受到操作者经验水平、工装、定位精度以及测量精度等因素的影响，需对导管焊接后存在较大的误差和变形，无法达到精度要求，可见传统的导管生产工艺已经无法满足现代产品的制造需求，所以很有必要研究建立面向管路生产的柔性装焊系统。

所设计的管路柔性装焊方案基于视觉测量和机器人，根据测量到的导管数据，通过计算得到机器人末端执行器位姿，然后控制机器人抓取导管完成对接装配任务。所以如何计算机器人手部在空间的位姿是实现管路柔性装焊的关键问题。

发明内容

本发明的目的在于提供一种针对基于视觉测量和机器人的管路柔性装焊方案的机器人位姿计算方法，用以快速计算机器人末端执行器位姿，控制机器人抓取导管运动，实现管路的柔性装焊。

管路柔性装焊方案：管路柔性装焊示意图如图1所示。系统工作时，机器人抓取两段导管在计算得到的机器人控制文件控制下运动，实现两段导管的对接装配。

图1中出现参数，说明如下：

O-XYZ：全局坐标系取机器人的基座标系。

O_T-X_TY_TZ_T：机器人工具坐标系即TCP坐标系。

O_L-X_LY_LZ_L：导管固联坐标系，在导管模型上定义。

导管设计数据，即导管三维数模信息，包括导管对接端面中心点坐标P_c(x_c,y_c,z_c)，导管非对接端面中心点坐标P_fc(x_fc,y_fc,z_fc)，机器人抓取直线段方向V_l(vx_l,vy_l,vz_l)，导管对接端面法向V_p(vx_p,vy_p,vz_p)。

其他技术参数，说明如下：

导管测量数据：包括导管对接端(余量端)中心点测量坐标P_m(x_m,y_m,z_m)；导管非对接端中心点测量坐标P_Fm(x_fm,y_fm,z_fm)；机器人抓取导管测量时的TCP测量位姿T^m _TCP；机器人回归零位时的TCP零位位姿T^s _TCP；导管余量A(Allowance)；导管半径R。

机器人回归零位时，导管对接端(余量端)中心点坐标P_s(x_s,y_s,z_s)，导管非对接端中心点坐标P_Fs(x_fs,y_fs,z_fs)，导管对接端(余量端)中心点在机器人TCP坐标系下的坐标P_tcp(x_t,y_t,z_t)，导管非对接端中心点在机器人TCP坐标系下的坐标P_Ftcp(x_ft,y_ft,z_ft)，导管位姿T^s _L，机器人TCP点和导管非对接端中心点在导管固联坐标系下的坐标P^p _tcp(x_tp,y_tp,z_tp)和P_Fp(x_fp,y_fp,z_fp)。

机器人抓取导管对接时，导管对接端中心点坐标即导管对接位置P_d(x_d,y_d,z_d)，导管非对接端中心点坐标P_Fj(x_fj,y_fj,z_fj)，导管位姿T^j _L，机器人TCP对接位姿T^j _TCP。

本发明提出的一种基于测量数据的管路柔性装焊机器人位姿计算方法，包括下述主要步骤：

步骤一：计算机器人回归零位时导管两端中心点坐标。

步骤二：确定导管固联坐标系，计算机器人回归零位时导管位姿。

步骤三：计算导管对接装配时，机器人TCP位姿。

步骤四：计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。

本发明给出的一种基于测量数据的管路柔性装焊机器人位姿计算方法，其优点及功效在于：与传统的管路装配焊接方法相比，本发明省去了繁多的样管和夹具，提升了通用性，降低了成本；能够快速测量导管参数并且根据测量数据快速计算机器人位姿，控制机器人运动实现导管对接装配，提升了管路生产效率。

附图说明

图1为本发明的管路柔性装焊示意图。

图2为本发明中机器人位姿计算流程图。

图3为本发明中导管固联坐标系建立示意图。

具体实施方式

下面结合附图，对本发明的技术方案做进一步的说明：

如图2所示，本发明提出一种基于测量数据的管路柔性装焊机器人位姿计算方法，具体步骤如下：

步骤一：计算机器人回归零位时导管两端中心点坐标。

根据公式(1)，可求得机器人回归零位时，导管两端面中心点坐标P_s(x_s,y_s,z_s)，P_Fs(x_fs,y_fs,z_fs)，以及两中心点在机器人TCP坐标系下的坐标P_tcp(x_t,y_t,z_t)，P_Ftcp(x_ft,y_ft,z_ft)。

$\left\{\begin{array}{c}{P}_m={T^m}_{\mathrm{TCP}}\×P_{\mathrm{tcp}}\\ P_s={T^s}_{\mathrm{TCP}}\×P_{\mathrm{tcp}}\\ P_{\mathrm{Fm}}={T^m}_{\mathrm{TCP}}\×P_{\mathrm{Ftcp}}\\ P_{\mathrm{Fs}}={T^s}_{\mathrm{TCP}}\×P_{\mathrm{Ftcp}}\end{array}\right.---\left(1\right)$

步骤二：确定导管固联坐标系，计算机器人零位时导管位姿。

如图3所示，将机器人TCP坐标系相对自身坐标系进行一定的平移和旋转变换即可得到导管的固联坐标系O_L-X_LY_LZ_L，导管坐标系原点(O_L)取导管对接端面中心点。

求解导管对接端面相对夹持直线段偏角θ：

$\cos \mathrm{\θ}=\frac{{\mathrm{vx}}_1\·{\mathrm{vx}}_p+{\mathrm{vy}}_1\·{\mathrm{vy}}_p+{\mathrm{vz}}_1\·{\mathrm{vz}}_p}{\sqrt{{{\mathrm{vx}}_l}^2+{{\mathrm{vy}}_l}^2+{{\mathrm{vz}}_l}^2}\·\sqrt{{{\mathrm{vx}}_p}^2+{{\mathrm{vy}}_p}^2+{{\mathrm{vz}}_p}^2}}---\left(2\right)$

求解机器人TCP坐标系到导管固联坐标系的转换矩阵T₁：

$T_1=\left(\begin{array}{cccc}1& 0& 0& x_t\\ 0& \cos \mathrm{\θ}& -\sin \mathrm{\θ}& y_t\\ 0& \sin \mathrm{\θ}& \cos \mathrm{\θ}& z_t\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

可得机器人回归零位时导管位姿：

${T^s}_L={T^s}_{\mathrm{TCP}}\×T_1=\left(\begin{array}{cccc}{\mathrm{nx}}_{\mathrm{ls}}& {\mathrm{ox}}_{\mathrm{ls}}& {\mathrm{ax}}_{\mathrm{ls}}& x_s\\ {\mathrm{ny}}_{\mathrm{ls}}& {\mathrm{oy}}_{\mathrm{ls}}& {\mathrm{ay}}_{\mathrm{ls}}& y_s\\ {\mathrm{nz}}_{\mathrm{ls}}& {\mathrm{oz}}_{\mathrm{ls}}& {\mathrm{az}}_{\mathrm{ls}}& z_s\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

机器人TCP点在导管固联坐标系中的坐标P^p _tcp(x_tp,y_tp,z_tp)与导管非对接端中心点在导管坐标系中的坐标P_Fp(x_fp,y_fp,z_fp)可由下式求解。

${P^p}_{\mathrm{tcp}}=T_1^{-1}\×\left(\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right)---\left(5\right)$

P_Fp＝(T^s _L)^-1×P_Fs  (6)

步骤三：计算导管对接装配时机器人TCP位姿。

导管装配第一步是对接两段导管，即将两端导管对接端面重合。给定导管对接位置P_d(x_d,y_d,z_d)，其中A段导管保持姿态不变，根据图3所示的导管固联坐标系，将B段导管进行平移和旋转操作，使其位姿与A段导管位姿相同即可保证A、B段导管端面重合。

求解导管的对接位姿：

${T^e}_L=\left(\begin{array}{cccc}{\mathrm{nx}}_{\mathrm{ls}}& {\mathrm{ox}}_{\mathrm{ls}}& {\mathrm{ax}}_{\mathrm{ls}}& x_d\\ {\mathrm{ny}}_{\mathrm{ls}}& {\mathrm{oy}}_{\mathrm{ls}}& {\mathrm{ay}}_{\mathrm{ls}}& y_d\\ {\mathrm{nz}}_{\mathrm{ls}}& {\mathrm{oz}}_{\mathrm{ls}}& {\mathrm{az}}_{\mathrm{ls}}& z_d\\ 0& 0& 0& 1\end{array}\right)---\left(7\right)$

求解导管固联坐标系到机器人TCP坐标系的转换矩阵T₂：

$T_2=\left(\begin{array}{cccc}1& 0& 0& x_{\mathrm{tp}}\\ 0& \cos (-\mathrm{\θ})& -\sin (-\mathrm{\θ})& y_{\mathrm{tp}}\\ 0& \sin (-\mathrm{\θ})& \cos (-\mathrm{\θ})& z_{\mathrm{tp}}\\ 0& 0& 0& 1\end{array}\right)---\left(8\right)$

导管对接时机器人TCP位姿T^e _TCP为：

T^e _TCP＝T^e _L×T₂  (9)

接下来第二步是将导管绕对接端面法向即导管坐标系Y轴进行旋转，以保证导管非对接端面的相对位姿。

计算导管非对接端中心点的理论坐标P_Fe(x_fe,y_fe,z_fe)：

$\left\{\begin{array}{c}{x}_{\mathrm{fe}}=x_d+(x_{\mathrm{fc}}-x_c)\\ y_{\mathrm{fe}}=y_d+(y_{\mathrm{fc}}-y_c)\\ z_{\mathrm{fe}}=z_d+(z_{\mathrm{fc}}-z_c)\end{array}\right.---\left(10\right)$

假设旋转角度为Φ，则导管坐标系的旋转矩阵为T₃：

$T_3=\left(\begin{array}{cccc}\cos \mathrm{\φ}& 0& \sin \mathrm{\φ}& 0\\ 0& 1& 0& 0\\ -\sin \mathrm{\φ}& 0& \cos \mathrm{\φ}& 0\\ 0& 0& 0& 1\end{array}\right)---\left(11\right)$

求解导管经旋转变换后的位姿T^j _L：

T^j _L＝T^e _L×T₃  (12)

导管经旋转变换后，非对接端中心点坐标P_Fj(x_fj,y_fj,z_fj)为：

P_Fj＝T^j _L×P_Fp  (13)

根据下式求解旋转角度Φ：

P_Fj＝P_Fe  (14)

求解得到机器人最终TCP位姿T^j _TCP：

${T^j}_{\mathrm{TCP}}={T^j}_L\×T_2=\left(\begin{array}{cccc}\mathrm{nx}& \mathrm{ox}& \mathrm{ax}& a\\ \mathrm{ny}& \mathrm{oy}& \mathrm{ay}& b\\ \mathrm{nz}& \mathrm{oz}& \mathrm{az}& c\\ 0& 0& 0& 1\end{array}\right)---\left(14\right)$

步骤四：计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。

确定机器人TCP坐标系绕全局坐标系各轴的旋转顺序，选定旋转顺序为Z→Y→X，设各旋转角度依次为α、β、γ。

则根据下式可求得机器人TCP位姿旋转角。

${T^j}_{\mathrm{TCP}}=\left(\begin{array}{cccc}\cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& 0\\ \sin \mathrm{\α}\cos \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& 0\\ -\sin \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\β}\cos \mathrm{\γ}& 0\\ 0& 0& 0& 1\end{array}\right)=\left(\begin{array}{cccc}\mathrm{nx}& \mathrm{ox}& \mathrm{ax}& a\\ \mathrm{ny}& \mathrm{oy}& \mathrm{ay}& b\\ \mathrm{nz}& \mathrm{oz}& \mathrm{az}& c\\ 0& 0& 0& 1\end{array}\right)---\left(15\right)$

下表所示为管路柔性装焊系统的测量数据：

下表所示为计算得到的机器人位姿参数：

Claims (2)

1.一种针对基于视觉测量和机器人的管路柔性装焊方案的机器人位姿计算方法，其特征在于：它包含如下参数：

O-XYZ：全局坐标系取机器人的基座标系。

O_T-X_TY_TZ_T：机器人工具坐标系即TCP坐标系。

O_L-X_LY_LZ_L：导管固联坐标系，在导管模型上定义。

导管设计数据，即导管三维数模信息，包括导管对接端面中心点坐标P_c(x_c,y_c,z_c)，导管非对接端面中心点坐标P_fc(x_fc,y_fc,z_fc)，机器人抓取直线段方向V_l(vx_l,vy_l,vz_l)，导管对接端面法向V_p(vx_p,vy_p,vz_p)。

导管测量数据：包括导管对接端(余量端)中心点测量坐标P_m(x_m,y_m,z_m)；导管非对接端中心点测量坐标P_Fm(x_fm,y_fm,z_fm)；机器人抓取导管测量时的TCP测量位姿T^m _TCP；机器人回归零位时的TCP零位位姿T^s _TCP；导管余量A(Allowance)；导管半径R。

机器人回归零位时，导管对接端(余量端)中心点坐标P_s(x_s,y_s,z_s)，导管非对接端中心点坐标P_Fs(x_fs,y_fs,z_fs)，导管对接端(余量端)中心点在机器人TCP坐标系下的坐标P_tcp(x_t,y_t,z_t)，导管非对接端中心点在机器人TCP坐标系下的坐标P_Ftcp(x_ft,y_ft,z_ft)，导管位姿T^s _L，机器人TCP点和导管非对接端中心点在导管固联坐标系下的坐标P^p _tcp(x_tp,y_tp,z_tp)和P_Fp(x_fp,y_fp,z_fp)。

机器人抓取导管对接时，导管对接端中心点坐标即导管对接位置P_d(x_d,y_d,z_d)，导管非对接端中心点坐标P_Fj(x_fj,y_fj,z_fj)，导管位姿T^j _L，机器人TCP对接位姿T^j _TCP。

2.根据权利要求1所述针对基于视觉测量和机器人的管路柔性装焊方案的机器人位姿计算方法，其特征在于：它包括如下步骤：

步骤一：计算机器人回归零位时导管两端中心点坐标。

步骤二：确定导管固联坐标系，计算机器人回归零位时导管位姿。

步骤三：计算导管对接装配时，机器人TCP位姿。

步骤四：计算机器人TCP坐标系绕全局坐标系三个坐标轴的旋转角度。