CN109940626B - 一种基于机器人视觉的画眉机器人***的控制方法 - Google Patents

Abstract

本发明公开了一种基于机器人视觉的画眉机器人***及其控制方法，本发明采用双目视觉获取人脸左右图像。通过靶标标定，获取相机内参和外参，完成左右图片的像素坐标到相对于一个摄像头虚拟点的空间坐标转化。利用手眼标定，求取摄像头与机械臂末端的关系矩阵。通过面部关键点的求取得出10组像素坐标，用这10组像素坐标通过立体转化公式得到眉毛点相对与摄像头虚拟点的空间坐标。然后把这些相对与摄像头虚拟点的空间坐标通过手眼转化变成机械臂末端坐标，用我们机械臂控制算法，即可使机械臂末端的眉笔到达指定的位置，完成画眉动作，提高了画眉机器人的精确性。

Description

一种基于机器人视觉的画眉机器人***的控制方法

技术领域

本发明涉及机器人智能控制、图像处理、机器视觉等领域，尤其涉及一种基于机器人视觉的画眉机器人***及其控制方法。

背景技术

现有的化妆机器人***动作粗糙，无法正确地定位和控制机械臂以完成真正意义上的化妆，导致使用者的前额、眼睑和鼻梁附近都留下化妆墨。而且由于不能按照每个女性的唇形涂抹口红，所以使用者的嘴唇和下巴处也会残留一些化妆品污渍。

通过文献调研发现国内外对化妆机器人的研究较少且已有的设计并没有显示出很好的眉毛化妆效果。随着传感器技术、计算机技术以及人工智能技术的不断发展，人们已不再满足在固定环境中按照程序设定功能完成重复工作的机器人，纷纷向具有高灵活度、环境适应强的智能型机器人进行研究。

目前，我国广泛使用的机器人处于示教再现阶段，机器人的轨迹都是事先确定好的，这样生产线就缺乏灵活性，而且要求机器人对周边的定位精度很高，即使最终产品精度要求不高也必须满足这一条件，这大大降低了生产的柔性，已经难以达到人们生产生活的要求了，机器人的发展必然是朝着多领域的融合。

近年来，随着工业自动化的发展，特别是数字传感器、图像处理和模式识别等技术的快速发展，对目标的精确定位以及跟踪不再是难以解决的问题，使基于视觉伺服控制的机器人得到广泛研究与应用。

发明内容

本发明的目的在于克服现有的化妆机器人存在的精度不高、动作粗糙、灵活度低等不足，提供了一种基于机器人视觉的画眉机器人***。

本发明解决其技术问题采用的技术方案是：一种基于机器人视觉的画眉机器人***，它包括6自由度机械臂、眉笔、双目摄像头和计算机，6自由度机械臂通过网线与计算机相连，双目摄像头通过USB数据线与计算机相连，眉笔固定在6自由度机械臂末端。

本发明的另一目的是提供一种基于机器人视觉的画眉机器人***的控制方法，具体包括如下步骤：

(1)双目标定：

根据双目摄像头的成像原理，建立视觉***模型，分析标定靶标的元素与视觉***模型参数之间的关系，最后获得双目摄像头的参数；

(2)手眼关系的标定：

双目摄像头拍摄标定物的左右两组图像，将左右两组图像上的像素点通过空间转化得到相对于双目摄像头虚拟点的空间坐标；在双目摄像头下方空间选取不少于四个点作为标定点，将6自由度机械臂末端依次移动到标定点上，获得6自由度机械臂末端的空间坐标；建立双目摄像头虚拟点的空间坐标与6自由度机械臂末端的空间坐标之间的转换矩阵；

(3)眉毛图像的处理：

利用双目摄像头获取的图像，然后利用图像处理算法对图片进行处理，获取眉毛的像素坐标；

(4)三维数据的获取：

利用已标定的双目摄像头的双目视觉参数，经过视差原理恢复出眉毛在双目摄像头坐标系中的三维信息，再通过转换矩阵获得获取眉毛在6自由度机械臂坐标系中的三维信息；

(5)基于视觉的机械臂运动控制算法

计算机将眉毛在6自由度机械臂坐标系中的三维信息发送给6自由度机械臂，实现画眉操作。

进一步的，所述双目标定具体为：

