姿态矩阵的表示方式及应用

姿态矩阵的表示方式及应用

路线概览

欧拉角（最直觉） → 旋转矩阵（从局部到全局） → 轴角（本质揭示） → 四元数（完美插值） → 6D 表示（深度学习最爱）

每一步都是为了解决前一步的致命缺陷，而且数学上可以严格推导。

---

0. 预备：旋转群 SO(3)SO(3)SO(3)

三维空间中的所有旋转构成特殊正交群：
SO(3)={R∈R3×3∣RTR=I,det⁡R=+1} SO(3)=\{ \mathbf{R}\in\mathbb{R}^{3\times3} \mid \mathbf{R}^T\mathbf{R}=\mathbf{I}, \det\mathbf{R}=+1 \} SO(3)={R∈R3×3∣RTR=I,detR=+1}

任给 R∈SO(3)\mathbf{R}\in SO(3)R∈SO(3)，它把刚体连体坐标系的三根轴（各自单位正交）变换到世界坐标系，矩阵的列就是连体轴在世界系下的坐标。

旋转合成直接用矩阵乘法，结合律成立且不引入歧义。这是唯一无任何奇异性、无任何冗余的全局表示，但它有9个参数，自身不适合直接做插值。

1：欧拉角 ------ 最简单，最危险

直觉来源

你用手去拧一个夹爪，会自然地说"先绕 Z 轴偏航 30°，再绕 Y 轴俯仰 15°，再绕 X 轴滚转 10°"。这就是欧拉角------用三个独立转角按固定顺序描述姿态。

数学本质

任意旋转可分解为三个基本旋转的乘积：
R=Rx(ϕ) Ry(θ) Rz(ψ) \mathbf{R} = \mathbf{R}_x(\phi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_z(\psi) R=Rx(ϕ)Ry(θ)Rz(ψ)

其中 ϕ,θ,ψ\phi,\theta,\psiϕ,θ,ψ 是角度，顺序可定义（如 XYZ 外旋或内旋）。三维向量 (ϕ,θ,ψ)(\phi,\theta,\psi)(ϕ,θ,ψ) 就是欧拉角，参数量最少（只有 3 个），也最符合人类直觉。

致命伤

• 万向节死锁 ：当俯仰角 θ=±90∘\theta = \pm 90^\circθ=±90∘ 时，绕 X 和绕 Z 的旋转轴在空间中对齐，丧失一个自由度，导致旋转轨迹在此处奇变。
• 周期性边界 ：角度在 ±180∘\pm 180^\circ±180∘ 附近表达不唯一，线性插值 179° → -179° 会绕 358° 长弧。这也是你末端姿态"剧烈抖动"的直接原因。

既然欧拉角只是旋转矩阵的一种参数化，我们自然要去看那个被当作"积木"的矩阵。

1.1 定义

选定一个旋转顺序（例如 Z-Y-X 内旋），任意旋转可分解为
R=Rz(ψ) Ry(θ) Rx(ϕ) \mathbf{R} = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_x(\phi) R=Rz(ψ)Ry(θ)Rx(ϕ)

其中基本旋转矩阵为：
Rx(ϕ)=1000cos⁡ϕ−sin⁡ϕ0sin⁡ϕcos⁡ϕ,  Ry(θ)=cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ,  Rz(ψ)=cos⁡ψ−sin⁡ψ0sin⁡ψcos⁡ψ0001 \mathbf{R}_x(\phi)=\begin{bmatrix}1&0&0\\0&\cos\phi&-\sin\phi\\0&\sin\phi&\cos\phi\end{bmatrix},\; \mathbf{R}_y(\theta)=\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix},\; \mathbf{R}_z(\psi)=\begin{bmatrix}\cos\psi&-\sin\psi&0\\\sin\psi&\cos\psi&0\\0&0&1\end{bmatrix} Rx(ϕ)= 1000cosϕsinϕ0−sinϕcosϕ ,Ry(θ)= cosθ0−sinθ010sinθ0cosθ ,Rz(ψ)= cosψsinψ0−sinψcosψ0001

乘出后，得到矩阵的显式（略）。给定三元组 (ϕ,θ,ψ)(\phi,\theta,\psi)(ϕ,θ,ψ)，可唯一计算出 R\mathbf{R}R。反向从 R\mathbf{R}R 提取欧拉角：
θ=arcsin⁡(−r31)ϕ=atan2⁡(r32/cos⁡θ,  r33/cos⁡θ)ψ=atan2⁡(r21/cos⁡θ,  r11/cos⁡θ) \begin{aligned} \theta &= \arcsin(-r_{31}) \\ \phi &= \operatorname{atan2}(r_{32}/\cos\theta,\; r_{33}/\cos\theta) \\ \psi &= \operatorname{atan2}(r_{21}/\cos\theta,\; r_{11}/\cos\theta) \end{aligned} θϕψ=arcsin(−r31)=atan2(r32/cosθ,r33/cosθ)=atan2(r21/cosθ,r11/cosθ)

