三维空间中的几何变换(旋转矩阵 变换矩阵 旋转向量 欧拉角 四元数)

旋转矩阵表示形式

由欧拉旋转定理可得：刚体在三位空间里的一般运动可以分解为刚体上某一点的平移，以及绕过该点旋转轴的转动；

只考虑旋转：

三维空间中的点p在坐标系 ( e 1 , e 2 , e 3 ) (e_1, e_2, e_3) (e1​,e2​,e3​)中的坐标为 ( a 1 , a 2 , a 3 ) (a_1, a_2, a_3) (a1​,a2​,a3​),当坐标系 ( e 1 , e 2 , e 3 ) (e_1, e_2, e_3) (e1​,e2​,e3​)旋转到坐标系 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e_1^{\prime}, e_2^{\prime}, e_3^{\prime}) (e1′​,e2′​,e3′​),点p的坐标为 ( a 1 ′ , a 2 ′ , a 3 ′ ) (a_1^{\prime}, a_2^{\prime}, a_3^{\prime}) (a1′​,a2′​,a3′​)，如式1所示：

[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (1) \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]\tag1 [e1​,e2​,e3​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​(1)

等式两边同乘 [ e 1 , e 2 , e 3 ] T \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]^T [e1​,e2​,e3​]T，由于 e 1 , e 2 , e 3 \boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3} e1​,e2​,e3​为正交向量有：

[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] ≜ R a ′ (2) \left[\begin{array}{c} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq \boldsymbol{R} \boldsymbol{a}^{\prime}\tag2 ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​≜Ra′(2)

则称 R R R 为旋转矩阵，其包含以下性质：

R是正交矩阵: R − 1 = R , R T R = I R^{-1}=R, \ R^TR = I R−1=R,RTR=IR的行列式为1： ∣ R ∣ = 1 |R|=1 ∣R∣=1

注：上式两个条件是充分必要条件，只要满足上式两个条件的矩阵都可以作为一个旋转矩阵，即：

S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } (3) S O(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}\tag3 SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(3)

将所有满足式3的矩阵称为 Special Orthognal Group （特殊正交群），下面只考虑三维空间中的特殊正交群，坐标系1到坐标系2的之间的旋转关系可由式4来表示：

a 1 = R 12 a 2 a 2 = R 21 a 1 (4) a_1 = R_{12}a_2\\ a_2 = R_{21}a1\tag4 a1​=R12​a2​a2​=R21​a1(4)

其中 R 21 = R 12 − 1 = R 12 T R_{21}=R_{12}^{-1}=R_{12}^{T} R21​=R12−1​=R12T​，若一个旋转矩阵取转置或取逆，则表示一个相反方向的旋转；

旋转+平移：

若同时考虑平移，则两个坐标系下p点的坐标关系如式5所示：

a ′ = R a + t (5) a^{\prime} = Ra+t\tag5 a′=Ra+t(5)

具体表示形式为：

a 2 = R 21 ⋅ a 1 + t 21 a 1 = R 12 ⋅ a 2 + t 12 a_{2}=R_{21} \cdot a_{1}+t_{21} \\ a_{1}=R_{12} \cdot a_{2}+t_{12} a2​=R21​⋅a1​+t21​a1​=R12​⋅a2​+t12​

注： t 12 ≠ − t 21 t_{12}\neq -t_{21} t12​=−t21​

齐次坐标表示：

由于刚体在三维空间中可能发生多次旋转与平移，如式6所示：

b = R 1 a + t 1 , c = R 2 b + t 2 . ⟹ c = R 2 ( R 1 a + t 1 ) + t 2 . (6) \boldsymbol{b}=\boldsymbol{R}_{1} a+t_{1}, \quad c=\boldsymbol{R}_{2} b+t_{2} . \quad \Longrightarrow \quad c=\boldsymbol{R}_{2}\left(\boldsymbol{R}_{1} a+t_{1}\right)+t_{2} .\tag6 b=R1​a+t1​,c=R2​b+t2​.⟹c=R2​(R1​a+t1​)+t2​.(6)

为了方便表达这种运动，引入了其次坐标表示形式如式7所示：

[ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] [a′1​]=[R0T​t1​][a1​]≜T[a1​]

其中 T = [ R t 0 T 1 ] T = \left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right] T=[R0T​t1​] 为变换矩阵(同时包含旋转与平移),变换矩阵的逆 T − 1 \boldsymbol{T}^{-1} T−1为：

