4、运动学基础：正运动学与逆运动学，以SCARA机器人为例

各位同学，今天我们聊点硬核的——运动学。

说实话，很多搞多轴控制的工程师，一听到「运动学」三个字就头大。觉得那是搞算法的人的事，自己只要会调PID就够了。我以前也这么想，直到有一次在产线上调试一台SCARA机器人，死活走不准直线。折腾了两天，最后发现是逆运动学解算里一个符号搞反了。嗯，从那以后，我再也不敢小看运动学了。

4.1 什么是正运动学？

正运动学，说白了就是：已知关节角度，求末端位置。

你给机器人每个关节发一个角度指令，它末端到底在哪儿？这就是正运动学要回答的问题。

我个人习惯把正运动学比作「顺藤摸瓜」。你从基座开始，沿着每个关节的连杆一路算下去，最后就能摸到末端执行器的坐标。

4.2 SCARA机器人的正运动学推导

SCARA机器人有四个关节：两个旋转关节（J1、J2），一个垂直移动关节（J3），还有一个末端旋转关节（J4）。但做位置运动学时，我们主要关心前三个关节。

先看个简单的结构图：

上图展示的是SCARA的简化模型。J1和J2是旋转关节，J3是垂直移动关节。末端位置由三个参数决定：θ1、θ2和d3。

正运动学公式其实很简单：

// SCARA正运动学
x = L1 * cos(θ1) + L2 * cos(θ1 + θ2)
y = L1 * sin(θ1) + L2 * sin(θ1 + θ2)
z = d3  // 注意：d3是垂直偏移量，通常向下为正

你看，就是两个圆的叠加。L1是第一个连杆长度，L2是第二个。θ1和θ2加起来就是第二个连杆相对于基座的角度。

4.3 逆运动学——这才是难点

逆运动学正好反过来：已知末端位置，求关节角度。

你告诉机器人「我要去那个点」，它得自己算出每个关节该转多少度。这才是实际控制中最常用的。

为什么说逆运动学难？因为同一个末端位置，可能对应多组关节角度。这就是所谓的「多解问题」。

4.4 SCARA逆运动学推导

SCARA的逆运动学相对简单，因为它只有两个旋转关节，而且都在一个平面内。

推导过程分三步：

1. 先算θ2——利用余弦定理
2. 再算θ1——利用几何关系
3. 最后算d3——直接赋值

公式如下：

// SCARA逆运动学
// 输入：目标位置 (x, y, z)
// 输出：关节角度 (θ1, θ2, d3)

// 第一步：计算θ2
cosθ2 = (x² + y² - L1² - L2²) / (2 * L1 * L2)
θ2 = ±acos(cosθ2)  // 注意：有正负两个解！

// 第二步：计算θ1
k1 = L1 + L2 * cosθ2
k2 = L2 * sinθ2
θ1 = atan2(y, x) - atan2(k2, k1)

// 第三步：计算d3
d3 = z  // 直接赋值

4.5 避坑指南——我踩过的那些坑

我曾经在调试一台SCARA时，发现机器人偶尔会「抽风」——末端突然弹到另一边。查了半天，发现是逆运动学解算时，没有处理奇异点。

什么是奇异点？就是当目标位置在机器人工作空间边界时，某些关节角度会变得不确定。比如SCARA的J1和J2完全伸直时，θ2=0，这时候cosθ2=1，但sinθ2=0，导致k2=0，atan2计算会出问题。

我的处理方法是：

• 在代码里加一个阈值判断，当|cosθ2| > 0.999时，强制使用上一个周期的θ2值
• 或者干脆限制工作空间，不让机器人走到边界附近

4.6 代码实现——从理论到实践

下面是我在实际项目中用过的SCARA运动学代码片段。注意，我加了防抖处理和边界检查。

// SCARA逆运动学 - 实际项目代码
typedef struct {
    float theta1;
    float theta2;
    float d3;
} JointAngles;

JointAngles scara_inverse_kinematics(float x, float y, float z, 
                                      float L1, float L2,
                                      float prev_theta2) {
    JointAngles result;
    float cos_theta2, sin_theta2;
    float k1, k2;
    float theta2_candidate[2];
    float dist_sq = x*x + y*y;
    
    // 边界检查
    float max_reach = L1 + L2;
    float min_reach = fabs(L1 - L2);
    if (dist_sq > max_reach*max_reach || dist_sq < min_reach*min_reach) {
        // 超出工作空间，返回错误
        result.theta1 = 0;
        result.theta2 = 0;
        result.d3 = 0;
        return result;
    }
    
    // 计算θ2
    cos_theta2 = (dist_sq - L1*L1 - L2*L2) / (2 * L1 * L2);
    
    // 防奇异点处理
    if (cos_theta2 > 0.999f) cos_theta2 = 0.999f;
    if (cos_theta2 < -0.999f) cos_theta2 = -0.999f;
    
    sin_theta2 = sqrt(1 - cos_theta2*cos_theta2);
    
    // 两个解：正和负
    theta2_candidate[0] = atan2(sin_theta2, cos_theta2);   // 肘部朝下
    theta2_candidate[1] = atan2(-sin_theta2, cos_theta2);  // 肘部朝上
    
    // 选择离上一周期最近的解
    float diff0 = fabs(theta2_candidate[0] - prev_theta2);
    float diff1 = fabs(theta2_candidate[1] - prev_theta2);
    result.theta2 = (diff0 < diff1) ? theta2_candidate[0] : theta2_candidate[1];
    
    // 计算θ1
    k1 = L1 + L2 * cos(result.theta2);
    k2 = L2 * sin(result.theta2);
    result.theta1 = atan2(y, x) - atan2(k2, k1);
    
    // d3直接赋值
    result.d3 = z;
    
    return result;
}

4.7 运动学在控制中的实际应用

你可能会问：知道了正逆运动学，然后呢？

在实际的多轴协调控制中，运动学是基础中的基础。比如你要让SCARA画一个圆，你不能直接给末端发坐标，得先把圆轨迹离散成一系列点，然后对每个点做逆运动学解算，得到关节角度序列，再发给伺服驱动器。

我个人的工作流程是这样的：

1. 上位机规划轨迹（比如直线、圆弧、样条曲线）
2. 插补生成密集的位置点序列
3. 对每个点调用逆运动学，得到关节角度
4. 关节角度经过速度规划和加速度规划
5. 最后发给伺服驱动器执行

你看，运动学是整个链条的「翻译官」。没有它，你没法把笛卡尔空间的轨迹翻译成关节空间的动作。

4.8 小结

正运动学和逆运动学，说白了就是坐标变换。正运动学简单直接，逆运动学需要处理多解和奇异点。

SCARA因为结构简单，逆运动学有解析解，所以很适合作为入门案例。但如果你以后遇到六轴机器人，逆运动学就没这么简单了——那玩意儿通常需要数值解法，比如牛顿-拉夫森迭代法。

不过别担心，先把SCARA吃透，后面的事就好办了。

---

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