纱线自动装卸方法及系统与流程

本发明属于纺织技术领域，特别涉及一种纱线自动装卸方法及系统。

背景技术：

在纺织行业，在纱柱(串纱线的柱子)上进行纱线的装卸是一个重要步骤。在实际工作过程中，大量的纱柱呈圆盘状垂直分布在托盘上，托盘先被放置在机械抓手下方且位于支承框架下方中部位置。为了让机器抓手正确到达每一纱柱顶端进行纱线的装卸工作，在机器抓手动作之前首先要对每一托盘的纱柱位置坐标通过人工逐个进行坐标标注，然后将每一纱柱对应的坐标点逐个输入至控制系统，而后机器抓手按照预定的坐标运行进行纱线装卸。例如申请号为2008201827153，申请日为2008年12月23日的中国专利文献公开了名为“一种新型搬运机器手”的实用新型专利，在实际应用中，该搬运机器手顶部通过移动机构连接在支承框架上和其底部设置有可开合的机器抓手，通过控制系统控制移动机构和移动机构带动搬运机器手移动至某一纱柱正上方，然后控制机器抓手往纱柱顶端方向运动，当机器抓手运动至设定位置时再通过控制系统控制机械抓手进行柱纱的抓取，抓取成功之后，再通过控制系统控制搬运机器手将纱柱搬运至指定位置。

但是在实际工作中，托盘纱柱数量多，上述坐标标注工作量大且容易标注错误，标注过程费时费力。在纺织行业，虽然抓取柱纱工作只是很简单的一个步骤，但是该步骤的快慢及其自动化程度严重影响着整个纺织行业的发展程度，因此如何提高托盘上每一纱线的装卸自动化程度一直是纺织行业研究与探讨的关键课题。

技术实现要素：

本发明要解决的技术问题是，提供一种纱线自动装卸方法及系统，实现纱柱的自动标定以及纱线的主动装卸，提高纱线装卸工作的自动化、智能化以及工作效率。

为解决上述技术问题，本发明所提供的纱线自动装卸方法，包括以下步骤：

s1：机器人夹具携带纱线运动至若干标定点，利用若干图像采集装置各自采集每个标定点的纱线深度图像；

s2：各个图像采集装置从各自的深度图像中提取出各自对应的局部坐标系下的纱线点云数据集合；

s3：对各个纱线点云数据集合进行预处理，利用预处理后的纱线点云数据计算从各个局部坐标系到机器人绝对坐标系的转移矩阵；

s4：确定局部坐标系下的纱柱点云数据，利用所述转移矩阵进行机器人绝对坐标系下的纱柱坐标标定；

s5：根据所述纱柱标定，机器人执行纱线的装卸任务。

在上述技术方案中，通过采集局部坐标系下的纱线点云数据可得到各个局部坐标系到机器人绝对坐标系的转移矩阵，进而根据局部坐标系下的纱柱点云数据得到机器人绝对坐标系下的纱柱顶端坐标，从而实现了各个纱柱顶端坐标的自动标定，在此基础上再进行纱线的装卸任务。上述技术方案整个过程无需人工参与，自动化和智能化程度高且省时省力，能够提高纱线装卸工作效率，降低工作成本，从而大大促进整个纺织行业发展。

作为本发明技术方案的进一步改进，上述预处理包括以下方式或者各方式的结合：

去除不具备物理意义或与任何物体上没有连接的点；去除与机器人执行任务无关的场景数据；去除不符合机器人已知运动的移动点。

通过预处理过程可以去除与转移矩阵计算无关的信息，并缩减用于计算转移矩阵的点云数据集合的大小，从而降低数据计算代价，提高转移矩阵的计算精度。

作为本发明技术方案的进一步改进，所述图像采集装置有n个，所述纱线点云数据中的每一个测量点，其坐标通过概率密度分布表示：

求测量点之间的概率分布的乘积，根据乘积值确定所述纱线点云数据中相同的点，可以得到不同图像采集装置视野重叠处描述相同物理对象的点。这些点用于经典迭代就近点算法，以得到更加准确的转移矩阵。

作为本发明技术方案的进一步改进，根据所述纱柱标定，机器人执行纱线的装卸任务包括：

在所述机器人夹具上安装图像采集装置，当纱柱顶部中心在该图像采集装置所采集图像上的投影点位于虚拟图像中心点时，机器人夹具与纱柱中心轴保持一致，可以执行纱线的装卸任务；所述虚拟图像中心点按照如下方式获得：

