Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter3(原创系列教程)（最关键一章）

Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter3(原创系列教程)（最关键一章）

本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 fanzexuan135@163.com

首先说明这章学完你就具备一个高级机器人研究人员的关键知识了，满满的精华，请静心吸收，用心学习。

Chapter3 扰动、导数和积分（Perturbations, derivatives and integrals）

在许多实际应用中,我们需要分析四元数表示的旋转在空间和时间上的变化。这就涉及到了扰动(perturbation)、导数(derivative)、积分(integral)等概念。本章我们将探讨如何在四元数的框架下定义和计算这些量,并给出一些常用的公式和性质。

3.1 四元数的不同形式（Quaternion flavors）

在前面的章节中,我们系统地介绍了四元数的基本概念、运算法则、旋转表示以及几何解释。然而,在实际应用中,我们经常会遇到不同的四元数定义和约定,这可能会导致一些混淆和错误。本节我们将讨论四元数的不同约定,并明确本文所采用的选择。

四元数及其应用的注意事项

1. 四元数元素的顺序

四元数可以表示为 $\mathbf{q}=q_w+q_{x}i+q_{y}j+q_{z}k$。在计算中，我们可以选择不同的顺序来存储四元数的元素：

• $\mathbf{q}=(q_w,q_x,q_y,q_z)$
• $\mathbf{q}=(q_x,q_y,q_z,q_w)$

在前者中，实部（$q_w$）在第一个位置，称为"实部在前"的表示法。在后者中，实部在最后一个位置，称为"实部在后"的表示法。

2. 四元数乘法的定义

四元数乘法依赖于四元数单位 $i,j,k$ 之间的乘法关系：

• $i^2=j^2=k^2=ijk=-1$
• $ij=k$, $jk=i$, $ki=j$
• $ji=-k$, $kj=-i$, $ik=-j$

这里的差别在于 $ij$ 和 $ji$ 的顺序。四元数乘法是非交换的，这意味着 $ij\ne ji$。

$ij$ 和 $ji$ 的顺序根据定义的右手系和左手系的不同而不同：

• 右手系：符合右手法则，如果你用右手握住一个坐标轴，拇指指向正方向，其他四指的方向就是旋转方向。
• 左手系：符合左手法则，如果你用左手握住一个坐标轴，拇指指向正方向，其他四指的方向就是旋转方向。

3. 旋转算子的作用

旋转算子可以应用于旋转坐标系（主动旋转）或旋转向量（被动旋转）：

• 主动旋转（Active Rotation）：旋转坐标系变化而保持向量不变。
• 被动旋转（Passive Rotation）：旋转向量变化而保持坐标系不变。

4. 四元数旋转的方向

四元数用于3D旋转时，可以通过四元数的左乘或右乘实现旋转：

1. 右乘（Right Multiplication）：

$${\mathbf{v}}^{\prime }=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$$

2. 左乘（Left Multiplication）：

$${\mathbf{v}}^{\prime }={\mathbf{q}}^{\ast }\otimes \mathbf{v}\otimes \mathbf{q}$$

其中：

• $\mathbf{q}$ 是用于旋转的四元数。
• ${\mathbf{q}}^{\ast }$ 是 $\mathbf{q}$ 的共轭。
• $\mathbf{v}$ 是表示为纯四元数的向量，即 $\mathbf{v}=0+v_{x}i+v_{y}j+v_{z}k$。

右乘和左乘的几何意义

右乘（Right Multiplication）

在右乘公式中：

$${\mathbf{v}}^{\prime }=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$$

这种方式的意义是使用四元数 $\mathbf{q}$ 将向量 $\mathbf{v}$ 进行旋转。具体过程如下：

1. 向量 $\mathbf{v}$ 先与四元数 $\mathbf{q}$ 相乘。
2. 再与四元数 $\mathbf{q}$ 的共轭 ${\mathbf{q}}^{\ast }$ 相乘。

这种旋转方式常用于主动旋转（Active Rotation），即旋转坐标系而保持向量不变。

左乘（Left Multiplication）

在左乘公式中：

$${\mathbf{v}}^{\prime }={\mathbf{q}}^{\ast }\otimes \mathbf{v}\otimes \mathbf{q}$$

这种方式的意义是使用四元数 $\mathbf{q}$ 的共轭 ${\mathbf{q}}^{\ast }$ 将向量 $\mathbf{v}$ 进行旋转。具体过程如下：

1. 向量 $\mathbf{v}$ 先与四元数 ${\mathbf{q}}^{\ast }$ 相乘。
2. 再与四元数 $\mathbf{q}$ 相乘。

这种旋转方式常用于被动旋转（Passive Rotation），即旋转向量而保持坐标系不变。

应用背景和区别

• 主动旋转（Active Rotation）：

  • 使用右乘 ${\mathbf{v}}^{\prime }=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$。
  • 应用于旋转坐标系，这在机器人学和计算机图形学中很常见。
  • 例如，当你想旋转一个对象的参考坐标系时使用这种方式。

• 被动旋转（Passive Rotation）：

  • 使用左乘 ${\mathbf{v}}^{\prime }={\mathbf{q}}^{\ast }\otimes \mathbf{v}\otimes \mathbf{q}$。
  • 应用于旋转向量，这在姿态控制和航空航天中很常见。
  • 例如，当你想将一个物体的姿态描述相对于某个固定坐标系时使用这种方式。

选择不同的四元数表示和乘法定义主要依赖于应用的需要和标准。理解主动旋转和被动旋转、以及右乘和左乘的区别，这些概念和定义对于使用四元数进行3D旋转非常重要。理解这些基础知识可以帮助你在实际应用中做出正确的选择。

这四个方面可以组合出 16 种不同的四元数约定。在实践中,最常用的有两种约定:Hamilton 约定和 JPL 约定。它们的区别如下:

方面	Hamilton 约定	JPL 约定
元素顺序	$(q_w,q_x,q_y,q_z)$	$(q_x,q_y,q_z,q_w)$
乘法定义	$ij=k$ (右手系)	$ji=k$ (左手系)
旋转作用	旋转坐标系	旋转坐标系
旋转方向	右乘	左乘

可以看到,Hamilton 约定和 JPL 约定在元素顺序和乘法定义上正好相反,而在旋转作用和方向上则保持一致。这两种约定在航天、机器人等领域都有广泛应用,选择哪一种主要取决于个人习惯和应用场景。

需要注意的是,不同约定之间的转换并不是简单地改变元素顺序或乘法符号就可以的,它们在旋转矩阵、指数映射、导数、积分等方面都有细微的差别。因此,在使用四元数时,我们必须明确说明所采用的约定,并在整个系统中保持一致。

在本文中,选择使用 Hamilton 约定 即:

1. 四元数的元素顺序为 $(q_w,q_x,q_y,q_z)$,实部在前。

2. 四元数乘法满足 $ij=k$,对应于右手系。

3. 四元数表示坐标系的旋转,而不是向量的旋转。

4. 四元数旋转通过右乘实现,即 ${\mathbf{v}}^{\prime }=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$。

这个选择主要是出于以下考虑:

1. Hamilton 约定在数学和物理学界更为常用,也是四元数的原始定义。它与复数的表示更为一致,便于理解和推导。

2. 右手系是三维空间中的常用约定,与向量叉乘、旋转矩阵等概念更为协调。

3. 坐标系的旋转在几何学和力学中更为常见,也更符合直觉。它与欧拉角、矩阵表示等方法可以直接对应。

4. 右乘的旋转方向与矩阵乘法的顺序一致,便于记忆和推导。

下面是一个 Python 例子,展示了如何在 Hamilton 约定下进行四元数的基本运算:

import numpy as np

def quaternion_multiply(q1, q2):
    """
    Multiply two quaternions following Hamilton convention.
    
    Parameters:
        q1 (numpy.ndarray): First quaternion (w, x, y, z).
        q2 (numpy.ndarray): Second quaternion (w, x, y, z).
        
    Returns:
        numpy.ndarray: Quaternion product (w, x, y, z).
    """
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2 
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    
    return np.array([w, x, y, z])

def quaternion_conjugate(q):
    """
    Compute the conjugate of a quaternion.
    
    Parameters:
        q (numpy.ndarray): Quaternion (w, x, y, z).
        
    Returns:
        numpy.ndarray: Conjugate quaternion (w, -x, -y, -z).
    """
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quaternion_rotate(v, q):
    """
    Rotate a vector by a quaternion following Hamilton convention.
    
    Parameters:
        v (numpy.ndarray): 3D vector to be rotated.
        q (numpy.ndarray): Unit quaternion (w, x, y, z) representing the rotation.
        
    Returns:
        numpy.ndarray: Rotated vector.
    """
    q_conj = quaternion_conjugate(q)
    v_quat = np.concatenate(([0], v))
    
    v_rot_quat = quaternion_multiply(quaternion_multiply(q, v_quat), q_conj)
    return v_rot_quat[1:]

# Example usage
v = np.array([1, 0, 0])
q = np.array([np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)])

v_rot = quaternion_rotate(v, q)
print(v_rot)
# Output: [0.70710678 0.         0.70710678]

