【机器人学习】小白学习第02讲：位姿、旋转矩阵与姿态表示

一句话核心：用 3 个数说清「在哪儿」，用 9 个数（实际只有 3 个自由度）说清「朝哪儿」；最后把这两件事塞进一个 4×4 矩阵里。

• 本讲目标：会写位姿 = (位置, 姿态)；理解旋转矩阵的三重含义与两条乘法规则（内旋后乘 / 外旋前乘）；掌握欧拉角、角轴、四元数、齐次变换四种工具各自的用处

一、为什么需要这个

小讲 1：位姿 = 位置 + 姿态

快递员必须说清的两件事

先打个比方。快递员要把包裹送到你手上，必须说清两件事：

• 位置：哪一栋楼、哪个房间门牌号（用 3 个坐标就能写清）。
• 姿态：包裹正面向哪儿（书要正着插进书架，还是可以侧着、倒着）。

只有位置没有姿态，机器人手爪可能会朝下往工件上「砸」而不是「夹」。这两件事合起来，给了个专门名词：

位姿 pose

位置 position+姿态 orientation，合称 pose。刚体 rigid body：运动中任意两点距离保持不变的理想物体——先假设它不会弯。

一个刚体在空间里被它的位姿完全描述。这句话是地基，后面所有公式都是从它长出来的。

为什么需要一套「翻译规则」

还有一个更根本的动机（ME625 的坐标系变换也讲这个）：同一根向量，在不同坐标系里写出来数值不同。我们需要一套「翻译规则」，把同一个物理对象在两个系之间搬来搬去。旋转矩阵就是这套规则。

停下来想一想

停下来想一想：只知道位置、不知道姿态，机器人手爪去夹工件时会出什么状况？位姿 pose 由哪两件事合成？

二、核心概念

小讲 2：位置是约束向量，姿态是九个方向余弦

2.1 位置：一个约束向量

在刚体上挑一个点 O′，它在参考系 O–xyz 中的位置写成：

$$o^{\prime }=o_x^{\prime }x+o_y^{\prime }y+o_z^{\prime }z$$

符号	含义
o′	从参考系原点 O 指向刚体上点 O′ 的向量
x, y, z	参考系三根轴的单位向量
o′x, o′y, o′z	向量 o′ 在三根轴上的（标量）分量

这里要强调：位置向量是约束向量 bound vector——不但方向和长度定死了，作用点和作用线也定死了。对比一下「力」是自由向量（可以平移），位置不行，这就是后面齐次变换要小心的地方。

2.2 姿态：把刚体系的三个单位向量写下来

位置解决完，剩下一个问题：这个刚体自身「朝哪儿」。做法很朴素——在刚体上钉一个坐标系 O′–x′y′z′，把它的三根单位向量用参考系表示出来：

$$\begin{array}{r}{x}^{\prime }=x_x^{\prime }x+x_y^{\prime }y+x_z^{\prime }z\\ y^{\prime }=y_x^{\prime }x+y_y^{\prime }y+y_z^{\prime }z\\ z^{\prime }=z_x^{\prime }x+z_y^{\prime }y+z_z^{\prime }z\end{array}$$

这九个数就是方向余弦 direction cosines：x′_x其实是 x′ 与 x 夹角的余弦。直觉上：姿态 = 三个新轴分别在老坐标轴上的投影。

把这九个数排成一个 3×3 矩阵，就得到全章的主角。

小讲 3：旋转矩阵的三重含义与四种姿态表示怎么选

图 1 旋转矩阵的构造与含义。把刚体系三根轴的方向余弦按「竖着摞」排成三列，就得到 R；每一列本身就是一根单位轴，这直接导致了正交约束。

2.3 旋转矩阵：一个矩阵，三种含义

$$\begin{array}{r}R=[x^{\prime }{y}^{\prime }{z}^{\prime }]=\\ \left[\begin{array}{ccc}{x}_x^{\prime }& y_x^{\prime }& z_x^{\prime }\\ x_y^{\prime }& y_y^{\prime }& z_y^{\prime }\\ x_z^{\prime }& y_z^{\prime }& z_z^{\prime }\end{array}\right]\end{array}$$

