文章目录

• * 坐标系约定与外积的关系
  • 1 三维向量的旋转变换
  • * 1.1 右手旋转
    • 1.2 左手旋转
  • 2 正交群
  • 3 旋转矩阵
  • * 3.1 指数映射
    • 3.2 左右手法则
    • 3.3 坐标变换
  • 4 四元数
  • * 4.1 指数映射
    • 4.2 Hamilton与JPL
  • 参考文献

坐标系约定与外积的关系

如果统一使用右手坐标系，那么外积的方向要根据右手法则来判断；如果使用左手坐标系，就要使用左手法则来判断。
但是无论哪一种方式，外积的计算都可以写成以下代数形式。给定三维向量 a , b a,b a,b，其外积可以表示为：

a × b = [ a ] × b = [ 0 − a z a y a z 0 − a x − a y a x 0 ] a\times b=[a]_{\times}b=\left[\begin{matrix} 0 & -a_z & a_y \\ a_z & 0& -a_x\\ -a_y & a_x & 0 \end{matrix}\right] a×b=[a]×​b=⎣⎡​0az​−ay​​−az​0ax​​ay​−ax​0​⎦⎤​
本篇文章默认使用右手坐标系。

1 三维向量的旋转变换

在某个坐标系中，将一个向量 x x x沿着单位轴向量 u u u旋转角度 ϕ \phi ϕ，得到的新向量记为 x ′ x^{'} x′，那么 x x x与 x ′ x^{'} x′有什么关系呢？（这里称 ϕ u \phi u ϕu为旋转向量）
考虑如下两种情况，右手法则旋转和左手法则旋转。

1.1 右手旋转

如下图所示：

