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

置顶小老鼠不吃猫

已于 2023-08-14 10:26:44 修改

阅读量4.8k

点赞数 11

分类专栏： # 持之以恒系列——数学算法文章标签：矩阵线性代数

于 2023-02-03 17:30:19 首次发布

本文链接：https://blog.csdn.net/qq_43572400/article/details/128872699

版权

持之以恒系列——数学算法专栏收录该内容

5 篇文章 3 订阅

订阅专栏

文章介绍了姿态的几种表示方法，包括旋转向量、四元数、欧拉角、旋转矩阵和位姿矩阵，并提供了旋转向量与四元数之间的转换接口，以及欧拉角与旋转矩阵的转换函数。此外，提到了在线转换工具和工业机器人如ABB、FANUC、UR在位姿表示上的差异。

摘要由CSDN通过智能技术生成

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

3D Rotation Converter：https://www.andre-gaschler.com/rotationconverter/
Rotation Conversion Tool： https://danceswithcode.net/engineeringnotes/quaternions/conversion_tool.html
四元数在线可视化转换网站： 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://zhuanlan.zhihu.com/p/626258420
欧拉角(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)