正运动学基础：从关节角算出指尖位姿

00先认识一下这个名字

你在前面两个实验里已经让机械臂动起来了。URDF 交代了它长什么样子，串口协议让它真的转起来。可是有个问题一直悬着：五个舵机各自转了一个角，指尖现在到底在哪？

回答这个问题的学问，叫做 Forward Kinematics，中文一般译成「正运动学」，平时大家直接叫它 FK。它的意思很直白：关节转了多少，是已知的；末端在哪、朝着哪，是要求的。

叫「正」，是因为顺序和人的习惯一样：先把胳膊摆成某个姿势，再去看手落在哪。反过来那个问题你也一定想过：如果我想让手够到桌上的杯子，各关节该转多少？那是另一个学问，叫逆运动学（IK），下一章才讲。这一章我们只顺着来。

那"在哪、朝着哪"具体是几个数

新手常犯的一个误会，是以为算出 x、y、z 三个坐标就算完事了。其实不够。想想你伸手去拿杯子：手放在杯子的位置，但掌心朝上还是朝下，结果完全不同。所以完整的答案有两部分：

两个加起来 6 个数，行内把这一整套叫「位姿」（pose）。后面讲到最后你会发现，光有位置没有姿态，夹爪是抓不住东西的。

要动用的数学，先看一眼全貌

下面这张表你现在不用全懂，混个眼熟就行。之所以提前列出来，是想让你心里有个数：这一章不会突然冒出一堆没见过的符号。

顺便说几个这里故意不讲的东西：叉积、点积、雅可比、四元数。不是它们不重要，是这一章用不上。叉积只在判断"正转是往哪边"时提一句右手定则，不写公式。等你学到 IK 和轨迹规划，它们自然会回来找你。

01先看清这台机器长什么样

要算指尖，得先知道身上有几个能转的地方。我们用一个教学模型 simple-arm.urdf 来说明，它和真机的关节链条、转轴方向完全一样，只是把真机上那些几毫米的装配偏差抹平了，方便你用计算器算。

它有五个关节，五根连杆。别看数量不少，分工其实很清楚。左边这张拆解图说明每个关节管什么，右边是同一台机器人按 simple-arm.urdf 加载出来的真模型，两边可以对着看：

左边是拆解，右边是实体，说的是同一台机器。你要是对不上号，就在右边转一转，找到那个关节——比如把视角转到侧面，能看到 J2、J3、J4 那三根轴确实指向同一个方向。

关节数、每根连杆多长，这些都是 URDF 文件里写死的，不用你去量。真正需要你动脑子的是：这些关节一起转起来，指尖会跑到哪。

怎么学：七个小台阶

这事一次说完会很难消化，所以下面拆成七个台阶。前面几级你连矩阵都不用碰，拿个计算器就能算；等手算彻底算通了，再回过头用矩阵把同样的过程重写一遍，那时候你会觉得矩阵是来帮忙的，而不是来为难你的。

图 1-2｜七级台阶怎么排队。第 1~3 级先把手算打通，第 4 级你自己就能算出完整位姿，走到这儿你就真的懂了。第 5、6 级把同一套计算换成矩阵重写一遍，答案不变，但只有这样程序才能算——真机和 ROS 里跑的就是这两级。第 7 级上真机核对。两条都得走。

几个会反复出现的词

这些词下面会一直出现，先花半分钟过一遍，不认识也没关系，看到后面自然就懂了。

顺带把坐标方向定下来

算坐标之前得先说好，x、y、z 分别指哪边。不然你算出来的数跟别人的对不上。你可以想象自己站在小车正后方，面朝车头看：

这套约定是 ROS 官方定下的（REP-103），simple-arm.urdf 里每个 link 的 rpy 都是 0 0 0，老老实实照它来，所以不会出现某个关节反着转的情况。

02台阶一：先只算一根棍子

七个台阶里最简单的就是这个。把整台机器先忘掉，只留一根棍子：一端固定在 J2，长度 L = 0.126 米，绕 J2 转了 θ 度。它的末端落在哪？