s51：提取属于纱柱顶尖的像素；

s52：执行椭圆拟合程序以获得纱柱顶尖的投影轮廓和中心；

s53：变换机器人夹具与纱柱顶端之间的高度并检测椭圆的中心；

s54：构建关于虚拟图像中心点函数如下：

其中:

及:

及:

hi是s53中第i次检测时机器人夹具与纱柱顶端之间的高度，(xhi，yhi)是虚拟图像中心。

通过本技术方案，能够使机器人夹具处于执行装卸任务的最佳姿态，提高装卸工作效率。

为解决上述技术问题，本发明所提供的纱线自动装卸系统包括局部场景建模模块、预处理模块、控制模块以及plc控制单元；其中，

局部场景建模模块，包括若干图像采集装置，每个图像采集装置用于采集各自局部坐标系下的纱线点云数据以及纱柱点云数据；

预处理模块，用于对所述纱线点云数据进行预处理；

控制模块，用于根据预处理后的纱线点云数据计算从各个局部坐标系到机器人绝对坐标系的转移矩阵以及利用所述转移矩阵进行机器人绝对坐标系下的纱柱坐标标定，并向所述plc控制单元发送控制信号；

plc控制单元，用于根据所述控制模块的控制信号控制机器人执行纱线的装卸任务。

在上述技术方案中，通过局部场景建模模块采集局部坐标系下的纱线点云数据和纱柱点云数据，通过控制模块计算各个局部坐标系到机器人绝对坐标系的转移矩阵以及机器人绝对坐标系下的纱柱顶端坐标，从而实现了各个纱柱顶端坐标的自动标定，在此基础上再通过plc控制单元控制机器人进行纱线的装卸任务。上述技术方案整个过程无需人工参与，自动化和智能化程度高且省时省力，能够提高纱线装卸工作效率，降低工作成本，从而大大促进整个纺织行业发展。

作为本发明技术方案的进一步改进，该纱线自动装卸系统还包括定位模块，所述定位模块包括设置于机器人夹具上的图像采集装置，用于采集纱柱图像以及纱柱顶部中心在所采集图像上的投影点；所述控制模块还用于计算虚拟图像中心点，并向所述plc控制单元发送控制信号以控制机器人运动，使纱柱顶部中心在所采集图像上的投影点与所述虚拟图像中心点重合。通过定位模块可以使机器人夹具与纱柱中心轴保持一致，从而处于执行装卸任务的最佳姿态，提高装卸工作效率。

作为本发明技术方案的进一步改进，所述图像采集装置为rgb-d传感器，具有色彩检测可靠性好，应用性好的特点。

作为本发明技术方案的进一步改进，所述局部场景建模模块包括两个rgb-d传感器，分别安装在机器人支承框架的两侧，能够完全覆盖机器人的整个活动区域。

作为本发明技术方案的进一步改进，所述定位模块还包括惯性测量模块，所述惯性测量模块至少用于测量机器人的加速度和角运动，所述控制模块还用于根据所述惯性测量模块的检测数据计算机器人的位置和速度。

作为本发明技术方案的进一步改进，机器人夹具设置有压力传感器，所述控制模块还用于根据所述压力传感器的法向受力计算出压力的模拟量测量值，根据所述模拟量测量值判断所述机器人夹具与纱线是否产生接触。若机器人的夹具在非装卸阶段时触碰到纱线，则将此情况判断此为异常情况并发出警报。

附图说明

图1所示为本发明的框架流程图；

图2所示为本发明机器人夹具上的元件示意图；

图3所示为本发明圆柱体位置示意图；

图4所示为本发明纱柱图像示意图；

图5所示为本发明概率示意图；

图6所示为本发明验证纱柱垂直状态示意图；

图7所示为本发明验证纱柱非垂直状态示意图；

图8所示为本发明虚拟图像示意图；

图9所示为本发明速度曲线示意图。

图中：1，rgb-d传感器ⅰ；2，预处理单元ⅰ；3，rgb-d传感器ⅱ；4，预处理单元ⅱ；5，控制模块；6，plc控制单元；7，rgb-d传感器ⅲ；8，预处理单元ⅲ；9，微程序控制器；10，3d加速度仪；11，陀螺仪；12，压力传感器，13，夹具。

