推荐阅读:DCM Tutorial – An Introduction to Orientation Kinematics 中文翻译:方向余弦矩阵(DCM)简介
以下内容来自上文,简单总结一下:
定义全局坐标系为OXYZ,本体坐标系为Oxyz,两者原点相同。全局坐标系中定义各轴(X、Y、Z)的单位向量分别为
I
、
J
、
K
I、J、K
I、J、K,同理本体坐标系中各轴对应的单位向量为
i
、
j
、
k
i、j、k
i、j、k。
全局坐标系下的单位向量
I
、
J
、
K
I、J、K
I、J、K表示为 本体坐标系下的单位向量
i
、
j
、
k
i、j、k
i、j、k表示为 其中G表示全局坐标系,B表示本体坐标系。具体的推导过程省略,这边可以简单表示为
i
x
G
i{_x^G}
ixG 表示的是i向量在全局坐标系中X轴上的投影,整个过程用点乘实现计算,注意仔细理解上式,我们以后会以
c
o
s
(
I
,
i
)
cos(I, i)
cos(I,i) 或者
I
.
i
I.i
I.i 的形式表示点乘。那么
i
i
i 向量在全局坐标系中的投影为 由此我们可以获得向量
i
、
j
、
k
i、j、k
i、j、k在全局坐标系G中的投影坐标,表示为方向余弦矩阵为 而同样的,在本体坐标系中表示全局坐标系的单位向量
I
、
J
、
K
I、J、K
I、J、K在本质上其实是对称的。举例来说
I
B
I^B
IB表示的就是
I
I
I向量在本体坐标系的投影
由此获得向量
I
、
J
、
K
I、J、K
I、J、K在本体坐标系中的投影(注意
I
.
i
I.i
I.i和
i
.
I
i.I
i.I等价)
可以发现
D
C
M
G
DCM^G
DCMG和
D
C
M
B
DCM^B
DCMB是各为彼此的转置,并且DCM本身为一个正交矩阵。
n
⃗
=
x
i
⃗
+
y
j
⃗
+
z
k
⃗
\vec{n}=x\vec{i}+y\vec{j}+z\vec{k}
n=xi+yj+zk
并且向量n为单位向量
∣
∣
n
⃗
∣
∣
=
1
||\vec{n}||=1
∣∣n∣∣=1
则用四元数描述这个旋转过程为
q
=
c
o
s
θ
2
+
(
x
+
y
+
z
)
s
i
n
θ
2
q=cos\frac{θ}{2}+(x+y+z)sin\frac{θ}{2}
q=cos2θ+(x+y+z)sin2θ
表示为
q
=
[
q
0
,
q
1
,
q
2
,
q
3
]
=
[
c
o
s
θ
2
,
x
s
i
n
θ
2
,
y
s
i
n
θ
2
,
z
s
i
n
θ
2
]
=
[
c
o
s
θ
2
,
n
s
i
n
θ
2
]
q=[q_0 ,q_1,q_2,q_3]=[cos\frac{θ}{2},xsin\frac{θ}{2},ysin\frac{θ}{2},zsin\frac{θ}{2}]=[cos\frac{θ}{2},nsin\frac{θ}{2}]
q=[q0,q1,q2,q3]=[cos2θ,xsin2θ,ysin2θ,zsin2θ]=[cos2θ,nsin2θ]
那么反过来也可以求得轴角
θ
=
2
∗
a
r
c
c
o
s
(
q
0
)
θ=2*arccos(q_0)
θ=2∗arccos(q0)
[
x
,
y
,
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
s
i
n
θ
2
[x,y,z]^T=\frac{[q_1,q_2,q_3]^T}{sin\frac{θ}{2}}
[x,y,z]T=sin2θ[q1,q2,q3]T
四元数恢复轴角形式
虽然上面给出了公式,然而一种更直观的计算方法如下,前提是已知四元数表达
下图来自旋转表达之四元数
6.1.2 四元数表示旋转
向量旋转
假设
q
q
q是一个用四元数表示的旋转过程,
v
1
∈
R
3
v_1\in R^3
v1∈R3表示一个向量,那么在
q
q
q的作用下,向量
v
1
v_1
v1旋转为向量
v
1
′
v_1'
v1′可表示为
(
0
v
1
′
)
=
q
×
(
0
v
1
)
×
q
−
1
\begin{pmatrix} 0 \\ v_1' \\ \end{pmatrix}=q×\begin{pmatrix} 0 \\ v_1 \\ \end{pmatrix}×q^{-1}
(0v1′)=q×(0v1)×q−1