网站网络建设,网站有效内容的宣传及推广,可以在线观看的免费资源,十大国际展览公司四元数
简介
在数学上#xff0c;四元数一般表示为q w xi yj zk,其中w是实部#xff0c;x、y、z是虚部对应旋转轴上的分量#xff08;i j k ijk -1#xff09;。
在ROS2中#xff0c;四元数用来跟踪和旋转#xff0c;表示为(x, y, z, w)#xff0c;实部w放在…四元数
简介
在数学上四元数一般表示为q w xi yj zk,其中w是实部x、y、z是虚部对应旋转轴上的分量i² j² k² ijk -1。
在ROS2中四元数用来跟踪和旋转表示为(x, y, z, w)实部w放在了最后但是在其他的一些库(比如Eigen)中遵从数学定义将w放在了第一位。
一个通用的单元四元数(0, 0, 0, 1)代表不围绕三轴旋转我们可用利用如下代码去创建这样的四元数。
#include tf2/LinearMath/Quaternion.h
...tf2::Quaternion q;
// 翻滚/俯仰/偏航 弧度都为0
q.setRPY(0, 0, 0);
// print出来的四元数为 (0, 0, 0, 1)
RCLCPP_INFO(this-get_logger(), %f %f %f %f, q.x(), q.y(), q.z(), q.w());四元数的模大小应该始终为1。否则ROS2将会打印警告信息。因此我们需要对四元数像下面这样进行归一化处理。
q.normalize();四元数类型
ROS2中的两种四元数数据类型tf2::Quaternion和geometry_msgs::msg::Quaternion二者是等价的。可以利用tf2_geometry_msgs进行转换。
#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);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);USE
从RPY转换到四元数
如果要绕x轴正转90°并绕z轴逆转的操作对于RPY(Roll-x, Pitch-y,Yaw-z)的形式我们很容易就能写出(1.57070-1.5707),那么四元数各个组成的值该如何表示我们可以利用API将RPY(欧拉形式)转换成四元数形式。
# 在turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py中使用quaternion_from_euler方法
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]}.)应用四元数旋转
要将一个四元数的旋转应用于位姿只需将位姿的前一个四元数乘以表示所需旋转的四元数即可。(这个乘法运算的顺序很重要)。
#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();逆转四元数
逆转四元数的一种简单方法是对w分量求反
q[3] -q[3]相对旋转
假如我们有从同一个坐标系(帧)下的四元数q_1和q_2,我们定义一个四元数q_r用来表示从q_1转换到q_2的转换因子我们可以这样表示
q_2 q_r * q_1再利用矩阵逆运算的相关属性来求出q_r
q_r q_2 * q_1_inverse下面是一个获取从机器人的前一个位姿旋转到机器人当前位姿的相对旋转四元数操作
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)
