CN101359400B - 一种基于视觉的管口空间位置定位方法 - Google Patents

Abstract

本发明提出一种基于视觉的管口空间位置定位方法，主要步骤包括：1)机器人手眼关系标定，多个坐标系的标定与转换，确定2D像素坐标与三维空间场景间的几何映射关系；2)获取冷凝器图像，管口图像点集的分割，拟合管口曲线，提取每个管口的中心像点；3)根据管口中心的像点及参数K、R、t，计算深度信息，进一步确定出管口空间实际位置。水下清洗机器人在启动高压喷水枪清洗管口时，使用该方法辅助其搜索及定位管口位置，实现机器人对管口自动定位及完成清洗工作。该方法可极大提高机器人对管口定位的准确性及机器人代替人类工作的自动化程度，提高机器人的作业性能和对环境的适应性。

Description

一种基于视觉的管口空间位置定位方法

技术领域

本发明属于图像处理和模式识别领域，涉及一种基于视觉的管口空间位置定位方法。

背景技术

目前，从大型机械设备加工的焊缝自动跟踪，精密装配作业，食品罐装、医药灌装的自动化生产线，高楼大厦窗户的自动擦洗，再到恶劣环境下的清洗工作等自动化的各种应用领域中，机器人对物体的定位是机器人作业过程中的关键技术之一。

使用激光传感器、视觉传感器等在机器人系统中已得到成功应用，但是还不能够提供一种通用的满足现代社会高效的、自动化的解决方案。要提高机器人的作业能力、自动化程度及环境的适应性，需针对具体情况、问题设计相应的方案。

本发明主要涉及精密装配、大型厂矿企业的恶劣环境下的作业，机器人利用视觉传感器对工件或工作面自动定位，计算工作场景与机器人的相对位置，辅助机器人自动完成作业，提高机器人的自动化程度及作业性能。

本发明主要针对大型电厂的水下环境作业，智能机器人需在线完成对冷凝器的清洗工作。大型冷凝器由数以万计的小孔径冷凝管组成，流经冷凝管的冷却水冷却用水都是直接取自江、河、湖、海等自然水源，水源不洁净、同时热交换时发生化学反应，造成冷凝器积聚污垢，甚至堵塞。通常的清洗方法多采用胶球、喷丸清洗，人工、机械方式清洗。研究大型冷凝器清洗的自动化关键技术、水下在线智能化清洗装备，以保障安全生产、实现节能降耗，解决人工清洗效率低、工作强度大、环境恶劣等问题，减小机组停机造成的经济损失，具有十分重大的经济社会意义。

目前的冷凝器智能清洗设备大多采用高压水射流清洗，智能机器人启动射流清洗首要解决的问题就是如何将清洗设备的高压喷水枪对准冷凝管管口。管口直径大约3CM左右，冷凝管一般以正方形直列、正方形错列、正三角形错列等几种分布方式嵌于管板上，但由于水流、使用时间久、安装等原因会一定变形，管口空间位置坐标分布并不是完全均匀的、规律的，需确切地分析计算其位置。

智能清洗机器人对管口定位是否准确，以及能否自动寻找到管口是提高机器人自动化程度和作业性能的关键。传统方法大多是人工计算管口位置再控制机器人移动，机器人执行效率低，易带来误差，自动化程度低。

发明内容

本发明主要解决的技术问题：智能机器人在水下环境作业，如何将高压喷水枪自动对准冷凝管管口，提高机器人作业性能和效率。实际水下环境的管口会受到污垢、水下悬浮的沼泽物、水的流动等的干扰，采用视觉传感器获取的管口图像，检测不到清晰的、完整的管口图像，这是管口中心定位的一个难点。由管口中心像点到管口实际位置的转换涉及多坐标系标定与转换，这是另一个需解决的问题。

本发明提供一种基于视觉的管口空间位置定位方法，可以实现冷凝管管口的精确定位，为清洗机器人的清洗工作提供必要的支持。

本发明的技术解决方案如下：

一种基于视觉的管口空间位置定位方法，采用装配了摄像机的机器人对需要清洗的冷凝管管口进行定位，其特征在于，包括以下步骤：

3)离线标定机器人手眼关系R、t和摄像机内参数K；其中R为旋转关系，t为平移关系；

4)通过摄像机获取管口图像，从图像中分离出管口并经拟合获取第i个管口中心像点坐标m_i；其中i＝1～n，n为总的管口数；

