我的沉寂终将轰动

文章目录

第 3 讲三维空间刚体运动
- 3.1 旋转矩阵
- - 3.1.1 点、向量和坐标系
  - 3.1.2 坐标系间的欧式变换
  - 3.1.3 变换矩阵与齐次坐标
- 3.3 旋转向量和欧拉角
- - 3.3.1 旋转向量
  - 3.3.2 欧拉角
- 3.4 四元数
- - 3.4.1 四元数的定义
  - 3.4.2 四元数的运算
  - 3.4.3 用四元数表示旋转
  - 3.4.4 四元数到其他旋转表示的转换

第 3 讲三维空间刚体运动

3.1 旋转矩阵

3.1.1 点、向量和坐标系

位置=坐标，姿态=向量

三维空间中的某个点的坐标也可以用 R3\mathbb{R}^3R3 来描述。假设在这个线性空间中，找到了该空间的一组基 (e1,e2,e3)\left(e_1,e_2,e_3\right)(e1,e2,e3)，那么，任意向量 aaa 在这组基下就有一个坐标：a=[e1,e2,e3][a1a2a3]=a1e1+a2e2+a3e3a=\left[e_1,e_2,e_3\right]\begin{bmatrix}a_1 \\a_2\\a_3 \end{bmatrix}=a_1e_1+a_2e_2+a_3e_3a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3这里的 (a1,a2,a3)T\left(a_1,a_2,a_3\right)^T(a1,a2,a3)T 称为 aaa 在此基下的坐标。
通常意义下的内积可以写成a⋅b=aTb=∑i=13aibi=∣a∣∣b∣cos<a,b>.a\cdot{b}=a^Tb=\sum_{i=1}^3a_ib_i=|a||b|cos\left<a,b\right>.a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩.其中 <a,b>\left<a,b\right>⟨a,b⟩ 指向量 a,ba,ba,b 的夹角。
外积可以写成这个样子：a×b=[0−a3a2a30−a1−a2a10]b=a∧b.a\times{b}= \begin{bmatrix}0 & -a_3 & a_2 \\a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}b=a^{\wedge}b.a×b=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b.外积的结果是一个向量，它的方向垂直于这个向量，大小为 ∣a∣∣b∣sin<a,b>|a||b|sin\left<a,b\right>∣a∣∣b∣sin⟨a,b⟩，是两个向量张成的四边形的有向面积。这里使用 ∧^\wedge∧ 符号，将 aaa 写成一个矩阵，这个矩阵事实上是一个反对称矩阵。这样子外积 a×ba\times{b}a×b 就可以表示为矩阵和向量的乘法 a∧ba^{\wedge}ba∧b ，把它变成了线性运算。此处的矩阵和反对称矩阵是一一对应的。

3.1.2 坐标系间的欧式变换

如果考虑到运动的机器人，常见的做法是设定一个惯性坐标系（或者叫世界坐标系，worldmap）。那么，相机视野中的某个向量 ppp ，它在相机坐标系下的坐标为 pcp_cpc ，而从世界坐标系下看，它的坐标为 pwp_wpw ，那么，这两个坐标之间是如何转换的呢？我们可以使用一个矩阵 TTT 来描述机器人位姿和世界坐标系之间的变换关系。

两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。在刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。可以说相机坐标系和世界坐标系之间差了一个欧式变换（Euclidean Transform）

