乐高机器人还原魔方

去年儿子过生日时收到了一套乐高42099，放那儿很久也没拆。放寒假了没啥事情干，指导他把越野车拼好了，通过官方App控制，还挺好玩儿的。

在搭建越野车的过程中，我注意到了套装里一共有3个马达，一个蓝牙Hub。我就想起好基友前同事给我发过一个他用EV3套装搭建魔方机器人还原魔方的视频，我也动心了。是买一套EV3照着别人的图纸来拼，还是利用手里的这套自己做呢？并不是一个很纠结的问题，因为我太喜欢自己动手做一些没有的东西了。

一、首先要考虑的是技术可行性。有几个问题需要解决：

1. 要想还原魔方，机器人首先得能识别魔方，也就是必须知道魔方6个面上9个方块分别是什么颜色。EV3那套有一个颜色传感器，可以去读取魔方的每一个色块。越野车没有，但可以从某宝买到。

2. 主控程序，EV3有一个主机，颜色传感器和马达都通过线缆连接在主机上，还可以把python代码灌进去（别人写好了现成的）。通过读取颜色传感器的值获取魔方某块的颜色，计算出还原步骤，再通过马达旋转来还原魔方，不需要外部力量就可以完成。然鹅，我在拼搭和操控越野车的时候就发现，42099这套的蓝牙Hub是不带主机的，不能灌代码进去，也似乎接不了颜色传感器。越野车的官方App给了我灵感，既然官方App可以通过蓝牙连接Hub来控制马达，那我自己也写一个App，也应该能控制马达，App还能利用手机摄像头来拍照，识别颜色，顺便解决了没有颜色传感器的问题。所以我打算采用自己写一个App来充当主控。

这就是蓝牙Hub，拼越野车时贴上了A、B、C

3. 接下来就是做几个技术验证：

1) 自己写App通过蓝牙连接Hub。打开Hub的开关，用手机搜索蓝牙设备，发现根本就搜不到，和想象的不一样呀。不过一番努力之下，我谷歌到了乐高蓝牙hub的协议，花了两天时间仔细研读，基本弄清楚了怎么连接，怎么发指令（A口马达以什么速度和力量旋转多少度之类的）。简单写了几行代码，连上hub，命令马达旋转90度。这个技术问题就算解决了。

用自己的代码控制马达旋转，这个时候越野车还没拆，只是拆了一个齿轮，和车轮之间的传动分开了

2) 颜色识别。用App拍照不难，难点在于定位魔方每一块的位置。我计划是会搭建一个手机支架，把手机放在上面拍魔方，那魔方在图片中的位置基本就是固定的，最多因为手机型号的区别而有稍许不同，根据手机型号来判断一下即可。可这种做法显然是太lo了，我也没有所有的苹果手机型号。于是我就想用opencv之类的来识别图片上的矩形，后来想起曾经用iOS自带的SDK来检测人脸，找来文档一看，果然也支持识别矩形。我试了一下，识别率还不错。但识别出来的矩形有时候会把魔方削掉三分之一，这个很好处理，因为一共要拍六张照片，六张照片中魔方的位置是一样的，只要把六张照片里识别到的六个矩形拿出来一起计算分析一下，把异常值替换为去除异常值的平均值即可。得到魔方的位置矩形后，再分成9块，每块取中间四分之一大小的一张图片，缩小成一个点，就得到了平均颜色。再和六个中间块平均颜色的RGB差值平方相加，取最小的平方和，就知道每一块是和哪个中心块颜色最接近了。

3) 魔方还原。通常我们玩魔方时，入门还原方法是逐层还原，平均有115步（我后来也实现了用机器人来逐层还原，尝试了好几万次随机打乱20步后求解还原步骤，得到的平均值就是115步，不是理论计算得到的），再转换成机器人的步骤平均有370步。所以需要一个快速还原的算法，好在有人已经做好了，最多20步（上帝之数是20，这也是别人暴力算出来的，没有理论证明）。因为这是别人的劳动成果，所以我的App里，快速还原步骤没有收费。

二、搭建