你会直觉地觉得"就是斜着的那个点"，但要写成分量，其实只需要回答两个数：往前伸了多少，往上抬了多少。

这两个式子不是背下来的，你把棍子、水平线、竖直线画在一起，就围出了一个直角三角形。棍子是斜边，θ 是从竖直那边量起来的角，那么"往前"那条是对边，"往上"那条是邻边。sin 和 cos 的定义就是这么来的，一代进去就是这个结果。

到这里你已经学会一件事：斜着伸出去的距离，可以拆成"往前"和"往上"两份，分别用 sin 和 cos 算。还不需要向量，更不需要矩阵，一个计算器足够了。

拿个题练一下手：换成一根 0.2 米的棍子，转到 60 度，末端比起点往前伸了多少、往上抬了多少？（往前 0.173 米，往上 0.100 米）

03台阶二：往哪边伸，由 J1 说了算

台阶一我们只在一个竖直平面里算。可是机械臂会转圈，J1 就是干这个的。它有个特点：不改变你伸出去多远，只改变你朝哪个方向伸。

你可以站在地上往下看。台阶一算出来的那个前伸量 r，就像一根平躺在地面上的棍子。J1 转 q1，就是在水平面上把这根棍子拨了一下：

到这儿，位置的三个数就凑齐了。不过先别高兴太早，这三个数是"每个角度各算一遍、再加起来"得到的，换个姿态就得从头再算，很费事。这个麻烦留到台阶五解决。

为什么 J2、J3、J4 只在竖直平面里动

因为它们三根轴都指向同一个方向（都是 Y）。绕同一个方向的轴转，末端只会在一个垂直于这根轴的平面里移动，也就是 X-Z 那个竖直平面。这解释了我们为什么要先算平面内的结果（台阶一），最后才套上航向（台阶二）。两件事可以分开算，但不能把顺序搞反。

04台阶三：三节接起来，角度要累加

把大臂、小臂、工具段接上。这里几乎每个人都会犯同一个错，先给你提个醒：

大臂先转了 q2，小臂再转 q3 的时候，它可不是从竖直方向开始转 q3，而是在大臂已经转过去的基础上再转 q3。所以小臂真正相对竖直方向的倾角是 q2 + q3，写成 q3 就错了。

拿 simple-arm.urdf 里的真实长度来算（单位是米）：

拿笔自己算一遍

J2 离地 0.1295 加 0.0595，也就是 0.189 米，我们从这里出发往上累加。关节角取默认那一组：q2 = −50°，q3 = 105°，q4 = 35°。

算完了，指尖落在 (x, y, z) = (0.157, 0.000, 0.342) 米。这个数字先记着，等会儿程序的输出、3D 页面的读数、你拿卷尺量的结果，都要跟它对上。

还有个细节值得停下来看一眼：最后那段工具"往上"的量是 0。因为累计角刚好 90 度，工具段完全躺平了，一点竖直分量都没有。这看着像巧合，其实马上就要用到。

05台阶四：位置有了，还差"朝着哪"

位置算完了。但前面说过，只说指尖在哪是不完整的。指尖是戳着、躺着还是平着，决定了夹爪能不能真正夹到东西。

这台机器上，姿态就是一个数

因为 J2、J3、J4 绕的是同一根轴，它们的角度可以一路加下去。加出来的那个数，就是工具段相对竖直方向歪了多少：

这 3 个数（俯仰、航向、自转）合起来，课本上叫欧拉角。名字听着唬人，其实就是"用三个角描述朝向"这件事，你刚才已经算出来了。

到这一步，位置 (0.157, 0, 0.342) 和姿态（倾角 90°、航向 q1、自转 q5）都有了。你能用手算把它算出来了。

但手算只能应付考试，程序得替你做这件事。下面两级把同一套计算换成矩阵重写一遍。答案不变，可只有这样才写得成代码，真机和 ROS 里跑的就是那一套。

那为什么还要学矩阵

刚才那套手算有两个问题。一是每换一组角度，三个式子都得重新算一遍，算一百组就是三百遍。二是我们偷偷做了个简化：算的时候把"转"和"挪"拆开处理，先转角度再加上长度。可真实情况是每一节都在转完马上挪，转和挪交替着来，手写公式很快就乱成一团。

