python旋转矩阵与欧拉角互转
说明如何实现python旋转矩阵与欧拉角互转并拓展解释什么是欧拉角
学习链接:Rotation Matrix To Euler Angles | LearnOpenCV #
先上代码说明如何实现python旋转矩阵与欧拉角互转:
欧拉角 ——> 旋转矩阵
import math
import numpy as np
def eulerAngles2rotationMat(theta, format='degree'):
"""
Calculates Rotation Matrix given euler angles.
:param theta: 1-by-3 list [rx, ry, rz] angle in degree
:return:
RPY角,是ZYX欧拉角,依次 绕定轴XYZ转动[rx, ry, rz]
"""
if format is 'degree':
theta = [i * math.pi / 180.0 for i in theta]
R_x = np.array([[1, 0, 0],
[0, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0])]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot(R_y, R_x))
return R
print(eulerAngles2rotationMat([120,0,0]))
旋转矩阵——> 欧拉角
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype = R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 1e-6
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :
assert(isRotationMatrix(R))
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular :
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])
知识拓展:什么是欧拉角?
用一句话说,欧拉角就是物体绕坐标系三个坐标轴(x,y,z轴)的旋转角度。
在这里,坐标系可以是世界坐标系,也可以是物体坐标系,旋转顺序也是任意的,可以是xyz,xzy,yxz,zxy,yzx,zyx中的任何一种,甚至可以是xyx,xyy,xzz,zxz等等等等。。。。。。
所以说欧拉角多种多样。欧拉角可分为两种情况:
1,静态:即绕世界坐标系三个轴的旋转,由于物体旋转过程中坐标轴保持静止,所以称为静态。
2,动态:即绕物体坐标系三个轴的旋转,由于物体旋转过程中坐标轴随着物体做相同的转动,所以称为动态。
对于分别绕三个坐标轴旋转的情况,下述定理成立:
物体的任何一种旋转都可分解为分别绕三个轴的旋转,但分解方式不唯一。如:
假设绕y轴旋转为heading,绕x轴旋转为pitch,绕z轴旋转为bank,则先heading45°再pitch90°等价于先pitch90°再bank45°。
前面曾提到过,heading-pitch-bank系统不是惟一的欧拉角系统,绕任意三个互相垂直轴的任意旋转序列都能定义一个方位。所以,多种选择导致了欧拉角约定的多样性:
1)heading-pitch-bank系统有两个名称,当然,不同的名字并不代表不同的约定,这其实并不重要,一组常用的术语是roll-pitch-yaw,其中的roll对应与bank,yaw对应于heading,它定义了从物体坐标系到惯性坐标系的旋转顺序
2)任意三个轴都能作为旋转轴,不一定必须是笛卡尔轴,但是用笛卡尔轴最有意义
3)也可以选用右手坐标规则
4)旋转可以以不同的顺序进行
3,优点:1)容易使用;2)表达简洁;3)任意三个角都是合法的
4,缺点:1)给定方位的表达方式不唯一;2)两个角度间求插值非常困难
采用限制欧拉角的方法来避免以上问题的出现:heading限制在+-180,pitch为+-90。以上为欧拉角的定义。旋转的方法如下:
从欧拉角矢量转换很容易,困难的部分是转换回来。XNA提供了一个方法可以创建旋转矩阵,但它并没有提供转换回来的方法,因此我们将不得不自己实现。
首先,我们转换为旋转矩阵,Matrix.CreateFromYawPitchRoll()方法可以做到这一点。如果这里使用欧拉角,我们需要以以下顺序提供坐标:
Yaw(偏航):欧拉角向量的y轴
Pitch(俯仰):欧拉角向量的x轴
Roll(翻滚): 欧拉角向量的z轴
想象一下飞机,yaw指水平方向的机头指向,它绕y轴旋转。Pitch指与水平方向的夹角,绕x轴旋转。Roll指飞机的翻滚,绕z轴旋转。
除欧拉角以外,常用的还有四元素法和旋转矩阵法。简而言之,三种方法的特点如下:
1)欧拉角最直观、最容易理解、存储空间少,但是欧拉角存在万向节死锁现象、插值速度不均匀等缺点,而且不可以在计算机中直接运算;
2)四元素不存在万向节死锁问题、利用球面插值可以获得均匀的转速、存储空间也较少,但是不好理解、不直观;
3)旋转矩阵法是最便于计算机处理的,但不可以直接插值、冗余信息多、费存储空间,同样不直观。所以,在机器人学中,一般人机交互端会用欧拉角,插值等用四元素,正逆运动学运算中用矩阵表示法。
拓展知识转载链接:什么是欧拉角/姿态角?
更多推荐
所有评论(0)