机械臂末端位姿计算：正运动学与齐次变换从零推导

1. 位姿是什么：位置 + 姿态 = 6 个自由度

机械臂末端在空间里的完整状态叫位姿（Pose），拆两半：

位置（Position）：末端坐标系原点在基坐标系下的三维坐标 (x, y, z)——3 个自由度；
姿态（Orientation）：末端坐标系相对基坐标系"转了多少"——3 个自由度（绕谁转、转多少）。

合起来 6 自由度。光有位置不够：末端到了同一点，工具朝向可以完全不同（抓杯子和抓杯把）；光有姿态也不够——同一朝向可以在不同位置。所以控制器里的"目标点"永远是位置 + 姿态的完整描述。

为什么要先算位姿

正运动学（FK）是逆运动学（IK）、轨迹规划、动力学、奇异分析的共同输入——所有上层都问"末端现在哪、朝哪"。把 FK 算对，后面才有得谈。

2. 数学地基：旋转矩阵与齐次变换矩阵

两个坐标系之间的刚体变换 = 旋转 + 平移：

旋转矩阵 R(3x3)：正交阵，R⁻¹ = Rᵀ，det(R)=+1
绕 X/Y/Z 轴旋转 θ 的基本矩阵：
    ⎡1   0     0  ⎤        ⎡cosθ  0  sinθ⎤        ⎡cosθ -sinθ 0⎤
Rx= ⎢0  cosθ -sinθ⎥   Ry= ⎢  0   1   0 ⎥   Rz= ⎢sinθ  cosθ 0⎥
    ⎣0  sinθ  cosθ⎦        ⎣-sinθ 0  cosθ⎦        ⎣  0     0   1⎦

齐次变换矩阵 T(4x4)：把旋转和平移装进一个矩阵
        ⎡      ⎤
        ⎢  R   t⎥      R: 3x3 姿态   t: 3x1 平移
    T = ⎢      ⎥
        ⎣ 0 0 0 1⎦      (0 0 0 1): 透视行，保证矩阵可逆

   ⎡x'⎤       ⎡x⎤
   ⎢y'⎥ = T · ⎢y⎥      —— 点从"局部坐标"变成"基坐标"
   ⎢z'⎥       ⎢z⎥
   ⎣1 ⎦       ⎣1⎦
            

链式法则（关键）：从基坐标系到末端，经过一串中间坐标系，总变换就是逐级矩阵左乘：

T_base_end = T_base_1 · T_1_2 · T_2_3 · … · T_{n-1}_end

顺序不能换：矩阵乘法不交换。T_base_1 先把基系带到关节1，
再乘 T_1_2 带到关节2……一路带到末端。*/
            

这就是正运动学的全部骨架——剩下的事就是"怎么写出每一级 T"。D-H 参数法就是干这个的。

3. 姿态表示法：旋转矩阵 / 欧拉角 / 四元数

同一姿态三种记法，各有各的用场：

表示法	形式	优点	缺点
旋转矩阵	3×3 正交阵	计算直接、链式连乘方便	9 个数冗余，不直观
欧拉角 (ZYX)	(α, β, γ)	人可读、示教器显示用	顺序依赖、万向锁（β=±90° 丢自由度）
四元数	(w, x, y, z)	无万向锁、插值平滑、紧凑	不直观、需归一化

ZYX 欧拉角约定（工业机械臂最常用，即 RPY 滚转-俯仰-偏航）：

R = Rz(α) · Ry(β) · Rx(γ)

先绕 X 轴转 γ（Roll），再绕新 Y 轴转 β（Pitch），
再绕新 Z 轴转 α（Yaw）——"外到内"逐级左乘。

工程习惯：
  α = Yaw（偏航，绕重力轴）——示教器上的 A
  β = Pitch（俯仰）
  γ = Roll（滚转）*/
            

四元数到旋转矩阵（单位四元数 w²+x²+y²+z²=1）：