下一页开始的两个台阶就是来解决这个的。把"转"写成一个矩阵，再把"转"和"挪"打包成 4×4，之后就像串珠子一样一路乘下去，姿态换了也不用手忙脚乱。

06台阶五：用矩阵把"转"打包

先只处理"转"这一件事。台阶一里我们把"斜着伸"拆成了两个式子，每个姿态都要写一遍。能不能写成一个固定模板，把角度套进去就出结果？

能。绕着 Y 轴转 θ 角的模板长这样：

R(θ) = [  cos θ   sin θ ]
       [ −sin θ   cos θ ]

别看这两行，它作用在一个点 (x, z) 上，效果就是把这个点绕原点转 θ。公式先别硬背，拖一下下面这个滑块，看那个红点转起来就明白了。

省事在哪儿

三段接起来的时候，每段的累计角不一样，但用的是同一个模板，只是把 θ 换成不同的值：

第 1 段：v1 = R(q2)      · (0, L1)
第 2 段：v2 = R(q2+q3)   · (0, L2)
第 3 段：v3 = R(q2+q3+q4)· (0, L3)
指尖 = 起点 + v1 + v2 + v3

这样写，换姿态只需要改代入的角度，不用重新推公式。而且如果你把两个矩阵乘起来，会发现一件有意思的事：R(30度) 乘 R(40度) 等于 R(70度)。矩阵相乘就是角度相加，这正是台阶三说的"角度会累加"，只是换了个说法。

三维里，姿态就藏在矩阵的列里

二维的模板是 2×2，到了三维写成 3×3。这个 3×3 里有个特别好用的性质：它的三列，就是这个物体自己三根轴在空间里的方向。

看第 3 列是 (1, 0, 0)，说明工具段指向 +X，就是水平朝前。这跟台阶四用"累计角 90 度"得出来的结论是同一件事。所以姿态用矩阵列表示、或者用欧拉角表示，只是两种写法。

不放心的话可以对照算：把台阶三的手算和这里的矩阵方法各跑一遍，换四组姿态比较，最大差值 1.1×10⁻¹⁶ 米，纯粹是计算机浮点数的毛刺，等于完全一致。

07台阶六：把"转"和"挪"合成一个矩阵

台阶五解决了转，可每一节实际做的是转完之后，再沿着转过去的方向挪一段。写成两步很麻烦：

p = R · p    （先转）
p = p + d    （再挪）

能不能一步干完？可以。把矩阵从 3×3 扩成 4×4，多出来的那一列放平移量，最后一行补上 (0, 0, 0, 1) 占位就行：

T = [ R(3×3)   d(3×1) ]      左上角管转，最后一列管挪
    [   0 0 0     1    ]

好处是能一直乘下去

换成 4×4 之后，整条胳膊就变成一串矩阵相乘：

T_total = T_base · T_J1 · T_J2 · T_J3 · T_J4 · T_J5 · T_TCP

从 T_total 里直接读两样东西：
  TCP 位置 = 最后一列
  工具朝向 = 第 3 列

ROS、URDF、以及各种机器人库，内部干的就是这件事，包括我们页面里那个 3D 模型加载器 urdf-arm.js。

三种写法摆在一起看

三个结果一样。所以矩阵不是什么更高级的数学，它只是把同样的计算写得更省事而已。手算能算通的话，你已经真的懂了。

另外顺手回答一个很多人会问的问题：非得用 DH 参数吗？不非得。DH 只是另一种把每节变换写下来的约定，好处是参数少、推公式方便。我们的 URDF 已经把关节位置和转轴方向都写明白了，不换算成 DH 一样能算 FK，ROS 自己就是这么干的。等你要手推解析 IK 的时候，DH 才显得顺手，那是下一章的事。

08台阶七：上真机，从 PWM 算到位姿

前面所有推导的前提都是"关节角已知"。可你在真机上手里拿到的其实是舵机的 PWM 值，不是角度。所以真实链路要多一步换算：

