【ROS2】中级:tf2-四元数基础知识

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

教程级别：中级

 时间：10 分钟

 目录

•  背景

•  先决条件

• 四元数的组成部分

• ROS 2 中的四元数类型

•  四元数运算

  • 1. 首先在 RPY 中思考，然后转换为四元数

  • 2 应用四元数旋转

  • 3 反转四元数

  •  4 相对旋转

•  摘要

 背景

四元数是一种表示方向的四元组，比旋转矩阵更简洁。四元数在分析涉及三维旋转的情况时非常高效。四元数广泛应用于机器人学、量子力学、计算机视觉和 3D 动画。

您可以在维基百科上 https://en.wikipedia.org/wiki/Quaternion 了解更多关于基础数学概念的信息。您还可以观看由 3blue1brown https://www.youtube.com/3blue1brown  制作的可视化四元数 https://eater.net/quaternions 视频系列。

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

 先决条件

然而，这不是一个硬性要求，您可以坚持使用任何其他最适合您的几何变换库。您可以查看诸如 transforms3d、scipy.spatial.transform、pytransform3d、numpy-quaternion 或 blender.mathutils 之类的库。

https://github.com/matthew-brett/transforms3d

https://github.com/scipy/scipy/tree/main/scipy/spatial/transform

https://github.com/dfki-ric/pytransform3d

https://github.com/moble/quaternion

https://docs.blender.org/api/master/mathutils.html

四元数的组成部分

ROS 2 使用四元数来跟踪和应用旋转。一个四元数有 4 个分量 (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());

四元数的大小应始终为一。如果数值误差导致四元数大小不为一，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);
// 将 tf2::Quaternion 转换为 geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);


// 将 geometry_msgs::msg::Quaternion 转换为 tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// 或者
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python

from geometry_msgs.msg import Quaternion
...


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


# 将列表转换为 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); // 设置初始姿态的四元数
// 将前一个姿态绕X轴旋转180度
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)

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 中获取从先前机器人姿态到当前机器人姿态的相对旋转：

import numpy as np


def quaternion_multiply(q0, q1):
    """
    乘两个四元数。


    输入
    :param q0: 一个包含第一个四元数的4元素数组 (q0w, q0x, q0y, q0z)
    :param q1: 一个包含第二个四元数的4元素数组 (q1w, q1x, q1y, q1z)


    输出
    :return: 一个包含最终四元数的4元素数组 (qw, qx, qy, qz)


    """
    # 从 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元素数组
    final_quaternion = np.array([q0q1_w, q0q1_x, q0q1_y, q0q1_z])


    # 返回一个包含最终四元数的4元素数组 (qw, qx, qy, qz)
    return final_quaternion


# 获取前一个姿态的四元数的逆（共轭）或者单w部取反
q1_inv = [0, 0, 0, 0]
q1_inv[0] = prev_pose.pose.orientation.w
q1_inv[1] = -prev_pose.pose.orientation.x
q1_inv[2] = -prev_pose.pose.orientation.y
q1_inv[3] = -prev_pose.pose.orientation.z # 取负以求逆


# 获取当前姿态的四元数
q2 = [0, 0, 0, 0]
q2[0] = current_pose.pose.orientation.w
q2[1] = current_pose.pose.orientation.x
q2[2] = current_pose.pose.orientation.y
q2[3] = current_pose.pose.orientation.z


# 计算最终的四元数
qr = quaternion_multiply(q2, q1_inv)

 摘要

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