文章讲述了万向死锁现象在静态全局坐标系中的表现，介绍了欧拉角、四元数和旋转矩阵之间的转换，包括Python示例和autolab_core库的实现，以及如何用C++和scipy处理四元数到旋转矩阵的转换，以避免万向节死锁问题。
目录
万向死锁现象
在静态全局坐标系中，会出现
在平衡环架中容易复现这个问题
主要就是一个轴旋转90度，会和另一个轴重合，两个轴旋转效果一样，丢失一个自由度。
不错的讲解：
欧拉角(横滚角、俯仰角、偏航角)、旋转矩阵、四元数的转换与解决万向节死锁_站心坐标系下的欧拉角-CSDN博客
scipy把四元数转旋转矩阵
conda install scipy
原文：python中将四元数转换为旋转矩阵_python 四元数转旋转矩阵-CSDN博客
from scipy.spatial.transform import Rotation as R
import numpy as np
print('test')
# use [:, np.newaxis] to transform from row vector to col vector
position = np.array([0.6453529828252734, -0.26022684372145516, 1.179122068068349])[:, np.newaxis]
share_vector = np.array([0,0,0,1], dtype=float)[np.newaxis, :]
print('share_vector:\n', share_vector)
print('position:\n',position)
r = R.from_quat([-0.716556549511624,-0.6971278819736084, -0.010016582945017661, 0.02142651612120239])
r.as_matrix()
print('rotation:\n',r.as_matrix())
rotation_matrix = r.as_matrix()
print(rotation_matrix)
#combine three matrix or vector together
m34 = np.concatenate((rotation_matrix, position), axis = 1)
print(m34)
m44 = np.concatenate((m34, share_vector), axis=0)
# m44 = np.hstack((m34, share_vector))
print(m44)
rot_vec = r.as_rotvec()
print('rot_vec:\n', rot_vec)
rot_euler = r.as_euler('zyx', degrees = False)
print('rot_euler:\n',rot_euler)
r = R.from_matrix(rotation_matrix)
print('as_quat():\n',r.as_quat())
print('as_rotvec():\n', r.as_rotvec())
print('as_euler():\n', r.as_euler('zyx', degrees=True))autolab_core库实现
import numpy as np
from autolab_core import RigidTransform
# 写上用四元数表示的orientation和xyz表示的position
orientation = {'y': -0.6971278819736084, 'x': -0.716556549511624, 'z': -0.010016582945017661, 'w': 0.02142651612120239}
position = {'y': -0.26022684372145516, 'x': 0.6453529828252734, 'z': 1.179122068068349}
rotation_quaternion = np.asarray([orientation['w'], orientation['x'], orientation['y'], orientation['z']])
translation = np.asarray([position['x'], position['y'], position['z']])
# 这里用的是UC Berkeley的autolab_core，比较方便吧，当然可以自己写一个fuction来计算，计算公式在https://www.cnblogs.com/flyinggod/p/8144100.html
T_qua2rota = RigidTransform(rotation_quaternion, translation)
print(T_qua2rota)
# 以下是打印的结果
Tra: [ 0.64535298 -0.26022684 1.17912207]
Rot: [[ 0.02782477 0.99949234 -0.01551915]
[ 0.99863386 -0.02710724 0.0446723 ]
[ 0.04422894 -0.01674094 -0.99888114]]
Qtn: [-0.02142652 0.71655655 0.69712788 0.01001658]
from unassigned to world
c++实现：
有tf版本，Eigen库
欧拉角，四元数，旋转矩阵相互转化（c++, python) - 知乎
6、Python示例
6.1、欧拉角转旋转矩阵
import math
import numpy as np
def euler_to_matrix(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
分别计算绕x、y、z轴的旋转矩阵，然后以某个顺序做点积运算即可。
print(euler_to_matrix([math.pi/3,0,math.pi/6]))
/*
[[ 0.8660254 -0.25 0.4330127]
[ 0.5 0.4330127 -0.75 ]
[-0. 0.8660254 0.5 ]]
*/
使用transforms3d或者scipy里面的库，分别来验证下，答案是正确的：
import transforms3d as tfs
print(tfs.euler.euler2mat(math.pi/3,0,math.pi/6,"sxyz"))
print(np.degrees(tfs.euler.mat2euler(euler_to_matrix([math.pi/3,0,math.pi/6]),"sxyz"))) #[60. -0. 30.]
//使用弧度制
from scipy.spatial.transform import Rotation as R
print(R.from_euler('xyz', [math.pi/3,0,math.pi/6], degrees=False).as_matrix())
print(np.degrees(R.from_matrix(euler_to_matrix([math.pi/3,0,math.pi/6])).as_euler('xyz'))) #[60. 0. 30.]
//使用角度制
print(R.from_euler('xyz', [60,0,30], degrees=True).as_matrix())
一般我们都是以弧度为标准，当然有些情况为了直观，我们也可以转成角度来运算。
6.2、欧拉角转四元数
import math
def euler_to_quaternion(roll, pitch, yaw):
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return w, x, y, z
print(euler_to_quaternion(math.pi/3,0,math.pi/6))
#(0.8365163037378079, 0.4829629131445341, 0.12940952255126034, 0.2241438680420134)
print(tfs.euler.euler2quat(math.pi/3,0,math.pi/6,"sxyz"))
#[0.8365163 0.48296291 0.12940952 0.22414387]
print(euler_to_quaternion(math.pi/3,math.pi,math.pi/2))
#(0.3535533905932738, -0.6123724356957946, 0.6123724356957946, -0.3535533905932737)
print(tfs.euler.euler2quat(math.pi/3,math.pi,math.pi/2,"sxyz"))
#array([ 0.35355339, -0.61237244, 0.61237244, -0.35355339])
6.3、四元数转旋转矩阵
#x, y ,z ,w
def quaternion_to_rotation_matrix(q):
rot_matrix = np.array(
[[1.0 - 2 * (q[1] * q[1] + q[2] * q[2]), 2 * (q[0] * q[1] - q[3] * q[2]), 2 * (q[3] * q[1] + q[0] * q[2])],
[2 * (q[0] * q[1] + q[3] * q[2]), 1.0 - 2 * (q[0] * q[0] + q[2] * q[2]), 2 * (q[1] * q[2] - q[3] * q[0])],
[2 * (q[0] * q[2] - q[3] * q[1]), 2 * (q[1] * q[2] + q[3] * q[0]), 1.0 - 2 * (q[0] * q[0] + q[1] * q[1])]],
dtype=q.dtype)
return rot_matrix
r_matrix=quaternion_to_rotation_matrix(np.array([0.4829629,0.12940952,0.22414387,0.8365163]))
print(r_matrix)
/*
[[ 8.66025403e-01 -2.50000007e-01 4.33012693e-01]
[ 4.99999996e-01 4.33012726e-01 -7.49999975e-01]
[ 1.23449401e-09 8.66025378e-01 5.00000027e-01]]
*/
6.4、旋转矩阵转欧拉角
def rotation_matrix_to_euler(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])
print(rotation_matrix_to_euler(r_matrix))
//弧度
#[ 1.04719751e+00 -1.23449401e-09 5.23598772e-01]
print(np.degrees(rotation_matrix_to_euler(r_matrix)))
//角度
#[ 5.99999979e+01 -7.07312966e-08 2.99999998e+01]
7、小结
这里通过画图直观了解到两种坐标系的关系，以及飞机在飞行过程中产生的欧拉角的多种表示方法，由于欧拉角是三自由度，当两根轴重叠之后将发生“万向节死锁”的问题，所以我们一般都使用四元数来代替欧拉角，四个自由度避免了万向节发生死锁。
由于不同顺序的旋转将会产生不一样的旋转矩阵，这里我也通过三角函数的知识，将欧拉角分别沿着三个轴做旋转得到的旋转矩阵做了推导，希望可以帮助到大家更好地理解。
原文链接：https://blog.csdn.net/weixin_41896770/article/details/134346795
1098
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
