欧拉角顺序与转换-创新互联
更多转换见:
基于eigen实现
基于python实现
这里就记录下顺序:
Z-Y-X
顺序,先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,即为RPY
c++版本:// 转欧拉角
Eigen::Quaternionf q_;
q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
// 转四元数
// euler_angles存储顺序是:yaw(z) pitch(y) roll(x)
Eigen::Matrix3d rotation_matrix
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
Eigen的角度范围有点问题,
// 实验室自己的代码,返回的是[roll pitch yaw], 也是ZYX顺序
void get_euler_from_R(Vector<3>&e, const Matrix<3,3>&R) {float phi = atan2(R(2, 1), R(2, 2)); // roll
float theta = asin(-R(2, 0)); // pitch
float psi = atan2(R(1, 0), R(0, 0)); // yaw
float pi = M_PI;
if (fabs(theta - pi/2.0f)< 1.0e-3) { phi = 0.0f;
psi = atan2(R(1, 2), R(0, 2));
} else if (fabs(theta + pi/2.0f)< 1.0e-3) { phi = 0.0f;
psi = atan2(-R(1, 2), -R(0, 2));
}
e(0) = phi;
e(1) = theta;
e(2) = psi;
}
推导:假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma,则对应先roll,后ptich,最后yaw的旋转可以计算如下:
import numpy as np
import math
from scipy.spatial.transform import Rotation as R
# `Z-Y-X`顺序,先绕x轴roll,再绕y轴pitch,最后绕z轴yaw
# euler_angles存储顺序是:[yaw(z) pitch(y) roll(x)]
# 转欧拉角
Rotation = R.from_quat([x,y,z,w]) # Rotation只是一个旋转,不是矩阵也不是四元数
euler_angles = Rotation.as_euler('zyx', degrees=False)
# 转四元数
Rotation = R.from_euler('zxy', [yaw, pitch, roll], degrees=False)
rotation_matrix = Rotation.as_matrix()
quaternion = Rotation.as_quat()
你是否还在寻找稳定的海外服务器提供商?创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源,准确流量调度确保服务器高可用性,企业级服务器适合批量采购,新人活动首月15元起,快前往官网查看详情吧
当前标题:欧拉角顺序与转换-创新互联
标题来源:http://scyanting.com/article/hedso.html