📚 课程目录第1章运动学基础回顾

1. 运动学基础回顾：从DH参数到正逆解，理解机器人运动的数学本质

做机器人标定，绕不开运动学。说白了，运动学就是描述机器人怎么动的一门学问。你给每个关节一个角度，末端执行器会跑到哪个位置？反过来，你想让末端去抓一个杯子，每个关节该转多少度？这两个问题，就是正解和逆解。

我刚开始做机器人时，觉得这东西太理论了，不就是矩阵乘来乘去嘛。直到有一次在现场调一个六轴协作臂，末端总是差那么两三毫米对不准工件……嗯，从那以后我才老老实实把DH参数表从头捋了一遍。

1.1 什么是DH参数？为什么它这么重要？

DH参数，全称Denavit-Hartenberg参数。它用四个参数来描述相邻两个连杆坐标系之间的变换关系。你想想看，一个六轴机器人有六个关节，七个连杆（包括基座和末端），每个相邻对之间都有一套DH参数。

参数	符号	物理含义	我的理解
连杆长度	a_i-1	沿x_i-1轴，从z_i-1到z_i的距离	说白了就是两个关节轴之间的最短距离
连杆扭角	α_i-1	绕x_i-1轴，从z_i-1到z_i的转角	两个关节轴之间的空间夹角
连杆偏距	d_i	沿z_i轴，从x_i-1到x_i的距离	关节沿自身轴线的偏移量
关节转角	θ_i	绕z_i轴，从x_i-1到x_i的转角	这就是我们实际控制的关节角度

这里有个坑，我必须要说。DH参数有两种主流约定：标准DH和改进DH。标准DH把变换矩阵写成 T = Rot_z(θ) · Trans_z(d) · Trans_x(a) · Rot_x(α)，而改进DH的顺序是 T = Rot_x(α) · Trans_x(a) · Rot_z(θ) · Trans_z(d)。我曾经在项目里混用过这两种，结果正解算出来末端位置差了十几厘米……排查了整整一个下午。

⚠️ 避坑指南： 我建议你在项目一开始就明确约定使用哪种DH约定，并在所有文档和代码中保持一致。混合使用标准DH和改进DH，是运动学建模中最常见的错误之一。

1.2 正解：从关节空间到笛卡尔空间

正解，就是已知六个关节角度 θ₁ ~ θ₆，求末端位姿 T₀⁶。计算过程很简单：把相邻连杆的变换矩阵连乘起来。

// 正解核心代码（伪代码）
Matrix4x4 forwardKinematics(float* theta) {
    Matrix4x4 T = I;  // 初始化为单位矩阵
    for (int i = 1; i <= 6; i++) {
        Matrix4x4 A = dhTransform(a[i-1], alpha[i-1], d[i], theta[i]);
        T = T * A;
    }
    return T;
}

正解是唯一的，也是确定的。你给一组关节角，末端位姿就只有一个。我在调试机器人时，经常先用正解验证DH参数表有没有写错——让所有关节归零，看末端是不是在理论上的“零位姿态”。

💡 调试技巧： 正解计算量很小，在STM32上跑完全没问题。我习惯在机器人上电初始化时，先做一次正解校验，如果末端位姿与理论值偏差超过0.1mm，就报错停机，防止带着错误参数运动。

1.3 逆解：从笛卡尔空间回到关节空间

逆解就麻烦多了。已知末端位姿 T₀⁶，求六个关节角。这个问题有多解性——同一个末端位姿，可能有8组甚至16组关节角解。

为什么会有多解？你想想看，一个六轴机器人，肘关节可以朝上也可以朝下，腕关节可以翻转也可以不翻转。每种组合都对应一组解。

逆解的常用方法有两种：

解析法（封闭解）：适用于满足Pieper准则的机器人（三个相邻关节轴交于一点）。速度快，精度高。
数值法（迭代法）：通用性强，但计算量大，可能有收敛问题。

我个人更偏爱解析法。在STM32上，解析法可以在几十微秒内完成一次逆解，而数值法可能要几百微秒甚至几毫秒。对于实时控制来说，这个差距很关键。

🔑 关键点： 逆解的多解选择策略很重要。我一般遵循“最小关节位移”原则——在所有可行解中，选择让每个关节运动量最小的那组。这样可以避免机器人出现大幅度的“甩臂”动作。

1.4 运动学知识体系总览

下面这张图，是我自己整理的运动学知识体系。每次带新人时，我都会先让他们看这张图，建立全局视角。

1.5 从理论到实践：标定为什么需要运动学？

你可能会问：既然正解逆解都是成熟的理论，为什么还要做标定？

原因很简单——理论模型和实际机器人之间有误差。DH参数表里写的 a=300mm，实际加工出来可能是 299.87mm。关节转角 θ 的零点偏移，装配时的微小变形，这些都会导致末端定位误差。

标定的本质，就是通过测量实际位姿，反过来修正DH参数中的微小误差。这个过程叫做“参数辨识”。

🔑 核心思路： 标定 = 运动学模型 + 测量数据 + 参数优化。没有扎实的运动学基础，标定就是空中楼阁。

我记得第一次做标定时，用的就是正解模型。让机器人走几十个不同的姿态，用激光跟踪仪测量末端实际位置，然后对比理论位置和实际位置的差值，用最小二乘法反推出DH参数的误差值。那次标定做完，机器人的绝对定位精度从±2mm提升到了±0.3mm。

1.6 在STM32上实现运动学计算的注意事项

STM32的资源有限，做运动学计算时要注意几点：

浮点运算：STM32F4系列有FPU，单精度浮点运算很快。我建议全程用float，不要用double，速度差一倍以上。
三角函数：sin/cos是计算瓶颈。可以预先计算好常用角度的三角函数值，或者用查表法加速。
矩阵运算：4x4矩阵乘法，用循环展开比嵌套循环快30%左右。

// STM32上优化的4x4矩阵乘法（循环展开）
void matrixMul4x4(float* C, float* A, float* B) {
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            C[i*4+j] = A[i*4+0]*B[0*4+j] +
                       A[i*4+1]*B[1*4+j] +
                       A[i*4+2]*B[2*4+j] +
                       A[i*4+3]*B[3*4+j];
        }
    }
}

💡 性能数据： 在STM32F407上，一次完整的六轴正解计算（6次DH变换矩阵连乘）大约需要12μs。逆解（解析法）大约需要45μs。这个速度完全满足1kHz的控制周期。

好了，运动学基础就回顾到这里。这些内容看起来简单，但它们是整个标定课程的基石。后面几章我们会深入DH参数的标定方法、测量设备的选择、以及如何在STM32上实现实时标定算法。每一步都离不开今天讲的这些内容。