CN109623206A - 用于机器人管道焊接中优化离线规划的焊枪位姿的方法 - Google Patents

Abstract

本发明提出一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法，属于管道焊接中优化调整焊枪位姿的领域。该方法首先搭建用于离线规划焊枪位姿优化的结构光测量系统并进行系统参数标定，然后进行管道焊接路径点的离线规划；调节焊枪到期望位姿并生成对应此位姿下的相位图，从相位图中提取视觉伺服的视觉特征并设计视觉伺服控制律；在离线规划的每一个路径点，利用视觉伺服控制律对每一个路径点的位姿进行优化并记录对应机器人各关节角度。本发明结合结构光测量精度高和视觉伺服控制精度高的优点，并且充分利用了机器人重复定位精度高的优点，能有效优化实际焊接过程中焊枪的位姿。

Description

用于机器人管道焊接中优化离线规划的焊枪位姿的方法

技术领域

本发明属于管道焊接中优化调整焊枪位姿的领域，尤其涉及一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法。

背景技术

管道运输是远距离运输石油、天然气等物资的主要方式之一，所以远距离管道铺设具有重要意义。而单个管道的长度有限，所以管道铺设主要依赖管道接合技术，其主要结合方式有螺纹连接、法兰连接、焊接连接、承插连接、管道黏合连接等方法。而其中，管道焊接因其接口牢固严密，构造简单，管路美观整齐，节省了大量的定型管件，接口严密，不用填料，可减少维修工作，接口不受管径限制，作业速度快等一系列优点，成为了目前最广泛使用的管道接合技术。

现在的管道焊接主要有结构化的焊接小车焊接和机器人焊接两种方式。前者需要在管道内表面或外表面铺设小车行走的轨道，所以比较耗时。而机器人焊接因其只需要离线规划一次得到焊枪位姿便可进行多次焊接管道从而能大幅提升大批量管道焊接的生产效率。但是在机器人焊接中，离线规划得到的焊枪位姿精度受限于工件安装误差、建模误差、工件变形以及机器人绝对精度不高等缺点，所以焊接质量难以得到保证。因此优化离线规划的焊枪位姿，消除以上误差带来的影响，进而提高管道焊接的质量就有了重要的技术意义。

发明内容

本发明的目的是针对机器人管道焊接中离线规划的焊枪位姿的精度受限于工件安装误差、建模误差、工件变形以及机器人的绝对精度不高等缺点，提出一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法。本发明结合结构光的测量精度高，视觉伺服控制精度高的优点来优化离线规划的焊枪位姿，而且，此方法优化每一个离线规划的路径点并将优化结果存储在计算机中，从而可以充分利用机器人的重复定位精度较高的优点，进而提高管道焊接的质量。

本发明提出一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法，其特征在于，该方法包括以下步骤：

1)搭建用于离线规划焊枪位姿优化的结构光测量系统；

所述系统包括：带有机器人基座和机器人末端执行器的工业机器人，固定底座，焊枪，激光跟踪仪，投影仪，相机和计算机；所述机器人基座通过螺纹连接固定在光学平台上，固定底座通过螺纹连接固定在机器人末端执行器上，焊枪，投影仪和相机通过螺纹连接依次固定在固定底座上，其中投影仪位于焊枪与相机之间，投影仪与相机的光轴保持在设定的距离且平行，焊枪不出现在相机和投影仪的公共视野内；将待焊接管道通过夹具悬空固定在光学平台上方，激光跟踪仪和机器人分别位于待焊接管道两侧设定距离处；工业机器人，投影仪和相机分别连接计算机；

2)系统参数标定；

标定相机成像模型的内参矩阵M_c，标定投影仪成像模型的内参矩阵M_p，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^cT_p,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待焊接管道上的工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w，利用固定底座的三维模型和焊枪的三维模型测量和计算得到焊枪坐标系F_t相对于机器人末端坐标系F_e的变换矩阵^bT_t，通过用游标卡尺测量K次待焊接管道的外半径取平均值作为待焊接管道的外半径R；

3)离线规划；

由步骤2)中得到的变换矩阵^bT_w和^tT_e，给定焊枪相对待焊接管道的位姿约束以及待焊接管道的外半径R，进行管道焊接路径点的离线规划；

4)调节焊枪到期望位姿并生成对应此位姿下的相位图，具体步骤如下：

4-1)将焊枪移动至步骤3)离线规划得到的第一个路径点，通过激光跟踪仪拟合焊枪及待焊接管道的轴线，两条轴线的关系用距离d和夹角θ来表示；对焊枪的位姿进行调整，使得0≤d＜1mm，89°＜θ＜91°；以该状态下焊枪相对于待焊接管道的位姿作为后续路径点视觉伺服的期望位姿；

4-2)在期望位姿下，利用投影仪投影N张单位频率的正弦条纹图和N张高频率的正弦条纹图投影到待焊接管道表面，同时利用相机采集投影后在待焊接管道表面变形的2N张条纹图，通过相位解包裹方法得到在期望位姿下的相位图；相位图为一个和相机的分辨率相同大小的矩阵，其中每个元素的值是对应该元素位置的空间点在投影仪成像平面下的像点的横坐标；

5)从相位图中提取视觉特征；

从相位图中提取能表征待焊接管道的位姿的轴线特征和能表征焊缝位置的坐标特征，并将其作为视觉伺服的视觉特征；具体步骤如下：

5-1)将相位图的某一个元素用U_p(U_c,V_c)表示，其中U_p是该元素的元素值，(U_c,V_c)是该元素的坐标；通过下面两个公式将相位图的元素值和坐标转化到归一化平面坐标下：

其中(u_p,v_p)，(u_c,v_c)分别代表的是归一化之后元素u_p的投影仪坐标和相机坐标；

5-2)通过对相位图的每一个元素计算u_p(u_c,v_c)-u_c，然后找到相位图中每一行计算结果最大值对应的元素，将该元素的元素值记为Q_max(u_c,v_c)，同时记录其对应坐标u_c(v_c)，则n个元素值及对应坐标组成一个n×2的矩阵，其中n是相位图的行数；

5-3)对于n×2的矩阵中每一个v_c，通过三角测量原理求解出这一点的相机坐标系下的空间位置(X_D,Y_D,Z_D)，其中Z_D＝L/Q_max，X_D＝Z_D·u_c和Y_D＝Z_D·v_c，其中L为变换矩阵^cT_p的第一行第四列的元素值；对于n个不同的v_c，得到n个不同的点，通过最小二乘法拟合上述n个点得到待焊接管道圆柱的特征母线方程，即其中(u 1 w)为拟合出的特征母线在F_c下的方向向量，(x_D z_D)为特征母线与平面X_cO_cZ_c的交点；

将特征母线和轴线在X_c和Z_c方向上的偏移量分别记为Δ_X和Δ_Z，其中，和

则待焊接管道圆柱轴线的方程如下：

其中

轴线特征即由轴线与X_cO_cZ_c的交点坐标(x_o,z_o)和两个自由方向向量(u,w)组成；

5-4)利用一个缝隙检测器在相位图上识别出焊缝所在位置的所有点的坐标，对其求平均值得到焊缝中心点在F_c下的三维坐标(x_s,y_s,z_s)，取坐标y_s为代表焊缝在相位图中位置的坐标特征；

5-5)利用步骤5-3)和5-4)的结果，得到视觉特征如下：s＝[x_o z_o u w y_s]^T；

6)设计视觉伺服控制律；

以步骤5)得到的视觉特征为基础，建立对应视觉特征的视觉伺服的交互矩阵：

对应的视觉伺服控制律为：其中V_c为相机的六维速度，λ是增益系数，是交互矩阵的伪逆的近似估计量，其中为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征；

7)将步骤3)离线规划得到的每一个路径点作为视觉伺服的初始状态，利用视觉伺服控制律对每一个路径点的位姿进行优化，在每一个路径点，视觉伺服收敛完成以后，此时机器人的位姿即为该路径点的优化位姿，记录每一个路径点优化位姿下的机器人各关节角度，直到得到离线规划的每个路径点的优化位姿和对应机器人各关节角度，方法结束。

本发明的特点及有益效果在于：

本发明的优点在于：1)结合了结构光测量精度高以及视觉伺服控制精度高的优点，所以可以消除工件安装误差，建模误差，工件变形等带来的误差，提高实际焊接过程中焊枪的位姿精度。2)对每个离线规划的路径点进行了优化，并用计算机记录下了优化后焊枪的位姿，所以可以克服机器人绝对精度低的缺点，充分利用机器人的高重复定位精度，从而提高实际焊接过程中焊枪的位姿精度，进而提高机器人管道焊接的质量。

附图说明

图1为本发明方法的整体流程图。

图2为本发明实施例中的测量系统结构示意图。

图3为本发明实施例中的圆柱轴线特征提取的建模示意图。

图4为本发明实施例中的轴线和特征母线的关系示意图。

图5为本发明实施例中的一般位置圆柱在相机坐标系中的侧视图。

图中：1-机器人基座,2-机器人末端执行器,3-固定底座,4-焊枪,5-待焊接管道,6-激光跟踪仪,7-投影仪,8-相机。

具体实施方式

本发明提出一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法，下面结合附图和具体实施例对本发明进一步详细说明如下。以下实施例用于说明本发明，但不限于本发明的范围。

本发明提出一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法，整体流程如图1所示，包括以下步骤：

1)搭建用于离线规划焊枪位姿优化的基于结构光的视觉伺服控制系统；

本发明实施例的基于结构光的视觉伺服控制系统结构如图2所示，所述系统包括：带有机器人基座1和机器人末端执行器2的六自由度工业机器人，固定底座3，焊枪4，激光跟踪仪6，投影仪7，相机8和计算机。所述工业机器人基座1通过螺纹连接固定在光学平台上，固定底座3通过螺纹连接固定在机器人末端执行器2上，焊枪4、投影仪7和相机8通过螺纹连接依次固定在固定底座3上，其中投影仪7位于焊枪4与相机8之间，投影仪7与相机8的光轴需要保持平行且保持一定距离，距离过小则会影响焊枪位姿的优化精度，距离过大则会使相机和投影仪公共视野太小，根据实验效果可以调整，一般取70～150mm，本实施例取110mm左右，而且应保证焊枪4不出现在相机8和投影仪7的公共视野内，如果位置不方便也可使焊枪4出现在其公共视野内，那么后续图像处理中需要增加一步：将相位图中对应焊枪的区域删去。将待焊接管道通过夹具悬空固定在光学平台上方，为保证激光跟踪仪测量过程中机器人不干涉对靶标的检测，激光跟踪仪和机器人分别放置在待焊接管道两侧设定距离处；激光跟踪仪放置过远会导致测量精度受影响，放置过近会导致能测量的范围有限，因此一般离带焊接管道50～200cm，即可符合要求，本实施例中激光跟踪仪6放置在离待焊接管道80cm左右。工业机器人，投影仪和相机分别连接计算机。图2中各个坐标系分别由如下符号表示：F_b表示机器人基座坐标系，F_e表示机器人末端执行器坐标系，F_p表示投影仪坐标系，F_c表示相机坐标系，F_t表示焊枪坐标系，F_w表示工件坐标系，即与带焊接管道固连的坐标系，F_L表示激光跟踪仪坐标系。

