ROS 中使用 C/C++ 实现四元数（xyzw）和欧拉角（roll, pitch, yaw）的转换

Quaternion - 四元数，Euler angles - 欧拉角，Roll - 侧倾/翻滚，Pitch - 俯仰，Yaw - 横摆/偏航

基础知识

四元数

围绕一个固定点，任何三维旋转以及三维旋转的叠加都等价于：围绕穿过该固定点的某一轴线 $\mathbf{e}$，转过一个角度 $\theta$，如下图所示。

四元数就是针对绕轴旋转的一种空间旋转表达方式，所谓四元即四个变量：xyzw，其中包含一个向量和一个标量，即 $w+x\mathbf{i}+y\mathbf{j}+z\mathbf{k}$，在已知轴线 $e_x\mathbf{i}+e_y\mathbf{j}+e_z\mathbf{k}$ 和转过的角度 $\theta$ 时，可以计算对应的四元数 $\mathbf{q}$：

$$\mathbf{q}=\cos \frac{\theta }{2}+\left(e_x\mathbf{i}+e_y\mathbf{j}+e_z\mathbf{k}\right)\sin \frac{\theta }{2}=\cos \frac{\theta }{2}+\mathbf{e}\sin \frac{\theta }{2}$$

四元数的特点：

• 多个四元数表示的绕轴旋转可以通过相乘叠加，易于计算
• 相比欧拉角的绕 XYZ 轴旋转而言，四元数较难理解

欧拉角

包括侧倾角（Roll），俯仰角（Pitch）和横摆角（Yaw），如下图所示。

C/C++ 转换

四元数转欧拉角

// 引入 tf2::Matrix3x3 和 tf2::Quaternion
#include <tf2/LinearMath/Matrix3x3.h>

// 首先赋值待转换的四元数
tf2::Quaternion myQuaternion(0, 0, 0, 1);
//使用 Matrix3x3 工具，其构造函数可以直接输入四元数
tf2::Matrix3x3 myMat(myQuaternion);

// 定义待赋值的 RPY 变量，使用 getRPY 对其赋值
double roll, pitch, yaw;
myMat.getRPY(&roll, &pitch, &yaw);

欧拉角转四元数

// 引入 tf2::Quaternion
#include <tf2/LinearMath/Quaternion.h>
// 引入 M_PI
#define _USE_MATH_DEFINES
#include <math.h>

tf2::Quaternion myQuaternion;
// setRPY 的输入为弧度值的 RPY
// 例如下面表示绕 Z 轴旋转 90 度
myQuaternion.setRPY(0, 0,  M_PI / 2.0);

// 使用 .get?() 访问对应的四元数
ROS_INFO_STREAM("x: " << myQuaternion.getX() << " y: " << myQuaternion.getY() << 
                " z: " << myQuaternion.getZ() << " w: " << myQuaternion.getW());