Skip to content

机器人运动学:关节角怎样变成末端坐标 ​

WARNING

🧪 Beta公测版本提示:教程主体已完成,正在优化细节,欢迎大家提Issue反馈问题或建议。

运动学不问力,只问几何。给定关节角,手在哪(正运动学 FK);给定手要去的点,关节该怎么弯(逆运动学 IK);角转得快,手就瞬时往哪冲(雅可比)。本章用平面 2R 把这三件事从向量加法推到公式、再推到数字例子。力与加速度见 动力学;时间轴上的 θ(t) 见 轨迹规划;六轴怎么写成矩阵链见 DH。先读 导论 里「工作空间 ≠ 构型空间」。

本章阅读方式:正文永远先给结论公式和几何直觉;需要逐步代数时点开灰色的「逐步推导」。每个推导旁都有一张原理图。建议第一遍只读正文和数字例,卡在某一行再展开推导。


一、先建立画面:两根刚尺,两次「转完再走一截」 ​

把基座钉在原点 O。第一根杆是长度为 ℓ1 的刚尺,相对基座的 x 轴转过 θ1。它的末端——以后叫肘——只能落在以 O 为圆心、ℓ1 为半径的圆上:

p1=(ℓ1cos⁡θ1,ℓ1sin⁡θ1).

第二根杆长度 ℓ2。关键点来了:θ2 不是相对 x 轴的角,而是相对第一杆延长线的相对角。 因此第二杆相对基座的绝对角是 θ1+θ2。手(末端)= 肘再沿这个绝对方向走 ℓ2:

p2=p1+(ℓ2cos⁡(θ1+θ2),ℓ2sin⁡(θ1+θ2)).

把 p1 代进去,就是正运动学。没有矩阵也能写;矩阵的好处是「转完再挪」可以标准化,以后加第三杆只多乘一次。

为什么绝对角是 θ1+θ2

图解说明:左图 θ2 从第一杆的延长线量起;中图第二杆对基座的绝对角是 θ1+θ2;右图把两次「旋转 + 沿杆平移」接成向量和。

保姆级数字例(先不用矩阵)。 取与 demo 相同的杆长 ℓ1=1,ℓ2=0.7,角用弧度。

θ1θ2肘 p1手 p2你该看见什么
00(1,0)(1.7,0)两杆沿 +x 伸直,外边界
0π(1,0)(0.3,0)第二杆折回来,内边界
π/20(0,1)(0,1.7)整条臂竖起来
π/2π/2(0,1)(−0.7,1)第二杆相对第一杆再转 90∘,绝对角 π

公式和 NumPy 的 sin/cos 只用弧度。打印给人看时再用 rad2deg。混用是运动学第一大笔误。

平面齐次变换把点写成 (x,y,1),把「转 θ、沿杆走 a」收成一个 3×3:

T(θ,a)=(cos⁡θ−sin⁡θacos⁡θsin⁡θcos⁡θasin⁡θ001).

T(θ1,ℓ1)T(θ2,ℓ2) 的最后一列前两维,就是上面的 p2。DH 章把这个 3×3 抬成空间 4×4。

平面齐次变换

图解说明:左是坐标系旋转平移;中是 3×3 乘齐次点;右是两帧连乘。最后一列是原点,左上是姿态。

逐步推导:为什么 T(θ,a) 长成这样(点击展开)

先只谈一个坐标系相对另一个做「绕原点转 θ,再沿已经转过的 x 轴走 a」。这正是「杆绕关节转,杆长把下一关节送出去」。

第 1 步:旋转。 平面里把基座标 (x,y) 变到转过 θ 的新轴上,或等价地把新系的基向量写在旧系里:

R(θ)=(cos⁡θ−sin⁡θsin⁡θcos⁡θ).

第一列是新 x 轴(就是杆的方向),第二列是新 y 轴(杆的法向)。

第 2 步:平移。 下一关节的原点,就在「沿新 x 走 a」处,写在旧系里是

t=a(cos⁡θsin⁡θ).

