文章目录
坐标系约定与外积的关系
如果统一使用右手坐标系,那么外积的方向要根据右手法则来判断;如果使用左手坐标系,就要使用左手法则来判断。
但是无论哪一种方式,外积的计算都可以写成以下代数形式。给定三维向量
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
]
[
b
x
b
y
b
z
]
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] \left[\begin{matrix} b_x \\ b_y\\ b_z \end{matrix}\right]
a×b=[a]×b=⎣⎡0az−ay−az0axay−ax0⎦⎤⎣⎡bxbybz⎦⎤
本篇文章默认使用右手坐标系。
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⊥′=e1cosϕ+e2sinϕ
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<r(v),r(v)\right>}=\sqrt{\left<v,v\right>}=||v|| ∣∣r(v)∣∣=⟨r(v),r(v)⟩=⟨v,v⟩=∣∣v∣∣ - 保持向量之间的夹角不变。(这里指余弦值不变)
< r ( v ) , r ( w ) > = < v , w > = ∣ ∣ v ∣ ∣ ∣ ∣ w ∣ ∣ c o s α \left<r(v),r(w)\right>=\left<v,w\right>=||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<r(v),r(v)\right>=(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<r(v),r(w)\right>=v^TR^TRw=v^Tw=\left<v,w\right> ⟨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=v1i+v2j+v3k,可以证明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