具体实施方式

本实施方式所提供的纱线自动装卸方法包括以下步骤：

s1：机器人夹具(即机器人末端夹持部)携带纱线运动至若干标定点(提前随机设定)，利用n个图像采集装置各自采集每个标定点的纱线深度图像，这n个图像采集装置能够完全覆盖机器人的整个活动区域。

s2：各个图像采集装置从各自的深度图像中提取出各自对应的局部坐标系下的纱线点云数据集合。

s3：对各个纱线点云数据集合进行预处理，预处理包括以下方式或者各方式的结合。

去除不具备物理意义或与任何物体上没有连接的点；去除与机器人执行任务无关的场景数据；去除不符合机器人已知运动的移动点。

通过预处理过程可以去除与转移矩阵计算无关的信息，并缩减用于计算转移矩阵的点云数据集合的大小，从而降低数据计算代价，提高转移矩阵的计算精度。

利用预处理后的纱线点云数据计算从上述各个局部坐标系到机器人绝对坐标系的转移矩阵。

当图像采集装置有n个时，所述纱线点云数据中的每一个测量点，其坐标通过概率密度分布表示：

概率表示在该位置(x，y，z)有物理点的可能性，这样可以减少测量噪声的影响。随着时间的推移，在同一位置看到的点越多，概率就越接近1。

求测量点之间的概率分布的乘积，乘积值较大(可以根据需要确定“较大”的标准)的概率分布所对应的点是相同的点，从而可以得到不同图像采集装置视野重叠处描述相同物理对象的点。这些点用于经典迭代就近点算法，以得到更加准确的转移矩阵。

s4：确定局部坐标系下的纱柱点云数据，利用所述转移矩阵进行机器人绝对坐标系下的纱柱坐标标定；

s5：根据所述纱柱标定，机器人执行纱线的装卸任务。具体包括：在纱柱标定的基础上，机器人夹具携带纱线运动到步骤s4所标定的纱柱坐标正上方，进而执行纱线的装卸任务。

在s5的基础上，机器人夹具实际运动到的位置与实际所要到达的纱柱目标的正上方位置可能存在偏差。故进一步，在所述机器人夹具上安装图像采集装置，当纱柱顶部中心在该图像采集装置所采集图像上的投影点位于虚拟图像中心点时，机器人夹具与纱柱中心轴保持一致，即机器人夹具位于纱柱目标的正上方，可以执行纱线的装卸任务。上述虚拟图像中心点按照如下方式获得：

s51：提取属于纱柱顶尖的像素；

s52：执行椭圆拟合程序以获得纱柱顶尖的投影轮廓和中心；

s53：变换机器人夹具与纱柱顶端之间的高度并检测椭圆的中心；

s54：构建关于虚拟图像中心点函数如下：

其中:

及:

及:

hi是s53中第i次检测时机器人夹具与纱柱顶端之间的高度，(xhi，yhi)是虚拟图像中心。

通过上述过程，即可实现各个纱柱顶端坐标的自动标定，并能够使机器人夹具运动至与纱柱中心轴保持一致，即与实际所要到达的纱柱目标的正上方位置一致，从而位于实际处于执行装卸任务的最佳姿态，提高装卸工作效率。

如图1所示，本实施方式所提供的纱线自动装卸系统包括局部场景建模模块、预处理模块、控制模块5以及plc控制单元6。其中，

局部场景建模模块，包括若干图像采集装置，每个图像采集装置用于采集各自对应的局部坐标系下的纱线点云数据以及纱柱点云数据；

预处理模块，用于对所述纱线点云数据进行预处理；

控制模块5，用于根据预处理后的纱线点云数据计算从各个局部坐标系到机器人绝对坐标系的转移矩阵以及利用所述转移矩阵进行机器人绝对坐标系下的纱柱坐标标定，并向所述plc控制单元6发送控制信号；

plc控制单元6，用于根据所述控制模块5的控制信号控制机器人执行纱线的装卸任务。

下面对本纱线自动装卸系统进行进一步说明：