双目摄像头拍摄标定图片，利用Visual Studio调用opencv，对标定图片进行标定计算，求出双目摄像头的内部参数、旋转矩阵R、平行向量t；

假设世界坐标系O_xy中的点(x,y)在u、v坐标系中的坐标为(u₀,v₀)，每一个像素在x轴与y轴方向上的物理尺寸为宽d_xmm，高d_y mm，则图像中任意一个像素的索引坐标与物理坐标满足下面的换算关系：

将上式写成矩阵的形式：

这里把之前的坐标(u,v)和(x,y)都转成齐次坐标(u,v,1)和(x,y,1)；

空间上任何一点在图像上的投影位置P为光心O与P点的连线OP与图像平面的交点，这种关系也被称为中心射影或透视投影；由几何比例关系可得出：

其中f为相机的焦距，(x_u,y_u)为P在二维图像坐标系下的坐标，(X_C,Y_C,Z_C)为空间点P在摄像机坐标系下的坐标；

摄像机坐标系与世界坐标系之间的关系可以用旋转矩阵R与平移向量t来描述，即：

其中R为3×3的矩阵；t为3×1的向量；M_b为4×4的矩阵，也被称为摄像机外部参数矩阵；

将公式(2)和公式(5)代入公式(6)，就可以得到P点在世界坐标系下的坐标(X_W,Y_W,Z_W) 与其在图像平面的投影点p的坐标(u,v)的关系：

其中，

M＝M_aM_b为3×4矩阵，称为投影矩阵；M_a完全由a_x、a_y、 u₀、v₀决定的摄像机内部结构(如焦距、光心)有关，称为摄像机内部参数；M_b完全由摄像机相对于世界坐标系的方位(如摆放位置和拍摄角度)决定，称为摄像机外部参数矩阵；P_w是对齐次世界坐标(X_w,Y_w,Z_w,1)的替代表达，P_w＝(X_w,Y_w,Z_w,1)；

确定某一摄像机的内部和外部参数，就完成了被称为摄像机的标定(Calibration)。

进一步的，所述手眼关系的标定具体如下：

选取四个点即A,B,C,D，双目摄像头拍摄图像，读取由左摄像头拍摄的图像中的四个点的像素坐标Al(x，y),Bl(x，y),Cl(x，y),Dl(x，y)和由右摄像头拍摄的图像中的四个点的像素坐标Ar(x，y),Br(x，y),Cr(x，y),Dr(x，y)，根据步骤(1)获得的双目摄像头的参数，利用深度测量将A,B,C,D四点转化成相对于左摄像头的空间坐标A(x，y，z),B (x，y，z),C(x，y，z),D(x，y，z)；使用探针法或者在程序中增加Tcp的长度，即眉笔的长度；并且读出机械臂末端A,B,C,D的坐标；通过矩阵转化后即可得出手眼的相应关系。

进一步的，所述图像处理算法为Dlib和OpenCV算法。

进一步的，所述基于视觉的机械臂运动控制算法具体如下：

采用D-H参数法建立6自由度机械臂的坐标系以及如下表1所示的D-H参数，其中αi表示相连两个部件之间的扭角，α₀代表初始扭角，其扭角为0度，α₁代表了第一手腕和第二手腕之间的扭角，α₂代表了第二手腕和第一连杆之间的扭角，α₃代表了第一连杆和手肘之间的扭角，α₄代表了手肘和第二连杆之间的扭角，α₅代表了第二连杆和肩膀之间的扭角，α₆代表了肩膀和基座之间的扭角；li表示各部件长度，l₁代表了第一手腕的长度，l₂代表了第二手腕的长度，l₃代表了第一连杆的长度，l₄代表了手肘的长度，l₅代表了第二连杆的长度， l₆代表了肩膀的长度；di表示相连两个部件之间的相对位置，d₁代表了第一手腕和第二手腕之间的相对位置，d₂代表了第二手腕和第一连杆之间的相对位置，d₃代表了第一连杆和手肘之间的相对位置，d₄代表了手肘和第二连杆之间的相对位置，d₅代表了第二连杆和肩膀之间的相对位置，d₆代表了肩膀和基座之间的相对位置；θi表示相连两个部件之间的夹角，θ₁代表了第一手腕和第二手腕之间的夹角，θ₂代表了第二手腕和第一连杆之间的夹角，θ₃代表了第一连杆和手肘之间的夹角，θ₄代表了手肘和第二连杆之间的夹角，θ₅代表了第二连杆和肩膀之间的夹角，θ₆代表了肩膀和基座之间的夹角；