在这个例子中,我们定义了三个函数:

1. quaternion_multiply 实现了 Hamilton 约定下的四元数乘法。
2. quaternion_conjugate 计算了四元数的共轭。
3. quaternion_rotate 使用四元数对向量进行旋转,遵循右乘的约定。

在主程序中,我们将向量 $\mathbf{v}=(1,0,0)$ 绕 $z$ 轴旋转 $45^{\circ }$,得到旋转后的向量 $(0.7071,0,0.7071)$,与预期结果一致。

通过这个例子,我们看到了如何在 Hamilton 约定下进行四元数运算。

3.2 SO(3)中的加法和减法运算（The additive and subtractive operators in SO3）

我们知道,在欧氏空间(例如二维平面或三维空间)中,我们可以对向量进行加法和减法运算。例如,给定两个向量 $\mathbf{a}$ 和 $\mathbf{b}$,我们可以计算它们的和 $\mathbf{a}+\mathbf{b}$,得到一个新的向量。

然而,旋转群 $SO(3)$ 并不是一个向量空间,它的元素是旋转矩阵,而不是向量。这意味着我们不能直接用普通的加法和减法来操作旋转。那么,我们如何在旋转群上定义类似的运算呢?

这就是流形和切空间的概念发挥作用的地方。流形可以看作是一个在局部与欧氏空间相似的空间。例如,地球表面在局部可以看作是平的,但在全局却是一个球面。旋转群 $SO(3)$ 也是一个流形,它在局部可以用欧氏空间来近似。

切空间是与流形上的每个点相关联的一个向量空间。它可以看作是流形在该点的最佳线性近似。对于旋转群 $SO(3)$,其切空间恰好是一个三维向量空间,我们可以用它来表示旋转的微小变化。

现在,我们可以利用切空间来定义旋转群上的 “加法” 运算。给定一个旋转 $R$ 和一个微小的旋转变化 $\mathbf{\theta }$ (这个 $\mathbf{\theta }$ 属于切空间,因此是一个三维向量),我们可以将 $\mathbf{\theta }$ “添加” 到 $R$ 上,得到一个新的旋转 $S$。这个过程可以写成:

$S=R\oplus \mathbf{\theta }$

其中 $\oplus$ 表示我们新定义的 “加法” 运算。这个运算通过指数映射 $\mathrm{Exp}$ 将切空间中的向量 $\mathbf{\theta }$ 映射到旋转群 $SO(3)$ 上,然后与原来的旋转 $R$ 复合,得到新的旋转 $S$。

总之,虽然我们不能在旋转群 $SO(3)$ 上直接使用普通的加法,但我们可以利用它的流形结构,在其切空间上定义一种等价的 “加法” 运算。这种运算允许我们将微小的旋转变化 “叠加” 到现有的旋转上,得到新的旋转。这个概念在分析和优化旋转问题时非常有用。

具体来说,给定 $SO(3)$ 中的一个旋转 $R$ 和其切空间 $T_{R}SO(3)$ 中的一个微小扰动 $\mathbf{\theta }\in {\mathbb{R}}^3$,我们定义加法运算 $\oplus :SO(3)\times {\mathbb{R}}^3\to SO(3)$ 为:

$S=R\oplus \mathbf{\theta }=R\mathrm{Exp}(\mathbf{\theta })$

其中 $S$ 是扰动后的新旋转,$\mathrm{Exp}$ 是指数映射。这个运算将切空间中的微小扰动 “叠加” 到原来的旋转上,得到新的旋转。

类似地,我们定义 $SO(3)$ 上的减法运算 $\ominus :SO(3)\times SO(3)\to {\mathbb{R}}^3$ 为:

$\mathbf{\theta }=S\ominus R=\mathrm{Log}(R^{-1}S)$

其中 $\mathrm{Log}$ 是对数映射。这个运算计算了两个旋转之间的 “差异”,得到切空间中的扰动向量。

需要注意的是,这里的 $\mathbf{\theta }$ 是相对于参考旋转 $R$ 定义的。换句话说,加法和减法运算都依赖于一个参考点,在不同的点上进行相同的运算可能得到不同的结果。这反映了 $SO(3)$ 作为流形的本质特性。

下面是 Python 中实现 $SO(3)$ 加减法的代码:

import numpy as np

def so3_exp(theta):
    """
    Exponential map from so(3) to SO(3).
    
    Parameters:
        theta (numpy.ndarray): Rotation vector (3,).
        
    Returns:
        numpy.ndarray: Rotation matrix (3, 3).
    """
    theta_norm = np.linalg.norm(theta)
    if theta_norm < 1e-8:
        return np.eye(3)
    
    theta_hat = np.array([[0, -theta[2], theta[1]],
                          [theta[2], 0, -theta[0]],
                          [-theta[1], theta[0], 0]])
    
    return np.eye(3) + np.sin(theta_norm)/theta_norm*theta_hat + (1-np.cos(theta_norm))/theta_norm**2*theta_hat@theta_hat

def so3_log(R):
    """
    Logarithmic map from SO(3) to so(3).
    
    Parameters:
        R (numpy.ndarray): Rotation matrix (3, 3).
        
    Returns:
        numpy.ndarray: Rotation vector (3,).
    """
    theta = np.arccos((np.trace(R) - 1) / 2)
    if np.abs(theta) < 1e-8:
        return np.zeros(3)
    
    return theta / (2*np.sin(theta)) * np.array([R[2, 1]-R[1, 2], R[0, 2]-R[2, 0], R[1, 0]-R[0, 1]])

def so3_add(R, theta):
    """
    Add a perturbation to a rotation matrix.
    
    Parameters:
        R (numpy.ndarray): Rotation matrix (3, 3).
        theta (numpy.ndarray): Rotation vector (3,).
        
    Returns:
        numpy.ndarray: Perturbed rotation matrix (3, 3).
    """
    return R @ so3_exp(theta)

def so3_subtract(R1, R2):
    """
    Subtract two rotation matrices.
    
    Parameters:
        R1 (numpy.ndarray): First rotation matrix (3, 3).
        R2 (numpy.ndarray): Second rotation matrix (3, 3).
        
    Returns:
        numpy.ndarray: Rotation vector (3,).
    """
    return so3_log(R1.T @ R2)

# Example usage
R1 = so3_exp(np.array([0, 0, np.pi/4]))
R2 = so3_exp(np.array([0, 0, np.pi/3]))

R_add = so3_add(R1, np.array([0, 0, np.pi/6]))
print(R_add)
# Output:
# [[0.5       0.5       0.7071068]
#  [0.5       0.5       0.7071068]
#  [0.7071068 0.7071068 0.       ]]

theta_subtract = so3_subtract(R1, R2)
print(theta_subtract)
# Output: [ 0.         -0.          0.26179939]

在这个例子中,我们先定义了指数映射 so3_exp 和对数映射 so3_log,然后利用它们实现了 $SO(3)$ 的加法 so3_add 和减法 so3_subtract。在主程序中,我们构造了两个旋转矩阵 R1 和 R2,然后计算它们之间的扰动 theta_subtract,以及在 R1 上叠加扰动 [0, 0, np.pi/6] 得到的新旋转 R_add。结果与我们的预期一致。

3.3 四种可能的导数定义（The four possible derivative definitions）

在 $SO(3)$ 上定义了加法和减法运算后,我们就可以讨论如何计算旋转的导数了。事实上,根据函数的定义域和值域的不同,我们可以定义四种类型的导数,z在机器人领域经常会用到。

假设我们有一个机器人手臂,它的末端执行器(例如夹爪)的位置和姿态可以用一个旋转矩阵 $R$ 和一个位置向量 $\mathbf{t}$ 来描述。现在,我们希望设计一个控制器,使机器人的末端执行器沿着某个预定的轨迹运动。为了实现这一点,我们需要知道末端执行器的位置和姿态如何随时间变化,也就是需要计算 $R(t)$ 和 $\mathbf{t}(t)$ 的导数。

根据 $R(t)$ 和 $\mathbf{t}(t)$ 的定义域和值域,我们就会遇到这四种不同类型的导数:

1. 从向量空间到向量空间的函数:对于位置向量 $\mathbf{t}(t)$,它的定义域和值域都是向量空间(三维欧氏空间),因此它的导数 $\dot{\mathbf{t}}(t)$ 也是一个向量,表示末端执行器的线速度。这种情况下,我们可以使用普通的向量导数。

2. 从 $SO(3)$ 到 $SO(3)$ 的函数:对于旋转矩阵 $R(t)$,如果我们将其视为从 $SO(3)$ 到 $SO(3)$ 的函数,那么它的导数 $\dot{R}(t)$ 应该描述旋转矩阵的变化率,但这个变化率本身也应该是一个旋转矩阵。这种情况下,我们需要使用 $SO(3)$ 上特殊的导数定义。

3. 从向量空间到 $SO(3)$ 的函数:假设我们有一个关节角度向量 $\mathbf{\theta }(t)$,它决定了机器人末端执行器的姿态 $R(\mathbf{\theta }(t))$。这里, $R$ 是一个从向量空间(关节角度空间)到 $SO(3)$ 的函数。为了计算 $R$ 相对于 $\mathbf{\theta }$ 的导数,我们需要将关节角度空间的变化转化为 $SO(3)$ 上的变化。

