解析：Delta 类并联机器人运动学代码

本文档解析了提供的 Python 代码，该代码实现了类似 Delta 机器人（具有中心柱）的逆运动学 (IK) 和正运动学 (FK) 计算，并包含可视化功能。

1. 概述

代码包含以下主要部分：

  1. 库导入: 导入 NumPy (数值计算), SymPy (符号数学), Matplotlib (绘图), math 和 time。

  2. 机器人参数: get_robot_params() 函数定义了机器人的固定几何尺寸。

  3. 逆运动学 (IK): inverse_kinematics() 函数根据期望的末端执行器位置 \((x_p, y_p, z_p)\) 计算所需的关节变量（驱动杆长度 \(q_1, q_2, q_3\) 和中心柱变量 \(q_4, \theta_1, \theta_2\)）。

  4. 正运动学 (FK) - 符号设置: define_symbolic_fk() 函数使用 SymPy 定义 FK 问题的非线性方程组及其雅可比矩阵。

  5. 正运动学 (FK) - 数值求解: forward_kinematics() 函数使用牛顿-拉夫逊 (Newton-Raphson) 迭代法求解由 define_symbolic_fk 定义的方程组，以找到给定驱动杆长度 \(q_1, q_2, q_3\) 时的末端执行器位置。

  6. 可视化: plot_robot() 函数使用 Matplotlib 在 3D 空间中绘制机器人的当前姿态。

  7. 主程序与测试: if __name__ == "__main__": 块包含测试用例，包括单点 IK/FK 验证和轨迹动画。

2. 机器人参数

get_robot_params() 函数返回一个包含机器人几何尺寸的字典。这些参数是固定的：

参数

值

描述

a11

135.0

移动平台上连接点 1 到平台中心的 Y 向距离

a22

135.0

移动平台上连接点 2 到平台中心的 X 向距离

a33

135.0

移动平台上连接点 3 到平台中心的 X 向距离

b11

570.0

固定平台上连接点 1 到平台中心的 Y 向距离

b22

320.0

固定平台上连接点 2 到平台中心的 X 向距离

b33

320.0

固定平台上连接点 3 到平台中心的 X 向距离

e

345.0

中心柱的固定部分长度

注意: a 参数描述了移动平台上的连接点相对于移动平台中心的位置（在移动平台坐标系中），而 b 参数描述了固定平台上的连接点相对于固定平台中心（原点）的位置（在固定坐标系中）。

3. 逆运动学 (IK) - inverse_kinematics

目标: 给定末端执行器（移动平台中心）的目标位置 \(\mathbf{r}_p = [x_p, y_p, z_p]^T\)，计算平台姿态 \((\theta_1, \theta_2)\)、中心柱可变长度 \(q_4\) 以及三个驱动杆的计算长度 \(q_1, q_2, q_3\)。

输入: xp, yp, zp (目标位置), params (机器人参数)。 输出: 包含计算结果的字典 (q1, q2, q3, q4, theta1, theta2, rp, R4, w4) 或错误信息。