3)根据机器人手眼关系R、t和摄像机内参数K以及管口中心像点坐标m_i求得第i个管口的空间位置(X_i，Y_i)；

所述步骤1)中标定机器人手眼关系R、t的步骤为：

A)精确控制机器人的机械手末端执行器做2次以上运动，机械手平台运动量记为Me，Me从光电编码器读出；

B)机器人的摄像机运动量记为Mc，Mc的确定如下：摄像机对场景中具有直线的景物进行成像，空间景物直线Lc，及对应的像直线l，透视中心O位于同一平面π，取像直线方向

及像直线的垂线

作为平面π内的正交矢量基；空间景物直线Lc在平面π内的正交分解：

以四元数表示

线矩

γ，β是正交分解的系数；P是空间景物直线Lc上的点；

平台从位置A移动到B，移动前后分别提取图像中对应的直线l_a，l_b，其垂线分别为l_a⊥，l_b⊥，摄像机的运动量满足：

是摄像机运动量Mc的双四元数表示；

D)平台运动量Me与摄像机运动量Mc之间手眼关系满足：

为平台运动量Me的双四元数表示；

其中，双四元数手眼关系

包含了旋转关系R和平移关系t，将上式化简整理为矩阵形式，最小二乘法求解双四元数手眼关系

分解出旋转关系R和平移关系t；

其中，

分别为l_a、l_b和l_c⊥的双四元数表示，

为

的共轭；

为的共轭；

所述步骤2)包括：

A)聚类：运用均值平移算法对二值图像I的边缘数据点{x_k}_k＝1 ^N进行聚类，收敛于聚类中心点x：

公式 $f_{\mathrm{\σ}}\left(x\right)=\frac{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left({\left|\right|\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)x_k}{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left({\left|\right|\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)}-x,$

式中，x_k＝{u，v}是像点位置2D信息数据点；ω(·)是高斯核函数，σ是聚类带宽；N个数据点预划分在s个子集set_j内，其中j＝1，…，s，s在聚类过程中自动更新；

B)起终点搜索：运用模板与当前块执行“与”运算，遍历当前点I(u，v)为边缘曲线点的所有点，若结果为2时，即ss为起点或终点，

公式：

式中tmp 1是3×3元素为1的模板，I_B是二值图像I的3×3当前检测块；符号“⊙”表示“与”运算；

C)分割：在聚类数据块内进行连通域检测，标记出不同的管口点集，从每个起点或终点开始进行8邻域的连通域递归检测分割标记；

检测标记detect＝(tmp2⊙I_B(u，v))；

重复标记检测re_detect＝tmp2⊙mark_B(u，v)；

得到连通边缘点标记

式中tmp 2是3×3中心元素为0，其它为1的模板，mark_B是连通标记mark的当前块；符号

表示“或”运算；

D)将各聚类区域的每条连通线局部坐标转换到整个像平面的全局坐标，记为连通域C_i(i＝1，2，…)；

E)管口中心像点的提取

整个视场内管口图像点集分割后，根据连通域C_i(i＝1，2，…)边缘点数据，采用最小二乘法、Hough变换进行二次曲线拟合，拟合为圆或椭圆，提取管口中心像点坐标m_i；

所述步骤3)为：

任一点从空间场景物体坐标系X_w到摄像机坐标系X_c的转换关系为：从空间场景物体坐标系到机器人坐标系的转换关系^bT_w，从机器人坐标系到机械手臂坐标系的转换关系^eT_b，从机械手臂坐标系到摄像机坐标系的手眼转换为^cT_e，则各坐标系转换关系X_c＝^cT_e ^eT_b ^bT_wX_w，图像坐标系下的像点关系为λ_im_i＝KX_c，λ_i为深度；选机器人坐标系与空间场景物体坐标系为同一坐标系，^eT_b＝{R_b，t_b}从光电编码器读取，R_b，t_b分别表示机器人机械臂在清洗中执行的旋转关系、平移关系，^cT_e＝{R，t}是手眼关系，则任一管口中心像点m_i与其对应的空间坐标位置X_wi之间存在关系：

λ_iR_b ^TR^TK^-1m_i-R_b ^T(t_b+R^Tt)＝X_wi；

其中，R_b ^TR^TK^-1m_i是一个三维列向量，其三个分量分别为a_i，b_i，c_i；R_b ^T(t_b+R^Tt)是一个三维列向量，其三个分量分别为t₁，t₂和t₃；

