四元数基础知识
教程级别： 中级
时间: 10分钟
内容
背景
四元数是表示方向的四元组表示法，比旋转矩阵更简洁。四元数在涉及三维旋转的情况下非常高效。四元数广泛应用于机器人技术、量子力学、计算机视觉和3D动画。
您可以在 维基百科 上了解更多有关底层数学概念的内容。您还可以观看 3blue1brown 制作的可视化四元数视频系列 Visualizing quaternions。
在本教程中，您将学习ROS 2中四元数和转换方法的工作原理。
先决条件
然而，这不是硬性要求，您可以选择最适合您的任何其他几何变换库。您可以查看像 transforms3d、scipy.spatial.transform、pytransform3d、numpy-quaternion 或 blender.mathutils 这样的库。
四元数的组成部分
ROS 2使用四元数来跟踪和应用旋转。一个四元数有四个分量 (x, y, z, w)。在ROS 2中，w 在最后一位，但在一些库中如Eigen中，w 可以放在第一位。常用的单位四元数在x/y/z轴上不产生旋转，表示为 (0, 0, 0, 1)，可以用以下方式创建：
#include <tf2/LinearMath/Quaternion.h>
...
tf2::Quaternion q;
// Create a quaternion from roll/pitch/yaw in radians (0, 0, 0)
q.setRPY(0, 0, 0);
// Print the quaternion components (0, 0, 0, 1)
RCLCPP_INFO(this->get_logger(), "%f %f %f %f",
q.x(), q.y(), q.z(), q.w());
四元数的模应该始终为1。如果数值误差导致四元数模不为1，ROS 2将打印警告。为避免这些警告，请对四元数进行归一化：
q.normalize();
ROS 2中的四元数类型
ROS 2使用两种四元数数据类型：tf2::Quaternion``和它的等效类型``geometry_msgs::msg::Quaternion。在C++中进行类型转换，可以使用``tf2_geometry_msgs``中的方法。
C++
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...
tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);
// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);
Python
from geometry_msgs.msg import Quaternion
...
# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]
# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])
四元数操作
1. 首先按照RPY（滚转、俯仰、偏航）的思维进行计算，然后再转换为四元数。
我们更容易以轴旋转的方式思考，但很难用四元数来思考。一个建议是首先按照滚转（绕X轴）、俯仰（绕Y轴）和偏航（绕Z轴）计算目标旋转，然后再将其转换为四元数。
# quaternion_from_euler method is available in turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py
q = quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')
2 应用四元数旋转
要将一个四元数的旋转应用于姿势，只需将姿势的先前四元数乘以表示所需旋转的四元数。此乘法的顺序很重要。
C++
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...
tf2::Quaternion q_orig, q_rot, q_new;
q_orig.setRPY(0.0, 0.0, 0.0);
// Rotate the previous pose by 180* about X
q_rot.setRPY(3.14159, 0.0, 0.0);
q_new = q_rot * q_orig;
q_new.normalize();
Python
q_orig = quaternion_from_euler(0, 0, 0)
# Rotate the previous pose by 180* about X
q_rot = quaternion_from_euler(3.14159, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)
4 相对旋转
假设您有来自同一帧的两个四元数 q_1 和 q_2。您希望找到相对旋转 q_r，它以以下方式将 q_1 转换为 q_2：
q_2 = q_r * q_1
你可以类似于解矩阵方程来解``q_r``。求``q_1``的逆矩阵，并将两边右乘。再次强调，乘法的顺序很重要：
q_r = q_2 * q_1_inverse
下面是一个示例，用Python计算从之前的机器人姿态到当前机器人姿态的相对旋转：
def quaternion_multiply(q0, q1):
"""
Multiplies two quaternions.
Input
:param q0: A 4 element array containing the first quaternion (q01, q11, q21, q31)
:param q1: A 4 element array containing the second quaternion (q02, q12, q22, q32)
Output
:return: A 4 element array containing the final quaternion (q03,q13,q23,q33)
"""
# Extract the values from q0
w0 = q0[0]
x0 = q0[1]
y0 = q0[2]
z0 = q0[3]
# Extract the values from q1
w1 = q1[0]
x1 = q1[1]
y1 = q1[2]
z1 = q1[3]
# Computer the product of the two quaternions, term by term
q0q1_w = w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1
q0q1_x = w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1
q0q1_y = w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1
q0q1_z = w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1
# Create a 4 element array containing the final quaternion
final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])
# Return a 4 element array containing the final quaternion (q02,q12,q22,q32)
return final_quaternion
q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w # Negate for inverse
q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w
qr = quaternion_multiply(q2, q1_inv)