计算步骤:

  1. 位置向量: \(\mathbf{r}_p = [x_p, y_p, z_p]^T\)。

  2. 可达性检查: 计算 \(\mathbf{r}_p\) 的范数 \(||\mathbf{r}_p|| = \sqrt{x_p^2 + y_p^2 + z_p^2}\)。如果 \(||\mathbf{r}_p|| < e\)，目标点在中心柱固定长度内，无法达到。

  3. 中心柱可变长度 \(q_4\): \(q_4 = ||\mathbf{r}_p|| - e\)。这是中心柱伸出的长度。

  4. 姿态角 \(\theta_1, \theta_2\): 这些角度定义了移动平台的姿态。它们通过 \(\mathbf{r}_p\) 的分量计算得出：

    • \(\sin(\theta_2) = x_p / (q_4 + e)\)

    • \(\theta_2 = \arcsin(\sin(\theta_2))\) (注意值域限制)

    • \(\theta_1\) 的计算考虑了 \(\cos(\theta_2)\) 可能接近零的情况，使用 atan2 保证数值稳定性：

      • 如果 \(|\cos(\theta_2)|\) 较大: \(\theta_1 = \arctan2\left( \frac{-y_p / (q_4 + e)}{\cos(\theta_2)}, \frac{z_p / (q_4 + e)}{\cos(\theta_2)} \right)\)

      • 如果 \(|\cos(\theta_2)|\) 接近零: \(\theta_1 = \arctan2(-y_p, z_p)\) (或在特殊情况下设为 0)

  5. 旋转矩阵 \(\mathbf{R}_4\): 根据欧拉角（这里是 \(\theta_1, \theta_2\) 的特定组合）计算从移动平台坐标系到固定坐标系的旋转矩阵：

    \[\begin{split} \mathbf{R}_4 = \begin{bmatrix} \cos(\theta_2) & 0 & \sin(\theta_2) \\ \sin(\theta_1)\sin(\theta_2) & \cos(\theta_1) & -\sin(\theta_1)\cos(\theta_2) \\ -\cos(\theta_1)\sin(\theta_2) & \sin(\theta_1) & \cos(\theta_1)\cos(\theta_2) \end{bmatrix} \end{split}\]

  6. 连接点坐标:

    • 固定平台连接点 (固定坐标系): \(\mathbf{b}_1 = [0, -b_{11}, 0]^T\), \(\mathbf{b}_2 = [b_{22}, 0, 0]^T\), \(\mathbf{b}_3 = [-b_{33}, 0, 0]^T\)。

    • 移动平台连接点 (移动平台自身坐标系): \(\mathbf{a}_{10} = [0, -a_{11}, 0]^T\), \(\mathbf{a}_{20} = [a_{22}, 0, 0]^T\), \(\mathbf{a}_{30} = [-a_{33}, 0, 0]^T\)。

    • 移动平台连接点相对于平台中心 \(\mathbf{r}_p\) 的向量 (固定坐标系): \(\mathbf{a}_i = \mathbf{R}_4 \mathbf{a}_{i0}\) for \(i=1, 2, 3\)。

  7. 中心柱方向向量 \(\mathbf{w}_4\): 这是从原点指向中心柱顶端的单位向量（固定坐标系）。

    \[\begin{split} \mathbf{w}_4 = \begin{bmatrix} \sin(\theta_2) \\ -\sin(\theta_1)\cos(\theta_2) \\ \cos(\theta_1)\cos(\theta_2) \end{bmatrix} \end{split}\]

    理论上，\(\mathbf{w}_4 = \mathbf{r}_p / ||\mathbf{r}_p|| = \mathbf{r}_p / (q_4 + e)\)。代码中包含对此的验证（已注释）。

  8. 驱动杆计算长度 \(q_1, q_2, q_3\): 重要: 代码中 \(q_1, q_2, q_3\) 的计算方式 不是 标准的物理连杆长度（即 \(L_i = ||(\mathbf{r}_p + \mathbf{a}_i) - \mathbf{b}_i||\)）。这里的 \(q_i\) 是根据以下公式计算的：

    \[ q_i = || \mathbf{r}_p - \mathbf{b}_i + \mathbf{a}_i - e \mathbf{w}_4 || \quad \text{for } i=1, 2, 3 \]

    这个 \(q_i\) 是一个派生值，似乎代表从固定平台基座点 \(\mathbf{b}_i\) 到移动平台连接点 \(\mathbf{a}_i\) 的向量，再减去中心柱固定部分的向量 \(e \mathbf{w}_4\) 后，最终得到的向量 \((\mathbf{r}_p + \mathbf{a}_i - \mathbf{b}_i) - e \mathbf{w}_4\) 的模长。这个定义是为了简化后续的正解方程。

4. 正运动学 (FK)

目标: 给定三个驱动杆的计算长度 \(q_{1,in}, q_{2,in}, q_{3,in}\)（由 IK 计算得到或直接指定），找出对应的末端执行器位置 \((x_p, y_p, z_p)\) 以及姿态变量 \((q_4, \theta_1, \theta_2)\)。FK 通常比 IK 更复杂，因为它需要求解非线性方程组。

4.1 define_symbolic_fk

目标: 使用 SymPy 建立 FK 的符号方程和雅可比矩阵。

输入: params, q1_in, q2_in, q3_in (输入的驱动杆计算长度)。 输出: 符号方程组 Func_sym, 符号雅可比矩阵 J_sym, 以及它们的快速数值计算函数 num_func, num_jac。

