作品全称：基于探索者Rob-GS01、OpenCV Python、Arduino的双臂二指魔方机器人

      魔方机器人是机器人智能化的一种新体现，魔方机器人的机械结构设计以及还原算法的分析编译是该机器人的核心内容。

      本文的双臂二指魔方机器人是基于探索者Rob-GS01、Open CV 来设计与制作，由本体机械结构、控制系统、外围数据采集装备（四方位摄像头、支架）组成，整体结构如图 2-1 所示。它涵盖了魔方颜色识别，还原步骤算法解析计算，机械手夹取系统以及机械臂旋转系统。主体机械结构由2只机械臂组成，通过“M”形框架搭建固定在支撑架上，在2组舵机和步进电动机的分别驱动下，实现机械臂的转动以及夹爪的夹取运动，从而实现魔方面的转动。

主体机械结构由探索者 Rob-GS01 箱子中的铝制零件拼接。其他配件包括：3D 打印的二指机械手*2、42 步进电机*2、舵机*2、摄像头*4、杜邦线、补光灯、胶带（若干）。

控制系统的上位机（电脑），用于连接摄像头进行视觉识别、图像处理、魔方还原方案的制定，确定魔方还原方案后，通过 USB 串口通讯将还原魔方的指令发送到下位机（单片机控制板）。下位机收到指令后即驱动机械手完成复原魔方的操作。

下位机所用的单片机控制板由探索者 Rob-GS01 箱子中的 Basra 控制板、Big fish 扩展板、SH-ST扩展板三块单板拼接而成，其中 Basra 控制板用于搭载程序，同时串口通讯接收电脑的指令，Big fish 扩展板用于连接舵机和传感器，SH-ST 扩展板连接锂电池供电，驱动步进电机使 机械手旋转。整机的操作流程如图2-2 所示。

图2-2机器人运作流程图

      目前魔方机器人的机械结构主要有类人双臂式和四轴旋转式2种类型，相比较之下，类人双臂式的魔方放机器人结构更为简单，控制系统协调度更高，成本更低。该魔方机器人复原过程需要2只机械手同时工作，相互配合运动来完成相应复原转动的动作。为使机械手能够互相独立工作，做回转运动时互不干扰，机械手需满足手臂的中心线在同一平面且垂直，故机械臂采用双臂交叉正交结构，分别在魔方的两侧斜45°向上，从两侧支撑魔方。两机械手通过“M”形框架装夹固定在与水平桌面形成45°夹角的平面上，完成机械手臂以及夹爪的相应动作。

1.“M”形框架设计

      框架决定了机械手配合的平稳性和协调性，因此在设计的过程中应用三角形的稳定性，决定将机械手对位斜 45°放置。安置机械手、步进电机、舵机的框架呈“M”形。这样的框架既能合理分担承重，又不会因机械手动作的振动而产生位移，相对稳定。

2.机械手结构设计

      对于抓取魔方的机械手设计，传统的魔方还原机器人多选用五指结构，但其结构复杂，稳定性差，本文设计的魔方还原机器人采用二指结构，力量调节更方便，结构更简单，自身转动惯量较小，质量分布较均匀，更适合一边转动一边抓取。

      如下图为本文设计的二指机械手。由于两个指尖的形状和材质会严重影响魔方还原速度，我们对机械手指采用ug软件进行了多版设计和 3D 打印，并确定了最终方案，机械手指内侧设还计了防滑纹，从两侧支撑魔方，从而减小因魔方自重在转动过程中产生的误差，保证机械手在夹取和松开时，魔方的准确位置。

3.器件的选型

      根据设计要求，步进电机经联轴器连接机械手使其随输出轴顺、逆时针180°旋转；舵机顺、逆时针180°旋转带动一字摆臂使机械手手指松开或夹紧；具体的器件选型如下：

3.1步进电机的选型

      步进电机相对于其它控制用途电机的最大区别是，它接收数字控制信号（电脉冲信号）并转化成与之相对应的角位移或直线位移，它本身就是一个完成数字模式转化的执行元件。机器人采用步进电机链接联轴器驱动机械手旋转。步进电机参数的选择主要考虑该机构框架尺寸以及步距角的大小。

图3-4步进电机主视图

3.2 舵机的选型