1. 两轴魔方机器人。手拧魔方时，六个面都可以旋转。但我这里只有3个马达，想实现魔方6个面都能旋转，似乎只有一种办法，那就是用一个马达控制魔方篮子的旋转，再用一个或两个马达控制一个卡钳状的部件。当用卡钳卡住魔方旋转篮子时，就能旋转魔方的底层（D、D'或D2）。当卡钳放开魔方，旋转篮子时，魔方可以整体旋转（y、y‘或者y2），还可以用钳子夹住魔方做一个横向拖拽，让魔方滚动（z或者z')，通过滚动和整体旋转的组合，可以把任意面朝下。通过马达控制篮子很容易设计。但卡钳很麻烦，我做了很多的尝试，但是因为马达力量不够，总是夹不紧魔方。最后我不想在这个上面花太多时间，就参考了EV3搭建的方法，做了一个盖子。这个盖子的设计非常精巧，只用了一个马达控制，就可以做到三种动作：放开魔方（以便整体旋转魔方）、卡紧魔方（以便旋转魔方的底层）和翻滚魔方（可以把侧面旋转到朝下的方向）。所以虽然我有三个马达在手上，但最后只用到了两个马达。这是一个两轴魔方机器人。这也就是说，我在得到了魔方的手拧还原步骤（D、U、F、R、L'这种）后，还要转换成机器人的步骤，这个问题很好解决，写个算法就搞掂了。网上也有人用乐高搭的别的机器人，用一根棍子从下往上顶来翻滚魔方的视频，需要用到3个马达。我也想到过这种方式，但得在篮子上开4个洞，没有图纸的情况下也不容易搭建出来。当然，还有更厉害的6轴机器人，需要更多的马达，还原非常迅速。

手动控制盖子让魔方翻滚

2. 篮子大小。一个乐高单位是8mm，标准三阶魔方是56mm，于是我在开始搭建时，没有依照EV3那个设计搭建成8x8的大小。而是用了7x7。7x8=56mm，正好能把魔方放进去。最为关键的是，7x7的篮子大小，会有一个完美的中心孔，可以插如旋转的轴。8x8的篮子，不是围绕着篮子的中心转的。但当我把篮子完全搭好，再搭盖子时，发现我犯了一个错误，那就是虽然7个乐高单位正好能把魔方放进去，但卡得有点儿紧，魔方翻滚非常困难。眼看着辛苦搭建的半成品成为废品，需要拆掉重来时，我突然灵光一现，会不会有小一点儿的魔方呢？淘宝京东找了半天，总算找到了54mm的魔方，但那个时候已经快要过年了，很多淘宝卖家连消息都不回，就在京东选了一个自营的，赶在除夕前一天送到了。

这张照片其实是后来为了做拼搭手册拆下来时拍的

右边是54mm的魔方，就比正常的魔方小一点点

3. 齿距比。马达旋转时，并没有直接带动篮子或者盖子旋转。我用了一组齿轮，齿距比是三比一。也就是想让魔方旋转90度，马达要旋转270度。这么设计的好处是旋转篮子的力量会更大，篮子旋转角度的误差也会更小。

这个齿轮有60个齿，马达带动一个20齿的米色齿轮，米色齿轮再带动这个黑色齿轮，黑色齿轮带动篮子，齿距比就是3：1

四、编码

1. App的编码没有特别难的，我做iOS开发多年，驾轻就熟，很快就搞掂了。

五、优化

1. 识别准确率。有时候因为光照等原因，颜色识别会出错，比如把橙色识别为黄色。我写了一段代码，如果只有一两块颜色识别错了，能纠正过来，其实就是把颜色超过9个的色块，看看第二匹配的颜色是不是少于9块。具体就不多说了。

2. 旋转角度控制。一开始我用的是相对角度来控制马达，比如要让篮子顺时针旋转90度，我就让相应的马达逆时针（因为有一组马达改变了方向）270度。但因为有阻力，马达旋转可能会有偏差。下个命令再让马达旋转270度，又会有偏差。多次旋转后，这个偏差累计就会非常大，篮子都转歪了。后来我发现可以让马达旋转到绝对角度，就是启动Hub时，马达都是处于0度，我只需要让马达旋转到270度或者-270度即可。这个问题就迎刃而解了。

3. 旋转速度。马达旋转时，会因为惯性而不容易停下来，需要刹车。旋转太快了刹不住，旋转太慢了则还原太慢。所以我设计的是先让马达全速旋转90%的目标角度，后10%用中等速度旋转。这样既兼顾了还原速度，又能精准地旋转到需要的角度。

六、准备App上架

1. 花了那么多时间和精力，我打算做成付费的版本，考虑到付费App比较不容易推广，我把搭建手册和控制机器人的功能做成了内购。这就需要我准备一份搭建手册，找了个软件，找着我的实物机器人在软件里又搭建了一个虚拟的，中间发现一些问题，还优化了两次。然后再生成搭建步骤，添加说明。这也是我第一次做内购，花了些时间去学习和了解。

2. 给苹果审核人员录了一个视频，告诉他们这个App是卖拼搭手册+控制机器人的（我不可能给他们寄一套乐高或者一个机器人去），以为上架会比较麻烦，结果异常顺利，一个晚上就审核通过上架了。

七、花絮

1. 一开始魔方还原大概需要两分半钟，调几个参数我就优化到了75秒，不过上架版本为了提高成功率，放慢了速度，大概需要90秒。

2. 儿子学会了逐层还原魔方的方法，和他约定了，每周日比赛一次，如果他能赢了我的机器人（五局三胜），就拆掉机器人给他把越野车拼回去，机器人目前还健在，但被拆掉是迟早的事情。