T − 1 = [ R T − R T t 0 T 1 ] \boldsymbol{T}^{-1}=\left[\begin{array}{cc} \boldsymbol{R}^{T} & -\boldsymbol{R}^{T} t \\ \mathbf{0}^{T} & 1 \end{array}\right] T−1=[RT0T​−RTt1​]

将变换矩阵的集合称为特殊欧式群SE(3) (Special Euclidean Group)：

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } (7) \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\}\tag7 SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}(7)

其中 [ a ′ 1 ] \left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1\end{array}\right] [a′1​] 为齐次坐标，齐次坐标乘以任意非零常熟仍然表示三维空间中的同一个坐标：

a ~ = [ a 1 ] = k [ a 1 ] (8) \tilde{a}=\left[\begin{array}{l} a \\ 1 \end{array}\right]=k\left[\begin{array}{l} a \\ 1 \end{array}\right]\tag8 a~=[a1​]=k[a1​](8)

则多次的旋转与平移可以如式9所示：

b ~ = T 1 a ~ , c ~ = T 2 b ~ ⇒ c ~ = T 2 T 1 a ~ (9) \tilde{b}=\boldsymbol{T}_{1} \tilde{\boldsymbol{a}}, \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \tilde{\boldsymbol{b}} \quad \Rightarrow \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \boldsymbol{T}_{\mathbf{1}} \tilde{\boldsymbol{a}}\tag9 b~=T1​a~,c~=T2​b~⇒c~=T2​T1​a~(9)

旋转向量

旋转矩阵用9个元素的旋转矩阵仅仅表示3个自由度的信息，表达形式过于复杂，不节省内存空间。从直观上看，旋转可以表示为绕某个轴 n n n 旋转一定的角度 θ \theta θ，为此引入了旋转向量(Rotation Vector)或角轴/轴角（Angle Axis), 旋转矩阵与旋转向量是等价的，可以互相转换：

旋转向量到旋转矩阵的变换： 罗德里格斯公式(Rodrigus’s Formula):

R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ n ∧ (10) \boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}+\sin \theta \boldsymbol{n}^{\wedge}\tag{10} R=cosθI+(1−cosθ)nnT+sinθn∧(10)

其中 cos ⁡ θ I \cos \theta \boldsymbol{I} cosθI 为3x3矩阵， ( 1 − cos ⁡ θ ) n n T (1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T} (1−cosθ)nnT也为3x3的矩阵， n ∧ \boldsymbol{n}^{\wedge} n∧表示旋转向量 n n n 的叉积。

旋转矩阵到旋转向量：

θ = arccos ⁡ ( tr ⁡ ( R ) − 1 2 ) R n = n (11) \theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)\\ Rn = n\tag{11} θ=arccos(2tr(R)−1​)Rn=n(11)

n n n 为 R R R 矩阵对应特征值为1的特征向量；

欧拉角(Euler Angles)

旋转向量表示的旋转方式不够直观，无法从旋转向量中直接看出旋转轴的位置，欧拉角将旋转分解为三个方向上的旋转（如Z-Y-X）方向，轴可以为定轴或者动轴，轴的顺序也可以改变，如最常用绕动轴的Z-Y-X旋转方式:

绕物体的Z轴转动，得到偏航角yaw；绕旋转之后的Y轴旋转，得到俯仰角pitch；绕旋转之后的X轴旋转，得到滚转角roll；

万向锁（Gimbal Lock）：欧拉角在某些旋转情况下，旋转自由度会减1，存在奇异性；由于万向锁的存在，欧拉角不适合做插值或迭代，应用不广泛；

四元数

四元素基础

四元数是一种既节省空间，又不存在奇异性的刚体旋转表示方式；

在复数平面坐标系下，可以用单位复数来表达旋转操作：

z = x + i y = ρ e i θ (12) z=x+i y=\rho e^{i \theta}\tag{12} z=x+iy=ρeiθ(12)

当 z z z 乘以一个纯虚数 i i i 表示原来的向量旋转90度：

z i = ( x + i y ) i = ρ e i ( θ + π 2 ) (13) zi=(x+i y)i=\rho e^{i (\theta+\frac{\pi}{2})}\tag{13} zi=(x+iy)i=ρei(θ+2π​)(13)

在三维情况下， 四元数可以作为复数的扩充：

q = q 0 + q 1 i + q 2 j + q 3 k (14) \boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k\tag{14} q=q0​+q1​i+q2​j+q3​k(14)

四元数包含1个实部与3个虚部，虚部之间满足以下的关系（自己与自己运算像复数运算，自己与别人运算像叉乘）：

