RPY角和Euler角

滴滴哒哒答答

已于 2025-05-03 19:32:37 修改

阅读量462

点赞数 3

分类专栏：数学知识文章标签：自动驾驶人工智能 ubuntu linux

于 2024-08-10 14:36:47 首次发布

本文链接：https://blog.csdn.net/m0_49384824/article/details/141090305

版权

数学知识专栏收录该内容

3 篇文章

订阅专栏

结论：RPY角和Euler角本质上是一样的，角度都是一样的，表达的是相同的旋转。

假设绕XYZ三个轴旋转的角度分别为 𝛼 𝛽 𝛾 ，则三次旋转的旋转矩阵计算方法如下：

$\begin{gathered} R_{x}(\left.\alpha)=\left[ \begin{array} {ccc}{1} & {0} & {0} \\ {0} & {\cos\alpha} & {-\sin\alpha} \\ {0} & {\sin\alpha} & {\cos\alpha} \end{array}\right.\right] \\ R_{y}(\beta)= \begin{bmatrix} \cos\beta & 0 & \sin\beta \\ 0 & 1 & 0 \\ -\sin\beta & 0 & \cos\beta \end{bmatrix} \\ R_{z}(\gamma)= \begin{bmatrix} \cos\gamma & -\sin\gamma & 0 \\ \sin\gamma & \cos\gamma & \\ 0 & 0 & 1 \end{bmatrix} \end{gathered}$

RPY角，fix轴(旋转增量左乘)，旋转顺序为 XYZ

$R_1=R_z(\gamma)*R_y(\beta)*R_x(\alpha) \\= \begin{pmatrix} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha - \sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha + \sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha + \cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha - \cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{pmatrix}$

Euler角，active轴(旋转增量右乘)，旋转顺序为ZYX

$R_2=R_z(\gamma)*R_y(\beta)*R_x(\alpha) \\= \begin{pmatrix} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha - \sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha + \sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha + \cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha - \cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{pmatrix}$

即 $R_1=R_2$ !

1、RPY角

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#define PI 3.14159265358979323846

int main(int argc, char const *argv[])
{
    // Frame 1 ---[roll=-pi/2, pitch=pi/3, yaw=pi/4]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 定轴，左乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // XYZ 左乘，即 Z*Y*X
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}

2、Euler

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#define PI 3.14159265358979323846
 
int main(int argc, char const *argv[])
{
    // Frame 1 ---[z=pi/4, y=pi/3, x=-pi/2]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 动轴，右乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // ZYX 右乘，即 Z*Y*X
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}

3、结果

RPY：

***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************

Euler：

***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************

4、获取RPY角/Euler角的方式

Eigen 库：eulerAngles()函数

ROS `tf` 库：getRPY()函数

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <tf/transform_datatypes.h> 
#define PI 3.14159265358979323846

int main(int argc, char const *argv[])
{
    // Frame 1 ---[roll=-pi/2, pitch=pi/3, yaw=pi/4]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 定轴，左乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // XYZ 左乘，即 Z*Y*X

    // 方式①
    // 提取 RPY (Roll, Pitch, Yaw) 角
    // 参数 (2, 1, 0) 指定了提取欧拉角的顺序，即旋转轴的顺序为 Z Y X
    Eigen::Vector3d ypr = roation_matrix.eulerAngles(2, 1, 0);  // Yaw, Pitch, Roll 顺序
    std::cout << "方式①" << std::endl;
    std::cout << "R:" << ypr.z() << " P:" << ypr.y() << " Y:" << ypr.x() << std::endl;

    //方式②
    tf::Matrix3x3 tf_matrix;

    // 逐元素复制数据
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            tf_matrix[i][j] = roation_matrix(i, j);
        }
    }
    double my_roll, my_pitch, my_yaw;
    tf_matrix.getRPY(my_roll, my_pitch, my_yaw);
    std::cout << "方式②" << std::endl;
    std::cout << "R:" << my_roll << " P:" << my_pitch << " Y:" << my_yaw << std::endl;
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}

结果：

方式①
R:-1.5708 P:1.0472 Y:0.785398
方式②
R:-1.5708 P:1.0472 Y:0.785398
***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************

参考: 欧拉角和旋转矩阵之间的转换