（当 cos⁡θ≠0\cos\theta\neq0cosθ=0）

1.2 问题：万向节死锁

当 θ=±π2\theta = \pm\frac{\pi}{2}θ=±2π，则 cos⁡θ=0\cos\theta=0cosθ=0，上面公式失效。此时矩阵退化为
R=0sin⁡(ϕ±ψ)cos⁡(ϕ±ψ)0cos⁡(ϕ±ψ)∓sin⁡(ϕ±ψ)−100 \mathbf{R} = \begin{bmatrix} 0 & \sin(\phi\pm\psi) & \cos(\phi\pm\psi)\\ 0 & \cos(\phi\pm\psi) & \mp\sin(\phi\pm\psi)\\ -1 & 0 & 0 \end{bmatrix} R= 00−1sin(ϕ±ψ)cos(ϕ±ψ)0cos(ϕ±ψ)∓sin(ϕ±ψ)0

可见第一行和第二行的元素只依赖于 (ϕ±ψ)(\phi\pm\psi)(ϕ±ψ) 这一个组合，无法解出 ϕ\phiϕ 和 ψ\psiψ 独立值------丧失了一个自由度，这就是万向节死锁。在参数空间中，这一条线对应 SO(3) 的一个二维子集，映射不是局部微分同胚。

1.3 问题：周期性边界与非最短弧插值

欧拉角是周期函数，将 ψ\psiψ 从 179∘179^\circ179∘ 变到 −179∘-179^\circ−179∘ 实际上对应于 SO(3) 中几乎重合的两个姿态（只差 2∘2^\circ2∘），但在参数空间它们相差 358∘358^\circ358∘。对欧拉角直接线性插值：
θ(t)=(1−t)θstart+tθend \boldsymbol{\theta}(t) = (1-t)\boldsymbol{\theta}{\text{start}} + t\boldsymbol{\theta}{\text{end}} θ(t)=(1−t)θstart+tθend

得到的路径在参数空间是一条直线，映射回 SO(3) 后路径会穿过 180∘180^\circ180∘ 的边界，导致物理上夹爪旋转接近一整圈。这正是你遇到的抖动根源。

所以欧拉角只适合作为"路标"（单点表示），绝不能用于规划连续的姿态轨迹。

---

2：旋转矩阵 ------ 无歧义的全局描述

怎么从欧拉角推出来

你把三个基本旋转矩阵乘起来，就得到了一个 3×33\times33×3 的正交矩阵：
R=r11r12r13r21r22r23r31r32r33∈SO(3) \mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in SO(3) R= r11r21r31r12r22r32r13r23r33 ∈SO(3)

这个矩阵的每一列就是末端坐标系的 X、Y、Z 轴在世界坐标系下的单位向量。它没有死锁 ，不受边界困扰，旋转合成直接矩阵乘法。

致命伤

• 9 个参数，有 6 个约束（正交性），冗余且不适合直接做插值。
• 如果把矩阵的每个元素做线性插值，得到的中间矩阵通常不再是正交矩阵，强行"掰正"后旋转路径不匀速，甚至不是最短弧。

但我们直觉知道：任何刚性旋转都应该可以用 绕某个空间固定轴转一个角度 来描述。这个想法直接导致轴角的诞生。

2.1 从旋转矩阵提取轴与角

定理（欧拉旋转定理） ：任何旋转 R≠I\mathbf{R}\neq\mathbf{I}R=I 有唯一的旋转轴单位向量 u\mathbf{u}u 和转角 θ∈(0,π)\theta\in(0,\pi)θ∈(0,π)，使得 R\mathbf{R}R 可写成罗德里格斯公式：
R=I+sin⁡θ u×+(1−cos⁡θ) u×2(1) \mathbf{R} = \mathbf{I} + \sin\theta\,\\mathbf{u}\times + (1-\cos\theta)\,\\mathbf{u}\times^2 \tag{1} R=I+sinθu×+(1−cosθ)u×2(1)

其中反对称矩阵
u×=0−uzuyuz0−ux−uyux0 \\mathbf{u}_\times = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix} u×= 0uz−uy−uz0uxuy−ux0

从给定 R\mathbf{R}R 求 θ\thetaθ 与 u\mathbf{u}u：
tr⁡(R)=1+2cos⁡θ⟹θ=arccos⁡ ⁣(tr⁡(R)−12)(2) \operatorname{tr}(\mathbf{R}) = 1 + 2\cos\theta \quad\Longrightarrow\quad \theta = \arccos\!\left(\frac{\operatorname{tr}(\mathbf{R})-1}{2}\right) \tag{2} tr(R)=1+2cosθ⟹θ=arccos(2tr(R)−1)(2)
u=12sin⁡θ(r32−r23r13−r31r21−r12),θ≠0,π(3) \mathbf{u} = \frac{1}{2\sin\theta} \begin{pmatrix} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-r_{12} \end{pmatrix}, \quad \theta\neq0,\pi \tag{3} u=2sinθ1 r32−r23r13−r31r21−r12 ,θ=0,π(3)

