一种基于视觉的机器人机械臂末端位姿的调整方法

1.本发明属于机器人机械臂控制技术领域，具体的，涉及一种基于视觉的机器人机械臂末端位姿的调整方法。


背景技术：

2.机器人的刚需时代正在来临，机器人技术正在深刻改变着人类的生产和生活方式。随着其技术的不断发展和成熟，机器人产业发展逐渐从制造业向生活领域转移，然而现实生活中的环境复杂多变，要使机器人能够在多种场合下服务于人们的生活和工作，尚需要解决很多问题。
3.变电站是对电能的电压和电流进行变换、集中和分配的场所，受变电站户内设备体型小、运动空间狭窄等因素限制，站内设备操作大多依靠人工进行，操作人员不仅费时费力，而且面临较大的劳动风险。为了实现变电所无人化管理，许多变电所采用移动机械臂代替操作人员操作变电所内的设备，如对电气柜上的按钮开关下达指令，但在实际应用过程中，机器人很难精准识别和定位目标按钮，常常因为机器人无法正确完成操作过程而引起更大的安全问题，因此，为了解决机器人精确识别和定位目标按钮开关，精准按下按钮开关完成操作过程的问题，本发明提出一种基于视觉的机械臂末端位姿调整的方法，利用可靠的取像装置开发有效的数学模型，以达到通过机器人技术代替人工对设备进行精准操作的目的。
4.中国发明专利文献(cn108453727b)于2018年8月28日公开的《基于椭圆特征的机械臂末端误差校正方法及系统》采用一种基于椭圆特征的机械臂末端误差校正方法，该方法虽然做到了校正机械臂末端因局部柔性或关节间隙引起的控制误差，进而完成机械臂末端位姿校正，但受机械臂构型约束且校正过程较于复杂，适用范围较为局限，难以适应不同机械臂末端的位姿校正。
5.在机器人作业中，机械臂末端位姿的准确性，直接影响着机器人是否能精确完成作业及操作。其中，对于机器人操作电气柜按钮开关技术的研究还存在以下技术问题：
6.1、移动小车到达目标电气柜操作位置，定位不精确，存在定位误差，小车上面机械臂的初始位姿及按钮开关拍照位姿和电气柜操作面板因此产生一定的倾斜，使得机械臂无法精确地操作目标按钮开关。
7.2、受电气柜的限制，电气柜操作面板可能与地面不垂直，存在一定的倾斜，机械臂到达按钮开关拍照位姿时，机械臂末端轴垂面和电气柜操作面板并不平行，因而在随后的相机拍照识别和按钮开关定位过程中，机械臂操作总是存在偏差，无法更为精确的完成按下目标按钮开关的过程。


技术实现要素：