则深度

其中Z为机器人坐标系原点到管口平面的距离，为已知量；

管口的空间位置(X_i，Y_i)为

作为改进，步骤E)还包括：当同一个圆被聚为不同的类，标记为不同的连通域，将不同的连通域进行曲线拟合后，再取其中心点的均值作为管口中心像点坐标m_i。

作为改进，所述步骤E)还包括：采取几何方法矫正管口圆中心点。基于透视投影变换和空间解析几何理论，建立中心像点在像平面上的畸变误差数学模型，获得该畸变误差的变化规律，矫正中心像点，详细参见仪器仪表学报上魏振忠的论文《透视投影变换中椭圆中心畸变误差模型及其仿真研究》。

本发明的有益效果：

与现有技术相比，本发明的优点就在于：

1.提出基于直线的手眼关系标定方法，标定过程中只需提取场景中的直线基元的像，与传统方法相比，无需标定平台在不同方位下的摄像机的姿态。线基元比点基元标定的结果精度更高，运动过程对机械手的运动控制操作方便、算法实现简洁。

2.改进了连通域标记方法，连通域标记中，起终点搜索是影响标记速度的关键。提出运用聚类分块，再在区域块内搜索起终点，提高了标记速度，避免重复标记。

3.针对在实际的水下环境获取的图像不清晰，不完整，干扰大，给冷凝管管口图像分割带来的困难，提出基于聚类的连通域检测方法，采用基于聚类的算法能够克服干扰，快速准确分割管口点集。

4.利用所有管口位于同一平面的信息，给出单幅图像的深度信息计算方法，确定管口图像到空间位置的映射关系。

附图说明

图1是本发明基于视觉的管口空间位置定位方法原理框图；

图2是本发明的管口空间定位过程流程图；(a)冷凝管管口边缘检测结果，(b)冷凝管管口边缘分割及中心像点；

图3是本发明的定位方法具体实施流程图；

图4是本发明基于直线的手眼标定方法原理框图；

图5是本发明基于聚类的管口边缘检测分割方法原理框图；

具体实施方式

下面参照附图和实施例对本发明的实施进行说明。

实施例1：

将水下清洗机器人置于大型发电厂、炼钢厂等恶劣环境中执行清洗管口任务，机器人的机械臂装配有视觉系统，通过视觉系统感测待清洗管口，计算其对应的管口空间位置。根据作业场地面积的大小及摄像机的有效视场，离线人工计算将工作面划分区域，分块粗定位，控制机器人移动，在机器人移动到粗定位的某一确定位置，摄像机观察局部范围内管口分布，利用视觉系统精确定位每个管口的位置。

本发明过程原理框图如下图1，首先离线标定机器人手眼关系，确定2D像素坐标与三维空间场景间的几何映射关系；然后，机器人移动到某一局部区域，摄像机获取视场内数根管口的图像，采用基于聚类的连通域检测法进行管口点集分割，将图像中数个管口分割开，对每个管口点集曲线拟合确定出管口中心像点；进一步计算管口到机器人的深度距离信息，最后确定管口的实际空间位置。图形说明过程如图2。

本方法由全局分块粗定位和局部视觉细定位两步完成，具体实施过程如图3，详细步聚如下。

1、离线标定手眼关系R，t

利用视觉传感器对目标成像的像点来确定目标的空间位置，主要是涉及图像坐标系、摄像机坐标系、机器人及机械手臂、空间场景物体坐标系等多个坐标系之间的转换，简言之，确定2D图像像素坐标系与3D场景坐标系之间的几何映射对应关系。

机器人装配的视觉系统是将摄像机固定于机械臂上，从机器人机械手臂坐标系到摄像机坐标之间的变换关系称为手眼关系，包括旋转关系R，平移关系t两部分。

基本原理

说明：手眼标定流程如图4，推导本发明提出的基于直线进行手眼关系标定基本原理，即得到的公式(1～5)。

手眼关系标定过程中直线采用双四元素表示法，存在以下基本关系：

方向l，线矩m的直线表示为

且线矩m为点P与方向的叉积，

直线(l_a，m_a)经过旋转R_ab，平移

运动后得到(l_b，m_b)，有

其中的q是任一运动量(R_ab，

)的四元素表示。

标记符号说明：矢量四元素q、双四元素共轭

算法：

