旋转矩阵求解欧拉角Python实现
笔者在外实习过程中，发现公司使用的不同的工业机器人默认欧拉角顺规有所不同，应部门组长要求，写一个输入3*3旋转矩阵，输出12种不同的动系欧拉角与12种不同的静系欧拉角的程序进行验证。
在此记录，希望能够帮助遇到相同问题的同学~
基本知识
绕静系与动系旋转
绕静系旋转：每次旋转，相对第一次旋转之前的初始的坐标系进行旋转；
绕动系旋转：每次旋转，相对上一次旋转之后的得到的坐标系进行旋转。
静系动系与左乘右乘
绕静系进行旋转，新的旋转对应的矩阵乘在旧的旋转矩阵的左边；
绕动系进行旋转，新的旋转对应的矩阵乘在旧的旋转矩阵的右边。
旋转矩阵连乘的两种理解
给定一定的姿态变换，其可由一系列的旋转得到，对应一系列的旋转矩阵连乘。
在得到旋转矩阵连乘公式，例如：Rx(α) * Ry(β) * Rz(γ)，
其有以下两种理解方式：
1.每次绕静系旋转
依次绕 Z 转 γ，绕 Y 转 β，绕 X 转 α。
2.每次绕动系旋转
依次绕 X 转 α，绕 Y 转 β，绕 Z 转 γ。
两种旋转得到的结果等效，即静动转变，顺序相反，角度不变。
文件目录
Root
|__ r2euler_self.py
|__ r2euler_sci.py
|__ RotationMatrix.txt
RotationMatrix.txt文件中保存了一个旋转矩阵
0.8137977 -0.3839980 0.4362096
0.4698463 0.8764841 -0.1049763
-0.3420202 0.2903810 0.8937008
其对应的ZYX动系欧拉角 = xyz静系欧拉角 = pi/6, pi/9, pi/10
实现方式
使用矩阵方程手动求解
笔者的计算方法是：根据动系欧拉角的12种不同顺规对应的旋转矩阵建立矩阵方程，反解动系欧拉角。
其中，考虑到了第二次旋转角度的特殊值的情况，有以下求解12种不同的动系欧拉角的求解程序。
另外，静系欧拉角，根据其与动系欧拉角的对应关系进行求解。
"""
FileName: r2euler_self.py
File to caluculate euler from rotation matrix
Method of using basic matrix equations
Input str in upper -- intrinsic
Input str in lower -- extrinsic
"""
import math
import numpy as np
from scipy.spatial.transform import Rotation as R
def r2euler(R, type):
R = np.array(R)
type = str(type).upper()
err = float(0.001)
if np.shape(R) != (3,3):
print("The size of R matrix is wrong")
sys.exit(0)
else:
pass
if type == "XYZ":
# R[0,2]/sqrt((R[1,2])**2 + (R[2,2])**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(R[0,2], math.sqrt((R[1,2])**2 + (R[2,2])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(R[1,0], R[1,1])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
beta = -math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[1,0], R[1,1])
else:
alpha = math.atan2(-(R[1,2])/(math.cos(beta)),(R[2,2])/(math.cos(beta)))
gamma = math.atan2(-(R[0,1])/(math.cos(beta)), (R[0,0])/(math.cos(beta)))
elif type == "XZY":
# -R[0,1]/sqrt((R[1,1])**2 + (R[2,1])**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(-R[0,1], math.sqrt((R[1,1])**2 + (R[2,1])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[1,2], R[1,0])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[1,2], -R[1,0])
else:
alpha = math.atan2((R[2,1])/(math.cos(beta)), (R[1,1])/(math.cos(beta)))
gamma = math.atan2((R[0,2])/(math.cos(beta)), (R[0,0])/(math.cos(beta)))
elif type == "YXZ":
# -R[1,2]/sqrt(R[0,2]**2 + R[2,2]**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(-R[1,2], math.sqrt((R[0,2])**2 + (R[2,2])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], R[0,0])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
beta = -math.pi/2
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], R[0,0])
else:
alpha = math.atan2((R[0,2])/(math.cos(beta)), (R[2,2])/(math.cos(beta)))
gamma = math.atan2((R[1,0])/(math.cos(beta)), (R[1,1])/(math.cos(beta)))
elif type == "YZX":
# R[1,0]/sqrt(R[0,0]**2 + R[2,0]**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(R[1,0], math.sqrt((R[0,0])**2 + (R[2,0])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], -R[0,1])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
beta = -math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,2], R[0,1])
else:
alpha = math.atan2(-(R[2,0])/(math.cos(beta)), (R[0,0])/(math.cos(beta)))
gamma = math.atan2(-(R[1,2])/(math.cos(beta)), (R[1,1])/(math.cos(beta)))
elif type == "ZXY":
# R[2,1]/sqrt(R[0,1]**2 + R[1,1]**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(R[2,1], math.sqrt((R[0,1])**2 + (R[1,1])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], R[0,0])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
beta = -math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], R[0,0])
else:
alpha = math.atan2(-(R[0,1])/(math.cos(beta)), (R[1,1])/(math.cos(beta)))
gamma = math.atan2(-(R[2,0])/(math.cos(beta)), (R[2,2])/(math.cos(beta)))
elif type == "ZYX":
# -R[2,0]/sqrt(R[0,0]**2 + R[1,0]**2) == sin(beta)/|cos(beta)|
# ==> beta (-pi/2, pi/2)
beta = math.atan2(-R[2,0], math.sqrt((R[0,0])**2 + (R[1,0])**2))
if beta >= math.pi/2-err and beta <= math.pi/2+err:
beta = math.pi/2
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,1], R[1,2])
elif beta >= -(math.pi/2)-err and beta <= -(math.pi/2)+err:
beta = -math.pi/2
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], -R[1,2])
else:
alpha = math.atan2((R[1,0])/(math.cos(beta)), (R[0,0])/(math.cos(beta)))
gamma = math.atan2((R[2,1])/(math.cos(beta)), (R[2,2])/(math.cos(beta)))
elif type == "XYX":
# sqrt(R[0,1]**2 + R[0,2]**2)/R[0,0] == |sin(beta)|/cos(beta)
# ==> beta (0, pi)
beta = math.atan2(math.sqrt((R[0,1])**2 + (R[0,2])**2), R[0,0])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[1,2], R[1,1])
elif beta >= math.pi-err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[1,2], R[1,1])
else:
alpha = math.atan2((R[1,0])/(math.sin(beta)), -(R[2,0])/(math.sin(beta)))
gamma = math.atan2((R[0,1])/(math.sin(beta)), (R[0,2])/(math.sin(beta)))
elif type == "XZX":
# sqrt(R[1,0]**2 + R[2,0]**2)/R[0,0] == |sin(beta)|/cos(beta)
# ==> beta (0, pi)
beta = math.atan2(math.sqrt((R[1,0])**2 + (R[2,0])**2), R[0,0])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[1,2], R[1,1])
elif beta >= math.pi-err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[1,2], -R[1,1])
else:
alpha = math.atan2((R[2,0])/(math.sin(beta)), (R[1,0])/(math.sin(beta)))
gamma = math.atan2((R[0,2])/(math.sin(beta)), -(R[0,1])/(math.sin(beta)))
elif type == "YXY":
# sqrt(R[0,1]**2 + R[2,1]**2)/R[1,1] == |sin(beta)|/cos(beta)
# ==> beta(0, pi)
beta = math.atan2(math.sqrt((R[0,1])**2 + (R[2,1])**2), R[1,1])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], R[0,0])
elif beta >= math.pi-err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], R[0,0])
else:
alpha = math.atan2((R[0,1])/(math.sin(beta)), (R[2,1])/(math.sin(beta)))
gamma = math.atan2((R[1,0])/(math.sin(beta)), -(R[1,2])/(math.sin(beta)))
elif type == "YZY":
# sqrt(R[0,1]**2 + R[2,1]**2)/R[1,1] == |sin(beta)|/cos(beta)
# ==> beta(0, pi)
beta = math.atan2(math.sqrt((R[0,1])**2 + (R[2,1])**2), R[1,1])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,2], R[0,0])
elif beta >= math.pi-err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,2], -R[0,0])
else:
alpha = math.atan2((R[2,1])/(math.sin(beta)), -(R[0,1])/(math.sin(beta)))
gamma = math.atan2((R[1,2])/(math.sin(beta)), (R[1,0])/(math.sin(beta)))
elif type == "ZXZ":
# sqrt(R[0,2]**2 + R[1,2]**2)/R[2,2] == |sin(beta)|/cos(beta)
# ==> beta(0, pi)
beta = math.atan2(math.sqrt((R[0,2])**2 + (R[1,2])**2), R[2,2])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], R[0,0])
elif beta >= math.pi-err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], R[0,0])
else:
alpha = math.atan2((R[0,2])/(math.sin(beta)), -(R[1,2])/(math.sin(beta)))
gamma = math.atan2((R[2,0])/(math.sin(beta)), (R[2,1])/(math.sin(beta)))
elif type == "ZYZ":
# sqrt(R[0,2]**2 + R[1,2]**2)/R[2,2] == |sin(beta)|/cos(beta)
# ==> beta(0, pi)
beta = math.atan2(math.sqrt((R[0,2])**2 + (R[1,2])**2), R[2,2])
if beta >= 0.0-err and beta <= 0.0+err:
beta = 0.0
# alpha + gamma is fixed
alpha = 0.0
gamma = math.atan2(-R[0,1], R[0,0])
elif beta >= math.pi+err and beta <= math.pi+err:
beta = math.pi
# alpha - gamma is fixed
alpha = 0.0
gamma = math.atan2(R[0,1], -R[0,0])
else:
alpha = math.atan2((R[1,2])/(math.sin(beta)), (R[0,2])/(math.sin(beta)))
gamma = math.atan2((R[2,1])/(math.sin(beta)), -(R[2,0])/(math.sin(beta)))
return alpha, beta, gamma
if __name__ == "__main__":
RM = np.loadtxt ('RotationMatrix.txt')
print("Upper -- intrinsic\nLower -- extrinsic")
euler_type = str(input("Please input the type of euler angle...\n"))
if euler_type.isupper():
angle_0, angle_1, angle_2 = r2euler(RM, euler_type)
print("Intrinsic")
print("Angle about {} is {}".format(euler_type[0], angle_0))
print("Angle about {} is {}".format(euler_type[1], angle_1))
print("Angle about {} is {}".format(euler_type[2], angle_2))
elif euler_type.islower():
angle_0, angle_1, angle_2 = r2euler(RM, euler_type.upper()[::-1])[::-1]
print("extrinsic")
print("Angle about {} is {}".format(euler_type[0], angle_0))
print("Angle about {} is {}".format(euler_type[1], angle_1))
print("Angle about {} is {}".format(euler_type[2], angle_2))
else:
pass
通过实际计算，能够找到大致的规律：
-
第二次旋转角度有重要的影响：
对于ABC型的欧拉角旋转方式：
第二次旋转角度为theta_B2 = ±pi/2时，会导致第一、三次旋转同向或反向，即theta_A1 ± thetaC_3 = 定值，具体需要根据右手定则判断。对于
ABA型的欧拉角旋转方式：
第二次旋转角度为theta_B2 = 0时，导致第一、三次旋转同向，theta_A1 + thetaA_3 = 定值
第二次旋转角度为theta_B2 = pi时，导致第一、三次旋转反向，theta_A1 - thetaA_3 = 定值 -
计算思路为：
先计算第二次旋转角度theta_2：
(1) 找到theta_2的正弦/余弦
(2)theta_2与其余一角的两个方程，求平方和，开方得到余弦/正弦的绝对值）
(3) 使用四象限反正切函数 atan2(y,x)求解theta_2的值，其中绝对值对应了取值范围
(4) 判断theta_2是否是特殊值，若是，其余两角和/差为定值，令一者为0，求解。
(5) 若不是特殊值，使用其余角与theta_2正弦/余弦的乘积的矩阵元素进行求解，分别得到其余角的正弦/余弦。
(6) 使用四象限反正切函数 atan2(y,x)求解其余角的值。
SCIPY库求解
scipy库中具有旋转矩阵相关的强大库函数，使用很方便！
动系欧拉角、静系欧拉角、四元数等等都可以求
比在学校的时候用Matlab手写好太多了[手动苦涩]
其中，需要注意输入参数euler_type的大写与小写。
大写表示绕动系（内旋、intrinsic rotations），矩阵右乘；
小写表示绕静系（外旋、extrinsic rotations），矩阵左乘。
"""
FileName: r2euler_sci.py
File to caluculate euler from rotation matrix
Method of using package scipy
"""
import numpy as np
from scipy.spatial.transform import Rotation as R
if __name__ == "__main__":
RM = np.loadtxt ('RotationMatrix.txt')
euler_type = str(input("Please input the type of euler angle...\n"))
sciangle_0, sciangle_1, sciangle_2 = R.from_matrix(RM).as_euler(euler_type)
# Upper word means a rotate about self coordinate (intrinsic rotations)
# Lower word means a rotate about world coordinate (extrinsic rotations)
print("Angle about {} is {}".format(euler_type[0], sciangle_0))
print("Angle about {} is {}".format(euler_type[1], sciangle_1))
print("Angle about {} is {}".format(euler_type[2], sciangle_2))
在线计算网站
如果只需要临时进行少量的数值计算，使用界面的网站更加直观~
3D Rotation Converter
总结
以上是笔者在求解欧拉角的实现方法，希望能对各位同学有所帮助。
请路过的大佬多多指点~
6054
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
