从旋转矩阵中提取欧拉角

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 )

 

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

旋转矩阵提取欧拉角或轴角是一种常见的方法,用于表示物体的姿态信息。以下是两种常见的方法: 1. 从旋转矩阵提取欧拉角欧拉角是一种常用的姿态表示方法,可以通过旋转矩阵计算得到。通常采用的欧拉角顺序是“roll-pitch-yaw”,即先绕X轴旋转roll角,再绕Y轴旋转pitch角,最后绕Z轴旋转yaw角。以下是一个简单的Python代码示例: ```python import numpy as np # 假设旋转矩阵为R R = np.array([[0.707, -0.707, 0], [0.707, 0.707, 0], [0, 0, 1]]) # 计算欧拉角 yaw = np.arctan2(R[1, 0], R[0, 0]) pitch = np.arctan2(-R[2, 0], np.sqrt(R[2, 1]**2 + R[2, 2]**2)) roll = np.arctan2(R[2, 1], R[2, 2]) # 打印欧拉角 print("Yaw: ", yaw) print("Pitch: ", pitch) print("Roll: ", roll) ``` 这个代码片段通过旋转矩阵计算了物体的欧拉角,假设旋转矩阵为[[0.707, -0.707, 0], [0.707, 0.707, 0], [0, 0, 1]]。计算欧拉角的过程,需要使用arctan2函数,以避免由于分母为零而导致的错误。 2. 从旋转矩阵提取轴角。 轴角是一种简单的姿态表示方法,它由一个旋转轴和旋转角度组成。可以通过旋转矩阵计算得到。以下是一个简单的Python代码示例: ```python import numpy as np # 假设旋转矩阵为R R = np.array([[0.707, -0.707, 0], [0.707, 0.707, 0], [0, 0, 1]]) # 计算轴角 u, s, vh = np.linalg.svd(R - np.eye(3)) axis = u[:, 2] angle = np.arccos((np.trace(R) - 1) / 2) # 打印轴角 print("Axis: ", axis) print("Angle: ", angle) ``` 这个代码片段通过旋转矩阵计算了物体的轴角,假设旋转矩阵为[[0.707, -0.707, 0], [0.707, 0.707, 0], [0, 0, 1]]。其,使用了SVD分解来计算旋转轴,然后计算旋转角度。需要注意的是,这种方法可能会存在一些数值问题,需要进行一些额外的处理,以确保得到正确的轴角。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值