四元数基础

目标学习 ROS 2 中使用四元数的基础知识。

辅导水平： 中级

时间 10 分钟

背景介绍 

四元数是方位的 4 元组表示法，比旋转矩阵更简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数被广泛应用于机器人、量子力学、计算机视觉和三维动画等领域。

您可以在以下网站上了解更多有关基本数学概念的信息维基百科.您还可以观看可探索的系列视频可视化四元数由 3blue1brown.

在本教程中，您将了解 ROS 2 中四元数和转换方法的工作原理。

先决条件 

不过，这并不是硬性要求，您可以使用任何其他最适合您的几何转换库。您可以参考以下库 transforms3d, scipy.spatial.transform, pytransform3d, 四元数或 blender.mathutils.

四元数的组成部分 

ROS 2 使用四元数来跟踪和应用旋转。一个四元数有 4 个分量 (x, y, z, w).在 ROS 2、 w 是最后一个，但在某些图书馆，如 Eigen、 w 可以放在第一个位置。常用的单位四元数是 (0, 0, 0, 1)可以通过以下方式创建：

#include <tf2/LinearMath/Quaternion.h>；
...

tf2::四元数 q;
// 用滚动/俯仰/偏航创建一个四元数，单位为弧度 (0, 0, 0)
q.设置 RPY(0, 0, 0);
// 打印四元数分量 (0, 0, 0, 1)
RCLCPP_INFO(此->；get_logger(), "%f%f%f%f"；,
            q.x(), q.y(), q.z(), q.w());

四元数的大小应始终为一。如果数值错误导致四元数的大小不是 1，ROS 2 将发出警告。为避免这些警告，请对四元数进行归一化处理：

q.正常化();

ROS 2 中的四元数类型 

ROS 2 使用两种四元数数据类型： tf2::Quaternion 及其等价物 几何_msgs::msg::四元数.要在 C++ 中进行它们之间的转换，请使用 tf2_geometry_msgs.

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>；
...

tf2::四元数 tf2_quat, tf2_quat_from_msg;
tf2_quat.设置 RPY(滚动, 沥青, 打呵欠);
// 将 tf2::Quaternion 转换为 geometry_msgs::msg::Quaternion
几何参数::信息::四元数 msg_quat = tf2::至信息(tf2_quat);

// 将 geometry_msgs::msg::Quaternion 转换为 tf2::Quaternion
tf2::转化(msg_quat, tf2_quat_from_msg);
// 或
tf2::发件人信息(msg_quat, tf2_quat_from_msg);

Python

从 几何_msgs.msg 舶来品 四元数
...

# 创建与 tf2 兼容的浮点列表
# 四元数方法
quat_tf = [0.0, 1.0, 0.0, 0.0]

# 将列表转换为几何_msgs.msg.四元数
msg_quat = 四元数(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

四元数运算 

1 在 RPY 中思考，然后转换为四元数 

我们很容易想到轴的旋转，但很难想到四元数。一个建议是用滚动（围绕 X 轴）、俯仰（围绕 Y 轴）和偏航（围绕 Z 轴）来计算目标旋转，然后转换成四元数。

# quaternion_from_euler 方法在 turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py 中提供。
q = 来自欧拉的四元数(1.5707, 0, -1.5707)
打印(f'四元数表示为 x： {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

2 应用四元数旋转 

要将一个四元数的旋转应用于一个姿势，只需将姿势的前一个四元数乘以代表所需旋转的四元数即可。乘法的顺序很重要。

C++

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>；
...

tf2::四元数 q_orig, q_rot, q_new;

q_orig.设置 RPY(0.0, 0.0, 0.0);
// 将前一个姿势围绕 X 旋转 180* 。
q_rot.设置 RPY(3.14159, 0.0, 0.0);
q_new = q_rot * q_orig;
q_new.正常化();

Python

q_orig = 来自欧拉的四元数(0, 0, 0)
# 将前一个姿势围绕 X 旋转 180*
q_rot = 来自欧拉的四元数(3.14159, 0, 0)
q_new = 四元数乘法(q_rot, q_orig)

3 反四元数 

反转四元数的简单方法是否定 w 分量：

q[3] = -q[3]

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 获取从上一个机器人姿态到当前机器人姿态的相对旋转的示例：

捍卫 四元数乘法(q0, q1):
    """；
    两个四元数相乘。

    输入
    参数 q0： 包含第一个四元数（q01、q11、q21、q31）的 4 元素数组
    参数 q1： 包含第二个四元数（q02、q12、q22、q32）的 4 元素数组

    输出
    返回包含最终四元数（q03,q13,q23,q33）的 4 元素数组

    """；
    # 从 q0 中提取数值
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # 从 q1 中提取数值
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # 逐项计算两个四元数的乘积
    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

    # 创建一个包含最终四元数的 4 元素数组
    最终四元数 = np.矩阵([q0q1_w, q0q1_x, q0q1_y, q0q1_z])

    # 返回包含最终四元数（q02,q12,q22,q32）的 4 元素数组
    返回 最终四元数

q1_inv[0] = 预置.姿势.方向.x
q1_inv[1] = 预置.姿势.方向.y
q1_inv[2] = 预置.姿势.方向.z
q1_inv[3] = -预置.姿势.方向.w # 否定取反

q2[0] = 当前姿势.姿势.方向.x
q2[1] = 当前姿势.姿势.方向.y
q2[2] = 当前姿势.姿势.方向.z
q2[3] = 当前姿势.姿势.方向.w

qr = 四元数乘法(q2, q1_inv)

摘要 

在本教程中，您将了解到四元数的基本概念及其相关数学运算，如反转和旋转。您还将了解到四元数在 ROS 2 中的使用示例以及两个独立四元数类之间的转换方法。