符号	含义
R	旋转矩阵 rotation matrix，共 9 个元素
R 的第 1 列	刚体系 x′ 轴在参考系里的三个分量
R 的第 2 列	刚体系 y′ 轴在参考系里的三个分量
R 的第 3 列	刚体系 z′ 轴在参考系里的三个分量

末尾把这归纳成三条等价含义，请务必背下来，这是后面所有推导的开关：

1. 描述姿态：R 描述两个坐标系之间的相对朝向；它的列向量就是转动后那个系的轴在原参考系里的方向余弦。
2. 换坐标（两系原点重合时）：把同一个点在新系的坐标翻译成老系的坐标，p = R p′。
3. 旋转向量（同一个系内部）：把一根向量就地转一个角度（长度不变），p = R p′。

注意 2 和 3 公式长得一样，物理含义完全不同：2 是「同一个点换了一套记账方式」，3 是「真的把它转过去了」。判断哪一种是靠语境。

2.4 为什么 9 个数只有 3 个自由度

三列都不是随便给的向量，它们是同一个正交单位标架的三根轴，所以必须满足：

$$\begin{array}{r}{x}^{\prime \top }{y}^{\prime }=0\\ y^{\prime \top }{z}^{\prime }=0\\ z^{\prime \top }{x}^{\prime }=0\\ \text{（两两正交）}\\ x^{\prime \top }{x}^{\prime }=1\\ y^{\prime \top }{y}^{\prime }=1\\ z^{\prime \top }{z}^{\prime }=1\\ \text{（长度都是}1\text{）}\end{array}$$

一共6 个约束（3 个正交 + 3 个单位长度），于是：9 − 6 = 3 个独立自由度。这就解释了为什么确定空间姿态只需要三个参数（比如飞机：偏航、俯仰、横滚）。

6 个约束合成一条式子：RᵀR = I₃

这 6 个约束合成一个漂亮的矩阵式子：

$$\begin{array}{r}{R}^{\top }R=I_3\Rightarrow \\ R^{-1}=R^{\top }\end{array}$$

直觉：旋转矩阵求逆不用做任何计算，转置一下就是逆。这是全书用得最多的一条性质。

det® 的符号与 SO(3)

另外det(R) = +1（右手系）或det(R) = −1（左手系，相当于镜像，物理上做不到）。只考虑右手系时，R 属于特殊正交群 SO(3)；平面的 2×2 情形属于 SO(2)。

2.5 四种姿态表示该怎么选

既然只要 3 个数，为什么要发明好几种？因为它们各有各的毛病。一共给了四种：

表示法	用几个数	优点	致命缺点
旋转矩阵 R	9（实际 3 自由度）	乘法规则最简单、无奇异	冗余；数值积分久了会「不再正交」，要重新正交化
欧拉角（ZYZ / RPY）	3，最小表示	最直观，人能读懂，机器人示教器常用	存在表示奇异（万向锁）
角轴（ϑ, r）	4（−1 个约束 = 3）	几何意义极清晰：「绕某轴转某角」	(ϑ, r) 与 (−ϑ, −r) 表示同一个旋转，不唯一；ϑ=0 时奇异
单位四元数 Q = {η, ε}	4（−1 个约束 = 3）	无奇异、适合插值（SLERP）、计算量小	不直观；Q 与 −Q 是同一个旋转

一句话记处境：给人看用欧拉角，做插值和仿真用四元数，需要求逆/连乘时用旋转矩阵。

停下来想一想

停下来想一想：旋转矩阵有 9 个元素，为什么确定一个姿态只需要 3 个参数？那 6 个约束分别是什么？

三、关键公式

小讲 4：基本旋转矩阵与旋转合成的内旋 / 外旋规则

3.1 基本旋转：绕单根坐标轴转

规定：逆时针为正（从轴的正方向往原点看）。这三个小矩阵是所有复杂旋转的积木。