第 3 步:齐次坐标。 旋转加平移对普通二维向量不是线性的(有常数项 t)。补一个 1,把仿射变成线性:

(x′y′1)=(R(θ)t0⊤1)(xy1).

把 R 和 t 填进去就是正文的 T(θ,a)。最后一列前两维 = 新原点;左上 2×2 = 姿态。

第 4 步:两杆连乘。 点先在手坐标系里是 (0,0,1)(手就是第二杆末端的原点)。右乘第二帧、再右乘第一帧:

(p21)=T(θ1,ℓ1)T(θ2,ℓ2)(001).

乘出来只看最后一列,正是 p2。顺序不能反:先应用靠近手的那一帧,再送回基座——Python 里写成 T1 @ T2。

第 5 步:第三维为什么是 1 而不是 0。 1 表示「这是一个点」;方向向量(例如速度)齐次坐标最后一维是 0,平移项乘不上,这正是我们想要的:姿态转速度,原点挪不动速度的方向。


二、正运动学:唯一、圆环带、以及一个必须手算的数字 ​

把上一节的向量和写开:

x=ℓ1cos⁡θ1+ℓ2cos⁡(θ1+θ2),y=ℓ1sin⁡θ1+ℓ2sin⁡(θ1+θ2).

永远唯一:一组角只对应一个点。反向不唯一——同一点可以肘上也可以肘下。

可达半径 r=x2+y2 落在圆环带

|ℓ1−ℓ2|≤r≤ℓ1+ℓ2.

外圆:两杆伸直(θ2=0);内圆:两杆尽量折叠(θ2=±π)。ℓ1=ℓ2 时内圆缩成原点,手可以摸到基座。转 θ1 只是把这个三角形绕原点转一圈,扫满整个圆环带,所以工作空间与 θ1 无关。

工作空间半径只取决于 θ2

图解说明:△OAP 的第三边就是 r。余弦定理给出 r2=ℓ12+ℓ22+2ℓ1ℓ2cos⁡θ2。θ1 只旋转整个三角形。

逐步推导:从正运动学平方相加到余弦定理(点击展开)

目标:证明 r2=ℓ12+ℓ22+2ℓ1ℓ2cos⁡θ2,从而 r 的范围与 θ1 无关。

把 x,y 平方相加:

r2=(ℓ1c1+ℓ2c12)2+(ℓ1s1+ℓ2s12)2=ℓ12(c12+s12)+ℓ22(c122+s122)+2ℓ1ℓ2(c1c12+s1s12).

前两项各是 1,所以 ℓ12+ℓ22。交叉项用余弦差角公式:

cos⁡Acos⁡B+sin⁡Asin⁡B=cos⁡(A−B),

这里 A=θ1, B=θ1+θ2,于是 A−B=−θ2,而 cos 是偶函数,

c1c12+s1s12=cos⁡θ2.

因此

r2=ℓ12+ℓ22+2ℓ1ℓ2cos⁡θ2.

cos⁡θ2∈[−1,1],于是

(ℓ1−ℓ2)2≤r2≤(ℓ1+ℓ2)2,

开方(半径非负)得到 |ℓ1−ℓ2|≤r≤ℓ1+ℓ2。几何上这就是:两圆(以 O 为心半径 ℓ1、以手为心半径 ℓ2)要能交成一个肘点,两圆心距必须落在 |ℓ1−ℓ2| 与 ℓ1+ℓ2 之间。

数字例(与 demo 相同)。 ℓ1=1,ℓ2=0.7,理论半径区间 [0.3,1.7]。目标 (1.1,0.6):

r=1.12+0.62=1.21+0.36=1.57≈1.253,

落在 (0.3,1.7) 内,应有两支实数逆解。若目标改成 (2,0),r=2>1.7,两圆不相交,无解。

C++ fk2r.hpp 就是那两行余弦,用来对照 Python。

平面 2R 正 / 逆运动学