首先利用文献杂志IEEE PAMI上“A flexible new technique for camera calibration”张正友的平面模板法，离线对摄像机内参数K进行标定，然后再进行手眼标定。采用直线基元建立机械手末端执行器与摄像机两坐标系之间相对位置的约束方程组，利用双四元素法线性求解手眼关系旋转矩阵R，平移向量t。

1)精确控制机械手末端执行器做n(n＞2)次以上运动，机械手平台运动量记为Me，Me从光电编码器读出；

2)摄像机运动量记为Mc，Mc的确定如下：

摄像机对场景中具有直线的景物进行成像，空间景物直线Lc，及对应的像直线l，透视中心O位于同一平面π，取像直线方向及像直线的垂线

作为平面π内的正交矢量基。直线Lc在平面π内的正交分解：

以四元素表示

线矩

γ，β是正交分解的系数。

若平台从位置A移动到B，移动前后分别提取图像中对应的直线l_a，l_b，其垂线分别为l_a⊥，l_b⊥，摄像机的运动量满足：

(

是旋转及平移运动量Mc的双四元素表示)；

3)平台运动量

与摄像机运动量

之间手眼关系满足

4)将上式化简整理为矩阵形式，最小二乘法求解手眼关系

分解出旋转关系R，平移关系t。此处进行手眼标定的技术为现有技术。

2、基于聚类的连通域检测分割法

根据作业场地面积的大小及摄像机的有效视场，计算机器人的局部定位点，直接控制机器人移动的距离。驱动机械结构，开启采集系统，调节光圈，设定焦距，获取图像。对图像进行边缘检测，突出待分析的管口目标物，提取管口的边缘点集，分割属于每个管口的边缘点集，确定管口中心的像点。

实际场景中获取的冷凝器管口图像通过边缘检测提取的管口圆边缘像点，存在很多干扰点，采取滤波去噪、形态学等去除管口周围的污垢、水渣等影响，得到较清晰的管口边缘信息的二值图像如附图2(a)，再进行提取分割。

基本原理

说明：管口边缘点检测分割流程如图5，推导本发明提出的基于聚类的连通域检测分割法的基本原理，即得到的公式(6～7)。

算法：

1)聚类：运用均值平移算法对二值图像I的边缘数据点{x_k}_k＝1 ^N进行聚类，收敛于聚类中心点x：

公式 $f_{\mathrm{\σ}}\left(x\right)=\frac{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left({\left|\right|\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)x_k}{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left({\left|\right|\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)}-x,$

式中，x_k＝{u，v}是像点位置2D信息数据点；ω(·)是高斯核函数，σ是聚类带宽；N个数据点预划分在s个子集set_j内，其中j＝1，…，s，s在聚类过程中自动更新；

2)起终点搜索：运用模板与当前块执行“与”运算，遍历当前点I(u，v)为边缘曲线点的所有点，若结果为2时，即ss为起点或终点，

公式：

式中tmp 1是3×3元素为1的模板，I_B是二值图像I的3×3当前检测块；符号“⊙”表示“与”运算；

3)若某一聚类只包含一个管口的点集，则该聚类中心即为管口中心的像点，但是由于管口之间的间隔较小，多个管口的部分点集聚为一类，此时必须在聚类数据块内进行连通域检测，标记出不同的管口点集。在此上两步基础上，从每个起点或终点开始进行8邻域的连通域递归检测分割标记。

公式检测标记detect＝(tmp2⊙I_B(u，v))；

重复标记检测re_detect＝tmp2⊙mark_B(u，v)；

得到连通边缘点标记

式中tmp2是3×3中心元素为0，其它为1的模板，mark_B是连通标记mark的当前块；符号表示“或”运算；

4)将各聚类区域的每条连通线局部坐标转换到整个像平面的全局坐标，记为连通域C_i(i＝1，2，…)如附图2(b)的规则的不规则的分段圆。

3.管口中心像点的提取

整个视场内管口图像点集分割后，根据连通域C_i(i＝1，2，…)边缘点数据，采用最小二乘法、Hough变换进行二次曲线拟合，拟合为圆或椭圆，提取中心。同一个圆可能被聚为不同的类，也就标记为不同的连通域，可以连通域合并后再拟合。也可以不同的连通域进行曲线拟合后，再取其中心点的均值，后者更准确。

管口圆图像的中心一般与实际圆心的像点是不重合的，采取几何方法矫正管口圆中心点，提取管口空间中心的实际像点如附图2(b)的各个分段圆的圆心。