R = ⎡1-2(y²+z²)   2(xy-wz)    2(xz+wy) ⎤
    ⎢ 2(xy+wz)   1-2(x²+z²)   2(yz-wx) ⎥
    ⎣ 2(xz-wy)    2(yz+wx)   1-2(x²+y²)⎦*/
            

位姿计算的工程结论：内部运算用矩阵/四元数（稳），对外显示用欧拉角（可读）——所以第 6 节要从矩阵"提"回欧拉角。

4. 正运动学：D-H 参数法四步建系

D-H（Denavit-Hartenberg）把相邻连杆的变换压缩成 4 个参数：

参数	含义	变量类型
θᵢ（theta）	绕 zᵢ₋₁ 轴从 xᵢ₋₁ 转到 xᵢ 的角度	旋转关节 = 关节变量
dᵢ（d）	沿 zᵢ₋₁ 轴平移的距离	棱柱关节 = 关节变量
aᵢ（a）	沿 xᵢ 轴平移的距离（连杆长度）	常量（结构）
αᵢ（alpha）	绕 xᵢ 轴从 zᵢ₋₁ 转到 zᵢ 的角度（连杆扭角）	常量（结构）

相邻连杆变换矩阵（标准 DH）：

     ⎡cosθᵢ  -sinθᵢ·cosαᵢ   sinθᵢ·sinαᵢ   aᵢ·cosθᵢ⎤
Aᵢ = ⎢sinθᵢ   cosθᵢ·cosαᵢ  -cosθᵢ·sinαᵢ   aᵢ·sinθᵢ⎥
     ⎢  0        sinαᵢ        cosαᵢ         dᵢ     ⎥
     ⎣  0          0            0            1     ⎦

从"上一个坐标系"到"这个坐标系"：
  先绕 zᵢ₋₁ 转 θᵢ → 沿 zᵢ₋₁ 平移 dᵢ → 沿 xᵢ 平移 aᵢ → 绕 xᵢ 转 αᵢ
（记住口诀：转 z、移 z、移 x、转 x —— 名字全在 D-H 表里）*/
            

拿到 DH 表（厂商手册里给的就是这张表），末端位姿就是机械连乘：T_0n = A₁·A₂·…·Aₙ。正运动学本身没有任何魔法——查表、代公式、连乘。

5. 实例：6 轴机械臂 DH 表与逐关节连乘

以常见 6R 垂直关节构型（UR5 类）为例，标准 DH 表（单位：mm/deg，示意参数）：

关节 i	θᵢ（变量）	dᵢ	aᵢ	αᵢ
1	θ₁	d₁ = 89.2	a₁ = 0	α₁ = 90°
2	θ₂	d₂ = 0	a₂ = -425	α₂ = 0°
3	θ₃	d₃ = 0	a₃ = -392.25	α₃ = 0°
4	θ₄	d₄ = 109.15	a₄ = 0	α₄ = 90°
5	θ₅	d₅ = 94.65	a₅ = 0	α₅ = -90°
6	θ₆	d₆ = 82.3	a₆ = 0	α₆ = 0°

T_06 = A₁(θ₁)·A₂(θ₂)·A₃(θ₃)·A₄(θ₄)·A₅(θ₅)·A₆(θ₆)

计算流程（C 里就是循环 6 次矩阵乘）：
  T = I
  for i in 1..6:
      T = T · A_i(θᵢ, dᵢ, aᵢ, αᵢ)
  结果 T 的右上 3 列 = 末端位置 (x, y, z)
  结果 T 的左上 3×3 = 末端姿态 R
            

注意 α₁=90° 和 α₅=-90° 两个扭角——它们让关节 2/3 的转轴从"垂直"翻到"水平"，这是 6R 构型能全向到达的关键。DH 表里的符号（a₂=-425 负值）是构型约定的结果，照表代公式即可，不要"顺手改正"。

6. 从矩阵到人可读：提取 ZYX 欧拉角

连乘得到的是 3×3 矩阵，但示教器/调试器要显示 (α, β, γ)。提取公式（ZYX 约定，R 的第 2 行第 1 列起手）：

