ccot 目标跟踪全称_一种焊缝跟踪系统的实时姿态估计方法与流程

本发明属于焊接机器人焊缝跟踪领域，特别涉及一种焊缝跟踪系统的实时姿态估计方法。

背景技术：

现在的焊接机器人基本上都是在焊接之前先进行示教，让机器人每次都走一个固定的轨迹，这种方式有一个好处就是重复精度高，运动轨迹不需要修正，但是，该方法有一个致命的缺陷就是不够随机应变，不够灵活，当需要焊接的工件加工精度比较差。

实时的焊缝跟踪系统以及从图像处理得到的坐标转换成机器人相应的运动是当前焊接机器人焊接过程中亟待解决的问题，国内外的很多学者围绕目标跟踪算法、相应的图像处理、标定算法和机器人实时通讯系统进行了深入细致的研究。经过学者们细致地研究，目前在一定程度上能够实现自动焊接焊缝。但是，在如上研究中，很多学者仅仅考虑到将焊接工件平放装夹这一单一的焊接工况，在焊接过程中机器人姿态并无改变。然而在实际焊接中，焊接工件存在着各种复杂的焊接工况，机器人的姿态也应实时做出调整，故对焊接工件进行实时姿态估计至关重要。

n点透视(pnp)问题是姿态估计领域的经典问题。为解决此问题，lepetit提出了一种n点透视问题的精确解(epnp),世界坐标系下的三维点由4个控制点线性组合，联立三维点求解3d姿态。dementhon提出了比例正交投影迭代变换算法(posit)，通过输入非共面四点的三维坐标和对应图像的二维坐标，进行透视投影变换并进行迭代求得旋转矩阵和平移向量，进而估算出物体的3d姿态。如上姿态估计算法虽然实现了对物体的实时姿态估计，但是均是在无噪声环境下实现的。在焊接过程中，存在着强弧光飞溅等强噪声的干扰，导致了焊接工件的实时姿态估计难度大大提升。在如上算法的启发下，提出了一种焊缝跟踪系统的实时姿态估计方法，通过实时获取焊接工件的点云数据进而实现焊缝的实时姿态估计。为了在强噪声的焊接环境下实时准确地获取点云数据，采用高效卷积操作滤波器跟踪算法(eco)和形态学交叉法。

martindanelljan等人提出了连续型卷积操作滤波器算法(ccot)跟踪算法，使用深度神经网络vgg-net提取特征，通过立方插值，将不同分辨率的特征图插值到连续空间域，再应用黑塞(hessian)矩阵可以求得亚像素精度的目标位置。考虑到更新参数众多影响实时性，且大多滤波器贡献都不大，原作者提出了高效卷积操作滤波器跟踪算法，并作了改进：通过引入因式分解的卷积算子减少模型的训练参数，只保留贡献较大的滤波器参数。此外，通过每隔ns帧更新一次模型，节约时间且避免了模型的漂移。高效卷积操作滤波器跟踪算法能在维持原有精度的情况下，准确定位焊缝特征点，大大提高跟踪速度，满足焊缝跟踪系统实时焊接的需求。

形态学交叉法通过阈值分割、形态学处理、区域选择、骨架提取、提取交叉点等一系列操作提取交叉点。该方法稳定且速度较快，满足焊缝跟踪系统实时焊接的需求。

因此，为了解决在强噪声干扰的焊接环境下对焊接工件进行实时姿态估计的问题，研究了点云姿态估计算法并结合了高效卷积操作滤波器跟踪算法、形态学交叉法，并设计了焊接机器人进行焊缝姿态估计实验，本发明依此提出。

技术实现要素：

本发明的目的在于克服现有技术的缺点与不足，提供一种焊缝跟踪系统的实时姿态估计方法，解决了现有的焊接机器人不能适应于各种焊接工况以及焊接质量、精度不高的问题。

本发明至少通过如下技术方案之一实现。

一种焊缝跟踪系统的实时姿态估计方法，该方法基于的焊缝跟踪系统包括焊接机器人、焊枪、激光视觉传感器、激光视觉传感器外连接件、工作台、支撑垫块、配套焊接设备、嵌入式工控机、控制柜、工件，该方法包括步骤：