该系统包括局部场景建模模块、预处理模块、控制模块5和定位模块，所述局部场景建模模块包括rgb-d传感器ⅰ1和rgb-d传感器ⅱ3，预处理模块包括预处理单元ⅰ2和预处理单元ⅱ4，所述rgb-d传感器ⅰ1连接预处理单元ⅰ2，所述rgb-d传感器ⅱ3连接预处理单元ⅱ4，预处理单元ⅰ2和预处理单元ⅱ4均与控制模块5连接，控制模块5连接plc控制单元6。所述rgb-d传感器ⅰ1和rgb-d传感器ⅱ3分别安装在机械人支承框架的两侧，以保证能够完全覆盖机器人的整个活动区域。

如图2所示，该纱线自动装卸系统还包括定位模块，定位模块包括设置于机器人夹具13上的图像采集装置，该图像采集装置可以采用rgb传感器ⅲ7。rgb传感器ⅲ7连接相应的嵌入式预处理单元ⅲ8，预处理单元ⅲ8连接控制模块5。

下面介绍自标定过程：

1)从深度图像中提取点云数据

rgb-d传感器ⅰ1与rgb-d传感器ⅱ3获得的是im(i,j,d)灰度图像i(对应图像平面而言，深度数据由于显示需要将深度数据转化到0-255的灰度值范围内，其中(i，j)是点p(x，y，z)在图像平面上的投影点,d是从rgb-d传感器到点p(x，y，z)点的距离，(p点是rgb-d传感器在三维坐标系下的点，点p(x,y,z)和图像平面中im(i,j,d)是一一对应的)。

rgb-d传感器ⅰ1和ⅱ3所得到的点云数据集合表示如下：

所述pc1表示由rgb-d传感器ⅰ1获得的n1＝9个相对于局部坐标r1的点云数据集合，pc2表示由rgb-d传感器ⅱ3获得的n2＝9个相对于局部坐标r2的点云数据集合。

对pc1和pc2集合数据通过各自连接的预处理单元进行滤波处理以除去以下无用信息：

去除孤立点，不具备物理意义或与任何物体上没有连接的点会被认为是噪声并去除；

缩减pc1和pc2集合大小：rgb-d传感器ⅰ1和ⅱ3覆盖范围大于机器人工作空间范围，除去与机器人执行任务无关的场景数据以降低数据计算代价；

警戒点：不符合机器人已知运动的移动点被认为是异常点，它们将会被标记为报警点并去除；

在上述预处理过程中预处理单元ⅰ2、预处理单元ⅱ4可以根据数据预定义属性提取点云数据集合pc1和pc2的子集用于机器人夹具13的自标定过程，标定借助：

移动点；

特定颜色点；

具有给定对称性(球体，圆柱体，锥体等)的形状的点；

上述两个或更多条件的组合；

本实施方式中，我们可以使用移动点信息子集mpc1和mpc2进行自标定。两个子集存贮关于同一移动对象的局部信息，其中mpc2具有各向异性对称，通常为轴向对称，以便在任意观察位置都能获取相同的形状信息。

2)提取机器人夹具13和纱线的移动信息

从2个rgb-d传感器ⅰ1和ⅱ3获取的图像信息中，机器人移动部分会产生颜色改变，而静止部分颜色保持不变，因此从两幅图像的颜色差值提取机器人移动部分信息。

pm(x，y，z)，difimage(i，j，t)＝||im(i，j，d，t)-im(i，j，d，t-1)||＞threshold

pm表示图像差异大于给定阈值的点的集合，即为机器人抓取纱线运动时检测到的空间中所运动的点。

3)提取纱线信息

已知纱线品种颜色(ry,gy,by),提取相对应的pyarn(x,y,z,r,g,b)点位置(pyarn是机器人空间中纱线纱柱上的点，(x,y,z)对应空间坐标，(r,g,b)对应坐标位置上的颜色)。

首先对图像中颜色信息作归一化处理：

对于每个移动点pm,检测其颜色信息是否在相应颜色域内，当且仅当满足如下条件时判断点pm是纱线上某一点:

pm(xm,ym,zm,rm,gm,bm)∈yarn,iif

其中0<α<1。

4)估计纱线轴和移动物体中心

