一种基于aruco码的电力二次设备机械臂定位方法
技术领域
1.本发明涉及一种基于aruco码的电力二次设备机械臂定位方法，属于机械臂控制技术领域。
背景技术：
2.电力系统中设备巡检是及时准确发现设备安全隐患、保证电网设备正常工作、变电站安全稳定运行的一项十分重要的常规性工作，特别是变电站这种设备数量和种类较多的环境，一旦有隐患没有及时排除，轻则无法正常运行，重则影响周边大面积区域的正常生产生活，甚至造成人身伤害。当前人工和机器人巡检作为电网的两种常见的巡检方式，随着国家电网系统的不断完善，电力巡检系统的自动化水平越来越高，人工巡检方式已经不能满足当下及未来电网智能发展需求，也正是在这样的背景下，智能巡检机器人也应运而生。智能巡检机器人集成仿生机器人技术、多传感器融合技术、智能控制技术、导航定位技术以及物联网技术等，能够实现电网设备全天候、全方位、全自主智能巡检和监控，大幅度降低电网工人劳动强度，降低电网系统运维成本。然而当前研发巡检机器人主要功能仅仅停留在对电力设备的远程测量和监控上，不具备直接操作电力设备的能力，与期待的智能化水平还相差甚远。随着自动化、智能化水平的发展，将机器臂与机器视觉图像识别技术相结合，实现不同规程特征下机器人自主作业是未来巡检机器人的重点发展方向。传统的机械臂自主作业方法一般是采用拖动示教的方式，将机器人手臂拖动到预先设置好的指定位置，机器人完成既定轨迹存储后再进行重复操作。由于缺乏视觉定位信息，对外部环境、目标特征状态和抓取任务依赖性很强，一旦发生了变化，就需要重新配置，灵活性差。基于机器视觉的机械臂自主作业主要是利用图像检测得到目标在机械臂基坐标系下的三维坐标，近几年来随着深度学习视觉技术的迅速发展，将深度学习技术与机器人操作结合起来具有较广的应用，但是该目标检测方法易受光照变化、遮挡等条件的影响，在实际机器臂自主作业过程中存在识别定位率低、鲁棒性差、运行时间长等问题。
技术实现要素：
3.本发明目的是提供一种基于aruco码的电力二次设备机械臂定位方法，在较低的使用成本情况下可以最大限度减少外部环境因素对视觉定位效果的影响，定位准确度高，可以实现机械臂对电力设备柜门把手目标的实时识别与抓取，能够满足当前电力巡检机器人自主作业的需求，解决了背景技术中存在的问题。
4.本发明的技术方案是：
5.一种基于aruco码的电力二次设备机械臂定位方法，包含如下步骤：将深度相机安装于机械臂末端，与机械臂的夹爪保持平行，通过识别aruco码不断调整相机位姿，保证相机坐标系z轴与目标坐标系z轴平行，根据手眼标定获得目标在机械臂基坐标系下的三维坐标，通过逆运动学求解得到rml63人形协作机械臂六个关节角，最终通过ros机器人操作系统控制机械臂运动完成目标抓取任务。
6.具体步骤如下：
7.①
通过图像识别技术或相机来对aruco码进行姿态估计，建立aruco码图像坐标系和相机坐标系的坐标转换关系；
8.②
利用基于对偶四元数的手眼标定模型，测算出rml63人形协作机械臂坐标系与realsense d455深度相机坐标系间的相对位姿关系，从而确定待抓取物体在机器人坐标系下的三维空间坐标值，然后通过机器人运动学方程，采用rrt-connect轨迹规划算法进行规划，确定行程轨迹实现机械臂自主作业；
9.机械臂手眼关系矩阵x以及机械臂基坐标系与世界坐标系方位关系y可以表示为矩阵方程
10.ax＝yb
ꢀꢀ
(1)
11.其中，a表示世界坐标系到不同姿态的摄像机坐标系的位姿变换矩阵，b表示为不同姿态的机械臂工具坐标系到机械臂基坐标系的位姿变换矩阵，令ra，r
x
，ry，rb分别表示a、x、y、b 4
×
4的位姿变换矩阵中的旋转部分，ta，t
x
，ty，tb分别表示a、x、y、b 4
×
4变换矩阵中的平移部分，则齐次矩阵方程(1)可以表示为：
[0012][0013]
将上式解耦展开成旋转和平移分开的形式为：
[0014]
rar
x
＝ryrbꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0015]
rat
x
ta＝rytb tyꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0016]
将上式用对偶四元数参数化法表示，用向量a、b分别表示两个连续有限运动的四元数，则由四元数矩阵乘法交换性可得，此时可以运用lm最小二乘方法求解对应的旋转矩阵和平移向量，确定机器人坐标系与世界坐标系之间的变换矩阵y，从而确定aruco码在机械臂坐标系的三维空间坐标值；
[0017][0018]
其中，w(a
′
，b
′
)表示由对偶四元数的对偶部分a
′
，b
′
所组成的4
×
8的矩阵，q(x，y)可以由奇异值分解解出，q
′
(x，y)就是3
×
3平移向量t
x
，ty所对应的对偶四元数中的对偶部分，经n(n≥3)次测量得到v(ai，bi)和w(ai′
，bi′
)，建立4n
×
8阶矩阵l
′
＝[w(a1′
，b1′
)，w(a2′
，b2′
)，
…
，w(an′
，bn′
)]和l，将上式转化为齐次方程组：
[0019]
lq′
(x，y) l
′
q(x，y)＝0 (6)
[0020]
再利用对偶四元数的单位行和正交性限制条件，方程组(6)可以表示为带有约束条件的最优化问题：
[0021][0022]
其中，||
·
||表示矩阵f-范数，利用拉格朗日乘法和非线性优化算法，式(7)中多元函数最优化问题可以很好收敛到极值；待抓取目标与aruco码同处在一个平面，以aruco码的中心点作为世界坐标系，待抓取目标的基点作为物体坐标系，将待抓取目标坐标系的轴线与世界坐标系的轴线依次平行，同时对目标物体的旋转矩阵进行约束，再通过(7)式确定aruco码所在世界坐标系与机械臂基坐标系的坐标变换关系，最终可通过世界坐标系与
目标坐标系之间平移矩阵求出待抓取目标在机械臂下的位姿信息。
[0023]
所述aruco码的数量为四个，四个aruco码组成的码盘围绕在电力设备柜门把手，以左上角的码为第一检测码，出现遮挡则顺时针对其他进行检测，识别出aruco码后，会从视野范围寻找电力设备柜门把手目标进行抓取。
[0024]
本发明的有益效果是：设备成本低，无需昂贵三维定位设备，仅仅使用普通的相机和廉价的二维码贴即可实现电力二次设备目标识别与精确定位；运行效率高，无需复杂双目立体视觉算法即能够实现对电力设备柜门把手小目标的抓取，aruco二维码图像显著性好，易于编码，容易检测，可以满足嵌入式开发板实时运算需求；算法鲁棒性好，在较低的使用成本情况下可以最大限度减少外部环境因素对视觉定位效果的影响，增强电力巡检机器人自主作业的可靠性。
附图说明
[0025]
图1为本发明自主抓取系统总体组成与工作原理；
[0026]
图2为本发明aruco码的内部id信息图；
[0027]
图3为本发明aruco码的黑色边框图；
[0028]
图4为本发明aruco码图；
[0029]
图5为本发明手眼关系模型图；
[0030]
图6为本发明机械臂抓取实验平台示意图；
[0031]
图7为本发明rml63机械臂抓取电力柜门把手过程示意图。
具体实施方式
[0032]
以下结合附图，通过实施例对本发明做进一步的说明。
[0033]
一种基于aruco码的电力二次设备机械臂定位方法，包含如下步骤：将深度相机安装于机械臂末端，与机械臂的夹爪保持平行，通过识别aruco码不断调整相机位姿，保证相机坐标系z轴与目标坐标系z轴平行，根据手眼标定获得目标在机械臂基坐标系下的三维坐标，通过逆运动学求解得到rml63人形协作机械臂六个关节角，最终通过ros机器人操作系统控制机械臂运动完成目标抓取任务。
[0034]
具体步骤如下：
[0035]
①
通过图像识别技术或相机来对aruco码进行姿态估计，建立aruco码图像坐标系和相机坐标系的坐标转换关系；考虑到基于模型的姿态估计方法主要是通过对比真实图像和模型图像，进行相似度计算目标姿态，非常依赖于局部特征的准确检测，当噪声较大无法提取准确的局部特征的时候，会影响姿态估计方法的稳定性。而基于学习的姿态估计方法一般通过图像的全局特征进行匹配分析，但需要预先定义好姿态类别，通过模式分类的方法训练姿态分类器以实现姿态识别，因此该方法鲁棒性很大程度依赖于训练样本的充分程度。因此本发明利用容易检测且具有较高的鲁棒性aruco码(参照附图2、3、4)作为视觉检测的协助来完成电力设备柜门把手等小目标位置和位姿确定；
[0036]
②
利用基于对偶四元数的手眼标定模型，准确地测算出tml63人形协作机械臂坐标系与realsense d455深度相机坐标系间的相对位姿关系，从而确定待抓取物体在机器人坐标系下的三维空间坐标值，然后通过机器人运动学方程(denavit-hartenberg参数)，采
用rrt-connect轨迹规划算法进行规划，确定行程轨迹实现机械臂自主作业；
[0037]
参照附图5，机械臂手眼关系矩阵x以及机械臂基坐标系与世界坐标系方位关系y可以表示为矩阵方程
[0038]
ax＝yb
ꢀꢀ
(1)
[0039]
其中，a表示世界坐标系到不同姿态的摄像机坐标系的位姿变换矩阵，b表示为不同姿态的机械臂工具坐标系到机械臂基坐标系的位姿变换矩阵，令ra，r
x
，ry，rb分别表示a、x、y、b4
×
4的位姿变换矩阵中的旋转部分，ta，t
x
，ty，tb分别表示a、x、y、b 4
×
4变换矩阵中的平移部分，则齐次矩阵方程(1)可以表示为：
[0040][0041]
将上式解耦展开成旋转和平移分开的形式为：
[0042]
rar
x
＝ryrbꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0043]
rat
x
ta＝rytb tyꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0044]
将上式用对偶四元数参数化法表示，用向量a、b分别表示两个连续有限运动的四元数，则由四元数矩阵乘法交换性可得，此时可以运用lm最小二乘方法求解对应的旋转矩阵和平移向量，确定机器人坐标系与世界坐标系之间的变换矩阵y，从而确定aruco码在机械臂坐标系的三维空间坐标值；
[0045][0046]
其中，w(a
′
，b
′
)表示由对偶四元数中的对偶部分a
′
，b
′
所组成的4
×
8的矩阵，q(x，y)可以由奇异值分解解出，q
′
(x，y)就是3
×
3平移向量t
x
，ty所对应的对偶四元数中的对偶部分，经n(n≥3)次测量得到v(ai，bi)和w(ai′
，bi′
)，建立4n
×
8阶矩阵l
′
＝[w(a1′
，b1′
)，w(a2′
，b2′
)，
…
，w(an′
，bn′
)]和l，将上式转化为齐次方程组：
[0047]
lq′
(x，y) l
′
q(x，y)＝0
ꢀꢀ
(6)
[0048]
再利用对偶四元数的单位行和正交性限制条件，方程组(6)可以表示为带有约束条件的最优化问题：
[0049][0050]
其中，||
·
||表示矩阵f-范数，利用拉格朗日乘法和非线性优化算法，式(7)中多元函数最优化问题可以很好收敛到极值；考虑到待抓取目标与aruco码同处在一个平面，以aruco码的中心点作为世界坐标系，待抓取目标的基点作为物体坐标系，将待抓取目标坐标系的轴线与世界坐标系的轴线依次平行，同时对目标物体的旋转矩阵进行约束，再通过(7)式确定aruco码所在世界坐标系与机械臂基坐标系的坐标变换关系，最终可通过世界坐标系与目标坐标系之间平移矩阵求出待抓取目标在机械臂下的位姿信息。
[0051]
考虑到单个aruco码在数据采集时易被遮挡，鲁棒性不高，本发明aruco码的数量为四个，四个aruco码组成的码盘围绕在电力设备柜门把手，以左上角的码为第一检测码，出现遮挡则顺时针对其他进行检测，识别出aruco码后，会从视野范围寻找电力设备柜门把手目标进行抓取。
[0052]
参照附图6，利用睿尔曼rml63人形协作机械臂、英特尔intel realsense d455深
度相机及aruco码，搭建机械臂抓取实验系统，对电力设备柜门把手进行定位抓取实验，其中，人形协作机械臂的重复定位精度为
±
0.05mm，工作半径638.5mm，相机分辨率为1280
×
720pixel，视场角为87
°×
58
°×
95
°
。实验前先将相机固定在机械臂末端执行器法兰盘上。上位机配置为nvidta jetson tx2嵌入式开发板，384-core nvidia volta
tm gpu with 48tensor cores，8gb 128位lpddr4内存，运行系统linux版本号为ubuntu18.04，控制器放置在机械臂底盘内部。抓取过程如图7所示，实验过程数据如表1所示，
[0053]
表1实验结果表
[0054][0055]
抓取时是一次性抓取，机械臂抓到门把手打开记为循环抓取一次，共循环抓取10次，记录抓取成功率。表1中平均识别时间为1.98s，该时间包括从图片中检测到aruco码并预测其相对于机械臂位姿的时间。平均导航时间为15.92s，该时间包含机械臂运动到电力设备门把手拍照点的时间，拍照点即机械臂根据aruco码检测位姿运动到一个合适的位姿，以便让门把手成像在机械臂末端相机视野中心。平均定位时间为1.18s，该时间指的是计算电力设备门把手相对于机械臂位姿的时间，方便后续机械臂完成后续抓取开门操作。在nvidia jetson tx2嵌入式处理器下，计算抓取目标所需要的识别、导航、定位、抓取、回程等平均时间，从开始检测到aruco码到开电柜门结束最后机械臂回归初始化位置总时间为48.3s，可以满足电力巡检机器人自主作业的需求。
[0056]
本发明首先分析aruco码图像识别技术，得到aruco码在摄像机坐标系中的位置，然后结合姿态估计与机器人手眼标定算法求解待抓取目标在机器人基坐标的空间坐标，最后通过ros机器人操作系统控制机械臂运动完成目标抓取任务。本发明设备成本低，无需昂贵三维定位设备(如激光雷达，结构光或双目相机)仅仅使用普通的相机和廉价的二维码贴即可以实现电力二次设备目标识别与精确定位；运行效率高，无需复杂双目立体视觉算法即能够实现对电力设备柜门把手小目标的抓取，aruco二维码图像显著性好，其由一个宽的黑色边框和一个内部二进制矩阵组成，易于编码，容易检测，可以满足嵌入式开发板实时运算需求；算法鲁棒性好，无论是传统视觉还是深度学习视觉目标检测方法都易受光照变化、遮挡等条件的影响，灵活性差，而基于aruco码的电力二次设备目标定位方法在较低的使用成本情况下可以最大限度减少外部环境因素对视觉定位效果的影响，增强电力巡检机器人自主作业的可靠性。