s1、焊接开始前，将工件放置于支撑垫块上，在执行每次焊接前，调节支撑垫块的角度，使得工件具有不同的初始姿态，并采用g型夹具装夹固定，保证焊接机器人能在不同的焊接工况下进行焊接；激光视觉传感器中的工业相机先采集焊缝图像并发送到嵌入式工控机，所述嵌入式工控机对采集的图像进行初始化处理得到初始焊缝特征点的像素坐标值，采用形态学交叉法得到焊缝两侧交叉点的像素坐标值，将两像素坐标值转换为焊接机器人基坐标系下的三维坐标值；

s2、焊接开始后，所述激光视觉传感器的工业相机将连续采集的每一帧图像发送至所述嵌入式工控机，采集图像中焊缝特征点和焊缝两侧交叉点的像素坐标值，将两像素坐标值转换成焊接机器人基坐标系下的三维坐标值并保存为点云数据，利用保存的点云数据计算得到焊接机器人的位置和姿态；

s3、将步骤s2得到的焊接机器人的位置和姿态与当前焊接机器人的位置和姿态作差，将得到的偏差实时发送给机器人控制柜，控制柜输出控制信号传输给焊接机器人，使焊枪末端的焊丝沿着工件的焊缝运动，完成焊缝跟踪系统的实时姿态估计。

进一步地，所述步骤s1具体包括：

s11、焊接开始前，在每种焊接工况下，调整焊接机器人机械臂的位置，使得固定在焊枪上的激光视觉传感器处于最佳工作位置，即在焊接过程中既能捕捉到清晰的图像，又不会使得激光视觉传感器和工件发生干涉；

s12、激光视觉传感器中的工业相机采集图像并发送到嵌入式工控机，嵌入式工控机通过调用halcon软件的库函数进行阈值处理、形态学修正的初始化处理，得到初始的焊缝特征点的像素坐标值，将此初始焊缝特征点作为高效卷积操作滤波器跟踪算法的初始目标；同时，嵌入式工控机通过调用halcon软件的库函数进行形态学交叉，即通过阈值分割、形态学处理、区域选择、骨架提取、提取交叉点的一系列操作得到焊缝两侧交叉点的像素坐标值；

s13、通过标定算法，将得到的初始焊缝特征点和焊缝两侧交叉点的像素坐标值转换成焊接机器人基坐标系下的三维坐标值。

进一步地，所述步骤s2具体包括：

s21、焊接开始后激光视觉传感器的相机以60fps的采样频率连续采集图像，并发送至嵌入式工控机进行处理；

s22、将采集图像中焊缝特征点和焊缝两侧交叉点的像素坐标值为对当前帧图像采用高效卷积操作滤波器跟踪算法处理，进行目标跟踪定位确定图像中焊缝特征点的像素坐标值，同时进行形态学交叉确定图像中焊缝两侧交叉点的像素坐标值；

s23、通过标定算法，将得到的初始焊缝特征点和焊缝两侧交叉点的像素坐标值转换成焊接机器人基坐标系下的三维坐标值；

s24、将采集的数据保存为点云，通过构建点云数据、实时构建工件坐标系、获取旋转角度，得到焊接机器人的位置和姿态。

进一步地，步骤s22中的高效卷积操作滤波器跟踪算法处理具体包括以下步骤：

s221、以步骤s1提取的初始焊缝特征点为中心的跟踪目标作为第一个训练样本来训练高效卷积操作滤波器跟踪算法的滤波器；

s222、将每一个特征通道通过插值法转换到连续域上：

其中xd即代表特征通道,nd表示该特征通道里所采样的像素点数，bd表示插值函数，t代表插值后的连续域的范围；jd{xd}代表插值样本的特征、t表示时间、n表示离散空间变量，n∈{0,…,nd}、d是每一个特征通道；

s223、高效卷积操作滤波器跟踪算法采用提取插值样本的特征jd{xd}的子集，从d维的特征中选其中的c维，只使用一组基础滤波器f1,…fc,其中c＜d，然后使用一组学习系数pd,c将层数为d的p特征的滤波器构造为滤波器fc的一个线性组合这些系数用维度为d×c的矩阵p＝pd,c来表示；新的多通道滤波器写成矩阵向量乘积pf，此时检测函数为:

其中，j{x}表示整体插值特性映射,j{x}∈rd,符号*表示卷积运算；spf{x}表示线性卷积算子、f代表滤波器、rd代表实数域、x表示训练样本；

s224、为了简化符号，用表示插值后的特征图的傅里叶系数z＝j{x},代表d通道插值后的特征图、xd代表xd的离散傅里叶变换、k表示样本序号、表示移位后的插值函数，跟踪的目标函数如下所示:

其中，代表对应样本中跟踪算法跟踪到的目标位置，代表正则化参数，||p||f为p的弗罗贝尼乌斯规范正规化,λ为权重控制的参数；e(f,p)代表跟踪的目标函数、代表插值后的特征图、代表滤波器、l代表范数名称，此处采用的是欧式距离、代表每一个滤波器、代表权重系数、

s225、通过古斯-牛顿法解最优化问题，而后对每一帧图像进行滤波器参数的更新。

进一步地，步骤s22中，所述的形态学交叉同步骤s12所述，即嵌入式工控机通过调用halcon软件的库函数进行形态学交叉，即通过阈值分割、形态学处理、区域选择、骨架提取、提取交叉点的一系列操作得到焊缝两侧交叉点的像素坐标值。

进一步地，所述构建点云数据是采用点、线、面方法，获取焊接工件的点云数据；点、线、面方法是通过高效卷积操作滤波器跟踪算法确定特征点的像素坐标值，形成点；进而采用形态学交叉法确定焊缝两侧交叉点的像素坐标值，形成线；随着焊缝长度的增加，点和线横向扩展形成面，最终构建出了点云数据；

所述实时构建工件坐标系是选取最新的m帧点云数据实时计算单位向量xt、yt、zt，实时构建工件坐标系；

所述获取旋转角度是通过计算工件坐标系与机器人基坐标的转换矩阵，进而反求出绕x、y、z轴旋转的角度，对应的角度以rx、ry、rz表示，用以实时更新机器人姿态。

进一步地，所述构建工件坐标系包括以下步骤：

s2421、建立单位向量zt，为了使zt能够较好地反映在当前焊接工件平面倾斜情况，确保姿态的实时性，在每次建立单位向量zt时，选取焊缝两侧交叉点点云数据的最新m帧作为数据源，拟合数据源得到点云平面，得到点云平面法向量z，单位化求得单位向量zt；使用鲁棒性最小二乘法拟合点云平面，得到点云平面方程，其方程公式如下：

apx+bpy+cpz+dp＝0

其中，ap、bp、cp、dp代表点云平面方程的系数、x、y、z表示点的三维坐标；

s2422、该点云平面的法向量为z{ap,bp,cp}，为了方便获取机器人基坐标系与工件坐标系之间的转换矩阵，此处将法向量z单位化，得到单位向量zt{apc,bpc,cpc}，随着跟踪焊缝长度的增加，实时建立一系列的单位向量zt；

s2423、建立单位向量xt，在每次建立单位向量xt时，选取特征点点云数据的最新m帧作为数据源，将数据源投影到apx+bpy+cpz+dp＝0平面上，得到投影数据源；对投影数据源进行曲线拟合，因工件为圆弧型，故此处采用圆函数对数据进行拟合，得到圆函数方程：

(x-a)2+(y-b)2＝r2

其中，a,b分别为圆心的坐标，r为圆的半径；x,y为特征点的坐标；

s2424、选取投影数据源的第末15帧，获取该点坐标处的切向量x,令apx+bpy+cpz+dp＝0、(x-a)2+(y-b)2＝r2的数据移至一侧，这两个数据分别设为f(x,y,z)、g(x,y,z)，采用隐函数，得切向量x{dp,ep,fp}，并进行单位化得到单位向量xt{dpc,epc,fpc}：

式中，

x0、y0表示为跟踪的特征点在机器人基坐标系下横坐标和纵坐标；

s2425、随着跟踪焊缝长度的增加，实时建立一系列的单位向量xt；

s2426、通过正交单位向量zt和单位向量xt，得到向量yt，实时构建出工件坐标系。

进一步地，所述获取旋转角度包括以下步骤：

s2431、已知机器人基坐标系和工件坐标系，将工件坐标系的三个单位向量xt、yt、zt分别对应投影到机器人基坐标系的三个单位向量xb、yb、zb，转换矩阵的如下：

其中，xb、yb、zb分别对应xb{1,0,0},yb{0,1,0},zb{0,0,1}为机器人基坐标单位向量；

s2432、在x-y-z固定角坐标系下，通过转换矩阵得到旋转角rx,ry,rz，即得到焊接机器人姿态，具体如下：

其中，atan2表示反函数、r11、r21、r31、r32、r33分别表示转换矩阵的参数。