8.针对现有技术的不足，本发明提供了一种基于视觉的机械臂末端位姿调整的方法，能够使机器人精确识别定位电气柜按钮开关，进而精准按下按钮开关。
9.本发明的目的是这样实现的，本发明提供了一种基于视觉的机器人机械臂末端位姿的调整方法，本调整方法涉及的系统包括机器人、电气柜和校正板；所述电气柜的操作面板上装有按钮开关，将按钮开关的圆心记为点a；所述校正板粘贴在电气柜操作面板上，该校正板上设有三个黑色的圆，分别记为圆1、圆2和圆3，将圆1、圆2和圆3的圆心分别记为点c1、点c2和点c3，校正板粘贴完成后，点c1和点c2连成的直线为水平线，c3位于点c1和点c2的下方，且点c3与点c1连成的直线与点c1和点c2连成的直线垂直；
10.所述机器人包括agv小车、6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述6 个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，6个旋转关节依次分别绕第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴旋转，其中第四关节轴和第五关节轴相互垂直，第五关节轴和第六关节轴相互垂直；所述末端执行器和深度相机均安装在6自由度机械臂的末端上，且深度相机的光轴与第六关节轴平行，深度相机跟随6自由度机械臂运动；
11.所述调整方法包括以下步骤：
12.步骤1，定义图像坐标系、像素坐标系和相机坐标系，并使用相机标定算法对深度相机进行相机标定；
13.所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系，包括互相垂直的图像x轴和图像y轴；所述像素坐标系以图像左上角为原点建立的以像素为单位的直接坐标系，包括互相垂直的像素u轴和像素v轴，像素u轴上的横坐标为像素在其图像中的列数，像素v轴上的纵坐标为像素在其图像中的行数，且像素u轴与图像x 轴平行、像素v轴与图像y轴平行；所述相机坐标系是以相机光心为原点建立的坐标系，包括相机xc轴、相机yc轴和相机zc轴，其中，相机zc轴为相机光轴，相机xc轴与像素坐标系中的像素u轴平行，相机yc轴与像素坐标系中的像素v轴平行；
14.使用相机标定算法对深度相机进行相机标定，标定出的结果为深度的内参矩阵m，其表达式为：
[0015][0016]
式中，
[0017]fx
为图像坐标系中图像x轴上的归一化焦距，f
x
＝f/d
x
，其中， f为深度相机的焦距，单位为mm，d
x
为像素坐标系中的每个像素在图像x轴上的物理尺寸，单位为mm/pixel；
[0018]fy
为图像坐标系中图像y轴上的归一化焦距，fy＝f/dy，dy为像素坐标系下每个像素在图像y轴上的物理尺寸，单位为mm/pixel；
[0019]
u0为图像坐标系的原点在像素坐标系中的横坐标，v0为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为(u0，v0)；
[0020]
步骤2，机器人移动到目标电气柜操作面板板前，驱动6自由度机械臂使深度相机到达校正板拍摄位置进行拍照，然后对拍摄得到的图像做图像处理，得到点c1、点c2和点c3的像素坐标和深度值，分别为：点c1的像素坐标(u1，v1)和深度值d1，点c2的像素坐标(u2，v2)和深度值d2，点c3的像素坐标(u3，v3)和深度值d3；
[0021]
步骤3，根据步骤1标定出的内参矩阵m和圆心坐标变换公式一分别计算出点c1在相机坐标系下的相机坐标(x
c1
，y
c1
，z
c1
)、点c2在相机坐标系下的相机坐标(x
c2
，y
c2
，z
c2
)，点c3在相机坐标系下的相机坐标(x
c3
，y
c3
，z
c3
)，所述圆心坐标变换公式一的表达式为：
[0022][0023]
其中，z是圆心在相机坐标系下测出来的深度值，即圆心到深度相机平面的距离，u为圆心在像素坐标系下的横坐标，v为圆心在像素坐标系下的纵坐标，(xc，yc，zc)为圆心在相机坐标系下的坐标；
[0024]
步骤4，根据点c1、点c2和点c3的坐标关系，求深度相机在xc、yc、zc三个方向的倾斜角，所述求解过程如下：
[0025]
步骤4.1，过点c1做一条平行于相机xc轴的直线，过点c2做该直线的垂线，并将其交点记为交点q1，连接点c1、点c2、交点q1构成一个直角三角形，∠q1c1c2即为深度相机在xc轴方向的倾斜角，其表达式为：
[0026][0027]
步骤4.2，沿着深度相机光轴的方向，将点c3投影到过点c1且平行于相机xc-yc轴的平面上，记为点q2，连接点c3、点c1、点q2构成一个直角三角形，∠q2c1c3即为深度相机在yc轴方向的倾斜角，其表达式为：
[0028][0029]
步骤4.3，沿着深度相机光轴的方向，将点c1投影到过点c2且平行于相机xc-yc轴的平面上，记为点q3，连接点c1、点c2、点q3构成一个直角三角形c1q3c2，∠q3c2c1即为深度相机在zc轴方向的倾斜角，其表达式为：
[0030][0031]
步骤5，进行6自由度机械臂末端位姿的调整，具体的，将旋转关节四旋转∠q2c1c3，将旋转关节五旋转∠q3c2c1，将旋转关节六旋转∠q1c1c2；
[0032]
步骤6，按照步骤2的方法再次获取点c1、点c2、点c3的像素坐标和深度值，并分别记为调整后的点c1的像素坐标(u
inv1n
)和深度值 d1n、调整后的点c2的像素坐标(u
2n
，v
2n
)和深度值d2n和调整后的点c3的像素坐标(u
3n
，v
3n
)和深度值d3n，其中，n为调整次数，n≥1；
[0033]
给定预设值δ和η，定义第一差值δ1、第二差值δ2、第三差值δ3、第四差值η1和第五差值η2，δ1＝|d
1n-d
2n
|，δ2＝|d
1n-d
3n
|，δ 3＝|d
2n-d
3n
|，η1＝|v
1n-v
2n
|，η2＝|u
1n-u
3n
|，并进行如下判断：
[0034]
若满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，则6自由度机械臂末端位姿的调整结束，进入步骤7；
[0035]
若不满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，返回步骤2，进行下一轮调整；
[0036]
步骤7，在6自由度机械臂末端完成位姿调整后，驱动6自由度机械臂使深度相机到
达目标按钮开关拍照位姿进行拍照，然后对拍摄得到的图像做图像处理，得到点a的像素坐标(ua，va)和深度值da；
[0037]
步骤8，首先利用步骤3中的圆心坐标变换公式一，将步骤7得到的点a的像素坐标(ua，va)转换成点a在相机坐标系下的相机坐标 (x
ca
、y
ca
、z
ca
)；然后利用圆心坐标变换公式二将点a的相机坐标 (x
ca
、y
ca
、z
ca
)转换成机械臂基座坐标系下的机械臂基座坐标 (x
ba
、y
ba
、z
ba
)，所述圆心坐标变换公式二的表达式为：
[0038][0039]
其中，是机械臂手眼标定出的相机外参矩阵，是由机械臂系统得出的齐次变换矩阵；
[0040]
步骤9，驱动6自由度机械臂使其末端执行器到达(x
ba
、y
ba
、z
ba
)位姿，随后末端执行器按下按钮开关，完成操作；
[0041]
步骤10，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令。
[0042]
优选地，步骤2所述图像处理包括图像灰度化和霍夫圆变换，步骤7所述图像处理包括格式转换和霍夫圆变换；
[0043]
所述图像灰度化指的是利用图像处理工具将原图灰度化，使黑白区分更加明显；所述格式转换指的是利用图像处理工具将rgb格式的图像转化为hsv格式的图像；所述霍夫圆变换是用于检测出图像中存在的圆，并获得其圆心像素坐标。
[0044]
优选地，所述机械臂基座标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座xb轴、机械臂基座yb轴和机械臂基座zb轴，其中，机械臂基座zb轴和第一关节轴平行，机械臂基座xb轴以agv 小车正前方为正方向，机械臂基座yb轴由坐标系右手法则确定。
[0045]
优选地，所述相机外参矩阵用于描述机械臂末端坐标系相对于相机坐标系的位姿变换，其表达式为：
[0046][0047]
式中，是一个3
×
3的旋转矩阵，用于描述机械臂末端坐标系相对于相机坐标系的坐标旋转，是一个3
×
1的平移向量，用于描述机械臂末端坐标系相对于相机坐标系的坐标平移；
[0048]
所述齐次变换矩阵用于描述机械臂末端坐标系相对于机械臂基座坐标系的位姿变换，其表达式为：
[0049][0050]
式中，是一个3
×
3的旋转矩阵，用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标旋转，是一个3
×
1的平移向量，用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标平移；
[0051]
所述机械臂末端坐标系为以关节六中心为原点建立的坐标系，包括机械臂末端xe轴、机械臂末端ye轴和机械臂末端ze轴，其中，机械臂末端ze轴为末端旋转轴，竖直向下方向为机械臂末端xe轴正方向，机械臂末端ye轴由坐标系右手法则确定；所述机械臂基座标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座xb轴、机械臂基座yb轴和机械臂基座zb轴，其中，机械臂基座zb轴和关节一的旋转轴平行，机械臂基座xb轴以agv小车正前方为正方向，机械臂基座yb轴由坐标系右手法则确定。
[0052]
与现有技术相比，本发明的有益效果如下；
[0053]
1、本发明采用基于视觉的机器人机械臂末端位姿的调整方法，用于机器人机械臂操作电气柜上的按钮开关，解决了由于agv小车定位误差和电气柜做工限制带给机器人机械臂末端位姿的影响，从而使机器人机械臂更为精确地操作电气柜按钮开关。
[0054]
2、本发明采用基于视觉的机器人机械臂末端位姿的调整方法，校正过程简单快速，易于实施，满足机器人机械臂操作电气柜按钮开关的需求。
附图说明
[0055]
图1为本发明的系统结构图；
[0056]
图2为本发明中电气柜操作面板正视图；
[0057]
图3为本发明调整方法的流程图；
[0058]
图4为本发明中深度相机在xc轴方向的倾斜角∠q1c1c2的示意图；
[0059]
图5为本发明中深度相机在yc轴方向的倾斜角∠q2c1c3的示意图；
[0060]
图6为本发明中深度相机在zc轴方向的倾斜角∠q3c2c1的示意图；
[0061]
图7为本发明机器人机械臂末端校正后拍取的校正板图；
[0062]
图8为本发明调整方法的流程简图。
具体实施方式
[0063]
下面结合附图对本发明实施例作进一步的描述。
[0064]
图1为本发明的系统结构图，图2为本发明中电气柜操作面板正视图。由图1、图2可见，本调整方法涉及的系统包括机器人、电器柜和校正板。
[0065]
所述电气柜的操作面板上装有按钮开关，将按钮开关的圆心记为点a。所述校正板粘贴在电气柜操作面板上，该校正板上设有三个黑色的圆，分别记为圆1、圆2和圆3，将圆1、圆2和圆3的圆心分别记为点c1、点c2和点c3，校正板粘贴完成后，点c1和点c2连成的直线为水平线，c3位于点c1和点c2的下方，且点c3与点c1连成的直线与点c1和点c2连成的直线垂直。
[0066]
所述机器人包括agv小车、6自由度机械臂、末端执行器和深度相机，所述6自由度机械臂包括6个旋转关节和机械臂基座，所述6 个旋转关节由机械臂基座开始依次为旋转关节一、旋转关节二、旋转关节三、旋转关节四、旋转关节五、旋转关节六，6个旋转关节依次分别绕第一关节轴、第二关节轴、第三关节轴、第四关节轴、第五关节轴、第六关节轴旋转，其中第四关节轴和第五关节轴相互垂直，第五关节轴和第六关节轴相互垂直；所述末端执行器和深度相机均安装在6自由度机械臂的末端上，且深度相机的光轴与第六关节轴平行，深度相机跟随6自由度机械臂运动；
[0067]
在本调整方法中，末端执行器用于按下按钮开关。
[0068]
图3为本发明调整方法的流程图，图8为本发明调整方法的流程简图。由图3可见，本发明的调整方法包括以下步骤：
[0069]
步骤1，定义图像坐标系、像素坐标系和相机坐标系，并使用相机标定算法对深度相机进行相机标定。
[0070]
所述图像坐标系以相机光轴与图像平面的交点为原点建立的坐标系，包括互相垂直的图像x轴和图像y轴；所述像素坐标系以图像左上角为原点建立的以像素为单位的直接坐标系，包括互相垂直的像素u轴和像素v轴，像素u轴上的横坐标为像素在其图像中的列数，像素v轴上的纵坐标为像素在其图像中的行数，且像素u轴与图像x 轴平行、像素v轴与图像y轴平行；所述相机坐标系是以相机光心为原点建立的坐标系，包括相机xc轴、相机yc轴和相机zc轴，其中，相机zc轴为相机光轴，相机xc轴与像素坐标系中的像素u轴平行，相机yc轴与像素坐标系中的像素v轴平行。
[0071]
使用相机标定算法对深度相机进行相机标定，标定出的结果为深度的内参矩阵m，其表达式为：
[0072][0073]
式中，
[0074]fx
为图像坐标系中图像x轴上的归一化焦距，f
x
＝f/d
x
，其中，f为深度相机的焦距，单位为mm，d
x
为像素坐标系中的每个像素在图像x轴上的物理尺寸，单位为mm/pixel；
[0075]fy
为图像坐标系中图像y轴上的归一化焦距，fy＝f/dy，dy为像素坐标系下每个像素在图像y轴上的物理尺寸，单位为mm/pixel；
[0076]
u0为图像坐标系的原点在像素坐标系中的横坐标，v0为图像坐标系的原点在像素坐标系中的纵坐标，即图像坐标系的原点在像素坐标系中的坐标为(u0，v0)。
[0077]
步骤2，机器人移动到目标电气柜操作面板板前，驱动6自由度机械臂使深度相机到达校正板拍摄位置进行拍照，然后对拍摄得到的图像做图像处理，得到点c1、点c2和点c3的像素坐标和深度值，分别为：点c1的像素坐标(u1，v1)和深度值d1，点c2的像素坐标(u2，v2)和深度值d2，点c3的像素坐标(u3，v3)和深度值d3。
[0078]
步骤3，根据步骤1标定出的内参矩阵m和圆心坐标变换公式一分别计算出点c1在相机坐标系下的相机坐标(x
c1
，y
c1
，z
c1
)、点c2在相机坐标系下的相机坐标(x
c2
，y
c2
，z
c2
)，点c3在相机坐标系下的相机坐标(x
c3
，y
c3
，z
c3
)，所述圆心坐标变换公式一的表达式为：
[0079][0080]
其中，z是圆心在相机坐标系下测出来的深度值，即圆心到深度相机平面的距离，u为圆心在像素坐标系下的横坐标，v为圆心在像素坐标系下的纵坐标，(xc，yc，zc)为圆心在相机坐标系下的坐标。
[0081]
步骤4，根据点c1、点c2和点c3的坐标关系，求深度相机在xc、yc、zc三个方向的倾斜角，所述求解过程如下：
[0082]
步骤4.1，过点c1做一条平行于相机xc轴的直线，过点c2做该直线的垂线，并将其交点记为交点q1，连接点c1、点c2、交点q1构成一个直角三角形，∠q1c1c2即为深度相机在xc轴方向的倾斜角，其表达式为：
[0083][0084]
步骤4.2，沿着深度相机光轴的方向，将点c3投影到过点c1且平行于相机xc-yc轴的平面上，记为点q2，连接点c3、点c1、点q2构成一个直角三角形，∠q2c1c3即为深度相机在yc轴方向的倾斜角，其表达式为：
[0085][0086]
步骤4.3，沿着深度相机光轴的方向，将点c1投影到过点c2且平行于相机xc-yc轴的平面上，记为点q3，连接点c1、点c2、点q3构成一个直角三角形c1q3c2，∠q3c2c1即为深度相机在zc轴方向的倾斜角，其表达式为：
[0087][0088]
步骤5，进行6自由度机械臂末端位姿的调整，具体的，将旋转关节四旋转∠q2c1c3，将旋转关节五旋转∠q3c2c1，将旋转关节六旋转∠q1c1c2；
[0089]
图4、图5和图6给出了三个倾斜角∠q1c1c2、∠q2c1c3和∠q3c2c1的示意图。图7为本发明机器人机械臂末端校正后拍取的校正板。
[0090]
步骤6，按照步骤2的方法再次获取点c1、点c2、点c3的像素坐标和深度值，并分别记为调整后的点c1的像素坐标(u
1nv1n
)和深度值dln、调整后的点c2的像素坐标(u
2nv2n
)和深度值d2n和调整后的点c3的像素坐标(u
3n
，v
3n
)和深度值d3n，其中，n为调整次数，n≥1；
[0091]
给定预设值δ和η，定义第一差值δ1、第二差值δ2、第三差值δ3、第四差值η1和第五差值η2，δ1＝|d
1n-d
2n
|，δ2＝|d
1n-d
3n
|，δ 3＝|d
2n-d
3n
|，η1＝|v
1n-v
2n
|，η2＝|u
1n-u
3n
|，并进行如下判断：
[0092]
若满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，则6自由度机械臂末端位姿的调整结束，进入步骤7；
[0093]
若不满足δ1＜δ且δ2＜δ且δ3＜δ且η1＜η且η2＜η，返回步骤2，进行下一轮调整。
[0094]
在本实施例中，预设值δ＝1mm，预设值η＝2pixel。
[0095]
步骤7，在6自由度机械臂末端完成位姿调整后，驱动6自由度机械臂使深度相机到
达目标按钮开关拍照位姿进行拍照，然后对拍摄得到的图像做图像处理，得到点a的像素坐标(ua，va)和深度值da；
[0096]
步骤8，首先利用步骤3中的圆心坐标变换公式一，将步骤7得到的点a的像素坐标(ua，va)转换成点a在相机坐标系下的相机坐标 (x
ca
、y
ca
、z
ca
)；然后利用圆心坐标变换公式二将点a的相机坐标 (x
ca
、y
ca
、z
ca
)转换成机械臂基座坐标系下的机械臂基座坐标 (x
ba
、y
ba
、z
ba
)，所述圆心坐标变换公式二的表达式为：
[0097][0098]
其中，是机械臂手眼标定出的相机外参矩阵，是由机械臂系统得出的齐次变换矩阵；
[0099]
所述机械臂基座标系以机械臂基座中心点为原点建立的坐标系，包括机械臂基座xb轴、机械臂基座yb轴和机械臂基座zb轴，其中，机械臂基座zb轴和关节一的旋转轴平行，机械臂基座xb轴以agv小车正前方为正方向，机械臂基座yb轴由坐标系右手法则确定。
[0100]
步骤9，驱动6自由度机械臂使其末端执行器到达(x
ba
、y
ba
、z
ba
)位姿，随后末端执行器按下按钮开关，完成操作；
[0101]
步骤10，6自由度机械臂恢复到不操作时的位姿，等待下次操作的指令。
[0102]
在本实施例中，步骤2所述图像处理包括图像灰度化和霍夫圆变换，步骤7所述图像处理包括格式转换和霍夫圆变换。
[0103]
所述图像灰度化指的是利用图像处理工具将原图灰度化，使黑白区分更加明显；所述格式转换指的是利用图像处理工具将rgb格式的图像转化为hsv格式的图像；所述霍夫圆变换是用于检测出图像中存在的圆，并获得其圆心像素坐标。
[0104]
在本实施例中，所述相机外参矩阵用于描述机械臂末端坐标系相对于相机坐标系的位姿变换，其表达式为：
[0105][0106]
式中，是一个3
×
3的旋转矩阵，用于描述机械臂末端坐标系相对于相机坐标系的坐标旋转，是一个3
×
1的平移向量，用于描述机械臂末端坐标系相对于相机坐标系的坐标平移。
[0107]
所述齐次变换矩阵用于描述机械臂末端坐标系相对于机械臂基座坐标系的位姿变换，其表达式为：
[0108][0109]
式中，是一个3
×
3的旋转矩阵，用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标旋转，是一个3
×
1的平移向量，用于描述机械臂末端坐标系相对于机械臂基座坐标系的坐标平移。
[0110]
所述机械臂末端坐标系为以关节六中心为原点建立的坐标系，包括机械臂末端xe轴、机械臂末端ye轴和机械臂末端ze轴，其中，机械臂末端ze轴为末端旋转轴，竖直向下方向为机械臂末端xe轴正方向，机械臂末端ye轴由坐标系右手法则确定。