四元数相关运算
四元数
q
=
[
q
x
q
y
q
z
q
w
]
T
=
[
q
v
q
w
]
T
q=\begin{bmatrix}q_x & q_y & q_z & q_w\end{bmatrix}^T = \begin{bmatrix}q_v & q_w\end{bmatrix}^T
q=[qxqyqzqw]T=[qvqw]T 则
p
⊗
q
=
[
p
w
−
p
z
p
y
p
x
p
z
p
w
−
p
x
p
y
−
p
y
p
x
p
w
p
z
−
p
x
−
p
y
−
p
z
p
w
]
[
q
x
q
y
q
z
q
w
]
=
[
p
w
q
x
−
p
z
q
y
+
p
y
q
z
+
p
x
q
w
p
z
q
x
+
p
w
q
y
−
p
x
q
z
+
p
y
q
w
−
p
y
q
x
+
p
x
q
y
+
p
w
q
z
+
p
z
q
w
−
p
x
q
x
−
p
y
q
y
−
p
z
q
z
+
p
w
q
w
]
=
[
p
w
q
w
−
p
v
T
q
v
p
w
q
v
+
q
w
p
v
+
p
v
×
q
v
]
p \otimes q = \begin{bmatrix} p_w & -p_z & p_y & p_x \\ p_z & p_w & -p_x & p_y \\ -p_y & p_x & p_w & p_z \\ -p_x & -p_y & -p_z & p_w \end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z \\ q_w\end{bmatrix} = \begin{bmatrix} p_wq_x - p_zq_y + p_yq_z + p_xq_w \\ p_zq_x + p_wq_y - p_xq_z + p_yq_w \\ -p_yq_x + p_xq_y + p_wq_z + p_zq_w \\ -p_xq_x - p_yq_y - p_zq_z + p_wq_w \end{bmatrix} = \begin{bmatrix} p_wq_w - p_v^Tq_v \\ p_wq_v + q_wp_v + p_v \times q_v \end{bmatrix}
p⊗q=⎣⎢⎢⎡pwpz−py−px−pzpwpx−pypy−pxpw−pzpxpypzpw⎦⎥⎥⎤⎣⎢⎢⎡qxqyqzqw⎦⎥⎥⎤=⎣⎢⎢⎡pwqx−pzqy+pyqz+pxqwpzqx+pwqy−pxqz+pyqw−pyqx+pxqy+pwqz+pzqw−pxqx−pyqy−pzqz+pwqw⎦⎥⎥⎤=[pwqw−pvTqvpwqv+qwpv+pv×qv]
p
v
×
q
v
=
[
p
x
p
y
p
z
]
×
[
q
x
q
y
q
z
]
=
[
0
−
p
x
p
y
p
x
0
−
p
z
−
p
y
p
z
0
]
[
q
x
q
y
q
z
]
=
[
−
p
x
q
y
+
p
y
q
z
p
x
q
x
−
p
z
q
z
−
p
y
q
x
+
p
z
q
y
]
p_v \times q_v = \begin{bmatrix}p_x \\ p_y \\ p_z\end{bmatrix} \times \begin{bmatrix} q_x \\ q_y \\ q_z \end{bmatrix} = \begin{bmatrix}0 & -p_x & p_y \\ p_x & 0 & -p_z \\ -p_y & p_z & 0\end{bmatrix}\begin{bmatrix}q_x \\ q_y \\ q_z\end{bmatrix} = \begin{bmatrix} -p_xq_y + p_yq_z \\ p_xq_x - p_zq_z \\ -p_yq_x + p_zq_y \end{bmatrix}
pv×qv=⎣⎡pxpypz⎦⎤×⎣⎡qxqyqz⎦⎤=⎣⎡0px−py−px0pzpy−pz0⎦⎤⎣⎡qxqyqz⎦⎤=⎣⎡−pxqy+pyqzpxqx−pzqz−pyqx+pzqy⎦⎤
结合律:
(
p
⊗
q
)
⊗
r
=
p
⊗
(
q
⊗
r
)
\left(p \otimes q\right) \otimes r = p \otimes \left(q \otimes r\right)
(p⊗q)⊗r=p⊗(q⊗r)
分配律:
p
⊗
(
q
+
r
)
=
p
⊗
q
+
p
⊗
r
p \otimes \left(q + r\right) = p \otimes q + p \otimes r
p⊗(q+r)=p⊗q+p⊗r
(
p
+
q
)
⊗
r
=
p
⊗
r
+
q
⊗
r
\left(p +q\right) \otimes r = p \otimes r + q \otimes r
(p+q)⊗r=p⊗r+q⊗r
左乘右乘:
q
1
⊗
q
2
=
[
q
1
]
L
q
2
q_1 \otimes q_2 = \left[q_1\right]_Lq_2
q1⊗q2=[q1]Lq2
q
1
⊗
q
2
=
[
q
2
]
R
q
1
q_1 \otimes q_2 = \left[q_2\right]_Rq_1
q1⊗q2=[q2]Rq1
[
q
]
L
=
[
q
w
−
q
z
q
y
q
x
q
z
q
w
−
q
x
q
y
−
q
y
q
x
q
w
q
z
−
q
x
−
q
y
−
q
z
q
w
]
=
q
w
I
+
[
[
q
v
]
×
q
v
−
q
v
T
0
]
\left[q\right]_L = \begin{bmatrix} q_w & -q_z & q_y & q_x \\ q_z & q_w & -q_x & q_y \\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix} \left[q_v\right]_{\times} & q_v \\ \\ -q_v^{T} & 0 \end{bmatrix}
[q]L=⎣⎢⎢⎡qwqz−qy−qx−qzqwqx−qyqy−qxqw−qzqxqyqzqw⎦⎥⎥⎤=qwI+⎣⎡[qv]×−qvTqv0⎦⎤
[
q
]
R
=
[
q
w
q
z
−
q
y
q
x
−
q
z
q
w
q
x
q
y
q
y
−
q
x
q
w
q
z
−
q
x
−
q
y
−
q
z
q
w
]
=
q
w
I
+
[
−
[
q
v
]
×
q
v
−
q
v
T
0
]
\left[q\right]_R = \begin{bmatrix} q_w & q_z & -q_y & q_x \\ -q_z & q_w & q_x & q_y \\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix} = q_wI + \begin{bmatrix}-\left[q_v\right]_{\times} & q_v \\ \\ -q_v^T & 0\end{bmatrix}
[q]R=⎣⎢⎢⎡qw−qzqy−qxqzqw−qx−qy−qyqxqw−qzqxqyqzqw⎦⎥⎥⎤=qwI+⎣⎡−[qv]×−qvTqv0⎦⎤
[
a
]
×
≜
[
0
−
a
z
a
y
a
z
0
−
a
x
−
a
y
a
x
0
]
\left[a\right]_{\times} \triangleq \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}
[a]×≜⎣⎡0az−ay−az0axay−ax0⎦⎤
[
a
]
×
T
=
−
[
a
]
×
\left[a\right]_{\times}^T = -\left[a\right]_{\times}
[a]×T=−[a]×
[
a
]
×
b
=
a
×
b
,
∀
a
,
b
∈
R
\left[a\right]_{\times}b = a \times b, \ \ \ \ \forall a, b \in \mathbb{R}
[a]×b=a×b, ∀a,b∈R
Finally, since
(
q
⊗
x
)
⊗
p
=
[
p
]
R
[
q
]
L
x
\left(q \otimes x\right) \otimes p = \left[p\right]_R\left[q\right]_L x
(q⊗x)⊗p=[p]R[q]Lx
q
⊗
(
x
⊗
p
)
=
[
q
]
L
[
p
]
R
x
q\otimes\left(x\otimes p\right) = \left[q\right]_L\left[p\right]_Rx
q⊗(x⊗p)=[q]L[p]Rx
[
p
]
R
[
q
]
L
=
[
q
]
L
[
p
]
R
\left[p\right]_R\left[q\right]_L = \left[q\right]_L\left[p\right]_R
[p]R[q]L=[q]L[p]R
四元数的逆:
q
−
1
=
q
∗
q
⊗
q
∗
=
1
q^{-1} = q^{*} \ \ \ \ \ \ \ q \otimes q^* = 1
q−1=q∗ q⊗q∗=1
四元数的导数:
q
˙
=
1
2
q
⊗
ω
\dot{q} = \frac{1}{2}q \otimes \omega
q˙=21q⊗ω
指数映射:
q
=
exp
(
u
ϕ
/
2
)
q = \exp\left(u \phi / 2\right)
q=exp(uϕ/2)
对数映射:
log
(
q
)
=
u
ϕ
/
2
\log\left(q\right) = u \phi / 2
log(q)=uϕ/2
两个四元数运算:
q
1
⊗
q
2
q_1 \otimes q_2
q1⊗q2
旋转操作:
q
=
cos
(
ϕ
/
2
)
+
u
sin
(
ϕ
/
2
)
q = \cos \left(\phi / 2\right) + u \sin \left(\phi / 2\right)
q=cos(ϕ/2)+usin(ϕ/2)
旋转作用:
q
⊗
x
⊗
q
∗
q \otimes x \otimes q^*
q⊗x⊗q∗
旋转矩阵相关运算
R
x
=
[
1
0
0
0
cos
(
θ
x
)
−
sin
(
θ
x
)
0
sin
(
θ
x
)
cos
(
θ
x
)
]
R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) \\ 0 & \sin\left(\theta_x\right) & \cos\left(\theta_x\right) \end{bmatrix}
Rx=⎣⎡1000cos(θx)sin(θx)0−sin(θx)cos(θx)⎦⎤
R
y
=
[
cos
(
θ
x
)
0
sin
(
θ
x
)
0
1
0
−
sin
(
θ
x
)
0
cos
(
θ
x
)
]
R_y = \begin{bmatrix} \cos\left(\theta_x\right) & 0 & \sin\left(\theta_x\right) \\ 0 & 1 & 0 \\ -\sin\left(\theta_x\right) & 0 & \cos\left(\theta_x\right) \end{bmatrix}
Ry=⎣⎡cos(θx)0−sin(θx)010sin(θx)0cos(θx)⎦⎤
R
z
=
[
cos
(
θ
x
)
−
sin
(
θ
x
)
0
sin
(
θ
x
)
cos
(
θ
x
)
0
0
0
1
]
R_z = \begin{bmatrix} \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) & 0 \\ \sin\left(\theta_x\right) & \cos\left(\theta_x\right) & 0 \\ 0 & 0 & 1 \end{bmatrix}
Rz=⎣⎡cos(θx)sin(θx)0−sin(θx)cos(θx)0001⎦⎤
旋转矩阵的逆:
R
−
1
=
R
T
R
R
−
1
=
R
R
T
=
I
d
e
t
(
R
)
=
1
R^{-1} = R^T \ \ \ \ \ RR^{-1} = RR^T = I \ \ \ \ det\left(R\right) = 1
R−1=RT RR−1=RRT=I det(R)=1
旋转矩阵的导数:
R
˙
=
R
[
ω
]
×
\dot{R} = R\left[\omega\right]_{\times}
R˙=R[ω]×
指数映射:
R
=
exp
(
[
u
ϕ
]
×
)
R = \exp\left(\left[u \phi\right]_{\times}\right)
R=exp([uϕ]×)
对数映射:
log
(
R
)
=
[
u
ϕ
]
×
\log\left(R\right) = \left[u \phi\right]_{\times}
log(R)=[uϕ]×
两个旋转矩阵运算:
R
1
R
2
R_1 R_2
R1R2
旋转操作:
R
=
I
+
sin
(
ϕ
[
u
]
×
)
+
(
1
−
cos
(
ϕ
)
)
[
u
]
×
2
R = I + \sin\left(\phi \left[u\right]_{\times}\right) + \left(1 - \cos\left(\phi\right)\right)\left[u\right]_{\times}^2
R=I+sin(ϕ[u]×)+(1−cos(ϕ))[u]×2
旋转作用:
R
x
Rx
Rx