$$\begin{array}{r}{R}_z(\alpha )=\left[\begin{array}{ccccc}\cos & \alpha & -\sin & \alpha & 0\\ \sin & \alpha & \cos & \alpha & 0\\ 0& 0& 1& & \end{array}\right]\\ R_y(\beta )=\left[\begin{array}{ccccc}\cos & \beta & 0& \sin & \beta \\ 0& 1& 0& & \\ -\sin & \beta & 0& \cos & \beta \end{array}\right]\\ R_x(\gamma )=\left[\begin{array}{ccccc}1& 0& 0& & \\ 0& \cos & \gamma & -\sin & \gamma \\ 0& \sin & \gamma & \cos & \gamma \end{array}\right]\end{array}$$

符号	含义
α, β, γ	分别绕 z、y、x 轴的转角
Rk(−ϑ) = Rkᵀ(ϑ)	转负角等价于正角矩阵的转置（也是它的逆）

直觉：矩阵里那一行1和那一列0/0说明「转轴那根坐标没有变」；剩下 2×2 小块就是你熟悉的平面旋转矩阵，符号为[cos, −sin; sin, cos]。

3.2 旋转的合成：顺序为什么重要

用一套上/下标记法：Rij表示第 i 个系相对于第 j 个系的旋转矩阵，且转换公式是pj = Rji pi（上标 = 「用哪个系记账」，下标 = 「相对于谁」）。

$$\begin{array}{r}{p}^1=R_2^1{p}^2\\ p^0=R_1^0{p}^1\Rightarrow \\ R_2^0=R_1^0{R}_2^1\\ R_i^j=(R_j^i{)}^{-1}=(R_j^i{)}^{\top }\end{array}$$

于是有两条必须背死的规则：

相对「当前系」旋转（内旋）

每次转完，下次绕新系的轴转。
后乘（右乘）：R⁰₂ = R⁰₁ R¹₂。
像「先迈左腿再迈右腿」——每一步都以上一步结束时自己的朝向为基准。

相对「固定系」旋转（外旋）

每次都绕世界坐标那根不动的轴转。
前乘（左乘）：R̄⁰₂ = R̄¹₂ R⁰₁。
像「在地图上按固定的东南西北走」。

常见误区

矩阵乘法不可交换，所以旋转次序不能随便颠倒。Example 2.3 用图直接演示：先转 A 再转 B，和先转 B 再转 A，最终姿态完全不同。但有一个重要对偶：绕固定系「A 后 B」的效果，等于绕当前系「B 后 A」的效果。也就是说固定系与当前系的旋转序列成果互为倒序。

顺带记住的一句话：R 的列= 当前系轴在固定系里的方向余弦；R 的行（也就是 Rᵀ 的列）= 固定系轴在当前系里的方向余弦。

小讲 5：ZYZ 与 RPY 欧拉角，以及万向锁为什么会发生

3.3 欧拉角 ZYZ

思路：任何姿态都能拆成「绕不同轴转三次」，只要相邻两次不能绕平行轴。27 种组合里合法的有12 种，每一种叫一组欧拉角。重点讲两种。ZYZ 是工业机械臂球形腕的标准。

KaTeX parse error: Undefined control sequence: \phic at position 99: …egin{bmatrix} c\̲p̲h̲i̲c̲\varthetac\psi …

符号	含义
ϕ	第一步，绕参考系 z 轴转
ϑ	第二步，绕当前系 y′ 轴转
ψ	第三步，绕当前系 z″ 轴转
c·, s·	分别是 cos(·) 和 sin(·) 的简写

因为是相对当前系旋转，所以三个矩阵依次右乘——这就是那条规则的直接应用。

反过来，已知 R 求角（ϑ 取 (0, π) 那一支）：

$$\begin{array}{r}\varphi =Atan2(r_{23},r_{13})\\ \vartheta =Atan2(\sqrt{r_{13}^2+r_{23}^2},r_{33})\\ \psi =Atan2(r_{32},-r_{31})\end{array}$$

Atan2(y, x)是「双参数反正切」：它不是直接算 y/x，而是看两个参数各自的符号来判断象限，因此能把角度唯一地定在 2π 范围内。这是所有反解姿态代码的标配；用普通 arctan 会在第二、三象限出错。同时给出了 ϑ ∈ (−π, 0) 的第二组解。

3.4 RPY（ZYX）角

另一套常用欧拉角来自航海航空，用来描述飞行器姿态。