2.2 旋转向量（rotation vector）

定义三维向量 ϕ=θu\boldsymbol{\phi} = \theta \mathbf{u}ϕ=θu，其方向为转轴，模长为转角。当 θ=0\theta=0θ=0 时 ϕ=0\boldsymbol{\phi}=\mathbf{0}ϕ=0；当 θ=π\theta=\piθ=π 时，ϕ\boldsymbol{\phi}ϕ 与 −ϕ-\boldsymbol{\phi}−ϕ 表示同一个旋转（轴反向）。旋转向量的集合是 R3\mathbb{R}^3R3 中半径为 π\piπ 的开球，球面上的对跖点等同。

2.3 从旋转向量到矩阵（指数映射）

旋转向量 ϕ\boldsymbol{\phi}ϕ 通过指数映射生成 SO(3) 元素：
R=exp⁡(ϕ×)=I+sin⁡∥ϕ∥∥ϕ∥ϕ×+1−cos⁡∥ϕ∥∥ϕ∥2ϕ×2(4) \mathbf{R} = \exp(\\boldsymbol{\\phi}\times) = \mathbf{I} + \frac{\sin\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|}\\boldsymbol{\\phi}\times + \frac{1-\cos\|\boldsymbol{\phi}\|}{\|\boldsymbol{\phi}\|^2}\\boldsymbol{\\phi}_\times^2 \tag{4} R=exp(ϕ×)=I+∥ϕ∥sin∥ϕ∥ϕ×+∥ϕ∥21−cos∥ϕ∥ϕ×2(4)

这恰好是罗德里格斯公式的等价形式。

2.4 轴角的插值缺陷

对两旋转向量 ϕ0,ϕ1\boldsymbol{\phi}_0,\boldsymbol{\phi}_1ϕ0,ϕ1 做线性插值 ϕ(t)=(1−t)ϕ0+tϕ1\boldsymbol{\phi}(t)=(1-t)\boldsymbol{\phi}_0+t\boldsymbol{\phi}1ϕ(t)=(1−t)ϕ0+tϕ1，对应的旋转路径为 R(t)=exp⁡(ϕ(t)×)\mathbf{R}(t)=\exp(\\boldsymbol{\\phi}(t)\times)R(t)=exp(ϕ(t)×)。这条路径在 SO(3) 上不是测地线 （不是等角速度最短弧），并且当两点跨过 θ=π\theta=\piθ=π 的对跖边界时，同样会出现绕远路的问题。因此轴角虽比欧拉角好（没有死锁），但仍非插值最优选择。

3：轴角 ------ 揭示旋转的本质

从旋转矩阵推导轴角

对任何旋转矩阵 R\mathbf{R}R，总存在一个单位向量 u\mathbf{u}u 和一个角度 θ∈0,π\theta\in0,\\piθ∈0,π，满足：
R=I+sin⁡θ u×+(1−cos⁡θ) u×2 \mathbf{R} = \mathbf{I} + \sin\theta\,\\mathbf{u}\times + (1-\cos\theta)\,\\mathbf{u}\times^2 R=I+sinθu×+(1−cosθ)u×2

这就是罗德里格斯公式 。其中 u×\\mathbf{u}\timesu× 是叉乘反对称矩阵。反过来，从矩阵提取轴角：
θ=arccos⁡ ⁣(tr⁡(R)−12),u=12sin⁡θ(r32−r23r13−r31r21−r12) \theta = \arccos\!\left(\frac{\operatorname{tr}(\mathbf{R})-1}{2}\right), \quad \mathbf{u} = \frac{1}{2\sin\theta}\begin{pmatrix} r{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12} \end{pmatrix} θ=arccos(2tr(R)−1),u=2sinθ1 r32−r23r13−r31r21−r12

把轴和角打包成一个三维向量 ϕ=θu\boldsymbol{\phi} = \theta \mathbf{u}ϕ=θu，就是旋转向量。它的模长就是转角，方向就是转轴。参数只有 3 个，且几何意义明确。

解决了什么？

欧拉角的死锁在轴角中不存在，因为死锁是局部参数化奇点，而轴角是"绕任意轴"的统一形式。

又带来什么新问题？

• θ=π 时的歧义 ：如果旋转恰好 180°，那么 u\mathbf{u}u 和 −u-\mathbf{u}−u 对应同一个旋转（因为绕轴转 180° 等于绕反向轴转 180°）。旋转向量的长度等于 π，所有这样的对跖点都映射到同一个旋转，边界上仍然有双值问题。
• 插值非测地线：对旋转向量直接线性插值（二维平面上的直线）映射回 SO(3) 后，角速度不是恒定的，而且当角度接近 π 时路径同样会绕远路。你之前的问题如果用旋转向量插值可能比欧拉角好一些，但依然不是最短弧。

