1. Toy模板网首页
  2. > Toy博客

持之以恒（一）位姿转换：姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵

9月前 作者：小老鼠不吃猫 分类：Toy博客 阅读(53) 违法举报

这篇具有很好参考价值的文章主要介绍了持之以恒（一）位姿转换：姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

1. 简介

1.1 位姿的几种表示形式

姿态的几种表示形式，姿态角、四元数、欧拉角、旋转矩阵、位姿矩阵。

姿态 说明 表示形式 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 内部的计算均为弧度值。

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. 位姿转换接口

欧拉角分类

enum EulerAngleSequence{
    //经典欧拉角ZXZ系列
    ZXZ, XYX, YZY, ZYZ, XZX, YXY,
    //泰特布莱恩角ZYX系列
    XYZ, YZX, ZXY, XZY, ZYX, YXZ,
    //固定角
    YPR, RYP, PRY, PYR, RPY, YRP
};

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);
}

四元数 转 旋转向量

  • 四元数转旋转向量公式
/* 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);
}

2.2 旋转矩阵与四元数

旋转矩阵 转 四元数

法1：使用Eigen库

Eigen::Quaternionf RotateMattoQuaternion(const Eigen::Matrix3f& rotateMat) {
    return Eigen::Quaternionf(rotateMat);
}

四元数 转 旋转矩阵

2.3 欧拉角与旋转矩阵

欧拉角 转 旋转矩阵

法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);
}

2.4 欧拉角与四元数

欧拉角 转 四元数

法1：使用Eigen库

Eigen::Quaternionf EulerAngletoRotateMat(const Eigen::Vector3f& eulerAngle,
                                         EulerAngleSequence sequence) {
    Eigen::Matrix3f rotate_mat = EulerAngletoQuaternion(eulerAngle, sequence);
    return Eigen::Quaternionf(rotate_mat);
}

参考文献：文章来源地址https://www.toymoban.com/news/detail-553731.html

  1. 三维旋转之欧拉角： https://zhuanlan.zhihu.com/p/626258420
  2. 欧拉角(Euler Angle)： https://blog.csdn.net/thefist11cc/article/details/126595776

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 欧拉角 ZYX Eigen::Vector3f(Degrees)

到了这里，关于持之以恒（一）位姿转换：姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！

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

分享到：
领支付宝红包 赞助服务器费用

相关文章

  • 三轴加速度计解算姿态（四元数）

    当传感器载体静止时，加速度计只会输出重力加速度，可以凭此来计算载体的俯仰角和滚转角。 假设导航坐标系为东北天，载体坐标系为右前上。 初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示 载体在导航坐标系下的旋转 。 先将四元数转为

    2024年02月15日
    浏览(47)

  • STM32无人机-四轴四元数姿态解算与卡尔曼滤波

    MPU6050是一种非常流行的空间运动传感器芯片，可以获取器件当前的三个加速度分量和三个旋转角速度。 什么是四元数 这部分很难，新手知道四元数的功能是将 6 轴传感器数据转化为三轴姿态角度数据即可。 四元数解算程序店家已经封装成一个函数，输入 MPU6050 数值，解算周

    2024年02月03日
    浏览(42)
  • 【无人机/平衡车/机器人】详解STM32+MPU6050姿态解算—卡尔曼滤波+四元数法+互补滤波（文末附3个算法源码）

    【无人机/平衡车/机器人】详解STM32+MPU6050姿态解算—卡尔曼滤波+四元数法+互补滤波（文末附3个算法源码）

     效果： MPU6050姿态解算-卡尔曼滤波+四元数+互补滤波 目录 基础知识详解 欧拉角

    2024年04月14日
    浏览(74)
  • 四元数，欧拉角和旋转矩阵相互转换

    四元数，欧拉角和旋转矩阵相互转换

    打印输出： 在线转换网站：1、三维在线旋转变换网站 https://www.andre-gaschler.com/rotationconverter/ 2、 Rotation Conversion Tool https://danceswithcode.net/engineeringnotes/quaternions/conversion_tool.html 3、角度、弧度在线转换工具 https://www.osgeo.cn/app/sc210 参考链接：https://www.jianshu.com/p/4fda4c34b829 https://

    2024年02月14日
    浏览(43)
  • 坐标变换基础-欧拉角&固定角与位姿矩阵的相互转换

    坐标变换基础-欧拉角&固定角与位姿矩阵的相互转换

    空间中一个坐标系相对于另一个坐标系的变换关系用新坐标系的三个坐标轴相对于原坐标系的方向矢量来确定，可用 矩阵来描述。用齐次矩阵（4x4）来统一描述刚体的位置和姿态： 其中，R便是描述姿态的旋转矩阵。 和沿着三个坐标轴的平移运动不一样，旋转矩阵显得很不直

    2024年02月04日
    浏览(38)
  • 转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换

    转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换

    由于在平时总是或多或少的遇到平移旋转的问题，每次都是现查资料，然后查了忘，忘了继续查，这次弄明白之后干脆写一篇文章，给人方便同时于己方便，后续如有扩充或变动也方便添加。 假设有两个向量 a 1 = ( x 1 , y 1 , z 1 ) a_1 = (x_1, y_1, z_1) a 1 ​ = ( x 1 ​ , y 1 ​ , z 1 ​

    2024年02月03日
    浏览(79)

  • 【Eigen库使用】角轴、旋转矩阵、欧拉角、四元数转换

    在slam中经常用到的四种描述机器人orientation的变量，他们之间可以相互转化，使用Eigen库可以很容易的做到这一点， 需要特别关注的是：欧拉角与其余量之间的转换关系 ： 1）首先要明确的是， 必须要明确欧拉角的旋转次序 ，你可以选择RPY、YPR等方式，在相同的orientation下，

    2024年01月18日
    浏览(51)
  • 相机的位姿在地固坐标系ECEF和ENU坐标系的转换

    相机的位姿在地固坐标系ECEF和ENU坐标系的转换

    在地球科学和导航领域，通常使用地心地固坐标系（ECEF，Earth-Centered, Earth-Fixed）和东北天坐标系（ENU，East-North-Up）来描述地球上的位置和姿态。如下图所示： ​地心地固坐标ecef和东北天ENU坐标系 在倾斜摄影测量过程中，通常涉及这两个坐标系的转换，将相机的位姿互转，

    2024年02月12日
    浏览(45)
  • 使用Matlab机器人工具箱完成四元数到旋转矩阵的转换，附程序

    使用Matlab机器人工具箱完成四元数到旋转矩阵的转换，附程序

    在进行机械臂操作或写论文时，经常需要进行四元数、旋转矩阵、欧拉角等的转换。 此时，我们利用matlab里的机器人工具箱（Peter 开发）内置的函数就可完成，具体程序如下： 环境：Matlab2020b+robotics toolbox（安装方法在前几期文章里有） 此时运行matlab可得以下结果： 重要注

    2024年02月13日
    浏览(56)
  • 【踩坑记录】colmap中的相机位姿的坐标系定义及其可视化结果的隐含转换

    【踩坑记录】colmap中的相机位姿的坐标系定义及其可视化结果的隐含转换

      这个问题来自于我想要使用colmap的稀疏重建结果，然后发现由于相机坐标系的定义没弄清楚，导致我获取的结果存在问题。    1 问题引出   下面先从我们还不知道坐标系定义的视角开始理解，引出问题所在。使用的是一份无人机影像数据，共有59张影像：   下图

    2024年02月06日
    浏览(176)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包，优惠每天领

二维码1

领取红包

二维码2

领红包