图解说明:左图由 θ1,θ2 推出末端,虚线圆是最大/最小伸长;右图同一目标有肘上、肘下两套角。余弦定理先解 θ2,再用 atan2 解 θ1。

动画说明:先正运动学扫一圈;再对同一目标画出肘上 / 肘下;最后把目标拖出工作空间,两圆不再相交。


三、逆解:把「两圆相交」写成可执行的三步 ​

已知 (x,y),求 (θ1,θ2)。几何图像:以 O 为圆心画半径 ℓ1 的圆,以目标 P 为圆心画半径 ℓ2 的圆。交点就是肘。两个交点 = 肘上、肘下;相切 = 边界一支;不相交 = 无解。

逆解两支:三角形沿对角线翻折

图解说明:左:两圆相交得到两个肘点;中:△OAP 沿对角线 OP 翻折就是另一支;右:arccos 丢象限,atan2(y,x) 保住第三象限。

第 1 步:只由距离定 θ2。 余弦定理(或上节 r2 式)

cos⁡θ2=r2−ℓ12−ℓ222ℓ1ℓ2.

若右边绝对值 >1,目标在圆环外,无解。代码里 clip 到 [−1,1] 是为了浮点别让 sqrt 变 NaN,物理上应先检查再求解。

第 2 步:正弦的符号 = 肘号。 sin⁡θ2=±1−cos2⁡θ2。正号、负号各给一个 θ2。几何上:三角形 △ 基座-肘-手 可以沿「基座到手」这条对角线翻折,肘在对角线两侧,就是肘上 / 肘下。

第 3 步:θ1 把「指向目标的方位」扣掉第二杆造成的偏角。 把第二杆折进第一杆:等效

k1=ℓ1+ℓ2cos⁡θ2,k2=ℓ2sin⁡θ2,

则

θ1=atan2(y,x)−atan2(k2,k1).

必须用 atan2(y,x),参数顺序是先 y 后 x。 只用 arccos 会丢象限,手在第三象限时 θ1 会错。

逐步推导:从余弦定理到 θ1 的 atan2 公式(点击展开)

A. θ2 从哪来。 三角形三边 ℓ1,ℓ2,r。夹在 ℓ1 与 ℓ2 之间的外角(相对角)就是 θ2。余弦定理对边 r:

r2=ℓ12+ℓ22−2ℓ1ℓ2cos⁡(内角).

平面 2R 的 θ2 定义为「相对延长线」,伸直时 θ2=0、内角为 π,折叠时 θ2=π、内角为 0。因此内角 =π−θ2,cos⁡(π−θ2)=−cos⁡θ2,代入后

r2=ℓ12+ℓ22+2ℓ1ℓ2cos⁡θ2,

与正运动学平方相加完全一致。解出 cos⁡θ2 就是正文那一行。

cos 是偶函数:θ2 与 −θ2 给出同一个 cos,所以必须用 sin 的符号把两支分开。代码用 atan2(s2, c2) 而不是 arccos,因为 arccos 只返回 [0,π],肘下的负角会被折叠掉。

B. θ1 从哪来。 把正运动学看成:先把长度为 ℓ2 的第二杆「折进」第一杆,得到一个等效向量

(k1k2)=(ℓ1+ℓ2cos⁡θ2ℓ2sin⁡θ2),

它在「第一杆坐标系」里指向手。再把这个向量绕基座转 θ1,就应落到 (x,y):

(xy)=R(θ1)(k1k2).

两边左乘 R(θ1)⊤=R(−θ1),比较幅角:

atan2(y,x)=θ1+atan2(k2,k1),

移项即正文。直觉:atan2(y,x) 是「基座看手」的方位;atan2(k2,k1) 是「第一杆看手」相对第一杆的偏角;两者相减才是第一杆自己相对 x 轴的角。

C. 与 demo 对表。 目标 (1.1,0.6),ℓ1=1,ℓ2=0.7:

cos⁡θ2=1.57−1−0.491.4=0.081.4≈0.05714,θ2=±arccos⁡(0.05714)≈±1.514 rad (±86.7∘).