纱线在2个rgb-d图像中应为圆柱体形状。使用一类经典的形状拟合方法提取2个rgb-d传感器ⅰ1和ⅱ3中的圆柱信息。纱线两侧分别由2个rgb-d传感器ⅰ1和ⅱ3显示。从rgb-d传感器1和rgb-d传感器2，可以取得两组代表纱线一部分的点云数据。已知，点云数据中的点可能是属于纱线的。因此，我们可以通过将得到的点云拟合到纱线的理论形状来获得纱线的坐标。换句话说，我们发现纱线的坐标，最大限度地减少实际的点云和给出的纱线坐标间的距离。在纱线两个3d模型中，分别用pi1(xi1，yi1，zi1)表示rgb-d传感器ⅰ1的第ith个视图中提取到以圆柱体下底面为中心。pi2(xi2，yi2，zi2)表示rgb-d传感器ⅱ3中的第ith个视图中提取到以圆柱体下底面为中心。本实施方式中，选用了9个位置作为标定点(提前随机设定)用于标定，圆柱体在9个位置上分别得到一个视图，如图3所示。根据实际需要，也可以变换用于标定的位置的个数。

5)rgb-d传感器到机器人转移矩阵

当机器人移动时，2个rgb-d传感器ⅰ1和ⅱ3捕获机器人位置qi(xi,yi,zi)下的2*n个3d点云数据[pi1(xi1,yi1,zi1),pi2(xi2,yi2,zi2)]。

求解如下运动方程，分别获得从rgb-d传感器ⅰ1和rgb-d传感器ⅱ3到机器人位置的转移矩阵ts1-r,ts2-r，

其中r是旋转矩阵是平移向量，旋转矩阵求解如下：

r＝v

v是由矩阵pi和qi的协方差矩阵s通过奇异值分解，

s＝u.σ.v^t＝x.y^t

其中x和y矩阵表示:

平移矩阵为:

根据已知物体(机器人末端执行器上点)的移动信息，可以分别推导出机器人夹具13与pc1之间的转移矩阵ts1-r及其与pc2之间的转移矩阵ts2-r。根据转移矩阵ts1-r和ts2-r将局部点云集合pc1和pc2中的数据转化为以机器人夹具13为绝对坐标系的3d点云集合pi(xi,yi,zi)i＝(1,n)，其中0<n<n1+n2，集合中每个点pi(xi,yi,zi)i＝(1,n)表示在机器人工作空间中物体表面点的3d坐标，利用物体的对称性，用形状拟合的方法提取物体对称轴验证自标定过程的准确性。

特别地，本实施方式中还进行了2个rgb-d传感器ⅰ1和ⅱ3数据的并置融合，具体如下：

rgb-d传感器ⅰ1和ⅱ3采集深度信息从而获取一组局部坐标系下关于x、y、z方向的3d点云数据集合。自标定完成后，两个rgb-d传感器ⅰ1和ⅱ3就能够提供机器人绝对坐标系下的3d点集。

rgb-d传感器能够通过红外线反射捕捉物理点的坐标，这些点属于物体的表面。捕捉到的点的坐标值x,y,z带有噪声p^*(x,y,z)＝p(x,y,z)+(εx,εy,εz)，p(x,y,z)是测量点的实际位置，(εx,εy,εz)是在3个维度上的噪声。

考虑到不同的rgb-d传感器的视角不同以及受到噪声的影响，3d点云数据存在误差：由于标定和测量的误差，即使是描述同一个物理对象的3d点，当它处于不同传感器视野中时，点的3d坐标也会有轻微的偏差。

点云中的每一个测量点的坐标都可以用一个概率密度分布表示，即实际的点坐标服从一个概率分布，即高斯分布：我们假设测量误差遵循高斯分布。例如，p(x，y，z)在球心上时，概率取最大值，其概率值会随着距离球心的距离增大而逐渐减小。最后，求测量点之间的概率分布的乘积，乘积值较大的概率分布所对应的两个点是相同的点。

这样，不同传感器视野重叠处，描述相同物理对象的点就可以被检测出来。这些点之后被用于经典icp(迭代就近点算法)以得到更加精确的变换ts1-r和ts2-r，(解释下icp算法)从而减小3d重构的误差。迭代就近点算方法(ipc)是一种已知的优化算法，通过迭代减少点云数据假定代表的形状和给定的形状的距离。方法的输入是理论形状的位置和点云数据，输出是理论形状的位置。

6)纱线立柱(简称纱柱)定位

检测立柱时，只有x和y坐标方向被考虑到，从而一个概率圆((x，y)二维对应一个圆)代替了概率球((x,y,z)三维对应一个球)。因此，每个3d场景点p(xk,yk,zk)被表示为以此点为中心的高斯概率分布：

a和σ为常数。为了确定立柱在垂直方向的概率分布，对n*m个单元上所有的点做水平方向上做概率积分，得出灰度图im(i,j):