设 R = ⎡r11 r12 r13⎤
        ⎢r21 r22 r23⎥
        ⎣r31 r32 r33⎦

β = atan2(-r31, √(r11² + r21²))     —— Pitch
α = atan2(r21, r11)                 —— Yaw
γ = atan2(r32, r33)                 —— Roll

β = ±90° 时（万向锁）：r11=r21=0，
    √ 项为 0 → 退化，只能取 α=0 并解 γ=atan2(r12, r22)*/
            

三个工程坑：

必须用 atan2 不要 atan：atan(y/x) 分不清象限（(-1,-1) 和 (1,1) 都会给 45°），位姿错 180° 就是这么来的；
提取公式和正运动学必须同一约定：ZYX 还是 XYZ、先乘哪个，手册说哪个就用哪个，混用结果全错；
万向锁不是 bug 是几何事实：β→±90° 时偏航和滚转共轴，任何表示法都会丢信息——轨迹规划时绕开奇异区即可。

7. 逆运动学：一句话定位

正运动学是"关节角 → 位姿"（唯一、直接）；逆运动学是"位姿 → 关节角"（多解、复杂）。解法两类：

解析法：利用构型几何（Pieper 准则：后三轴交于一点，多数 6 轴商用臂满足）直接解出闭式表达式——快、解可分类；
数值法：雅可比迭代（阻尼最小二乘），通用但慢、依赖初值。

IK 的输入正是 FK 的输出（位姿矩阵）——所以本文的 T_06 表达是 IK 解算的起点。IK 多解需要按"关节限位 / 最小动作 / 避障"选一组，那是另一篇的事。

8. 工具坐标系与基坐标系：法兰 → TCP

控制器报的末端位姿通常是法兰盘（第 6 轴）的位姿，但实际干活的是工具（TCP）——夹爪尖、焊枪口、吸盘面。两者差一个固定偏移：

T_base_TCP = T_base_flange · T_flange_TCP

T_flange_TCP = 工具安装偏移（出厂标定 / 示教四点法标定）

示例：法兰沿自身 Z 轴往前 200mm 装夹爪
        ⎡1 0 0   0⎤
        ⎢0 1 0   0⎥
T_flange_TCP = ⎢0 0 1 200⎥
        ⎣0 0 0   1⎦

同理可在基座前加"用户坐标系" T_user：
T_user_TCP = T_user_base · T_base_flange · T_flange_TCP*/
            

这就是为什么同一台臂、换个工具，目标位姿全要重算——换的是中间的 T_flange_TCP。视觉抓取的"相机→基座→TCP"标定链，本质就是这些矩阵的级联求逆。

9. 可编译 C 代码：DH 连乘 + 欧拉角提取

完整可编译的正运动学骨架（标准 DH，6 轴）：

/* arm_fk.c —— 6R 机械臂正运动学：关节角 → 末端位姿矩阵 + ZYX 欧拉角 */
#include <stdio.h>
#include <math.h>

typedef struct { double m[4][4]; } Mat4;

/* 标准 DH 相邻连杆变换：转z→移z→移x→转x */
static Mat4 dh(double th, double d, double a, double al) {
    Mat4 T = {0}; double c = cos(th), s = sin(th);
    double ca = cos(al), sa = sin(al);
    T.m[0][0]=c;    T.m[0][1]=-s*ca; T.m[0][2]= s*sa; T.m[0][3]=a*c;
    T.m[1][0]=s;    T.m[1][1]= c*ca; T.m[1][2]=-c*sa; T.m[1][3]=a*s;
    T.m[2][1]=sa; T.m[2][2]=ca;    T.m[2][3]=d;
    T.m[3][3]=1;
    return T;
}
static Mat4 mul(Mat4 A, Mat4 B) {  /* T = A·B */
    Mat4 R = {0};
    for (int i=0;i<4;i++)
        for (int j=0;j<4;j++)
            for (int k=0;k<4;k++) R.m[i][j]+=A.m[i][k]*B.m[k][j];
    return R;
}

