从旋转矩阵中提取欧拉角

https://www.gregslabaugh.net/publications/euler.pdf

\theta_1\theta_2\theta_3分别是x、y,z轴。每一个旋转被应用于一个世界轴,而不是一个身体轴(这说的是个啥),R_x\left ( \theta_1 \right )R_y\left ( \theta_2 \right )R_z\left ( \theta_3 \right )=\begin{pmatrix} 1 & 0 & 0\\ 0 & c_1& s_1\\ 0 & -s_1& c_1 \end{pmatrix}\begin{pmatrix} c_2 & 0 & -s_2\\ 0 & 1& 0\\ s_2 & 0& c_2 \end{pmatrix}\begin{pmatrix} c_3 & s_3 & 0\\ -s_3 & c_3& 0\\ 0 & 0& 1 \end{pmatrix}=\begin{pmatrix} c_2c_3 & c_2s_3 & -s_2\\ s_1s_2c_3-c_1s_3 & s_1s_2s_3+c_1c_3& s_1c_2\\ c_1s_2c_3+s_1s_3 & c_1s_2s_3-s_1c_3& c_1c_2 \end{pmatrix}

这里面c_1 = cos \theta_1s_1 = sin \theta_1

假设我们有一个矩阵M=\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10}& m_{11} & m_{12}\\ m_{20}& m_{21} & m_{22} \end{pmatrix}我们要从旋转序列中提取旋转角度\theta_1\theta_2\theta_3

Shoemake的代码直接求解

\theta_1 = atan2\left ( m_{12},m_{22} \right ) =atan2\left ( s_1c_2,c_1c_2 \right )

c_2 = \sqrt{m_{00}^2 + m_{01}^2} =\sqrt{c_2^2c_3^2+c_2^2s_3^2}

\theta_2 = atan2\left ( -m_{02},c_{2} \right )

\theta_3 = atan2\left ( m_{01},m_{00} \right )=atan2\left ( c_2s_3,c_2c_3 \right )

如果m_{00},m_{01}非常小甚至是0,那问题就来了,这样m_{12},m_{22}也会变得很小,也许也是0(m_{02}不就趋于\pm 1),那\theta_1不就不好提取了么。

Shoemake根据一个很小的阈值来测试计算的c_2值——如果它低于这个阈值,那么矩阵元素大约减少到以下形式:

\begin{pmatrix} 0 & 0& -\left ( \pm 1 \right ) \\ \pm s_1c_3 - c_1s_3 &\pms_1s_3+c_1c_3 &0 \\ \pm c_1c_3+s_1s_3 & \pm c_1c_3-s_1s_3 & 0 \end{pmatrix}

在这种情况下,角度\theta_1\theta_3被一个不同的编码路径提取。这个矩阵提供了一个gimbal lock现象的例子,在这个例子中,第1和第3轴通过第2轴旋转对齐,有效地失去了一个自由度,因为现在\theta_1\theta_3的组合作为一个单一参数。Shoemake通过将其强制\theta_3为0来处理这种情况-这是可以的,因为gimbal lock允许我们任意地设置一个\theta_1和一个\theta_3然后推出另一个。这进一步将矩阵简化为以下形式:

\begin{pmatrix} 0 & 0& -\left ( \pm 1 \right ) \\ \pm s_1 &c_1 &0 \\ \pm c_1 & \pm -s_1 & 0 \end{pmatrix}

\theta_1 = atan2\left ( -m_{21},m_{11} \right )

{M}' =\left ( \begin{pmatrix} 1 & 0& 0\\ 0 & c_1& s_1\\ 0 & -s_1& c_1 \end{pmatrix}\begin{pmatrix} c_2 & 0& -s_2\\ 0 & 1& 0\\ s_1 & 0& c_2 \end{pmatrix}\right )^T\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10} & m_{11} & m_{12}\\ m_{20} & m_{21} & m_{22} \end{pmatrix}=\begin{pmatrix} c_2 & s_1s_2 & c_1s_2\\ 0 & c_1 & -s_1\\ -s_2 & s_1c_2 & c_1c_2 \end{pmatrix}\begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10} & m_{11} & m_{12}\\ m_{20} & m_{21} & m_{22} \end{pmatrix}

如果此乘积表示绕z轴的纯旋转,则其形式必须为:

{M}'=\begin{pmatrix} c_3 & s_3 & 0\\ -s_3 & c_3 & 0\\ 0 & 0 & 1 \end{pmatrix}

\theta_3提取应该是直接的。在实践中,我们甚至不需要计算一个完整的矩阵乘积。从{M}'中间一行读取,我们得到

就是把矩阵乘对应起来

c_1m_{10} - s_1m_{20} = -s_3c_1m_{11} - s_1m_{21} = c_3

\theta_3 = atan2\left ( s_1m_{20}-c_1m_{10},c_1m_{11}-s_1m_{21} \right )

最后:

\theta_1 = atan2\left ( m_{12},m_{22} \right )

c_2=\sqrt{m_{00}^2+m_{01}^2}

\theta_2 = atan2\left ( -m_{02},c_{2} \right )

s_1 =sin\left ( \theta_1 \right ),c_1 =cos\left ( \theta_1 \right )

\theta_3 = atan2\left ( s_1m_{20}-c_1m_{10},c_1m_{11}-s_1m_{21} \right )

 

注意啊这里用的是行向量,所以和正常的关系是转置关系

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值