前言

        四元数是一个表示方向的四元组，它比旋转矩阵更为简洁。四元数对于分析涉及三维旋转的情况非常有效。四元数在机器人技术、量子力学、计算机视觉和3D动画中得到了广泛的应用。

        关于四元数的相关知识网上资料很多，比如维基百科，还有由3blue1brown制作的Visualizing quaternions视频，这些能帮助我们加深理解。

        但我们只需了解一些基本即可（除非有更深入的需求及应用场景），所以此节不会深入介绍四元数，而是去学习如何在ROS 2中去使用（转换）。

        目前关于几何转换相关的库有transforms3d, scipy.spatial.transform, pytransform3d, numpy-quaternion 和 blender.mathutils这些，我们可以使用我们熟悉的即可。

四元数组成

        在数学上，四元数一般表示为：q = w + xi + yj + zk,其中w是实部，x、y、z是虚部对应旋转轴上的分量（i² = j² = k² = ijk = -1）。

        在ROS 2中，四元数用来跟踪和旋转，表示为（x, y, z, w），实部w放在了最后，但是在其他的一些库（比如Eigen）中，w放在了第一位，如同数学定义中的那样。

        一个通用的单元四元数，（0, 0, 0, 1），代表不围绕x/y/z轴旋转，我们可用利用下面的代码去创建这样的单元四元数：

#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中四元数的数据类型

        ROS 2中使用了两种四元数数据类型：tf2::Quaternion和geometry_msgs::msg::Quaternion，二者是等价的。只不过在使用时需要利用tf2_geometry_msgs方法（C++）对它们进行转换。

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

四元数操作

从RPY转换到四元数

        比如绕x/y/z轴旋转的情景，我们是很容易想象并理解的，但对于四元素却似有力有不逮之感。如果要绕x轴正转90°并绕z轴逆转的操作，对于RPY（Roll-x, Pitch-y,Yaw-z）的形式我们很容易就能写出（1.5707，0，-1.5707）,那么四元数各个组成的值该如何表示？日常操作中对于四元数的表示，我们可以先将RPY的形式写出来，然后利用API转换成四元数形式就好了，也是比较方便的，如下就是将欧拉形式（RPY）的表示转换成四元数。

# 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(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

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)

逆转四元数

        逆转四元数的一种简单方法是对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)

本篇完。