4. 从 $SO(3)$ 到向量空间的函数:假设我们定义了一个函数 $f(R(t))$,它将末端执行器的姿态 $R(t)$ 映射到某个性能指标(例如,夹爪和目标物体之间的距离)。这里, $f$ 是一个从 $SO(3)$ 到向量空间(实数空间)的函数。为了优化这个性能指标,我们需要计算 $f$ 相对于 $R$ 的导数,这要求我们将 $SO(3)$ 上的变化转化为实数空间上的变化。

在实际的机器人控制和优化问题中,经常会遇到上述四种类型的函数和导数。为了设计高效、鲁棒的算法,我们需要根据具体的问题选择适当的导数定义。这就是为什么我们要在 $SO(3)$ 上引入特殊的加法和减法运算,并定义多种类型的导数。

总之,旋转群 $SO(3)$ 不同于普通的欧氏空间,它的特殊结构要求我们发展专门的数学工具和概念。通过引入流形、切空间、指数映射等概念,并定义适当的导数运算,这样可以更好地分析和求解涉及旋转的各种问题,如机器人控制、计算机视觉、图形学等。这些数学工具为我们提供了一种统一、严谨的语言,来描述和处理三维旋转。
下面我就用严谨的数学形式来说明它们。

3.3.1 从向量空间到向量空间的函数（Functions from vector space to vector space）

对于从向量空间到向量空间的函数 $f:{\mathbb{R}}^m\to {\mathbb{R}}^n$,我们使用通常的导数定义:

$\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}={\lim }_{\delta \mathbf{x}\to 0}\frac{f(\mathbf{x}+\delta \mathbf{x})-f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{n\times m}$

其中 $\delta \mathbf{x}$ 是 $\mathbf{x}$ 的一个微小扰动。这个导数可以用欧拉法(Euler method)近似:

$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\in {\mathbb{R}}^n$

其中 $\Delta \mathbf{x}$ 是一个有限的扰动。

3.2.2 从SO(3)到SO(3)的函数（Functions from SO(3) to SO(3)）

对于从 $SO(3)$ 到 $SO(3)$ 的函数 $f:SO(3)\to SO(3)$,我们需要使用 $SO(3)$ 上的加减法来定义导数:

$\frac{\partial f(R)}{\partial \mathbf{\theta }}={\lim }_{\delta \mathbf{\theta }\to 0}\frac{f(R\oplus \delta \mathbf{\theta })\ominus f(R)}{\delta \mathbf{\theta }}\in {\mathbb{R}}^{3\times 3}$

其中 $\mathbf{\theta }\in {\mathbb{R}}^3$ 是切空间中的微小扰动。这个导数可以近似为:

$f(R\oplus \Delta \mathbf{\theta })\approx f(R)\oplus \left(\frac{\partial f(R)}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\right)=f(R)\mathrm{Exp}\left(\frac{\partial f(R)}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\right)\in SO(3)$

3.2.3 从向量空间到SO(3)的函数（Functions from vector space to SO(3)）

对于从向量空间到 $SO(3)$ 的函数 $f:{\mathbb{R}}^m\to SO(3)$,我们需要将向量空间中的扰动映射到切空间:

$\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}={\lim }_{\delta \mathbf{x}\to 0}\frac{f(\mathbf{x}+\delta \mathbf{x})\ominus f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{3\times m}$

其近似为:

$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})\oplus \left(\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\right)=f(\mathbf{x})\mathrm{Exp}\left(\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\right)\in SO(3)$

3.2.4 从SO(3)到向量空间的函数（Functions from SO(3) to vector space）

对于从 $SO(3)$ 到向量空间的函数 $f:SO(3)\to {\mathbb{R}}^n$,我们需要将切空间中的扰动映射到向量空间:

$\frac{\partial f(R)}{\partial \mathbf{\theta }}={\lim }_{\delta \mathbf{\theta }\to 0}\frac{f(R\oplus \delta \mathbf{\theta })-f(R)}{\delta \mathbf{\theta }}\in {\mathbb{R}}^{n\times 3}$

其近似为:

$f(R\oplus \Delta \mathbf{\theta })\approx f(R)+\frac{\partial f(R)}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }=f(R)+\mathrm{Exp}\left(\frac{\partial f(R)}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\right)\in SO(3)$

这四种导数形式涵盖了所有可能的情况。在实际问题中,我们需要根据函数的具体定义选择适当的导数形式,并使用相应的近似方法进行计算。

下面是一个 Python 例子,展示了如何计算从 $SO(3)$ 到 ${\mathbb{R}}^3$ 的函数的导数:

import numpy as np

def rotation_to_vector(R):
    """
    Convert a rotation matrix to a 3D vector.
    
    Parameters:
        R (numpy.ndarray): Rotation matrix (3, 3).
        
    Returns:
        numpy.ndarray: 3D vector (3,).
    """
    return np.array([R[0, 0], R[1, 1], R[2, 2]])

def rotation_to_vector_derivative(R):
    """
    Compute the derivative of rotation_to_vector at R.
    
    Parameters:
        R (numpy.ndarray): Rotation matrix (3, 3).
        
    Returns:
        numpy.ndarray: Derivative matrix (3, 3).
    """
    epsilon = 1e-8
    derivative = np.zeros((3, 3))
    
    for i in range(3):
        dtheta = np.zeros(3)
        dtheta[i] = epsilon
        
        R_perturbed = so3_add(R, dtheta)
        v_perturbed = rotation_to_vector(R_perturbed)
        
        derivative[:, i] = (v_perturbed - rotation_to_vector(R)) / epsilon
        
    return derivative

# Example usage
R = so3_exp(np.array([0, 0, np.pi/4]))
print(rotation_to_vector(R))
# Output: [0.70710678 0.70710678 1.        ]

print(rotation_to_vector_derivative(R))
# Output:
# [[ 0.          0.70710678  0.        ]
#  [-0.70710678  0.          0.        ]
#  [ 0.          0.          0.        ]]

在这个例子中,我们定义了一个函数 rotation_to_vector,它将旋转矩阵转换为一个三维向量(对角线元素)。然后,我们计算这个函数在给定旋转矩阵 R 处的导数矩阵。为了计算数值导数,我们使用了有限差分法(finite difference method),即在每个维度上加一个微小的扰动 epsilon,计算函数值的变化,再除以 epsilon。结果表明,导数矩阵反映了输入旋转矩阵的微小变化对输出向量的影响。

当然,这只是一个简单的例子。在实际应用中,我们可能需要计算更复杂函数的导数,如相机投影、误差函数等。但是,无论函数如何定义,只要我们清楚地区分定义域和值域,并选择适当的导数形式,就可以使用类似的方法进行计算。掌握这些技巧,对于分析和优化基于旋转的系统至关重要。

3.3 旋转雅可比矩阵:链接微小变化的桥梁（Jacobians of the rotation）

这节知识是重中之重，务必掌握

在许多应用中,我们经常需要计算旋转相关函数的雅可比矩阵。雅可比矩阵是一个非常有用的工具,它描述了函数输入的微小变化如何影响函数输出。可以说,雅可比矩阵就像一座桥梁,连接着输入空间和输出空间,让我们能够分析和控制复杂的非线性系统。在这一节中,我们将介绍一些常用的旋转雅可比矩阵,并通过具体的例子来理解它们的应用。

首先,让我们回顾一下雅可比矩阵的定义。给定一个从n维空间到m维空间的函数 $\mathbf{f}:{\mathbb{R}}^n\to {\mathbb{R}}^m$,它的雅可比矩阵 $J_{\mathbf{f}}$ 是一个 $m\times n$ 的矩阵,其第 $(i,j)$ 个元素为:

$J_{\mathbf{f}}(i,j)=\frac{\partial f_i}{\partial x_j}$

也就是说,雅可比矩阵的每一行对应输出向量的一个分量,每一列对应输入向量的一个分量,每个元素表示输出对输入的偏导数。

现在,让我们来看一个具体的例子。假设我们有一个机器人手臂,它的末端执行器的位置 $\mathbf{p}$ 由关节角度 $\mathbf{\theta }$ 决定,这可以用一个函数 $\mathbf{f}$ 来表示:

$\mathbf{p}=\mathbf{f}(\mathbf{\theta })$

如果我们想知道关节角度的微小变化 $\Delta \mathbf{\theta }$ 会如何影响末端执行器的位置,我们就需要计算函数 $\mathbf{f}$ 的雅可比矩阵 $J_{\mathbf{f}}$。有了这个雅可比矩阵,我们就可以用下面的公式来近似末端执行器位置的变化:

$\Delta \mathbf{p}\approx J_{\mathbf{f}}\Delta \mathbf{\theta }$

这个公式告诉我们,关节角度的微小变化 $\Delta \mathbf{\theta }$ 通过雅可比矩阵 $J_{\mathbf{f}}$ 的线性变换,近似地等于末端执行器位置的变化 $\Delta \mathbf{p}$。这就是雅可比矩阵的作用:它将输入空间的微小变化映射到输出空间,让我们能够分析和控制系统的局部行为。