KaTeX parse error: Undefined control sequence: \phic at position 96: …egin{bmatrix} c\̲p̲h̲i̲c̲\vartheta & c\p…

符号	含义（航空叫法）
ψ	绕 x 轴，偏航 yaw
ϑ	绕 y 轴，俯仰 pitch
ϕ	绕 z 轴，横滚 roll

停下来想一想

和 ZYZ 的关键区别有两条：

① ZYZ 是「三次都相对当前系」→右乘；RPY 是「三次都相对固定系」→左乘。三个矩阵书写顺序看上去都是 z-y-x，但方向反过来理解不同。

② ZYZ 首尾同轴（都是 z），适合描述腕关节；RPY 三轴各不同，适合描述车辆/飞行器。

还给了一个有用的等价：绕固定系做 XYZ 顺序 = 绕当前系做 ZYX 顺序。

反解（ϑ 取 (−π/2, π/2)）：

$$\begin{array}{r}\varphi =Atan2(r_{21},r_{11})\\ \vartheta =Atan2(-r_{31},\sqrt{r_{32}^2+r_{33}^2})\\ \psi =Atan2(r_{32},r_{33})\end{array}$$

3.5 万向锁：理解表示奇异

常见误区

ZYZ 在 sin ϑ = 0（即 ϑ = 0 或 π）时反解退化；RPY 在 cos ϑ = 0（即 ϑ = ±π/2）时退化。

为什么会退化？ZYZ 情形下 ϑ = 0 意味着第一次和第三次转动的轴变成了平行轴（z 轴没被拧歪），于是 ϕ 和 ψ 的贡献无法区分——你只能求出 ϕ + ψ（或 ϕ − ψ）。这就是常说的万向锁 gimbal lock。

关键澄清：这是「表示」上的奇异，不是机器人真的动不了。明说这类位形刻画的是representation singularities（表示奇异）——机器人照样能从那里经过，只不过用这三个角来记账时会突然表达不出来。下一章讲雅可比时会遇到另一类「真的动不了」的运动学奇异，两者别混。

小讲 6：角轴、四元数与齐次变换——无奇异表示与位姿打包

3.6 角轴表示

与其绕三根轴各转一次，不如干脆说清楚：绕空间某根单位向量轴 r 直接转 ϑ 角。推导技巧是「把 r 甩到 z 轴上、转、再甩回去」：

$$\begin{array}{r}R(\vartheta ,r)=R_z(\alpha )R_y(\beta )R_z(\vartheta )R_y(-\beta )R_z(-\alpha )\\ \text{其中}\sin \alpha =r_y/\sqrt{r_x^2+r_y^2}\\ \cos \alpha =r_x/\sqrt{r_x^2+r_y^2}\\ \sin \beta =\sqrt{r_x^2+r_y^2}\\ \cos \beta =r_z\end{array}$$

代进去整理，得到著名的罗德里格斯公式 Rodrigues formula：

$$\begin{array}{r}R(\vartheta ,r)=\\ \left[\begin{array}{ccc}{r}_x^2(1-c\vartheta )+c\vartheta & r_x{r}_y(1-c\vartheta )-r_{z}s\vartheta & r_x{r}_z(1-c\vartheta )+r_{y}s\vartheta \\ r_x{r}_y(1-c\vartheta )+r_{z}s\vartheta & r_y^2(1-c\vartheta )+c\vartheta & r_y{r}_z(1-c\vartheta )-r_{x}s\vartheta \\ r_x{r}_z(1-c\vartheta )-r_{y}s\vartheta & r_y{r}_z(1-c\vartheta )+r_{x}s\vartheta & r_z^2(1-c\vartheta )+c\vartheta \end{array}\right]\end{array}$$

直觉：对角线上的rx²(1−cϑ)+cϑ是「沿轴的分量 + 垂直分量的余弦软化」；非对角那一坨(1−cϑ)加上反对称的±r·sϑ，反对称的部分就是叉乘矩阵，负责产生「垂直面内的转动」。

由 R 反解角度和轴：

