关于欧拉角的一些说明

旋转的各种形式中，欧拉角因为形式多样，所以很容易被混淆，在此进行一些说明。

---

前言

关于欧拉角的函数有很多，如Eigen中的eulerAngles()函数、tf2中的setRPY()、getRPY()函数以及ROS中的urdf等等，由于欧拉角形式的多样性，不同函数对欧拉角的处理过程也有些区别。

欧拉角形式的多样性，主要是由旋转时选用的轴以及旋转顺序不一样造成的。常用的欧拉角有euler角和RPY角。其中，euler角是动轴转动，RPY角是定轴转动。
在RPY角中，R表示roll（滚转角-绕x轴旋转），P表示pitch（俯仰角-绕y旋转），Y表示yaw（偏航角-绕z轴旋转）。
注意：定轴的X-Y-Z旋转等价于动轴的Z-Y-X旋转。

欧拉角的一个重大缺点是会碰到著名的万向锁问题：在pitch为±90°时，第一次旋转与第三次旋转将使用同一个轴，导致系统丢失一个自由度（3次旋转变为2次旋转）。因此，欧拉角不适用于插值和迭代，往往只用于人机交互中。
注意：在右手坐标系中，大拇指指向旋转轴正方向，其余手指在握拳过程中的指向便是旋转的正方向。

---

一、eulerAngles()函数说明

Eigen中的eulerAngles()函数，可以将旋转矩阵转换为欧拉角的形式，同一个旋转矩阵因为设置参数的不同，可以得到不同数值的欧拉角，这与旋转的顺序及类型有关。
旋转的类型分为：定轴（外旋）和动轴（内旋），定轴运动是左乘运算，动轴运动是右乘运算。
Eigen::vector3d eulerAngles(Index a0, Index a1, Index a2)，其中参数a0、a1、a2 用 0、1、2表示旋转轴，其中0表示X轴，1表示Y轴，2表示Z轴。此外，a0 表示首先选择的轴，a1 表示其次旋转的轴，a2 表示最后旋转的轴。因此，输出结果的顺序与轴旋转的顺序相同。
例如：
Eigen::Vector3d eulerAngle = rot.eulerAngles(0,1,2);
这里， eulerAngle的存储顺序为：绕x轴的角度、绕y轴的角度、绕z轴的角度
旋转的顺序为：先绕x轴、再绕y轴、最后绕z轴（动轴）
计算旋转矩阵的连乘顺序为：R_matrix = Rx * Ry * Rz（右乘）

Eigen::Vector3d eulerAngle = rot.eulerAngles(2,1,0);
这里，eulerAngle的存储顺序为：绕z轴的角度、绕y轴的角度、绕x轴的角度
旋转的顺序为：先绕z轴、再绕y轴、最后绕x轴（动轴）
计算旋转矩阵的连乘顺序为：R_matrix = Rz * Ry * Rx（右乘）

注意：Eigen中的eulerAngle()函数在有些情况下不太稳定，下面介绍的tf2中关于欧拉角的函数会更加稳定一些。

---

二、tf2中的setRPY()、getRPY()函数

1.setRPY()函数说明

在ROS中可以使用tf2_ros::StaticTransformBroadcaster broadcaster发布坐标变换信息，信息格式为：geometry_msgs::TransformStamped，该消息的旋转部分是四元数格式。对旋转部分进行赋值，其中一种方法：使用tf2::Quaternion的setRPY(roll, pitch, yaw)函数，利用欧拉角初始化四元数。

    tf2_ros::StaticTransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped ts; // 存储坐标变换信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link"; // 父坐标系的名字
    ts.child_frame_id = "path"; // 子坐标系的名字
    ts.transform.translation.x = 0.0;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.0;
    tf2::Quaternion qtn; // 四元数
    // setRPY()采用定轴的旋转方式，xyz的顺序，左乘，即：R = R(z)R(y)R(x)
    qtn.setRPY(M_PI/2,M_PI,M_PI/2); // 利用欧拉角初始化四元数
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    broadcaster.sendTransform(ts); // 发布坐标变换信息

除了setRPY()，还有setEuler()、setEulerZYX()。其中，setEuler()是绕着动轴转动，先绕Y轴，再绕变换后的X轴，再绕变换后的Z轴旋转。从数学形式上说，这是YXZ矩阵依次右乘，即：R = R(y)R(x)R(z) 的顺序。setEulerZYX()也是绕动轴旋转，只不过次序为ZYX，矩阵也是右乘，即R = R(z) R(y)R(x)。因此，setEulerZYX()与setRPY()是等价的。

2.getRPY()函数说明

在ROS中，使用getRPY()将旋转矩阵转换为欧拉角，getRPY(double roll, double pitch, double yaw)可以得到定轴转动的roll、pitch、yaw，旋转顺序为xyz。

tf2::Quaternion qtn; // 注意，ROS中四元数默认的顺序是: (x, y, z, w)
// setRPY()采用定轴的旋转方式，xyz的顺序，左乘，即：R = R(z)R(y)R(x)
qtn.setRPY(0.8,0.3,0.4); // 利用欧拉角初始化四元数
double roll, pitch, yaw;
tf2::Matrix3x3(qtn).getRPY(roll, pitch, yaw);  // x，y，z的旋转顺序,定轴转动

cout << "roll is: " << roll << endl;
cout << "pitch is: " << pitch << endl;
cout << "yaw is: " << yaw << endl;

输出结果为：

roll is: 0.8
pitch is: 0.3
yaw is: 0.4

---

三、ROS中的urdf

在ROS中可以使用urdf发布坐标变换，其中旋转部分可以使用欧拉角的形式。urdf 中的 joint 标签可以描述机器人关节之间的相对位姿。其中，旋转部分的rpy属于定轴转动（外旋），旋转顺序为xyz。

<joint name="laser_link_joint" type="fixed"> 
    <parent link="base_link" /> 
    <child link="laser_link" />
    <origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
  </joint>

---

总结

三维刚体变换中，旋转有多种表示形式，相比于旋转矩阵、四元数，欧拉角因不具有唯一性而容易被混淆。由于旋转轴和旋转顺序的不同，一种旋转运动往往对应多种数值的欧拉角。因此，对于欧拉角相关的函数，需要明白该函数的参数含义，是定轴运动还是动轴运动，以及旋转的先后顺序。