四元数转旋转矩阵公式推导

容易知以下公式成立：

∂Rp∂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=[q0,qv]q=\left[q_{0}, q_{v}\right]q=[q0​,qv​]，q0q_{0}q0​为实部，qvq_{v}qv​为虚部，qv=[q1,q2,q3]q_{v}=\left[q_{1}, q_{2}, q_{3}\right]qv​=[q1​,q2​,q3​]，q∗q^{*}q∗是 qqq的共轭四元数。

q⊗p⊗q∗=⌊q⌋L⌊q∗∣Rpq \otimes p \otimes q^{*}=\lfloor q\rfloor_{L}\left\lfloor\left. q^{*}\right|_{R} p\right.q⊗p⊗q∗=⌊q⌋L​⌊q∗∣R​p

所以：

∂(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=[q0−qvTqvqωI3×3+[qv]×][q0qvT−qvq0I3×3+[qv]×]=[1q0qvT−qvTq0−qvT[qv]×qvq0−q0qv−[qv]×qv(qvqvT+q02I3×3+2q0[qv]×+[qv]×[qv]×]\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​​=[q0​qv​​−qvT​qω​I3×3​+[qv​]×​​][q0​−qv​​qvT​q0​I3×3​+[qv​]×​​]=[1qv​q0​−q0​qv​−[qv​]×​qv​​q0​qvT​−qvT​q0​−qvT​[qv​]×​(qv​qvT​+q02​I3×3​+2q0​[qv​]×​+[qv​]×​[qv​]×​]​​

容易推出:

q0qvT−qvTq0−qvT[qv]×=01×3qvq0−q0qv−[qv]×qv=03×1qvqvT+[qv]×[qv]×=−∥qv∥I3×3+2qvqvT\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}q0​qvT​−qvT​q0​−qvT​[qv​]×​qv​q0​−q0​qv​−[qv​]×​qv​qv​qvT​+[qv​]×​[qv​]×​​=01×3​=03×1​=−∥qv​∥I3×3​+2qv​qvT​​

所以可以化简为：

⌊q⌋L⌊q∗⌋R=[100(q02−∥qv∥)I3×3+2qvqvT+2q0[qv]×]\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​=[10​0(q02​−∥qv​∥)I3×3​+2qv​qvT​+2q0​[qv​]×​​]

所以：

R=(q02−∥qv∥)I3×3+2qvqvT+2q0[qv]×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​+2qv​qvT​+2q0​[qv​]×​

上述公式推导的结果和matlab推导的结果是一样的，主要代码如下：