$$\begin{array}{r}\vartheta ={\cos }^{-1}((r_{11}+r_{22}+r_{33}-1)/2)\\ r=[r_{32}-r_{23}{r}_{13}-r_{31}{r}_{21}-r_{12}{]}^{\top }/(2\sin \vartheta )\end{array}$$

符号	含义
r₁₁+r₂₂+r₃₃	矩阵的迹 trace；它只由 ϑ 决定，和轴的朝向无关
r	单位转轴，受约束 rx² + ry² + rz² = 1（所以 4 个数实际只有 3 个独立）

两个坑：① 由于R(−ϑ, −r) = R(ϑ, r)，同一姿态对应两组 (−ϑ,−r) 和 (ϑ,r)，表示不唯一；② sin ϑ = 0 时分母为零，公式失效；ϑ = 0（没转）时轴 r 完全任意。

3.7 单位四元数

四元数就是把上面的 4 个数换一种配法（η 是标量部分，ε 是向量部分）：

$$\begin{array}{r}\eta =\cos (\vartheta /2)\\ \epsilon =\sin (\vartheta /2)\cdot r\\ \text{约束：}{\eta }^2+{\epsilon }_x^2+{\epsilon }_y^2+{\epsilon }_z^2=1\end{array}$$

关键点是那个ϑ/2：半角。正因为取了半角，(ϑ, r)和(−ϑ, −r)算出来是同一个四元数，上一节的「表示不唯一」就被消掉了。

由四元数直接写旋转矩阵：

$$\begin{array}{r}R(\eta ,\epsilon )=\\ \left[\begin{array}{ccc}2({\eta }^2+{\epsilon }_x^2)-1& 2({\epsilon }_x{\epsilon }_y-\eta {\epsilon }_z)& 2({\epsilon }_x{\epsilon }_z+\eta {\epsilon }_y)\\ 2({\epsilon }_x{\epsilon }_y+\eta {\epsilon }_z)& 2({\eta }^2+{\epsilon }_y^2)-1& 2({\epsilon }_y{\epsilon }_z-\eta {\epsilon }_x)\\ 2({\epsilon }_x{\epsilon }_z-\eta {\epsilon }_y)& 2({\epsilon }_y{\epsilon }_z+\eta {\epsilon }_x)& 2({\eta }^2+{\epsilon }_z^2)-1\end{array}\right]\end{array}$$

由 R 反解（约定 η ≥ 0，即 ϑ 落在 [−π, π]）：

KaTeX parse error: Expected '}', got '&' at position 194: …\cdot & \sqrt{ &̲ r_{11} & - & r…

提示

为什么要用四元数？（这是本讲最实用的一条）

①反解无奇异：对比的角轴公式在 sin ϑ = 0 时直接失效，(2.34)(2.35) 对任何旋转都成立。明说「no singularity occurs」。

②适合插值：两个姿态之间平滑过渡时，欧拉角会万向锁、旋转矩阵插值会「掉出 SO(3)」；四元数在单位球面上走最短路径（球面线性插值）就对了。说它在末端姿态轨迹规划中特别有优势。

③ 复合与求逆很便宜：求逆就是Q−1 = {η, −ε}（把向量部分变号）；两个姿态复合用四元数积：

Q₁ ∗ Q₂ = { η₁η₂ − ε₁ᵀε₂ , η₁ε₂ + η₂ε₁ + ε₁ × ε₂ }，单位元是{1, 0}。

3.8 齐次变换：一个矩阵同时装下旋转和平移

前面的旋转矩阵只能处理「两系原点重合」。真实情况必然还有平移。 (2.38) 给出 vector 加法形式：

KaTeX parse error: Double superscript at position 90: …1} = -R^{0}_{1}^̲{\top} o^{0}_{1…

符号	含义
o⁰₁	系 1 的原点 O₁ 在系 0 里的位置向量（平移部分）
R⁰₁ p¹	先把 P 在系 1 里的坐标「转」成系 0 的方向
p⁰	P 在参考系 0 里的坐标

图 2 齐次变换把「旋转 + 平移」合成一个矩阵。左边是向量三角形 p⁰ = o⁰₁ + R⁰₁p¹；右边是把这一步塞进 4×4 矩阵后的样子——左上块管姿态，右上块管位置。

为什么要给向量补一个 1

直觉：先转，再挪。每次都要写「向量加法 + 矩阵乘法」两步，连乘很多次就会很啰嗦。于是发明了一个小技巧——给向量多加一个永远是 1 的分量：

$$\begin{array}{r}\tilde{p}=[p^{\top }1{]}^{\top }\\ A_1^0=\left[\begin{array}{cc}{R}_1^0& o_1^0\\ 0^{\top }& 1\end{array}\right]\\ {\tilde{p}}^0=A_1^0{\tilde{p}}^1\end{array}$$

符号	含义
p̃	齐次坐标：在三维坐标后面补一个 1，变成 4 维
A⁰₁	4×4 齐次变换矩阵，属于特殊欧氏群SE(3) = ℝ³ × SO(3)
左上 3×3	旋转部分 R⁰₁
右上 3×1	平移部分 o⁰₁
最后一行	0 0 0 1；注人流：左三元素给非零值会产生透视效果，右下那个 1 改成别的数会产生缩放

齐次矩阵怎么求逆

逆矩阵有闭式，记住这个样子：

KaTeX parse error: Double superscript at position 62: …} = ⎡ R^{0}_{1}^̲{\top} \\ -R^…