表1：

由6自由度机械臂的结构参数，可得到各连杆参数以及变量工作范围；

根据两相连部件变换公式和表1参数可得，变换矩阵：

为各连杆的变换矩阵，c(θ_i)为cosθ_i,，s(θ_i)为sinθ_i,，c(α_i-1)为cosα_i-1,c(α_i)为cosα_i,，s(α_i) 为sinα_i,s(α_i-1)为sinα_i-1；

得到各连杆的变换矩阵，将各连杆变换矩阵相乘，便可得到6自由度机械臂的变换矩阵

上式中，c_l代表cosθ₁,c₂代表cosθ₂,c₃代表cosθ₃,c₄代表cosθ₄,c₅代表cosθ₅,c₆代表cosθ₆,s₁代表sinθ₁,s₁代表sinθ₁,s₂代表sinθ₂,s₃代表sinθ₃,s₄代表sinθ₄,s₅代表sinθ₅,s₆代表sinθ₆；

其中：

n_x＝c₆(s₁s₅+c₅c₁c₂₃₄)-s₆c₁s₂₃₄

n_y＝c₆(c₅s₁c₂₃₄-c₁s₅)-s₆s₁s₂₃₄

n_z＝c₅c₆s₂₃₄+c₂₃₄s₆

o_x＝-s₆(c₅c₁c₂₃₄+s₁s₅)-c₆c₁s₂₃₄

o_y＝-s₆(c₅s₁c₂₃₄-c₁s₅)-c₆s₁s₂₃₄

o_z＝c₆s₂₃₄-c₅c₆s₂₃₄

a_x＝-s₅c₁c₂₃₄+c₅s₁

a_y＝-s₁s₅c₂₃₄-c₁c₅

a_z＝-s₅s₂₃₄

p_x＝d₅c₁s₂₃₄+d₄s₁+d₆(c₅s₁-s₅c₁c₂₃₄)+l₂c₁c₂+l₃c₁c₂₃

p_y＝d₅s₁s₂₃₄-d₄c₁-d₆(c₅s₁+s₅s₁c₂₃₃)+l₂s₁c₂+l₃s₁c₂₃

p_z＝d₁-d₅c₂₃₄+l₂s₂+l₃s₂₃-d₆s₅s₂₃₄

上式中，c_l代表cosθ₁,c₅代表cosθ₅,c₆代表cosθ₆,c₂₃代表cos(θ₂+θ₃),c₂₃₄代表 cos(θ₂+θ₃+θ₄),s₁代表sinθ₁,s₂代表sinθ₂,s₅代表sinθ₅,s₂₃代表sin(θ₂+θ₃),s₂₃₄代表 sin(θ₂+θ₃+θ₄)；

将6自由度机械臂初始角度代入式(8)结果第四列的前三行代表了6自由度机械臂末端点的位置坐标。

本发明的有益效果是：本发明通过双目摄像头获取人物图像，计算机识别该人物图像中眉毛的关键点相对于图片左上角的像素点坐标，将平面像素坐标转换为空间立体坐标后再转化成机械臂可识别的坐标，从而驱动其机械臂末端到达对应的位置。本发明通过双目摄像头获取人物图像，坐标精确度相对较高，能够有效地判断出人像的眉毛位置，并准确地到达对应位置，6自由度机械臂的运动范围和灵活性高，从而可以提高机器人的定位精度与对不同工况环境下的适应力。

附图说明

图1是基于6自由度机械臂和双目视觉的画眉机器人***的结构示意图；

图2是图1的左视图；

图3是***控制流程图；

图4(a)是右摄像头拍摄图，图4(b)是左摄像头拍摄图；

图5(a)是世界坐标系图，图5(b)是针孔成像模型图，图5(c)是平面图像坐标系图；

图6是UR3机械臂D-H坐标系的示意图；

图中：双目摄像头1、眉笔2、第一手腕3、第二手腕4、第一连杆5、手肘6、第二连杆7、肩膀8、基座9。

具体实施方式

下面结合附图做进一步说明。

图1、图2示出了一种基于6自由度机械臂和双目视觉的画眉机器人***，一种基于机器人视觉的画眉机器人***，它包括6自由度机械臂、眉笔5、双目摄像头1和计算机，6自由度机械臂通过网线与计算机相连，双目摄像头通过USB数据线与计算机相连，眉笔固定在6自由度机械臂末端。6自由度机械臂可以采用丹麦优傲公司生产的UR3机械臂，它是一种具有六自由度的高精度机械臂。包括：第一手腕3、第二手腕4、第一连杆5、手肘6、第二连杆7、肩膀8、基座9。双目摄像头1可以采用迈迪威视公司生产的MV-GE300C型号的双目摄像头。所述UR3机械臂是一种小型的协作式桌面机器人，重量只有24.3磅(11千克)，但有效载荷可达6.6磅(3千克)，所有手腕关节均可实现360度旋转，末端关节可无限旋转。

所述双目摄像头，有效像素为300万，使用的传感器类型为CMOS，采用的曝光方式为滚动行曝光，最大分辨率为2048×1536，像元尺寸为3.2μm，像素位深度为10bit，帧率为13，光学尺寸为1/2″，最小曝光时间为0.05ms，输出的图像为彩色图像。

如图3所示，本发明还提供一种基于机器人视觉的画眉机器人***的控制方法，具体包括如下步骤：

(1)双目标定：

根据双目摄像头的成像原理，建立视觉***模型，分析标定靶标的元素与视觉***模型参数之间的关系，最后获得双目摄像头的参数；

(2)手眼关系的标定：

双目摄像头拍摄标定物的左右两组图像，将左右两组图像上的像素点通过空间转化得到相对于双目摄像头虚拟点的空间坐标；在双目摄像头下方空间选取不少于四个点作为标定点，将6自由度机械臂末端依次移动到标定点上，获得6自由度机械臂末端的空间坐标；建立双目摄像头虚拟点的空间坐标与6自由度机械臂末端的空间坐标之间的转换矩阵；

(3)眉毛图像的处理：

利用双目摄像头获取的图像，然后利用图像处理算法对图片进行处理，获取眉毛的像素坐标；

(4)三维数据的获取：

利用已标定的双目摄像头的双目视觉参数，经过视差原理恢复出眉毛在双目摄像头坐标系中的三维信息，再通过转换矩阵获得获取眉毛在6自由度机械臂坐标系中的三维信息；

(5)基于视觉的机械臂运动控制算法

计算机将眉毛在6自由度机械臂坐标系中的三维信息发送给6自由度机械臂，实现画眉操作。

如图4(a)和图4(b)，其中图像处理程序即面部眉毛关键点的提取，主要是采用了Visual Studio调用Dlib和OpenCV两款工具。通过双目摄像头对人物面部左右各拍一张照片，Dlib 程序会检索人面部特征点，并选取眉毛位置的特征点，输出眉毛位置特征点的像素坐标，为后面获取立体坐标做准备。

如图5(a)-(c)，所采用的是Eye-to—hand双目视觉***，是使相机固定然后对工作区间进行图像采集。此种相机安装方式由于相机与机器人本体相互分离，因此相机可以在一个视场下同时观测到操作台上全部目标物体与机械臂末端工具，能够直观的得到两者之间的位姿误差，便于控制器的反馈控制。Eye-to-hand固定相机的安装方式一般能获得较大的视场范围，不会丢失目标。

先通过黑白方格纸靶标完成双目摄像机的单目标定。单目相机标定的目标是获取相机的内参和外参，内参a_x、a_y、u₀、v₀决定的摄像机内部结构(如焦距、光心)有关表征摄像机结构参数，外参是像机相对于世界坐标系的方位由相机的旋转矩阵R和平移向量t决定。Eye-to-hand是常见的CCD针孔相机模型，分别建立世界坐标系(O_wX_wY_wZ_w)、相机坐标系(O_cX_cY_cZ_c)、像平面坐标系(O1_XY)以及像素坐标系(O_uv)。其中相机坐标系原点O_c为相机光心，一般为镜头中心点，Z_c轴沿光轴指向景物方向，X_c轴Y_c轴分别与像素行和列平行。图像坐标系原点O1为光轴与像平面的交点，x，y轴分别与Xc，Yc平行且指向相同。像素坐标(u，v)实际上是像点在像素阵列中的行数与列数，选择u轴和v轴分别平行于X_c轴 Y_c轴且方向相同，原点位于像素阵列的一个角上，阵列内所有像素的u和v坐标为正值。在相机进行标定的过程中，空间点的成像过程必然涉及到相机坐标系，图像坐标系和世界坐标系三者之间的转化。具体转化步骤如下：

