文章目录
- 1. 简介
- 1.1 位姿的几种表示形式
- 1.2 姿态转换在线工具
- 2. 位姿转换接口
- 2.1 旋转向量 转 四元数
- 2.2 四元数 转 旋转向量
- 2.3 四元数 与 旋转矩阵
- 3. 机器人相关应用
-
1. 简介
1.1 位姿的几种表示形式
姿态的几种表示形式，旋转向量、四元数、欧拉角、旋转矩阵、位姿矩阵。
| 姿态 | 表示形式 | Eigen |
|---|
| 旋转向量 | rx,ry,rz | Eigen::Vector3f(Degrees) |
| 四元数 | w,x,y,z | Eigen::Quaternionf |
| 欧拉角 | ex,ey,ez,sequence | |
| 旋转矩阵 | 3×3 | Eigen::Matrix3f |
| 位姿矩阵 | 4×4 | Eigen::Matrix4f |
注意：Eigen 内部的计算均为弧度值。
1.2 姿态转换在线工具
- 3D Rotation Converter：https://www.andre-gaschler.com/rotationconverter/
- Rotation Conversion Tool： https://danceswithcode.net/engineeringnotes/quaternions/conversion_tool.html
- 四元数在线可视化转换网站： https://quaternions.online/
2. 位姿转换接口
2.1 旋转向量 转 四元数
/* 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);
}
2.2 四元数 转 旋转向量
/* 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 Posture::toRotateVector(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 / PI, angle * y * 180 / PI, angle * z * 180 / PI);
}
法2：使用公式
Eigen::Vector3f Posture::toRotateVector(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 / PI, angle * y * 180 / PI, angle * z * 180 / PI);
}
2.3 四元数 与 旋转矩阵
3. 机器人相关应用
3.1 不同厂家协作机器人的位姿表示形式
| 厂家 | 位姿形式 | 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 | 旋转向量 | Eigen::Vector3f(Degrees) |
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
