网站设计是什么意思,优质网站建设,建筑效果图网站有哪些,家电维修品牌网站建设背景
四元数是方向的 4 元组表示形式#xff0c;它比旋转矩阵更简洁。 四元数对于分析涉及三维旋转的情况非常有效。 四元数广泛用于机器人技术、量子力学、计算机视觉和 3D 动画。
您可以在 Wikipedia 上了解有关基本数学概念的更多信息。 您还可以观看由 3blue1brown 制…背景
四元数是方向的 4 元组表示形式它比旋转矩阵更简洁。 四元数对于分析涉及三维旋转的情况非常有效。 四元数广泛用于机器人技术、量子力学、计算机视觉和 3D 动画。
您可以在 Wikipedia 上了解有关基本数学概念的更多信息。 您还可以观看由 3blue1brown 制作的可探索视频系列 Visualizeizing quaternions。
在本教程中您将了解四元数和转换方法在 ROS 2 中的工作原理。
先决条件
但是这不是一个硬性要求您可以坚持使用最适合您的任何其他几何转换库。 您可以查看 transforms3d、scipy.spatial.transform、pytransform3d、numpy-quaternion 或 blender.mathutils 等库。
四元数的组成部分
ROS 2 使用四元数来跟踪和应用旋转。 四元数有 4 个分量。 在 ROS 2 中是最后的但在一些库如 Eigen中可以放在第一个位置。 不产生绕 x/y/z 轴旋转的常用单位四元数是 可以通过以下方式创建(x, y, z, w)ww(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。 如果数值错误导致四元数幅度不是 1ROS 2 将打印警告。 要避免这些警告请规范化四元数 q.normalize();ROS 2 中的四元数类型
ROS 2 使用两种四元数数据类型 及其等效数据类型。 要在 C 中在它们之间进行转换请使用 .tf2::Quaterniongeometry_msgs::msg::Quaterniontf2_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);蟒 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(xquat_tf[0], yquat_tf[1], zquat_tf[2], wquat_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(fThe 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();或 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_1q_2q_rq_1q_2 q_2 q_r * q_1您可以像求解矩阵方程一样进行求解。 反转并右乘两侧。同样乘法的顺序也很重要q_rq_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 q0w0 q0[0]x0 q0[1]y0 q0[2]z0 q0[3]# Extract the values from q1w1 q1[0]x1 q1[1]y1 q1[2]z1 q1[3]# Computer the product of the two quaternions, term by termq0q1_w w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1q0q1_x w0 * x1 x0 * w1 y0 * z1 - z0 * y1q0q1_y w0 * y1 - x0 * z1 y0 * w1 z0 * x1q0q1_z w0 * z1 x0 * y1 - y0 * x1 z0 * w1# Create a 4 element array containing the final quaternionfinal_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_quaternionq1_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 inverseq2[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.wqr quaternion_multiply(q2, q1_inv)总结
在本教程中您了解了四元数的基本概念及其相关的数学运算例如反转和旋转。 您还了解了它在 ROS 2 中的使用示例以及两个单独的 Quaternion 类之间的转换方法。
