14-具身智能 · 机器人学基础¶
本篇定位:具身智能 / 机器人算法岗的"第一块砖"。 不管你是要做 VLA 大模型、强化学习策略,还是运动控制,机器人的"身体"永远用同一套数学语言来描述—— 位姿、变换、运动学、雅可比。看懂本篇,你就拥有了和机械臂、双足、四足机器人对话的"母语"。
前置知识:线代矩阵(01-线代)、三角函数(00-高数),本篇全部从直觉讲起,可零基础入门。
本节要解决的问题¶
| 要解决的问题 | 对应内容 |
|---|---|
| 机器人的"手"现在在哪、朝向哪? | 刚体位姿表示:位置 + 姿态 |
| 姿态有哪几种写法?各有什么坑? | 旋转矩阵 / 欧拉角 / 四元数 |
| 怎么用一个矩阵同时表达"旋转 + 平移"? | 齐次变换矩阵 SE(2) / SE(3) |
| 多个关节变换怎么拼在一起? | 变换复合(左乘 vs 右乘) |
| 怎么用 4 个参数描述相邻两个连杆? | DH 参数 |
| 已知关节角,末端在哪? | 正运动学(FK) |
| 已知末端位姿,关节角是多少? | 逆运动学(几何法 / 数值法) |
| 关节速度与末端速度怎么换算? | 雅可比矩阵 |
| 让末端走一条直线,关节怎么动? | 关节空间 vs 任务空间轨迹 |
故事引入¶
某天你网购了一个快递箱,被快递员放在了走廊尽头的墙角。你懒得动,于是指挥家里的机械臂大厨去把它端回来。
你说:"把那箱子端过来。"听起来一句话,但机械臂的脑子里得连续回答一串问题:箱子在哪? —— 先要一个"位置",告诉它 (x, y, z) 三个坐标;箱子口朝哪? —— 还得有"姿态",不然它伸手的方向错了会抓空。位置加姿态,合起来叫位姿。接着,它要低头看看自己:肩膀、肘、手腕这 3 个关节分别转到多少度,才能让"手"正好够到那个位姿?这就是逆运动学。它还要一路小心:不能把箱子举太高洒了水,也不能让关节转到极限位置"咔哒"卡死——所以它得规划一条轨迹:要么直接告诉每个关节"按这条角度曲线转"(关节空间),要么让手走一条笔直的路线、每个时刻反算关节角(任务空间)。最后,假如中途有人碰了一下箱子,它还要重新算一遍——每一步都离不开雅可比矩阵这个"速度翻译官"。
你闭着眼也能把杯子端起来,是因为你的小脑在毫秒之间解了无数个正逆运动学;而机械臂没有小脑,它靠的就是本篇这一套严格的数学。理解了这套"身体语言",后面所有具身智能的内容——抓取、移动、VLA 大模型输出动作——都是在它之上盖楼。
核心概念¶
(a) 🗂️ 机器人学知识结构总览¶
整篇内容就是一条"从怎么描述身体 → 到怎么算身体动作"的主线:先学会表示位姿,再把位姿串成变换,然后用 DH 参数描述机械臂结构,最后用正/逆运动学与雅可比回答"手在哪、怎么动"。
flowchart TB
A["🧠 位姿表示<br/>位置 + 姿态"]
B["🧠 旋转矩阵 / 欧拉角 / 四元数<br/>三种姿态语言"]
C["🧠 齐次变换矩阵 SE(2) / SE(3)<br/>[R t; 0 1] 一个矩阵搞定"]
D["🧠 变换复合<br/>右乘 = 相对当前坐标系"]
E["🧠 DH 参数<br/>4 个参数描述相邻连杆"]
F["🧠 正运动学 FK<br/>关节角 → 末端位姿"]
G["🧠 逆运动学 IK<br/>末端位姿 → 关节角"]
H["🧠 雅可比矩阵<br/>关节速度 ↔ 末端速度"]
I["🧠 轨迹规划<br/>关节空间 vs 任务空间"]
A --> B
B --> C
C --> D
D --> E
E --> F
F --> G
G --> H
H --> I
- 位姿表示:刚体在空间里既要知道"在哪"(位置坐标),也要知道"朝向哪"(姿态)。位置 + 姿态合起来叫位姿,一共 6 个自由度(3 平移 + 3 旋转)。
- 旋转矩阵 / 欧拉角 / 四元数:三种描述姿态的语言。旋转矩阵最严谨但冗余(9 个数),欧拉角最直观但有"万向锁"坑,四元数无奇点、插值顺滑,是机器人系统的主流。
- 齐次变换矩阵:把"旋转 + 平移"打包成一个 4×4 矩阵,最后一行为 [0 0 0 1]。从此复合变换、求逆都只是矩阵乘法,数学结构封闭。
- 变换复合:多个变换串起来就是矩阵连乘,关键是分清"左乘 = 相对世界(固定)系、右乘 = 相对当前(动)系",搞反了机械臂会飞。
- DH 参数:用 4 个参数(a / α / d / θ)描述相邻两连杆的相对位姿,是描述任意串联机械臂的"标准简历",也是 FK 的输入。
- 正运动学 FK:已知每个关节的角度,算出末端执行器的位姿——从"因"到"果",答案唯一,机械臂每时每刻都在算。
- 逆运动学 IK:已知末端要到的位姿,反推关节角——从"果"到"因",可能多解、可能无解、可能奇异,比 FK 难得多。
- 雅可比矩阵:描述"关节动一点,末端动多少"的速度映射,是数值 IK、奇异性分析、力控(τ = JᵀF)的共同地基。
- 轨迹规划:关节空间(直接插值关节角)简单稳健但末端路径不可控;任务空间(末端走直线)直观但每个点都要解 IK、还要躲奇异。
🕸️ 网络配图(Wikimedia Commons)
📷 来源:Wikimedia Commons · <a href="//commons.wikimedia.org/wiki/User:Mr.Doctor.No" title="User…
深入原理¶
这一节把上面每个概念拆开讲透:先讲"人话"建立直觉,再上公式,再给关键推导,最后附一句"面试怎么问"。
(1) 位姿:先回答"在哪"和"朝哪"¶
直觉: 描述一个刚体(机械臂末端、机器人身体、快递箱),光说"在 (1,2,3)"还不够——同一个位置,你可以正面朝它,也可以背对它,伸手方向完全不同。所以位姿 = 位置 + 姿态。
公式:
位置是三维向量 $p = (p_x, p_y, p_z)$;姿态用一个旋转矩阵 $R \in SO(3)$ 表示,$SO(3)$ 是所有"右手系旋转"构成的集合:
$$SO(3) = \big\{ R \in \mathbb{R}^{3\times 3} \ \big|\ R^T R = I,\ \det(R) = +1 \big\}$$
位姿合起来记为 $g = (p, R)$,或者用下一节要讲的齐次矩阵表示。
关键推导:
- $R^T R = I$:说明 $R$ 是正交矩阵,作用在向量上不改变长度和夹角 → 刚体不变形("刚性"就来自这里)。
- $\det(R) = +1$:保证不产生镜像翻转,也就是始终满足右手系(伸右手:拇指 x、食指 y、中指 z)。
- $R$ 的三列 $r_1, r_2, r_3$ 分别就是物体坐标系三根轴在世界坐标系里的投影,所以"读出矩阵 = 知道朝向"。
面试怎么问: "旋转矩阵为什么要求正交且行列式为 1?" → 正交保证长度/夹角不变(刚体),det = +1 保证右手系、不镜像。
(2) 姿态的三种语言:旋转矩阵、欧拉角、四元数¶
直觉: 同一个"朝向"有三种写法,就像同一个电话号码可以存在名片、通讯录、Excel 里。旋转矩阵 9 个数最啰嗦但最严谨;欧拉角 3 个数最好懂,但会"万向锁";四元数 4 个数没有奇点、插值顺滑,是机器人/航姿系统的主流。
公式:
绕 $z$ 轴转 $\theta$(右手系,逆时针为正)的旋转矩阵:
$$R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$$
欧拉角(ZYX 约定)是三次基本旋转的复合:
$$R = R_z(\phi)\, R_y(\theta)\, R_x(\psi)$$
四元数 $q = (w, x, y, z)$,绕单位轴 $u$ 转 $\theta$:
$$q = \Big( \cos\frac{\theta}{2},\ \sin\frac{\theta}{2}\, u_x,\ \sin\frac{\theta}{2}\, u_y,\ \sin\frac{\theta}{2}\, u_z \Big)$$
关键推导:
- 2D 旋转矩阵怎么来的:点 $(x, y)$ 绕原点转 $\theta$,用和角公式展开得 $(x\cos\theta - y\sin\theta,\ x\sin\theta + y\cos\theta)$,写成矩阵即得。
- 万向锁:ZYX 欧拉角在中间角 $\theta = \pm 90°$ 时,绕 x 与绕 z 的两根轴重合,丢失一个自由度,姿态"卡死",插值也会诡异。
- 四元数插值用 slerp(球面线性插值),过渡自然;单位四元数还能直接由 Rodrigues 公式转成旋转矩阵,是工程里的"标准格式"。
面试怎么问: "欧拉角有什么缺点?为什么机器人常用四元数?" → 万向锁 + 插值不自然;四元数无奇点、只有 4 个数、slerp 平滑,代价是不直观。
(3) 齐次变换矩阵:把"旋转 + 平移"装进一个矩阵¶
直觉: 旋转矩阵只能转方向,不能"搬家"。数学家用了一个经典技巧:升维。给 3D 向量补一个 1,旋转矩阵扩成 4×4,平移就"藏"进了最后一列——从此旋转和平移可以一起作用、一起复合、一起求逆,全部是矩阵乘法。
公式:
$$T = \begin{bmatrix} R & t \\ 0_{1\times 3} & 1 \end{bmatrix} \in SE(3)$$
对点的作用(齐次坐标写法):
$$\begin{bmatrix} p_A \\ 1 \end{bmatrix} = T \begin{bmatrix} p_B \\ 1 \end{bmatrix} \quad\Longleftrightarrow\quad p_A = R\, p_B + t$$
逆变换(从 A 回到 B):
$$T^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0 & 1 \end{bmatrix}$$
关键推导:
- 为什么最后一行必须是 $[0\ 0\ 0\ 1]$?为了保证两个变换相乘后结构不变(左上还是旋转、右上还是平移),矩阵乘法才能一直复合下去。
- 逆变换推导:由 $p_A = R p_B + t$ 解出 $p_B = R^T(p_A - t) = R^T p_A - R^T t$,所以旋转部分取转置、平移部分取 $-R^T t$。
- 平面机器人(AGV、2D 定位)用 SE(2):$T = \begin{bmatrix} R_{2\times2} & t_{2\times1} \\ 0 & 1 \end{bmatrix}$,3×3 同理,代码实战会手写验证。
面试怎么问: "为什么用 4×4 齐次矩阵而不是 3×3 矩阵加一列平移?" → 为了让复合、求逆都封闭在矩阵乘法里,数学结构统一、代码实现一致。
✍️ §(3).1 手算旋转矩阵与齐次变换:90° 转完再挪 (2,3)¶
旋转矩阵 $R=\begin{bmatrix}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{bmatrix}$ 与齐次变换——2D 手算,把"旋转 + 平移"变成一次矩阵乘。
第 1 步:2D 旋转 90°
$$R(90°)=\begin{bmatrix}0&-1\\1&0\end{bmatrix}$$
$$(1,0)\xrightarrow{R(90°)}(0,1)$$
第 2 步:齐次变换(旋转 90° 后平移 (2,3),转成 3×3)
$$T=\begin{bmatrix}0&-1&2\\1&0&3\\0&0&1\end{bmatrix}$$
$$T\begin{pmatrix}1\\0\\1\end{pmatrix}=\begin{pmatrix}0\cdot1+(-1)\cdot0+2\\1\cdot1+0\cdot0+3\\1\end{pmatrix}=\begin{pmatrix}2\\4\\1\end{pmatrix}\Rightarrow(2,4)$$
手算收获:
- 旋转矩阵列向量 = 原坐标轴变换后的位置:$R(90°)$ 第 1 列 $[0,1]^\top$ 就是 $x$ 轴转到 $y$ 方向(§(1) 位姿直觉)
- 旋转是线性、平移是仿射:硬拼 $y=Rx+t$ 要先转后挪——齐次变换把两者统一成矩阵乘 = 仿射变换(§(3) 齐次坐标动机)
- 复合顺序:左乘比右乘"先做"(§(4) 变换复合)——本手算 T = 先 R 后 t,与 $T\cdot p$ 左乘一致
- 面试速答:旋转矩阵正交($R^\top R=I$、$\det R=1$),3 自由度用 3 个欧拉角、无万向锁用四元数(§(2))
下面 numpy 复算。
# ---- §(3).1 验证:numpy 复算旋转与齐次变换 ----
import numpy as np
th = np.deg2rad(90)
R = np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])
r = R @ np.array([1.0, 0.0])
print(f'R(90°) = {np.round(R, 4).tolist()}, (1,0)→{np.round(r, 4).tolist()}')
assert np.allclose(R, [[0, -1], [1, 0]]) and np.allclose(r, [0, 1])
T = np.array([[0, -1, 2], [1, 0, 3], [0, 0, 1]])
out = T @ np.array([1.0, 0.0, 1.0])
print(f'齐次变换: (1,0)→({out[0]:.0f},{out[1]:.0f})')
assert np.allclose(out[:2], [2, 4])
print('\n✓ 与手算一致')
(4) 变换复合:左乘 vs 右乘,别再搞反¶
直觉: 机械臂有 6 个关节,就是把 6 个"相邻坐标系之间的变换"串起来。串联的顺序用矩阵乘法表达,有个秒杀技巧:下标相邻就能消去——$T_{WA} \cdot T_{AB} = T_{WB}$。再记一句口诀:右乘 = 相对当前(动)坐标系接着动;左乘 = 相对世界(固定)坐标系动。
公式:
$$T_{WB} = T_{WA}\, T_{AB}$$
对点:$p_W = T_{WA}\, p_A$,$p_A = T_{AB}\, p_B$,代入得 $p_W = T_{WA}\, T_{AB}\, p_B$。
关键推导:
- 看乘法顺序:$T_{WA}\, T_{AB}$ 作用到点上时,右边的 $T_{AB}$ 先作用(点先从 B 变到 A),左边的 $T_{WA}$ 后作用(从 A 变到 W)。
- 代码里写
T = T1 @ T2的含义是"先做 T1,再做(相对 T1 结果坐标系的)T2",所以叫右乘 = 相对当前系。 - 验证方法(代码实战会做):让 T2 只旋转不平移。右乘时新坐标系原点不动(绕自己转);左乘时原点被转走(绕世界原点转)。一下就能分清。
面试怎么问: "T = T_a @ T_b 是先做哪个变换?" → 先 $T_a$ 再 $T_b$;右乘的变换是相对"当前坐标系"定义的,用下标链消去法秒判。
(5) DH 参数与正运动学:把机械臂写进"简历"¶
直觉: 每个关节转多少、每根连杆多长、相邻关节轴怎么错位——这些信息 DH 参数用 4 个数 $(a_i, \alpha_i, d_i, \theta_i)$ 全包了。**正运动学(FK)**就是把这些相邻变换矩阵乘起来:关节角 → 末端位姿,答案唯一。
公式: 标准 DH 相邻坐标系变换:
$$A_i = \mathrm{Rot}_z(\theta_i)\, \mathrm{Trans}_z(d_i)\, \mathrm{Trans}_x(a_i)\, \mathrm{Rot}_x(\alpha_i)$$
展开成矩阵(记住第 4 行是 $[0\ 0\ 0\ 1]$):
$$A_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$
正运动学:
$$T_{0n} = A_1\, A_2\, \cdots\, A_n$$
关键推导:
- 末端位置 = $T_{0n}$ 的右上 $3\times1$ 块;末端姿态 = 左上 $3\times3$ 块。
- 平面 2 连杆特例($\alpha = d = 0$):$x = l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2)$,$y = l_1\sin\theta_1 + l_2\sin(\theta_1+\theta_2)$ —— 代码实战会用 DH 矩阵验证这个解析式。
- 标准 DH 把坐标系 $i$ 放在连杆 $i$ 的起点($z_i$ 沿关节轴 $i+1$ 方向);另一种"改进 DH"坐标系附着方式不同,但都是 4 参数、连乘得 FK。
面试怎么问: "DH 的 4 个参数各自含义?标准 DH 和改进 DH 的区别?" → a 连杆长度、α 连杆扭转、d 关节偏距、θ 关节角;两者坐标系附着位置不同(标准:坐标系在连杆起点;改进:在连杆末端),但都能描述任意串联臂。
(6) 逆运动学:已知"手到哪",反推"关节怎么转"¶
直觉: FK 是"因→果"(唯一确定);IK 是"果→因",是个非线性反问题,可能多解、可能无解、可能奇异。平面 2 连杆可以用几何法(解三角形)得到闭式解;一般机械臂用数值法:先猜一组关节角,算末端误差,再沿误差下降方向迭代修正。
公式: 2 连杆平面臂几何法(末端 $(x,y)$,杆长 $l_1, l_2$,$c = \sqrt{x^2+y^2}$):
$$\cos\theta_2 = \frac{x^2 + y^2 - l_1^2 - l_2^2}{2 l_1 l_2}, \qquad \theta_2 = \pm\, \mathrm{atan2}\Big(\sqrt{1-\cos^2\theta_2},\ \cos\theta_2\Big)$$
$$\theta_1 = \mathrm{atan2}(y, x) - \mathrm{atan2}\big(l_2\sin\theta_2,\ l_1 + l_2\cos\theta_2\big)$$
数值法——雅可比转置迭代(代码实战会从零实现):
$$\theta_{k+1} = \theta_k + \alpha\, J(\theta_k)^T\, \big(p_{des} - p(\theta_k)\big)$$
关键推导:
- 几何法:由余弦定理 $c^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos\theta_2$ 解出 $\theta_2$,$\pm$ 对应"肘上 / 肘下"两个解;$\theta_1$ = "目标方向角" 减去 "连杆 2 造成的偏角"。
- 数值法为什么用 $J^T e$?因为 $\nabla_\theta \|e\|^2 = -2 J^T e$,沿 $J^T e$ 方向走就是最陡下降,步长足够小时误差单调下降。
- 目标不可达时:数值法会收敛到可达边界上的最近点——这也是一种"答案"(贴着边界推)。
面试怎么问: "IK 为什么比 FK 难?数值法怎么保证收敛?" → FK 是确定性映射、IK 是反问题(多解/无解/奇异);数值法用雅可比转置/伪逆/阻尼最小二乘迭代,选好初值并加阻尼防奇异发散。
(7) 雅可比矩阵:关节速度 ↔ 末端速度的"翻译官"¶
直觉: 每个关节转 $\dot\theta_i$,末端会跟着动。雅可比矩阵 $J$ 就是"关节转速 → 末端速度"的线性映射。它同时是数值 IK 的梯度来源、奇异性分析的判据、力控 $\tau = J^T F$ 的桥梁,是机器人学的"万能钥匙"。
公式:
$$\dot{x} = J(\theta)\, \dot{\theta}, \qquad \dot{x} = \begin{bmatrix} v \\ \omega \end{bmatrix}$$
对第 $i$ 个旋转关节,雅可比第 $i$ 列:
$$J_i = \begin{bmatrix} z_i \times (p_e - p_i) \\ z_i \end{bmatrix}$$
关键推导:
- 速度叠加原理:末端速度 = 每个关节单独转动引起的线速度之和。
- 转动线速度公式 $v = \omega \times r$:关节 $i$ 的角速度 $\dot\theta_i$ 沿其关节轴 $z_i$,末端相对关节 $i$ 的臂长是 $p_e - p_i$,叉积即得;角速度部分直接是 $z_i$。
- 奇异位形:当 $J$ 不满秩($\det(J) = 0$)时,某些方向末端"动不了",且逆解速度会爆炸——轨迹规划要么避开奇异,要么用阻尼(DLS)处理。
面试怎么问: "奇异位形是什么?冗余机械臂的雅可比是方阵吗?" → 奇异时 $J$ 退化、丢失某个方向的自由度;冗余臂关节数 > 末端自由度,$J$ 是"胖矩阵",用伪逆 $J^+$ 求最小范数解。
代码实战¶
用 numpy 从零实现三个核心演示(不需要任何第三方库,无随机、结果可复现):
- 手写 2D 旋转矩阵 + 齐次变换,验证"左乘 vs 右乘"的复合顺序;
- 用 DH 参数实现 2 连杆 / 3 连杆正运动学,画出工作空间;
- 数值逆运动学(雅可比转置迭代),验证从末端位置反推关节角。
# ---- matplotlib 中文字体(防图内中文乱码)----
import matplotlib
matplotlib.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei', 'Songti SC', 'Arial Unicode MS', 'DejaVu Sans']
matplotlib.rcParams['axes.unicode_minus'] = False
# -------------------------------------------
# ============================================================
# 编码设置:保证中文输出不乱码
# ============================================================
import sys, io
try:
sys.stdout.reconfigure(encoding='utf-8', errors='replace')
except Exception:
try:
sys.stdout = io.TextIOWrapper(sys.stdout.buffer, encoding='utf-8', errors='replace')
except Exception:
pass
import numpy as np
def rot2d(theta):
"""手写 2D 旋转矩阵(右手系,绕 z 轴逆时针为正)"""
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s],
[s, c]])
def se2(theta, tx, ty):
"""构造 2D 齐次变换矩阵 T = [R t; 0 1] ∈ SE(2)"""
T = np.eye(3)
T[:2, :2] = rot2d(theta)
T[:2, 2] = [tx, ty]
return T
# ---------------- 验证 1:旋转矩阵正交且 det = 1 ----------------
R = rot2d(np.pi / 2)
assert np.allclose(R @ R.T, np.eye(2)), "R 不是正交阵!"
assert np.isclose(np.linalg.det(R), 1.0), "det(R) != 1!"
print("验证1: 旋转 90° 矩阵满足 R·Rᵀ = I 且 det(R) = +1 ✔")
# ---------------- 验证 2:齐次变换对点的作用 ----------------
# 先旋转 90° 再平移 (1,0):点 (1,0) -> 旋转得 (0,1) -> 平移得 (1,1)
T = se2(np.pi / 2, 1.0, 0.0)
p = np.array([1.0, 0.0, 1.0]) # 齐次坐标
q = T @ p
assert np.allclose(q[:2], [1.0, 1.0]), q
print(f"验证2: [R|t] 作用于点 (1,0) -> ({q[0]:.3f}, {q[1]:.3f}),符合'先旋转再平移' ✔")
# ---------------- 验证 3:右乘 = 相对当前系;左乘 = 相对世界系 ----------------
T_wa = se2(0.0, 1.0, 0.0) # 先沿 x 平移到 (1,0)
T_ab = se2(np.pi / 2, 0.0, 0.0) # 再绕"当前坐标系"旋转 90°
T_wb = T_wa @ T_ab # 右乘:相对移动后的坐标系旋转
origin_b = T_wb @ np.array([0.0, 0.0, 1.0])
assert np.allclose(origin_b[:2], [1.0, 0.0]), origin_b
print("验证3a: 右乘复合后,B 系原点仍在 (1,0) —— '原地旋转' ✔")
T_wb2 = T_ab @ T_wa # 左乘:相对世界系先旋转
origin_b2 = T_wb2 @ np.array([0.0, 0.0, 1.0])
assert np.allclose(origin_b2[:2], [0.0, 1.0]), origin_b2
print("验证3b: 左乘复合后,B 系原点被转到 (0,1) —— '绕世界原点转' ✔")
# ---------------- 验证 4:逆变换公式 T⁻¹ = [Rᵀ, -Rᵀt; 0 1] ----------------
T_inv = np.eye(3)
T_inv[:2, :2] = T[:2, :2].T
T_inv[:2, 2] = -T[:2, :2].T @ T[:2, 2]
assert np.allclose(T @ T_inv, np.eye(3)), "T·T⁻¹ != I!"
print("验证4: 逆变换公式 T⁻¹ = [Rᵀ, -Rᵀt; 0 1] 成立(T·T⁻¹ = I)✔")
# ---------------- 验证 5:复合 = 依次作用(矩阵乘法与"两步走"一致) ----------------
np.random.seed(0)
for _ in range(5):
T1 = se2(np.random.uniform(-np.pi, np.pi), *np.random.uniform(-2, 2, 2))
T2 = se2(np.random.uniform(-np.pi, np.pi), *np.random.uniform(-2, 2, 2))
pt = np.array([*np.random.uniform(-2, 2, 2), 1.0])
step1 = T2 @ pt # 先 T2
step2 = T1 @ step1 # 再 T1
assert np.allclose((T1 @ T2) @ pt, step2)
print("验证5: (T1·T2)·p = T1·(T2·p),复合就是'先右后左依次作用' ✔")
验证1: 旋转 90° 矩阵满足 R·Rᵀ = I 且 det(R) = +1 ✔ 验证2: [R|t] 作用于点 (1,0) -> (1.000, 1.000),符合'先旋转再平移' ✔ 验证3a: 右乘复合后,B 系原点仍在 (1,0) —— '原地旋转' ✔ 验证3b: 左乘复合后,B 系原点被转到 (0,1) —— '绕世界原点转' ✔ 验证4: 逆变换公式 T⁻¹ = [Rᵀ, -Rᵀt; 0 1] 成立(T·T⁻¹ = I)✔ 验证5: (T1·T2)·p = T1·(T2·p),复合就是'先右后左依次作用' ✔
import numpy as np
import matplotlib
matplotlib.use('Agg') # 无显示环境也能出图
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
def dh_transform(a, alpha, d, theta):
"""标准 DH 参数 -> 相邻连杆坐标系变换矩阵 A_i = [R t; 0 1]"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1],
], dtype=float)
def fk_dh(thetas, links):
"""正运动学:T_0n = A_1·A_2·...·A_n(平面臂 alpha=d=0)"""
T = np.eye(4)
for l, th in zip(links, thetas):
T = T @ dh_transform(l, 0.0, 0.0, th)
return T
# ---------------- 验证 1:全伸直时末端在 (l1+l2, 0) ----------------
links2 = [1.0, 0.8]
T = fk_dh([0.0, 0.0], links2)
assert np.allclose(T[:2, 3], [1.8, 0.0]), T[:2, 3]
print("验证1: 2 连杆全伸直 θ=(0,0),末端 = (1.8, 0) ✔")
# ---------------- 验证 2:DH 结果与解析公式一致 ----------------
th1, th2 = 0.6, -0.4
T_dh = fk_dh([th1, th2], links2)
x_ana = links2[0]*np.cos(th1) + links2[1]*np.cos(th1 + th2)
y_ana = links2[0]*np.sin(th1) + links2[1]*np.sin(th1 + th2)
assert np.allclose(T_dh[:2, 3], [x_ana, y_ana]), (T_dh[:2, 3], x_ana, y_ana)
print("验证2: DH 正运动学与解析公式 x=l1·c1+l2·c12, y=l1·s1+l2·s12 一致 ✔")
# ---------------- 工作空间:遍历关节角网格 -> 末端位置 ----------------
def workspace2d(links, n1=150, n2=150):
t1 = np.linspace(-np.pi, np.pi, n1)
t2 = np.linspace(-np.pi, np.pi, n2)
T1, T2 = np.meshgrid(t1, t2)
l1, l2 = links
x = l1*np.cos(T1) + l2*np.cos(T1 + T2)
y = l1*np.sin(T1) + l2*np.sin(T1 + T2)
return x.ravel(), y.ravel()
def workspace3d(links, n1=60, n2=60, n3=24):
t1 = np.linspace(-np.pi, np.pi, n1)
t2 = np.linspace(-np.pi, np.pi, n2)
t3 = np.linspace(-np.pi, np.pi, n3)
l1, l2, l3 = links
T1 = t1[:, None, None]; T2 = t2[None, :, None]; T3 = t3[None, None, :]
x = l1*np.cos(T1) + l2*np.cos(T1+T2) + l3*np.cos(T1+T2+T3)
y = l1*np.sin(T1) + l2*np.sin(T1+T2) + l3*np.sin(T1+T2+T3)
return x.ravel(), y.ravel()
fig, axes = plt.subplots(1, 3, figsize=(15, 5))
x2, y2 = workspace2d(links2)
axes[0].scatter(x2, y2, s=0.5, c='steelblue')
axes[0].set_title('2 连杆工作空间(圆环带)')
axes[0].set_aspect('equal')
links3 = [1.0, 0.7, 0.5]
x3, y3 = workspace3d(links3)
axes[1].scatter(x3, y3, s=0.3, c='darkorange', alpha=0.6)
axes[1].set_title('3 连杆工作空间(实心圆盘)')
axes[1].set_aspect('equal')
# 画一组示例姿态:2 连杆在 θ=(0.6, -0.4)
th = np.array([0.6, -0.4])
j0 = np.zeros(2)
j1 = links2[0]*np.array([np.cos(th[0]), np.sin(th[0])])
j2 = j1 + links2[1]*np.array([np.cos(th[0]+th[1]), np.sin(th[0]+th[1])])
axes[2].plot([j0[0], j1[0], j2[0]], [j0[1], j1[1], j2[1]], '-o', lw=4, ms=8, color='seagreen')
axes[2].set_title(f'示例姿态 θ=({th[0]:.2f}, {th[1]:.2f})')
axes[2].set_aspect('equal')
for ax in axes:
ax.grid(alpha=0.3)
plt.tight_layout()
plt.savefig('images/emb00_workspace.png', dpi=120, bbox_inches='tight')
plt.show()
print("已保存图片: images/emb00_workspace.png ✔")
print("2 连杆工作空间: 内半径 |l1-l2|=0.2, 外半径 l1+l2=1.8(空心圆环带)")
print("3 连杆工作空间: 最远可达 l1+l2+l3=2.2,且所有半径连续可达 -> 实心圆盘")
验证1: 2 连杆全伸直 θ=(0,0),末端 = (1.8, 0) ✔ 验证2: DH 正运动学与解析公式 x=l1·c1+l2·c12, y=l1·s1+l2·s12 一致 ✔
已保存图片: images/emb00_workspace.png ✔ 2 连杆工作空间: 内半径 |l1-l2|=0.2, 外半径 l1+l2=1.8(空心圆环带) 3 连杆工作空间: 最远可达 l1+l2+l3=2.2,且所有半径连续可达 -> 实心圆盘
C:\Users\24260\AppData\Local\Temp\ipykernel_15728\653316101.py:86: UserWarning: FigureCanvasAgg is non-interactive, and thus cannot be shown plt.show()
工作空间图解读:

- 左图:2 连杆末端只能落在内外半径之间的圆环带(内半径 $|l_1 - l_2| = 0.2$,外半径 $l_1 + l_2 = 1.8$),因为两杆伸直/折叠都无法让末端太靠近基座。
- 中图:3 连杆自由度更多,任意半径连续可达 → 工作空间变成实心圆盘。杆数越多,"手"能覆盖的区域越满。
- 右图:一组具体姿态下的机械臂形状,方便把抽象的矩阵结果"看"出来。
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['Microsoft YaHei', 'SimHei']
plt.rcParams['axes.unicode_minus'] = False
def fk2(th, links):
"""2 连杆平面臂正运动学(解析式,与 DH 矩阵一致)"""
x = links[0]*np.cos(th[0]) + links[1]*np.cos(th[0] + th[1])
y = links[0]*np.sin(th[0]) + links[1]*np.sin(th[0] + th[1])
return np.array([x, y])
def num_jacobian(th, links, eps=1e-7):
"""数值雅可比:J 第 i 列 = 末端位置对 θ_i 的偏导(差分近似)"""
p0 = fk2(th, links)
J = np.zeros((2, len(links)))
for i in range(len(links)):
thp = th.copy()
thp[i] += eps
J[:, i] = (fk2(thp, links) - p0) / eps
return J
def ik_jacobian_transpose(target, links, init, alpha=0.5, iters=500):
"""数值逆运动学:θ_{k+1} = θ_k + α·Jᵀ·(p_des - p(θ_k))
步长归一化(有界)防止振荡发散"""
th = np.array(init, dtype=float)
trail = [fk2(th, links).copy()]
for _ in range(iters):
p = fk2(th, links)
err = target - p
if np.linalg.norm(err) < 1e-6:
break
J = num_jacobian(th, links)
d = J.T @ err # 梯度方向(最陡下降)
th = th + alpha * d / (1.0 + np.linalg.norm(d))
trail.append(fk2(th, links).copy())
return th, np.array(trail)
np.random.seed(0) # 固定随机种子,结果可复现
links = [1.0, 0.8]
target = np.array([1.2, 0.6])
th_sol, trail = ik_jacobian_transpose(target, links, init=[0.5, -0.5])
p_sol = fk2(th_sol, links)
res = np.linalg.norm(p_sol - target)
assert res < 1e-3, f"IK 未收敛, 残差 {res}"
print(f"验证1: 数值 IK 收敛,末端误差 = {res:.2e} ✔")
print(f" 解: θ = ({np.degrees(th_sol[0]):.2f}°, {np.degrees(th_sol[1]):.2f}°)")
print(f" 回代 FK(θ) = ({p_sol[0]:.4f}, {p_sol[1]:.4f}) ≈ 目标 (1.2, 0.6) ✔")
# 无解情况:目标超出可达范围 (2.5, 0) -> 收敛到可达边界
target_far = np.array([2.5, 0.0])
th_far, _ = ik_jacobian_transpose(target_far, links, init=[0.3, 0.3])
p_far = fk2(th_far, links)
print(f"验证2: 不可达目标 (2.5, 0) 时,IK 收敛到可达边界 {np.round(p_far, 3)},"
f"残差 {np.linalg.norm(p_far - target_far):.3f}(符合预期)✔")
# ---------------- 画图:迭代轨迹 + 最终位姿 ----------------
fig, ax = plt.subplots(figsize=(6.5, 6.5))
ax.plot(trail[:, 0], trail[:, 1], '-', color='crimson', lw=1, alpha=0.7, label='IK 迭代中末端轨迹')
ax.plot(target[0], target[1], 'r*', ms=18, label='目标点')
ax.plot(p_sol[0], p_sol[1], 'go', ms=10, label='IK 收敛末端')
th = th_sol
j0 = np.zeros(2)
j1 = links[0]*np.array([np.cos(th[0]), np.sin(th[0])])
j2 = j1 + links[1]*np.array([np.cos(th[0]+th[1]), np.sin(th[0]+th[1])])
ax.plot([j0[0], j1[0], j2[0]], [j0[1], j1[1], j2[1]], '-o', color='seagreen', lw=4, ms=8, label='收敛姿态')
ax.set_aspect('equal'); ax.grid(alpha=0.3); ax.legend(fontsize=9)
ax.set_title('数值逆运动学(雅可比转置迭代)收敛过程')
plt.tight_layout()
plt.savefig('images/emb00_ik.png', dpi=120, bbox_inches='tight')
plt.show()
print("已保存图片: images/emb00_ik.png ✔")
验证1: 数值 IK 收敛,末端误差 = 9.64e-07 ✔
解: θ = (62.96°, -84.26°)
回代 FK(θ) = (1.2000, 0.6000) ≈ 目标 (1.2, 0.6) ✔
验证2: 不可达目标 (2.5, 0) 时,IK 收敛到可达边界 [ 1.795 -0.13 ],残差 0.717(符合预期)✔
已保存图片: images/emb00_ik.png ✔
C:\Users\24260\AppData\Local\Temp\ipykernel_15728\587274954.py:73: UserWarning: FigureCanvasAgg is non-interactive, and thus cannot be shown plt.show()
IK 收敛图解读:

- 红色轨迹是迭代过程中末端的位置,从初始猜测一步步"走"向目标(红星)。
- 绿色是收敛后的机械臂姿态:末端恰好落在目标点上,关节角就是数值 IK 的解。
- 若目标在可达范围外,迭代会停在可达边界上(验证 2 的输出),这正是数值法的"尽力而为"。
模型详解(面试深度)¶
本节把具身智能最底层的数学地基按面试高频顺序过一遍:从"刚体在空间中怎么表示",到"机械臂怎么算末端位姿、怎么反解关节角、怎么分析速度与奇异",再到"轮式底盘怎么动、手怎么抓、动力学怎么建"。每个方法按 定位 → 结构 → 公式 → 使用要点 → 优缺点 → 记忆点 六块展开,公式务必能自己推一遍。
1. 空间位姿表示三件套:旋转矩阵、欧拉角、四元数¶
一句话定位:三种等价的刚体姿态描述——旋转矩阵冗余但通用,欧拉角直观但有万向锁,四元数紧凑且无奇异,工程上常混合使用。
结构/流程:
- 旋转矩阵 $R\in SO(3)$:$SO(3)=\{R\in\mathbb{R}^{3\times3}\mid R^TR=I,\ \det R=+1\}$,用 9 个数承载 3 个自由度(6 个正交约束),$R^{-1}=R^T$。
- 欧拉角(ZYX 顺序):先绕 $z$ 转 yaw $\psi$,再绕 $y$ 转 pitch $\theta$,最后绕 $x$ 转 roll $\phi$,即 $R=R_z(\psi)R_y(\theta)R_x(\phi)$。
- 四元数 $q=(w,x,y,z)=\big(\cos\frac{\theta}{2},\ \sin\frac{\theta}{2}\,u\big)$:单位四元数 $\|q\|=1$ 对应 $SO(3)$ 单位球 $S^3$ 上的点,且 $q$ 与 $-q$ 表示同一旋转(双覆盖)。
- 万向锁的几何本质:$\theta=\pm\frac{\pi}{2}$ 时 $R_z(\psi)$ 与 $R_x(\phi)$ 的旋转轴重合,第三个旋转被前两个吸收,姿态只剩两个自由度,且该奇异性是欧拉角参数化的固有缺陷、无法靠换顺序完全消除。
核心公式:
$$R = R_z(\psi)R_y(\theta)R_x(\phi)=\begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi-s_\psi c_\phi & c_\psi s_\theta c_\phi+s_\psi s_\phi\\ s_\psi c_\theta & s_\psi s_\theta s_\phi+c_\psi c_\phi & s_\psi s_\theta c_\phi-c_\psi s_\phi\\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix}$$
四元数乘法与向量旋转:
$$q_1\otimes q_2=\big(w_1w_2-v_1\cdot v_2,\ w_1v_2+w_2v_1+v_1\times v_2\big),\qquad v'=q\,v\,q^{-1},\quad q^{-1}=\frac{q^*}{\|q\|^2}$$
$R\to q$(Shepperd 法,选最大对角元做分母避免除以小量):
$$q_w=\tfrac12\sqrt{1+\operatorname{tr}R},\qquad q_x=\frac{R_{32}-R_{23}}{4q_w},\ q_y=\frac{R_{13}-R_{31}}{4q_w},\ q_z=\frac{R_{21}-R_{12}}{4q_w}$$
$q\to R$ 与 $q\to$ ZYX 欧拉角:
$$R=\begin{bmatrix} 1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx)\\ 2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) \end{bmatrix}$$
$$\psi=\operatorname{atan2}\big(2(xy+wz),\ 1-2(y^2+z^2)\big),\quad \theta=\operatorname{asin}\big(2(wy-xz)\big),\quad \phi=\operatorname{atan2}\big(2(yz+wx),\ 1-2(x^2+y^2)\big)$$
$R\to$ ZYX 欧拉角(用 atan2 双参数版提升数值鲁棒性):
$$\theta=\operatorname{atan2}\big(-R_{31},\ \sqrt{R_{11}^2+R_{21}^2}\big),\quad \psi=\operatorname{atan2}(R_{21},R_{11}),\quad \phi=\operatorname{atan2}(R_{32},R_{33})$$
训练/使用要点:$R$ 反解欧拉角在 $\theta=\pm\pi/2$(万向锁)时 $\cos\theta\to0$,$R_{11},R_{21},R_{32},R_{33}$ 同时趋零、只剩 $\psi\pm\phi$ 可恢复,这是欧拉角最常被拷打的点;四元数在滤波与插值中首选,slerp 公式为 $\operatorname{slerp}(q_1,q_2,t)=\frac{\sin((1-t)\Omega)q_1+\sin(t\Omega)q_2}{\sin\Omega}$($\Omega$ 为两四元数夹角),插值前先保证 $q_1\cdot q_2\ge0$ 以消除双覆盖歧义,且每个控制周期必须重归一化 $q\leftarrow q/\|q\|$,否则尺度漂移污染旋转;矩阵级联最直观但需周期性正交化,欧拉角适合人机交互显示。
优缺点:矩阵——通用、易复合与求导,但 9 参数冗余、需正交化;欧拉角——直观参数少,但万向锁且插值不平滑;四元数——4 参数紧凑、无奇异、便于复合与插值,但不直观、双覆盖有歧义。
面试记忆点:口诀"矩阵正交行列式一,欧拉 ZYX 怕锁死,四元数归一化后才算姿态"。
| 表示 | 参数数 | 约束 | 奇异情况 | 典型用途 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 6(正交 + det=1) | 无 | 坐标变换、级联、求导 |
| 欧拉角 | 3 | 无 | 万向锁(θ=±90°) | 显示、接口、人机交互 |
| 四元数 | 4 | 1(|q|=1) | 无 | 滤波、插值、嵌入式运算 |
2. 齐次变换矩阵 T∈SE(3)¶
一句话定位:把旋转和平移打包成 $4\times4$ 矩阵,统一描述刚体位姿与坐标系链式变换。
结构/流程:$T=\begin{bmatrix}R&t\\0&1\end{bmatrix}$,$SE(3)=\{(R,t)\mid R\in SO(3),\ t\in\mathbb{R}^3\}$;坐标变换满足下标消去律 $T_c^a=T_b^a\,T_c^b$,点坐标 $p_c=T_b^a\,p_b$。
核心公式:
$$T=\begin{bmatrix}R&t\\0&1\end{bmatrix},\qquad T^{-1}=\begin{bmatrix}R^T&-R^Tt\\0&1\end{bmatrix}$$
$$T_{\text{new}}=T_{\text{world}}\,T_{\text{old}}\ (\text{世界系左乘}),\qquad T_{\text{new}}=T_{\text{old}}\,T_{\text{body}}\ (\text{本体系右乘})$$
训练/使用要点:左乘=变换作用在世界坐标系(动"世界"),右乘=变换作用在当前本体系(动"自己")——"先绕世界 z 转 90° 再前进"与"先前进再绕自身 z 转 90°"结果不同,面试常出选择题;求逆用解析式 $[R^T,-R^Tt]$ 而非数值求逆;若矩阵含缩放/剪切则不属于 $SE(3)$,用 $3\times4$ 仿射矩阵;多坐标系用 TF 树(ROS tf2)管理,任意两坐标系间沿树相乘即可求得,例如相机–末端–基座的链 $T_{\text{base}}^{\text{cam}}=T_{\text{base}}^{\text{ee}}\,T_{\text{ee}}^{\text{cam}}$,级联后注意浮点误差累积。
优缺点:优点——形式统一、可复合可求逆、利于 TF 树与批量运算;缺点——4×4 冗余 12 个有效参数、级联后误差漂移、含缩放会破坏李群结构。
面试记忆点:"左乘动世界、右乘动本体,$T^{-1}$ 是 $R$ 转置、$t$ 乘上 $-R^T$"。
3. DH 参数建模与正运动学¶
一句话定位:用 4 个参数规范化相邻关节坐标系的关系,连乘所有 A 矩阵即得末端位姿(正运动学)。
结构/流程:每个连杆 $i$ 建立坐标系 $\{i\}$,$A_i$ 表示 $\{i-1\}\to\{i\}$ 的变换;标准 DH 顺序为绕 $z_{i-1}$ 转 $\theta_i$、沿 $z_{i-1}$ 移 $d_i$、沿 $x_i$ 移 $a_i$、绕 $x_i$ 转 $\alpha_i$;改进 DH(Craig)顺序为绕 $x_{i-1}$ 转 $\alpha_{i-1}$、沿 $x_{i-1}$ 移 $a_{i-1}$、绕 $z_i$ 转 $\theta_i$、沿 $z_i$ 移 $d_i$。
核心公式(记 $c_\theta=\cos\theta,\ s_\theta=\sin\theta$)。标准 DH:
$$A_i=\begin{bmatrix} c_{\theta_i} & -s_{\theta_i}c_{\alpha_i} & s_{\theta_i}s_{\alpha_i} & a_i c_{\theta_i}\\ s_{\theta_i} & c_{\theta_i}c_{\alpha_i} & -c_{\theta_i}s_{\alpha_i} & a_i s_{\theta_i}\\ 0 & s_{\alpha_i} & c_{\alpha_i} & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix}$$
改进 DH:
$$A_i=\begin{bmatrix} c_{\theta_i} & -s_{\theta_i} & 0 & a_{i-1}\\ s_{\theta_i}c_{\alpha_{i-1}} & c_{\theta_i}c_{\alpha_{i-1}} & -s_{\alpha_{i-1}} & -d_i s_{\alpha_{i-1}}\\ s_{\theta_i}s_{\alpha_{i-1}} & c_{\theta_i}s_{\alpha_{i-1}} & c_{\alpha_{i-1}} & d_i c_{\alpha_{i-1}}\\ 0 & 0 & 0 & 1 \end{bmatrix}$$
正运动学(末端相对基座位姿):
$$T_N^0=A_1A_2\cdots A_N=\prod_{i=1}^{N} A_i$$
训练/使用要点:建模先画关节轴 $z$,再作公垂线定 $x$ 轴;标准 DH 中 $z_{i-1}$ 是关节 $i$ 的转轴、坐标系 $\{i\}$ 原点落在关节 $i+1$ 上,改进 DH 中 $z_i$ 是关节 $i$ 的转轴、$\alpha_{i-1},a_{i-1}$ 属于连杆 $i-1$ 的几何;转动关节 $\theta_i$ 是变量、其余参数为常量;树形/并联结构用改进 DH 无歧义,工业手册多按标准 DH;可先用符号计算验证 $T_N^0$ 与几何直觉一致再写代码。快速自检例子:平面二连杆臂末端位置应满足 $p=\big(a_1\cos\theta_1+a_2\cos(\theta_1+\theta_2),\ a_1\sin\theta_1+a_2\sin(\theta_1+\theta_2)\big)$,连乘两个 $A$ 矩阵应能还原同一结果。
优缺点:标准 DH——教科书主流、与 URDF/工业手册习惯接近,但分支结构下同一连杆被多个 A 引用易歧义;改进 DH——连杆几何只出现在自己的矩阵中、适合树结构,但参数下标含义与标准 DH 不同、容易记混。
面试记忆点:"标准 DH 转 z-移 z-移 x-转 x,改进 DH 先 x 后 z、$\alpha,a$ 挂 $i-1$;末端位姿就是 $A$ 连乘"。
| 约定 | 变换顺序 | 参数下标 | z 轴对应 | 适用场景 |
|---|---|---|---|---|
| 标准 DH | Rot_z · Trans_z · Trans_x · Rot_x | $\theta_i,d_i,a_i,\alpha_i$ | $z_{i-1}$=关节 $i$ 轴 | 串联臂、工业手册 |
| 改进 DH | Rot_x · Trans_x · Rot_z · Trans_z | $\alpha_{i-1},a_{i-1},\theta_i,d_i$ | $z_i$=关节 $i$ 轴 | 树形/并联、Craig 教材 |
4. 逆运动学:解析解与数值迭代¶
一句话定位:已知末端期望位姿求关节角——满足 Pieper 条件就解析解耦,否则用雅可比迭代数值求解。
结构/流程:解析法(Pieper 条件:6 自由度臂存在三个相邻转动关节轴线交于一点,典型是球形腕)把问题解耦为"前三关节定腕心位置、后三关节定姿态";数值法把位姿误差 $e$ 经雅可比映射为关节增量 $\Delta q=J^\dagger e$,迭代至收敛。
核心公式。位置/姿态解耦:
$$p_d=f(q_1,q_2,q_3),\qquad R_d=R_{03}(q_1,q_2,q_3)\,R_{36}(q_4,q_5,q_6)\ \Rightarrow\ R_{36}=R_{30}^T R_d$$
雅可比迭代与伪逆:
$$e=\begin{bmatrix}p_d-p(q)\\ \operatorname{rot}(R_dR(q)^T)\end{bmatrix},\qquad \Delta q=J^\dagger e,\qquad q\leftarrow q+\Delta q$$
$$J^\dagger=J^T(JJ^T)^{-1}\quad(\text{行满秩时})$$
阻尼最小二乘(DLS,Levenberg–Marquardt 形式):
$$\min_{\Delta q}\ \|J\Delta q-e\|^2+\lambda^2\|\Delta q\|^2\ \Rightarrow\ \Delta q=J_\nu e,\qquad J_\nu=J^T(JJ^T+\lambda^2 I)^{-1}$$
训练/使用要点:解析解确定、可枚举全部解、适合在线控制,但要求构型满足 Pieper 条件且对几何标定误差敏感,工程上对常见 6R 球形腕构型会优先用 IKFast 等符号求解器预生成闭式解;数值解通用(冗余臂、人形、任意构型),需好初值、检查关节限位、以 $\|e\|<\varepsilon$ 判收敛;奇异附近 $JJ^T$ 病态,用阻尼 $\lambda$(越大越稳、跟踪精度越低)或 SVD 截断;多解选择策略:取距当前位形最近的解,或按关节限位、能耗加权。
优缺点:解析——实时、可给出全部解,但依赖特殊几何;数值——通用、易加约束与利用冗余,但依赖初值、可能局部极小、奇异处必须正则化。
面试记忆点:"Pieper 三轴交一点就能解耦;数值解记 $J^\dagger$ 与 $\lambda^2$ 阻尼,奇异不炸"。
5. 雅可比矩阵¶
一句话定位:关节速度到末端六维速度的线性映射 $v=J\dot q$,是速度、静力与可操作度分析的枢纽。
结构/流程:几何法按列构造——转动关节取"轴叉乘位置臂"列、移动关节取轴向量列;微分法对正运动学解析求导得解析雅可比,两者通过欧拉角速率与角速度关系 $\omega=E(\phi)\dot\phi$ 关联(后者在万向锁处奇异)。
核心公式:
$$v=\begin{bmatrix}\dot p_e\\\omega\end{bmatrix}=J(q)\,\dot q,\qquad J_i^{\text{rev}}=\begin{bmatrix}z_{i-1}\times(p_e-p_{i-1})\\ z_{i-1}\end{bmatrix},\qquad J_i^{\text{pris}}=\begin{bmatrix}z_{i-1}\\0\end{bmatrix}$$
静力对偶映射与可操作度(Yoshikawa):
$$\tau=J^T F\quad(\text{虚功原理}),\qquad w(q)=\sqrt{\det(JJ^T)}$$
可操作性椭圆($\|q\|_2\le1$ 时 $J$ 的像):
$$\{v:\|J^\dagger v\|\le1\}\iff v^T(JJ^T)^{-1}v\le1,\qquad \text{半轴长}=\text{奇异值 }\sigma_1,\dots,\sigma_m$$
训练/使用要点:奇异位形 $\det J=0$($6\times6$)或 rank 下降,典型三类:边界奇异(臂完全伸直/收回,处工作空间边界)、腕部奇异($z_4\parallel z_6$,腕两轴共线、末端某方向不可达)、肩部奇异(腕心落在肩轴延长线上);奇异处 $\dot q=J^{-1}v\to\infty$,表现为关节速度爆表;用可操作度 $w$ 监控接近奇异的程度、用 SVD 看最小奇异值;冗余臂($n>6$)用 $J^\dagger$ 给最小范数关节速度。对偶地,末端单位力 $\|F\|\le1$ 经 $\tau=J^TF$ 映射到关节空间构成力椭球,半轴长 $=1/\sigma_i$——速度灵巧的方向正是力孱弱的方向,这个对偶关系常被拿来考理解深度。
优缺点:优点——一阶线性、同时编码速度/静力/可操作度,是规划与控制的中介;缺点——位形相关、奇异处病态、加速度需额外求 $\dot J$。
面试记忆点:"$J$ 的列是 $z\times(p_e-p)$ 与 $z$,$\det=0$ 即奇异,可操作度是奇异值乘积"。
6. 移动机器人运动学¶
一句话定位:把轮速映射为车体线/角速度,再在非完整约束下积分得到位姿估计(里程计)。
结构/流程:差速驱动——两轮独立驱动,由 $(\omega_L,\omega_R)$ 合成 $(v,\omega)$;阿克曼转向——汽车式前轮转向,低速下用自行车模型近似;里程计——对 $(v,\omega)$ 做时间积分得 $(x,y,\theta)$。
核心公式($r$ 轮半径、$2b$ 轮距):
$$v=\frac{r}{2}(\omega_L+\omega_R),\qquad \omega=\frac{r}{2b}(\omega_R-\omega_L),\qquad R=\frac{v}{\omega}=b\,\frac{\omega_L+\omega_R}{\omega_R-\omega_L}$$
单车模型与非完整约束(不可瞬时侧滑):
$$\begin{bmatrix}\dot x\\\dot y\\\dot\theta\end{bmatrix}=\begin{bmatrix}v\cos\theta\\ v\sin\theta\\ \omega\end{bmatrix},\qquad \dot x\sin\theta-\dot y\cos\theta=0$$
阿克曼/自行车模型($L$ 轴距、$\delta$ 前轮转角;ICR 在后轴延长线与前轮垂线交点):
$$\omega=\frac{v}{L}\tan\delta,\qquad R=\frac{L}{\tan\delta}$$
里程计积分:
$$\theta\leftarrow\theta+\omega\,\Delta t,\qquad x\leftarrow x+v\cos\theta\,\Delta t,\qquad y\leftarrow y+v\sin\theta\,\Delta t$$
训练/使用要点:差速可原地旋转($\omega_L=-\omega_R$);轮径、轮距标定误差是里程计漂移主因,可用 UMBmark 类标定实验补偿;轮子打滑或悬空时编码器仍计数、里程计虚增位移,融合时通常把 $(v,\omega)$ 当控制输入、把里程计当观测残差;阿克曼低速无侧滑时成立、高速须考虑横向动力学,内/外前轮转角不同($\cot\delta_o-\cot\delta_i=T/L$,$T$ 轮距)以保证共 ICR;里程计短期可靠、长期漂移,需与 IMU/激光/视觉融合(卡尔曼滤波或因子图)。
优缺点:差速——结构简单、能原地转,但长距离漂移、越障差;阿克曼——运动与汽车一致、侧滑小,但不能原地转、转弯半径受限。
面试记忆点:"差速 $v$ 取和、$\omega$ 取差;自行车 $\omega=v\tan\delta/L$;里程计积分必漂、标定保命"。
7. 末端执行器与抓取约束¶
一句话定位:执行器是机器人与物体接触的接口,抓取成败取决于接触几何、摩擦与力/形封闭性质。
结构/流程:平行夹爪(两指平移开合、面/线接触)、吸盘(真空负压、单点面接触)、灵巧手(多指多关节、包络/捏取);抓取分析链路:接触点 → 摩擦锥 → 判断能否抵抗任意外力(力封闭)或纯几何锁死(形封闭)。
核心公式(Coulomb 摩擦锥与抓取旋量 $wrench$):
$$\|f_t\|\le\mu\,f_n,\qquad w=\begin{bmatrix}f\\ p\times f\end{bmatrix}\ (\text{接触点 }p\text{ 相对质心的旋量})$$
力封闭判据:所有接触点 primitive wrench 的凸包包含原点于内部,即对任意外 wrench 都能在摩擦锥内选组合力抵抗;形封闭:无摩擦点接触下几何包围物体、任意微小运动都被刚性接触阻断(3D 一般需 7 个无摩擦点接触、2D 需 4 个)。
训练/使用要点:夹爪宜用柔顺力控与夹持力反馈,软指垫提升等效 $\mu$;吸盘要求表面密封、非多孔,需真空度监控与脱落检测,重物多用多吸盘;灵巧手自由度多,抓取规划常用"力封闭性、摩擦余量、可操作椭球"等质量指标筛选候选抓取;仿真中先以抓取质量指标筛位姿再迁移真机,注意仿真/真实摩擦系数与柔顺差异;实际抓取流程分接近–接触–施力三阶段,前两阶段用位控、接触后用柔顺/力控,避免硬压损伤物体与机构。
优缺点:夹爪——简单可靠、成本低,但形状适配差;吸盘——对平整面高效、不伤表面,但透气/曲面/污染面失效;灵巧手——灵活通用,但贵、控制与感知难、鲁棒性差。
面试记忆点:"摩擦锥兜得住任意外力叫力封闭,纯几何卡死叫形封闭;抓取先看锥、再看凸包"。
8. 刚体动力学基础(建模与仿真)¶
一句话定位:建立关节力矩与加速度的关系 $M\ddot q+C\dot q+g=\tau$,是仿真器与后续控制(计算力矩、阻抗、MPC)的地基。
结构/流程:牛顿–欧拉法——前向递推(基座→末端)算各连杆角速度/角加速度/质心加速度,后向递推(末端→基座)算关节力/力矩,复杂度 $O(n)$;拉格朗日法——由动能减势能 $L=K-P$ 对广义坐标求导,得到封闭的 $M,C,g$。
核心公式(拉格朗日方程及其机器人形式):
$$\frac{d}{dt}\frac{\partial L}{\partial\dot q}-\frac{\partial L}{\partial q}=\tau\ \Rightarrow\ M(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau$$
牛顿–欧拉递推(转动关节,示意):
$$\omega_{i+1}=\omega_i+\dot\theta_{i+1}\,z_i,\qquad a_{c,i}=a_i+\alpha_i\times r_{i,c}+\omega_i\times(\omega_i\times r_{i,c})$$
$$f_i=f_{i+1}+m_i a_{c,i},\qquad n_i=n_{i+1}+r_{i,c}\times f_i+I_i\alpha_i+\omega_i\times I_i\omega_i,\qquad \tau_i=n_i\cdot z_i$$
关键性质与计算力矩控制:
$$M=M^T\succ0,\qquad \dot M-2C\ \text{反对称}\ \Rightarrow\ \tau=M(q)\big(\ddot q_d+K_v\dot e+K_p e\big)+C(q,\dot q)\dot q+g(q)$$
训练/使用要点:$g(q)$ 重力项在低速占主导,控制必须前馈补偿;$C\dot q$ 与速度平方相关,高速运动显著;惯性参数可用激励轨迹采集 $(q,\dot q,\ddot q,\tau)$ 做线性回归辨识(动力学对惯性参数线性);仿真步进时由 $\tau$ 解广义加速度 $\ddot q=M^{-1}(\tau-C\dot q-g)$ 再数值积分(多用半隐式欧拉),闭链与接触约束用拉格朗日乘子或摩擦锥求解器处理,不同仿真器(MuJoCo/PyBullet/Isaac)的接触与摩擦模型差异是 sim2real 差距的主要来源之一;计算力矩控制把非线性系统精确线性化,是阻抗控制、MPC、全身控制(WBC)的起点。
优缺点:牛顿–欧拉——$O(n)$ 高效、显式含约束力、适合实时控制,但实现繁琐、需逐连杆惯性参数;拉格朗日——方程紧凑、显式 $M/C/g$ 便于分析与设计控制器,但 $O(n^2)$、推导繁琐。
面试记忆点:"$M$ 惯性、$C$ 科氏、$g$ 重力,$\dot M-2C$ 反对称是白送的考点;先补偿 $g$ 再 PD 反馈就是计算力矩"。
面试考点¶
机器人算法岗高频考点,Q&A 形式,含追问与 30 秒速答。
Q1 旋转矩阵的性质¶
Q: 旋转矩阵必须满足哪些性质? A: $R^T R = I$(正交)且 $\det(R) = +1$(右手系);三列分别是体坐标系三轴在世界系中的投影。 追问: 怎么判断一个矩阵是不是合法旋转矩阵?→ 验证正交性 + 行列式;若 $\det = -1$ 是反射(镜像),不是旋转。 30 秒速答: 正交保长保角、det=+1 保手性;列向量 = 坐标轴投影。
Q2 欧拉角 / 四元数¶
Q: 为什么机器人计算常用四元数而不是欧拉角? A: 欧拉角有万向锁(中间角 $\pm 90°$ 时两轴重合、丢失自由度)且插值不平滑;四元数 4 个参数无奇点、slerp 插值自然、存储紧凑。 追问: 四元数怎么转旋转矩阵?→ 由 Rodrigues 公式或 $R = I + 2w[u]_\times + 2[u]_\times^2$(对单位四元数)得到。 30 秒速答: 万向锁 + 插值问题 → 换四元数。
Q3 齐次变换矩阵¶
Q: 为什么用 4×4 齐次矩阵表示位姿?最后一行为什么是 [0 0 0 1]? A: 把旋转和平移统一成矩阵乘法,复合、求逆、链式变换全部封闭;最后一行保证乘法后矩阵结构不变。 追问: 求逆公式?→ $T^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0 & 1 \end{bmatrix}$。 30 秒速答: 升维统一乘法;逆 = 旋转转置 + 平移项取 $-R^T t$。
Q4 变换复合顺序¶
Q: T = T_1 @ T_2 是先做哪个变换?左乘右乘什么区别?
A: 先 $T_1$ 后 $T_2$;右乘(新变换在右边)= 相对当前(动)坐标系,左乘 = 相对世界(固定)坐标系。
追问: 怎么用代码验证?→ 让新变换纯旋转:右乘时原点不动、左乘时原点被转走。
30 秒速答: 右乘动系、左乘静系;下标链消去法秒判。
Q5 DH 参数与正运动学¶
Q: DH 四参数各自含义?正运动学怎么算? A: a 连杆长度、α 连杆扭转、d 关节偏距、θ 关节角;FK 就是 $T_{0n} = A_1 A_2 \cdots A_n$ 连乘。 追问: 标准 DH 和改进 DH 区别?→ 坐标系附着位置不同(标准:坐标系在连杆起点;改进:在连杆末端),z 轴都沿关节轴。 30 秒速答: 4 参数"简历" + 连乘 = FK。
Q6 逆运动学¶
Q: IK 为什么比 FK 难?数值法怎么做? A: IK 是非线性反问题,可能多解/无解/奇异;数值法常用雅可比转置($J^T$)、伪逆($J^+$)、阻尼最小二乘($(J^T J + \lambda I)^{-1}J^T$)迭代。 追问: 多解怎么选?→ 结合关节限位、避障、尽量贴近当前姿态(最小关节变化)。 30 秒速答: 果→因多解;迭代求解,加阻尼防奇异。
Q7 雅可比矩阵¶
Q: 雅可比矩阵是什么?奇异位形呢? A: 关节速度 → 末端速度的线性映射 $\dot{x} = J\dot{\theta}$;旋转关节列 $J_i = \begin{bmatrix} z_i \times (p_e - p_i) \\ z_i \end{bmatrix}$;$\det(J) = 0$ 时奇异,某方向自由度丢失。 追问: 冗余机械臂怎么处理?→ 关节数 > 末端自由度,$J$ 非方阵,用伪逆求最小范数解。 30 秒速答: 速度翻译官;奇异 = 某方向动不了。
Q8 轨迹规划¶
Q: 关节空间 vs 任务空间轨迹的区别? A: 关节空间:直接对关节角插值(简单、不奇异、但末端路径不可控);任务空间:对末端位姿插值(能走直线,但每点要解 IK、可能过奇异)。 追问: 实际工程怎么选?→ 关节空间 + 检查路径是否碰撞/奇异;要精确直线时用任务空间 + 奇异规避。 30 秒速答: 关节空间稳、任务空间直。
小结与下一章预告¶
小结: 本篇把机器人的"身体语言"完整过了一遍——
- 位姿 = 位置 + 姿态,共 6 自由度;
- 姿态有三种语言:旋转矩阵(严谨)、欧拉角(直观但会万向锁)、四元数(无奇点、工程主流);
- 齐次变换矩阵把旋转平移装进一个 4×4 矩阵,复合、求逆全封闭;
- 变换复合记住一句话:右乘 = 当前系,左乘 = 世界系;
- DH 参数 + FK 从关节角算末端位姿(唯一);IK 从末端位姿反推关节角(几何法/数值法);
- 雅可比在速度域给"关节 ↔ 末端"搭桥,也是奇异与力控的关键;
- 轨迹规划分关节空间与任务空间,各有取舍。
面试高频三件套:手写旋转矩阵、齐次变换乘法、雅可比与奇异——本篇代码实战全部手写验证过。
下一章预告: 机器人要"动手",先得"看懂"世界。下一篇 01-感知与环境理解 讲相机模型(针孔/内参外参/畸变)、RGB-D 与点云、坐标变换、ICP 配准、3D 检测/占据栅格和 SLAM——把"像素"变成机器人能用的"3D 场景"。