在旋转的上下文中,我们经常需要计算三种类型的雅可比矩阵:

1. 相对于旋转向量的雅可比矩阵:这个矩阵描述了旋转向量 $\mathbf{\varphi }$ 的微小变化如何影响旋转后的向量 ${\mathbf{x}}^{\prime }=R(\mathbf{\varphi })\mathbf{x}$。

2. 相对于四元数的雅可比矩阵:这个矩阵描述了四元数 $\mathbf{q}$ 的微小变化如何影响旋转后的向量 ${\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$。

3. 相对于旋转矩阵的雅可比矩阵:这个矩阵描述了旋转矩阵 $R$ 的微小变化如何影响旋转后的向量 ${\mathbf{x}}^{\prime }=R\mathbf{x}$。

这些雅可比矩阵在机器人控制、计算机视觉、图形学等领域都有广泛应用。例如,在视觉SLAM(同时定位与建图)问题中,我们需要估计相机的位姿(即旋转和平移)。这通常是通过最小化重投影误差来实现的,而重投影误差对相机位姿的雅可比矩阵在优化过程中扮演了关键角色。

另一个例子是机器人运动规划。当我们想要让机器人的末端执行器沿着某条轨迹运动时,我们需要计算关节角度随时间的变化。这就需要用到机器人运动学方程的雅可比矩阵,它将关节角度空间的速度映射到末端执行器空间的速度。

总之,雅可比矩阵(Jacobian matrix)是一个非常有用的工具,它在许多涉及旋转的应用中扮演了重要角色。通过理解和计算各种旋转相关函数的雅可比矩阵,我们可以更好地分析和控制复杂的非线性系统,如机器人、相机等。在接下来的小节中,我们将给出一些常用旋转雅可比矩阵的具体计算公式。

3.3.1 相对于向量的雅可比矩阵（Jacobian with respect to the vector）

对于向量的旋转函数 $f(\mathbf{x})=R\mathbf{x}$ 或 $f(\mathbf{x})=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$,其相对于向量 $\mathbf{x}$ 的雅可比矩阵非常简单:

$\frac{\partial (R\mathbf{x})}{\partial \mathbf{x}}=\frac{\partial (\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast })}{\partial \mathbf{x}}=R$

也就是说,无论是用旋转矩阵还是四元数来表示旋转,其相对于被旋转向量的雅可比矩阵都等于旋转矩阵本身。这个结果非常直观,因为旋转是一个线性变换。

3.3.2 相对于四元数的雅可比矩阵（Jacobian with respect to the quaternion）

相比之下,旋转相对于四元数的雅可比矩阵就没有那么简单了。为了推导这个雅可比矩阵,我们需要使用四元数乘法的定义,并对每个分量求导。
设 $\mathbf{q}=(q_w,q_x,q_y,q_z)=(q_w,{\mathbf{q}}_v)$,旋转函数为 $f(\mathbf{x})=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$。经过一系列的推导(这里省略了详细步骤),我们可以得到:

$\frac{\partial (\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast })}{\partial \mathbf{q}}=2\left[\begin{array}{c}{q}_w{\mathbf{x}}^{\top }+{\mathbf{q}}_v\times \mathbf{x}\\ ({\mathbf{q}}_v\cdot \mathbf{x})I_3+{\mathbf{q}}_v{\mathbf{x}}^{\top }-\mathbf{x}{\mathbf{q}}_v^{\top }-q_w[\mathbf{x}{]}_{\times }\end{array}\right]\in {\mathbb{R}}^{3\times 4}$

其中 $I_3$ 是 $3\times 3$ 的单位矩阵,$[\mathbf{x}{]}_{\times }$ 是向量 $\mathbf{x}$ 的叉乘矩阵(定义见第一章)。可以看到,这个雅可比矩阵的表达式相当复杂,体现了四元数乘法的非线性特性。

3.3.3 SO(3)的右雅可比矩阵（Right Jacobian of SO(3)）

另一个非常有用的雅可比矩阵是 $SO(3)$ 的右雅可比矩阵(right Jacobian)。它描述了切空间中的扰动 $\delta \mathbf{\theta }$ 如何影响旋转矩阵 $R$。具体来说,假设我们有一个旋转矩阵 $R=\mathrm{Exp}(\mathbf{\theta })$,其中 $\mathbf{\theta }\in {\mathbb{R}}^3$ 是轴角向量。如果我们在 $\mathbf{\theta }$ 上加一个微小扰动 $\delta \mathbf{\theta }$,那么旋转矩阵就会发生变化:

$\mathrm{Exp}(\mathbf{\theta })\oplus \delta \mathbf{\varphi }=\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })$

这里的 $\delta \mathbf{\varphi }$ 是扰动在切空间 $T_{R}SO(3)$ 上的表示。右雅可比矩阵 $J_r(\mathbf{\theta })$ 就是将 $\delta \mathbf{\theta }$ 映射为 $\delta \mathbf{\varphi }$ 的线性映射:

$J_r(\mathbf{\theta })={\lim }_{\delta \mathbf{\theta }\to 0}\frac{\delta \mathbf{\varphi }}{\delta \mathbf{\theta }}={\lim }_{\delta \mathbf{\theta }\to 0}\frac{\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\ominus \mathrm{Exp}(\mathbf{\theta })}{\delta \mathbf{\theta }}\in {\mathbb{R}}^{3\times 3}$

这个雅可比矩阵有一个很漂亮的解析表达式:

$J_r(\mathbf{\theta })=I-\frac{1-\cos \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}[\mathbf{\theta }{]}_{\times }+\frac{\Vert \mathbf{\theta }\Vert -\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^3}[\mathbf{\theta }{]}_{\times }^2$

其中 $I$ 是单位矩阵。右雅可比矩阵有许多重要的性质,例如:

$\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\approx \mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}(J_r(\mathbf{\theta })\delta \mathbf{\theta })$

$\mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}(\delta \mathbf{\theta })\approx \mathrm{Exp}(\mathbf{\theta }+J_r^{-1}(\mathbf{\theta })\delta \mathbf{\theta })$

$\mathrm{Log}(\mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}(\delta \mathbf{\theta }))\approx \mathbf{\theta }+J_r^{-1}(\mathbf{\theta })\delta \mathbf{\theta }$

这些性质在处理旋转的扰动和优化问题时非常有用。下面是 Python 中计算右雅可比矩阵的代码:

import numpy as np

def so3_right_jacobian(theta):
    """
    Compute the right Jacobian of SO(3) at theta.
    
    Parameters:
        theta (numpy.ndarray): Rotation vector (3,).
        
    Returns:
        numpy.ndarray: Right Jacobian matrix (3, 3).
    """
    theta_norm = np.linalg.norm(theta)
    if theta_norm < 1e-8:
        return np.eye(3)
    
    theta_hat = np.array([[0, -theta[2], theta[1]],
                          [theta[2], 0, -theta[0]],
                          [-theta[1], theta[0], 0]])
    
    return np.eye(3) - (1-np.cos(theta_norm))/theta_norm**2*theta_hat + (theta_norm-np.sin(theta_norm))/theta_norm**3*theta_hat@theta_hat

# Example usage
theta = np.array([0, 0, np.pi/4])
print(so3_right_jacobian(theta))
# Output:
# [[0.70710678 0.        0.        ]
#  [0.         0.70710678 0.        ] 
#  [0.         0.         1.        ]]

在这个例子中,我们计算了轴角向量 $\mathbf{\theta }=(0,0,\pi /4)$ 处的右雅可比矩阵。结果是一个对角矩阵,表明在这个点上,切空间中的扰动会被等比例地映射到旋转矩阵上。

3.3.4 相对于旋转向量的雅可比矩阵（Jacobian with respect to the rotation vector）

最后,让我们来看旋转相对于轴角向量的雅可比矩阵。这个雅可比矩阵描述了轴角向量 $\mathbf{\theta }$ 的微小变化如何影响旋转后的向量 ${\mathbf{x}}^{\prime }=R\mathbf{x}=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }$。利用前面的结果,我们可以推导出:

$\frac{\partial (R\mathbf{x})}{\partial \mathbf{\theta }}=\frac{\partial (\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast })}{\partial \mathbf{\theta }}=-R[\mathbf{x}{]}_{\times }{J}_r(\mathbf{\theta })$

其中 $R=\mathrm{Exp}(\mathbf{\theta })$,$J_r(\mathbf{\theta })$ 是右雅可比矩阵。这个结果表明,旋转后的向量对轴角向量的导数,可以通过旋转矩阵、向量的叉乘矩阵和右雅可比矩阵的乘积来计算。

3.3.5 旋转复合的雅可比矩阵（Jacobians of the rotation composition）

旋转复合的雅可比矩阵描述了两个旋转的复合 $R=R_1{R}_2$ 或 $\mathbf{q}={\mathbf{q}}_1\otimes {\mathbf{q}}_2$ 相对于其中一个旋转的导数。设 $R_1=\mathrm{Exp}({\mathbf{\theta }}_1),R_2=\mathrm{Exp}({\mathbf{\theta }}_2)$,则有:

$\frac{\partial (R_1{R}_2)}{\partial R_1}=\frac{\partial ({\mathbf{q}}_1\otimes {\mathbf{q}}_2)}{\partial {\mathbf{\theta }}_1}=R_2^{\top }$

$\frac{\partial (R_1{R}_2)}{\partial R_2}=\frac{\partial ({\mathbf{q}}_1\otimes {\mathbf{q}}_2)}{\partial {\mathbf{\theta }}_2}=I$

这两个结果都很直观:旋转复合相对于第一个旋转的雅可比矩阵等于第二个旋转矩阵的转置,而相对于第二个旋转的雅可比矩阵等于单位矩阵。这反映了旋转复合的非交换性质。

以上就是一些常用的旋转雅可比矩阵。在实际应用中,我们经常需要计算这些雅可比矩阵,并将其用于优化、滤波、控制等问题中。掌握这些矩阵的性质和计算方法,是分析和设计基于旋转的系统的关键。

3.4 扰动、不确定性、噪声（Perturbations, uncertainties, noise）

当我们研究旋转时,经常需要考虑旋转的微小变化或扰动。例如,当我们用陀螺仪测量物体的旋转时,测量值总会有一些噪声和误差。这些噪声和误差可以看作是在真实旋转上叠加的微小扰动。为了描述和分析这些扰动,我们引入了局部扰动的概念。

想象你有一个三维的物体,它的姿态可以用一个旋转矩阵 $R$ 或者一个四元数 $\mathbf{q}$ 来表示。现在,如果这个物体的姿态发生了一个微小的变化,变成了 $\tilde{R}$ 或 $\tilde{\mathbf{q}}$,我们如何描述这个变化呢?一种直观的方式是用另一个旋转 $\Delta R_L$ 或 $\Delta {\mathbf{q}}_L$ 来表示这个变化,使得:

$\tilde{R}=R\Delta R_L,\tilde{\mathbf{q}}=\mathbf{q}\otimes \Delta {\mathbf{q}}_L$

这里, $\Delta R_L$ 和 $\Delta {\mathbf{q}}_L$ 就是局部扰动。它们描述了如何从原始的姿态 $R$ 或 $\mathbf{q}$ 出发,通过一个微小的旋转变换到扰动后的姿态 $\tilde{R}$ 或 $\tilde{\mathbf{q}}$。

但是,旋转矩阵和四元数都有一定的约束(旋转矩阵必须是正交的,四元数必须是单位的),所以我们不能随意选择 $\Delta R_L$ 和 $\Delta {\mathbf{q}}_L$。这就是切空间和指数映射发挥作用的地方。切空间是一个没有约束的向量空间,我们可以在其中自由地表示旋转的微小变化。指数映射则提供了一种方式,将切空间中的向量映射到旋转矩阵或四元数空间,同时保证满足它们的约束。

具体来说,我们可以用一个三维向量 $\Delta {\mathbf{\theta }}_L$ 来表示切空间中的微小旋转。然后,通过指数映射,我们可以将这个向量转换为旋转矩阵空间或四元数空间中的局部扰动:

$\Delta R_L=\mathrm{Exp}(\Delta {\mathbf{\theta }}_L),\Delta {\mathbf{q}}_L=\exp (\Delta {\mathbf{\theta }}_L/2)$

反过来,如果我们知道旋转矩阵或四元数的扰动 $\Delta R_L$ 或 $\Delta {\mathbf{q}}_L$,也可以通过对数映射将其转换回切空间的向量 $\Delta {\mathbf{\theta }}_L$:

$\Delta {\mathbf{\theta }}_L=\mathrm{Log}(R^{\top }\tilde{R})=\mathrm{Log}({\mathbf{q}}^{\ast }\otimes \tilde{\mathbf{q}})$

局部扰动的一个重要应用是表示旋转的不确定性。在许多实际问题中,我们无法精确地知道物体的姿态,而只能给出一个估计值和不确定性的度量。例如,当我们用视觉或惯性传感器估计机器人的姿态时,估计结果总会有一些误差。这些误差可以用旋转的概率分布来描述,即给定一个估计值,真实值可能在其附近的一个区域内。

然而,由于旋转空间是非线性的,直接在旋转矩阵或四元数空间上定义概率分布是很困难的。相反,我们可以利用局部扰动,将旋转的不确定性表示在切空间中。具体来说,我们可以假设局部扰动向量 $\Delta {\mathbf{\theta }}_L$ 服从一个高斯分布,其均值为零(因为我们已经有了旋转的估计值),协方差矩阵表示了不确定性的大小。这样,我们就可以使用卡尔曼滤波等经典的线性估计方法来处理旋转的不确定性。

下面是一个简单的例子。假设我们有一个旋转矩阵 $R$ 的估计值,以及一个表示不确定性的协方差矩阵 $\Sigma$。我们可以按照以下步骤来表示和传播这个不确定性:

1. 从高斯分布 $\mathcal{N}(0,\Sigma )$ 中采样一组局部扰动向量 $\Delta {\mathbf{\theta }}_{L,i}$。

2. 使用指数映射将这些向量转换为旋转矩阵空间中的扰动 $\Delta R_{L,i}=\mathrm{Exp}(\Delta {\mathbf{\theta }}_{L,i})$。

3. 将扰动应用到估计值上,得到一组可能的真实值 ${\tilde{R}}_i=R\Delta R_{L,i}$。

4. 如果需要的话,可以对这组旋转矩阵进行分析或进一步处理,例如计算它们的平均值或协方差。

5. 在估计或优化的迭代过程中,我们可以不断地更新估计值 $R$ 和不确定性 $\Sigma$,以反映新的观测信息。

在实际的估计和优化算法中,我们通常不需要显式地采样扰动,而是直接在切空间上进行计算。但是,局部扰动的思想是相同的:它提供了一种在切空间上局部线性化的方式,让我们能够使用简单而强大的线性方法来处理旋转的不确定性。

除了不确定性的表示,局部扰动还可以用于许多其他问题,例如插值、平滑、边缘检测等。总之,局部扰动是一个非常有用的概念,它建立了旋转空间和切空间之间的桥梁,让我们能够在保持旋转约束的同时,利用向量空间的简单性和灵活性。

在许多实际问题中,我们需要处理旋转的扰动、不确定性和噪声。这些都可以用旋转的微小变化来表示,但具体的形式和处理方法可能有所不同。下面我们将讨论几种常见的扰动表示方法。

3.4.1 局部扰动（Local perturbations）

局部扰动是指在切空间中定义的微小旋转变化。给定一个参考旋转 $R$ 或 $\mathbf{q}$,我们可以将扰动后的旋转 $\tilde{R}$ 或 $\tilde{\mathbf{q}}$ 表示为:

$\tilde{R}=R\Delta R_L,\tilde{\mathbf{q}}=\mathbf{q}\otimes \Delta {\mathbf{q}}_L$

其中 $\Delta R_L$ 和 $\Delta {\mathbf{q}}_L$ 是局部扰动,它们可以通过指数映射从切空间中的扰动向量 $\Delta {\mathbf{\theta }}_L$ 得到:

$\Delta R_L=\mathrm{Exp}(\Delta {\mathbf{\theta }}_L),\Delta {\mathbf{q}}_L=\exp (\Delta {\mathbf{\theta }}_L/2)$

反之,我们也可以通过对数映射将局部扰动映射回切空间:

$\Delta {\mathbf{\theta }}_L=\mathrm{Log}(R^{\top }\tilde{R})=\mathrm{Log}({\mathbf{q}}^{\ast }\otimes \tilde{\mathbf{q}})$

如果扰动角度 $\Delta {\mathbf{\theta }}_L$ 很小,我们可以用一阶泰勒展开近似局部扰动:

$\Delta R_L\approx I+[\Delta {\mathbf{\theta }}_L{]}_{\times },\Delta {\mathbf{q}}_L\approx [1,\Delta {\mathbf{\theta }}_L/2{]}^{\top }$

局部扰动的一个重要应用是表示旋转的不确定性。在很多估计问题中,我们需要计算旋转的均值和协方差。由于旋转流形的非线性,直接对旋转进行平均和计算协方差是没有意义的。相反,我们可以将旋转的不确定性表示在切空间中,即用局部扰动向量 $\Delta {\mathbf{\theta }}_L$ 的均值和协方差来描述。这样,我们就可以使用传统的线性估计方法,如卡尔曼滤波(Kalman filter),来处理旋转的不确定性。

下面是一个 Python 例子,展示了如何在局部切空间中表示旋转的不确定性:

import numpy as np

# Generate a random rotation matrix and a random local perturbation
R_true = so3_exp(np.random.randn(3))
theta_pert = np.random.randn(3) * 0.1

# Perturb the rotation matrix
R_pert = so3_add(R_true, theta_pert)

# Compute the local perturbation vector
theta_local = so3_log(R_true.T @ R_pert)

# Compute the mean and covariance of the local perturbation
num_samples = 1000
theta_samples = np.zeros((num_samples, 3))