再代入 k1,k2 与 atan2,FK 回去必须精确回到 (1.1,0.6)(浮点误差 10−15 量级)。对不上就查 atan2 顺序或肘号。

工业臂还要在连续轨迹上选哪一支(不能这一采样肘上、下一采样肘下)。那是路径规划;本章只把两支都画出来。


四、雅可比:对 FK 逐项求导,列就是「单关节速度」 ​

末端速度是位置对时间的导数。链式法则:

(vxvy)=J(θ)(θ˙1θ˙2),J=∂(x,y)∂(θ1,θ2).

把正运动学逐项求导(记 s1=sin⁡θ1,s12=sin⁡(θ1+θ2) 等):

J=(−ℓ1s1−ℓ2s12−ℓ2s12ℓ1c1+ℓ2c12ℓ2c12).

列的含义(保姆级):第一列是「只转关节 1、关节 2 锁死」时手的速度方向——整个臂绕基座转,手速度垂直于「基座→手」。第二列是只转肘,手绕肘转,速度垂直于第二杆。这就是平面版的「雅可比列 = 关节旋量」。

雅可比的列 = 单关节给手的速度

图解说明:左:两列给出的速度张成平面。右:两杆共线时两支速度平行,丢失一个方向。底栏 v=Jθ˙。

逐步推导:正运动学怎样变成 J 的四个元素(点击展开)

从

x=ℓ1cos⁡θ1+ℓ2cos⁡(θ1+θ2),y=ℓ1sin⁡θ1+ℓ2sin⁡(θ1+θ2)

出发。对 θ1 求导(注意第二项里 θ1+θ2 也含 θ1):

∂x∂θ1=−ℓ1sin⁡θ1−ℓ2sin⁡(θ1+θ2),∂y∂θ1=ℓ1cos⁡θ1+ℓ2cos⁡(θ1+θ2).

对 θ2 求导(第一杆不含 θ2):

∂x∂θ2=−ℓ2sin⁡(θ1+θ2),∂y∂θ2=ℓ2cos⁡(θ1+θ2).

排成矩阵就是 J。几何核验:第二列 =ℓ2(−sin⁡ϕ,cos⁡ϕ),其中 ϕ=θ1+θ2 是第二杆绝对角。向量 (−sin⁡ϕ,cos⁡ϕ) 恰是第二杆方向 (cos⁡ϕ,sin⁡ϕ) 逆时针转 90∘,再乘 ℓ2θ˙2 就是「绕肘的线速度」。第一列同理,不过半径是「基座到手」的整段。

行列式(2R 有闭式):把 J 的两列看成向量 j1,j2,

detJ=j1xj2y−j1yj2x.

代入后交叉项消去,只剩

detJ=ℓ1ℓ2sin⁡θ2.

(可以用符号计算或耐心展开验证。练习建议:令 ℓ1=ℓ2=1,手算一个 θ2=π/2 的数值 detJ=1。)

θ2=0 或 ±π(伸直或折叠)时 detJ=0:奇异。几何上两杆共线,两个关节产生的末端速度变成平行,少了一个可动方向。工作空间的内外边界正好是这些构型。

雅可比与奇异

图解说明:上半 v=Jθ˙;下半两杆共线时 detJ=0。奇异附近不要硬求 J−1。

动画说明:橙箭头是「只转基座」给手的速度方向,绿箭头是「只转肘」。伸直时两支变成平行,|detJ|→0。

速度逆解 θ˙=J−1v。奇异时无解或无穷放大。实践用阻尼伪逆 (J⊤J+λI)−1J⊤,以牺牲一点跟踪换稳定。冗余臂 J 不是方的,用伪逆,零空间还可以用来躲障碍。


五、静力学一句:虚功把末端力变成关节力矩 ​

功率 τ⋅θ˙=f⋅v=f⋅Jθ˙,对任意 θ˙ 都成立,故

τ=J⊤f.

末端顶墙的力,会以 J⊤ 映射成关节力矩。奇异时这个映射同样病态:很小的力也可能要极大的力矩,或者某个方向的力根本「顶不住」(缺列)。