本发明中所有部件均可采用常规型号。本实施例中，相机为灰度工业相机；型号为JAI GO5000,投影仪为德州仪器的DLP4500；固定底座同时将相机，投影仪，焊枪固定到机器人法兰盘上，其材料为15mm厚的铝板；六自由度工业机器人为ABB IRB 1200,焊接设施由Kemppi A7-MIG450气体金属弧焊(GWAM)焊机和宾采尔ABIROBA500焊枪组成，焊机为焊枪提供能源和保护气体，机器人控制柜一样属于支撑硬件，对建模过程没有影响，所以焊枪和机器人控制柜都没有在图中描述，放置在安全的位置即可；激光跟踪仪为Leica LaserTracker AT901；计算机为联想Y720。上述所有硬件均无特殊要求。

2)系统参数标定；

标定相机成像模型的内参矩阵M_c，标定投影仪成像模型的内参矩阵M_p，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^cT_p,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待焊接管道上的工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w，利用自己设计的连接相机，投影仪，焊枪和机器人末端执行器的固定底座和从宾采尔官网下载到的ABIROBA500焊枪的三维模型测量和计算得到焊枪坐标系F_t相对于机器人末端坐标系F_e的变换矩阵^bT_t，通过用游标卡尺测量三到五次待焊接管道的外半径取平均值作为待焊接管道的外半径R。

3)离线规划；

由步骤2)中得到的变换矩阵^bT_w和^tT_e，给定焊枪相对待焊接管道的位姿约束以及待焊接管道的外半径R，进行管道焊接路径点的离线规划，受限与机器人和待焊接管道的位置关系，有些臂展小的机器人可能并不能饶管道连续旋转一周，此实施例提到的ABBIRB1200的臂展就不能做到，所以先饶管道焊接一次，饶轴线旋转管道180°，再次进行第二次焊接。具体方法如下：

首先设定约束条件：使焊枪垂直待焊接管道轴线，并对准焊缝，且保持一定距离,使焊枪喷嘴到焊缝的距离为10-18mm最合适，本实施例取15mm。由上述约束可以得到焊枪坐标系F_t相对工件坐标系F_w的变换矩阵^tT_w，再结合变换矩阵^bT_w和^tT_e，可以求得对应每个路径点机器人末端坐标系F_e相对于基座坐标系F_b的变换矩阵^bT_e＝^bT_w·^tT_w ^-1·^tT_e，进而通过机器人逆运动学求得对应此变换矩阵下机器人在每个路径点的各个关节的关节角度。本实施例中饶管道轴线每隔3°设置一个路径点，所以一共规划60个路径点。等一次焊接完成后，饶轴线旋转管道180°，重复上述步骤继续规划新的60个路径点，一共得到120个路径点。

4)调节焊枪到期望位姿并生成对应此位姿下的相位图，具体步骤如下：

4-1)将焊枪移动至步骤3)离线规划得到的的第一个路径点，通过激光跟踪仪拟合焊枪及待焊接管道的轴线，两条轴线的关系用距离d和夹角θ来表示。

具体方式为：利用与激光跟踪仪配套的软件Polyworks选择探测及拟合圆柱功能，首先探测：一边拿靶标贴紧待焊接管道表面，另一边采集数据；采集超过30个点后，然后选择拟合圆柱得到圆柱1。同样通过此方法得到拟合焊枪得到圆柱2。将这两个圆柱导出成曲面模型，在Pro/E中测量其轴线，则可以得到这两个参数d和θ。

得到这两个参数之后，可以对机器人末端位姿进行微调，从而对焊枪的位姿进行调整，使这两个参数处在理想的范围内，即0≤d＜1mm，89°＜θ＜91°，即可满足管道焊接的要求。然后以该状态下焊枪相对于待焊接管道的位姿作为后续路径点视觉伺服的期望位姿。