for i in range(num_samples):
    R_sample = so3_add(R_true, np.random.randn(3) * 0.1)
    theta_samples[i] = so3_log(R_true.T @ R_sample)
    
theta_mean = np.mean(theta_samples, axis=0)
theta_cov = np.cov(theta_samples.T)

print("True perturbation:")
print(theta_pert)
print("Estimated mean:")
print(theta_mean)
print("Estimated covariance:")
print(theta_cov)

在这个例子中,我们首先生成一个随机的真实旋转矩阵 R_true 和一个随机的局部扰动向量 theta_pert。然后,我们将扰动应用于旋转矩阵,得到扰动后的旋转矩阵 R_pert。接着,我们计算局部扰动向量 theta_local,作为真实扰动的参考。最后,我们生成 1000 个随机扰动样本,计算它们在切空间中的均值和协方差,并与真实扰动进行比较。结果表明,我们可以用局部扰动的均值和协方差来近似表示旋转的不确定性。

3.4.2 全局扰动（Global perturbations）

与局部扰动不同,全局扰动是在旋转群 $SO(3)$ 上定义的。给定一个参考旋转 $R$ 或 $\mathbf{q}$,我们可以将扰动后的旋转表示为:

$\tilde{R}=\Delta R_{G}R,\tilde{\mathbf{q}}=\Delta {\mathbf{q}}_G\otimes \mathbf{q}$

其中 $\Delta R_G$ 和 $\Delta {\mathbf{q}}_G$ 是全局扰动,它们也可以通过指数映射从切空间中的扰动向量 $\Delta {\mathbf{\theta }}_G$ 得到:

$\Delta R_G=\mathrm{Exp}(\Delta {\mathbf{\theta }}_G),\Delta {\mathbf{q}}_G=\exp (\Delta {\mathbf{\theta }}_G/2)$

反之,我们也可以通过对数映射将全局扰动映射回切空间:

$\Delta {\mathbf{\theta }}_G=\mathrm{Log}(\tilde{R}{R}^{\top })=\mathrm{Log}(\tilde{\mathbf{q}}\otimes {\mathbf{q}}^{\ast })$

全局扰动在某些问题中可能更加自然和方便,特别是当我们需要在固定坐标系下描述旋转的变化时。然而,在处理不确定性时,全局扰动可能会引入额外的复杂性,因为它们改变了切空间的基点。在实践中,局部扰动通常更容易处理,也更常用。

3.5 时间导数（Time derivatives）

旋转的时间导数及其应用

在三维空间中描述和分析物体的旋转运动是许多领域的重要课题,如航天器姿态控制、机器人运动规划、虚拟现实等。为了刻画旋转随时间的变化规律,我们引入了旋转的时间导数的概念。

定义与计算

假设我们有一个随时间变化的旋转矩阵 $R(t)$ 或四元数 $\mathbf{q}(t)$,它们的时间导数可以表示为:

• 旋转矩阵的时间导数:
  $\dot{R}(t)=R(t)[\mathbf{\omega }\_L]\_\times$ 或 $\dot{R}(t)=[\mathbf{\omega }\_G]\_\times R(t)$

• 四元数的时间导数:
  $\dot{\mathbf{q}}(t)=\frac{1}{2}\mathbf{q}(t)\otimes \mathbf{\omega }\_L$ 或 $\dot{\mathbf{q}}(t)=\frac{1}{2}\mathbf{\omega }\_G\otimes \mathbf{q}(t)$

其中 $\mathbf{\omega }\_L$ 和 $\mathbf{\omega }\_G$ 分别是局部坐标系和全局坐标系下的角速度向量。

应用实例

1. 航天器姿态估计:

def attitude_estimation(q, omega, dt):
    # 使用四元数微分方程更新姿态
    q_dot = 0.5 * quaternion_multiply(q, [0, omega[0], omega[1], omega[2]])
    q_new = q + q_dot * dt
    q_new = quaternion_normalize(q_new)
    return q_new

2. 机器人运动规划:

def rotation_trajectory(R_start, R_end, t_span):
    # 计算旋转矩阵的对数映射
    omega_G = logarithm_map(R_start.T @ R_end)
    
    # 生成中间过程的旋转矩阵
    R_trajectory = []
    for t in t_span:
        R_t = R_start @ exponential_map(omega_G * t)
        R_trajectory.append(R_t)
    
    return R_trajectory

3. 计算机动画插值:

def quaternion_slerp(q0, q1, t_span):
    # 球面线性插值生成中间过程的四元数
    q_trajectory = []
    for t in t_span:
        q_t = quaternion_slerp(q0, q1, t)
        q_trajectory.append(q_t)
    
    return q_trajectory

通过引入旋转的时间导数,我们可以建立旋转表示与角速度之间的联系,并在此基础上解决实际问题。这一概念在航天、机器人、计算机图形学等领域都有广泛应用,是研究三维空间中旋转运动的重要数学工具。

在许多动力学问题中,我们需要计算旋转的时间导数。这在描述刚体运动、估计角速度、设计控制器等方面都有重要应用。本节我们将讨论如何定义和计算旋转的时间导数。

假设我们有一个随时间变化的旋转 $R(t)$ 或 $\mathbf{q}(t)$,我们希望计算它们的时间导数 $\dot{R}(t)$ 或 $\dot{\mathbf{q}}(t)$。根据局部扰动的定义,我们可以将旋转在时刻 $t$ 的值 $R(t)$ 或 $\mathbf{q}(t)$ 视为参考旋转,将时刻 $t+\Delta t$ 的值视为扰动后的旋转,即:

$R(t+\Delta t)=R(t)\Delta R(t)\approx R(t)\mathrm{Exp}(\Delta {\mathbf{\theta }}_L)$

$\mathbf{q}(t+\Delta t)=\mathbf{q}(t)\otimes \Delta \mathbf{q}(t)\approx \mathbf{q}(t)\otimes \exp (\Delta {\mathbf{\theta }}_L/2)$

其中 $\Delta {\mathbf{\theta }}_L$ 是时间间隔 $\Delta t$ 内的局部扰动。根据导数的定义,我们有:

$\dot{R}(t)={\lim }_{\Delta t\to 0}\frac{R(t+\Delta t)-R(t)}{\Delta t}={\lim }_{\Delta t\to 0}\frac{R(t)\mathrm{Exp}(\Delta {\mathbf{\theta }}_L)-R(t)}{\Delta t}=R(t)[{\mathbf{\omega }}_L{]}_{\times }$

$\dot{\mathbf{q}}(t)={\lim }_{\Delta t\to 0}\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}={\lim }_{\Delta t\to 0}\frac{\mathbf{q}(t)\otimes \exp (\Delta {\mathbf{\theta }}_L/2)-\mathbf{q}(t)}{\Delta t}=\frac{1}{2}\mathbf{q}(t)\otimes {\mathbf{\omega }}_L$

其中 ${\mathbf{\omega }}_L$ 是局部坐标系下的角速度向量,定义为:

$$\begin{gathered} {\mathbf{\omega }}_L(t)= \\ li{m}_{\Delta t\to 0}\frac{\Delta {\mathbf{\theta }}_L}{\Delta t}\in {\mathbb{R}}^3 \end{gathered}$$

这些导数表达式与我们在第一章中得到的完全一致。现在,我们可以明确地将角速度 ${\mathbf{\omega }}_L$ 与旋转 $R(t)$ 或 $\mathbf{q}(t)$ 所定义的局部坐标系联系起来。从这个角度来看,上述导数表达式描述了一个参考系的旋转运动,其中角速度是在该参考系下定义的。

类似地,我们也可以定义全局坐标系下的旋转导数:

$\dot{R}(t)=[{\mathbf{\omega }}_G{]}_{\times }R(t)$

$\dot{\mathbf{q}}(t)=\frac{1}{2}{\mathbf{\omega }}_G\otimes \mathbf{q}(t)$

其中 ${\mathbf{\omega }}_G$ 是全局坐标系下的角速度向量,定义为:

${\mathbf{\omega }}_G(t)={\lim }_{\Delta t\to 0}\frac{\Delta {\mathbf{\theta }}_G}{\Delta t}\in {\mathbb{R}}^3$

这些表达式描述了一个参考系相对于固定坐标系的旋转运动,其中角速度是在固定坐标系下定义的。

下面是一个 Python 例子,展示了如何计算旋转矩阵和四元数的时间导数:

import numpy as np

def rotation_matrix_derivative(R, omega):
    """
    Compute the time derivative of a rotation matrix.
    
    Parameters:
        R (numpy.ndarray): Rotation matrix (3, 3).
        omega (numpy.ndarray): Angular velocity vector (3,).
        
    Returns:
        numpy.ndarray: Time derivative of the rotation matrix (3, 3).
    """
    return R @ so3_hat(omega)

def quaternion_derivative(q, omega):
    """
    Compute the time derivative of a quaternion.
    
    Parameters:
        q (numpy.ndarray): Quaternion (4,).
        omega (numpy.ndarray): Angular velocity vector (3,).
        
    Returns:
        numpy.ndarray: Time derivative of the quaternion (4,).
    """
    omega_quat = np.concatenate(([0], omega))
    return 0.5 * quaternion_multiply(q, omega_quat)