步骤:

  1. 定义符号变量: 未知数是 \(\mathbf{X} = [q_{4,sym}, \theta_{1,sym}, \theta_{2,sym}]^T\)。

  2. 建立约束方程: 基于机器人的几何约束（闭环方程），建立三个方程 \(F=0, G=0, H=0\)。这些方程将输入的 \(q_{1,in}, q_{2,in}, q_{3,in}\) 与未知的 \(q_4, \theta_1, \theta_2\) 联系起来。代码中给出的方程是：

    \[ F(q_4, \theta_1, \theta_2) = a_{11}^2 + b_{11}^2 + q_4^2 - q_{1,in}^2 - 2 a_{11} b_{11} \cos(\theta_1) - 2 b_{11} q_4 \sin(\theta_1) \cos(\theta_2) = 0 \]
    \[ G(q_4, \theta_1, \theta_2) = a_{22}^2 + b_{22}^2 + q_4^2 - q_{2,in}^2 - 2 b_{22} q_4 \sin(\theta_2) - 2 a_{22} b_{22} \cos(\theta_2) = 0 \]
    \[ H(q_4, \theta_1, \theta_2) = a_{33}^2 + b_{33}^2 + q_4^2 - q_{3,in}^2 + 2 b_{33} q_4 \sin(\theta_2) - 2 a_{33} b_{33} \cos(\theta_2) = 0 \]

    这些方程可能是通过对三个驱动链应用余弦定理或向量范数平方推导出来的，并利用了 IK 中定义的 \(q_i\)。令 \(\mathbf{Func} = [F, G, H]^T\)。

  3. 计算雅可比矩阵: \(\mathbf{J} = \frac{\partial \mathbf{Func}}{\partial \mathbf{X}}\)。这是一个 \(3 \times 3\) 矩阵，包含了 \(F, G, H\) 对 \(q_4, \theta_1, \theta_2\) 的偏导数。1

    \[\begin{split} \mathbf{J} = \begin{bmatrix} \frac{\partial F}{\partial q_4} & \frac{\partial F}{\partial \theta_1} & \frac{\partial F}{\partial \theta_2} \\ \frac{\partial G}{\partial q_4} & \frac{\partial G}{\partial \theta_1} & \frac{\partial G}{\partial \theta_2} \\ \frac{\partial H}{\partial q_4} & \frac{\partial H}{\partial \theta_1} & \frac{\partial H}{\partial \theta_2} \end{bmatrix} \end{split}\]

  4. 生成数值函数: 使用 sp.lambdify 将符号表达式转换为高效的 NumPy 函数，用于后续的数值迭代。

4.2 forward_kinematics

目标: 使用牛顿-拉夫逊法数值求解 \(\mathbf{Func}(\mathbf{X}) = \mathbf{0}\)。

输入: q1_in, q2_in, q3_in, params, initial_guess (对 \(q_4, \theta_1, \theta_2\) 的初始估计), max_iter, tolerance。 输出: 包含计算得到的 \((x_p, y_p, z_p)\) 和 \((q_4, \theta_1, \theta_2)\) 的字典，以及迭代信息或错误。

步骤:

  1. 获取数值函数: 调用 define_symbolic_fk 获取 num_func 和 num_jac。

  2. 初始化: \(\mathbf{X}_k = \text{initial\_guess}\)。

  3. 迭代: 重复以下步骤直到收敛或达到最大迭代次数： a. 计算函数值: \(\mathbf{F}_k = \text{num\_func}(\mathbf{X}_k)\)。 b. 检查收敛: 如果 \(||\mathbf{F}_k|| < \text{tolerance}\)，则停止迭代，认为已收敛。 c. 计算雅可比矩阵: \(\mathbf{J}_k = \text{num\_jac}(\mathbf{X}_k)\)。 d. 求解线性方程组: 求解 \(\mathbf{J}_k \Delta \mathbf{X} = -\mathbf{F}_k\) 得到更新步长 \(\Delta \mathbf{X}\)。如果 \(\mathbf{J}_k\) 奇异（行列式接近零），则无法求解，迭代失败。 e. 更新变量: \(\mathbf{X}_{k+1} = \mathbf{X}_k + \Delta \mathbf{X}\)。

  4. 处理结果:

    • 如果收敛，记录最终的 \(\mathbf{X}_k = [q_{4,k}, \theta_{1,k}, \theta_{2,k}]^T\)。

    • 如果达到最大迭代次数或遇到奇异雅可比矩阵，标记为失败。

  5. 计算末端位置: 如果迭代成功，使用最终的 \(q_{4,k}, \theta_{1,k}, \theta_{2,k}\) 计算末端执行器位置：

    \[ x_{p,fk} = (q_{4,k} + e) \sin(\theta_{2,k}) \]
    \[ y_{p,fk} = (q_{4,k} + e) (-\sin(\theta_{1,k}) \cos(\theta_{2,k})) \]
    \[ z_{p,fk} = (q_{4,k} + e) \cos(\theta_{1,k}) \cos(\theta_{2,k}) \]