将 x x x分解为与 u u u垂直的 x ⊥ x_{\perp} x⊥​，和平行的 x ∥ x_{\parallel} x∥​：
x = x ∥ + x ⊥ x=x_{\parallel}+x_{\perp} x=x∥​+x⊥​
x ∥ = u ( ∣ ∣ x ∣ ∣ c o s α ) = u u T x , x ⊥ = x − x ∥ = x − u u T x x_{\parallel}=u(||x||cos\alpha)=uu^Tx,\quad x_{\perp}=x-x_{\parallel}=x-uu^Tx x∥​=u(∣∣x∣∣cosα)=uuTx,x⊥​=x−x∥​=x−uuTx
而与轴平行部分不会旋转，所以有：
x ∥ ′ = x ∥ x_{\parallel}^{'}=x_{\parallel} x∥′​=x∥​

我们构建两个正交向量 e 1 e_1 e1​和 e 2 e_{2} e2​， ∣ ∣ e 1 ∣ ∣ = ∣ ∣ e 2 ∣ ∣ ||e_1||=||e_2|| ∣∣e1​∣∣=∣∣e2​∣∣：
e 1 = x ⊥ , e 2 = u × x ⊥ = u × x e_1=x_{\perp},\quad e_2=u\times x_{\perp}=u\times x e1​=x⊥​,e2​=u×x⊥​=u×x
所以有：
x ⊥ ′ = e 1 c o s ϕ + e 2 s i n ϕ x_{\perp}^{'}=e_1cos\phi+e_2sin\phi x⊥′​=e1​cosϕ+e2​sinϕ
x ⊥ ′ = x ⊥ c o s ϕ + ( u × x ) s i n ϕ x_{\perp}^{'}=x_{\perp}cos\phi +(u\times x)sin \phi x⊥′​=x⊥​cosϕ+(u×x)sinϕ
得到 x ′ x^{'} x′为：
x ′ = x ∥ + x ⊥ c o s ϕ + ( u × x ) s i n ϕ x^{'}=x_{\parallel}+x_{\perp}cos\phi +(u\times x)sin \phi x′=x∥​+x⊥​cosϕ+(u×x)sinϕ
将 x ∥ = u u T x x_{\parallel}=uu^Tx x∥​=uuTx和 x ⊥ = x − u u T x x_{\perp}=x-uu^Tx x⊥​=x−uuTx带入上式得到：
x ′ = [ I + s i n ϕ [ u ] × + ( 1 − c o s ϕ ) [ u ] × 2 ] x x^{'}=\left[I+sin\phi[u]_{\times}+(1-cos\phi)[u]_{\times}^2\right]x x′=[I+sinϕ[u]×​+(1−cosϕ)[u]×2​]x

1.2 左手旋转

将 x x x沿着 u u u左手法则旋转角度 ϕ \phi ϕ等价于右手法则旋转 − ϕ -\phi −ϕ，所以立即得到：
x ′ = [ I − s i n ϕ [ u ] × + ( 1 − c o s ϕ ) [ u ] × 2 ] x x^{'}=\left[I-sin\phi[u]_{\times}+(1-cos\phi)[u]_{\times}^2\right]x x′=[I−sinϕ[u]×​+(1−cosϕ)[u]×2​]x

2 正交群

定义一个线性变换 r r r:
r : R 3 → R 3 ; v → r ( v ) r:R^3 \rightarrow R^3; \quad v\rightarrow r(v) r:R3→R3;v→r(v)
其满足如下条件：

• 保持向量的长度的不变：
  ∣ ∣ r ( v ) ∣ ∣ = < r ( v ) , r ( v ) > = < v , v > = ∣ ∣ v ∣ ∣ ||r(v)||=\sqrt{\left}=\sqrt{\left}=||v|| ∣∣r(v)∣∣=⟨r(v),r(v)⟩ ​=⟨v,v⟩ ​=∣∣v∣∣
• 保持向量之间的夹角不变。（这里指余弦值不变）
  < r ( v ) , r ( w ) > = < v , w > = ∣ ∣ v ∣ ∣ ∣ ∣ w ∣ ∣ c o s α \left=\left=||v||||w||cos\alpha ⟨r(v),r(w)⟩=⟨v,w⟩=∣∣v∣∣∣∣w∣∣cosα
• 保持向量之间的相对方向不变。（满足右手准则关系）
  变 换 之 前 ： u × v = w ； 那 么 变 换 之 后 要 满 足 ： r ( u ) × ( v ) = r ( w ) 变换之前：u\times v=w；\quad 那么变换之后要满足：r(u)\times(v)=r(w) 变换之前：u×v=w；那么变换之后要满足：r(u)×(v)=r(w)
  其中前两个条件是等价的，证明如下：
  因为 r ( . ) r(.) r(.)是线性变换，不妨设 r ( v ) = R v r(v)=Rv r(v)=Rv，如果已知第一个条件，那么有：
  < r ( v ) , r ( v ) > = ( R v ) T R v = v T R T R v = v T v \left=(Rv)^T{Rv}={v^T}{R^T}Rv={v^T}v ⟨r(v),r(v)⟩=(Rv)TRv=vTRTRv=vTv
  R T R = I = R R T {R^T}R=I=RR^T RTR=I=RRT
  < r ( v ) , r ( w ) > = v T R T R w = v T w = < v , w > \left=v^TR^TRw=v^Tw=\left ⟨r(v),r(w)⟩=vTRTRw=vTw=⟨v,w⟩
  如果已知第二个条件，那么也可按照上述方法证出第一个条件。
  现在我们定义正交群 S O ( 3 ) SO(3) SO(3)：
  S O ( 3 ) : { r : R 3 → R 3 / ∀ v , w ∈ R 3 ,   ∣ ∣ r ( v ) ∣ ∣ = ∣ ∣ v ∣ ∣ ,   r ( v ) × ( w ) = r ( v × w ) } SO(3):\left\{r:{R^3}\rightarrow{R^3}\quad /\quad \forall v,w \in R^3, \, ||r(v)||=||v||,\, r(v)\times(w)=r(v\times w) \right\} SO(3):{r:R3→R3/∀v,w∈R3,∣∣r(v)∣∣=∣∣v∣∣,r(v)×(w)=r(v×w)}

3 旋转矩阵

3.1 指数映射

令 u u u是一个单位向量， ϕ \phi ϕ是一个标量值，令 v = ϕ u v=\phi u v=ϕu，定义指数映射Exp(.)，将旋转矩阵 R R R定义如下：
R = E x p ( v ) = e [ v ] × R=Exp(v)=e^{[v]_{\times}} R=Exp(v)=e[v]×​
可以验证该 R R R代表的线性变换满足正交群的条件。
计算 R R R：
R = e ϕ [ u ] × = I + ϕ [ u ] × + 1 2 ϕ 2 [ u ] × 2 + 1 3 ϕ 3 [ u ] × 3 + . . . R=e^{\phi [u]_{\times}}=I+\phi [u]_{\times}+\frac{1}{2}\phi^2[u]^2_{\times}+\frac{1}{3}\phi^3[u]^3_{\times}+… R=eϕ[u]×​=I+ϕ[u]×​+21​ϕ2[u]×2​+31​ϕ3[u]×3​+…
因为：
[ u ] × 2 = u u T − I , [ u ] × 3 = − [ u ] × [u]_{\times}^2=uu^T-I, \quad [u]^3_{\times}=-[u]_{\times} [u]×2​=uuT−I,[u]×3​=−[u]×​
所以有:
[ u ] × 4 = − [ u ] × 2 , [ u ] × 5 = [ u ] × , [ u ] × 6 = [ u ] × 2 , [ u ] × 7 = − [ u ] × [u]_{\times}^4=-[u]^2_{\times} ,\quad [u]_{\times}^5=[u]_{\times} ,\quad [u]_{\times}^6=[u]^2_{\times} ,\quad [u]_{\times}^7=-[u]_{\times} [u]×4​=−[u]×2​,[u]×5​=[u]×​,[u]×6​=[u]×2​,[u]×7​=−[u]×​
将其带入 R R R的表达式，可以化简得到：
R = I c o s ϕ + [ u ] × s i n ϕ + u u T ( 1 − c o s ϕ ) R=Icos\phi +[u]_{\times}sin\phi+uu^T(1-cos\phi) R=Icosϕ+[u]×​sinϕ+uuT(1−cosϕ)
我们可以看到这里的 R R R和本文开头推出的三维向量旋转变换的结果一样（右手法则）：
x ′ = R x x^{'}=Rx x′=Rx

3.2 左右手法则

所以在三维向量旋转时，如果我们知道旋转轴为 u u u，那么沿右手方向旋转 ϕ \phi ϕ后，得到的新向量为：
x ′ = e ϕ [ u ] × x x^{'}=e^{\phi [u]_{\times}}x \\ x′=eϕ[u]×​x
沿左手方向旋转 ϕ \phi ϕ后，得到的新向量为：
x ′ = e − ϕ [ u ] × x x^{'}=e^{-\phi [u]_{\times}}x \\ x′=e−ϕ[u]×​x

3.3 坐标变换

我们前面一直在讨论向量 x x x绕着过原点的轴 u u u旋转角度 ϕ \phi ϕ之后得到的 x ′ x^{'} x′坐标是多少，不过这里只涉及一个坐标系，记为M，下面我们换一种思路来解释这个过程。

我们把整个坐标系M都绕着 u u u转过角度 ϕ \phi ϕ（当然也包括 x x x），得到的新坐标系记为N；记 x N x_N xN​为 x x x在N系的坐标， x M x_M xM​为 x x x在M系的坐标；显然 x N x_N xN​等于旋转之前x在M系的坐标，也就是说：
x M = e ϕ [ u ] × x N = R x N x_M=e^{\phi [u]_{\times}}x_{N}=Rx_N xM​=eϕ[u]×​xN​=RxN​

显然，R可以表示某向量在两个坐标系之间的坐标变换关系，这在SLAM中十分有用。

因为 x x x是过原点的直线，所以M与N系的原点重合。当N系原点在M系中的坐标为t时候，我们按照上式算出的结果是在与N系原点重合并与M系各轴平行的临时坐标系 M 1 M_{1} M1​中的坐标，我们需要再加上t：
x M = R x N + t x_{M}=Rx_{N}+t xM​=RxN​+t

4 四元数

4.1 指数映射

假如 v = ϕ u v=\phi u v=ϕu表示旋转向量，定义指数映射：
q = E x p ( ϕ u ) = e ϕ u 2 = c o s ϕ 2 + u s i n ϕ 2 = [ c o s ϕ 2 u s i n ϕ 2 ] q=Exp(\phi u)=e^{\frac{\phi u}{2}}=cos\frac{\phi}{2}+usin{\phi}{2}=\left[\begin{matrix}cos{\frac{\phi}{2}} \\ usin{\frac{\phi}{2}} \end{matrix}\right] q=Exp(ϕu)=e2ϕu​=cos2ϕ​+usinϕ2=[cos2ϕ​usin2ϕ​​]
对于上述单位四元数q，定义线性变换r(.)如下：
r ( v ) = q ⨂ v ⨂ q ∗ r(v)=q\bigotimes v\bigotimes q^{*} r(v)=q⨂v⨂q∗
其中 v v v是向量 v v v对应的纯虚四元数 v = v 1 i + v 2 j + v 3 k v={v_1}i+{v_2}j+{v_3}k v=v1​i+v2​j+v3​k，可以证明r(.)满足正交群的定义。

而且可以验证在Hamilton四元数的定义下（ i j = k ij=k ij=k，右手）：
x ′ = q ⨂ x ⨂ q ∗ = x ∥ + x ⊥ c o s ϕ + ( u × x ) s i n ϕ x^{'}=q\bigotimes x \bigotimes q^{*}=x_{\parallel}+x_{\perp}cos\phi +(u\times x)sin \phi x′=q⨂x⨂q∗=x∥​+x⊥​cosϕ+(u×x)sinϕ
在JPL四元数定义下（ i j = − k ij=-k ij=−k，左手）：
x ′ = q ⨂ x ⨂ q ∗ = x ∥ + x ⊥ c o s ϕ − ( u × x ) s i n ϕ x^{'}=q\bigotimes x \bigotimes q^{*}=x_{\parallel}+x_{\perp}cos\phi -(u\times x)sin \phi x′=q⨂x⨂q∗=x∥​+x⊥​cosϕ−(u×x)sinϕ
这与前面轴角推出的结果一致，显然这种 r ( . ) r(.) r(.)也可以表示三维向量的旋转。

4.2 Hamilton与JPL

他们的数值之间的关系为：
q J P L = q L G , l e f t = q L G , r i g h t ∗ = q G L , r i g h t = q H a m i l t o n q_{JPL}=q_{LG,left}=q^{*}_{LG,right}=q_{GL,right}=q_{Hamilton} qJPL​=qLG,left​=qLG,right∗​=qGL,right​=qHamilton​

对于小角度的扰动 ϕ u \phi u ϕu，对于Hamiton:
q ≈ [ 1 1 2 ϕ u ] R = I + ϕ [ u ] × q \approx\left[\begin{matrix}1 \\ \frac{1}{2}\phi u\end{matrix}\right] \quad R=I+\phi [u]_{\times} q≈[121​ϕu​]R=I+ϕ[u]×​
对于JPL：
q ≈ [ 1 1 2 ϕ u ] R = I − ϕ [ u ] × q\approx \left[\begin{matrix}1 \\ \frac{1}{2}\phi u\end{matrix}\right] \quad R=I-\phi [u]_{\times} q≈[121​ϕu​]R=I−ϕ[u]×​

参考文献

（1）Quaternion kinematics for the error-state Kalman filter