容易知以下公式成立:
∂
R
p
∂
p
=
R
\frac{\partial R p}{\partial p}=R
∂p∂Rp=R
上面公式用四元数表示如下:
∂
(
q
⊗
p
⊗
q
∗
)
∂
p
=
R
\frac{\partial\left(q \otimes p \otimes q^{*}\right)}{\partial p}=R
∂p∂(q⊗p⊗q∗)=R
由于
∂
(
q
⊗
p
⊗
q
∗
)
∂
p
\frac{\partial\left(q \otimes p \otimes q^{*}\right)}{\partial p}
∂p∂(q⊗p⊗q∗)是用四元数的形式表示的,所以即可完成四元数到旋转矩阵的转换公式推导,详细推导如下:
设
q
=
[
q
0
,
q
v
]
q=\left[q_{0}, q_{v}\right]
q=[q0,qv],
q
0
q_{0}
q0为实部,
q
v
q_{v}
qv为虚部,
q
v
=
[
q
1
,
q
2
,
q
3
]
q_{v}=\left[q_{1}, q_{2}, q_{3}\right]
qv=[q1,q2,q3],
q
∗
q^{*}
q∗是
q
q
q的共轭四元数。
q
⊗
p
⊗
q
∗
=
⌊
q
⌋
L
⌊
q
∗
∣
R
p
q \otimes p \otimes q^{*}=\lfloor q\rfloor_{L}\left\lfloor\left. q^{*}\right|_{R} p\right.
q⊗p⊗q∗=⌊q⌋L⌊q∗∣Rp
所以:
∂
(
q
⊗
p
⊗
q
∗
)
∂
p
=
⌊
q
⌋
L
⌊
q
∗
⌋
R
\frac{\partial\left(q \otimes p \otimes q^{*}\right)}{\partial p}=\lfloor q\rfloor_{L}\left\lfloor q^{*}\right\rfloor_{R}
∂p∂(q⊗p⊗q∗)=⌊q⌋L⌊q∗⌋R
⌊
q
⌋
L
⌊
q
∗
⌋
R
=
[
q
0
−
q
v
T
q
v
q
ω
I
3
×
3
+
[
q
v
]
×
]
[
q
0
q
v
T
−
q
v
q
0
I
3
×
3
+
[
q
v
]
×
]
=
[
1
q
0
q
v
T
−
q
v
T
q
0
−
q
v
T
[
q
v
]
×
q
v
q
0
−
q
0
q
v
−
[
q
v
]
×
q
v
(
q
v
q
v
T
+
q
0
2
I
3
×
3
+
2
q
0
[
q
v
]
×
+
[
q
v
]
×
[
q
v
]
×
]
\begin{aligned} \lfloor q\rfloor_{L}\left\lfloor q^{*}\right\rfloor_{R} &=\left[\begin{array}{cc} q_{0} & -q_{v}^{T} \\ q_{v} & q_{\omega} I_{3 \times 3}+\left[q_{v}\right]_{\times} \end{array}\right]\left[\begin{array}{cc} q_{0} & q_{v}^{T} \\ -q_{v} & q_{0} I_{3 \times 3}+\left[q_{v}\right]_{\times} \end{array}\right] \\ &=\left[\begin{array}{cc} 1 & q_{0} q_{v}^{T}-q_{v}^{T} q_{0}-q_{v}^{T}\left[q_{v}\right]_{\times} \\ q_{v} q_{0}-q_{0} q_{v}-\left[q_{v}\right]_{\times} q_{v} & \left(q_{v} q_{v}^{T}+q_{0}^{2} I_{3 \times 3}+2 q_{0}\left[q_{v}\right]_{\times}+\left[q_{v}\right]_{\times}\left[q_{v}\right]_{\times}\right] \end{array}\right. \end{aligned}
⌊q⌋L⌊q∗⌋R=[q0qv−qvTqωI3×3+[qv]×][q0−qvqvTq0I3×3+[qv]×]=[1qvq0−q0qv−[qv]×qvq0qvT−qvTq0−qvT[qv]×(qvqvT+q02I3×3+2q0[qv]×+[qv]×[qv]×]
容易推出:
q
0
q
v
T
−
q
v
T
q
0
−
q
v
T
[
q
v
]
×
=
0
1
×
3
q
v
q
0
−
q
0
q
v
−
[
q
v
]
×
q
v
=
0
3
×
1
q
v
q
v
T
+
[
q
v
]
×
[
q
v
]
×
=
−
∥
q
v
∥
I
3
×
3
+
2
q
v
q
v
T
\begin{aligned} q_{0} q_{v}^{T}-q_{v}^{T} q_{0}-q_{v}^{T}\left[q_{v}\right]_{\times} &=0_{1 \times 3} \\ q_{v} q_{0}-q_{0} q_{v}-\left[q_{v}\right]_{\times} q_{v} &=0_{3 \times 1} \\ q_{v} q_{v}^{T}+\left[q_{v}\right]_{\times}\left[q_{v}\right]_{\times} &=-\left\|q_{v}\right\| I_{3 \times 3}+2 q_{v} q_{v}^{T} \end{aligned}
q0qvT−qvTq0−qvT[qv]×qvq0−q0qv−[qv]×qvqvqvT+[qv]×[qv]×=01×3=03×1=−∥qv∥I3×3+2qvqvT
所以可以化简为:
⌊
q
⌋
L
⌊
q
∗
⌋
R
=
[
1
0
0
(
q
0
2
−
∥
q
v
∥
)
I
3
×
3
+
2
q
v
q
v
T
+
2
q
0
[
q
v
]
×
]
\lfloor q\rfloor_{L}\left\lfloor q^{*}\right\rfloor_{R}=\left[\begin{array}{cc} 1 & 0 \\ 0 & \left(q_{0}^{2}-\left\|q_{v}\right\|\right) I_{3 \times 3}+2 q_{v} q_{v}^{T}+2 q_{0}\left[q_{v}\right]_{\times} \end{array}\right]
⌊q⌋L⌊q∗⌋R=[100(q02−∥qv∥)I3×3+2qvqvT+2q0[qv]×]
所以:
R
=
(
q
0
2
−
∥
q
v
∥
)
I
3
×
3
+
2
q
v
q
v
T
+
2
q
0
[
q
v
]
×
R=\left(q_{0}^{2}-\left\|q_{v}\right\|\right) I_{3 \times 3}+2 q_{v} q_{v}^{T}+2 q_{0}\left[q_{v}\right]_{\times}
R=(q02−∥qv∥)I3×3+2qvqvT+2q0[qv]×
上述公式推导的结果和matlab推导的结果是一样的,主要代码如下: