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

大家好，欢迎来到IT知识分享网。

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());