ROS2教程(12) - 四元数 - Linux

四元数

简介

在数学上，四元数一般表示为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.5707，0，-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(f'The 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 q0
    w0 = q0[0]
    x0 = q0[1]
    y0 = q0[2]
    z0 = q0[3]

    # Extract the values from q1
    w1 = q1[0]
    x1 = q1[1]
    y1 = q1[2]
    z1 = q1[3]

    # Computer the product of the two quaternions, term by term
    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

    # Create a 4 element array containing the final quaternion
    final_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_quaternion

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