1 左手坐标系下四元数转换为旋转矩阵

1.1 转换思路

给定一个用于旋转的单位四元数q=w+xi+yj+zk和被旋转的三维向量v,那么首选需要构造一个纯四元数:

p=(v,0)

设旋转后的向量为v',那么旋转之后的向量构造的纯四元数为

p'=(v',0)

那么,

p'=qpq^{-1}

1.2 转换过程和结果

假设有单位四元数q=w+xi+yj+zk,其中x^2+y^2+z^2=1,那么我们可以通过该四元数构造一个旋转矩阵,设旋转矩阵的形式如下

R(q)=\begin{bmatrix}m_{11}&m_{12}&m_{13}\\m_{21}&m_{22}&m_{23}\\m_{31}&m_{32}&m_{33}\end{bmatrix}

若四元数为行主序的旋转矩阵,则该旋转矩阵为:

R(q)=\begin{bmatrix}1-2(y^2+z^2)&2(xy-zw)&2(xz+yw)\\2(xy+zw)&1-2(x^2+z^2)&2(yz-xw)\\2(xz-yw)&2(yz+xw)&1-2(x^2+y^2)\end{bmatrix}

若四元数为列主序的旋转矩阵,则该旋转矩阵为:

R(q)=\begin{bmatrix}1-2(y^2+z^2)&2(xy+zw)&2(xz-yw)\\2(xy-zw)&1-2(x^2+z^2)&2(yz+xw)\\2(xz+yw)&2(yz-xw)&1-2(x^2+y^2)\end{bmatrix}

2 旋转矩阵转换为四元数

2.1 转换前提

旋转矩阵为正交矩阵即旋转矩阵满足RR^T=R^TR=I,其中I为单位矩阵。

2.2 转换思路

2.2.1 行主序的旋转矩阵转换

这里上述行主序的旋转矩阵为示例进行说明:

R(q)=\begin{bmatrix}1-2(y^2+z^2)&2(xy-zw)&2(xz+yw)\\2(xy+zw)&1-2(x^2+z^2)&2(yz-xw)\\2(xz-yw)&2(yz+xw)&1-2(x^2+y^2)\end{bmatrix}

观察下列式子:

m_{32}-m_{23}=2(yz+xw)-2(yz-xw)=4xw\\m_{13}-m_{31}=2(xz+yw)-2(xz-yw)=4yw\\m_{21}-m_{12}=2(xy+zw)-2(xy-zw)=4zw

我们可以观察到经过上述拼凑的等式,我们可以发现,如果我们可以再使用一个表达式表示分量w,那么x,y,z分量就可以被表示,我们观察下个式子:

tr(R(q))=m_{11}+m_{22}+m_{32}\\=3-4(x^2+y^2+z^2)\\=4(1-(x^2+y^2+z^2))-1\\=4w^2-1

其中tr(R(q))是矩阵的迹。
trace为旋转矩阵对角线元素的和,现在w分量可以用矩阵中的元素表示,那么x,y,z分量也可以用w进行表示:

w=\frac{\sqrt{(tr(R))+1}}2\\
x=\frac{m_{32}-m_{23}}{4w}\\
y=\frac{m_{13}-m_{31}}{4w}\\
z=\frac{m_{21}-m_{12}}{4w}

2.2.2 列主序的旋转矩阵转换

R(q)=\begin{bmatrix}1-2(y^2+z^2)&2(xy+zw)&2(xz-yw)\\2(xy-zw)&1-2(x^2+z^2)&2(yz+xw)\\2(xz+yw)&2(yz-xw)&1-2(x^2+y^2)\end{bmatrix}

拼凑式子:

m_{23}-m_{32}=2(yz+xw)-2(yz-xw)=4xw\\m_{31}-m_{13}=2(xz+yw)-2(xz-yw)=4yw\\m_{12}-m_{21}=2(xy+zw)-2(xy-zw)=4zw

则:

tr(R(q))=m_{11}+m_{22}+m_{32}\\=3-4(x^2+y^2+z^2)\\=4(1-(x^2+y^2+z^2))-1\\=4w^2-1

其中tr(R(q))是矩阵的迹。
trace为旋转矩阵对角线元素的和,现在w分量可以用矩阵中的元素表示,那么x,y,z分量也可以用w进行表示:

w=\frac{\sqrt{(tr(R))+1}}2\\
x=\frac{m_{23}-m_{32}}{4w}\\
y=\frac{m_{31}-m_{13}}{4w}\\
z=\frac{m_{12}-m_{21}}{4w}

当前分类随机文章推荐

全站随机文章推荐

关于本站站长 StubbornHuang
旋转矩阵与四元数的转换-StubbornHuang Blog纵使晴明无雨色,入云深处亦沾衣。