4、管口中心空间位置的确定

所有冷凝管的管口位于同一平面，即墙平面，设其空间位置采用卡笛尔坐标(X，Y，Z)表示，墙平面距离机器人的垂直距离Z是可以设定，即为已知量，通过离线标定的摄像机内参数及手眼关系，将管口中心像点坐标转换为管口空间位置(X，Y)，即管口中心位置(相对于机器人质心)左右、上下偏移量(X，Y)的值。如下式(10)，求解过程如下。

基本原理

说明：推导管口空间位置坐标的确定过程，即公式(8～10)。

算法：

1)离线标定摄像机内参数K及手眼关系R，t；

2)分割管口图像点集；

3)曲线拟合，提取及矫正管口中心像点

4)求解在摄像机坐标系下光轴Z方向管口到摄像机光心的距离，即深度信息λ_i，

由于墙平面距离机器人的垂直距离Z是已知的，利用这个条件确定深度信息λ_i。设任一点从空间物体坐标系X_w到摄像机坐标系X_c的转换关系为：从空间场景物体坐标系到机器人坐标系的转换关系^bT_w，从机器人坐标系到机械手臂坐标系的转换关系^eT_b，从机械手臂坐标系到摄像机坐标系的手眼转换为^cT_e，即各坐标系转换关系X_c＝^cT_e ^eT_b ^bT_wX_w，再到图像坐标系下的像点关系λ_im_i＝KX_c，其中m_i为任一管口中心像点。选机器人坐标系与世界坐标系为同一坐标系，^eT_b＝{R_b，t_b}从光电编码器读取，R_b，t_b是机器人机械臂在清洗中执行的旋转、平移运动。^cT_e＝{R，t}是手眼关系，则任一管口中心像点m_i与其对应的空间坐标位置X_wi，存在关系λ_iK^-1m_i＝RR_bX_wi+(Rt_b+t)

整理：   λ_iR_b ^TR^TK^-1m_i-R_b ^T(t_b+R^Tt)＝X_wi          (8)

令R_b ^TR^TK^-1m_i＝[a_i，b_i，c_i]^T，R_b ^T(t_b+R^Tt)＝[t₁，t₂，t₃]^T，

R_b ^TR^TK^-1m_i是一个三维列向量，可以求得，为方便，计为a_i，b_i，c_i；R_b ^T(t_b+R^Tt)也是一个三维列向量，分别记为t₁～t₃。

则深度 ${\mathrm{\λ}}_i=\frac{Z+t_3}{c_i}---\left(9\right)$

其中，Z是机器人坐标系原点到管口平面的距离，简言之，机器人到管口平面的距离，可设定的值，即已知量。

5)管口的空间位置(X_i，Y_i)为

$\left\{\begin{array}{c}{X}_i=a_i{\mathrm{\λ}}_i-t_1\\ Y_i=b_i{\mathrm{\λ}}_i-t_2\end{array}\right.---\left(10\right).$

Claims (2)

1.一种基于视觉的管口空间位置定位方法，采用装配了摄像机的机器人对需要清洗的冷凝管管口进行定位，其特征在于，包括以下步骤：

1)离线标定机器人手眼关系R、t和摄像机内参数K；其中R为旋转关系，t为平移关系；

2)通过摄像机获取管口图像，从图像中分离出管口并经拟合获取第i个管口中心像点坐标m_i；其中i＝1～n，n为总的管口数；

3)根据机器人手眼关系R、t和摄像机内参数K以及管口中心像点坐标m_i求得第i个管口的空间位置(X_i，Y_i)；

所述步骤1)中标定机器人手眼关系R、t的步骤为：

A)精确控制机器人的机械手末端执行器做2次以上运动，机械手平台运动量记为Me，Me从光电编码器读出；

B)机器人的摄像机运动量记为Mc，Mc的确定如下：

摄像机对场景中具有直线的景物进行成像，空间景物直线Lc，及对应的像直线l，透视中心O位于同一平面π，取像直线方向及像直线的垂线

作为平面π内的正交矢量基；空间景物直线Lc在平面π内的正交分解：

以四元数表示

线矩

γ，β是正交分解的系数；P是空间景物直线Lc上的点；

平台从位置A移动到B，移动前后分别提取图像中对应的直线l_a，l_b，其垂线分别为l_a⊥，l_b⊥，摄像机的运动量满足：

是摄像机运动量Mc的双四元数表示；