其中i＝integer(xk)和j＝interger(yk)对应待确定概率的点；。

如图4所示，给定纱柱形状求得图像im(i，j)，考虑到圆柱体的形状，最终概率图在大多数情况下像一叠墨西哥帽(如图5所示)：由于顶端包括的点相互之间距离非常的接近，所以在概率图上会有一个中心最大值。对于柱状体，最大值所在区域是一个圆就像外部的柱体一样，称之为圆柱。由此可以提取局部最大值推导出在机器人绝对坐标系x-y平面的纱柱顶尖位置。

其中：外部最大轮廓线用于验证纱柱是否垂直，如图6所示是垂直状态，图7所示为非垂直状态。

如果立柱不是垂直的，那么相应的最大值区域的轮廓是椭圆(倾斜的立柱在平面上的投影)并且，立柱顶部的最大值点将不会投影到椭圆中心处。

综上，rgb-d传感器ⅰ1和rgb-d传感器ⅱ3分别将收集的数据经相应的预处理单元进行滤波处理并重构局部的3d场景观测模型，之后再将这些局部坐标系下场景重建数据发送给控制模块5，控制模块5首先执行上述的自标定程序，计算相对于机器人坐标系下的垂直纱柱顶尖位置信息，再通过局部坐标系与机器人绝对坐标系的坐标转换，就可以构造关于垂直纱柱位置的似然函数的概率图，由于纱柱垂直投影属于同心圆，由此约束进行数据拟合获取纱柱顶尖的精确位置。

由上可知，整个系统的各个纱柱顶端坐标的自标定程序在机器人夹具13装载或移动纱线时自动进行，既不需要人为增加视觉的或磁力的特征点，也不需要在工作环境中的添加其他辅助元素。

根据纱柱顶尖位置信息和装卸工序要求，控制模块5将控制指令传给系统plc控制单元6，plc控制单元6控制机器人并使其向纱柱目标位置移动。由于机械结构的限制和安装误差，机器人夹具13实际运动到的位置与实际所要到达的纱柱目标的正上方位置可能存在偏差，因而可以当机器人夹具13运动到预定的纱柱目标位置(或标定位置)附近(根据实际需要确定“附近”的标准)时，与控制模块5相连rgb传感器ⅲ7定位传感激活，并将实时捕获每一纱柱相应的图像信息发送给嵌入式预处理单元ⅲ8进行处理，处理后提取每一纱柱的颜色和形状，根据hsv编码来选择对应于纱柱顶端颜色的色彩带，处理后的图像再根据相应投影中心来提取椭球轮廓，所述椭球轮廓用于判断机器人夹具13是否精准垂直于纱柱正上方，再根据视觉伺服原理控制模块5向plc控制单元6发出控制信号调整机器人夹具13姿态并使其达到垂直精度，即处于执行装卸任务的最佳姿态后开始装卸纱线。控制模块5根据视觉伺服原理的计算控制过程具体如下：

理论上，如果rgb传感器ⅲ7完美的安装在机器人夹具13上，传感器图像中心轴与立柱中心轴保持一致，那么不管传感器距离立柱顶部的高度如何，立柱中心始终保持在传感器提供的图像中心处。但是，由于机械结构的限制和安装的误差，传感器图像中心与机器人夹具13旋转中心轴无法保持一致(即无法保持在一条直线上)，从而传感器图像中心也就无法与立柱中心轴保持一致(即无法保持在一条直线上)。相应的，立柱顶部中心在图像上的投影坐标会与图像中心坐标存在差异，这种差异会因为传感器距离立柱顶部高度的不同而有所不同。这样图像中心就不能作为参考点，用于调节不同高度上机器人夹具13和立柱以使它们的中心轴保持在一条直线上。

考虑到这样的情况，选择立柱顶部在图像上的虚拟图像中心点，而不是图像中心点，帮助调节机器人夹具13位置。虚拟图像中心点：在给定机器人夹具13和立柱顶部距离情况下，移动机器人夹具13使得立柱顶端在传感器图像上的投影处于这某一点时，机器人夹具13和立柱中心轴保持一致。

该虚拟中心由控制模块5通过以下过程自动获得(如图8所示)：

1-提取属于纱柱顶尖的像素，

2-执行椭圆拟合程序以获得纱柱顶尖的投影轮廓和中心，

3-在3个(或其他数量)不同的高度(机器人夹具13与纱柱顶端的高度)h1，h2，h3处检测椭圆的中心(尖端投影)

4-构成摄像头关于虚拟中心高度微分的函数。

所构建的虚拟图像中心的函数如下：

其中:

及:

及:

hi是s53中第i次检测时机器人夹具13与纱柱顶端之间的高度，(xhi，yhi)是虚拟图像中心。

有了高度与投影点的位置函数以及机器人夹具13距离立柱中心的高度信息，就可以得到在任意高度上虚拟图像中心的坐标(x,y),以此控制模块5向plc控制单元6发送信号，来帮助调节机器人夹具13使得中心轴与立柱中心轴一致。此时虚拟图像中心(xhi，yhi)和当前观测到的立柱顶部投影点(xtip，ytip)之间的位置误差(dist)等于0。

通过上述过程，能够调整机器人夹具13姿态并使其达到垂直精度，即处于执行装卸任务的最佳姿态后开始装卸纱线。

如图2所示，所述定位模块还包括惯性测量单元，所述惯性测量单元(简称imu)包括分别安装在机器人夹具13的3d加速度仪10和陀螺仪11，所述3d加速度仪10和陀螺仪11两者集成在同一电路板上并分别由微程序控制器9通过usb串口连接嵌入式预处理单元ⅲ8，所述嵌入式预处理单元ⅲ8分别计算出机器人夹具13的位置、速度：imu根据三轴x，y和z提供加速度。通过积分，我们可以根据x，y和z轴得出3种速度。通过双重积分，我们得到x，y和z的位置。如下所示：

是惯性测量单元测得的加速度向量，是速度向量，对惯性测量单元的加速度向量求积分得出。是传感器的初始速度，传感器的位置向量通过对速度向量求积分得到是传感器在机器人绝对坐标系中的初始位置。

同时在机器人移动过程中，微程序控制器9对imu提供的数据进行卡尔曼滤波处理并对机器人夹具13的速度进行估计，根据估值进而矫正由机器人编码器提供的机器人的位置或速度信息。

由于机器人结构的问题，主要是机械结构的柔韧性，所以有必要设计一套特定的控制策略，以避免激发可能损坏机器人机械结构的振动。除此之外，对于一些非对称机器人，考虑到运动和加速度的具体方向，有必要设计特定的控制器。

机械手的运动按照以下公式以速度模式控制：机器人可以实现多种模式下控制(位置模式、速度模式、加速度模式、转矩控制等)。在此例中使用速度控制机器人，意味着是输入以速度表示，期望的输出是机器人的位置。

·v是速度；

·di是距离起始位置的距离；

·df是距离最终位置的距离；

·m是加速度系数；机器人速度在时间单位中的变化，机器人在减速停止或加速启动时的速度

·k是全局增益系数；k是一个简单的增益，换句话说，它是一个增加或减少最终速度的常数)

·l和k分别是开始和结束使能距离。

速度曲线如图9所示。

本实施方式中，在机器人夹具13两侧分别设置压力传感器12，所述压力传感器12呈阵列分布并设置为两行两列，压力传感器12通过微程序控制器9连接嵌入式预处理单元ⅲ8，微程序控制器9通过压力传感器12的法向受力推导出压力的模拟量测量值，所述模拟量测量值由usb串口发送给嵌入式预处理单元ⅲ8。所述模拟量测量值对应机器人夹具13与纱线之间的压力大小，通过模拟量测量值即可判断机器人夹具13与纱线是否有接触以及接触力度以及机器人夹具13上的纱线有无情况。若机器人夹具13在非装卸阶段时触碰到纱线，压力传感器12、微程序控制器9、嵌入式预处理单元ⅲ8判断此为异常情况并发出警报。

综上，本发明既能够实现纱线装卸过程中各个纱柱顶端坐标的自动标定，又能够对纱线装卸的机器人的包括速度和位置在内的运动过程进行控制，使得机器人夹具以最佳姿态自动进行纱线装卸工作，并能够保障机器人的运行安全性。另外，本发明还能够对纱线有无情况进行判断，避免机器人在非装卸阶段时触碰到纱线。本发明的整个纱线装卸过程无需人工参与，能够提高纱线装卸工作的自动化、智能化以及工作效率，降低纱线装卸成本，从而大大促进整个纺织行业发展。