欧式变换由旋转和平移组成。由于一个向量在欧式变换中自身并没有发生变换，故可以得到在两个坐标系下不同的坐标，有：[e1,e2,e3][a1a2a3]=[e1′,e2′,e3′][a1′a2′a3′].\left[e_1,e_2,e_3\right]\begin{bmatrix}a_1 \\a_2\\a_3 \end{bmatrix}=\left[e_1',e_2',e_3'\right]\begin{bmatrix}a_1' \\a_2'\\a_3' \end{bmatrix}.[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤.对上述等式的左右两边同时左乘 [e1Te2Te3T]\begin{bmatrix}e_1^T \\e_2^T\\e_3^T \end{bmatrix}⎣⎡e1Te2Te3T⎦⎤ ，那么左边的系数就变成了单位矩阵，所以：[a1a2a3]=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′][a1′a2′a3′]=Ra′.\begin{bmatrix}a_1 \\a_2\\a_3 \end{bmatrix}=\begin{bmatrix}e_1^Te_1' & e_1^Te_2' & e_1^Te_3' \\e_2^Te_1' & e_2^Te_2' & e_2^Te_3'\\e_3^Te_1' & e_3^Te_2' & e_3^Te_3' \end{bmatrix}\begin{bmatrix}a_1' \\a_2'\\a_3' \end{bmatrix}=Ra'.⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′.我们把中间的矩阵拿出来，定义为一个矩阵 RRR ，这个矩阵刻画了旋转前后同一个向量的坐标变换关系，也可以说 RRR 描述了旋转本身，因此可以称为旋转矩阵。

旋转矩阵是一个行列式为 1 的正交矩阵。因此 nnn 维旋转矩阵的集和定义如下：SO(n)={R∈Rn×n∣RRT=I,det⁡(R)=1}.SO\left(n\right)=\left\{R\in\mathbb{R}^{n\times n}|RR^T=I,\det\ \left(R \right)=1 \right\}.SO(n)={R∈Rn×n∣RRT=I,det (R)=1}.SO(n)SO\left(n\right)SO(n) 是特殊正交群（Special Orthogonal Group）的意思。这个集合由 nnn 维空间的旋转矩阵组成，特别地，SO(3)SO\left(3\right)SO(3) 就是指三维空间的旋转。

由于旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转。因此有：{a=Ra′a′=R−1a=RTa.\begin{cases}a=Ra' \\a'=R^{-1}a=R^Ta\end{cases}.{a=Ra′a′=R−1a=RTa.显然，RTR^TRT 刻画了一个相反的旋转。

在欧式变换中，除了旋转还有平移。考虑世界坐标系中的向量 aaa ，经过一次旋转（用 RRR 描述）和一次平移 ttt 后，得到了 a′a'a′ ，那么把旋转和平移合到一起，有a′=Ra+t.a'=Ra+t.a′=Ra+t.在实际情况中，我们会定义坐标系 1、坐标系 2，那么向量 aaa 在两个坐标系下的坐标为 a1a_1a1，a2a_2a2，它们之间的关系为：a1=R12a2+t12.a_1=R_{12}a_2+t_{12}.a1=R12a2+t12.这里的 R12R_{12}R12 是指 “把坐标系 2 的向量变换到坐标系 1” 中。如果以正常的xyz坐标系为世界坐标系，以逆时针旋转90°为相机坐标系，那么在相机坐标系下为（0，1，0）的坐标点，经过旋转后为（-1，0，0），这里就是相机坐标系下的向量变换到世界坐标系下的向量值。平移 t12t_{12}t12 ，它实际对应的是坐标系 1 原点指向坐标系 2 原点的向量，在坐标系 1 下取的坐标，所以记为 “从 1 到 2 的向量”。对于上一个例子，由于没有平移量，所以世界坐标系下的向量可以直接得到。

3.1.3 变换矩阵与齐次坐标

假设进行了两次变换：R1R_1R1，t1t_1t1 和 R2R_2R2，t2t_2t2 ：b=R1a+t1，c=R2b+t2.b=R_{1}a+t_{1}，c=R_{2}b+t_{2}.b=R1a+t1，c=R2b+t2.那么，从 aaa 到 ccc 的变换为 c=R2(R1a+t1)+t2.c=R_{2}\left(R_{1}a+t_{1}\right)+t_{2}.c=R2(R1a+t1)+t2.引入齐次坐标和变换坐标，重写变换矩阵为：[a′1]=[Rt0T1][a1]=T[a1].\begin{bmatrix}a' \\1\end{bmatrix}=\begin{bmatrix}R & t\\0^T & 1 \end{bmatrix}\begin{bmatrix}a \\1\end{bmatrix}=T\begin{bmatrix}a \\1\end{bmatrix}.[a′1]=[R0Tt1][a1]=T[a1].这里在一个三维向量的末尾添加 1，将其变为了四维向量，称为齐次坐标。再把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。在该式中，矩阵 TTT 称为变换矩阵。因此两次变换的叠加可以有更好的形式：b~=T1a~,c~=T2b~⇒c~=T2T1a~.\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}\Rightarrow\tilde{c}=T_2T_1\tilde{a}.b~=T1a~,c~=T2b~⇒c~=T2T1a~.之后默认 bbb 已经进行了齐次的转换。变换矩阵 TTT 又称为特殊欧氏群（Special Euclidean Group）：SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}.SE\left(3\right)=\left\{T=\begin{bmatrix}R & t \\0^T &1\end{bmatrix}\in\mathbb{R}^{4\times 4} |R\in SO\left(3\right),t\in \mathbb{R}^{3}\right\}.SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.SO(3)SO\left(3\right)SO(3) 叫特殊正交群，体现了 RRR 正交的性质，SE(3)SE\left(3\right)SE(3) 叫特殊欧氏群，代表 TTT 是专门用于向量之间进行欧式变换。并且用 T12T_{12}T12 表示从 2 到 1 的变换。

3.3 旋转向量和欧拉角

3.3.1 旋转向量

使用矩阵来表示旋转有以下两个缺点：

SO(3) 的旋转向量有 9 个量，但一次旋转只有 3 个自由度；使用 SE(3) 的 16 个量表达 6 个自由度的变换。这样的表达方式不够紧凑。
旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为 1。

事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量。而对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这样的变量正好是六维。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量 nnn，角度为 θ\thetaθ，那么向量 θn\theta nθn 也可以描述这个旋转。从旋转向量导旋转矩阵的转化过程由罗德里格斯公式表明：R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧.R=\cos\theta I+\left(1-\cos\theta\right)nn^T+\sin\theta n^\wedge.R=cosθI+(1−cosθ)nnT+sinθn∧.θ=arccos⁡tr(R)−12.\theta=\arccos\frac{tr\left(R\right)-1}{2}.θ=arccos2tr(R)−1.对于转轴 nnn，旋转轴上的向量在旋转后不发生改变，说明：Rn=n.Rn=n.Rn=n.

3.3.2 欧拉角

欧拉角使用了 3 个分离的转角，把一个旋转分解成 3 次绕不同轴的旋转。例如，先绕 XXX 轴旋转，再绕 YYY 轴，最后绕 ZZZ 轴，就得到了一个 XYZXY ZXYZ 轴的旋转。同理，可以定义 ZYZZY ZZYZ、ZYXZY XZYX 等旋转方式。

欧拉角当中比较常用的一种，便是用“偏航 − 俯仰 − 滚转”（yaw-pitch-roll）3 个角度来描述一个旋转。由于它等价于 ZYXZY XZYX 轴的旋转，那么：

绕物体的 ZZZ 轴旋转，得到偏航角 yaw.
绕旋转之后的 YYY 轴旋转，得到俯仰角 pitch.
绕旋转之后的 XXX 轴旋转，得到滚转角 roll.

此时就可以使用 [r,p,y]T\left[r,p,y\right]^T[r,p,y]T 这样一个三维的向量描述任意旋转。欧拉角会碰到万向锁问题。例如，在旋转过程中，pitch 为 ±90°\pm90\degree±90° 时，此时的 yaw 和 roll 描述的是相同的旋转，此时的旋转就少了一个自由度。

3.4 四元数

3.4.1 四元数的定义

使用复数集 C\mathbb{C}C 表示复平面上的向量，而复数的乘法则表示复平面上的旋转：例如，乘上复数 iii 相当于逆时针把一个复向量旋转 90°90\degree90°。类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数（Quaternion）。四元数既是紧凑的，也没有奇异性。一个四元数 qqq 拥有一个实部和三个虚部：q=q0+q1i+q2j+q3k.q=q_0+q_1i+q_2j+q_3k.q=q0+q1i+q2j+q3k.{i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j\begin{cases}i^2=j^2=k^2=-1\\ij=k,ji=-k\\jk=i,kj=-i \\ki=j,ik=-j\end{cases}⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j如果把 i,j,ki, j, ki,j,k 看成三个坐标轴，那么它们与自己的乘法和复数一样，相互之间的乘法和外积一样。有时人们也用一个标量和一个向量来表达四元数：q=[s,v]T,s=q0∈R,v=[q1,q2,q3]T∈R3.q=\left[s,v\right]^T,s=q_0\in \mathbb{R},v=\left[q_1,q_2,q_3\right]^T\in\mathbb{R}^3.q=[s,v]T,s=q0∈R,v=[q1,q2,q3]T∈R3.这里，sss 称为四元数的实部，而 vvv 称为它的虚部。如果一个四元数的虚部为 000，则称为实四元数；反之，若它的实部为 000，则称为虚四元数。

3.4.2 四元数的运算

现有两个四元数 qa,qbq_a,q_bqa,qb，它们向量表示为 [sa,va]T,[sb,vb]T\left[s_a,v_a\right]^T,\left[s_b,v_b\right]^T[sa,va]T,[sb,vb]T ，或者原始四元数表示为：qa=sa+xai+yaj+zak,qb=sb+xbi+ybj+zbk.q_a=s_a+x_ai+y_aj+z_ak,q_b=s_b+x_bi+y_bj+z_bk.qa=sa+xai+yaj+zak,qb=sb+xbi+ybj+zbk.

加法和减法qa±qb=[sa±sb,va±vb]T.q_a\pm q_b=\left[s_a\pm s_b,v_a\pm v_b\right]^T.qa±qb=[sa±sb,va±vb]T.
乘法
乘法是把 qaq_aqa 的每一项与 qbq_bqb 的每项相乘，最后相加：qaqb=[sasb−vaTvb,savb+sbva+va×vb]T.q_aq_b=\left[s_as_b-v_a^Tv_b,s_av_b+s_bv_a+v_a\times v_b\right]^T.qaqb=[sasb−vaTvb,savb+sbva+va×vb]T.由于最后一项外积的存在，四元数乘法通常是不可交换的，除非 vav_ava 和 vbv_bvb 在 R3\mathbb{R}^3R3 中共线，此时外积项为零。
模长
四元数的模长定义为：∣∣qa∣∣=sa2+xa2+ya2+za2.||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}.∣∣qa∣∣=sa2+xa2+ya2+za2.其中，两个四元数乘积的模即模的乘积，这使得单位四元数相乘后仍是单位四元数。
共轭
四元数的共轭是把虚部取成相反数：qa∗=sa−xai−yaj−zak=[sa,−va]T.q_a^*=s_a-x_ai-y_aj-z_ak=\left[s_a,-v_a\right]^T.qa∗=sa−xai−yaj−zak=[sa,−va]T.四元数的共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：q∗q=qq∗=[sa2+vTv,0]T.q^*q=qq^*=\left[s_a^2+v^Tv,0\right]^T.q∗q=qq∗=[sa2+vTv,0]T.
逆
一个四元数的逆为q−1=q∗/∣∣q∣∣2.q^{-1}=q^*/||q||^2.q−1=q∗/∣∣q∣∣2.按此定义，四元数和自己的逆的乘积为实四元数 111
数乘
和向量相似，四元数可以与数相乘：kq=[ks,kv]T.kq=\left[ks,kv\right]^T.kq=[ks,kv]T.

3.4.3 用四元数表示旋转

首先，把三维空间点用一个虚四元数来描述：p=[0,x,y,z]T=[0,v]T.p=\left[0,x,y,z\right]^T=\left[0,v\right]^T.p=[0,x,y,z]T=[0,v]T.相当于把四元数的 3 个虚部与空间中的 3 个轴相对应。那么，旋转后的点p′=qpq−1.p'=qpq^{-1}.p′=qpq−1.最后把 p′p'p′ 的虚部取出，即得到旋转之后点的坐标。

3.4.4 四元数到其他旋转表示的转换

四元数乘法也可以写成一种矩阵的乘法。设 q=[s,v]Tq=\left[s,v\right]^Tq=[s,v]T，那么，定义如下的符号 +^++ 和 ⊕^\oplus⊕ 为q+=[s−vTvsI+v∧],q⊕=[s−vTvsI−v∧].q^+=\begin{bmatrix}s & -v^T \\v & sI+ v^\wedge\end{bmatrix}, q^\oplus=\begin{bmatrix}s & -v^T \\v & sI- v^\wedge\end{bmatrix}.q+=[sv−vTsI+v∧],q⊕=[sv−vTsI−v∧].通过四元数的乘法可得：q1q2=q1∧q2=q2⊕q1.q_1q_2=q_1^\wedge q_2=q_2^\oplus q_1.q1q2=q1∧q2=q2⊕q1.然后，考虑使用四元数对空间点进行旋转的问题。有p′=qpq−1=q+p+q−1=q+(q−1)⊕p.p'=qpq^{-1}=q^+p^+q^{-1}=q^+\left(q^{-1}\right)^\oplus p.p′=qpq−1=q+p+q−1=q+(q−1)⊕p.代入两个符号对应的矩阵，得q+(q−1)⊕=[100TvvT+s2I+2sv∧+(v∧)2].q^+\left(q^{-1}\right)^\oplus=\begin{bmatrix}1 & 0 \\0^T & vv^T+s^2I+2sv^\wedge+ \left(v^\wedge\right)^2\end{bmatrix}.q+(q−1)⊕=[10T0vvT+s2I+2sv∧+(v∧)2].其中，右下角给出了从四元数到旋转矩阵的变换关系：R=vvT+s2I+2sv∧+(v∧)2.R=vv^T+s^2I+2sv^\wedge+ \left(v^\wedge\right)^2.R=vvT+s2I+2sv∧+(v∧)2.对了得到四元数到旋转向量的转换公式，对上式两侧求迹，得 tr(R)=4s2−1.tr\left(R\right)=4s^2-1.tr(R)=4s2−1.θ=arccos⁡(2s2−1).\theta=\arccos\left(2s^2-1\right).θ=arccos(2s2−1).θ=2arccos⁡s.\theta=2\arccos s.θ=2arccoss.总而言之，四元数到旋转向量得转换公式如下：{θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]T/sin⁡θ2.\begin{cases}\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{cases}.{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sin2θ.