四元数基金amentals [待校准@8494]
Goal目标: Learn学习ROS 2中四元数用法的基础知识。 [待校准@8495]
Tutorial教程级别: Intermediate中级 [待校准@6713]
时间: 10分钟 [Alyssa@7452]
内容
背景
四元数是方向的4元组表示，比旋转矩阵更简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数广泛应用于机器人学、量子力学、计算机视觉和3D动画。 [待校准@8496]
你可以了解基础数学概念 Wikipedia. You can also take a look at an explorable video series Visualizing quaternions 由 3blue1brown 。 [待校准@8497]
在本教程中，您将学习四元数和转换方法如何在ROS 2中工作。 [待校准@8498]
先决条件
对于本教程，您可能需要安装 tf_transformations ROS 2包，以遵循Python代码部分。您可以找到 tf_transformations 包 here 的源文件代码。 [待校准@8499]
然而，这不是一个硬性要求，你可以坚持任何其他最适合你的几何转换库。你可以看看像 transforms3d, scipy.spatial.transform, pytransform3d 、 numpy-quaternion or blender.mathutils 这样的图书馆。 [待校准@8500]
四元数的组成部分 [待校准@8501]
ROS 2使用四元数来跟踪和应用旋转。四元数有4个分量 “(x，y，z，w)”。在ROS 2中， w 是最后一个，但是在一些库中，如了海， w 可以放在第一个位置。在x/y/z轴上不产生旋转的常用单位四元数是 ''(0,0，0,1)''，可以通过以下方式创建: [待校准@8502]
#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将打印警告。要避免这些警告，请标准化四元数: [待校准@8503]
q.normalize();
ROS 2中的四元数类型 [待校准@8504]
ROS 2使用两个四元数数据类型: “tf2:: 四元数 `` and its equivalent `` 几何 _ msgs::msg:: 四元数”。要在c ++ 中转换它们，请使用 tf2_geometry_msgs 的方法。 [待校准@8505]
C++ [待校准@7215]
#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);
蟒蛇 [待校准@7127]
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])
四元数运算 [待校准@8506]
1在RPY中思考，然后转换为四元数 [待校准@8507]
我们很容易想到轴的旋转，但很难想到四元数。一个建议是根据滚动 (关于x轴) 、间距 (关于y轴) 和偏航 (关于z轴) 来计算目标旋转，然后转换为四元数。 [待校准@8508]
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]}.')
2应用四元数旋转 [待校准@8509]
要将一个四元数的旋转应用于一个姿势，只需将该姿势的前一个四元数乘以表示所需旋转的四元数。这个乘法的顺序很重要。 [待校准@8510]
C++ [待校准@7215]
#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();
蟒蛇 [待校准@7127]
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)
4相对旋转 [待校准@8513]
假设你有两个来自同一帧的四元数， q_1 和 q_2 。你想要找到相对旋转， q_r ，它通过以下方式将 q_1 转化为 q_2 : [待校准@8514]
q_2 = q_r * q_1
你可以像求解矩阵方程一样求解 q_r 。反转 q_1 ，右边-两边相乘。同样，乘法的顺序很重要: [待校准@8515]
q_r = q_2 * q_1_inverse
以下是在python中获取从先前机器人姿势到当前机器人姿势的相对旋转的示例: [待校准@8516]
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)
总结
在本教程中，您学习了四元数的基金ament的概念及其相关的数学运算，如反转和旋转。您还了解了它在ROS 2中的使用示例以及两个单独的四元数类之间的转换方法。 [待校准@8517]