空间点到像素坐标系下的转换：

在实际的相机中，并不是以物理单位(如mm)来表示某个成像点的位置的，而是用像素的索引。比如一台相机的像素是1600×1200，说明图像传感器(也就是以前的胶片)横向有1600个捕捉点，纵向有1200个，合计192万个。对于某个成像点，实际上都是这样表示的：假设O_xy在u、v坐标系中的坐标为(u₀,v₀)，每一个像素在x轴与y轴方向上的物理尺寸为宽 d_xmm，高d_ymm，则图像中任意一个像素的索引坐标与物理坐标满足下面的换算关系：

将上式写成矩阵的形式：

这里把之前的坐标(u,v)和(x,y)都转成齐次坐标(u,v,1)和(x,y,1)；

图像坐标系到摄像机坐标系下的转换：

空间上任何一点P在图像上的投影位置P为光心O与P点的连线OP与图像平面的交点，这种关系也被称为中心射影或透视投影；由几何比例关系可得出：

其中(x,y)为p的图像坐标，(X_C,Y_C,Z_C)为空间点P在摄像机坐标系下的坐标；

摄像机坐标系到世界坐标系下的转换：

摄像机坐标系与世界坐标系之间的关系可以用旋转矩阵R与平移向量t来描述，即：

其中R为3×3的矩阵；t为3×1的向量；M_b为4×4的矩阵，也被称为摄像机外部参数矩阵；

世界坐标与像素坐标系下的转换：

将公式(2)和公式(5)代入公式(6)，就可以得到P点在世界坐标系下的坐标(X_W,Y_W,Z_W) 与其在图像平面的投影点p的坐标(u,v)的关系：

其中，

M＝M_aM_b为3×4矩阵，称为投影矩阵；M_a完全由a_x、a_y、 u₀、v₀决定的摄像机内部结构(如焦距、光心)有关，称为摄像机内部参数；M_b完全由摄像机相对于世界坐标系的方位(如摆放位置和拍摄角度)决定，称为摄像机外部参数矩阵；P_w是对齐次世界坐标(X_w,Y_w,Z_w,1)。

完成左右摄像机单目标定后，进行双目标定，双目标定的第一步需要分别获取左右相机的内外参数，之后通过立体标定对左右两幅图像进行立体校准和对其，最后就是确定两个相机的相对位置关系，即中心距，完成双目标定的时候也就确定了两个相机的联系了。其次通过选取四个点即A,B,C,D，通过双目摄像头拍出照片，并且读取这个Al(x，y),Bl(x，y),Cl (x，y),Dl(x，y)和Ar(x，y),Br(x，y),Cr(x，y),Dr(x，y)的像素坐标，通过程序转化成相对于左摄像头的空间坐标A(x，y，z),B(x，y，z),C(x，y，z),D(x，y，z)。使用探针法或者在程序中增加Tcp的长度，即眉笔的长度。并且读出机械臂末端A,B,C,D的坐标。通过矩阵转化后即可得出手眼的相应关系。这样通过读取摄像机拍摄图片的左右像素点就能转化成机械臂可识别的机械臂末端坐标。

所述基于视觉的机械臂运动控制算法具体如下：

