赞
踩
姿态的几种表示形式,姿态角、四元数、欧拉角、旋转矩阵、位姿矩阵。
姿态 | 说明 | 表示形式 | Eigen |
---|---|---|---|
姿态角 | 指的是机体坐标系与地理坐标系的夹角,即旋转向量 | rx,ry,rz | Eigen::Vector3f(Degrees) |
四元数 | 四元素不存在万向节死锁问题、利用球面插值可以获得均匀的转速 | w,x,y,z | Eigen::Quaternionf |
欧拉角 | 绕机体坐标系三个轴旋转的角度 经典欧拉角ZXZ系列 :ZXZ, XYX, YZY, ZYZ, XZX, YXY 泰特布莱恩角ZYX系列:XYZ, YZX, ZXY, XZY, ZYX, YXZ | ex,ey,ez,sequence | |
RPY | 欧拉角的变种,滚转角、俯仰角、偏航角 与ZYX系列对应为 :YPR,RYP,PRY,PYR,RPY,YRP | ex,ey,ez,sequence | |
旋转矩阵 | 3×3 | Eigen::Matrix3f | |
位姿矩阵 | 4×4 | Eigen::Matrix4f |
注意:坐标系的定义与旋转矩阵是配套的,禁止混用。Eigen 内部的计算均为弧度值。
欧拉角分类
enum EulerAngleSequence{
//经典欧拉角ZXZ系列
ZXZ, XYX, YZY, ZYZ, XZX, YXY,
//泰特布莱恩角ZYX系列
XYZ, YZX, ZXY, XZY, ZYX, YXZ,
//固定角
YPR, RYP, PRY, PYR, RPY, YRP
};
/* rx,ry,rz 分别转化为弧度值
* w_abs = sqrt(rx^2 + ry^2 + rz^2);
* qw = cos(w_abs / 2)
* qx = sin(w_abs / 2) * rx / w_abs
* qy = sin(w_abs / 2) * ry / w_abs
* qz = sin(w_abs / 2) * rz / w_abs
*/
法1:使用Eigen库
Eigen::Quaternionf RotateVectortoQuaternionf(const Eigen::Vector3f& rotateVec) {
Eigen::Vector3f vecfDegree = Eigen::Vector3f(rotateVec[0] * PI / 180,
rotateVec[1] * PI / 180,
rotateVec[2] * PI / 180);
float w_abs = sqrt(vecfDegree[0] * vecfDegree[0] +
vecfDegree[1] * vecfDegree[1] +
vecfDegree[2] * vecfDegree[2]);
return Eigen::Quaternionf(cos(w_abs / 2),
sin(w_abs / 2) * vecfDegree[0] / w_abs,
sin(w_abs / 2) * vecfDegree[1] / w_abs,
sin(w_abs / 2) * vecfDegree[2] / w_abs);
}
法2:使用公式
Eigen::Quaternionf RotateVectortoQuaternionf(const float rx, const float ry, const float rz) {
float rxx = rx * PI / 180;
float ryy = ry * PI / 180;
float rzz = rz * PI / 180;
float w_abs = sqrt(rxx * rxx + ryy * ryy + rzz * rzz);
return Eigen::Quaternionf(cos(w_abs / 2),
sin(w_abs / 2) * rxx / w_abs,
sin(w_abs / 2) * ryy / w_abs,
sin(w_abs / 2) * rzz / w_abs);
}
/* qx,qy,qz,qw
* angle = acos(qw) * 2
* rx = qx / sin(angle /2) * angle * 180 / PI
* ry = qy / sin(angle /2) * angle * 180 / PI
* rz = qz / sin(angle /2) * angle * 180 / PI
*/
法1:使用Eigen库
Eigen::Vector3f QuaterniontoRotateVector(const Eigen::Quaternionf& quaternion) {
float angle = acos(quaternion.w()) * 2;
float x = quaternion.x() / sin(angle / 2);
float y = quaternion.y() / sin(angle / 2);
float z = quaternion.z() / sin(angle / 2);
return Eigen::Vector3f(angle * x * 180 / M_PI,
angle * y * 180 / M_PI,
angle * z * 180 / M_PI);
}
法2:使用公式
Eigen::Vector3f QuaterniontoRotateVector(const float qx, const float qy,
const float qz, const float qw) {
float angle = acos(qw) * 2;
float x = qx / sin(angle / 2);
float y = qy / sin(angle / 2);
float z = qz / sin(angle / 2);
return Eigen::Vector3f(angle * x * 180 / M_PI,
angle * y * 180 / M_PI,
angle * z * 180 / M_PI);
}
法1:使用Eigen库
Eigen::Quaternionf RotateMattoQuaternion(const Eigen::Matrix3f& rotateMat) {
return Eigen::Quaternionf(rotateMat);
}
法1:使用Eigen库
Eigen::Matrix3f EulerAngletoRotateMat(const Eigen::Vector3f& eulerAngle, EulerAngleSequence sequence) { Eigen::AngleAxis<float> Rx = Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitX()); Eigen::AngleAxis<float> Ry = Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitY()); Eigen::AngleAxis<float> Rz = Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitZ()); Eigen::Matrix3f rotate_mat = Eigen::Matrix3f::Zero(); switch (sequence) { default: case XYZ: case YPR: rotate_mat = Eigen::Matrix3f(Rx * Ry * Rz); break; case YZX: case RYP: rotate_mat = Eigen::Matrix3f(Ry * Rz * Rx); break; case ZXY: case PRY: rotate_mat = Eigen::Matrix3f(Rz * Rx * Ry); break; case ZXY: case PRY: rotate_mat = Eigen::Matrix3f(Rz * Rx * Ry); break; case XZY: case PYR: rotate_mat = Eigen::Matrix3f(Rx * Rz * Ry); break; case ZYX: case RPY: rotate_mat = Eigen::Matrix3f(Rz * Ry * Rx); break; case YXZ: case YRP: rotate_mat = Eigen::Matrix3f(Ry * Rx * Rz); break; case YZY: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitY())); break; case ZYZ: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitZ())); break; case XZX: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitX())); break; case ZXZ: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitZ())); break; case XYX: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitX())); break; case YXY: rotate_mat = Eigen::Matrix3f( Eigen::AngleAxisf(eulerAngle[0], Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(eulerAngle[1], Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(eulerAngle[2], Eigen::Vector3f::UnitY())); break; } return Eigen::Quaternionf(rotate_mat); }
法1:使用Eigen库
Eigen::Quaternionf EulerAngletoRotateMat(const Eigen::Vector3f& eulerAngle,
EulerAngleSequence sequence) {
Eigen::Matrix3f rotate_mat = EulerAngletoQuaternion(eulerAngle, sequence);
return Eigen::Quaternionf(rotate_mat);
}
参考文献:
厂家 | 位姿形式 | Eigen |
---|---|---|
ABB | 四元数 | Eigen::Quaternionf |
FANUC | 欧拉角 ZYX | Eigen::Matrix3f(Eigen::AngleAxisf(rx, Eigen::Vector3f::UnitX()), Eigen::AngleAxisf(ry, Eigen::Vector3f::UnitY()), Eigen::AngleAxisf(rz, Eigen::Vector3f::UnitZ())) |
UR | 欧拉角 ZYX | Eigen::Vector3f(Degrees) |
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。