def so3_hat(v):
    """
    Compute the skew-symmetric matrix of a vector.
    
    Parameters:
        v (numpy.ndarray): 3D vector.
        
    Returns:
        numpy.ndarray: Skew-symmetric matrix (3, 3).
    """
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

# Example usage
R = so3_exp(np.array([0, 0, np.pi/4]))
q = rotation_vector_to_quaternion(np.array([0, 0, np.pi/4]))
omega = np.array([0.1, 0.2, 0.3])

R_dot = rotation_matrix_derivative(R, omega)
q_dot = quaternion_derivative(q, omega)

print("Rotation matrix derivative:")
print(R_dot)
print("Quaternion derivative:")  
print(q_dot)

在这个例子中,我们首先定义了计算旋转矩阵和四元数时间导数的函数 rotation_matrix_derivative 和 quaternion_derivative,以及计算向量叉乘矩阵的辅助函数 so3_hat。然后,我们构造了一个旋转矩阵 R 和对应的四元数 q,表示绕 $z$ 轴旋转 $45^{\circ }$。给定一个角速度向量 omega,我们计算旋转矩阵和四元数的时间导数 R_dot 和 q_dot,并将结果打印出来。

3.5.1 局部到全局的关系（Global-to-local relations）

从上面的讨论中,我们可以看到局部角速度 ${\mathbf{\omega }}_L$ 和全局角速度 ${\mathbf{\omega }}_G$ 之间有一个简单的关系:

$\frac{1}{2}{\mathbf{\omega }}_G\otimes \mathbf{q}=\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_L$

右乘 ${\mathbf{q}}^{\ast }$ 可得:

${\mathbf{\omega }}_G=\mathbf{q}\otimes {\mathbf{\omega }}_L\otimes {\mathbf{q}}^{\ast }=R{\mathbf{\omega }}_L$

类似地,对于小的角度扰动 $\Delta \mathbf{\theta }$ 和小的时间间隔 $\Delta t$,我们有近似关系 $\Delta \mathbf{\theta }\approx \mathbf{\omega }\Delta t$,因此:

$\Delta {\mathbf{\theta }}_G=\mathbf{q}\otimes \Delta {\mathbf{\theta }}_L\otimes {\mathbf{q}}^{\ast }=R\Delta {\mathbf{\theta }}_L$

这意味着,我们可以像处理普通向量一样,用四元数或旋转矩阵对角速度向量 $\mathbf{\omega }$ 和小角度扰动 $\Delta \mathbf{\theta }$ 进行坐标变换。这一点可以通过将角速度或扰动写成轴角式 $\mathbf{\omega }=\omega \mathbf{u}$ 或 $\Delta \mathbf{\theta }=\Delta \theta \mathbf{u}$ 得到进一步的确认,因为旋转轴 $\mathbf{u}$ 可以像普通向量一样进行变换:

${\mathbf{u}}_G=\mathbf{q}\otimes {\mathbf{u}}_L\otimes {\mathbf{q}}^{\ast }=R{\mathbf{u}}_L$

3.5.2 四元数乘积的时间导数（Time-derivative of the quaternion product）

在处理复合旋转的动力学时,我们经常需要计算四元数乘积的时间导数。对于两个四元数 ${\mathbf{q}}_1$ 和 ${\mathbf{q}}_2$ 的乘积,我们可以使用乘积求导法则:

$\frac{\mathrm{d}({\mathbf{q}}_1\otimes {\mathbf{q}}_2)}{\mathrm{d}t}={\dot{\mathbf{q}}}_1\otimes {\mathbf{q}}_2+{\mathbf{q}}_1\otimes {\dot{\mathbf{q}}}_2$

$\frac{\mathrm{d}(R_1{R}_2)}{\mathrm{d}t}={\dot{R}}_1{R}_2+R_1{\dot{R}}_2$

但是,由于四元数乘积是不可交换的,我们需要注意因子的顺序。特别地,对于四元数的平方,我们有:

$\frac{\mathrm{d}({\mathbf{q}}^2)}{\mathrm{d}t}=\dot{\mathbf{q}}\otimes \mathbf{q}+\mathbf{q}\otimes \dot{\mathbf{q}}\ne 2\mathbf{q}\otimes \dot{\mathbf{q}}$

这与实数情况不同,体现了四元数乘法的非交换性。

3.5.3 其他有用的导数表达式（Other useful expressions with the derivative）

利用四元数导数的定义,我们可以得到一些有用的表达式。例如,局部角速度可以写成:

${\mathbf{\omega }}_L=2{\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}},[{\mathbf{\omega }}_L{]}_{\times }=R^{\top }\dot{R}$

而全局角速度可以写成:

${\mathbf{\omega }}_G=2\dot{\mathbf{q}}\otimes {\mathbf{q}}^{\ast },[{\mathbf{\omega }}_G{]}_{\times }=\dot{R}{R}^{\top }$

这些表达式在分析和控制旋转运动时非常有用。

具体来说:

局部角速度ωL描述的是物体相对于自身坐标系的角速度,适合研究物体自身的旋转运动。
全局角速度ωG描述的是物体相对于绝对参考系的角速度,适合研究物体在整个空间坐标系下的运动。

通过四元数导数的定义,我们可以把这些角速度用四元数和其导数的形式精确表达出来,这在分析和控制物体运动时非常有用。
例如,在机器人控制领域,需要根据机器人关节的运动学模型,精确计算机械臂各关节的角速度,以便对其运动进行有效规划和控制。航天器姿态控制也需要精确计算飞行器相对于惯性空间的角速度,并根据这些信息控制其方向。

3.6 旋转速度的时间积分（Time-integration of rotation rates）

在许多实际应用中,我们无法直接测量旋转 $R(t)$ 或 $\mathbf{q}(t)$,而只能测量角速度 $\mathbf{\omega }(t)$。这种情况在使用IMU(惯性测量单元)的系统中尤其常见,因为IMU通常包含陀螺仪,可以测量局部角速度。为了估计旋转,我们需要对角速度进行时间积分。

设我们有一个时间间隔 $[t_n,t_{n+1}]$,其中 $t_n=n\Delta t$,我们想估计旋转在时刻 $t_{n+1}$ 的值 $R(t_{n+1})$ 或 $\mathbf{q}(t_{n+1})$。假设我们已知旋转在时刻 $t_n$ 的值 $R(t_n)$ 或 $\mathbf{q}(t_n)$,以及整个时间间隔内的角速度测量值 $\mathbf{\omega }(t)$,则我们可以使用数值积分方法来估计旋转。

本节我们将介绍几种常用的数值积分方法,包括零阶法(Euler法)和一阶法。这些方法都基于旋转的泰勒级数展开,只是截断的阶数不同。我们将重点讨论局部角速度的积分,因为这在实践中更为常见。

3.6.1 零阶积分（Zeroth order integration）

零阶积分法假设角速度在整个时间间隔内保持不变,即 $\mathbf{\omega }(t)={\mathbf{\omega }}_n,\forall t\in [t_n,t_{n+1}]$。这时,我们可以直接对旋转的微分方程进行积分:

$R(t_{n+1})=R(t_n)\mathrm{Exp}({\mathbf{\omega }}_n\Delta t)$

$\mathbf{q}(t_{n+1})=\mathbf{q}(t_n)\otimes \exp ({\mathbf{\omega }}_n\Delta t/2)$

这里的指数 $\mathrm{Exp}({\mathbf{\omega }}_n\Delta t)$ 和 $\exp ({\mathbf{\omega }}_n\Delta t/2)$ 可以用罗德里格斯公式或四元数指数公式计算。

根据使用的角速度测量值不同,我们可以进一步划分为前向法(forward)、后向法(backward)和中值法(midward):

• 前向法使用时刻 $t_n$ 的角速度 ${\mathbf{\omega }}_n$ 进行积分:

$R_{n+1}=R_n\mathrm{Exp}({\mathbf{\omega }}_n\Delta t)$