C)平台运动量Me与摄像机运动量Mc之间手眼关系满足：

为平台运动量Me的双四元数表示；

其中，双四元数手眼关系

包含了旋转关系R和平移关系t，将上式化简整理为矩阵形式，最小二乘法求解双四元数手眼关系

分解出旋转关系R和平移关系t；

其中，

分别为l_a、l_b和l_b⊥的双四元数表示，

为

的共轭；为的共轭；

所述步骤2)包括：

A)聚类：运用均值平移算法对二值图像I的边缘数据点{x_k}_k＝1 ^N进行聚类，收敛于聚类中心点x：

公式 $f_{\mathrm{\σ}}\left(x\right)=\frac{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left({\left|\right|\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)x_k}{{\mathrm{\Σ}}_{j=1}^s{\mathrm{\Σ}}_{x_k\∈{\mathrm{set}}_j}\mathrm{\ω}\left(\right|\left|{\frac{x_k-x}{\mathrm{\σ}}\left|\right|}^2\right)}-x,$

式中，x_k＝{u，v}是像点位置2D信息数据点；ω(·)是高斯核函数，σ是聚类带宽；N个数据点预划分在s个子集set_j内，其中j＝1，…，s，s在聚类过程中自动更新；

B)起终点搜索：运用模板与当前块执行“与”运算，遍历当前点I(u，v)为边缘曲线点的所有点，若结果为2时，即ss为起点或终点，

公式：

式中tmp1是3×3元素为1的模板，I_B是二值图像I的3×3当前检测块；符号“⊙”表示“与”运算；

C)分割：在聚类数据块内进行连通域检测，标记出不同的管口点集，从每个起点或终点开始进行8邻域的连通域递归检测分割标记；

检测标记det ect＝(tmp2⊙I_B(u，v))；

重复标记检测re_det ect＝tmp2⊙mark_B(u，v)；

得到连通边缘点标记 $\mathrm{mark}=\det \mathrm{ect}\⊕\mathrm{re}\_\det \mathrm{ect};$

式中tmp2是3×3中心元素为0，其它为1的模板，mark_B是连通标记mark的当前块；符号

表示“或”运算；

D)将各聚类区域的每条连通线局部坐标转换到整个像平面的全局坐标，记为连通域C_i(i＝1，2，…)；

E)管口中心像点的提取

整个视场内管口图像点集分割后，根据连通域C_i(i＝1，2，…)边缘点数据，采用最小二乘法、Hough变换进行二次曲线拟合，拟合为圆或椭圆，提取管口中心像点坐标m_i；

所述步骤3)为：

任一点从空间场景物体坐标系X_w到摄像机坐标系Y_c的转换关系为：从空间场景物体坐标系到机器人坐标系的转换关系^bT_w，从机器人坐标系到机械手臂坐标系的转换关系^eT_b，从机械手臂坐标系到摄像机坐标系的手眼转换为^cT_e，则各坐标系转换关系X_c＝^cT_e ^eT_b ^bT_wX_w，图像坐标系下的像点关系为λ_im_i＝KX_c；λ_i为深度；选机器人坐标系与空间场景物体坐标系为同一坐标系，^eT_b＝{R_b，t_b从光电编码器读取，R_b，t_b分别表示机器人机械臂在清洗中执行的旋转关系、平移关系，^cT_e＝{R，t}是手眼关系，则任一管口中心像点m_i与其对应的空间坐标位置X_wi之间存在关系：

λ_iR_b ^TR^TK^-1m_i-R_b ^T(t_b+R^Tt)＝X_wi；

其中，R_b ^TR^TK^-1m_i是一个三维列向量，其三个分量分别为a_i，b_i，c_i；R_b ^T(t_b+R^Tt)是一个三维列向量，其三个分量分别为t₁，t₂和t₃；

则深度

其中Z为机器人坐标系原点到管口平面的距离，为已知量；

管口的空间位置(X_i，Y_i)为 $\left\{\begin{array}{c}{X}_i=a_i{\mathrm{\λ}}_i-t_1\\ Y_i=b_i{\mathrm{\λ}}_i-t_2\end{array}\right..$

2.根据权利要求1所述的基于视觉的管口空间位置定位方法，其特征在于，所述步骤E)还包括：当同一个圆被聚为不同的类，标记为不同的连通域，将不同的连通域进行曲线拟合后，再取其中心点的均值作为管口中心像点坐标m_i。

Images