本发明相对于现有技术具有如下的优点及效果：

(1)本发明通过激光视觉传感器自动识别焊缝的特征点和焊缝两侧的交叉点，并通过嵌入式工控机进行后续的通讯、计算和处理，装置结构简单，系统易于维护，通过嵌入式工控机实现数据的自动采集和处理，能有效提高数据处理的效率；

(2)本发明自动化程度比较高，能够实现焊缝图像自动实时采集，自动焊接，在各种复杂工况下实现焊缝自动实时姿态估计，大大提高了生产效率。

附图说明

图1是本实施例的焊接机器人的焊缝跟踪系统总体结构示示意图；

图2是本实施例的焊接机器人的焊缝跟踪系统中的激光视觉传感器的结构示意图；

图3是本实施例的焊缝跟踪系统的实时姿态估计方法流程示意图；

图4是本实施例焊缝跟踪系统的实时姿态估计方法中的焊接实验典型噪声示意图；

图中：1-配套焊接设备；2-焊接机器人；3-支撑垫块；4-工件；5-焊枪；6-激光传感器外连接件；7-激光视觉传感器；71-传感器外壳；72-相机；73-透光性隔板；74-激光发生器；8-工作台；9-嵌入式工控机；10-控制柜；11-焊缝特征点；12-飞溅；13-焊缝两侧交叉点；14-弧光。

具体实施方式

下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1、图2所示，一种焊缝跟踪系统的实时姿态估计方法，该方法基于的焊缝跟踪系统，该系统包括焊接机器人2、焊枪5、激光视觉传感器7、激光视觉传感器外连接件6(外连接件6是螺栓螺母连接件，将激光视觉传感器7与传感器外壳71相连与固定)、工作台8、支撑垫块3、配套焊接设备1、嵌入式工控机9、控制柜10和工件4；工件4放置在支撑垫块3上，支撑垫块3放置在工作台8上，工件4的倾斜角度可以通过支撑垫块3调节，使得工件4具有不同的姿态，亦使得焊接机器人2焊接时具有不同的焊接工况；激光视觉传感器7固定安装在焊枪5末端，所述焊枪5通过焊枪夹持装置(该装置主要由夹具固定座、螺栓螺母组成，将焊枪5放置于夹具固定座中，采用螺栓螺母紧固连接)安装在焊接机器人2末端上，嵌入式工控机9通过以太网线与激光视觉传感器7相连，焊枪5和配套焊接设备1(焊接设备为焊机和保护气瓶。采用的焊机为安川的motoweld-rd350，用于机器人的送丝和退丝。采用的保护气瓶装着二氧化碳(20％)与氮气(80％)，以在焊接过程中起到保护的作用)通过电缆线与焊接电源相连接，嵌入式工控机9通过以太网线与控制柜10相连，控制柜10和焊接机器人2、配套焊接设备1通过电缆线相连；激光视觉传感器7和焊枪5通过焊接机器人2的运动而改变空间位置；所述激光视觉传感器7包括黑色氧化处理的传感器外壳71、工业相机72、透光性隔板73、激光发生器74；所述工业相机72和激光发生器74固定在传感器外壳71内；所述透光性隔板73固定在传感器外壳71上且位于所述工业相机72和激光发生器74(激光发生器74通过螺栓螺母与传感器外壳71紧固连接，与工业相机72之间成30°的夹角)前端；所述的工作台上设置有g型夹具，所述工件4放置于支撑垫块3上，由两个及以上g型夹具进行装夹定位。

如图3所示，一种焊缝跟踪系统的实时姿态估计方法，包括以下步骤：

s1、焊接开始前，将工件4放置于支撑垫块上，在执行每次焊接前，调节支撑垫块3的角度，使得工件4具有不同的初始姿态，并采用g型夹具装夹固定，保证焊接机器人2能在不同的焊接工况下进行焊接；激光视觉传感器7中的工业相机72先采集图像并发送到嵌入式工控机9，所述嵌入式工控机9对采集的图像进行初始化处理得到初始焊缝特征点11的像素坐标值，采用形态学交叉法得到焊缝两侧交叉点13的像素坐标值，将两像素坐标值转换为焊接机器人基坐标系下的三维坐标值；

具体而言，所述步骤s1具体包括：

s11、焊接开始前，在每种焊接工况下，调整焊接机器人2机械臂的位置，使得固定在焊枪5上的激光视觉传感器7处于最佳工作位置，即在焊接过程中既能捕捉到清晰地图像(能分辨出图像线条间的区别的图像才是清晰的图像)，又不会使得激光视觉传感器7和工件4发生干涉；

