说在前面
水水水
内积
a
⃗
⋅
b
⃗
=
∣
a
⃗
∣
∣
b
⃗
∣
cos
<
a
⃗
,
b
⃗
>
=
∑
i
=
1
3
a
i
b
i
\vec a \cdot \vec b=|\vec a||\vec b|\cos<\vec a, \vec b>=\sum_{i=1}^3a_ib_i
a⋅b=∣a∣∣b∣cos<a,b>=i=1∑3aibi
外积
a
⃗
×
b
⃗
=
[
i
j
k
a
1
a
2
a
3
b
1
b
2
b
3
]
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
3
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
⃗
\vec a \times \vec b= \left[ \begin{matrix} i & j & k \\ a_1 &a_2 &a_3 \\ b_1 &b_2&b_3 \\ \end{matrix} \right]= \left[ \begin{matrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_3 \\ \end{matrix} \right]= \left[ \begin{matrix} 0 & -a_3 & a_2 \\ a_3 &0 &-a_1 \\ -a_2 &a_1&0 \\ \end{matrix} \right]\vec b
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b3⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b
大小为
∣
a
⃗
∣
∣
b
⃗
∣
sin
<
a
⃗
,
b
⃗
>
|\vec a||\vec b|\sin<\vec a, \vec b>
∣a∣∣b∣sin<a,b>，方向垂直于这两向量
我们将矩阵：
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
\left[ \begin{matrix} 0 & -a_3 & a_2 \\ a_3 &0 &-a_1 \\ -a_2 &a_1&0 \\ \end{matrix} \right]
⎣⎡0a3−a2−a30a1a2−a10⎦⎤
记为a^，表示
a
⃗
\vec a
a的反对称矩阵。
我们可以用a^b表示向量a到向量b的旋转
旋转矩阵
假设在某个单位正交基
(
e
1
,
e
2
,
e
3
)
(e_1,e_2,e_3)
(e1,e2,e3)中向量P的坐标为
(
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′)中向量P的坐标为
(
a
1
′
,
a
2
′
,
a
3
′
)
(a_1',a_2',a_3')
(a1′,a2′,a3′)，由于向量P大小和方向均未发生改变，所以有：
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
[e_1,e_2,e_3] \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \\ \end{matrix} \right]= [e_1',e_2',e_3'] \left[ \begin{matrix} a_1' \\ a_2' \\ a_3' \\ \end{matrix} \right]
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
两边左乘：
[
e
1
T
e
2
T
e
3
T
]
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
T
e
2
T
e
3
T
]
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
\left[ \begin{matrix} e_1^T \\ e_2^T \\ e_3^T \\ \end{matrix} \right] [e_1,e_2,e_3] \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \\ \end{matrix} \right]= \left[ \begin{matrix} e_1^T \\ e_2^T \\ e_3^T \\ \end{matrix} \right] [e_1',e_2',e_3'] \left[ \begin{matrix} a_1' \\ a_2' \\ a_3' \\ \end{matrix} \right]
⎣⎡e1Te2Te3T⎦⎤[e1,e2,e3]⎣⎡a1a2a3⎦⎤=⎣⎡e1Te2Te3T⎦⎤[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
即：
[
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
′
]
\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \\ \end{matrix} \right]= \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} a_1' \\ a_2' \\ a_3' \\ \end{matrix} \right]
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤
中间矩阵即旋转矩阵
旋转向量
旋转矩阵表达具有冗余；我们可以使用一个旋转向量来表达：向量方向（
n
⃗
\vec n
n单位向量）与旋转轴保持一致，大小（
θ
\theta
θ）表示旋转的角度。
使用罗德里格斯公式：
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
⃗
n
⃗
T
+
sin
θ
n
^
R=\cos\theta I+(1-\cos\theta)\vec n\vec n^T+\sin\theta \hat n
R=cosθI+(1−cosθ)nnT+sinθn^
其中R为旋转矩阵，I为单位阵，
n
^
\hat n
n^为向量n的反对称矩阵
Note that many rotation vectors correspond to the same rotation. In particular, a rotation vector of length θ + 2πM, for any integer M, encodes exactly the same rotation as a rotation vector of length θ. Thus, there are at least a countable infinity of rotation vectors corresponding to any rotation. Furthermore, all rotations by 2πM are the same as no rotation at all, so, for a given integer M, all rotation vectors of length 2πM, in all directions, constitute a two-parameter uncountable infinity of rotation vectors encoding the same rotation as the zero vector. These facts must be taken into account when inverting the exponential map, that is, when finding a rotation vector that corresponds to a given rotation matrix. The exponential map is onto but not one-to-one.
欧拉角
使用三个分离的转角，例如先绕 X 轴旋转， 再绕 Y 轴，最后绕 Z 轴。常用的绕轴顺序为：
- 绕物体的 Z 轴旋转，得到偏航角 yaw；
- 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
- 绕旋转之后的 X 轴旋转，得到滚转角 roll。
- 缺陷：具有奇异性（原因：从旋转矩阵严格推导时产生了不唯一解）；万向锁问题，例如俯仰角为
±
9
0
∘
\pm90^\circ
±90∘时，第一次旋转与第三次旋转将使用同一个轴。因此，在SLAM中很少使用。
(绿色：pitch；蓝色：roll；紫色：yaw)
四元数
形式：
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q = q_0 + q_1i + q_2j + q_3k
q=q0+q1i+q2j+q3k
其中
i
,
j
,
k
i,j,k
i,j,k为三个虚部，并满足：
{
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
\left\{ \begin{array}{c} i^2=j^2=k^2=-1\\ ij=k,ji=-k \\ jk=i,kj=-i\\ ki=j,ik=-j \end{array} \right.
⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
另一种表达：
q
=
[
s
,
v
]
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
q=[s,\bold v],s=q_0\in R,\bold v=[q_1,q_2,q_3]^T\in R^3
q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3
其中，s 称为四元数的实部，而 v 称为它的虚部；
- 对应的旋转矩阵
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R= \left[ \begin{matrix} 1-2q_2^2-2q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3 &1-2q_1^2-2q_3^2 &2q_2q_3+2q_0q_1 \\ 2q_1q_3+2q_0q_2 &2q_2q_3-2q_0q_1&1-2q_1^2-2q_2^2 \\ \end{matrix} \right] R=⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤ - 若已知旋转矩阵，则
q 0 = t r ( R ) + 1 2 q 1 = m 23 − m 32 4 q 0 q 2 = m 13 − m 31 4 q 0 q 3 = m 12 − m 21 4 q 0 q_0=\frac{\sqrt{tr(R)+1}}{2}\\ q_1=\frac{m_{23}-m_{32}}{4q_0} \\ q_2=\frac{m_{13}-m_{31}}{4q_0}\\ q3=\frac{m_{12}-m_{21}}{4q_0} q0=2tr(R)+1q1=4q0m23−m32q2=4q0m13−m31q3=4q0m12−m21
5440
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
