FallenSigh
83b32542ef
- 创建 arm_control_msgs 包：定义机械臂控制的消息和服务接口 - 消息：JointState, TCPPose - 服务：MoveJoints, MovePose, GetPose, SetGripper - 实现 arm_control 节点：独立的机械臂控制 ROS 节点 - 完整的逆运动学和正运动学 - 关节空间和笛卡尔空间运动控制 - UDP 通信与 ESP32 - 状态发布（10Hz） - 实现 vision_grasp 节点：自动化视觉抓取 - 相机坐标系到基坐标系的完整变换 - 自动抓取流程：释放→移动→抓取→回收 - 自动释放流程：移动→释放→回收 - 多线程执行器支持 - 添加完整文档 - ARM_CONTROL_README.md: 机械臂控制节点使用指南 - VISION_GRASP_README.md: 视觉抓取节点使用指南 - QUICKSTART.md: 快速开始指南 - 文档重命名：docs/craic.md → docs/arm.md
116 lines
5.2 KiB
Markdown
116 lines
5.2 KiB
Markdown
这是一个非常漂亮且实用的自定义机械臂！从你的描述和图片来看，这属于一个**类 SCARA（水平多关节）构型**的机械臂，带有一个直线 Z 轴和三个平面的旋转关节。
|
||
|
||
为了让数学表达更清晰，我们先统一一下坐标系和变量的定义。
|
||
|
||
除角度外，所有线性长度和坐标统一使用 **mm**。
|
||
|
||
### 变量与坐标系定义
|
||
|
||
我们将基坐标系原点设在红色滑轨的零点位置。
|
||
|
||
* **$d_1$**: 关节 1（高度滑块）的位置变量。
|
||
* **$\theta_2$**: 关节 2 的旋转角度（相对于基坐标系 X 轴）。
|
||
* **$\theta_3$**: 关节 3 的旋转角度（相对于上一连杆 $L_1$）。
|
||
* **$\theta_4$**: 关节 4 的旋转角度（相对于上一连杆 $L_2$）。
|
||
* **$(X, Y, Z, \Phi)$**: 夹爪末端（TCP - Tool Center Point）在基坐标系下的位姿。其中 $(X, Y, Z)$ 是空间坐标，$\Phi$ 是夹爪在水平面上的总朝向角（偏航角）。
|
||
* **已知常量**: $L_1$（二三关节距离）, $L_2$（三四关节距离）, $x_4$（夹爪相对关节四的 X 偏移）, $z_4$（夹爪相对关节四的高度偏移）。
|
||
|
||
---
|
||
|
||
### 1. 运动学正解 (Forward Kinematics)
|
||
|
||
正解的目的是：**已知各个电机的角度和滑块高度 $(d_1, \theta_2, \theta_3, \theta_4)$，求夹爪末端的位置 $(X, Y, Z, \Phi)$。**
|
||
|
||
因为 Z 轴的直线运动与 XY 平面的旋转运动是完全解耦的，我们可以分别计算：
|
||
|
||
**高度 (Z 轴):**
|
||
|
||
|
||
$$Z = d_1 + z_4$$
|
||
|
||
**平面朝向角 (偏航角 $\Phi$):**
|
||
|
||
|
||
$$\Phi = \theta_2 + \theta_3 + \theta_4$$
|
||
|
||
**平面坐标 (X, Y):**
|
||
|
||
|
||
$$X = L_1 \cos(\theta_2) + L_2 \cos(\theta_2 + \theta_3) + x_4 \cos(\theta_2 + \theta_3 + \theta_4)$$
|
||
|
||
$$Y = L_1 \sin(\theta_2) + L_2 \sin(\theta_2 + \theta_3) + x_4 \sin(\theta_2 + \theta_3 + \theta_4)$$
|
||
|
||
*(注意：在实际编程中，如果你的电机零点不是一条直线，需要在角度上加上相应的初始偏置)*
|
||
|
||
---
|
||
|
||
### 2. 运动学逆解 (Inverse Kinematics)
|
||
|
||
逆解的目的是：**给出夹爪期望到达的目标位置和朝向 $(X, Y, Z, \Phi)$，求出各关节需要运动到的目标值 $(d_1, \theta_2, \theta_3, \theta_4)$。**
|
||
|
||
> **注意：** 你的机械臂在 XY 平面上有 3 个旋转自由度，但平面位置只需要 2 个自由度 $(X,Y)$。这意味着如果只给定目标坐标，机械臂有无数种姿态可以到达（冗余）。因此，**为了得到唯一解，必须同时指定夹爪的最终期望朝向角 $\Phi$**。
|
||
|
||
下面是逆解的推导步骤，非常适合直接转化为固件中的控制代码：
|
||
|
||
#### 第一步：求解滑块高度 $d_1$
|
||
|
||
高度依然是解耦的，直接通过目标 $Z$ 坐标和常量偏移计算：
|
||
|
||
|
||
$$d_1 = Z - z_4$$
|
||
|
||
#### 第二步：反推关节 4 的坐标 $(X_4, Y_4)$
|
||
|
||
既然我们知道末端目标的坐标 $(X, Y)$ 和总朝向 $\Phi$，我们可以把夹爪的偏置 $x_4$ “剥离”掉，求出关节 4 中轴线在空间中的位置：
|
||
|
||
|
||
$$X_4 = X - x_4 \cos(\Phi)$$
|
||
|
||
$$Y_4 = Y - x_4 \sin(\Phi)$$
|
||
|
||
#### 第三步：求解关节 3 的角度 $\theta_3$
|
||
|
||
现在问题简化为了一个标准的双连杆（两轴）平面机械臂求逆解问题。目标点是 $(X_4, Y_4)$，连杆是 $L_1$ 和 $L_2$。
|
||
根据余弦定理，设目标点到原点的距离平方为 $r^2 = X_4^2 + Y_4^2$，有：
|
||
|
||
|
||
$$\cos(\theta_3) = \frac{X_4^2 + Y_4^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
|
||
|
||
设 $C_3 = \cos(\theta_3)$。在实际控制代码中，必须在这里做合法性检查：如果 $C_3 > 1$ 或 $C_3 < -1$，说明目标点超出了机械臂的物理工作空间（够不到）。
|
||
如果合法，则 $\theta_3$ 的正弦值为：
|
||
|
||
|
||
$$S_3 = \pm \sqrt{1 - C_3^2}$$
|
||
|
||
|
||
*(这里的 $\pm$ 代表机械臂的两种姿态：“左手系/右臂”或“右手系/左臂”，也就是俗称的“手肘朝左”还是“手肘朝右”。你可以根据防碰撞需求或当前姿态选择其中一个)*
|
||
|
||
最终使用反正切函数求解 $\theta_3$：
|
||
|
||
|
||
$$\theta_3 = \text{atan2}(S_3, C_3)$$
|
||
|
||
#### 第四步：求解关节 2 的角度 $\theta_2$
|
||
|
||
利用几何关系和已经求出的 $\theta_3$，可以通过组合角度直接求出 $\theta_2$：
|
||
|
||
|
||
$$\theta_2 = \text{atan2}(Y_4, X_4) - \text{atan2}(L_2 S_3, L_1 + L_2 C_3)$$
|
||
|
||
#### 第五步：求解关节 4 的角度 $\theta_4$
|
||
|
||
因为总朝向 $\Phi = \theta_2 + \theta_3 + \theta_4$，所以：
|
||
|
||
|
||
$$\theta_4 = \Phi - \theta_2 - \theta_3$$
|
||
|
||
---
|
||
|
||
### 💡 嵌入式固件实现建议
|
||
|
||
由于你很可能需要将这些公式写入 MCU（比如利用 C/C++ 或 Rust 编写固件），这里有几个实践建议：
|
||
|
||
1. **使用 `atan2` 替代 `asin/acos**`：上面逆解公式中我全部使用了 $\text{atan2}(y, x)$。在标准库中，`atan2` 能够自动处理四个象限的符号问题，且能避免 $x=0$ 时的除零错误，这在底层驱动中至关重要。
|
||
2. **死区与奇异点保护**：当 $X_4^2 + Y_4^2 \approx 0$ 时（关节 4 缩回到了原点正上方），此时 $\theta_2$ 会失去意义（奇异点）。在代码中应当加入对 $X_4^2 + Y_4^2 < \epsilon$（一个极小值）的判断，防止产生 NaN。
|
||
3. **角度范围归一化**：计算出的角度可能会超出电机支持的物理限位范围（例如超出了 $[-180^\circ, +180^\circ]$），在下发脉冲或指令前，记得对 $\theta_2, \theta_3, \theta_4$ 进行归一化和软限位拦截。
|