有没有一种参数化，既能继承轴角的简洁（轴+角），又能让插值天然沿着最短弧？于是我们把它升维到四维单位球面上。

---

4：四元数 ------ 被设计来完美插值

从轴角到四元数的神秘一跃

把轴角 (u,θ)(\mathbf{u}, \theta)(u,θ) 编码成一个四维向量：
q=(cos⁡(θ/2)uxsin⁡(θ/2)uysin⁡(θ/2)uzsin⁡(θ/2))=(w,x,y,z) \mathbf{q} = \begin{pmatrix} \cos(\theta/2) \\ u_x \sin(\theta/2) \\ u_y \sin(\theta/2) \\ u_z \sin(\theta/2) \end{pmatrix} = (w, x, y, z) q= cos(θ/2)uxsin(θ/2)uysin(θ/2)uzsin(θ/2) =(w,x,y,z)

并且强制 ∥q∥=1\|\mathbf{q}\|=1∥q∥=1。这就是单位四元数，它分布在四维空间的单位球面 S3S^3S3 上。关键性质 ：旋转角度 θ 变成了 2× 半角，因此一个旋转对应两个四元数 q\mathbf{q}q 和 −q-\mathbf{q}−q，这恰好解决了 θ=π 时轴的双值问题------在四维球面上，对跖点代表同一个旋转，而不再像三维球面那样是"边界"。

为什么插值能走最短弧

在 S3S^3S3 上，两个四元数之间的最短路径就是球面上的大圆，对应等角速度的旋转。公式（Slerp）是：
Slerp⁡(q0,q1,t)=sin⁡((1−t)Ω)sin⁡Ωq0+sin⁡(tΩ)sin⁡Ωq1 \operatorname{Slerp}(\mathbf{q}_0,\mathbf{q}_1,t)=\frac{\sin((1-t)\Omega)}{\sin\Omega}\mathbf{q}_0+\frac{\sin(t\Omega)}{\sin\Omega}\mathbf{q}_1 Slerp(q0,q1,t)=sinΩsin((1−t)Ω)q0+sinΩsin(tΩ)q1

其中 cos⁡Ω=q0⋅q1\cos\Omega = \mathbf{q}_0\cdot\mathbf{q}_1cosΩ=q0⋅q1。这个插值保证你从 179° 到 -179° 只绕 2° 而不是 358°。

缺点

• 四元数必须满足单位长度约束，深度学习直接回归容易漂移。
• 符号二义性（q\mathbf{q}q 和 −q-\mathbf{q}−q 等价）会给损失函数带来麻烦，需要特殊处理（如选择离真值更近的那个）。

4.1 从轴角构造四元数

将轴 (u,θ)(\mathbf{u},\theta)(u,θ) 编码为四元数：
q=(w,v)=(cos⁡θ2,  usin⁡θ2)(5) \mathbf{q} = (w,\mathbf{v}) = \left(\cos\frac{\theta}{2},\; \mathbf{u}\sin\frac{\theta}{2}\right) \tag{5} q=(w,v)=(cos2θ,usin2θ)(5)

其中 w∈Rw\in\mathbb{R}w∈R，v∈R3\mathbf{v}\in\mathbb{R}^3v∈R3。要求 ∥q∥2=w2+∥v∥2=1\|\mathbf{q}\|^2 = w^2 + \|\mathbf{v}\|^2 = 1∥q∥2=w2+∥v∥2=1，即单位四元数。

一个旋转对应两个四元数：q\mathbf{q}q 与 −q-\mathbf{q}−q（将 θ\thetaθ 换成 2π−θ2\pi-\theta2π−θ，轴反向）。这是 SO(3) 的双覆盖 S3→SO(3)S^3 \to SO(3)S3→SO(3)。

4.2 四元数乘法与旋转操作

定义四元数乘法：
q1q2=(w1w2−v1⋅v2,  w1v2+w2v1+v1×v2) \mathbf{q}_1\mathbf{q}_2 = (w_1w_2 - \mathbf{v}_1\cdot\mathbf{v}_2,\; w_1\mathbf{v}_2 + w_2\mathbf{v}_1 + \mathbf{v}_1\times\mathbf{v}_2) q1q2=(w1w2−v1⋅v2,w1v2+w2v1+v1×v2)

对于向量 p∈R3\mathbf{p}\in\mathbb{R}^3p∈R3 写为纯四元数 pq=(0,p)\mathbf{p}_q = (0,\mathbf{p})pq=(0,p)，旋转操作表示为：
p′=q pq q−1 \mathbf{p}' = \mathbf{q}\,\mathbf{p}_q\,\mathbf{q}^{-1} p′=qpqq−1