5. 可视化 - plot_robot

目标: 在 3D 图中绘制机器人的骨架结构。

输入: Matplotlib 3D 坐标轴 ax, 末端位置 \(\mathbf{r}_p\), 旋转矩阵 \(\mathbf{R}_4\), params, title。

步骤:

  1. 清空绘图: ax.cla()。

  2. 计算关键点坐标:

    • 原点 \(\mathbf{O} = [0,0,0]^T\)。

    • 固定平台基座点 \(\mathbf{B}_1, \mathbf{B}_2, \mathbf{B}_3\) (直接使用参数 b_ij)。

    • 移动平台连接点（相对于移动平台中心，在固定坐标系中） \(\mathbf{A}_1, \mathbf{A}_2, \mathbf{A}_3\) (通过 \(\mathbf{R}_4 \mathbf{a}_{i0}\) 计算)。

    • 移动平台连接点的绝对坐标 \(\mathbf{A}'_i = \mathbf{r}_p + \mathbf{A}_i\)。

    • 中心柱固定部分的顶端 \(\mathbf{E} = e \mathbf{w}_4\) (需要计算 \(\mathbf{w}_4\)，代码中通过临时调用 IK 或近似计算得到)。

  3. 绘制结构:

    • 固定平台 (连接 \(\mathbf{B}_1, \mathbf{B}_2, \mathbf{B}_3\))。

    • 移动平台 (连接 \(\mathbf{A}'_1, \mathbf{A}'_2, \mathbf{A}'_3\))。

    • 驱动杆 (连接 \(\mathbf{B}_i\) 到 \(\mathbf{A}'_i\)，这代表物理连杆)。

    • 中心柱 (固定部分 \(\mathbf{O}\) 到 \(\mathbf{E}\)，可变部分 \(\mathbf{E}\) 到 \(\mathbf{r}_p\))。

    • 标记关键点 (\(\mathbf{O}, \mathbf{r}_p, \mathbf{B}_i, \mathbf{A}'_i\))。

  4. 设置绘图属性: 坐标轴标签、标题、范围、图例、网格、尝试设置等比例轴。

6. 主程序与测试

if __name__ == "__main__": 块执行以下操作：

  1. 加载参数: 调用 get_robot_params()。

  2. 测试 1: 单点 IK/FK 验证:

    • 设定一个目标位置 \((x_p, y_p, z_p)\)。

    • 调用 inverse_kinematics 计算 \(q_1, q_2, q_3, q_4, \theta_1, \theta_2\)。

    • 使用 IK 计算出的 \(q_1, q_2, q_3\) 作为 forward_kinematics 的输入，并使用 IK 计算出的 \(q_4, \theta_1, \theta_2\) 作为初始猜测值。

    • 比较 FK 计算出的 \((x_{p,fk}, y_{p,fk}, z_{p,fk})\) 与原始目标 \((x_p, y_p, z_p)\)，验证一致性。

    • 调用 plot_robot 显示该姿态。

  3. 测试 2: 垂直轨迹动画:

    • 设置一个垂直移动的轨迹（\(x, y\) 固定，\(z\) 变化）。

    • 开启 Matplotlib 交互模式 (plt.ion())。

    • 循环遍历轨迹上的多个点：

      • 计算当前目标 \((x_p, y_p, z_p)\)。

      • 运行 inverse_kinematics。

      • 调用 plot_robot 更新显示机器人姿态。

      • 绘制已走过的轨迹线。

      • 暂停一小段时间 (plt.pause()) 形成动画效果。

    • 结束后关闭交互模式 (plt.ioff()) 并保持窗口显示 (plt.show())。

  4. 测试 3 (注释掉): 类似地定义了一个水平圆周轨迹。

7. 总结

该代码完整地实现了特定结构并联机器人的逆运动学和正运动学求解。

  • 逆解 是解析的，直接根据末端位置计算关节变量和姿态。注意 \(q_1, q_2, q_3\) 的定义并非标准物理杆长。

  • 正解 是数值的，使用牛顿法迭代求解由几何约束导出的非线性方程组。方程组的建立依赖于逆解中 \(q_1, q_2, q_3\) 的特殊定义。

  • 可视化 清晰地展示了机器人的三维结构。

  • 测试 验证了 IK 和 FK 的一致性，并展示了轨迹跟踪的应用。

代码结构清晰，注释良好，是理解和研究此类并联机器人运动学的一个很好的实例。