# 双臂魔方机器人（闭环版本）

**Repository Path**: robotdna/dual-arm-rubiks-cube-robot-v2

## Basic Information

- **Project Name**: 双臂魔方机器人（闭环版本）
- **Description**: 使用4个闭环步进电机控制的双臂双指解魔方机器人，支持力矩控制，可控制夹紧压力，防止魔方滑落。
- **Primary Language**: Python
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 12
- **Created**: 2025-06-23
- **Last Updated**: 2025-06-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 说明 😈
使用 4 个闭环步进电机控制的双臂双指解魔方机器人，支持力矩控制，可控制夹紧压力，防止魔方滑落。

分齿轮传动版本和同步带传动两个版本。

齿轮传动版本大部分部件使用 FDM 3D 打印方式制作，优缺点如下：

✨ **优点：**
*   成本低，方便加工

❗ **缺点：**
1.  旋转臂晃动较大，极限速度低
2.  强度有限，程序 bug 导致的异常状态下，结构件容易损坏

同步带传动版本大部分部件使用标准件、以及 CNC 加工部件，优缺点如下：

✨ **优点：**
1.  旋转臂晃动极小，还原速度快，可靠性高
2.  强度高，异常碰撞不会损坏

❗ **缺点：**
*   加工成本较高

**后续软件开发，只维护同步带传动版本**，两者的控制方式几乎一致，但是部分参数不同，如有需要可自行移植调试。

---

# 同步带传动版本 🚀
![装配图](img/belt.png)

## 🛠️ 部件加工说明
### 🔩 CNC 件加工说明
> 除了齿条，设计文件均适配铨XXX的智能制造工艺，可低成本打样，10个CNC件100多元。
> 推荐使用 6061 铝合金材料。
> 运动部件的重量会影响性能，不建议用密度太高的材料（黄铜、钢之类的）。

1.  **`魔方机器人--42电机固定座-右侧.step`**
    *   加工 1 个。
    *   如有条件，可精确控制环形凸台的外径，与 6700ZZ 轴承的内径紧配。
    *   如果没有条件，也可以留一点空隙，用 638 圆柱部件固持胶粘接。强度是满足要求的，实测 20kg 左右的拉力不松动。
2.  **`魔方机器人--42电机固定座-左侧.step`**
    *   加工 1 个，同上。这两个部件的外形是镜像关系。
3.  **`魔方机器人--手指-加工4个.step`**
    *   加工 4 个。
    *   与旋转臂接触的几个面尽量光滑平整。如果不平整，可通过后期砂纸打磨解决。
    *   部件上的 2 个孔，需要加工 M2 螺纹。
4.  **`魔方机器人--齿条-加工4个.step`**
    *   加工 4 个。
    *   可用 2mm 平底铣刀和 40° 0.2mm 平底尖刀加工。
    *   也可以考虑用成品 0.5 模齿条切割打孔制作。
5.  **`魔方机器人--旋转臂顶盖--加工2个.step`**
    *   加工 2 个。
    *   部件上的 4 个孔，需要加工 M2 螺纹。
6.  **`魔方机器人--旋转臂底部-含侧面扳手孔.step`** 或 **`魔方机器人--旋转臂底部-不含侧面扳手孔--加工2个.step`**
    *   选择其中某一文件，加工 2 个。
    *   侧面扳手孔用于拧凸台齿轮的固定螺丝，对精度要求很低，可以自己用 2mm 麻花钻加工。如果用 CNC，需要多装夹一次。

### 🖨️ 3D 打印件说明
**文件：** `魔方机器人--底座--加厚版本.stl`，加工 1 个

| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.2mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |
| 支撑             | 不需要                   |