其中 q−1=(w,−v)\mathbf{q}^{-1}=(w,-\mathbf{v})q−1=(w,−v)（共轭）。这等价于用旋转矩阵 R\mathbf{R}R 作用。将四元数转为旋转矩阵的公式：
R=1−2(y2+z2)2(xy−wz)2(xz+wy)2(xy+wz)1−2(x2+z2)2(yz−wx)2(xz−wy)2(yz+wx)1−2(x2+y2)(6) \mathbf{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} \tag{6} R= 1−2(y2+z2)2(xy+wz)2(xz−wy)2(xy−wz)1−2(x2+z2)2(yz+wx)2(xz+wy)2(yz−wx)1−2(x2+y2) (6)

4.3 Slerp：球面测地线插值

单位四元数分布在四维单位球面 S3S^3S3 上。两单位四元数 q0,q1\mathbf{q}_0,\mathbf{q}_1q0,q1 之间的夹角 Ω\OmegaΩ 满足：
cos⁡Ω=q0⋅q1=w0w1+x0x1+y0y1+z0z1(7) \cos\Omega = \mathbf{q}_0\cdot\mathbf{q}_1 = w_0w_1 + x_0x_1 + y_0y_1 + z_0z_1 \tag{7} cosΩ=q0⋅q1=w0w1+x0x1+y0y1+z0z1(7)