3.4整体框架的零件

     本文设计的魔方机器人框架搭建所需的零件主要采用探索者Rob-01提供的零件，然后根据所设计的结构进行组装搭建。

图3-4零件明细表

图3-5框架效果图

3.5单片机控制板

     单片机控制板、扩展板如图所示：

图3-8 SH-ST扩展板

1.方案选择

根据硬件实际需求以及本团队能力需求，视觉颜色的识别是本项目的不可缺少的一部分，方案的选择主要有一下三种类型：

(1)OpenMV

      简单的来说，它是一个可编程的摄像头，通过MicroPython语言，可以实现需求的逻辑，而且摄像头本身内置了一些图像处理算法，很容易使用。

（2）K210

      K210是由一家叫做嘉楠公司在去年推出的一款MCU，其特色在于芯片架构中包含了一个自研的神经网络硬件加速器KPU，可以高性能地进行卷积神经网络运算。在AI计算方面，K210的算力其实是相当可观的。根据嘉楠官方的描述，K210的KPU算力有0.8TOPS ，作为对比，拥有128个CUDA单元GPU的英伟达Jetson Nano的算力是0.47TFLOPS ；而最新的树莓派4只有不到0.1TFLOPS 。

      当然了，这个性能跟某些旗舰级别的SoC还是有差距的：A76级别的CPU本身就已经很变态，更何况旗舰SoC上面都会搭载用于AI加速的硬件用于异构运算，比如高通的Hexagon DSP、苹果的Neural Engine、华为的达芬奇架构NPU等等，这些NPU在某些应用下甚至能达到与数百W功耗的桌面级GPU接近的算力。

图4.1.2  K210硬件图

（3）OpenCV

      OpenCV是一个用于图像处理、分析、机器视觉方面的开源函数库.

      该库采用C及C++语言编写，可以在windows, linux, mac OSX系统上面运行。该库的所有代码都经过优化，计算效率很高，因为，它更专注于设计成为一种用于实时系统的开源库。opencv采用C语言进行优化，而且，在多核机器上面，其运行速度会更快。它的一个目标是提供友好的机器视觉接口函数，从而使得复杂的机器视觉产品可以加速面世。该库包含了横跨工业产品检测、医学图像处理、安防、用户界面、摄像头标定、三维成像、机器视觉等领域的超过500个接口函数。

      同时，由于计算机视觉与机器学习密不可分，该库也包含了比较常用的一些机器学习算法。或许，很多人知道，图像识别、机器视觉在安防领域有所应用。但，很少有人知道，在航拍图片、街道图片（例如google street view）中，要严重依赖于机器视觉的摄像头标定、图像融合等技术。

      根据现实硬件要求，openmv以及K210虽然继承了mirco python识别颜色的函数，但无法根据实际光照强度调整参数，并且无法外加更多的算法加强颜色识别，例如下图为我团队的魔方识别区域。综合考虑，本团队采用opencv进行颜色识别，并且使用主流的KMeans聚类算法，对黑色进行删除后，得出占比最大的颜色即为识别颜色。

2.实现流程

    实现流程可分为图片区域获取、颜色阈值设置以及颜色阈值匹配三个部分。

2.1 图像区域获取

      在获取图像时，我团队使用的是四摄像头四进程进行同步采样，能够更快的完成图像取样的动作，并且避免死机的现象，四个进程同时开启后，对摄像头捕获的图像进行保存，在后面的颜色识别处理中只需读取对应路径图片即可进行颜色识别。

图4.2.3  线程代码

2.2 图像区域分割

     对图像进行获取后，上下两个摄像头因为同时拍摄了魔方两个面的图片，需要进行图片切割以及旋转，该部分先使用旋转。

旋转效果

图4.2.5  区域旋转前、后效果图

旋转后进行透视变换，对周围干扰环境进行割除

透视变换效果

图4.2.6  区域透视变换前、后效果图

     这样便可以获得比较晚上的九色块图片，后续进行九个色块的聚类算法便可以得到对应的颜色。

2.3 颜色阈值设置

     在颜色设置中，为了方便调颜色阈值，我团队单独写了调阈值的程序，代码及效果如下：

     在颜色空间选取中，我们团队选择了颜色阈值更加分离的RGB色彩空间，抛弃了还原人眼看见的HSV色彩空间，更容易进行颜色的识别。

