持之以恒(一)位姿转换:姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵

文章目录

  • 1. 简介
    • 1.1 位姿的几种表示形式
    • 1.2 姿态转换在线工具
  • 2. 位姿转换接口
    • 2.1 旋转向量 转 四元数
    • 2.2 四元数 转 旋转向量
    • 2.3 四元数 与 旋转矩阵
  • 3. 机器人相关应用
    • 3.1 不同厂家协作机器人的位姿表示形式

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 姿态转换在线工具

  1. 3D Rotation Converter:https://www.andre-gaschler.com/rotationconverter/
  2. Rotation Conversion Tool: https://danceswithcode.net/engineeringnotes/quaternions/conversion_tool.html
  3. 四元数在线可视化转换网站: 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:使用公式文章来源地址https://uudwc.com/A/AZmbg

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)

原文地址:https://blog.csdn.net/qq_43572400/article/details/128872699

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请联系站长进行投诉反馈,一经查实,立即删除!

h
上一篇 2023年06月27日 14:26
下一篇 2023年06月27日 14:26