一种双臂解魔方机器人及其还原方法

1.本发明涉及机器人技术领域，尤其涉及一种双臂解魔方机器人及其还原方法。


背景技术：

2.魔方作为一种益智玩具，深受广大人民的喜爱。现有的魔方机器人由机械手臂、颜色传感器、摄像头、机座、回转气缸(气动式)等部件组成，利用计算机或者其他微处理器来自动还原魔方，融合了图像处理技术、计算机视觉、魔方还原算法、机械自动控制、模拟实现交互等技术，在控制系统和机械结构上存在不同。广泛应用于教学示例、博物馆展览等方面，其中所涵盖的技术也很大程度应用到基础工业和农业科技上面去。现有的魔方机器人虽然技术比较全面，但市场上有很多东西在技术处理上不是很完善，缺乏源代码和模板。其次，机械手臂结构刚度不够强，在机器运动过程中，会产生一些微量变形，质量偏重，使得自身运行速度受到影响，增加了对电机的负载，并且运行精度低会产生卡顿现象，机械手臂的运动速度加快，产生很大的惯性力，使得机器运行不平稳，旋转精度也会降低；还有就是单片机在运行过程中容易遇到故障或异常造成死机；针对现有的魔方机器人，控制系统耗能过大，制作机器的材料没有选好，导致机器体型大、成本高且工作性能不稳定。


技术实现要素：

3.本发明解决的技术问题是提供一种双臂解魔方机器人及其还原方法，能迅速稳定地识别魔方并在短时间内快速还原，控制机械手臂的运行以提高还原成功率和还原速度，优化结构设计减少体积。
4.为了达到上述发明目的，本发明采用的技术方案如下：
5.提供一种双臂解魔方机器人，包括控制箱，控制箱的上表面两端分别对称设置有一对用于夹持和旋转魔方的机械臂；
6.每个机械臂包括旋转装置和夹持装置；旋转装置包括步进电机，步进电机的输出轴上设置有传动轴；夹持装置包括气缸，气缸的上表面设置有两个用于夹持魔方的末端执行器，两个末端执行器同时相对或相离运动，每个末端执行器均呈“l”形结构，气缸的下表面设置有气缸联轴器，气缸通过气缸联轴器与传动轴的端部连接；
7.控制箱的上表面设置有支撑架，支撑架上设置有多个用于采集魔方6个面的图像的摄像头；
8.控制箱内设置有电机驱动系统、气压驱动系统和用于控制电机驱动系统和气压驱动系统的控制器，多个摄像头均与控制器电性连接。
9.进一步地，为了调整步进电机的安装位置，控制箱的上表面设置有水平滑轨，水平滑轨两端上分别活动设置有一个用于安装步进电机的角件，每个角件的上顶面与水平面之间的夹角为45
°
，两个步进电机通过步进电机支架与两个角件的上顶面固定连接。
10.进一步地，控制器包括相互电性连接的上位机和下位机，多个摄像头均与上位机电信连接；电机驱动系统和气压驱动系统均与下位机电性连接。
11.进一步地，支撑架包括门字形结构的第一支撑架和滑动设置在水平滑轨中部的第二支撑架，第一支撑架的两侧安装脚位于水平滑轨两侧，第一支撑架的左右两侧面内壁以及顶面内壁上均设有一个摄像头，第二支撑架的顶面有一个摄像头。
12.进一步地，电机驱动系统包括与下位机电性连接的步进电机驱动器，下位机通过步进电机驱动器用于控制步进电机的旋转角度及方向；
13.气压驱动系统包括设置于控制箱侧壁上的空压机气泵接头和与空压机气泵接头连通的电磁阀，电磁阀通过管道与气缸的进气口和出气口连通；电磁阀与下位机电性连接。
14.进一步地，传动轴外壁上套设有一个气滑环，气滑环的顶部端面与气缸联轴器的底部端面相接，电磁阀通过气滑环与气缸的出气口和进气口连通；气缸联轴器和步进电机之间设置有初始位置调零机构，初始位置调零机构包括调零码盘和与下位机电性连接的u性光电开关，调零码盘套设在传动轴外壁上，调零码盘上设置有一条细缝，u性光电开关通过固定支架设置在角件的上表面，调零码盘位于u性光电开关内。
15.本发明还提供一种双臂解魔方机器人的还原方法，其包括以下步骤：
16.步骤1，系统初始化，机械臂调零找正；
17.步骤2，将魔方放置在两个机械臂上，末端执行器夹紧魔方；
18.步骤3，多个摄像头采集魔方6个面的图像信息，并将图像信息传递给上位机；
19.步骤4，上位机处理魔方6个面的图像信息，并根据处理后魔方6个面的图像信息，完成还原魔方的解算指令；
20.步骤5，判断解算指令是否正确，如果解算指令正确，进入步骤6，否则进入步骤1；
21.步骤6，上位机将正确的解算指令传递给下位机；
22.步骤7，下位机根据正确的解算指令控制步进电机转动魔方，机械臂完成对魔方的还原。
23.上位机将魔方还原解算指令转换为字符串的形式发送给下位机，通过解算字符串得到所需旋转面、旋转角度和旋转方向，因为每次旋转魔方后都会实时更新各个面的位置，所以只需要搜索所需旋转的面的位置并计算该面旋转至末端执行器抓取面的最少步骤，通过比较步骤的步数大小自动选择由哪个机械臂进行旋转，以此类推，最终还原好魔方，张开爪子；初始化上位机继续还原下一个魔方，以此来实现一个能够高效、稳定还原一个任意打乱的三阶魔方的自动解魔方机器人。
24.进一步地，上位机处理魔方6个面的图像信息的方法包括以下步骤：
25.步骤4.1，对魔方6个面的图像信息进行方6个面的图像信息进行伽马非线性矫正降曝处理，以提高图像信息的亮度和色彩分块；
26.步骤4.2，采用canny算子对降曝处理后的图像进行边缘检测，提取出魔方六个面的所有色块，并提取出每个色块的中心色块；
27.步骤4.3，采用欧式距离算法对降每个色块的中心色块进行颜色识别视觉处理，通过对每个中心色块和其他非中心色块的相似度计算，把相似度最接近的9个色块划分到同一类中，循环6次后，完成分类。
28.进一步地，步骤4中欧式距离算法为：
29.30.式中，|c1‑
c2|表示两点的欧式距离，c
ix
(i＝1，2，x＝[r，g，b])表示每个像素点对应的值，c1c2表示颜色1和颜色2，c
1r
表示颜色1的r通道；
[0031]
两个颜色之间的色差为：
[0032]
δe
*
＝[(δl
*
)2+(δa
*
)2+(δb
*
)2]
1/2
[0033]
式中，δe表示色差，l表示明度，a表示红绿色差，b表示蓝黄色差，δl、δa、δb分别表示两个颜色之间在不同分量的差值；
[0034]
加权欧式距离算法为：
[0035][0036]
δr＝c
1r
‑
c
2r
[0037]
δg＝c
1g
‑
c
2g
[0038]
δb＝c
1b
‑
c
2b
[0039][0040]
δc为两个色块之间的综合色差，通过综合色差将相似度最接近的9个色块划分到同一类中，循环6次后，完成分类。
[0041]
本发明的有益效果为：解魔方机器人通过上位机控制程序，采用一个顶部摄像头进行颜色采集；对于光感，利用了一个伽马非线性矫正来做到降曝、提高亮度。将割取色块的系统进行拆分，最后模块与模块之间进行数据进行汇总和归类处理，解算出还原的步骤，打包后通过串口通讯发送到下位机，下位机根据接收的数据，通过u型光电传感器和码盘结构，确定魔方的初始位置，进而自动控制机械臂对一个任意打乱的魔方进行还原操作。以此来实现一个能够高效、稳定还原一个任意打乱的三阶魔方的自动解魔方机器人。
附图说明
[0042]
图1为一种双臂解魔方机器人的结构示意图。
[0043]
图2为机械臂的爆炸的结构示意图。
[0044]
图3为末端执行器的结构示意图。
[0045]
图4为机械臂的正视局部剖视结构示意图。
[0046]
图5为气缸的内部结构示意图。
[0047]
图6为控制箱的内部结构示意图。
[0048]
图7为初始位置调零机构的结构示意图。
[0049]
图8为魔方机器人对模仿的还原方法的流程图。
[0050]
图9为图像处理流程图。
[0051]
其中，1、控制箱；2、机械臂；3、步进电机；4、传动轴；5、气缸；6、末端执行器；7、气缸联轴器；8、支撑架；801、第一支撑架；802、第二支撑架；9、摄像头；10、水平滑轨；11、角件；12、步进电机支架；13、上位机；14、下位机；15、步进电机驱动器；16、空压机气泵接头；17、电磁阀；18、气滑环；19、调零码盘；20、u性光电开关；21、固定支架；22、联轴器；23、eva泡沫层。
具体实施方式
[0052]
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
[0053]
实施例1
[0054]
如图1～9所示，本发明提供了一种双臂解魔方机器人，包括控制箱1，控制箱1的上表面两端分别对称设置有一对用于夹持和旋转魔方的机械臂2；每个机械臂2包括旋转装置和夹持装置；魔方作是一个正立方体结构，在还原过程中需要拧动的面有六个，这六个面分别为上面、下面、左面、右面、前面和后面，在对魔方进行旋转时，需要先固定其中的一层，再对另外一层进行旋转；对称设置在机械臂2可以满足机器人还原魔方时的固定功能和旋转功能，可以满足机器人还原魔方时的固定功能和旋转功能。
[0055]
机械臂2具体的设置方案为：旋转装置包括步进电机3，步进电机3的输出轴上设置有传动轴4，传动轴4通过联轴器22和步进电机3连接；夹持装置包括气缸5，气缸5的上表面设置有两个用于夹持魔方的末端执行器6，两个末端执行器6同时相对或相离运动，每个末端执行器6均呈“l”形结构，气缸5的下表面设置有气缸联轴器7，气缸5通过气缸联轴器7与传动轴4的端部连接；动力系统分为气动和电动两部分。目的是为了能稳定的还原魔方，提高机器旋转精度，同时也避免了步进电机3的丢步问题，而且可以使机械臂2360度任意旋转。
[0056]
末端执行器6的材料为pla材料，由3d打印工艺制作，末端执行器6的夹持面上均贴有eva泡沫层23，其作用是增大与魔方的摩擦，使末端执行器6能够稳稳地夹住魔方。
[0057]
控制箱1的上表面设置有水平滑轨10，水平滑轨10两端上分别活动设置有一个用于安装步进电机3的角件11，每个角件11的上顶面与水平面之间的夹角为45
°
，两个步进电机3通过步进电机支架12与两个角件11的上顶面固定连接。角件11可以在水平滑轨10上滑动，可以实现调整步进电机3的安装位置，提高机械臂2的使用范围。
[0058]
控制箱1的上表面设置有支撑架，支撑架上设置有多个用于采集魔方6个面的图像的摄像头9；多个摄像头9位置具体的设置方案为：支撑架包括门字形结构的第一支撑架801和滑动设置在水平滑轨10中部的第二支撑架802，第一支撑架801的两侧安装脚位于水平滑轨10两侧，第一支撑架801的左右两侧面内壁以及顶面内壁上均活动设有一个摄像头9，第二支撑架802的顶面有一个摄像头9。摄像头9可以在第一支撑架801上调整位置，使用4个摄像头9一次性获得魔方6个面的图像，并将图像信息传递给上位机13。摄像头9的型号可以为迈德威视mv
‑
ged500c
‑
t型ccd彩色工业相机。
[0059]
控制箱1内设置有电机驱动系统、气压驱动系统和用于控制电机驱动系统和气压驱动系统的控制器，多个摄像头9均与控制器电性连接。
[0060]
控制器包括相互电性连接的上位机13和下位机14，上位机13为带有显示屏的pc上位机13，显示屏设置在控制箱1的侧壁，上位机13用于负责调用色块处理信息，并与下位机14进行异步通讯，与实时反馈信息相结合，得出还原程序，控制魔方还原；多个摄像头9均与上位机13电信连接；电机驱动系统和气压驱动系统均与下位机14电性连接，下位机14采用stm32f407zet6单片机，用于负责与上位机13通讯以及控制电磁阀17和步进电机3。
[0061]
上位机13计算将两个色块的rgb加权欧式距离，作为色块于色块之间的相似度。通过对每个中心色块和其他非中心色块的相似度计算，把相似度最接近的9个色块划分到同一类中，循环6次后，即可完成分类。该算法分类速度快、抗干扰能力强。即使环境发生变化，也不会造成影响。
[0062]
具体地，电机驱动系统包括与下位机14电性连接的步进电机驱动器15，下位机14通过步进电机驱动器15用于控制步进电机3的旋转角度；
[0063]
气压驱动系统包括设置于控制箱1侧壁上的空压机气泵接头16和与空压机气泵接头16连通的电磁阀17，空压机气泵接头16与外部空压机高压气源进行连通，为气缸5带动末端执行器6移动提供动力源。
[0064]
电磁阀17通过管道与气缸5的进气口和出气口连通；电磁阀17与下位机14电性连接；下位机14通过电磁阀17控制气缸5的进气和出气，气缸5实现对两个末端执行器6同时相对或相离运动，进而达到夹持和释放魔方的目的，下位机14通过步进电机驱动器15控制步进电机3的旋转角度，实现对魔方的一层进行旋转。
[0065]
传动轴4外壁上套设有一个气滑环18，气滑环18的顶部端面与气缸联轴器7的底部端面相接，电磁阀17通过气滑环18与气缸5的出气口和进气口连通；气滑环18的设置，可以使机械臂2360度任意旋转，摆脱气管的束缚。
[0066]
气缸联轴器7和步进电机3之间设置有初始位置调零机构，初始位置调零机构包括调零码盘19和与下位机14电性连接的u性光电开关20，调零码盘19套设在传动轴4外壁上，调零码盘19上设置有一条细缝，细缝的宽度为1mm，u性光电开关20通过固定支架21设置在角件11的上表面，调零码盘19位于u性光电开关20内。调零码盘19与末端执行器6固定在一起，随着电机带动末端执行器6一起旋转；u型光电开关固定在解魔方机器人支架上，没有移动自由度，在调零码盘19上开有一条细缝当调零码盘19旋转到细缝处时，u型光电开关的红外线光路联通，末端执行器6旋转到指定位置，下位机14控制步进电机3停止。每次开始复原魔方时，需要末端执行器6回到初始位置，初始位置调零机构完成上述动作，无论是当执行完一次完整的流程的正常停止还是在运行过程中由于魔方本身的结构问题导致异常停止，都可以将机械手臂进行一个初始化，将机械手臂回归原始位置。达到了闭环的目的，减少了人员的工作量，提高了效率。
[0067]
实施例2
[0068]
如图1～9所示，本实施例在实施例1的基础上做出进一步限定，作为步进电机3和气缸5的选型，标准三阶魔方的外形大小为：56
×
56
×
56mm；
[0069]
气缸5选型为mhf2
‑
16d1r，其夹持力大小为(0.5＝ma)＝90n，单侧行程为32mm，总程为64mm，大于魔方尺寸56mm，选择合理。
[0070]
为了能够稳定牢固的夹紧魔方，手指夹魔方时产生的摩擦力必须大于三阶魔方自身的重力mg，再考虑到其他因素的影响，例如旋转时的加速度和冲击力等，需要设定一个安全系数α。
[0071]
nμf＞αmg
[0072]
f＞αmg/nμ＝βmg
[0073]
β＝α/nμ
[0074]
式中μ
‑
摩擦系数，一般令μ＝0.1～0.2；α
‑
安全系数，一般令α＝4；对于2个手指，取
β：10～20；魔方质量m＝0.09kg。由式得：
[0075]
f＞βmg＝20
×
0.09
×
9.8＝17.64n
[0076]
平行气缸5上两个末端执行器6之间的距离为l＝59mm，气泵的气源压强p＝0.4mpa，此时气夹持力为24n，大于魔方的必要夹持力17.64n，故平行气缸5选用合理。
[0077]
在转动魔方的过程中，由电机带动mhf2
‑
16d1r平行气爪和单层魔方转动，故将mhf2
‑
16d1r平行气爪和单层魔方看成理想实体长方体，且质心过转动轴，分别计算其转动惯量；mhf2
‑
16d1r平行气爪参数：长：95mm、宽：50mm、高：25mm、质量：445g；单层魔方参数：长：56mm、宽：56mm、高：18mm、质量：30g。
[0078]
根据理想实体长方体转动惯量公式：
[0079][0080]
式中，w为实体长方体的宽；d为实体长方体的宽；m为实体长方体的质量。
[0081]
mhf2
‑
16d1r平行气爪的转动惯量为：
[0082][0083]
魔方的转动惯量为：
[0084][0085]
总转动惯量为：
[0086]
i
h总
＝i
h气爪
+i
h魔方
＝9.60
×
10
‑4+5.23
×
10
‑4＝14.83
×
10
‑4kg*m2[0087]
故选用步进电机3的转动惯量需要大于总转动惯量，故选用步进电机3的型号为60闭环步进伺服电机。
[0088]
实施例3
[0089]
如图1～9所示，本实施例在实施例1和实施例2的基础上做出进一步限定，本方案还提供一种双臂解魔方机器人的还原方法，其包括以下步骤：
[0090]
步骤1，系统初始化，步进电机3位置调零找正；
[0091]
步骤2，将魔方放置在两个机械臂2上，末端执行器6夹紧魔方；
[0092]
步骤3，多个摄像头9采集魔方6个面的图像信息，并将图像信息传递给上位机13；
[0093]
步骤4，上位机13处理魔方6个面的图像信息，并根据处理后魔方6个面的图像信息，完成还原魔方的解算指令；
[0094]
步骤4中，上位机13处理魔方6个面的图像信息的方法包括以下步骤：
[0095]
步骤4.1，对魔方6个面的图像信息进行方6个面的图像信息进行伽马非线性矫正降曝处理，以提高图像信息的亮度和色彩分块；
[0096]
步骤4.2，采用欧式距离算法对降曝处理后的图像信息进行颜色识别视觉处理，对魔方上面的六个颜色以及魔方每个面上的色块进行识别；
[0097]
步骤4.3，通过对每个中心色块和其他非中心色块的相似度计算，把相似度最接近
的9个色块划分到同一类中，循环6次后，完成分类。
[0098]
具体地，对于颜色识别来说，一张图片上的每一个点都有一个属于自己的像素值，而相近的颜色他们的像素值差别就不大；反之，色差较大的像素点他们的像素值差就大。我们将这种差值以另一种方式呈现，从而引入了另一种概念——颜色距离。简单的来说，颜色距离就是一个可以描述两种颜色是否相近的物理量。距离越大，色差越大，颜色越好区分。当我们上位机13完成魔方色块的割取后就利用了这种方法，将每个色块进行一个归类。总分六大类，分别代表魔方的六个面。具体实现的算法就是比较色块之间的欧式距离。在rgb空间内，可以得到两个颜色之间的距离为：
[0099][0100]
式中，|c1‑
c2|表示两点的欧式距离，c
ix
(i＝1，2，x＝[r，g，b])表示每个像素点对应的值，c1c2表示颜色1和颜色2，c
1r
表示颜色1的r通道。
[0101]
但是，由于rgb空间是线性的并且相互正交，而人眼的视觉系统并不是线性的，rgb空间并不能反映人眼对颜色的感知，相对应的颜色距离也不能很好的反映两个颜色是否相近。针对这个问题，于是有了lab颜色空间，及其对应的色差计算。
[0102]
两个颜色之间的色差为：
[0103]
δe
*
＝[(δl
*
)2+(δa
*
)2+(δb
*
)2]
1/2
[0104]
式中，δe表示色差，l表示明度，a表示红绿色差，b表示蓝黄色差，δl、δa、δb分别表示两个颜色之间在不同分量的差值；
[0105]
为了简化计算及保证计算效果，在rgb空间上通过公式计算出加权的欧式距离。加权欧式距离算法为：
[0106][0107]
δr＝c
1r
‑
c
2r
[0108]
δg＝c
1g
‑
c
2g
[0109]
δb＝c
1b
‑
c
2b
[0110][0111]
δc为两个色块之间的综合色差，魔方机器人引入了基于rgb加权欧氏距离的色块分类算法。该算法将两个色块的rgb加权欧式距离，作为色块于色块之间的相似度，通过对每个中心色块和其他非中心色块的相似度计算，把相似度最接近的9个色块划分到同一类中，循环6次后，即可完成分类。对魔方上面的六个颜色精准识别，避免识别颜色数量遗漏或者超出预定颜色数量的故障出现，并进行有效的颜色类匹配，有效避免了颜色识别产生错误，基本实现颜色识别及匹配零错误。该算法分类速度快、抗干扰能力强。即使环境发生变化，也不会造成影响。
[0112]
步骤5，判断解算指令是否正确，如果解算指令正确，进入步骤6，否则进入步骤1；
[0113]
步骤6，上位机13将正确的解算指令传递给下位机14；
[0114]
步骤7，下位机14根据正确的解算指令控制步进电机3转动魔方，机械臂2完成对魔方的还原。
[0115]
解算指令还原动作算法人手部对模仿的还原动作，类人型的两只机械臂2，必然会要有一只手固定，另一只手动作，可以避免在旋转不同面时，两只手出现太多的冗余切换动作。
[0116]
综上，解魔方机器人通过上位机13控制程序，采用一个顶部摄像头9进行颜色采集；对于光感，利用了一个伽马非线性矫正来做到降曝、提高亮度。将割取色块的系统进行拆分，最后模块与模块之间进行数据进行汇总和归类处理，解算出还原的步骤，打包后通过串口通讯发送到下位机14，下位机14根据接收的数据，通过u型光电传感器和码盘结构，确定魔方的初始位置，进而自动控制机械臂2对一个任意打乱的魔方进行还原操作。以此来实现一个能够高效、稳定还原一个任意打乱的三阶魔方的自动解魔方机器人。