图4.2.7  区域颜色阈值设置代码图

图4.2.8  区域颜色阈值设置效果图

     在效果图中，最左边为输出的颜色阈值范围，左二为原图，左三为颜色阈值范围筛选后绿色的掩模图，最右边为设置颜色阈值的滑动条。

2.4 颜色阈值匹配

     根据上图获取到每个色块的颜色阈值后，即可设置颜色识别阈值的范围，下图为6个色块的RGB颜色阈值区域。

图4.2.9  区域颜色阈值范围代码图

2.5 KMeans聚类算法

      本章节的核心为KMeans聚类算法，是识别出正确颜色的重要部分，相较于上面直接颜色阈值区域匹配，进行KMeans聚类算法后能够删除黑色区域，从而减少机械爪子对色块遮挡的干扰。

图4.2.10  KMeans聚类算法代码图

图4.2.11  KMeans聚类算法代码图

KMeans聚类算法的效果如下：

图4.2.12  KMeans聚类算法效果图

      左一为各颜色区域的阈值范围以及占比，中间为输入图片，右一为聚类分割后区分的三种颜色。可以明显看出对黑色区域筛选后，再根据占比最高，从而获得正确的红色阈值范围。

2.6 编号输出

     编号输出的方法包括六个面的序号排序，排序正确后再进行每个面九个色块的顺序编号，便于应对各种魔方摆放顺序的情况。

图4.2.12 编号输出代码图

图4.2.13 编号输出代码图

      魔方还原算法最早可以追溯到1980年捷克共和国的女大学生Jessica Fridrich发明的CFOP算法，目前这个算法仍然广泛应用于国际魔方速拧比赛中。而经过几十年的发展，魔方还原的主流算法有:层先法、角先法、TM算法和二阶段算法。关于算法的优劣与否，主要看它的两个方面：魔方还原步数、魔方寻优时间。本文算法追求的是在尽可能短的时间内完成魔方的还原。双臂二指魔方机器人还原魔方的时间包括寻优的时间和机器人拧魔方的时间，而双臂二指魔方机器人完成一次魔方旋转所用的时间是固定的，因此机器人还原魔方的时间与还原指令的长度呈线性关系，即还原指令长度越短，魔方还原所花费的时间越少。根据相关资料显示，Kociemba 二阶段算法是最优的选择。本章主要介绍Kociemba 二阶段算法。

      魔方的状态分析是实现魔方成功求解的前提和基础。如果把魔方的12个棱块放于12个边的位置，根据排列顺序有12!种情况，除此之外，还要考虑棱块的方向问题，有2种方向置于12个边上，根据乘法原理可知有        种情况，综上所述棱块的状态数一共有：

(5-1)

同理，魔方有8个角块，每个角块朝向有3种，所以角块的状态数一共是：

(5-2)

      然而，魔方的一些状态是不存在的。因为对于一个还原的魔方来说，不存在单独改变一个棱块或角块的朝向的情况以及单独一对角块和一对棱块互换位置的情况[37]，所以应该在魔方所有状态的基础上去掉一个倍数：

(5-3)

此可以得到魔方的所有可能状态数：

(5-4)

由此得到:

      魔方的所有可能状态有43252003274489856000种，说明魔方还原的难度非常之大，想要通过胡乱的旋转侥幸将魔方还原是几乎不可能的。因此各种各样的魔方还原算法接踵而来，目前人们已经将“上帝之数”压缩到20，即任何一种组合的魔方都能在20步以内还原。