常见误区

**注意：齐次矩阵不再正交，A⁻¹≠ Aᵀ。**别把旋转矩阵那套「转置即求逆」的直觉搬过来用。它的逆必须按上式算（旋转部分转置，平移部分还要取负并左乘那个转置）。

沿坐标系链一路连乘

最后是最有用的一条——多个坐标系可以连乘成一个矩阵：

$${\tilde{p}}^0=A_1^0{A}_2^1\dots A_n^{n-1}{\tilde{p}}^n$$

直觉：想知道最后一个系里的点在世界坐标哪儿？把沿途所有 4×4 矩阵按顺序左乘叠起来即可。这就是下一讲 DH 正运动学的核心操作。

停下来想一想

停下来想一想：绕固定系「先转 A 再转 B」，等于绕当前系按什么顺序转？为什么两种序列的效果互为倒序？

停下来想一想

停下来想一想：图 1 里 RᵀR = I₃ 带来哪条最常用的性质？图 2 里齐次矩阵为什么不再满足 A⁻¹ = Aᵀ？

五、例题 / 动手

例题 1：把 90° 转动用到一个点上

参考系与刚体系原点重合，刚体系相对参考系绕 z 轴逆时针转了 α = 90°。已知 P 在刚体系里的坐标 p′ = [1, 0, 0]ᵀ，求它在参考系里的坐标 p。

1. 写矩阵：cos 90° = 0，sin 90° = 1，所以 Rz(90°) = [ 0 −1 0; 1 0 0; 0 0 1 ]
2. 代入 (2.10)：p = R p′
3. 算：p = [0·1 + (−1)·0, 1·1 + 0·0, 0]ᵀ = [0, 1, 0]ᵀ
4. 检验：长度 √(0²+1²+0²) = 1，和原来一样——旋转不改变向量长度，这是 R 正交性的直接后果。

直觉上：刚体系的 x′ 轴转了 90° 之后指着参考系的 y 方向，所以刚体系里「沿自己 x′ 走 1」在参考系里就是「沿 y 走 1」，完美对上。

例题 2：顺序不能换（动手验证）

取 γ = 90° 绕 x、β = 90° 绕 y，分别算RxRy与RyRx，对比结果。

$$\begin{array}{r}{R}_x(90°)=\left[\begin{array}{c}100\\ 00-1\\ 010\end{array}\right]\\ R_y(90°)=\left[\begin{array}{c}001\\ 010\\ -100\end{array}\right]\\ R_x{R}_y=\left[\begin{array}{c}001\\ 100\\ 010\end{array}\right]\\ R_y{R}_x=\left[\begin{array}{c}010\\ 00-1\\ -100\end{array}\right]\end{array}$$

两个结果明显不同。动手建议：拿一本书当刚体。第一次先绕自己面前那根横轴翻 90°，再绕竖轴转 90°；然后交换顺序重做一次。你会看到最终封面朝向不一样。这就是「矩阵乘法不可交换」的物理版本。

