ROS2与tf2入门教程-四元数基础

ROS2与tf2入门教程-四元数基础

说明：

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

背景

• 四元数是方向的四元组表示，比旋转矩阵更简洁。
• 四元数对于分析涉及三维旋转的情况非常有效。
• 四元数广泛用于机器人、量子力学、计算机视觉和 3D 动画。
• 您还可以观看由 3blue1brown 制作的可探索视频系列 Visualizing quaternions。

要求：

• 对于本教程，您可能需要安装 tf_transformations ROS 2 包以遵循 Python 代码部分。

• 您可以在此处找到 tf_transformations 包的源代码。

• 但是，这不是硬性要求，您可以坚持使用最适合您的任何其他几何转换库。

• 您可以查看像这样的库

• transforms3d

• scipy.spatial.transform

• pytransform3d

• numpy-quaternion

• blender.mathutils

组成：

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

• 四元数的大小应始终为 1。
• 如果数值错误导致四元数幅度不是 1，ROS 2 将打印警告。
• 为了避免这些警告，规范化四元数：

q.normalize();

四元数类型：

• 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])

四元数运算：

• 欧拉角转换为四元数
• 我们很容易想到围绕轴的旋转，但很难想到四元数。
• 一个建议是根据滚动（关于 X 轴）、俯仰（关于 Y 轴）和偏航（关于 Z 轴）计算目标旋转，然后转换为四元数。

import tf_transformations
...

q = tf_transformations.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]}.')

• 使用四元数
• 要将一个四元数的旋转应用于姿势，只需将姿势的前一个四元数乘以表示所需旋转的四元数。
• 这种乘法的顺序很重要。
• 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用法

import tf_transformations
...

q_orig = tf_transformations.quaternion_from_euler(0, 0, 0)
# Rotate the previous pose by 180* about X
q_rot = tf_transformations.quaternion_from_euler(3.14159, 0, 0)
q_new = tf_transformations.quaternion_multiply(q_rot, q_orig)

• 反转四元数
• 反转四元数的一种简单方法是取反 w 分量：

q[3] = -q[3]

• 相对旋转
• 假设您有来自同一坐标系的两个四元数 q_1 和 q_2。
• 您想找到以下列方式将 q_1 转换为 q_2 的相对旋转 q_r：

q_2 = q_r * q_1

• 您可以求解 q r 类似于求解矩阵方程。
• 逆 q_1 并对两边进行右乘。 同样，乘法的顺序很重要：

q_r = q_2 * q_1_inverse

• 这是一个在 python 中获取从前一个机器人姿势到当前机器人姿势的相对旋转的示例：

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 = tf_transformations.quaternion_multiply(q2, q1_inv)

总结：

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