第二章 运动学基础：空间坐标系、齐次变换、正运动学推导

各位同学，欢迎来到第二章。这一章是Delta机器人的“骨架”——运动学基础。说实话，很多搞控制的工程师一上来就调PID参数，结果机器人乱跑，根本原因就是运动学没搞明白。我当年刚入行时也犯过这个错，后来被老工程师训了一顿，才老老实实回来补课。

这一章，我会带你搞懂三件事：空间坐标系怎么建、齐次变换怎么用、正运动学怎么推。别怕，咱们一步步来。

2.1 空间坐标系：给机器人一个“家”

Delta机器人有三个主动臂、三个从动臂，末端执行器在空间里飞来飞去。要描述它的位置和姿态，首先得有个坐标系。

世界坐标系：固定在机器人基座上的坐标系，通常记为{O}。原点在基座中心，Z轴向上，X轴指向某个主动臂方向。这是所有计算的基准。

工具坐标系：固定在末端执行器上的坐标系，记为{T}。原点在抓取中心，Z轴沿抓取方向。你想想看，如果工具坐标系歪了，抓取精度肯定完蛋。

关节坐标系：每个主动臂的旋转角度，记为θ₁、θ₂、θ₃。这是电机直接控制的东西。

我个人习惯把世界坐标系放在基座中心，这样后续计算对称性好，不容易出错。嗯，这里要注意：坐标系方向一定要统一，我见过有人把Z轴朝下，结果正运动学算出来末端位置全是负的，排查了半天。

2.2 齐次变换：让坐标“动”起来

坐标系建好了，怎么把一个点从一个坐标系变换到另一个坐标系？这就用到齐次变换矩阵。

齐次变换矩阵是一个4×4的矩阵，形式如下：

T = [ R   p ]
    [ 0   1 ]

其中R是3×3旋转矩阵，描述姿态；p是3×1平移向量，描述位置。

举个例子：从世界坐标系{O}到工具坐标系{T}的变换，记为^OT_T。如果末端执行器在世界坐标系中的位置是(0.2, 0.1, -0.3)米，姿态是绕Z轴旋转30度，那么：

R = [ cos30°  -sin30°  0 ]
    [ sin30°   cos30°  0 ]
    [ 0        0       1 ]

p = [0.2, 0.1, -0.3]ᵀ

我在项目中遇到过一个问题：齐次变换矩阵的乘法顺序搞反了。记住，左乘表示相对于固定坐标系变换，右乘表示相对于当前坐标系变换。这个坑我踩过，后来每次写代码都先画个图确认顺序。

2.3 正运动学推导：从角度到位置

正运动学，说白了就是：已知三个主动臂的角度θ₁、θ₂、θ₃，求末端执行器的位置(x, y, z)。

Delta机器人的正运动学推导，我习惯用几何法。为什么？因为直观，不容易出错。

推导步骤：

1. 建立坐标系：在世界坐标系{O}中，确定三个主动臂的基座位置A₁、A₂、A₃。它们均匀分布在120°的圆周上。
2. 计算主动臂末端位置：已知主动臂长度L₁和角度θᵢ，可以算出主动臂末端点Bᵢ的位置。
3. 计算从动臂约束：从动臂长度L₂固定，所以从动臂末端点Cᵢ（即末端执行器连接点）到Bᵢ的距离恒为L₂。
4. 求解末端位置：三个从动臂的交点就是末端执行器位置P。这相当于求三个球面的交点。

具体公式如下（以第一个主动臂为例）：

B₁ = [ R*cos(0°) + L₁*cos(θ₁)*cos(0°)
       R*sin(0°) + L₁*cos(θ₁)*sin(0°)
       -L₁*sin(θ₁) ]

其中R是基座半径。三个球面方程联立，解出P的坐标。嗯，这里要注意：三个球面通常有两个交点，一个在上方（不可达），一个在下方（可达），取Z值较小的那个。

正运动学的代码实现，我建议用Python或C++，把矩阵运算库用起来。下面是一个简化版的伪代码：

function forward_kinematics(theta1, theta2, theta3):
    # 计算三个主动臂末端位置
    B1 = compute_B(theta1, 0°)
    B2 = compute_B(theta2, 120°)
    B3 = compute_B(theta3, 240°)
    
    # 构建三个球面方程
    # (x - B1x)² + (y - B1y)² + (z - B1z)² = L₂²
    # ... 类似另外两个
    
    # 消去x, y，解出z
    z = solve_quadratic(...)
    
    # 回代求x, y
    x, y = back_substitute(z)
    
    return (x, y, z)

你想想看，如果正运动学算不准，逆运动学肯定也偏，抓取精度就别提了。所以这一步一定要验证：用已知的末端位置反算角度，再正算回来，看误差是否在允许范围内。

2.4 知识体系总览

为了让你更直观地理解本章的知识结构，我画了一张图：

这张图把本章的三个核心模块串起来了。空间坐标系是基础，齐次变换是工具，正运动学是目标。三者缺一不可。

2.5 本章小结

这一章我们讲了：

• 空间坐标系的三种类型：世界、工具、关节
• 齐次变换矩阵的结构和乘法规则
• 正运动学的几何推导方法和代码实现

说实话，运动学是Delta机器人控制的“内功”，内功不扎实，招式再花哨也没用。我建议你学完这一章后，自己动手写一个正运动学函数，用已知数据验证一下。嗯，这样印象才深刻。

下一章，我们会讲逆运动学——也就是从位置反算角度。那是抓取控制的关键，敬请期待。

---

公众号:蓝海资料掘金营，微信deep3321