4-2)在期望位姿下，利用投影仪投影N张单位频率的正弦条纹图和N张高频率的正弦条纹图投影到待焊接管道表面，同时利用相机采集投影后在待焊接管道表面变形的2N张条纹图，再通过相位解包裹方法得到在期望位姿下的相位图。本发明中N取值越大，相位图的精度越高，但解码生成相位的时间越长，所以综合考虑，本实施例取N＝20。相位图即是一个和相机的分辨率相同大小的矩阵，其中每个元素的值是对应该元素位置的空间点在投影仪成像平面下的像点的横坐标。

5)从相位图中提取视觉特征；

从相位图中提取能表征待焊接管道的位姿的轴线特征和能表征焊缝位置的坐标特征，并将其作为视觉伺服的视觉特征。具体方法如下：

先不考虑焊缝，这样待焊接管道可以被看成一个完整的圆柱，图3为本发明实施例中的圆柱轴线特征提取的建模示意图。

如图3所示。相位图的某一个元素可以用U_p(U_c,V_c)来表示，其中U_p是该元素的元素值，(U_c,V_c)是该元素的坐标，也是其对应的相机成像平面的坐标。通过下面两个公式将相位图的元素值和坐标转化到归一化平面坐标下： 其中(u_p,v_p)，(u_c,v_c)分别代表的是归一化之后元素u_p的投影仪坐标和相机坐标。

通过对相位图的每一个元素计算u_p(u_c,v_c)-u_c，然后找到相位图中每一行计算结果最大值(即差值最大)对应的元素，即对应每个v_c的所有元素中差值最大的元素，取其元素值记为Q_max(u_c,v_c)，同时记录其对应坐标u_c(v_c)。于是n个元素值及对应坐标可以共同组成一个n×2的矩阵，其中n是相位图的行数，也是相机像素矩阵的行数，对于JAI GO5000，n＝2048。

对于n×2的矩阵中每一个v_c，通过三角测量原理求解出这一点的相机坐标系下的空间位置(X_D,Y_D,Z_D)，其中Z_D＝L/Q_max，X_D＝Z_D·u_c和Y_D＝Z_D·v_c，其中L为变换矩阵^cT_p的第一行、第四列的元素值。对于n个不同的v_c，得到n个不同的点，而这些点都落在圆柱的一条特征母线上，因此通过最小二乘拟合来求出这条特征母线的方程，得：其中(u 1w)为拟合出的特征母线在F_c下的的方向向量，(x_D z_D)为特征母线与平面X_cO_cZ_c的交点。以上求出的特征母线的方程是在相机坐标系下的表示，因此只需求出这条母线和轴线在X_c和Z_c方向上的偏移量Δ_X和Δ_Z，就能得到轴线的方程。图4为本发明实施例中的轴线和特征母线的关系示意图。如图4所示，坐标系F₀是通过相机坐标系F_c平移得到的，而F₁是F₀饶Y₀旋转α得到的。在坐标系F₁下，椭圆的方程为标准方程。图5为本发明实施例中的一般位置圆柱在相机坐标系中的侧视图。

首先，由图5可知，通过构造坐标系F₁，我们可以得到椭圆在F₀中的方程：(⁰xcosα+⁰zsinα)²cos²γ+(⁰xsinα+⁰zcosα)²＝R²，通过对两边微分，并令d⁰z/d⁰x＝0，我们可以得到特征母线的方程为：通过矩阵变换，微分等数学运算，可以得到母线和轴线在X_c和Z_c方向上的偏移量Δ_X和Δ_Z分别为：和其中(u 1 w)是拟合得到的特征轴线在F_c下的方向向量。至此，我们得到了圆柱轴线的方程：其中轴线特征即由轴线与X_cO_cZ_c的交点坐标(x_o,z_o)，和两个自由方向向量(u,w)组成。再利用一个缝隙检测器就可以在相位图上识别出焊缝所在位置的所有点的坐标，对其求平均值可以得到焊缝中心点在F_c下的三维坐标(x_s,y_s,z_s)，取坐标y_s为代表焊缝在相位图中位置的坐标特征。所以我们定义视觉特征如下：s＝[x_oz_o u w y_s]^T。