{ i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j (15) \left\{\begin{array}{l} i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j \end{array}\right.\tag{15} ⎩ ⎨ ⎧​i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j​(15)

单位四元数可以表达旋转：

q = q 0 + q 1 i + q 2 j + q 3 k , q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 (16) \boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k, \quad \boldsymbol{q}=[s, \boldsymbol{v}], \quad s=q_{0} \in \mathbb{R}, \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}\tag{16} q=q0​+q1​i+q2​j+q3​k,q=[s,v],s=q0​∈R,v=[q1​,q2​,q3​]T∈R3(16)

基本运算如下所示：

加减法：

q a ± q b = [ s a ± s b , v a ± v b ] \begin{aligned} \boldsymbol{q}_{a} \pm \boldsymbol{q}_{b}=& {\left[s_{a} \pm s_{b}, \boldsymbol{v}_{a} \pm \boldsymbol{v}_{b}\right] }\end{aligned} qa​±qb​=​[sa​±sb​,va​±vb​]​

乘法：

q a q b = s a s b − x a x b − y a y b − z a z b + ( s a x b + x a s b + y a z b − z a y b ) i + ( s a y b − x a z b + y a s b + z a x b ) j + ( s a z b + x a y b − y b x a + z a s b ) k \begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{b} x_{a}+z_{a} s_{b}\right) k \end{aligned} qa​qb​=​sa​sb​−xa​xb​−ya​yb​−za​zb​+(sa​xb​+xa​sb​+ya​zb​−za​yb​)i+(sa​yb​−xa​zb​+ya​sb​+za​xb​)j+(sa​zb​+xa​yb​−yb​xa​+za​sb​)k​

q a q b = [ s a s b − v a T v b , s a v b + s b v a + v a × v b ] \begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=\left[s_{a} s_{b}-\boldsymbol{v}_{a}^{T} \boldsymbol{v}_{b}, s_{a} \boldsymbol{v}_{b}+s_{b} \boldsymbol{v}_{a}+\boldsymbol{v}_{a} \times \boldsymbol{v}_{b}\right] \end{aligned} qa​qb​=[sa​sb​−vaT​vb​,sa​vb​+sb​va​+va​×vb​]​

共轭：

q a ∗ = s a − x a i − y a j − z a k = [ s a , − v a ] \boldsymbol{q}_{a}^{*}=s_{a}-x_{a} i-y_{a} j-z_{a} k=\left[s_{a},-\boldsymbol{v}_{a}\right] qa∗​=sa​−xa​i−ya​j−za​k=[sa​,−va​]

取模长：

∥ q a ∥ = s a 2 + x a 2 + y a 2 + z a 2 \left\|\boldsymbol{q}_{a}\right\|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}} ∥qa​∥=sa2​+xa2​+ya2​+za2​ ​

取逆：

q − 1 = q ∗ / ∥ q ∥ 2 \boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2} q−1=q∗/∥q∥2

数乘：

k q = [ k s , k v ] k \boldsymbol{q}=[k s, k \boldsymbol{v}] kq=[ks,kv]

点乘：

q a ⋅ q b = s a s b + x a x b i + y a y b j + z a z b k \boldsymbol{q}_a \cdot \boldsymbol{q}_{b}=s_{a} s_{b}+x_{a} x_{b} i+y_{a} y_{b} j+z_{a} z_{b} k qa​⋅qb​=sa​sb​+xa​xb​i+ya​yb​j+za​zb​k

旋转角转换为四元数：

q = [ cos ⁡ θ 2 , n x sin ⁡ θ 2 , n y sin ⁡ θ 2 , n z sin ⁡ θ 2 ] T (17) \boldsymbol{q}=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T}\tag{17} q=[cos2θ​,nx​sin2θ​,ny​sin2θ​,nz​sin2θ​]T(17)

四元数转为旋转向量：

{ θ = 2 arccos ⁡ q 0 [ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / sin ⁡ θ 2 (18) \left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right.\tag{18} {θ=2arccosq0​[nx​,ny​,nz​]T=[q1​,q2​,q3​]T/sin2θ​​(18)

使用四元数表示三维空间中的旋转

若点 p p p 经过一次以四元数 q q q 表示的旋转后得 p ′ p^{\prime} p′ ，包含以下两个步骤：

将 p p p 坐标用纯虚的四元数表示： p = [ 0 , x , y , z ] p =[0,x,y,z] p=[0,x,y,z]使用四元数q将其旋转(前面乘以 q q q ，后面乘以 q − 1 q^{-1} q−1 )： p ′ = q p q − 1 (19) p^{\prime}=qpq^{-1}\tag{19} p′=qpq−1(19)