Slerp 沿 S3S^3S3 大圆等速运动：
Slerp⁡(q0,q1;t)=sin⁡((1−t)Ω)sin⁡Ω q0+sin⁡(tΩ)sin⁡Ω q1(8) \operatorname{Slerp}(\mathbf{q}_0,\mathbf{q}_1;t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\,\mathbf{q}_0 + \frac{\sin(t\Omega)}{\sin\Omega}\,\mathbf{q}_1 \tag{8} Slerp(q0,q1;t)=sinΩsin((1−t)Ω)q0+sinΩsin(tΩ)q1(8)

需检查若 cos⁡Ω<0\cos\Omega<0cosΩ<0，用 −q1-\mathbf{q}_1−q1 代替 q1\mathbf{q}_1q1 以确保走短弧 (Ω≤π2\Omega\le\frac{\pi}{2}Ω≤2π)。

4.4 为什么 Slerp 解决了抖动

Slerp 生成的四元数路径在 SO(3) 上对应等角速度最短弧旋转 。对于 179° 到 -179° 的两个欧拉角姿态，转换为四元数后，它们对应的 q\mathbf{q}q 几乎相同（差对应于 2° 旋转）。Slerp 直接输出几乎恒定四元数，物理旋转量仅 2°。

4.5 四元数的优缺点

优点 ：全局无奇点（覆盖空间是光滑球面）；Slerp提供完美最短弧插值。
缺点 ：需归一化约束；符号二义性（q\mathbf{q}q 与 −q-\mathbf{q}−q 等价）给神经网络损失函数带来困难；组件乘法非直观。

---

5：6D 表示 ------ 为深度学习而生

从旋转矩阵"粗暴"取两列

我们回到旋转矩阵：它 9 个数有 6 个正交约束，能不能去掉约束，让网络自由输出，最后再强制变成旋转矩阵？当然可以。取旋转矩阵的前两列：
a=r1∈R3,b=r2∈R3 \mathbf{a} = \mathbf{r}_1 \in \mathbb{R}^3,\quad \mathbf{b} = \mathbf{r}_2 \in \mathbb{R}^3 a=r1∈R3,b=r2∈R3

拼接成 6 维向量 d=a;b\mathbf{d}=\\mathbf{a};\\mathbf{b}d=a;b。网络输出这 6 个数，我们不对其施加任何约束。

如何恢复旋转矩阵？

用施密特正交化：
r1=a∥a∥,r2=b−(r1⋅b)r1∥b−(r1⋅b)r1∥,r3=r1×r2 \mathbf{r}_1 = \frac{\mathbf{a}}{\|\mathbf{a}\|},\quad \mathbf{r}_2 = \frac{\mathbf{b} - (\mathbf{r}_1\cdot\mathbf{b})\mathbf{r}_1}{\|\mathbf{b} - (\mathbf{r}_1\cdot\mathbf{b})\mathbf{r}_1\|},\quad \mathbf{r}_3 = \mathbf{r}_1\times\mathbf{r}_2 r1=∥a∥a,r2=∥b−(r1⋅b)r1∥b−(r1⋅b)r1,r3=r1×r2

只要 a,b\mathbf{a},\mathbf{b}a,b 不共线（概率几乎为 0），就能得到一个合法的旋转矩阵。这个映射 R6→SO(3)\mathbb{R}^6 \to SO(3)R6→SO(3) 在几乎所有地方都是光滑的，没有边界奇点 ，没有范数约束，非常适合用 L2 损失直接训练。

插值特性

如果在 6D 空间线性插值再正交化，会得到一条在 SO(3) 上接近测地线的路径，虽然不如 Slerp 精确，但在神经网络生成动作序列时已足够平滑。这也正是你 VLA 模型输出不抖动（很可能用了 6D 或四元数头），而后面轨迹规划用欧拉角却抖的原因。

这时我们问：能不能结合旋转矩阵的无约束性（列向量可以任意值，正交化后得到矩阵）与四元数的平滑性，创造一种适合神经网络输出的表示？

5.1 从旋转矩阵直接取两列

对于任一旋转矩阵 R=r1  r2  r3\mathbf{R}=\\mathbf{r}_1\\; \\mathbf{r}_2\\; \\mathbf{r}_3R=r1r2r3，取其前两列作为6D向量：
d=(r1r2)∈R6 \mathbf{d} = \begin{pmatrix} \mathbf{r}_1 \\ \mathbf{r}_2 \end{pmatrix} \in \mathbb{R}^6 d=(r1r2)∈R6

这里 r1,r2∈R3\mathbf{r}_1,\mathbf{r}_2\in\mathbb{R}^3r1,r2∈R3 是世界坐标系下刚体的 X 轴和 Y 轴方向。它们只需满足 r1⋅r2=0\mathbf{r}_1\cdot\mathbf{r}_2 = 0r1⋅r2=0 且 ∥r1∥=∥r2∥=1\|\mathbf{r}_1\|=\|\mathbf{r}_2\|=1∥r1∥=∥r2∥=1，但在网络输出时我们不施加任何约束。

5.2 从6D向量恢复旋转矩阵（Gram--Schmidt正交化）

给定任意6D向量 d=a;b\mathbf{d}=\\mathbf{a};\\mathbf{b}d=a;b（假设 a\mathbf{a}a 非零），执行：
r1=a∥a∥(9) \mathbf{r}_1 = \frac{\mathbf{a}}{\|\mathbf{a}\|} \tag{9} r1=∥a∥a(9)
r2=b−(r1⋅b)r1∥b−(r1⋅b)r1∥(10) \mathbf{r}_2 = \frac{\mathbf{b} - (\mathbf{r}_1\cdot\mathbf{b})\mathbf{r}_1}{\|\mathbf{b} - (\mathbf{r}_1\cdot\mathbf{b})\mathbf{r}_1\|} \tag{10} r2=∥b−(r1⋅b)r1∥b−(r1⋅b)r1(10)
r3=r1×r2(11) \mathbf{r}_3 = \mathbf{r}_1 \times \mathbf{r}_2 \tag{11} r3=r1×r2(11)

最终组合为 R=r1  r2  r3\mathbf{R}=\\mathbf{r}_1\\;\\mathbf{r}_2\\;\\mathbf{r}_3R=r1r2r3，它自动满足 SO(3) 约束。只要 a,b\mathbf{a},\mathbf{b}a,b 不平行（零测集），此映射 R6→SO(3)\mathbb{R}^6 \to SO(3)R6→SO(3) 是光滑的。

5.3 为何适合深度学习

• 映射无奇异：除了几乎不可能的退化情况，整个过程处处连续可微。
• 输出无约束：网络输出任意6个实数，无需考虑范数、周期性、边界等。
• 损失稳定：可直接对6D分量施加L2损失，配合正交化过程反向传播梯度，不会出现欧拉角边界处的梯度爆炸。
• 插值行为：在6D空间线性插值再正交化，在 SO(3) 上得到的路径接近测地线（虽不完全等速，但远比欧拉角插值合理），因此用于生成动作序列也足够平滑。

5.4 与欧拉角的本质区别

欧拉角（3参数）是局部坐标卡，存在奇点和周期边界，全局映射不光滑；6D（6参数）是通过嵌入 R6\mathbb{R}^6R6 再投影到 SO(3)，嵌入空间中没有人为的边界和奇点。欧拉角是三角参数化，6D是向量参数化。

---

6. 总结：阶梯式的数学演进

1. 欧拉角 ：R3\mathbb{R}^3R3 上的三个角度，通过 atan2/sin/cos 构建旋转矩阵。简洁但有万向节死锁和 180∘180^\circ180∘ 边界。
2. 旋转矩阵：9个约束参数，唯一全局无奇异表示，但不适合直接插值。
3. 轴角 ：通过对角线求迹得 θ\thetaθ，反对称差得轴，表达为 ϕ=θu\boldsymbol{\phi}=\theta\mathbf{u}ϕ=θu。消除了死锁，但 θ=π\theta=\piθ=π 有对跖歧义，且线性插值非测地线。
4. 四元数 ：将轴角替换为半角 (cos⁡θ2,usin⁡θ2)(\cos\frac{\theta}{2}, \mathbf{u}\sin\frac{\theta}{2})(cos2θ,usin2θ)，形成单位四元数 S3S^3S3，双覆盖 SO(3)。Slerp 给出大圆路径，解决最短弧插值。
5. 6D表示：取旋转矩阵前两列的6个分量，通过网络自由输出，再用施密特正交化映射到 SO(3)。保留了旋转矩阵的全局光滑性，且适合作为深度学习回归目标。

---

7. 插值和抖动

表示	谁发明/为何	解决了什么	引入新问题	适用场景
欧拉角	直觉导航	3 参数直观	死锁、边界跳变	仅用于 UI 输入，绝不用于插值
旋转矩阵	去除奇异性（乘起来）	无死锁、可组合	参数多、不能直接插值	坐标系变换、运动学
轴角	揭示旋转本质（绕轴转角）	统一轴角形式	θ=π 双值，插值非最短弧	物理理解、小角度插值
四元数	将轴角升维到球面	完美最短弧插值	网络回归需处理范数/符号	轨迹规划插值（用 Slerp）
6D 表示	掷掉约束，留两列给网络	深度学习回归极稳	需正交化步骤，非严格匀速	VLA 动作输出头

结论 ：VLA 输出可以用 欧拉角，进入轨迹规划后，必须将姿态转为四元数并用 Slerp 插值，严禁用欧拉角线性插值 。
针对调试 ：VLA 输出欧拉角（作为语义目标），但轨迹规划器必须将起终点欧拉角转换成四元数，再用 Slerp 插值。只此一步，就能根除"位置不动、姿态疯转"的问题。数学上，这是因为 Slerp 在覆盖空间 S3S^3S3 上走大圆，对应的物理旋转走 SO(3) 上的测地线，而欧拉角线性插值在参数空间走直线，映射回 SO(3) 时撕裂了连续边界。

8. 附录：各种表示法相互转换

import numpy as np
from scipy.spatial.transform import Rotation as R

def detect_rotation_type(data, seq='xyz', degrees=True):
    """
    检测输入姿态数据的类型。
    返回：'quaternion', 'matrix_3x3', 'matrix_4x4', 'euler', 'rotvec'
    """
    data = np.asarray(data, dtype=np.float64)
    shape = data.shape

    # 一维数组
    if data.ndim == 1:
        length = data.shape[0]
        if length == 4:
            # 四元数，检查模是否接近1
            norm = np.linalg.norm(data)
            if abs(norm - 1.0) < 1e-6:
                return 'quaternion'
            else:
                # 也可能是未归一化的四元数，这里强制认为
                return 'quaternion_raw'
        elif length == 3:
            # 可能是欧拉角或旋转向量
            # 旋转向量：模长是角度，通常有模长 <= pi；欧拉角可能每个分量范围较大
            # 启发式：如果模长接近0或pi，或用户显式指定，但这里我们默认区分：
            # 若用户没有指明，给出提示并使用默认'euler'
            norm = np.linalg.norm(data)
            if norm < 1e-10 or (np.pi - 1e-2 < norm < np.pi + 1e-2):
                # 极大概率是旋转向量，因为欧拉角极小出现零范数只有全零，但也可同时
                pass
            # 简单区分：如果每个分量都在[-2*pi,2*pi]但可能超出，仍难确定。
            # 通过参数 input_type 来处理，这里先返回 'unknown_vector'
            return 'unknown_vector'
        else:
            raise ValueError("Unknown 1D array length: {}".format(length))

    # 二维数组
    elif data.ndim == 2:
        rows, cols = data.shape
        if rows == 3 and cols == 3:
            return 'matrix_3x3'
        elif rows == 4 and cols == 4:
            return 'matrix_4x4'
        else:
            raise ValueError("Unknown 2D array shape: {}".format(shape))
    else:
        raise ValueError("Input data must be 1D or 2D array.")


def _to_rotation(data, input_type=None, seq='xyz', degrees=True):
    """
    将任何姿态表示转换为 scipy.spatial.transform.Rotation 对象。
    """
    if input_type is None:
        input_type = detect_rotation_type(data, seq, degrees)

    if input_type == 'quaternion' or input_type == 'quaternion_raw':
        q = np.asarray(data, dtype=np.float64)
        # 默认 scipy 四元数顺序为 (x, y, z, w)
        # 如果输入是 (w, x, y, z) 则需要转置，这里假设输入是 [x,y,z,w] 或 [w,x,y,z] 通过常用习惯
        # 安全做法：检查第一个分量绝对值是否显著大于1（w=cos(θ/2)∈[-1,1]），如果是则可能是w在前
        # 实际工程项目中，通常约定明确，此处提供参数可调
        if input_type == 'quaternion':
            # 假设规范四元数已经是 (x,y,z,w) 顺序
            pass
        # 如果未归一化，先归一化
        if np.linalg.norm(q) != 1.0:
            q = q / np.linalg.norm(q)
        return R.from_quat(q)  # 默认是 xyzw 顺序

    elif input_type == 'matrix_3x3':
        mat = np.asarray(data, dtype=np.float64)
        return R.from_matrix(mat)

    elif input_type == 'matrix_4x4':
        mat = np.asarray(data, dtype=np.float64)
        rot_mat = mat[:3, :3]
        return R.from_matrix(rot_mat)

    elif input_type == 'euler':
        angles = np.asarray(data, dtype=np.float64)
        return R.from_euler(seq, angles, degrees=degrees)

    elif input_type == 'rotvec':
        vec = np.asarray(data, dtype=np.float64)
        return R.from_rotvec(vec)

    elif input_type == 'unknown_vector':
        # 用户必须进一步指定，这里默认当作欧拉角处理
        print("Warning: 3D vector input detected but type unknown. Treating as euler angles (seq='{}', degrees={}).".format(seq, degrees))
        return R.from_euler(seq, data, degrees=degrees)

    else:
        raise ValueError("Unsupported input_type: {}".format(input_type))


def convert_rotation(data, output_type='quaternion', input_type=None, seq='xyz', degrees=True, quat_order='xyzw'):
    """
    万能姿态转换器。

    参数:
        data: 输入姿态数据，支持 numpy 数组或 list
        output_type: 输出格式，可选:
                     'quaternion'   - 四元数 (默认 xyzw 顺序)
                     'quaternion_wxyz' - 四元数 (w,x,y,z 顺序)
                     'matrix'       - 3x3 旋转矩阵
                     'euler'        - 欧拉角 (需用seq指定轴顺序)
                     'rotvec'       - 旋转向量 (轴角)
                     '6d'           - 6D 连续表示 (旋转矩阵的前两列展平)
        input_type: 输入数据类型，若不指定则自动检测。
                    可选: 'quaternion', 'quaternion_wxyz', 'matrix_3x3', 'matrix_4x4',
                          'euler', 'rotvec'
        seq: 欧拉角轴顺序 (如 'xyz', 'ZYX' 等)，仅用于欧拉角输入/输出
        degrees: 欧拉角是否使用角度 (True) 或弧度 (False)
        quat_order: 四元数输出的分量顺序，默认 'xyzw'，也可选 'wxyz'

    返回:
        转换后的姿态数据，numpy 数组
    """
    # 处理输入类型的细微差别
    if input_type == 'quaternion_wxyz':
        # 转换为 xyzw 顺序
        w, x, y, z = data
        data = np.array([x, y, z, w])
        input_type = 'quaternion'

    rot = _to_rotation(data, input_type=input_type, seq=seq, degrees=degrees)

    # 根据输出类型转换
    if output_type == 'quaternion':
        q = rot.as_quat()  # xyzw 顺序
        if quat_order == 'wxyz':
            q = np.array([q[3], q[0], q[1], q[2]])
        return q
    elif output_type == 'matrix':
        return rot.as_matrix()
    elif output_type == 'euler':
        return rot.as_euler(seq, degrees=degrees)
    elif output_type == 'rotvec':
        return rot.as_rotvec()
    elif output_type == '6d':
        mat = rot.as_matrix()
        # 6D 表示：旋转矩阵的前两列 (每个 3 维) 平铺成 6 维
        return mat[:, :2].reshape(-1, order='F')  # 或者直接 mat[:,:2].flatten()
    else:
        raise ValueError("Unsupported output_type: {}".format(output_type))


# ==================== 示例 ====================
if __name__ == '__main__':
    # 测试绕 Z 轴旋转 179° 和 -179°
    angle_start = 179.0
    angle_end = -179.0
    euler_start = np.array([0, 0, angle_start])
    euler_end = np.array([0, 0, angle_end])

    # 转为四元数
    q_start = convert_rotation(euler_start, 'quaternion', input_type='euler', degrees=True)
    q_end = convert_rotation(euler_end, 'quaternion', input_type='euler', degrees=True)
    print("起点四元数 (xyzw):", q_start)
    print("终点四元数 (xyzw):", q_end)

    # 确保短弧插值：检查点积
    if np.dot(q_start, q_end) < 0:
        q_end = -q_end
        print("翻转终点四元数以取短弧")

    # 转换为矩阵
    mat_start = convert_rotation(q_start, 'matrix', input_type='quaternion')
    mat_end = convert_rotation(q_end, 'matrix', input_type='quaternion')
    print("\n起点旋转矩阵:\n", mat_start)
    print("终点旋转矩阵:\n", mat_end)

    # 转换为轴角
    rotvec_start = convert_rotation(q_start, 'rotvec', input_type='quaternion')
    rotvec_end = convert_rotation(q_end, 'rotvec', input_type='quaternion')
    print("\n起点轴角:", rotvec_start)
    print("终点轴角:", rotvec_end)

    # 转换为 6D 表示
    sixd_start = convert_rotation(q_start, '6d', input_type='quaternion')
    sixd_end = convert_rotation(q_end, '6d', input_type='quaternion')
    print("\n起点6D表示:", sixd_start)
    print("终点6D表示:", sixd_end)