6)设计视觉伺服控制律；

以提取到的视觉特征为基础，建立对应视觉特征的视觉伺服的交互矩阵：

对应的视觉伺服控制律为：其中V_c为相机的六维速度，λ是一个增益系数，可视速度要求而定，λ较大，则收敛速度较快，但可能会导致收敛失败，本实施例取λ＝0.2，是交互矩阵的伪逆的近似估计量，在这里由于我们可以在每一步计算得到准确的交互矩阵，因此可以使用其中为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征。

7)将步骤3)离线规划得到的每一个路径点作为视觉伺服的初始状态，利用视觉伺服控制律对每一个路径点的位姿进行优化，在每一个路径点，视觉伺服收敛完成以后，此时机器人的位姿即为该路径点的优化位姿，用计算机记录每一个路径点优化位姿下的机器人各关节角度。直到离线规划的每个路径点都得到优化并被记录以后，该方法结束。由于每个路径点都得到了记录，所以机器人的高重复定位精度可以被用来进一步提高焊接过程中焊枪位姿的精度。

由于本发明结合了结构光测量精度高，视觉伺服控制精度高，以及机器人重复定位精度高的优点，所以可以消除工件安装误差，建模误差，工件变形以及机器人绝对精度不高对实际焊接过程中带来的误差，对离线规划的路径点通过上述步骤处理可以优化其焊枪的位姿，从而提高管道焊接的质量。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本发明的保护范围。

Claims (1)

1.一种用于机器人管道焊接中优化离线规划的焊枪位姿的方法，其特征在于，该方法包括以下步骤：

1)搭建用于离线规划焊枪位姿优化的结构光测量系统；

所述系统包括：带有机器人基座和机器人末端执行器的工业机器人，固定底座，焊枪，激光跟踪仪，投影仪，相机和计算机；所述机器人基座通过螺纹连接固定在光学平台上，固定底座通过螺纹连接固定在机器人末端执行器上，焊枪，投影仪和相机通过螺纹连接依次固定在固定底座上，其中投影仪位于焊枪与相机之间，投影仪与相机的光轴保持在设定的距离且平行，焊枪不出现在相机和投影仪的公共视野内；将待焊接管道通过夹具悬空固定在光学平台上方，激光跟踪仪和机器人分别位于待焊接管道两侧设定距离处；工业机器人，投影仪和相机分别连接计算机；

2)系统参数标定；

标定相机成像模型的内参矩阵M_c，标定投影仪成像模型的内参矩阵M_p，标定相机坐标系F_c与投影仪坐标系F_p之间的变换矩阵^cT_p,通过手眼标定标定相机坐标系F_c与机器人末端执行器坐标系F_e之间的变换矩阵^cT_e，利用激光跟踪仪标定固定在待焊接管道上的工件坐标系F_w与激光跟踪仪坐标系F_L的变换矩阵^wT_L，利用激光跟踪仪标定机器人基座坐标系F_b与激光跟踪仪坐标系F_L的变换矩阵^bT_L，进而求出工件坐标系F_w与机器人基座坐标系F_b之间的变换矩阵^bT_w，利用固定底座的三维模型和焊枪的三维模型测量和计算得到焊枪坐标系F_t相对于机器人末端坐标系F_e的变换矩阵^bT_t，通过用游标卡尺测量K次待焊接管道的外半径取平均值作为待焊接管道的外半径R；

3)离线规划；

由步骤2)中得到的变换矩阵^bT_w和^tT_e，给定焊枪相对待焊接管道的位姿约束以及待焊接管道的外半径R，进行管道焊接路径点的离线规划；

4)调节焊枪到期望位姿并生成对应此位姿下的相位图，具体步骤如下：