/* 6R 正运动学（UR5 类示意 DH，d/a 单位 mm，θ 单位 rad）*/
int main(void) {
    double th[6] = {0, -M_PI/3, M_PI/6, 0, M_PI/4, 0};  /* 示例关节角（避开万向锁）*/
    double d[6]  = {89.2, 0, 0, 109.15, 94.65, 82.3};
    double a[6]  = {0, -425, -392.25, 0, 0, 0};
    double al[6] = {M_PI/2, 0, 0, M_PI/2, -M_PI/2, 0};
    Mat4 T = {{{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}};
    for (int i=0;i<6;i++) T = mul(T, dh(th[i], d[i], a[i], al[i]));

    /* 位置：右上列；姿态：ZYX 欧拉角提取 */
    double x=T.m[0][3], y=T.m[1][3], z=T.m[2][3];
    double pitch = atan2(-T.m[2][0], hypot(T.m[0][0], T.m[1][0]));
    double yaw   = atan2(T.m[1][0], T.m[0][0]);
    double roll  = atan2(T.m[2][1], T.m[2][2]);
    printf("位置: x=%.1f y=%.1f z=%.1f mm\n", x, y, z);
    printf("姿态: yaw=%.1f° pitch=%.1f° roll=%.1f°\n",
           yaw*180/M_PI, pitch*180/M_PI, roll*180/M_PI);
    return 0;
}
/* 编译: gcc arm_fk.c -lm -o arm_fk && ./arm_fk */
            

这段代码就是工业 FK 库的骨架——加个"多组关节角循环"就是轨迹点批量计算，加个 T_flange_TCP 左乘就是 TCP 位姿。

10. 交互演示：平面 3 关节位姿计算器

拖三个关节角滑块，看末端位置和姿态实时变化——这就是正运动学：关节角进，位姿出。连杆长 L₁=250 / L₂=200 / L₃=120（示意），姿态角 φ = θ₁+θ₂+θ₃：

平面 3 关节机械臂 · 正运动学位姿计算器

θ₁ (-150°~150°)

θ₂ (-150°~150°)

θ₃ (-150°~150°)

红色箭头 = 末端姿态方向，φ = θ₁+θ₂+θ₃；读数同时给笛卡尔坐标与姿态角——同一套 FK 公式在 3 关节平面版和 6 轴空间版的差别只是"矩阵多乘几级"。
试试 θ₂ 从 -60° 拉到 -150°：末端从"够得着"变"折回来"，位置坐标连续变化——正运动学永远有唯一解，这正是它比逆运动学好验证的原因。

11. 常见坑与工程建议

坑	后果	对策
atan 代替 atan2	姿态错 180°（象限丢失）	统一 atan2(y, x)，永远不用 atan(y/x)
欧拉角顺序混用	ZYX 和 XYZ 结果全错	正运动学、提取、示教器三者同一约定
标准 DH vs 修正 DH 混用	参数对不上，末端乱飞	手册用哪种就跟哪种，查表不猜
角度单位混用	度数当弧度，位置错几倍	DH 表用弧度；显示用度；入口处统一转换
四元数没归一化	矩阵不正交，累积漂移	每次运算后 w²+x²+y²+z² 归一化
万向锁附近取欧拉角	角度跳变、插值异常	内部用四元数；轨迹避开奇异区
忘了 TCP 偏移	报的位姿和实际工具尖不符	T_base_TCP = T_base_flange × T_flange_TCP（第 8 节）

验证位姿算得对不对

两条捷径：① 零位自检——全部关节角置 0，按几何直觉估算末端坐标（如第 5 节 DH 表零位时 z = d₁+d₄+d₅+d₆），对不上就是 DH 表或符号问题；② 与示教器对照——实际臂摆几个姿态，对比读数。第 5 节表格 + 第 9 节代码可以直接当测试基准。

一句话收尾：正运动学 = DH 表 + 齐次变换连乘 + 姿态提取——没有高深数学，全是"查表、代公式、守约定"。算对位姿，机械臂控制的万里长征就迈出了第一步。