## 📦 物料清单 (BOM)
### 🔧 核心部件
| 编号 | 名称                                     | 规格参数                                  | 数量 | 备注                                                                 |
|------|------------------------------------------|-----------------------------------------|------|----------------------------------------------------------------------|
| 1    | 6700ZZ 滚珠轴承                          | 10x15x4mm                               | 4    |                                                                      |
| 2    | 步进电机                                 | 48 长度，轴长 24mm±1mm                  | 2    | 支持侧/底部出线，电机凸台高度 2-3mm，结构设计也**兼容 42 无刷电机** |
| 3    | 步进电机                                 | 48 长度，轴长 17mm±1mm                  | 2    | 可用买标准轴长的，自己切短一点                                       |
| 4    | 径向磁铁                                 | 6x2.5mm                                 | 4    | 闭环控制板用                                                       |
| 5    | 凸台齿轮                                 | 0.5 模 20 齿                            | 2    |                                                                      |
| 6    | 同步带轮                                 | 2GT-40 齿-AF-齿宽 7-内孔 15             | 2    |                                                                      |
| 7    | 同步带轮                                 | 2GT-20 齿-AF-齿宽 7-内孔 5              | 2    |                                                                      |
| 8    | 刚性法兰盘联轴器                         | 内径 5                                  | 1    | 用作摄像头支架                                                       |
| 9    | 5mm 直径金属棒                           | 长度 300mm                              | 1    | 用作摄像头支架，尺寸要求不高，买光轴，铁棒，钢筋之类的都行           |
| 10   | 2GT 同步带                               | 6mm 宽度，148mm 长度                    | 2    |                                                                      |
| 11   | 闭环 42 电机控制板                       |                                         | 4    | 参考另一个开源项目：https://gitee.com/hemn1990/closed-loop-stepper-motor  后续考虑做闭环的 42 无刷电机（伺服电机） |

> **注：** 部件的具体尺寸，可参考 CAD 设计文件，可用开源的 FreeCAD（版本号 1.0 或者更高）打开。