如图6所示，采用D-H参数法建立6自由度机械臂的坐标系以及如下表1所示的D-H参数，其中αi表示相连两个部件之间的扭角，α₁代表了第一手腕和第二手腕之间的扭角，α₂代表了第二手腕和第一连杆之间的扭角，α₃代表了第一连杆和手肘之间的扭角，α₄代表了手肘和第二连杆之间的扭角，α₅代表了第二连杆和肩膀之间的扭角，α₆代表了肩膀和基座之间的扭角；li表示各部件长度，l₁代表了第一手腕的长度，l₂代表了第二手腕的长度，l₃代表了第一连杆的长度，l₄代表了手肘的长度，l₅代表了第二连杆的长度，l₆代表了肩膀的长度；di表示相连两个部件之间的相对位置，d₁代表了第一手腕和第二手腕之间的相对位置，d₂代表了第二手腕和第一连杆之间的相对位置，d₃代表了第一连杆和手肘之间的相对位置，d₄代表了手肘和第二连杆之间的相对位置，d₅代表了第二连杆和肩膀之间的相对位置，d₆代表了肩膀和基座之间的相对位置；θi表示相连两个部件之间的夹角，θ₁代表了第一手腕和第二手腕之间的夹角，θ₂代表了第二手腕和第一连杆之间的夹角，θ₃代表了第一连杆和手肘之间的夹角，θ₄代表了手肘和第二连杆之间的夹角，θ₅代表了第二连杆和肩膀之间的夹角，θ₆代表了肩膀和基座之间的夹角；

表1：6自由度机械臂D-H参数表

由6自由度机械臂的结构参数，可得到各连杆参数以及变量工作范围。

根据两相邻连杆变换公式和表1参数可得，变换矩阵：

得到个连杆的变换矩阵，将各连杆变换矩阵相乘，便可得到UR3的变换矩阵

上式中c(θ_i)＝cosθ_i,，s(θ_i)＝sinθ_i,，c(α_i)＝cosα_i,，s(α_i)＝sinα_i。下式中，cl代表cosθ₁，s1代表sinθ₁，c12代表cos(θ₁+θ₂)，θ₁₂₃代表θ₁+θ₂+θ₃。

上式中，cl代表cosθ₁，s1代表sinθ₁，c12代表cos(θ₁+θ₂)，θ₁₂₃代表θ₁+θ₂+θ₃；

其中：

n_x＝c₆(s₁s₅+c₅c₁c₂₃₄)-s₆c₁s₂₃₄

n_y＝c₆(c₅s₁c₂₃₄-c₁s₅)-s₆s₁s₂₃₄

n_z＝c₅c₆s₂₃₄+c₂₃₄s₆

o_x＝-s₆(c₅c₁c₂₃₄+s₁s₅)-c₆c₁s₂₃₄

o_y＝-s₆(c₅s₁c₂₃₄-c₁s₅)-c₆s₁s₂₃₄

o_z＝c₆s₂₃₄-c₅c₆s₂₃₄

a_x＝-s₅c₁c₂₃₄+c₅s₁

a_y＝-s₁s₅c₂₃₄-c₁c₅

a_z＝-s₅s₂₃₄

p_x＝d₅c₁s₂₃₄+d₄s₁+d₆(c₅s₁-s₅c₁c₂₃₄)+l₂c₁c₂+l₃c₁c₂₃

p_y＝d₅s₁s₂₃₄-d₄c₁-d₆(c₅c+s₅s₁c₂₃₄)+l₂s₁c₂+l₃s₁c₂₃

p_z＝d₁-d₅c₂₃₄+l₂s₂+l₃s₂₃-d₆s₅s₂₃₄

将UR3机械臂初始角度代入(8)得出：

式(8)计算结果第四列的前三行代表了6自由度机械臂末端点的位置坐标。

综上所述，完成了画眉机器人的搭建，提高了画眉机器人的精确性，使用者可以体会机器人为人类化妆的愉悦感。

Claims (4)

1.一种基于机器人视觉的画眉机器人***的控制方法，其特征在于，所述一种基于机器人视觉的画眉机器人***包括6自由度机械臂、眉笔、双目摄像头和计算机，6自由度机械臂通过网线与计算机相连，双目摄像头通过USB数据线与计算机相连，眉笔固定在6自由度机械臂末端，该控制方法具体包括如下步骤：

(1)双目标定：

根据双目摄像头的成像原理，建立视觉***模型，分析标定靶标的元素与视觉***模型参数之间的关系，最后获得双目摄像头的参数；

所述双目标定具体为：

双目摄像头拍摄标定图片，利用Visual Studio调用opencv，对标定图片进行标定计算，求出双目摄像头的内部参数、旋转矩阵R、平行向量t；

假设世界坐标系O_xy中的点(x,y)在u、v坐标系中的坐标为(u₀,v₀)，每一个像素在x轴与y轴方向上的物理尺寸为宽d_xmm，高d_ymm，则图像中任意一个像素的索引坐标与物理坐标满足下面的换算关系：