${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \exp ({\mathbf{\omega }}_n\Delta t/2)$

• 后向法使用时刻 $t_{n+1}$ 的角速度 ${\mathbf{\omega }}_{n+1}$ 进行积分:

$R_{n+1}=R_n\mathrm{Exp}({\mathbf{\omega }}_{n+1}\Delta t)$

${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \exp ({\mathbf{\omega }}_{n+1}\Delta t/2)$

这种方法在实时系统中很常用,因为积分区间对应于最新的测量值。为了强调这一点,我们可以将时间索引改为 { n − 1 , n } \{n-1, n\} {n−1,n}:

$R_n=R_{n-1}\mathrm{Exp}({\mathbf{\omega }}_n\Delta t)$

${\mathbf{q}}_n={\mathbf{q}}_{n-1}\otimes \exp ({\mathbf{\omega }}_n\Delta t/2)$

• 中值法使用时刻 $t_n$ 和 $t_{n+1}$ 的角速度的平均值进行积分:

$\bar{\mathbf{\omega }}=\frac{{\mathbf{\omega }}_n+{\mathbf{\omega }}_{n+1}}{2}$

$R_{n+1}=R_n\mathrm{Exp}(\bar{\mathbf{\omega }}\Delta t)$

${\mathbf{q}}_{n+1}={\mathbf{q}}_n\otimes \exp (\bar{\mathbf{\omega }}\Delta t/2)$

这种方法在离线处理中较为常用,因为它利用了未来的测量值。

下面是一个 Python 例子,展示了如何使用不同的零阶积分法估计旋转:

import numpy as np

def forward_integration(R_prev, q_prev, omega, dt):
    """
    Forward integration of rotation.
    
    Parameters:
        R_prev (numpy.ndarray): Previous rotation matrix (3, 3).
        q_prev (numpy.ndarray): Previous quaternion (4,).
        omega (numpy.ndarray): Angular velocity vector (3,).
        dt (float): Time step.
        
    Returns:
        tuple: (R_next, q_next)
        - R_next (numpy.ndarray): Next rotation matrix (3, 3).
        - q_next (numpy.ndarray): Next quaternion (4,).
    """
    R_next = so3_add(R_prev, omega*dt)
    q_next = quaternion_multiply(q_prev, quaternion_exp(omega*dt/2))
    return R_next, q_next

def backward_integration(R_prev, q_prev, omega_next, dt):
    """
    Backward integration of rotation.
    
    Parameters:
        R_prev (numpy.ndarray): Previous rotation matrix (3, 3).
        q_prev (numpy.ndarray): Previous quaternion (4,).
        omega_next (numpy.ndarray): Next angular velocity vector (3,).
        dt (float): Time step.
        
    Returns:
        tuple: (R_next, q_next)
        - R_next (numpy.ndarray): Next rotation matrix (3, 3).
        - q_next (numpy.ndarray): Next quaternion (4,).
    """
    R_next = so3_add(R_prev, omega_next*dt)
    q_next = quaternion_multiply(q_prev, quaternion_exp(omega_next*dt/2))
    return R_next, q_next

def midward_integration(R_prev, q_prev, omega_prev, omega_next, dt):
    """
    Midward integration of rotation.
    
    Parameters:
        R_prev (numpy.ndarray): Previous rotation matrix (3, 3).
        q_prev (numpy.ndarray): Previous quaternion (4,).
        omega_prev (numpy.ndarray): Previous angular velocity vector (3,).
        omega_next (numpy.ndarray): Next angular velocity vector (3,).
        dt (float): Time step.
        
    Returns:
        tuple: (R_next, q_next)
        - R_next (numpy.ndarray): Next rotation matrix (3, 3).
        - q_next (numpy.ndarray): Next quaternion (4,).
    """
    omega_mid = (omega_prev + omega_next) / 2
    R_next = so3_add(R_prev, omega_mid*dt)
    q_next = quaternion_multiply(q_prev, quaternion_exp(omega_mid*dt/2))
    return R_next, q_next

# Example usage
R = np.eye(3)
q = np.array([1, 0, 0, 0])
omega = np.array([0.1, 0.2, 0.3])
dt = 0.01

R_forward, q_forward = forward_integration(R, q, omega, dt)
print("Forward integration:")
print("Rotation matrix:")
print(R_forward)
print("Quaternion:")
print(q_forward)

omega_next = np.array([0.2, 0.3, 0.4])
R_backward, q_backward = backward_integration(R, q, omega_next, dt)
print("Backward integration:")
print("Rotation matrix:")
print(R_backward)
print("Quaternion:")  
print(q_backward)

R_midward, q_midward = midward_integration(R, q, omega, omega_next, dt)
print("Midward integration:")
print("Rotation matrix:")
print(R_midward)
print("Quaternion:")
print(q_midward)

在这个例子中,我们定义了三个函数 forward_integration, backward_integration 和 midward_integration,分别实现了前向、后向和中值的零阶积分法。然后,我们构造了一个初始旋转 R 和 q (单位矩阵和四元数),以及两个角速度向量 omega 和 omega_next。给定时间步长 dt,我们使用三种积分法估计下一时刻的旋转,并将结果打印出来。

零阶积分法的优点是简单快速,适合实时应用。但是,它的精度有限,特别是当角速度变化较大时。为了提高精度,我们可以使用更高阶的积分法,如一阶法。

3.6.2 一阶积分（First order integration）

一阶积分法假设角速度在时间间隔内是线性变化的,即:

$\mathbf{\omega }(t)={\mathbf{\omega }}_n+\frac{t-t_n}{\Delta t}({\mathbf{\omega }}_{n+1}-{\mathbf{\omega }}_n),t\in [t_n,t_{n+1}]$

这时,我们需要对旋转的泰勒级数进行二阶截断:

$R(t_{n+1})\approx R(t_n)+\dot{R}(t_n)\Delta t+\frac{1}{2}\ddot{R}(t_n)\Delta t^2$

$\mathbf{q}(t_{n+1})\approx \mathbf{q}(t_n)+\dot{\mathbf{q}}(t_n)\Delta t+\frac{1}{2}\ddot{\mathbf{q}}(t_n)\Delta t^2$

其中 $\dot{R}(t_n),\ddot{R}(t_n),\dot{\mathbf{q}}(t_n),\ddot{\mathbf{q}}(t_n)$ 分别是旋转矩阵和四元数的一阶和二阶导数,可以用角速度表示。经过一系列推导,我们可以得到一阶积分公式:

$R_{n+1}\approx R_n+R_n[\bar{\mathbf{\omega }}{]}_{\times }\Delta t+\frac{\Delta t^2}{2}{R}_n([{\mathbf{\omega }}_n{]}_{\times }[{\mathbf{\omega }}_{n+1}{]}_{\times }-[{\mathbf{\omega }}_{n+1}{]}_{\times }[{\mathbf{\omega }}_n{]}_{\times })$

${\mathbf{q}}_{n+1}\approx {\mathbf{q}}_n\otimes \left(\exp (\bar{\mathbf{\omega }}\Delta t/2)+\frac{\Delta t^2}{24}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_n\times {\mathbf{\omega }}_{n+1}\end{array}\right]\right)$

其中 $\bar{\mathbf{\omega }}=({\mathbf{\omega }}_n+{\mathbf{\omega }}_{n+1})/2$ 是角速度的中值。

可以看到,一阶积分公式在零阶积分的基础上,增加了一个二阶修正项,与角速度的变化有关。当角速度变化较小时,这个修正项可以忽略;但当角速度变化较大时,这个修正项可以显著提高积分精度。

下面是一个 Python 例子,展示了如何使用一阶积分法估计旋转:

import numpy as np

def first_order_integration(R_prev, q_prev, omega_prev, omega_next, dt):
    """
    First-order integration of rotation.
    
    Parameters:
        R_prev (numpy.ndarray): Previous rotation matrix (3, 3).
        q_prev (numpy.ndarray): Previous quaternion (4,).
        omega_prev (numpy.ndarray): Previous angular velocity vector (3,).
        omega_next (numpy.ndarray): Next angular velocity vector (3,).
        dt (float): Time step.
        
    Returns:
        tuple: (R_next, q_next)
        - R_next (numpy.ndarray): Next rotation matrix (3, 3).
        - q_next (numpy.ndarray): Next quaternion (4,).
    """
    omega_mid = (omega_prev + omega_next) / 2
    omega_dot = (omega_next - omega_prev) / dt
    
    R_next = R_prev + R_prev @ so3_hat(omega_mid) * dt + 0.5 * dt**2 * R_prev @ (so3_hat(omega_prev) @ so3_hat(omega_next) - so3_hat(omega_next) @ so3_hat(omega_prev))
    
    q_next = quaternion_multiply(q_prev, quaternion_exp(omega_mid*dt/2) + np.array([0, *(omega_prev * dt**2 / 24)]))
    
    return R_next, q_next

# Example usage
R = np.eye(3)
q = np.array([1, 0, 0, 0])
omega_prev = np.array([0.1, 0.2, 0.3])
omega_next = np.array([0.2, 0.3, 0.4])
dt = 0.01

R_first_order, q_first_order = first_order_integration(R, q, omega_prev, omega_next, dt)
print("First-order integration:")
print("Rotation matrix:")
print(R_first_order)
print("Quaternion:")
print(q_first_order)

在这个例子中,我们定义了函数 first_order_integration,实现了一阶积分法。然后,我们构造了一个初始旋转 R 和 q,以及两个角速度向量 omega_prev 和 omega_next。给定时间步长 dt,我们使用一阶积分法估计下一时刻的旋转,并将结果打印出来。

需要注意的是,虽然一阶积分法的精度高于零阶法,但它的计算量也更大。在实际应用中,我们需要在精度和效率之间进行权衡,选择合适的积分方法。此外,还有许多其他的数值积分方法,如龙格-库塔法(Runge-Kutta methods),可以进一步提高精度,感兴趣的读者可以参考相关文献。

总之,旋转的时间积分是attitude estimation和控制中的一个基本问题。通过对角速度进行数值积分,我们可以估计旋转的变化,并用于状态更新、误差校正等目的。掌握不同的积分方法及其特点,对于设计和实现基于IMU的系统至关重要。

那么请你再回看下第一章结尾的IMU积分例子，一定觉得犹如拾芥，轻而易举。