虚功:τ = Jᵀ f

图解说明:功率相等 τ⋅θ˙=f⋅v。把 v=Jθ˙ 代入即得 τ=J⊤f。近奇异时顶墙会要很大的关节力矩。

逐步推导:从「功率相等」到 τ=J⊤f(点击展开)

电机做的瞬时功率是各关节力矩乘角速度之和:∑iτiθ˙i=τ⊤θ˙。

末端力 f 对末端速度 v 做的功率是 f⊤v。若关节无摩擦、杆为刚体,这两份功率必须相等(虚功原理:约束力不做功,铰链反力成对抵消):

τ⊤θ˙=f⊤v.

运动学已经给出 v=Jθ˙,代入

τ⊤θ˙=f⊤Jθ˙=(J⊤f)⊤θ˙.

这对任意 θ˙ 成立,因此向量相等:τ=J⊤f。注意是 J 的转置,不是逆。力映射和速度映射是对偶的:速度用 J 往前推,力用 J⊤ 往回推。

这为 动力学 做铺垫:那边还要加上惯性与科氏,但「力从哪来」这一段已经是 J⊤。


六、常见疑问 ​

Q:肘上肘下哪一个是「上」?
本代码默认 sin⁡θ2>0 叫 up。不同教材、不同基座朝向会反过来。以 FK 核验 为准,不要死记名字。

Q:为什么 θ2 用 atan2(sin,cos) 而不是 arccos?
arccos 只返回 [0,π],肘下的负角会被折叠掉。

Q:工作空间里的点一定有两个 IK 吗?
边界上两支合成一支(sin⁡θ2=0)。带关节限位时可能只剩一支,甚至零支(点在圆环内但角超限)。

Q:3D 六轴还是闭式 IK 吗?
部分腕部分离的臂(Pieper 条件)有代数解;一般臂用牛顿法:q←q+J+Δx。2R 的几何解是为了把「多解、奇异」看见。

Q:角增量能不能当向量加?
平面一个 θ 可以。三维姿态不行,见 李群。

Q:齐次矩阵最后一列和左上角分别是什么?
最后一列前两维 = 下一坐标系原点写在当前系;左上 2×2(空间里是 3×3)= 姿态。点用 (x,y,1),方向用 (vx,vy,0)。

Q:奇异时机器人卡死了吗?
不是卡死。瞬时少了一个可动的笛卡尔方向,沿那个方向的速度逆解会爆炸。你仍可以沿剩下的方向动,也可以改变构型离开奇异(例如不要走完全伸直)。


七、代码在做什么 ​

demo.py 对目标 (1.1,0.6) 求两支 IK,再 FK 回去核验,并打印 detJ。左图工作空间采样 + 两种肘形;右图固定 θ1=0.4,扫 θ2 看行列式过零。

bash
cd docs/robotics/kinematics/code
python demo.py
g++ -std=c++17 demo.cpp -o fk_demo

2R 肘形与奇异


八、小结 ​

概念一句话
FK角 → 末端;唯一;两杆绝对角 θ1+θ2
齐次 T(θ,a)左上姿态,最后一列原点
IK点 → 角;2R 常两解;atan2 保象限
工作空间圆环带,圈外无解;r 只靠 θ2
J 的列单关节运动时手的速度
奇异sin⁡θ2=0;不要硬求逆
τ=J⊤f力到力矩,速度映射的对偶
下游轨迹、动力学、DH、李群、旋量、ROS 2

下一章 轨迹规划:起终点有了,中间 θ(t) 怎么走才不甩。

📥 Code ​

FileViewDownload
demo.pyOpenDownload
fk2r.hpp—Download
demo.cpp—Download
exercise.pyOpenDownload

参考 ​

  1. Craig, Introduction to Robotics(正逆解与雅可比)
  2. Siciliano et al., Robotics: Modelling, Planning and Control
  3. Lynch & Park, Modern Robotics(速度旋量观点)