旋转的 4 种表示方式​

旋转的 4 种表示方式 ​一. 姿态角 ​即 RPY 角（Roll、Pitch、Yaw）。

俯仰角 θ（pitch）：围绕 Y 轴旋转的角度。

偏航角 ψ（yaw）：围绕 Z 轴旋转的角度。

滚转角 ϕ（roll）：围绕 X 轴旋转的角度。

是一种绕固定坐标系（外旋）的欧拉角，详见下一节。

二. 欧拉角 ​欧拉角是由 Leonhard Euler 提出的概念, 用来描述刚体/移动坐标系在一个固定坐标系中的姿态。简单的说是使用 XYZ 三个轴的旋转分量, 来描述一个 6 自由度的旋转。

欧拉角一般具有两大类表示方式, 每类按照旋转次序的不同分为 6 小类:

Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)

Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)

每个大类都使用了 3 个变量描述三次旋转过程中的旋转角度, 差别在于 Proper Euler angles 只涉及两个转轴，而 Tait–Bryan angles 涉及三个转轴。

Tait–Bryan angles 也被称为 Cardan angles, nautical angles, (heading, elevation, and bank), (yaw, pitch, and roll). 我们接触的比较多的是 yaw(偏航), pitch(俯仰), roll(横滚).三个变量一般对应(车体, 飞行器的)z, y, x 三个坐标轴。

1. 欧拉角描述旋转 ​如果给出一组欧拉角（后面都是指 Tait–Bryan angles），绕 x, y, z 三个轴的转角分别为 (α,β,γ)，我们不能能确定一个明确的姿态。需要再追加两个属性：(1)旋转顺序 (2)内旋/外旋。才能确定的给出这组欧拉角对应的姿态。

(1) 旋转顺序 ​旋转顺序就是我们上文提到的 Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)。

对于 x, y, z 三个轴的不同旋转顺序一共有六种组合。我们需要明确旋转顺序, 才能确定欧拉角所指的姿态。

(2) 内旋与外旋 ​按旋转的坐标系分为 内旋（intrinsic rotation）和 外旋（extrinsic rotation）。

内旋：绕物体自身的坐标系（object-space）旋转。每次旋转都会改变下一次旋转的轴。这种情况下旋转的轴是动态的（moving axis）。

text内旋(intrinsic rotations) = 旋转轴(rotated axis)外旋：绕惯性系（upright-space）旋转。upright space 指基向量平行于 world-space 或 parent-space，原点与 object-space 的原点重合的空间）。无论是三步旋转中的哪一步，轴都是固定的。

text外旋(extrinsic rotations) = 固定轴(static/fixed axis)

内在旋转与外在旋转的转换关系：互换第一次和第三次旋转的位置则两者结果相同。例如 Z-Y-X 旋转的内部旋转和 X-Y-Z 旋转的外部旋转的旋转矩阵相同。

(a) 绕定轴 X-Y-Z 旋转（RPY 角）（外旋） ​假设两个坐标系 A 和 B，二者初始时完全重合。 过程如下：B 绕 A 的 X 轴旋转 γ 角，再绕 A 的 Y 轴旋转 β 角，最后绕 A 的 Z 轴旋转 α 角，完成旋转。整个过程，A 不动 B 动。

每个旋转都是绕着固定参考坐标系{A}的轴。我们规定这种姿态表示法为 X-Y-Z 固定角坐标系。又是把它们定义为 回转角/滚转角（roll）、俯仰角（pitch） 和 偏转角/偏航角（yaw）。

可直接推导等价旋转矩阵 BARXYZ(γ,β,α)，因为所有旋转都是围绕着参考坐标系各轴的，即

BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=[c(α)−s(α)0s(α)c(α)0001][c(β)0s(β)010−s(β)0c(β)][1000c(γ)−s(γ)0s(γ)c(γ)]=[c(α)c(β)c(α)s(β)s(γ)−s(α)c(γ)c(α)s(β)c(γ)+s(α)s(γ)s(α)c(β)s(α)s(β)s(γ)+c(α)c(γ)s(α)s(β)c(γ)−c(α)s(γ)−s(β)c(β)s(γ)c(β)c(γ)]其中 c(α)=cos⁡(α)，s(α)=sin⁡(α)

