3. 基础数学工具回顾：向量与矩阵运算、三角函数、四元数与欧拉角基础、Sincos查表法优化

做运动控制，说白了就是跟数学打交道。很多同学一上来就啃四元数、旋转矩阵，结果代码跑起来全是坑。我个人习惯是先把基础工具磨利索，再上战场。这一章咱们就把那些绕不开的数学工具过一遍，重点放在“怎么用”和“别踩什么坑”上。

3.1 向量与矩阵运算：空间描述的基石

机械臂的每个关节位置、速度、力，本质上都是向量。而从一个坐标系到另一个坐标系的变换，就是矩阵乘法。我在调试六轴机器人时，80% 的 bug 都出在矩阵乘法的顺序上——左乘还是右乘，结果天差地别。

3.1.1 向量基础运算

• 点积（内积）： 结果是一个标量，用于计算投影、夹角。公式：a·b = |a||b|cosθ。在运动学里，判断两个向量是否垂直（点积为0）很常用。
• 叉积（外积）： 结果是一个向量，垂直于原两向量构成的平面。用于计算力矩、角速度方向。注意叉积不满足交换律：a×b = -(b×a)。
• 模长与归一化： 单位向量在方向控制中至关重要。归一化时记得判零，否则除零异常会让你的电机原地起飞。

3.1.2 矩阵运算与旋转矩阵

旋转矩阵是 3x3 的正交矩阵，行列式为 +1。说白了，它描述了一个坐标系相对于另一个坐标系的朝向。

运算类型	数学表达	运动学应用场景
矩阵乘法	C = A × B	连续坐标变换（链式法则）
矩阵转置	AT	旋转矩阵的逆（正交矩阵特性）
矩阵求逆	A-1	从工具坐标系回到基坐标系
齐次变换矩阵	4x4 矩阵	同时描述旋转 + 平移（DH 参数法）

3.2 三角函数：角度解算的命根子

运动学正解和逆解里，sin、cos、atan2 几乎是每行代码都在用。尤其是 atan2(y, x)，它比 atan(y/x) 好得多——能自动处理象限，避免除零。

3.2.1 常用三角函数关系

• sin²θ + cos²θ = 1： 用于归一化校验，或者从 sin 求 cos 时避免开方误差。
• 和差化积： 在傅里叶变换、滤波器设计里经常用到，但运动学里用得少，了解即可。
• atan2 的返回值范围： (-π, π]。注意和 atan 的 (-π/2, π/2) 区别开。我在做关节角度限位判断时，就因为用错了 atan 导致角度越界没检测到。

3.2.2 角度单位与归一化

STM32 的数学库默认用弧度。但很多编码器输出的是脉冲数或角度值。我建议在接口层统一转成弧度，内部运算全用弧度，只在最后输出给电机时再转回去。这样可以减少单位混淆的 bug。

3.3 四元数与欧拉角基础：绕开万向锁

欧拉角直观，但有个致命问题——万向锁。当两个旋转轴重合时，会丢失一个自由度。我在做云台控制时就遇到过：俯仰到 90° 时，航向和横滚突然乱跳。后来换成四元数，问题立刻解决。

3.3.1 欧拉角的三种常用顺序

顺序	表示	典型应用
ZYX	偏航 → 俯仰 → 横滚	无人机、机械臂末端姿态
ZYZ	偏航 → 俯仰 → 偏航	某些工业机器人腕部
XYZ	横滚 → 俯仰 → 偏航	IMU 姿态解算（常用）

你想想看，如果只用欧拉角做插值，中间姿态可能会绕远路。四元数就没有这个问题——它用四个数表示旋转，本质是在四维单位球面上做最短路径插值（Slerp）。

3.3.2 四元数核心公式

一个四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。单位四元数满足 w² + x² + y² + z² = 1。

• 旋转向量转四元数： 绕单位轴 u 旋转 θ 角：q = [cos(θ/2), u·sin(θ/2)]
• 四元数乘法： 对应旋转的复合。注意顺序：q1 * q2 表示先做 q2 旋转，再做 q1 旋转。
• 四元数转欧拉角： 公式略复杂，但网上有现成代码。我建议直接复制经过验证的版本，别自己推导——我试过，容易错。

3.4 Sincos查表法优化：把三角函数跑得更快

在 STM32 上调用 sinf() 和 cosf() 虽然方便，但每次调用要几十个微秒。如果你在 1kHz 的控制循环里算 6 个关节的 sin/cos，光三角函数就吃掉 30% 的 CPU。查表法可以把时间压到几微秒。

3.4.1 查表法原理

说白了，就是提前算好一批 sin 值存到数组里，运行时用角度查索引。比如把 0~2π 分成 1024 份，每份对应的 sin 值提前算好。查表时做一次线性插值，精度可以做到 0.1% 以内。

// 1024 点 sin 查表，角度范围 [0, 2π)
#define TABLE_SIZE 1024
float sin_table[TABLE_SIZE];

void init_sin_table(void) {
    for (int i = 0; i < TABLE_SIZE; i++) {
        sin_table[i] = sinf(2.0f * M_PI * i / TABLE_SIZE);
    }
}

float fast_sin(float angle) {
    // 将角度归一化到 [0, 2π)
    angle = fmodf(angle, 2.0f * M_PI);
    if (angle < 0) angle += 2.0f * M_PI;

    float idx = angle * TABLE_SIZE / (2.0f * M_PI);
    int i0 = (int)idx;
    int i1 = (i0 + 1) % TABLE_SIZE;
    float frac = idx - i0;

    // 线性插值
    return sin_table[i0] + frac * (sin_table[i1] - sin_table[i0]);
}

3.4.2 精度与性能权衡

表大小	内存占用	最大误差（近似）	单次调用时间（STM32F4 @168MHz）
256	1 KB	0.6%	~1.2 μs
1024	4 KB	0.04%	~1.5 μs
4096	16 KB	0.002%	~2.0 μs
标准 sinf()	—	双精度	~25 μs

3.4.3 同时计算 sin 和 cos 的技巧

很多算法需要同时用 sinθ 和 cosθ。如果分别查两次表，浪费了一半时间。可以利用三角恒等式：cosθ = sin(θ + π/2)。或者更直接——把 sin 和 cos 放在一个结构体里，一次查表算出两个值。

typedef struct {
    float sin_val;
    float cos_val;
} sincos_t;

sincos_t fast_sincos(float angle) {
    sincos_t result;
    // 先算 sin
    result.sin_val = fast_sin(angle);
    // cos 用查表 + 相位偏移
    result.cos_val = fast_sin(angle + M_PI_2);
    return result;
}

嗯，这里要注意：angle + M_PI_2 可能会超出表范围，记得在 fast_sin 里做归一化。我一般把归一化放在查表函数内部，这样调用方不用操心。