将上式写成矩阵的形式：

这里把之前的坐标(u,v)和(x,y)都转成齐次坐标(u,v,1)和(x,y,1)；

空间上任何一点在图像上的投影位置P为光心O与P点的连线OP与图像平面的交点，这种关系也被称为中心射影或透视投影；由几何比例关系可得出：

其中f为相机的焦距，(x_u,y_u)为P在二维图像坐标系下的坐标，(X_C,Y_C,Z_C)为空间点P在摄像机坐标系下的坐标；

摄像机坐标系与世界坐标系之间的关系可以用旋转矩阵R与平行向量t来描述，即：

其中R为3×3的矩阵；t为3×1的向量；M_b为4×4的矩阵，也被称为摄像机外部参数矩阵；O^T＝[0 0 0]；

将公式(2)和公式(5)代入公式(6)，就可以得到P点在世界坐标系下的坐标(X_W,Y_W,Z_W)与其在图像平面的投影点p的坐标(u,v)的关系：

其中

M＝M_aM_b为3×4矩阵，称为投影矩阵；M_a完全由a_x、a_y、u₀、v₀决定的摄像机内部结构有关，称为摄像机内部参数；M_b完全由摄像机相对于世界坐标系的方位决定，称为摄像机外部参数矩阵；P_w是对齐次世界坐标(X_w,Y_w,Z_w,1)的代替表达，即P_w＝(X_w,Y_w,Z_w,1)；

确定某一摄像机的内部和外部参数，就完成了被称为摄像机的标定；

(2)手眼关系的标定：

双目摄像头拍摄标定物的左右两组图像，将左右两组图像上的像素点通过空间转化得到相对于双目摄像头虚拟点的空间坐标；在双目摄像头下方空间选取不少于四个点作为标定点，将6自由度机械臂末端依次移动到标定点上，获得6自由度机械臂末端的空间坐标；建立双目摄像头虚拟点的空间坐标与6自由度机械臂末端的空间坐标之间的转换矩阵；

(3)眉毛图像的处理：

利用双目摄像头获取的图像，然后利用图像处理算法对图片进行处理，获取眉毛的像素坐标；

(4)三维数据的获取：

利用已标定的双目摄像头的双目视觉参数，经过视差原理恢复出眉毛在双目摄像头坐标系中的三维信息，再通过转换矩阵获得获取眉毛在6自由度机械臂坐标系中的三维信息；

(5)基于视觉的机械臂运动控制算法

计算机将眉毛在6自由度机械臂坐标系中的三维信息发送给6自由度机械臂，实现画眉操作。

2.根据权利要求1所述的一种基于机器人视觉的画眉机器人***的控制方法，其特征在于，所述手眼关系的标定具体如下：

选取四个点即A,B,C,D，双目摄像头拍摄图像，读取由左摄像头拍摄的图像中的四个点的像素坐标Al(x，y),Bl(x，y),Cl(x，y),Dl(x，y)和由右摄像头拍摄的图像中的四个点的像素坐标Ar(x，y),Br(x，y),Cr(x，y),Dr(x，y)，根据步骤(1)获得的双目摄像头的参数，利用深度测量将A,B,C,D四点转化成相对于左摄像头的空间坐标A(x，y，z),B(x，y，z),C(x，y，z),D(x，y，z)；使用探针法或者在程序中增加Tcp的长度，即眉笔的长度；并且读出机械臂末端A,B,C,D的坐标；通过矩阵转化后即可得出手眼的相应关系。

3.根据权利要求2所述的一种基于机器人视觉的画眉机器人***的控制方法，其特征在于，所述图像处理算法为Dlib和OpenCV算法。

4.根据权利要求3所述的一种基于机器人视觉的画眉机器人***的控制方法，其特征在于，所述基于视觉的机械臂运动控制算法具体如下：

