📚 课程目录第1章机器人运动学基础

1. 机器人运动学基础：刚体在空间中的位姿描述

各位同学，咱们正式开始。第一章的内容，说白了就是回答一个问题：机器人手臂上的某个点，到底在哪儿？

你想想看，要让STM32控制电机转起来，首先得知道末端执行器（比如一个夹爪）相对于基座的位置和朝向。这就是刚体位姿描述要干的事。我当年刚入行时，觉得这不过是高中数学，结果在调试六轴机械臂时，因为旋转矩阵的顺序搞反了，电机直接撞限位——嗯，从那以后我再也不敢小看这章了。

1.1 位置与姿态：两个基本概念

刚体在空间中有6个自由度：3个平移（x, y, z）和3个旋转（绕x、y、z轴）。我们用一个位置矢量描述平移，用一个旋转矩阵描述朝向。

  核心公式： 齐次变换矩阵 T = [R  p; 0  1]，其中 R 是 3×3 旋转矩阵，p 是 3×1 位置矢量。

我个人习惯把 T 看作一个“打包盒”——把旋转和平移塞进一个 4×4 矩阵里，方便连续变换。在 STM32 上做运动学计算时，这个矩阵就是你的核心数据结构。

1.2 旋转矩阵的几何意义

旋转矩阵 R 的每一列，代表刚体坐标系的一个轴在参考坐标系中的投影。举个例子：

R 的第一列 = 刚体 x 轴在参考系中的方向余弦
R 的第二列 = 刚体 y 轴在参考系中的方向余弦
R 的第三列 = 刚体 z 轴在参考系中的方向余弦

为什么这样定义？因为旋转矩阵本质上是坐标变换。一个点 p 在刚体坐标系下的坐标，左乘 R 就得到它在参考系下的坐标。我在做视觉引导抓取项目时，相机坐标系和机械臂基坐标系之间的转换，就是靠这个原理。

1.3 旋转矩阵的性质

旋转矩阵有几个重要性质，写代码时经常用到：

性质	数学表达	实际意义
正交性	R^TR = I	旋转不改变向量长度
行列式为1	det(R) = 1	右手坐标系，无镜像
逆等于转置	R^-1 = R^T	求逆运算简化，节省MCU算力

避坑指南： 我曾经在STM32F4上直接用浮点矩阵求逆，结果算一次要2ms。后来发现旋转矩阵的逆就是转置，直接改成转置操作，耗时降到0.1ms。在嵌入式平台上，能用转置就别求逆。

1.4 基本旋转矩阵

绕单个坐标轴的旋转是最基本的构建块。绕 x、y、z 轴旋转 θ 角的矩阵如下：

// 绕X轴旋转
R_x(θ) = [1      0       0;
          0   cosθ   -sinθ;
          0   sinθ    cosθ]

// 绕Y轴旋转
R_y(θ) = [ cosθ   0   sinθ;
           0      1      0;
          -sinθ   0   cosθ]

// 绕Z轴旋转
R_z(θ) = [cosθ  -sinθ   0;
          sinθ   cosθ   0;
          0      0      1]

注意符号：绕Y轴旋转时，左上角的 sinθ 是负的。我刚开始总记混，后来在纸上画了个坐标系才搞明白。建议你也动手画一画。

1.5 齐次变换矩阵：把旋转和平移统一起来

单独用旋转矩阵只能处理绕原点的旋转。如果刚体既旋转又平移，就需要齐次变换矩阵：

T = [R   p;
     0   1]

// 其中 R 是 3×3 旋转矩阵
// p 是 3×1 平移向量
// 最后一行是 [0 0 0 1]

这个矩阵的妙处在于：连续变换可以左乘叠加。比如从基座到关节1是 T1，关节1到关节2是 T2，那么基座到关节2就是 T = T1 * T2。在STM32上实现正运动学，就是一连串的矩阵乘法。

注意： 矩阵乘法不满足交换律！T1 * T2 和 T2 * T1 结果完全不同。我曾经在调试SCARA机器人时，把变换顺序搞反了，末端位置差了十几厘米。记住：先发生的变换放在右边，后发生的放在左边。

1.6 本章知识体系

下面这张图帮你理清本章的核心逻辑：

1.7 在STM32上的实现要点

理论讲完了，咱们聊聊实际代码怎么写。在STM32上实现齐次变换，我建议用结构体：

typedef struct {
    float R[3][3];  // 旋转矩阵
    float p[3];     // 平移向量
} HomogeneousTransform;

// 初始化单位矩阵
void HT_Init(HomogeneousTransform *ht) {
    // 旋转矩阵设为单位阵
    for(int i=0; i<3; i++)
        for(int j=0; j<3; j++)
            ht->R[i][j] = (i==j) ? 1.0f : 0.0f;
    // 平移向量设为零
    for(int i=0; i<3; i++)
        ht->p[i] = 0.0f;
}

// 矩阵乘法：C = A * B
void HT_Multiply(HomogeneousTransform *C,
                 HomogeneousTransform *A,
                 HomogeneousTransform *B) {
    // 旋转部分：C.R = A.R * B.R
    for(int i=0; i<3; i++)
        for(int j=0; j<3; j++) {
            C->R[i][j] = 0;
            for(int k=0; k<3; k++)
                C->R[i][j] += A->R[i][k] * B->R[k][j];
        }
    // 平移部分：C.p = A.R * B.p + A.p
    for(int i=0; i<3; i++) {
        C->p[i] = A->p[i];
        for(int j=0; j<3; j++)
            C->p[i] += A->R[i][j] * B->p[j];
    }
}

性能提示： 在STM32F4上，上面的乘法函数大约耗时 2-3μs（开启FPU和-O2优化）。如果你的控制周期是1ms，这个开销完全可以接受。但如果你要做逆运动学迭代求解，建议用定点数或查表法加速。

1.8 常见误区与避坑

旋转顺序搞反： 先绕X再绕Y，和先绕Y再绕X，结果完全不同。我建议在代码注释里写明顺序，比如 "ZYX" 顺序。
角度单位混淆： 三角函数用的是弧度，不是角度。我见过有人传了90度进去，结果cos(90)算出来是0，但实际应该是cos(π/2)。
浮点精度问题： 连续乘多个旋转矩阵后，正交性会丢失。建议每隔一段时间做一次正交化：R = R * (R^TR)^-1/2。

曾经踩过的坑： 我在做六轴机械臂标定时，发现末端位置误差越来越大。查了两天，最后发现是旋转矩阵的浮点误差累积导致正交性丢失。从那以后，我每次做完正运动学计算，都会加一行代码做正交化修正。别小看这个细节，在长时间运行的机器人系统里，它可能救你一命。

好了，第一章的内容就到这里。记住：位姿描述是机器人运动学的基石。后面的正运动学、逆运动学、轨迹规划，全都建立在这个基础上。把齐次变换矩阵和旋转矩阵搞透，后面的路就好走了。