4-1)将焊枪移动至步骤3)离线规划得到的第一个路径点，通过激光跟踪仪拟合焊枪及待焊接管道的轴线，两条轴线的关系用距离d和夹角θ来表示；对焊枪的位姿进行调整，使得0≤d＜1mm，89°＜θ＜91°；以该状态下焊枪相对于待焊接管道的位姿作为后续路径点视觉伺服的期望位姿；

4-2)在期望位姿下，利用投影仪投影N张单位频率的正弦条纹图和N张高频率的正弦条纹图投影到待焊接管道表面，同时利用相机采集投影后在待焊接管道表面变形的2N张条纹图，通过相位解包裹方法得到在期望位姿下的相位图；相位图为一个和相机的分辨率相同大小的矩阵，其中每个元素的值是对应该元素位置的空间点在投影仪成像平面下的像点的横坐标；

5)从相位图中提取视觉特征；

从相位图中提取能表征待焊接管道的位姿的轴线特征和能表征焊缝位置的坐标特征，并将其作为视觉伺服的视觉特征；具体步骤如下：

5-1)将相位图的某一个元素用U_p(U_c,V_c)表示，其中U_p是该元素的元素值，(U_c,V_c)是该元素的坐标；通过下面两个公式将相位图的元素值和坐标转化到归一化平面坐标下：

其中(u_p,v_p)，(u_c,v_c)分别代表的是归一化之后元素u_p的投影仪坐标和相机坐标；

5-2)通过对相位图的每一个元素计算u_p(u_c,v_c)-u_c，然后找到相位图中每一行计算结果最大值对应的元素，将该元素的元素值记为Q_max(u_c,v_c)，同时记录其对应坐标u_c(v_c)，则n个元素值及对应坐标组成一个n×2的矩阵，其中n是相位图的行数；

5-3)对于n×2的矩阵中每一个v_c，通过三角测量原理求解出这一点的相机坐标系下的空间位置(X_D,Y_D,Z_D)，其中Z_D＝L/Q_max，X_D＝Z_D·u_c和Y_D＝Z_D·v_c，其中L为变换矩阵^cT_p的第一行第四列的元素值；对于n个不同的v_c，得到n个不同的点，通过最小二乘法拟合上述n个点得到待焊接管道圆柱的特征母线方程，即其中(u 1 w)为拟合出的特征母线在F_c下的方向向量，(x_D z_D)为特征母线与平面X_cO_cZ_c的交点；

将特征母线和轴线在X_c和Z_c方向上的偏移量分别记为Δ_X和Δ_Z，其中，和

则待焊接管道圆柱轴线的方程如下：

其中

轴线特征即由轴线与X_cO_cZ_c的交点坐标(x_o,z_o)和两个自由方向向量(u,w)组成；

5-4)利用一个缝隙检测器在相位图上识别出焊缝所在位置的所有点的坐标，对其求平均值得到焊缝中心点在F_c下的三维坐标(x_s,y_s,z_s)，取坐标y_s为代表焊缝在相位图中位置的坐标特征；

5-5)利用步骤5-3)和5-4)的结果，得到视觉特征如下：s＝[x_o z_o u w y_s]^T；

6)设计视觉伺服控制律；

以步骤5)得到的视觉特征为基础，建立对应视觉特征的视觉伺服的交互矩阵：

对应的视觉伺服控制律为：其中V_c为相机的六维速度，λ是增益系数，是交互矩阵的伪逆的近似估计量，其中为交互矩阵的伪逆，e＝(s-s^*)为误差项，其中s^*为期望位姿下对应的视觉特征；

7)将步骤3)离线规划得到的每一个路径点作为视觉伺服的初始状态，利用视觉伺服控制律对每一个路径点的位姿进行优化，在每一个路径点，视觉伺服收敛完成以后，此时机器人的位姿即为该路径点的优化位姿，记录每一个路径点优化位姿下的机器人各关节角度，直到得到离线规划的每个路径点的优化位姿和对应机器人各关节角度，方法结束。