采用D-H参数法建立6自由度机械臂的坐标系，其中αi表示相连两个部件之间的扭角，α₀代表初始扭角，其扭角为0度，α₁代表了第一手腕和第二手腕之间的扭角，其扭角度数为π/2度，α₂代表了第二手腕和第一连杆之间的扭角，其扭角度数为0度，α₃代表了第一连杆和手肘之间的扭角，其扭角度数为0度，α₄代表了手肘和第二连杆之间的扭角，其扭角度数为π/2度，α₅代表了第二连杆和肩膀之间的扭角，其扭角度数为-π/2度；li表示各部件长度，l₁代表了第一手腕的长度，l₂代表了第二手腕的长度，l₃代表了第一连杆的长度，l₄代表了手肘的长度，l₅代表了第二连杆的长度；di表示相连两个部件之间的相对位置，d₁代表了第一手腕和第二手腕之间的相对位置，d₂代表了第二手腕和第一连杆之间的相对位置，d₃代表了第一连杆和手肘之间的相对位置，d₄代表了手肘和第二连杆之间的相对位置，d₅代表了第二连杆和肩膀之间的相对位置，d₆代表了肩膀和基座之间的相对位置；θi表示相连两个部件之间的夹角，θ₁代表了第一手腕和第二手腕之间的夹角，其夹角度数为0度，θ₂代表了第二手腕和第一连杆之间的夹角，其夹角度数为-π/2度，θ₃代表了第一连杆和手肘之间的夹角，其夹角度数为0度，θ₄代表了手肘和第二连杆之间的夹角，其夹角度数为-π/2度，θ₅代表了第二连杆和肩膀之间的夹角，其夹角度数为0度，θ₆代表了肩膀和基座之间的夹角，其夹角度数为0度；其中六个关节的活动范围是正负360度；

由6自由度机械臂的结构参数，可得到各连杆参数以及变量工作范围；

根据两相连部件变换公式可得，变换矩阵：

为各连杆的变换矩阵，c(θ_i)为cosθ_i，s(θ_i)为sinθ_i，c(α_i-1)为cosα_i-1，c(α_i)为cosα_i，s(α_i)为sinα_i，s(α_i-1)为sinα_i-1；

得到各连杆的变换矩阵，将各连杆变换矩阵相乘，便可得到6自由度机械臂的变换矩阵

上式中，c_l代表cosθ₁，c₂代表cosθ₂，c₃代表cosθ₃，c₄代表cosθ₄，c₅代表cosθ₅，c₆代表cosθ₆,s₁代表sinθ₁,s₁代表sinθ₁,s₂代表sinθ₂,s₃代表sinθ₃,s₄代表sinθ₄,s₅代表sinθ₅，s₆代表sinθ₆；

其中：

n_x＝c₆(s₁s₅+c₅c₁c₂₃₄)-s₆c₁s₂₃₄

n_y＝c₆(c₅s₁c₂₃₄-c₁s₅)-s₆s₁s₂₃₄

n_z＝c₅c₆s₂₃₄+c₂₃₄s₆

o_x＝-s₆(c₅c₁c₂₃₄+s₁s₅)-c₆c₁s₂₃₄

o_y＝-s₆(c₅s₁c₂₃₄-c₁s₅)-c₆s₁s₂₃₄

o_z＝c₆s₂₃₄-c₅c₆s₂₃₄

a_x＝-s₅c₁c₂₃₄+c₅s₁

a_y＝-s₁s₅c₂₃₄-c₁c5

a_z＝-s₅s₂₃₄

p_x＝d₅c₁s₂₃₄+d₄s₁+d₆(c₅s₁-s₅c₁c₂₃₄)+l₂c₁c₂+l₃c₁c₂₃

p_y＝d₅s₁s₂₃₄-d₄c₁-d₆(c₅s₁+s₅s₁c₂₃₄)+l₂s₁c₂+l₃s₁c₂₃

p_z＝d₁-d₅c₂₃₄+l₂s₂+l₃s₂₃-d₆s₅s₂₃₄

上式中，c₁代表cosθ₁，c₅代表cosθ₅，c₆代表cosθ₆，c₂₃代表cos(θ₂+θ₃)，c₂₃₄代表cos(θ₂+θ₃+θ₄)，s₁代表sinθ₁，s₂代表sinθ₂，s₅代表sinθ₅，s₂₃代表sin(θ₂+θ₃)，s₂₃₄代表sin(θ₂+θ₃+θ₄)；

将6自由度机械臂初始角度代入式(8)结果第四列的前三行代表6自由度机械臂末端点的位置坐标。

Images