那个 0.002356 是算出来的：这颗舵机是 270 度的总线舵机，等于 4.712 弧度，PWM 从 500 到 2500 跨度 2000 个格。平均下来一格就是 4.712 ÷ 2000 = 0.002356 弧度。

真机程序是怎么组织的

仓库里 ros2_ws/src/xflz_eai_car1/arm/kinematics/kinematics/ 放着这一章的可运行版本。它的组织方式和你刚才手算的顺序是一样的：

核心其实就三行

fk_demo.py 里最要紧的部分简化成这样：

T = np.eye(4)                      # 从单位矩阵开始，相当于底座

for joint in joints:
    Tx = 平移矩阵(joint.xyz)        # 挪到关节所在的位置
    Ra = 旋转矩阵(joint.axis, θ)    # 绕这根轴转 θ
    T  = T @ Tx @ Ra                # 累乘，在已有变换上再叠一层

tcp = T[:3, 3]                     # 最后一列就是 TCP 位置

要留意 T @ Tx @ Ra 这个顺序。对矩阵右乘，意思是在局部坐标系里做变换，正好对应本章说的"先转再挪，而且挪的方向是转过之后的方向"。顺序写反了，结果就错了。

程序会打印出什么

pwm_to_tcp.py 除了位置，还会把 3×3 矩阵换算回欧拉角，就是台阶四讲的那三个姿态角：

【TCP 位置】
  X: +0.157 m    Y: +0.000 m    Z: +0.342 m
【TCP 姿态】(Roll-Pitch-Yaw)
  Roll: 接近水平   Pitch: 水平向前   Yaw: 正前方
【TCP Z 轴方向】
  向量: (+1.000, +0.000, -0.000)   ← 工具指向正前方

看到 Z 轴方向是 (1, 0, 0) 了吗？跟台阶五那张表里"第 3 列 = (1,0,0)"是同一个东西。

最后一步要自己动手

程序里的几何常量都是标定值，真机装配难免有几毫米的偏差。所以这一章的验收，不是看你算的数字跟书上的一不一样，而是看三样东西能不能对上：你手算的结果、程序打印的结果、你拿卷尺量的结果，三者差在 5 毫米以内就算过了。

09动手验一遍

先把 3D 模型拖出来对账

下面这个模型跟本章用的是同一份 simple-arm.urdf，右上角的读数是直接从 URDF 的变换链算出来的，中间没有手工填数：

互动 9-1｜把 J2、J3、J4 三个滑杆拖到 −50°、105°、35°，右上角应该是 x 0.157、y 0.000、z 0.342，跟你台阶三手算的对上了。然后拖 J1 看看 y 怎么变、z 为什么不动。勾上"坐标帧"，能看到每个关节的转轴指着哪个方向。

再上真机跑一次

1. 把机械臂摆到默认姿态，记下五个舵机各自的 PWM 值
2. 运行 python3 pwm_to_tcp.py 1500 <PWM2> <PWM3> <PWM4> 1500
3. 从底座中心出发，拿卷尺量出指尖的 x、y、z
4. 把误差记下来，想一想差在哪：是标定参数不准，还是你摆的角度本来就有偏差？

做完了自检一下

• 能不看答案，算出手算默认姿态下指尖的位置，并说出每一段的累计角
• 能解释 J1 转了为什么 z 坐标不变
• 能说清"姿态"用几个数表示，默认姿态下分别是多少
• 能画出 4×4 矩阵的形状，指出哪块是转、哪块是挪
• 程序输出跟你量的位置差在 5 毫米以内

10回头看看走过的这七步

如果只能记住一句，记住台阶三那个坑：后一节的角度是叠在前一节上面的。初学者算错，八成错在这儿。

几道题，不急着答完

下一章

这一章是"摆好姿势，算指尖在哪"。下一章整个反过来：想让指尖到某个位置，关节该转多少。那就是逆运动学，难度会明显上去：可能压根没有解，可能有七八组解，还得考虑关节转不到那么大的角度。不过不管 IK 怎么算，验证它算得对不对，最后还得靠这一章的 FK 来对答案。