(b) 绕动轴 Z-Y-X 旋转（Euler 角）（内旋） ​过程如下：B 绕 B 的 Z 轴旋转 α 角，再绕 B 的 Y 轴旋转 β 角，最后绕 B 的 X 轴旋转 γ 角，完成旋转。整个过程，A 不动 B 动。

BAR=B′ARB″B′RBB″R{B}相对于 A 的最终姿态为

BARZ′Y′X′(γ,β,α)=RZ(α)RY(β)RX(γ)=[c(α)−s(α)0s(α)c(α)0001][c(β)0s(β)010−s(β)0c(β)][1000c(γ)−s(γ)0s(γ)c(γ)]=[c(α)c(β)c(α)s(β)s(γ)−s(α)c(γ)c(α)s(β)c(γ)+s(α)s(γ)s(α)c(β)s(α)s(β)s(γ)+c(α)c(γ)s(α)s(β)c(γ)−c(α)s(γ)−s(β)c(β)s(γ)c(β)c(γ)]这个结果与绕定轴 X-Y-Z 旋转的结果完全相同。也就是说：三次绕固定值旋转的最终姿态和以相反顺序三次绕运动坐标轴旋转的最终姿态相同。

2. 欧拉角表示方式的缺点 ​欧拉角的表示方式比较直观，但是有几个缺点：

欧拉角的表示方式不唯一。

给定某个起始朝向和目标朝向，即使给定 yaw、pitch、roll 的顺序，也可以通过不同的 yaw/pitch/roll 的角度组合来表示所需的旋转。比如，同样的 yaw-pitch-roll 顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁（Gimbal Lock）引起的。

欧拉角的插值比较难。

计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多 sin, cos，计算量较大。

3. 欧拉角-> 旋转矩阵 ​Euler angles - Wikipedia

内旋欧拉角转换为旋转矩阵如下：

（如是外旋欧拉角，则 X1Y2Z3→z3y2x1, s1→s3 ）

三. 旋转矩阵 ​1. 旋转矩阵转欧拉角 ​旋转矩阵转内旋欧拉角如下：

四. 四元数 ​参考：

四元数与三维旋转 https://krasjet.github.io/quaternion/quaternion.pdf四元数的可视化 https://www.bilibili.com/video/av333851051. 3D 旋转公式（向量型，一般情况） ​也叫做 Rodrigue's Rotation Formula：

3D 空间中任意一个 v 沿着单位向量 u 旋转 θ 角度之后的 v′ 为:

math\mathbf{v'} = \cos(\theta)\mathbf{v} + (1-\cos(\theta))(\mathbf{u} \cdot \mathbf{v})\mathbf{u} + \sin(\theta)(\mathbf{u} \times \mathbf{v})证明略。

2. 四元数（quaternion） ​四元数有三个虚部，所有四元数 q∈H （H 代表四元数的发现者 William Rowan Hamilton）可以写成以下形式：

q=a+bi+cj+dk, (a,b,c,d∈\R)其中：

i2=j2=k2=ijk=−1向量形式：

p=[abcd]此外，我们经常将四元数的实部与虚部分开，并用一个三维向量来表示虚部，将它表示为标量与向量的有序对形式

q=[s,v]. (v=[x,y,z]T, s,x,y,z∈\R)(1) 性质 ​模长（范数） ​‖q‖=a2+b2+c2+d2标量向量有序对形式：

‖q‖=s2+‖v‖2=s2+v⋅v加减法 ​将分量相加减

标量乘法 ​math\begin{aligned}

sq &= s(a + bi + cj + dk) \\

&= sa + sbi + scj + sdk

\end{aligned}满足交换律：$ sq = qs $

四元数乘法 ​不满足交换律，一般 $ q_1q_2 \ne q_2q_1 $，有左乘与右乘的区别。

$ q_1q_2 ：「q_2$ 左乘以 q1」或「q1 右乘以 q2」满足结合律和分配律。

如果有两个四元数 q1=a+bi+cj+dk 和 q2=e+fi+gj+hk，它们的乘积为：

q1q2=(a+bi+cj+dk)(e+fi+gj+hk)=ae+afi+agj+ahk+bei+bfi2+bgij+bhik+cej+cfji+cgj2+chjk+dek+dfik+dgjk+dhk2由于有 i2=j2=k2=ijk=−1，可得如下表格：

×1ijk11ijkii−1k−jjj−k−1ikkj−i−1q1q2=ae+afi+agj+ahk+bei−bf+bgk−bhj+cej−cfk−cg+chi+dek+dfj−dgi−dh=(ae−bf−cg−dh)+(be+af−dg+ch)i+(ce+df+ag−bh)j+(de−cf+bg+ah)k矩阵形式 ​q2 左乘以 q1：

q1q2=[a−b−c−dba−dccda−bd−cba][efgh]q2 右乘以 q1（用相同方法推导可得）：

q2q1=[a−b−c−dbad−cc−dabdc−ba][efgh](2) Graßmann 积 ​如果令 v=[b,c,d]T, u=[f,g,h]T，那么

v⋅u=bf+cg+dhv×u=|ijkbcdfgh|=(ch−dg)i−(bh−df)j−(bg−cf)kq1q2 的结果可以用向量点乘和叉乘的形式表示出来（按历史来说，叉乘是在这里被定义的）

q1q2=[ae−v⋅u,au+ev+v×u]定理： Graßmann 积

对任意四元数 q1=[s,v], q2=[t,u]

q1q2=[st−v⋅u,su+tv+v×u]该定理是将四元数与 3D 旋转联系起来的关键。

(3) 纯四元数（Pure Quaternion） ​如果一个四元数能写成 v=[0,v] 的形式，那么我们称 v 为一个 纯四元数，即仅有虚部的四元数。任意 3D 向量都可转换为纯四元数。

纯四元数有一个重要特性：

vu=[0−v⋅u,0+v×u]=[−v⋅u,v×u](4) 逆（Inverse）与共轭 ​q−1 是 q 的 逆，规定

qq−1=q−1q=1 (q≠0)右乘 q 的逆运算为右乘 q−1，左乘 q 的逆运算为左乘 q−1，这与矩阵性质非常相似。

可以利用四元数共轭的一些性质来获得 q−1。

定义 q=a+bi+cj+dk 的 共轭 为 q∗=a−bi−cj−dk （读作“q star”）。用标量向量有序对的形式定义的话，q=[s,v] 的共轭为 q∗=[s,−v]。

共轭的性质：

qq∗=[s,v]⋅[s,−v]=[s2−v⋅(−v),s(−v)+sv+v×(−v)]=[s2+v⋅v,0]=s2+x2+y2+z2=‖q‖2q∗q=(q∗)(q∗)∗=‖q∗‖2=s2+x2+y2+z2=‖q‖2=qq∗这个特殊的乘法 qq∗=q∗q 是遵守交换律的，利用它可以求四元数的逆：

qq−1=1q∗qq−1=q∗(q∗q)q−1=q∗‖q‖2q−1=q∗q−1=q∗‖q‖2如果 ‖q‖=1，也就是说 q 是一个 单位四元数 (Unit Quaternion)，那么

q−1=q∗12=q∗3. 四元数与 3D 旋转 ​如果我们需要将一个向量 v 沿着一个用 单位向量所定义的旋转轴 u 旋转 θ 度，那么我们可以将这个向量 v 拆分为正交于旋转轴的 v⊥ 以及平行于旋转轴的 v∥ ． 我们可以对这两个分向量分别进行旋转，获得 v⊥′ 和 v∥′ ． 将它们相加就是 v 旋转之后的结果 v′=v∥′+v⊥′

我们可以将这些向量定义为纯四元数

v=[0,v]v⊥=[0,v⊥]v∥=[0,v∥]u=[0,u]v′=[0,v′]v⊥′=[0,v⊥′]v∥′=[0,v∥′]这样我们就能得到

v=v⊥+v∥v′=v⊥′+v∥′(1) v⊥ 的旋转 ​如果一个向量 v⊥ 正交于旋转轴 u，那么

v⊥′=cos⁡(θ)v⊥+sin⁡(θ)(u×v⊥)我们将它表示成四元数形式。

如果有两个纯四元数 v⊥=[0,v⊥] 和 u=[0,u]，那么

uv⊥=[−u⋅v⊥,u×v⊥]=[0,u×v⊥]=u×v⊥将其代入得到

v⊥′=cos⁡(θ)v⊥+sin⁡(θ)(uv⊥)=(cos⁡(θ)+sin⁡(θ)u)v⊥如果我们将 (cos⁡(θ)+sin⁡(θ)u) 看做是一个四元数，我们就能将旋转写成四元数的乘积了。令 q=cos⁡(θ)+sin⁡(θ)u，可得

v⊥′=qv⊥如果能构造一个 q, 那么我们就能完成这个旋转了。对 q 继续变形

q=cos⁡(θ)+sin⁡(θ)u=[cos⁡(θ),0]+[0,sin⁡(θ)u]=[cos⁡(θ),sin⁡(θ)u]也就是说，如果旋转轴 u 的坐标为 [ux,uy,uz]T，旋转角为 θ，那么完成这一旋转所需要的四元数 q 可以构造为

q=cos⁡(θ)+sin⁡(θ)uxi+sin⁡(θ)uyj+sin⁡(θ)uzk3D 旋转公式（四元数型，正交情况） ​当 v⊥ 正交于旋转轴 u 时，旋转 θ 角度之后的 $\mathbf{v'_⊥} $ 可以使用四元数乘法来获得. 令 v⊥=[0,v⊥], q=[cos⁡(θ),sin⁡(θ)u], 那么

v⊥′=qv⊥这个四元数 q 还有一个性质

‖q‖=cos2⁡(θ)+(sin⁡(θ)u⋅sin⁡(θ)u)=cos2⁡(θ)+sin2⁡(θ)(u⋅u)=cos2⁡(θ)+sin2⁡(θ)(‖u‖2)=cos2⁡(θ)+sin2⁡(θ)=1也就是说，我们构造出来的这个 q 其实是一个单位四元数。复数乘法的几何意义可以理解为缩放与旋转的复合，这个性质类比到四元数，因为 ‖q‖=1，它所代表的变化不会对原向量进行缩放，是一个纯旋转。

(2) v∥ 的旋转 ​如果一个向量 v∥ 平行于 u， 那么旋转不会对它做出任何变换，也就是说：

3D 旋转公式（四元数型，平行情况） ​当 v∥ 平行于旋转轴 u 时, 旋转 θ 角度之后的 v∥′ 用四元数可以写为：

v∥′=v∥(3) v 的旋转 ​v′=v∥′+v⊥′=v∥+qv⊥(q=[cos⁡(θ),sin⁡(θ)u])我们有更好的办法进一步化简它：

引理 1 ​如果 q=[cos⁡(θ),sin⁡(θ)u], 且 u 为单位向量, 那么

q2=[cos⁡(2θ),sin(2θ)u]证明略(使用 Graßmann 积和一些三角恒等式即可)。

几何意义：如果绕同一个轴连续旋转 θ 度两次，那么所做出的变换等同于直接绕该轴旋转 2θ 度。

有了这个引理，加上四元数逆的定义 qq−1=1，可以对原本的旋转公式进行变形

v′=v∥+qv⊥(q=[cos⁡(θ),sin⁡(θ)u])=1⋅v∥+qv⊥=pp−1v∥+ppv⊥(p=[cos⁡(12θ),sin⁡(12θ)u],q=p2)=pp∗v∥+ppv⊥我们还能进一步化简这个公式：

引理 2 ​假设 v∥=[0,v∥] 是一个纯四元数, 而 q=[α,βu], 其中 u 是一个单位向量, α,β∈\R. 在这种条件下, 如果 v∥ 平行于 u, 那么 $qv_∥ = v_∥q $

证明：

等式左边

LHS=qv∥=[α,βu]⋅[0,v∥]=[0−βu⋅v∥,αv∥+0+βu×v∥]=[−βu⋅v∥,αv∥]等式右边

RHS=v∥q=[0,v∥]⋅[α,βu]=[0−v∥⋅βu,0+αv∥+v∥×βu]=[−v∥⋅βu,αv∥]=[−βu⋅v∥,αv∥]=LHS引理 3 ​假设 v⊥=[0,v⊥] 是一个纯四元数, 而 q=[α,βu], 其中 u 是一个单位向量, α,β∈\R. 在这种条件下, 如果 v⊥ 正交于 u, 那么 $qv_⊥ = v_⊥q^* $

证明：

等式左边

LHS=qv⊥=[α,βu]⋅[0,v⊥]=[0−βu⋅v⊥,αv⊥+0+βu×v⊥]=[0,αv⊥+βu×v⊥]等式右边

RHS=v⊥q∗=[0,v⊥]⋅[α,−βu]=[0+v⊥⋅βu,0+αv⊥+v⊥×(−βu)]=[0,αv⊥+v⊥×(−βu)]=[0,αv⊥−(−βu)×v⊥]=[0,αv⊥+βu×v⊥]=LHS对之前的公式做出最后的变形

v′=pp∗v∥+ppv⊥=pv∥p∗+pv⊥p∗=p(v∥+v⊥)p∗=pvp∗3D 旋转公式（四元数型，一般情况） ​任意向量 v 沿着以单位向量定义的旋转轴 u 旋转 θ 度之后的 v′ 可以使用四元数乘法来获得。令 v=[0,v], q=[cos⁡(12θ),sin⁡(12θ)u], 那么：

v′=qvq∗=qvq−1它与 Rodrigues' Rotation Formula 完全等价：

qvq∗=[0,cos⁡(θ)v+(1−cos⁡(θ))(u⋅v)u+sin⁡(θ)(u×v)]证明略（可能会用到 a×(b×c)=(a⋅c)b−(a⋅b)c 这个公式）。

因为所有的旋转四元数的实部都只是一个角度的余弦值，假设有一个单位四元数 q=[a,b]，如果我们想获取它所对应旋转的角度，那么我们可以直接得到

θ2=cos−1⁡(a)如果想要再获得旋转轴，那么只需要将 b 的每一项都除以 sin⁡(θ2) 即可

u=bsin⁡(cos−1⁡(a))4. 3D 旋转的矩阵形式 ​在实际的应用中，我们可能会需要将旋转与平移和缩放进行复合，所以需要用到四元数旋转的矩阵形式。

前面讨论过左乘四元数 q=a+bi+cj+dk 等同于如下矩阵

L(q)=[a−b−c−dba−dccda−bd−cba]右乘 q 等同于如下矩阵

R(q)=[a−b−c−dbad−cc−dabdc−ba]我们可以利用这两个公式将 v′=qvq∗ 写成矩阵形式。

假设

a=cos⁡(12θ)b=sin⁡(12θ)uxc=sin⁡(12θ)uyd=sin⁡(12θ)uzq=a+bi+cj+dk我们就能得到

qvq∗=L(q)R(q∗)v(等价于R(q∗)L(q)v)=[a−b−c−dba−dccda−bd−cba][abcd−bad−c−cda−b−d−cba](注意R(q∗)=R(q)T)v=[a2+b2+c2+d2ab−ab−cd+cdac+bd−ac−bdad−bc+bc−adab−ab+cd−cdb2+a2−d2−c2bc−ad−ad+bcbd+ac+bd+acac−bd−ac+bdbc+ad+ad+bcc2−d2+a2−b2cd+cd−ab−abad+bc−bc−adbd−ac+bd−accd+cd+ab+abd2−c2−b2+a2]v=[100001−2c2−2d22bc−ad2ac+2bd02bc+ad1−2b2−2d22cd−2ab02bd−2ac2ab+2cd1−2b2−2c2]v因为 a2+b2+c2+d2=1，这个式子可以化简为

qvq∗=[100001−2c2−2d22bc−ad2ac+2bd02bc+ad1−2b2−2d22cd−2ab02bd−2ac2ab+2cd1−2b2−2c2]v因为矩阵的最外圈不会对 v 进行任何变换，我们可以将它压缩成 3×3 矩阵（用作 3D 向量的变换）：

3D 旋转公式（矩阵型） ​任意向量 v 沿着以单位向量定义的旋转轴 u 旋转 θ 度之后的 v′ 可以使用矩阵乘法来获得. 令 a=cos⁡(12θ), b=sin⁡(12θ)ux, c=sin⁡(12θ)uy, d=sin⁡(12θ)uz, 那么:

v′=[1−2c2−2d22bc−ad2ac+2bd2bc+ad1−2b2−2d22cd−2ab2bd−2ac2ab+2cd1−2b2−2c2]v虽然 3D 旋转的矩阵形式不如四元数形式简单，而且占用更多的空间，但是对大批量的变换，使用 预计算 好的矩阵是比四元数乘法更有效率的。

5. 旋转的复合 ​四元数的旋转是可以复合的。

引理 4 ​对于任意四元数 q1=[s,v]、q2=[t,u]：

q1∗q2∗=(q2q1)∗证明略。

假设有两个表示沿着不同轴，不同角度旋转的四元数 q1，q2，我们先对 v 进行 q1 的变换，再进行 q2 的变换，得到

v′=q1vq1∗v″=q2v′q2∗=q2q1vq1∗q2∗=q2q1v(q2q1)∗也就是说 qnet=q2q1，它是沿着一个全新的旋转轴进行的一次等价旋转，仅仅只有旋转的结果相同。

这个结果也可以推广到更多个旋转的复合。

6. 双倍覆盖 ​四元数与 3D 旋转的关系并不是一对一的，同一个 3D 旋转可以使用两个不同的四元数来表示。对任意的单位四元数 q=[cos⁡(12θ),sin⁡(12θ)u]，q 与 −q 代表的是同一个旋转。如果 q 表示的是沿着旋转轴 u 旋转 θ 度，那么 −q 代表的是沿着相反的旋转轴 −u 旋转 (2π−θ) 度：

−q=[cos⁡(12θ),sin⁡(12θ)u]=[cos⁡(π−12θ),sin⁡(π−12θ)u]所以，这个四元数的旋转角度为 2(π−12θ)=(2π−θ) 。

从四元数的旋转公式也能推导出相同的结果

(−q)v(−q)∗=(−1)2qvq∗=qvq∗所以，我们经常说单位四元数与 3D 旋转有一个“2 对 1 满射同态”（2-1 Surjective Homomorphism）关系。或者说单位四元数 双倍覆盖（Double Cover）了 3D 旋转。

因为这个映射是满射，我们可以说所有的单位四元数都对应着一个 3D 旋转。或者说，一个四维单位超球面（也叫做 S3）上任意一点所对应的四元数（‖q‖=1）都对应着一个 3D 旋转，这一点会在之后讨论旋转插值的时候会用到。

由于旋转矩阵中的每一项都包含了四元数两个分量的乘积，q 与 −q 的旋转矩阵是完全相同的。旋转矩阵并不会出现双倍覆盖的问题 ．

7. 指数形式 ​类似于复数的欧拉公式，四元数也有一个类似的公式。如果 u 是一个单位 向量，那么对于单位四元数 𝑢=[0,u]，有

euθ=cos⁡(θ)+usin⁡(θ)=cos⁡(θ)+usin⁡(θ)也就是说， q=[cos⁡(12θ),sin⁡(12θ)u] 可以使用指数表示为 euθ。这个公式的证 36 明与欧拉公式的证明非常类似，直接使用级数展开就可以了。

有了指数型的表示方式，我们就能够将之前四元数的旋转公式改写为指数形式了：

3D 旋转公式（指数型） ​任意向量 v 沿着以单位向量定义的旋转轴 u 旋转 θ 角度之后的 v′ 可以 使用四元数的指数表示。令 v=[0,v], u=[0,u], 那么：

v′=euθ2ve−uθ2有了四元数的指数定义，我们就能够定义四元数的更多运算。

自然对数 ​log(q)=log(euθ)=[0,uθ]幂运算 ​qt=(euθ)t=eu(tθ)=[cos⁡(tθ),sin⁡(tθ)u]可以看到，一个单位四元数的 t 次幂等同于将它旋转角度缩放至 t 倍。

8. 四元数插值 ​假设有两个旋转变换 q0=[cos⁡(θ0),sin⁡(θ0)u0] 和 q1=[cos⁡(θ1),sin(θ1)u1], 希望找出一些中间变换 qt, 让初始变换 q0 能够平滑地过渡到最终变换 q1，t∈[0,1]。

TODO: ...