经上位机（电脑）视觉识别处理完成之后，就得到一串包含魔方相对位置与颜色的字符串，这串字符串就代表了这个魔方此时的状态信息，通过这串字符串，计算机也就可以准确的得到此时的魔方模型状态。目前来说，魔方机器人使用较为广泛的算法是来自 Kociemba 的二阶段算法，以下简称 Kociemba 算法。Kociemba 算法是一种能在较短时间内使用较短步数还原魔方的算法，通常能在 1s 内得到平均 20 步的解法，相比较于人工还原魔方的方法，如层先法、角先法，进阶的如 CFOP 法的动辄上百步还原步骤，是非常具有优势的， 所以我们也采用的此算法。

      我们可以把魔方简单的看做一个立方体，对每个面进行相应的标记，六个面分我们可以把魔方简单的看做一个立方体，对每个面进行相应的标记分别为 U（顶面）、F（前面）、 L（左面）、R（右面）、B（后面）、D（下面），对于每一个面，又分别有顺时针旋转 90°，逆时针旋转 90°， 旋转 180°（顺时针逆时针均可）这三种操作，我们可以把对应的操作记为 XY，其中 X 为面的代号，Y 为对 应操作代号（1 为顺时针旋转 90°，2 为旋转 180°，3 逆时针旋转 90°），如 U3 就代表着是顶层逆时针旋转 90°。经 Kociemba 算法得到的解法就是多个上述操作代号相连起来，例如“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是经 Kociemba 算法得出来的完整的一个解魔方步骤。

      我们可以把魔方简单的看做一个立方体，对每个面进行相应的标记，六个面分别为U(顶面)、F（前面）、L（左面）、R（右面）、B（后面）、D（下面），对于每一个面，又分别有顺时针旋转90°，逆时针旋转90°，旋转180°（顺时针逆时针均可）这三种操作，我们可以把对应的操作记为XY，其中X为面的代号，Y为对应操作代号（1为顺时针旋转90°，2为旋转180°，3逆时针旋转90°），如U3就代表着是顶层逆时针旋转90°。

经Kociemba算法得到的解法就是多个上述操作代号相连起来，例如“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是经Kociemba算法得出来的完整的一个解魔方步骤。对于我们的机器人而言，这样的一个步骤是不能直接运用的，对于双臂二指机器人而言，如图，它能直接转动的面只有D、B两个面，要想转动其他的面，还需要进行一些其他的机械臂的操作转化，接下来会在下位机算法中详细介绍。

 下位机进行串口缓存检测，一旦有数据输入时，就按顺序读取单个字符值，并转化为字符，循环赋值给a数组。这样就可以得到上位机传输过来的数据了。

      但这样得到的字符串并不能被下位机直接识别，将得到的字符串以空格为界，将两个空格之间的字符的ASCLL码相加，使得魔方的每一个转动方式都有一个对应的十进制数字与之相对应。

7. 机械臂控制及转化算法

      我们的双臂二指机器人包含两个舵机，分别控制左右两只手手指的夹紧与放开，另外还包含两个步进电机，用来分别控制左右两个机械臂的转动，我们可以做如下定义：

      L0左手手指张开

      L1左手手指夹紧

      Ls左手顺时针旋转90°

      Ln左手逆时针旋转90°

      L180左手旋转180°（至于是顺时针还是逆时针之后会有另行说明）

舵机控制代码：

      步进电机在转动的时候，根据手指的夹紧和张开状态，有三种速度可以选择，在不同负载下选择不同的速度来进行转动，其中lim值的作用在下文防缠绕算法中会详细说明：

R0、R1、Rs、Rn、R180则是右手相应动作。

      这样，我们就可以把魔方还原的解法通过一定的映射，转换为机器人相应的机械步骤，从而让机器人可以正确还原魔方。定义机械臂手指平行地面为归位，垂直地面为不归位，在进行一次转动操作以后，一共会出现三种状态，分别是1：左手归位、右手不归位2：左手不归位、右手归位3：左右手都归位。如在左右手臂都是初始状态时执行U3步骤：R1→L180→R0→Rn，对于其他步骤，同样存在相应的转化方法，下附表1。

      经过这样的操作之后，只是把待转动面移动到了机械爪的操作范围之内，至于是顺时针还是逆时针，用左手还是右手，经过程序的进一步判断以后就可以进行下一步操作。

8. 坐标转换算法

     当然，在双手机械臂还原魔方的过程中，会涉及到一定的坐标变换，如执行U3步骤之后，后续的所有还原步骤中：U→D、D→U、L→R、R→L，F和B面不变，不难发现，只要是对U面进行操作，都是这样的坐标转换步骤。当然，对于其他另外的5个面，在进行操作以后也会有一定的转换规则，下附表2。

在经过这样的一系列算法处理以后，魔方机器人就可以驱动电机，实现还原魔方的操作了。

通过 10 次的平稳运行测试，还原速度在 18s~25s 区间内，平均还原速度为 22.9 s。