第四章 运动学模型调试：正运动学与逆运动学验证

运动学模型，说白了就是机器人的「骨架逻辑」。你给它一个角度，它告诉你手在哪儿——这是正运动学。你告诉它手要去哪儿，它反算出每个关节该转多少——这是逆运动学。这两者搞不定，后面的轨迹规划、力控制全是空中楼阁。

我个人习惯，拿到一个新机器人，第一件事不是跑轨迹，而是先验证运动学模型对不对。为什么？因为模型错了，后面所有调试都是白费功夫。我曾经在一个六轴机器人项目上，花了三天找轨迹抖动的原因，最后发现是DH参数里一个符号写反了……嗯，从那以后我再也不敢跳过运动学验证。

4.1 正运动学验证：从关节到笛卡尔

正运动学相对简单，但容易出细节问题。核心就是：给定一组关节角度，计算末端位姿。

验证方法我推荐「两步走」：

1. 零位验证：所有关节归零，末端应该在机械原点。如果不对，检查DH参数中的偏移量。
2. 单轴转动验证：只动一个关节，其他锁死。观察末端轨迹是否与预期一致（圆弧还是直线）。

举个例子，我调试过一个SCARA机器人。正运动学算出来末端位置是(300, 0, 150)，但实际用激光测距仪一打，发现Z轴差了2mm。查了半天，原来是末端执行器的安装高度没算进去。你想想看，这种低级错误，不实测根本发现不了。

// 正运动学验证代码片段（伪代码）
function forwardKinematics(jointAngles) {
    // 1. 根据DH参数构建变换矩阵
    T01 = dhTransform(alpha0, a0, d1, jointAngles[0]);
    T12 = dhTransform(alpha1, a1, d2, jointAngles[1]);
    // ... 依次构建
    
    // 2. 连乘得到末端位姿
    T06 = T01 * T12 * T23 * T34 * T45 * T56;
    
    // 3. 提取位置和姿态
    position = [T06[0][3], T06[1][3], T06[2][3]];
    rpy = rotationMatrixToRPY(T06);
    
    return {position, rpy};
}

4.2 逆运动学验证：从笛卡尔到关节

逆运动学才是真正的「坑」。为什么？因为多解、无解、奇异点全在这里爆发。

我建议的验证流程是这样的：

• 先正后逆：给一组关节角，算正运动学得到位姿，再用这个位姿算逆运动学。如果算出来的关节角跟原来一致，说明模型闭环了。
• 多解测试：同一个位姿，逆运动学应该返回多组解。比如六轴机器人，肘部在上和肘部在下，都是合法解。
• 边界测试：把末端推到工作空间边界，看逆运动学是否还能收敛。

我记得有一次，一个协作机器人项目，逆运动学在某个区域总是报错。排查了两天，发现是迭代算法的初始值用了上一时刻的关节角，而那个位置刚好靠近奇异点。后来改成多初始值并行计算，问题就解决了。

// 逆运动学验证：闭环测试
function inverseKinematicsTest() {
    // 1. 随机生成一组关节角
    q_random = [30°, -45°, 60°, 0°, 90°, 20°];
    
    // 2. 正运动学得到位姿
    pose = forwardKinematics(q_random);
    
    // 3. 逆运动学反算关节角
    q_solved = inverseKinematics(pose);
    
    // 4. 比较误差
    error = norm(q_random - q_solved);
    if (error < 1e-6) {
        print("闭环验证通过");
    } else {
        print("模型有误，请检查DH参数");
    }
}

4.3 关节空间与笛卡尔空间映射

这两个空间的映射，是运动学调试的核心。说白了就是：

• 关节空间：每个电机转多少度，直观但轨迹不可控
• 笛卡尔空间：末端走直线还是圆弧，直观但计算量大

实际项目中，我一般这样处理：

场景	推荐空间	原因
点到点移动	关节空间	计算快，无奇异风险
直线焊接/涂胶	笛卡尔空间	路径精度要求高
避障路径	混合使用	关节空间规划路径，笛卡尔空间修正

你可能会问：为什么不全程用笛卡尔空间？原因很简单——计算量。六轴机器人每秒要算几百次逆运动学，如果每次都迭代求解，控制器扛不住。关节空间只需要做线性插值，CPU负载低得多。

4.4 奇异点检测与规避

奇异点，是运动学调试里最让人头疼的问题。什么是奇异点？就是机器人某个方向失去自由度，你给末端一个微小速度，关节可能飞转。

常见的奇异点有三种：

1. 腕部奇异：关节4和关节6的轴线重合，末端无法绕某个轴旋转
2. 肩部奇异：关节1和关节2的轴线共面，手臂伸直时发生
3. 肘部奇异：关节3和关节5的轴线共线，肘关节打直

检测方法，我推荐用雅可比矩阵的条件数：

// 奇异点检测：基于雅可比矩阵条件数
function checkSingularity(jointAngles) {
    J = computeJacobian(jointAngles);  // 计算6x6雅可比矩阵
    cond = conditionNumber(J);         // 计算条件数
    
    if (cond > 1000) {
        print("警告：接近奇异点，条件数 = " + cond);
        return true;
    } else if (cond > 100) {
        print("提示：条件数偏高，建议减速");
        return false;
    } else {
        return false;
    }
}

规避策略，我总结了三招：

• 路径重规划：检测到奇异点，绕道走，不硬闯
• 速度降级：接近奇异点时，降低运动速度，给控制器更多时间处理
• 关节空间过渡：在奇异点附近，从笛卡尔空间切回关节空间，绕过危险区域再切回来

嗯，这里要注意：奇异点规避不是「避开就完事」，而是要平滑过渡。否则机器人会突然变速，造成冲击。

4.5 本章知识体系

下面这张图，是我自己总结的运动学调试核心逻辑。你照着这个流程走，基本不会漏掉关键步骤。

这张图从左到右，从上到下，就是运动学调试的完整链路。你调试的时候，按这个顺序走一遍，基本能覆盖90%的问题。