s12、激光视觉传感器7中的工业相机72采集图像并发送到嵌入式工控机9，嵌入式工控机9通过调用halcon软件的库函数进行阈值处理、形态学修正的初始化处理，得到初始的焊缝特征点11的像素坐标值，将此初始焊缝特征点11作为高效卷积操作滤波器跟踪算法的初始目标；同时，嵌入式工控机9通过调用halcon软件的库函数进行形态学交叉，即通过阈值分割、形态学处理、区域选择、骨架提取、提取交叉点等一系列操作得到焊缝两侧交叉点13的像素坐标值；

s13、通过标定算法，将得到的初始焊缝特征点11和焊缝两侧交叉点13的像素坐标值转换成焊接机器人基坐标系下的三维坐标值。

s2、焊接开始后，所述激光视觉传感器7的工业相机72将连续采集的每一帧图像发送至所述嵌入式工控机9，采集图像中焊缝特征点11和焊缝两侧交叉点13的像素坐标值，将两像素坐标值转换成焊接机器人基坐标系下的三维坐标值并保存为点云数据，利用保存的点云数据得到焊接机器人2的位置和姿态；

具体而言，所述步骤s2具体包括：

s21、焊接开始后激光视觉传感器7的相机72以60fps的采样频率连续采集图像，并发送至嵌入式工控机9进行处理；

s22、所述的采集图像中焊缝特征点11和焊缝两侧交叉点13的像素坐标值为对当前帧图像采用高效卷积操作滤波器跟踪算法处理，进行目标跟踪定位确定图像中焊缝特征点11的像素坐标值，同时进行形态学交叉确定图像中焊缝两侧交叉点13的像素坐标值；

s23、同s13，即通过标定算法，将得到的初始焊缝特征点11和焊缝两侧交叉点13的像素坐标值转换成焊接机器人基坐标系下的三维坐标值；

s24、所述的将采集的数据保存为点云，通过构建点云数据、实时构建工件坐标系、获取旋转角度得到焊接机器人2的位置和姿态。

具体而言，步骤s22中，所述的将特征提取处理后的图像进行高效卷积操作滤波器跟踪算法处理，确定跟踪目标即图像中焊缝特征点11的位置的步骤具体包括：

s221、以s1提取的初始焊缝特征点11为中心的跟踪目标作为第一个训练样本来训练高效卷积操作滤波器跟踪算法的滤波器；

s222、将每一个特征通道通过插值的方法转换到连续域上：

其中xd即代表特征通道,nd表示该特征通道里所采样的像素点数，bd表示插值函数，t代表插值后的连续域的范围；jd{xd}代表插值样本的特征、t表示时间、n表示离散空间变量，n∈{0,…,nd}、d是每一个特征通道；

s223、高效卷积操作滤波器跟踪算法采用了提取插值样本的特征jd{xd}的子集，从d维的特征中选了其中的c维；高效卷积操作滤波器跟踪算法只使用一组较小的基础滤波器f1,…fc,其中c＜d。然后使用一组学习系数pd,c将层数为d的p特征的滤波器构造为滤波器fc的一个线性组合这些系数可以用维度为d×c的矩阵p＝pd,c来简明地表示；新的多通道滤波器可以写成矩阵向量乘积pf，此时检测函数为:

其中，j{x}表示整体插值特性映射,j{x}∈rd,符号*表示卷积运算；spf{x}表示线性卷积算子、f代表滤波器、rd代表实数域、x表示训练样本；

s224、为了简化符号，用表示插值后的特征图的傅里叶系数z＝j{x},代表d通道插值后的特征图、xd代表xd的离散傅里叶变换、k表示样本序号、表示移位后的插值函数)，跟踪的目标函数如下所示:

其中，y代表对应样本中跟踪算法跟踪到的目标位置，代表正则化参数，||p||f为p的弗罗贝尼乌斯规范正规化,λ为权重控制的参数；e(f,p)代表跟踪的目标函数、代表插值后的特征图、代表滤波器、l代表范数名称，此处采用的是欧式距离、代表每一个滤波器、代表权重系数、

s225、通过古斯-牛顿法解最优化问题，而后对每一帧图像进行滤波器参数的更新；

具体而言，步骤s22中，所述的形态学交叉同步骤s12所述，即嵌入式工控机9通过调用halcon软件的库函数进行形态学交叉，即通过阈值分割、形态学处理、区域选择、骨架提取、提取交叉点等一系列操作得到焊缝两侧交叉点的像素坐标值。

具体而言，所述步骤s24具体包括：

s241、所述的构建点云数据是采用点、线、面方法，获取焊接工件4的点云数据；点、线、面方法是通过高效卷积操作滤波器跟踪算法确定特征点11的像素坐标值，形成点；进而采用形态学交叉法确定焊缝两侧交叉点13的像素坐标值，形成线；随着焊缝长度的增加，点和线横向扩展形成面，最终构建出了点云数据；

s242、所述的实时构建工件坐标系是选取最新30帧点云数据实时计算向量xt、yt、zt，实时构建工件坐标系；

s243、所述的获取旋转角度是通过计算工件坐标系与机器人基坐标的转换矩阵，进而反求出绕x、y、z轴旋转的角度，下文以rx,ry,rz表示，用以实时更新机器人姿态。

所述步骤s242具体包括：

s2421、建立向量zt，为了使zt能够较好地反映在当前焊接工件平面倾斜情况，确保姿态的实时性，在每次建立单位向量zt时，选取焊缝两侧交叉点点云数据的最新30帧作为数据源，拟合数据源得到点云平面，得到点云平面法向量z，单位化求得单位向量zt；使用鲁棒性最小二乘法拟合点云平面，得到点云平面方程，其方程公式如下：

apx+bpy+cpz+dp＝0

其中，ap、bp、cp、dp代表点云平面方程的系数、x、y、z表示点的三维坐标；

s2422、该点云平面的法向量z{ap,bp,cp}。为了方便获取机器人基坐标系与工件坐标系之间的转换矩阵，此处将法向量z单位化，得到单位向量zt{apc,bpc,cpc}；随着跟踪焊缝长度的增加，实时建立了一系列的单位向量zt；

s2423、建立单位向量xt，在每次建立单位向量xt时，选取特征点点云数据的最新30帧作为数据源，将数据源投影到apc+bpy+cpz+dp＝0平面上，得到投影数据源；对投影数据源进行曲线拟合，因工件为圆弧型，故此处采用圆函数对数据进行拟合，得到圆函数方程：

(x-a)2+(y-b)2＝r2

其中，a,b分别为圆心的x,y坐标，r为圆的半径；

s2424、选取投影数据源的第末15帧，获取该点坐标处的切向量x,令apx+bpy+cpz+dp＝0、(x-a)2+(y-b)2＝r2的数据移至一侧，分别设为f(x,y,z)、g(x,y,z)，采用隐函数的方法，可得切向量x{dp,ep,fp}，并进行单位化得到单位向量xt{dpc,epc,fpc}：

式中，

x0、y0表示为跟踪的特征点在机器人基坐标系下横坐标和纵坐标；

s2425、随着跟踪焊缝长度的增加，实时建立了一系列的单位向量xt；

s2426、通过正交单位向量zt和单位向量xt，得到向量yt，实时构建出工件坐标系。

具体而言，所述步骤s243具体包括：

s2431、已知机器人基坐标系和工件坐标系，将工件坐标系的三个单位向量xt、yt、zt分别对应投影到机器人基坐标系的三个单位向量xb、yb、zb，转换矩阵的计算如下：

其中，xb{1,0,0},yb{0,1,0},zb{0,0,1}为机器人基坐标单位向量；

s2432、在x-y-z固定角坐标系下，可通过转换矩阵求得到旋转角rx,ry,rz，即求得焊接机器人2姿态，具体如下：

其中，atan2表示反函数、r11、r21、r31、r32、r33分别表示转换矩阵的参数。

s3、将步骤s2得到的焊接机器人2的位置和姿态与当前焊接机器人2的位置和姿态作差，将得到的偏差实时发送给机器人控制柜10，控制柜10输出控制信号传输给焊接机器人2，使焊枪5末端的焊丝沿着工件的焊缝运动，完成焊缝跟踪系统的实时姿态估计。

本实施例能使得焊接机器人较好地适应于各种焊接工况，如图4所示，在强烈弧光14、飞溅12等强噪声的干扰下依然能实现精确的焊缝姿态估计，具有完全自动化,焊接精度高,焊接质量高，实时性好,抗干扰能力强等优点。

上述实施例为本发明较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。