如何将四元数转换为旋转矩阵

什么是四元数？

四元数是表示物体在三维空间中的方向和旋转的几种数学方法之一。另一种方法是使用基于欧拉角的旋转矩阵，即滚动、俯仰和偏航，就像的封面图片。

通常使用四元数代替欧拉角旋转矩阵，因为“与 旋转矩阵相比 ，它们更紧凑、 数值更稳定且更高效”（来源：维基百科）。

请注意，四元数仅描述坐标系（即 3D 空间中的某个对象）绕任意轴的旋转，但它不会告诉您有关该对象位置的任何信息。

四元数在机器人中的应用

四元数是ROS中表示方向和旋转的默认方法，ROS 是最流行的机器人软件开发平台。

在机器人技术中，我们总是试图旋转东西。例如，我们可能会在相机中观察一个物体。为了让机械臂抓取物体，我们需要将相机参考系旋转到机器人参考系，以便机器人“知道”物体在自己的坐标系中的位置。

一旦从相机像素坐标到机器人基架坐标的旋转完成，机械臂就可以将其电机移动到适当的角度来拾取物体。

如何表示四元数

四元数是复数的扩展。然而，我们有四个值（a、b、c、d），而不是表示点（或向量）的两个值（例如a + b i 或 x + yi …相同的东西）：

q = a + b i + c j + d k

将点 (a, b) 可视化为二维 Argand 图上的复数。

四元数中的四个值由一个标量和一个三元素单位向量组成。

您通常会看到：而不是 a、b、c 和 d：

q = w + x i + y j + z k 或q = q 0 + q 1 i + q 2 j + q 3 k

• q 0是表示旋转角度的标量值
• q 1、q 2和q 3对应于围绕其执行旋转角度的旋转轴。

编写四元数的其他方法如下：

• q = (q 0 , q 1 , q 2 , q 3 )
• q = ( q 0 , q ) = q 0 + q

四元数最酷的一点是它们的工作原理就像复数一样。在二维中，你可以使用复数乘法旋转向量。你可以对四元数执行相同的操作。数学更复杂，有四个项点而不是两个，但原理是相同的。

让我们看一个复数乘法的二维示例，以便你了解乘以虚数（复数）来旋转向量的概念。四元数添加了更多变量来扩展此概念以表示 3D 空间中的旋转。

2D 示例

假设我们在 2D 平面上有一个具有以下规格的向量：(x = 3, y = 1)。

该向量可以用复数表示为：

3 + i （例如使用复数的 x +yi 形式）

让我们将该向量旋转 45 度（即π /4，以弧度表示）。

要旋转 45 度，我们将该数字乘以：

cos( π /4) + sin( π /4)i （德莫弗公式）

因此，我们有 sqrt 的意思（“取平方根”）：

(1/sqrt(2)+ i/sqrt(2)) * (3 + i) = sqrt(2) + 2sqrt(2)i

由于：

sqrt(2) = 1.414

我们的新向量是：

（x = 1.414，y = 4.242）

正如我之前提到的，将实数四元数相乘的数学比这更复杂，但原理是相同的。将方向（表示为四元数）乘以旋转（表示为四元数）以获得新方向。

将四元数转换为旋转矩阵

给定一个四元数，可以使用以下公式找到相应的三维旋转矩阵。

资料来源：JB Kuipers 的《四元数和旋转序列：轨道、航空航天和虚拟现实应用入门》 （第 5 章，第 5.14 节“四元数到矩阵”，第 125 页）

Python代码

import numpy as np
 
def quaternion_rotation_matrix(Q):
    """
    Covert a quaternion into a full three-dimensional rotation matrix.
 
    Input
    :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) 
 
    Output
    :return: A 3x3 element matrix representing the full 3D rotation matrix. 
             This rotation matrix converts a point in the local reference 
             frame to a point in the global reference frame.
    """
    # Extract the values from Q
    q0 = Q[0]
    q1 = Q[1]
    q2 = Q[2]
    q3 = Q[3]
     
    # First row of the rotation matrix
    r00 = 2 * (q0 * q0 + q1 * q1) - 1
    r01 = 2 * (q1 * q2 - q0 * q3)
    r02 = 2 * (q1 * q3 + q0 * q2)
     
    # Second row of the rotation matrix
    r10 = 2 * (q1 * q2 + q0 * q3)
    r11 = 2 * (q0 * q0 + q2 * q2) - 1
    r12 = 2 * (q2 * q3 - q0 * q1)
     
    # Third row of the rotation matrix
    r20 = 2 * (q1 * q3 - q0 * q2)
    r21 = 2 * (q2 * q3 + q0 * q1)
    r22 = 2 * (q0 * q0 + q3 * q3) - 1
     
    # 3x3 rotation matrix
    rot_matrix = np.array([[r00, r01, r02],
                           [r10, r11, r12],
                           [r20, r21, r22]])
                            
    return rot_matrix