三维空间的旋转

Posted on | In

某些表述不准确，需要验证和更新

旋转的表示

我们知道，旋转有多种表示形式，常用的有欧拉角，旋转向量，单位四元数和旋转矩阵。在不同的场景下，四种表示形式的直观程度不同。我们可以根据需要，将旋转表示为一种直观的形式，然后转换成我们需要的形式。另外，在Eigen库中，四种形式都提供了接口，因此可以方便我们验证。

应用场景

下面以一个具体的应用场景，理解旋转的不同表示形式。场景如下：

现有一位姿未知的激光雷达，扫描一水平面(法向量为(0,0,1))，得到点云。通过RANSAC拟合得到该点云的法向量，求这个激光雷达的位姿，用欧拉角表示。(平移不需要考虑)

现在已知两个法向量，我们希望求出一个旋转，将水平面的法向量旋转到点云拟合平面的法向量上。这个旋转就是激光雷达的位姿。尽管向量的旋转很简单，高中就学过，但是我们无法直接得到该旋转的欧拉角。所以可以利用旋转的不同表示形式之间的关系，通过下面的步骤，一步步得到最终的欧拉角：

  1. 先用旋转向量表示这个旋转；
  2. 通过罗德里格斯公式，由旋转向量得到旋转矩阵；
  3. 根据旋转矩阵得到欧拉角。

旋转向量的表示

大学我们就学过，向量的点乘得到两个向量的夹角，叉乘得到两个向量在右手系的法向量。所以，向量的旋转可以利用这个向量和夹角表示。其实这是旋转向量中最直观的表示形式。我们还可以选择不同的旋转向量，通过不同的转角，将这两个向量中的一个旋转到另一个的位置。如图所示，旋转向量的表示有无数种。

无论哪种表示，最终的旋转结果都一样。所以我们就采用最简单的，用点乘和叉乘表示这个旋转。

罗德里格斯公式

得到了旋转向量，我们可以通过如下的罗德里格斯公式，得到旋转矩阵：

观察这个公式，我们发现，即使选取不同的旋转轴和旋转角，最终得到的旋转矩阵都一样。

获得欧拉角

欧拉角的顺规不同，得到的三个数值也不同。ROS中使用的是ZYX顺规，本文也延续这种表示。同时，需要注意的是，我们在用欧拉角表示向量的旋转时，默认选取了一个不随旋转变化的坐标系，并在这个坐标系下分多次旋转。这种表示形式是外参的。作为对比，之前的博客中提到的ENU到NED坐标系的旋转，坐标系随着旋转而旋转。每次旋转后，下次的旋转都在新的坐标系下旋转。这种表示形式是内参的。

程序

示例代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<math.h>
#include<iostream>
const double PI = 3.14159265;

int main(){
    double x,y,z;
    std::cout<<"input normal x y z"; // 输入法向量的x y z分量，注意归一化
    std::cin>>x>>y>>z;
    std::cout<<"x,y,z: "<<x<<'\t'<<y<<'\t'<<z<<std::endl;
    Eigen::Vector3d z_unit = Eigen::Vector3d(0.0,0.0,1.0); // 平面的法向量，就是z轴方向单位向量
    Eigen::Vector3d normal = Eigen::Vector3d(x,y,z);
    
    // 叉乘得到转轴，而且是从z_axis转到normal
    Eigen::Vector3d rot_axis = z_unit.cross(normal); 
    rot_axis.normalize(); // 不要忘了归一化
    std::cout<<"axis': \n"<<rot_axis2<<std::endl;
    
    // 点乘得到转角
    double rot_angle = acos(z_unit.dot(normal));
    std::cout<<"angle: \n"<<rot_angle*180.0/3.1416<<std::endl;
    
    // 得到旋转矩阵
    Eigen::Matrix3d m_rot = Eigen::Matrix3d(Eigen::AngleAxisd(rot_angle, rot_axis));
    
    // ZYX顺规，得到欧拉角
    Eigen::Vector3d v_RPY = m_rot.eulerAngles(2,1,0);
    std::cout<<"eulerAngle: \n"<<v_RPY*180.0/3.1416<<std::endl;
    
    return 0;
}

总结

通过一个具体的应用场景，熟悉了旋转的不同表示形式。其中需要注意的是：

  1. 欧拉角表示需要分清顺规，是内参还是外参，实质上没有区别
  2. 欧拉角，旋转向量，单位四元数和旋转矩阵想要表示一个有效的旋转，都有约束。旋转向量和四元数需要归一化，而旋转矩阵需要满足特定的形式
  3. 欧拉角，四元数，旋转向量都有多重表示，最终得到的旋转矩阵并没有区别。可以选取最直观和最简单的表示方式

参考文献

  1. 四元数详解(强烈推荐) https://github.com/Krasjet/quaternion
  2. 高翔, 张涛等. 视觉SLAM十四讲