🔗 **部分器件的参考购买链接（仅供参考）：**
*   同步带轮：[https://item.taobao.com/item.htm?id=632391487295](https://item.taobao.com/item.htm?id=632391487295)
*   齿轮：[https://item.taobao.com/item.htm?id=806227801033](https://item.taobao.com/item.htm?id=806227801033)
*   步进电机：[https://detail.tmall.com/item.htm?id=623890275768](https://detail.tmall.com/item.htm?id=623890275768)
*   滚珠轴承：[https://item.taobao.com/item.htm?id=771754646342](https://item.taobao.com/item.htm?id=771754646342)
*   法兰盘联轴器：[https://item.taobao.com/item.htm?id=554246764479](https://item.taobao.com/item.htm?id=554246764479)
*   同步带：[https://item.taobao.com/item.htm?id=731066241303](https://item.taobao.com/item.htm?id=731066241303)

### 🔩 紧固件
| 类型                  | 规格       | 数量 | 备注                             |
|----------------------|------------|------|----------------------------------|
| M3 杯头内六角螺丝     | M3*6       | 32   |                                  |
| M2 杯头内六角螺丝     | M2*4       | 16   |                                  |
| M3 杯头内六角螺丝     | 45mm       | 16   | 闭环控制板                       |
| 热熔螺母             | M3x4x5     | 16   | 外径 5mm，光杆直径 4.3mm         |

### 📎 其他
*   M4x25 尼龙脚杯 × 4
*   5mm 塑料柱 × 16（闭环控制板支撑用，根据径向磁铁的安装高度调整）
*   638 厌氧胶水/圆柱形部件固持胶（不可以用 502 之类的通用胶水代替，不耐震动容易脱落）
*   润滑脂
*   2mm厚度硅胶垫（带双面胶）

## 🧩 装配步骤
1.  **安装轴承：** 将 4 个 6700ZZ 滚珠轴承分别安装在 42 电机固定座的环形凸台上，每侧 2 个。根据情况选择：
    *   用台钳压入（紧配）
    *   或用 638 胶水粘接（间隙配合）。
    *   ⚠️ **注意：** 千万不要把胶水弄到轴承内部！638 的粘度较高，不容易出现这个问题。如果用流动性更好的胶水，需要特别注意。
2.  **安装同步带轮：** 将 2 个 40 齿同步带轮，分别安装在旋转臂底部组件的环形凸台上，方式同上（压入或胶粘）。
3.  **固化胶水：** 等待几分钟，擦掉多余胶水，然后等待剩余的 638 胶水固化。
4.  **安装热熔螺母：** 在底座的 16 个盲孔上，加热压入热熔螺母。（通孔是用来安装摄像头支架的，不需要装螺母）
5.  **固定底座部件：** 将法兰盘联轴器、42 电机固定座用 M3 螺丝固定在底座上。
6.  **安装步进电机：** 安装 4 个步进电机。腰孔的螺丝暂时不需要锁紧。
7.  **安装金属棒：** 安装 5mm 直径金属棒（摄像头支架）。
8.  **安装同步带系统：**
    *   安装大小两种同步带轮和同步带。
    *   ⚠️ **注意：** 大同步带轮（40齿）的顶丝**不要太紧**！这个顶丝是顶到轴承侧壁的，轴承侧壁不能承受过大的压力。
9.  **张紧同步带：**
    *   在两个电机之间塞入厚度合适的物品（例如一字螺丝刀）以张紧同步带。
    *   锁紧腰孔的螺丝以固定电机位置。
10. **组装手指：**
    *   将齿条和手指组装在一起。
    *   使用 M2 螺丝锁紧。
    *   需要控制垂直度，可以夹在台钳的 90° 面上安装。
11. **安装齿轮：**
    *   安装 0.5 模 20 齿齿轮。
    *   通过旋转臂的侧孔，插入内六角螺丝刀锁紧固定螺丝。
    *   ⚠️ **注意：** 在不滑丝的前提下，锁紧力度尽可能大，否则可能会脱落。
    *   💡 **建议：** 可以考虑用螺纹胶（中低强度）。千万别用圆柱部件固持胶（如638），强度极高，粘上就很难拆卸。
12. **润滑与组装：**
    *   在滑动部位（如齿条、轴承）涂抹适当润滑脂。⚠️ **注意：** 同步带不需要润滑！
    *   安装旋转臂剩余部件（顶盖等）。
13. **测试与调整：**
    *   测试手指组件的滑动是否顺畅。
    *   旋转旋转臂组件，手指应当发生滑动。
    *   如果不顺滑，可适当打磨齿条和手指组件。可通过观察磨损位置，确定要打磨的点位。
14. **粘贴硅胶垫：**
    *   将硅胶垫裁剪为12x11mm的尺寸，分别粘贴在4个手指的尖端

---

# 齿轮传动版本 ⚙️
![装配图](img/gear.png)

## 🖨️ 打印参数说明
### 🧱 基座专用设置 (`魔方机器人基座.stl`)
| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.2mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |
| **特殊要求**     | 标记面（4, 3, 1, 2）朝 Z 轴正方向打印 |

### 🔧 其他部件通用设置
| 参数项           | 设置值                     |
|------------------|--------------------------|
| 打印技术         | FDM 3D 打印              |
| 推荐耗材         | PETG/ABS                 |
| 喷嘴直径         | 0.4mm                    |
| 层高             | 0.1mm                    |
| 结构厚度         | 壁厚/顶/底层均 2mm       |
| 填充率           | 40-90% (推荐 80%)        |

> ⚠️ **注意：** PLA 材料因脆性和时效强度衰减问题，不推荐用于任何结构件打印。

⚠️ **关键注意事项**
1.  推荐提前打印 `孔径校准圆柱.stl`，调整水平扩展参数使内径 ≈ 5mm。
2.  使用大象脚补偿功能确保齿轮工作面平整。
3.  **尺寸标注文件选择指南：**
    *   外圆偏小 → 选择 `旋转臂主体-17.2mm.stl`
    *   外圆偏大 → 选择 `旋转臂主体-17.0mm.stl`

## 📦 物料清单 (BOM)
### 🔧 核心部件
| 编号 | 名称                      | 规格参数                     | 数量 | 备注                     |
|------|--------------------------|----------------------------|------|--------------------------|
| 1    | 黄铜六角联轴器           | 对边 12mm×长 18mm×孔 5mm   | 2    |                          |
| 2    | 热熔螺母                 | M3x4x5                     | 6    | 外径 5mm，光杆直径 4.3mm |
| 3    | 6903 滚珠轴承            | 17×30×7mm                  | 2    |                          |
| 4    | 步进电机                 | 48 长度                    | 4    | 支持侧/底部出线          |
| 5    | 径向磁铁                 | 6x2.5mm                    | 4    | 闭环控制板专用           |

### 🔩 紧固件
| 类型                  | 规格              | 数量 | 备注                 |
|----------------------|-------------------|------|----------------------|
| M2x8 十字平头自攻螺丝 | 头径 ≤ 4.2mm      | 24   |                      |
| M3 机米螺丝          | 6mm               | 4    |                      |
| M3 杯头内六角螺丝     | 8mm               | 16   |                      |
| M3 杯头内六角螺丝     | 45mm              | 16   | 闭环控制板专用       |

### 📎 选配件
*   M4x25 尼龙脚杯 × 4
*   5mm 塑料柱 × 16（闭环控制板支撑用，根据径向磁铁的安装高度调整）

## 🧰 调试工具包
1.  **必备工具** 🛠️
    *   1.5mm 麻花钻（扩孔用）
    *   螺丝刀套装
    *   游标卡尺
    *   3D 打印机

2.  **电子调试工具** 🔌
    *   万用表
    *   示波器
    *   USB 转 485 转换器
    *   可调直流电源

3.  **辅助工具** 🧪
    *   焊台 + 热风枪
    *   镊子
    *   打磨工具组

💻 **开发环境**
支持跨平台开发（Windows/macOS/Linux）

# 软件调试说明（software）

## 各程序功能与运行机理

### 1. cube_color_detect.py - 自适应魔方颜色识别
![魔方块分割与标记结果](img/blocks_with_colors_results_annotated.jpg)
![基于hsv色彩空间的自适应颜色分裂](img/lab_clustering.png)
**功能**：  
1. 从三张不同角度的魔方照片中识别54个色块的颜色，输出魔方状态字符串
2. 自适应色彩识别，除了需要提前标定魔方所在的区域，不需要配置其他参数

**运行流程**：
1. 加载三张魔方照片和六个定位点
2. 对每张照片进行透视变换，分割出18个色块（三张共54个）
3. 对每个30x30像素的色块：
   - 使用改进的K-means聚类算法提取主色调
   - 动态合并相似颜色
   - 标记非主色区域为透明粉色
4. 基于色彩空间，对54个主色调进行固定大小聚类（6类）
5. 将聚类结果映射到魔方展开图
6. 输出魔方状态字符串（54个字符）

**调试要点**：
```python
# 启用绘图调试（会显示处理过程）
need_plot = True
```

### 2. cube_motion.py - 机械臂运动控制
**功能**：  
通过串口控制4个步进电机完成魔方的夹持、旋转和翻转动作

**核心控制逻辑**：
```python
# 运动控制指令示例
mc.motions(['R1', 'L*F', 'R0'])  # 执行一组动作

# 动作类型说明：
# L0/R0: 夹爪夹紧
# L1/R1: 夹爪松开
# L2/R2: 夹爪张开最大角度
# L+T/L-T/L*T(L可替换为R): 90°顺时针/逆时针旋转/180°旋转
# L+F/L-F/L*F(L可替换为R): 90°/180°翻转
# L+N/R+N: 旋转臂快速空转90°

# 动作举例
# L1 R-F L0 R2 R+N R0 L+T R1 L+F R0 R-T R2 R+N R0 L*T L1 R-F L0 R2 R+N R0 ......
```

**通信协议**：
1. 使用自定义帧结构（FF FF开头）
2. CRC8校验（多项式0x07）
3. 支持单指令多电机控制

**调试要点**：
```python
# 关键参数调整：
SPEED_FACTOR = 1.0  # 全局速度因子（建议从0.2开始调试）
ARM4_ZERO = 15095   # 左侧旋转臂零点
ARM2_ZERO = 803     # 右侧旋转臂零点
```

### 3. cube_optimizer.py - 动作序列优化
**功能**：  
将魔方解法转换为机械臂最优动作序列，减少总执行时间

**优化算法**：
1. 状态空间：(当前夹持面, 左臂垂直状态, 右臂垂直状态)
2. 使用BFS搜索2步内的翻面路径
3. 动态规划选择耗时最短的路径

**调试要点**：
```python
# 搜索深度，数值越大，结果越精确，但是搜索算法耗时越长
if new_depth < 2:
    queue.append((s1, l1, r1, new_flip_time, new_flip_sequence, new_depth))
```

### 4. cube_robot_test.py - 主控程序
**功能**：  
集成颜色识别、运动控制和优化模块，实现完整的魔方还原流程

**工作流程**：
1. 初始化摄像头和串口
2. 用户交互界面：
   - 实时显示摄像头画面
   - 可拖动6个点标定魔方位置
3. 按空格键启动还原：
   ```mermaid
   graph TD
   A[夹紧魔方] --> B[拍摄三张照片]
   B --> C[颜色识别]
   C --> D[Kociemba求解]
   D --> E[动作序列优化]
   E --> F[执行机械臂动作]
   F --> G[松开魔方]
   ```

### 5. verify_arm_finger_linkage.py - 运动规划验证
**功能**：  
模拟手臂和手指的联动运动，优化空转90°的运动参数

**输出参数**：
```python
V_NO_LOAD_20_70_DEG = 846  # 手臂20°-70°区间的空转速度
FINGER_NO_LOAD_START_ARM = 63.98  # 手臂开始空转时手指的位置
```

## 调试流程指南

### 准备工作
1. **硬件连接**：
   - 确保4个电机正确接线
   - 摄像头对准魔方区域
   - 所有设备供电正常

2. **环境配置**：
   ```bash
   pip install opencv-python numpy matplotlib pyserial kociemba
   ```

### 分模块调试步骤

**颜色识别模块**：
1. 准备三张魔方照片（不同角度）
2. 运行`cube_color_detect.py`
3. 检查输出：
   - 透视变换后的色块图
   - HSV空间聚类结果
   - 魔方状态字符串

**运动控制模块**：
1. 连接电机控制器
2. 运行`cube_motion.py`
3. 使用交互菜单：
   ```python
   [1]: 使能全部电机
   [2]: 禁用全部电机
   [3]: 查询电机角度
   [4]: 回零点(需要先回零点才能执行其他的！)
   [5]: 松开魔方
   [6]: 夹紧魔方
   [7]: 预留
   [8]: 测试旋转臂动作
   [9]: 打乱魔方再还原
   [q]: 退出程序
   ```

**主控程序调试**：
1. 运行`cube_robot_test.py`
2. 在摄像头画面中拖动6个点标定魔方
3. 按空格键开始自动还原

## 常见问题解决

1. **颜色识别不准确**：
   - 优化光照条件
   - 反光面积不可以超过单个色块的1/3
   - 魔方必须包含一个白色面

2. **机械臂动作异常**：
   ```python
   # 调整零点参数（根据实际位置）
   # 执行cube_motion.py
   # 选择[2]: 禁用全部电机
   # 人工移动旋转臂，使手指落在魔方的正中央
   # 选择[3]: 查询电机角度
   # 修改代码中的零点位置
   ARM4_ZERO = 15095   # 左侧旋转臂零点
   ARM2_ZERO = 803     # 右侧旋转臂零点
   ```

3. **魔方定位偏差**：
   - 确保6个标定点包围魔方

4. **串口通信错误**：
   - 检查端口号，当打开默认串口失败时，会自动列出可用串口
   ```
   可用串口设备:
   /dev/cu.wlan-debug - n/a
   /dev/cu.Bluetooth-Incoming-Port - n/a
   /dev/cu.usbserial-120 - USB Serial
   、、、
   - 检查电机控制板的状态

## 性能优化建议
1. **运动优化**：
   - 减小`SPEED_FACTOR`提高稳定性
   - 增大`SPEED_FACTOR`提高速度
   - 可单独修改翻转、旋转、空转、手指伸缩动作的速度


2. **典型性能指标**
   - 目前版本，从开始拍照，到魔方还原，耗时在6-7秒之间
   - 程序还没有完全开发完成，预计可优化到5秒之内

# 许可证
采用 **MIT开源协议**，允许商业用途。