例题 3：亲眼看见万向锁

取 ZYZ 欧拉角 ϑ = 0。此时R = Rz(ϕ) Ry′(0) Rz″(ψ) = Rz(ϕ) Rz(ψ) = Rz(ϕ+ψ)——三次转动塌缩成一次绕 z 的转动。

于是 ϕ = 30°、ψ = 50° 与 ϕ = 70°、ψ = 10° 给出完全相同的姿态。反解时你只能拿到和（或差）ϕ ± ψ，分不开它们。这就是为什么在 ZYZ 反解公式后面特别标注：sϑ = 0 时解退化。

停下来想一想

停下来想一想：把 R_x(90°) 和 R_y(90°) 交换顺序相乘，结果为什么不同？拿一本书按两种顺序各转一次，封面朝向一样吗？

六、常见误区

6.1 关于自由度与公式适用条件

常见误区

误区 1：以为 9 个元素是 9 个自由度。错。正交 + 单位长度带来 6 个约束，实际只有3 个自由度。通用的结论更漂亮：SO(m) 的最小表示需要m(m−1)/2个参数——SO(3) 要 3 个，平面的 SO(2) 只要 1 个（就是你熟悉的那一个转角）。

常见误区

误区 2：把 p = Rp′ 当成万能。这个式子只在两系原点重合时成立。有平移就必须用齐次形式 p̃⁰ = A⁰₁ p̃¹，或者记住完整公式 p⁰ = o⁰₁ + R⁰₁p¹。反过来由 p⁰ 求 p¹ 时也要记得减去 o⁰₁：p¹ = R⁰₁ᵀ (p⁰ − o⁰₁)。

6.2 关于奇异与「四个参数」

常见误区

误区 3：把「万向锁」当成机器人真的卡住了。这是表示奇异，只是用欧拉角记账时会出问题；机器人本体的运动能力毫无损失。真正让机器人动不了的是后面讲的运动学奇异（雅可比降秩），两者不是一回事。

常见误区

误区 4：认为四个旋转参数就等于四个自由度。角轴的 (ϑ, r) 有约束 r_x²+r_y²+r_z² = 1；四元数有 η²+ε_x²+ε_y²+ε_z² = 1。两个都叫「非最小表示」——多一个冗余换来的是没有奇异，这正是它们存在的理由。

停下来想一想

停下来想一想：角轴 (ϑ, r) 和四元数都有 4 个数，为什么说它们只有 3 个自由度？多出来的那个冗余换来了什么？

七、本讲小结

停下来想一想

停下来想一想：给人看用哪种姿态表示、做插值用哪种、连乘求逆用哪种？为什么欧拉角反解时要改用 Atan2 而不是普通 arctan？

要点回顾

1. 位姿 = 位置 + 姿态。位置是一根约束向量 o′；姿态由钉在刚体上的坐标系 O′–x′y′z′ 的三根单位轴决定。
2. 旋转矩阵 R 有三重等价含义：描述两系相对朝向 / 原点重合时的坐标变换 p = Rp′ / 就地旋转一个向量。它的列就是 x′, y′, z′ 的方向余弦。
3. RᵀR = I₃，R⁻¹ = Rᵀ，det® = +1。9 个数受 6 个约束，因此只有 3 个自由度。
4. 乘法规则：相对当前系（内旋）→ 后乘；相对固定系（外旋）→ 前乘。矩阵乘法不可交换，所以顺序一旦写错结果就错；两者的序列结果互为倒序。
5. 四种姿态工具：欧拉角直观但有表示奇异；角轴几何清楚但不唯一；四元数无奇异、适合插值；旋转矩阵最适合连乘求逆。最后用**齐次变换 A⁰₁ = [R⁰₁ o⁰₁; 0ᵀ 1]**把旋转和平移合成一个 4×4 矩阵，可沿坐标系链一路左乘。

**一句话总结：**姿态这回事，本质就是三根单位轴的摆放方式；矩阵是它的账本，欧拉角是它的口语，四元数是它的计算机表示，齐次变换则是把「位置 + 姿态」打包成一张可以连乘的票据。