多个旋转叠加的旋转矩阵----关于多个旋转的左乘/右乘问题

上式是始终成立的，不论是相对于当前坐标系的连续旋转的叠加还是相对于固定坐标系的连续旋转的叠加。实际上上式扩展到多个连续坐标系的变换依然成立。方便，这里只考虑两次旋转的叠加（或者说三个坐标系之间的关系）。假设有三个坐标系{0}，{1}，{2}，坐标系{1}相对于{0}的姿态为。直接上例题，坐标系1和坐标系2的关系如上，求得坐标系1中的点（1，1，1）在坐标系2下的坐标。首先明确，左乘和右乘是关于多个

滴滴哒哒答答

2417人浏览 · 2024-01-27 16:13:24

滴滴哒哒答答 · 2024-01-27 16:13:24 发布

旋转矩阵意义

首先明确，左乘和右乘是关于多个旋转叠加最终得到一个旋转矩阵，其中多个连续旋转是左乘还是右乘的问题。

其次，明确对于旋转矩阵 $R_{ab}$

从坐标系角度看， $R_{ab}$ 表示 坐标系a姿态到坐标系b姿态的旋转，即将坐标系a旋转到坐标系b的旋转过程。是由a到b的。
从坐标点角度看， $R_{ab}$ 表示 把坐标系b中的坐标变换到坐标系a中坐标的旋转，是由b到a的。

$P_{a}=R_{ab}P_{b}$

多个旋转的叠加

方便，这里只考虑两次旋转的叠加（或者说三个坐标系之间的关系）。假设有三个坐标系{0}，{1}，{2}，坐标系{1}相对于{0}的姿态为 $R_{01}$ ，坐标系{2}相对于{1}的姿态为 $R_{12}$ 。对于空间中的任意一点 $P$ ，假设其在三个坐标系下的描述分别为 $P_{0}$ ， $P_{1}$ ， $P_{2}$ 。由于描述的都是同一个点，那么他们之间一定存在如下关系：

$P_{0}=R_{01}P_{1}$

$P_{1}=R_{12}P_{2}$

$P_{0}=R_{02}P_{2}$

可得

$P_{0}=R_{01}R_{12}P_{2}$

$R_{02}=R_{01}R_{12}$

上式是始终成立的，不论是相对于当前坐标系的连续旋转的叠加还是相对于固定坐标系的连续旋转的叠加。实际上上式扩展到多个连续坐标系的变换依然成立。

$R_{0j}=R_{01}R_{12}...R_{j-2,j-1}R_{j-1,j}$

看到这里之后，大家一定要看一下机器人理论基础：相似变换与连续旋转的左乘/右乘 - 知乎的内容，明白左乘和右乘求得的旋转矩阵是 $R_{0j}$ ，（根据上文旋转矩阵意义理解）再看后面的内容。

在SLAM中，一般是已知初始坐标 $P_{0}$ ，求经过变换后的坐标 $P_{2}$ ，即

$P_{2}=R_{02}^{-1}P_{0}=R_{20}P_{0}=R_{21}R_{10}P_{0}$

推广到多个连续坐标系的变换

$R_{j0}=R_{j,j-1}R_{j-1,j-2}...R_{21}R_{10}$

直接上例题，坐标系1和坐标系2的关系如上，求得坐标系1中的点（1，1，1）在坐标系2下的坐标。这里左乘和右乘求得的旋转矩阵是 $R_{12}$ 。

坐标系的旋转------相对于当前坐标系（动轴）的连续旋转，旋转增量右乘

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#define PI 3.14159265358979323846
 
int main(int argc, char const *argv[])
{
    // 采用XYZ旋转顺序（即滚转-俯仰-偏航，ACTIVE轴作为参考）， 
    // 容易看到，Frame 1-[roll=-pi/2，pitch=-pi/2，yaw=0]->Frame 2 , 所以求得的是 R12
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = -PI/2;
    yaw = 0;
 
    // 将欧拉角转换为旋转矩阵
    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_matrix1 * roation_matrix2 * roation_matrix3;  // R12
    
    Eigen::Quaterniond quaternion(roation_matrix);
 
    //使用旋转矩阵将Frame 1下的已知点（如（1，1，1））转换为Frame 2， 
    Eigen::Vector3d point_frame1(1,1,1);
    Eigen::Vector3d point_frame2;
 
    point_frame2 = roation_matrix.transpose() * point_frame1;  // P2=R21*P1
 
    std::cout << "***************************************" << std::endl;
    std::cout << "point in frame1: " << point_frame1.transpose() << std::endl;
    std::cout << "point in frame2: " << point_frame2.transpose() << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;
    std::cout << "Quaternion [x, y, z, w]: " << quaternion.coeffs().transpose() << std::endl; // [x, y, z, w]
    std::cout << "Quaternion [w, x, y, z]: " << quaternion.w() << " " << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() << std::endl;
    std::cout << std::endl;
 
    return 0;
}

坐标系的旋转------相对于固定坐标系（世界坐标系）的连续旋转，旋转增量左乘（RPY角：定轴，XYZ顺序）

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#define PI 3.14159265358979323846
int main(int argc, char const *argv[])
{
    // rosrun tf tf_echo /base_link base_laser 该命令输出的是 相对于固定坐标系
    // Frame 1 ---[roll=-pi/2, pitch=0, yaw=pi/2]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = 0;
    yaw = PI/2;
 
    // 将欧拉角转换为旋转矩阵
    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;  // R12

    Eigen::Quaterniond quaternion(roation_matrix);
 
    // 使用旋转矩阵将Frame 1下的已知点（如（1，1，1））转换为Frame 2， 
    Eigen::Vector3d point_frame1(1,1,1);
    Eigen::Vector3d point_frame2;
 
    point_frame2 = roation_matrix.transpose() * point_frame1;  // P2=R21*P1
 
    std::cout << "***************************************" << std::endl;
    std::cout << "point in frame1: " << point_frame1.transpose() << std::endl;
    std::cout << "point in frame2: " << point_frame2.transpose() << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;
    // [x, y, z, w]   Quaternion: -0.5 -0.5  0.5  0.5
    std::cout << "Quaternion [x, y, z, w]: " << quaternion.coeffs().transpose() << std::endl; 
    // [w, x, y, z]   Quaternion: 0.5 -0.5 -0.5 0.5
    std::cout << "Quaternion [w, x, y, z]: " << quaternion.w() << " " << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
 
    return 0;
}

运行结果

ACTIVE轴

***************************************
point in frame1: 1 1 1
point in frame2:  1 -1 -1
旋转矩阵R12: 
6.12323e-17           0          -1
          1 6.12323e-17 6.12323e-17
6.12323e-17          -1  3.7494e-33
***************************************
Quaternion [x, y, z, w]: -0.5 -0.5  0.5  0.5
Quaternion [w, x, y, z]: 0.5 -0.5 -0.5 0.5

FIX轴

***************************************
point in frame1: 1 1 1
point in frame2:  1 -1 -1
旋转矩阵R12: 
 6.12323e-17 -6.12323e-17           -1
           1   3.7494e-33  6.12323e-17
           0           -1  6.12323e-17
***************************************
Quaternion [x, y, z, w]: -0.5 -0.5  0.5  0.5
Quaternion [w, x, y, z]: 0.5 -0.5 -0.5 0.5

可以看出，左乘和右乘表示的是一个旋转(微小量接近于0)。