机器人学数学基础

在机器人学中，精确描述机器人在三维空间中的位姿（pose）以及处理传感器数据中的不确定性，离不开一套严谨的数学工具。本章将系统性地介绍构成现代机器人学，尤其是状态估计与SLAM（Simultaneous Localization and Mapping）核心的三大数学支柱：三维坐标变换、概率论基础与最优化方法。这些工具不仅用于建模机器人的运动与感知，更是后端优化算法的理论基石。我们将从几何表示出发，逐步深入到代数结构与优化理论，确保读者不仅能使用这些工具，更能理解其背后的数学原理与局限性。

1. 三维空间坐标变换

机器人学中的坐标变换描述了从一种参考系（如世界坐标系）到另一种参考系（如机器人本体坐标系）的映射。这种映射是刚体运动（rigid body motion）的数学表达，包含旋转（rotation）和平移（translation）。完备的表示需要满足三个要求：无歧义性（每个姿态有唯一表示）、紧凑性（无冗余参数）、数值稳定性（便于插值与优化）。本节将依次探讨旋转矩阵、四元数、欧拉角三种主流表示，并引入李群SO(3)与李代数so(3)以解决非线性优化中的流形约束问题。

1.1 旋转矩阵与特殊正交群 SO(3)

旋转矩阵 (Rotation Matrix) 是最直观的旋转表示。一个三维旋转矩阵 $ \mathbf{R} \in \mathbb{R}^{3 \times 3} $ 是一个正交矩阵（$ \mathbf{R}^T \mathbf{R} = \mathbf{I} $）且行列式为+1（$ \det(\mathbf{R}) = 1 $）。它直接作用于向量，实现坐标旋转： $$ \mathbf{p}' = \mathbf{R} \mathbf{p} $$ 其中 $ \mathbf{p} $ 是原始坐标，$ \mathbf{p}' $ 是旋转后的坐标。旋转矩阵的9个元素受6个约束（3个正交性约束，1个行列式约束，但行列式可由正交性推导），故有3个自由度。

齐次变换矩阵 (Homogeneous Transformation Matrix) 将旋转与平移统一为一个 $4 \times 4$ 矩阵： $$ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}, \quad \mathbf{T} \in \mathrm{SE}(3) $$ 其中 $ \mathbf{t} \in \mathbb{R}^3 $ 是平移向量，$ \mathrm{SE}(3) $ 是三维特殊欧几里得群。变换作用于齐次坐标 $ \mathbf{p}^h = [\mathbf{p}^T, 1]^T $： $$ \mathbf{p}'^h = \mathbf{T} \mathbf{p}^h $$

性质与局限性：

• **优点**：直观，矩阵乘法即复合变换，逆变换为转置（$ \mathbf{R}^{-1} = \mathbf{R}^T $）。
• **缺点**：冗余（9个数表示3自由度），数值误差易破坏正交性，插值需特殊处理（如Slerp需先转四元数）。

代码示例：旋转矩阵的复合与逆 ```python import numpy as np

def compose_rotations(R1, R2): """复合旋转：先R2后R1""" return R1 @ R2

def rotation_inverse(R): """旋转矩阵的逆即转置""" return R.T

示例：绕Z轴旋转90度

Rz = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) print("Rz的逆：\n", rotation_inverse(Rz)) print("复合Rz @ Rz（旋转180度）：\n", compose_rotations(Rz, Rz)) ```

1.2 四元数与姿态表示

四元数 (Quaternion) 由爱尔兰数学家哈密顿提出，是扩展复数的高维数。单位四元数 $ \mathbf{q} = [q_w, q_x, q_y, q_z]^T $（或 $ \mathbf{q} = w + xi + yj + zk $）可无歧义表示三维旋转，且仅4个参数，是紧凑与稳定的折中。

四元数定义旋转轴 $ \mathbf{u} = [u_x, u_y, u_z]^T $（单位向量）与旋转角 $ \theta $： $$ \mathbf{q} = \begin{bmatrix} \cos(\theta/2) \\ \mathbf{u} \sin(\theta/2) \end{bmatrix} = \left[ \cos\frac{\theta}{2}, \sin\frac{\theta}{2} \cdot \mathbf{u}^T \right]^T $$ 注意 $ q_w $ 是标量部分，$ (q_x, q_y, q_z) $ 是向量部分。旋转操作作用于纯四元数 $ \mathbf{p} = [0, \mathbf{p}^T]^T $： $$ \mathbf{p}' = \mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{-1} $$ 其中 $ \otimes $ 是四元数乘法，$ \mathbf{q}^{-1} = [q_w, -\mathbf{q}_v]^T $（单位四元数逆即共轭）。

四元数乘法 对应旋转复合： $$ \mathbf{q}_1 \otimes \mathbf{q}_2 = \begin{bmatrix} q_{1w}q_{2w} - \mathbf{q}_{1v}^T \mathbf{q}_{2v} \\ q_{1w}\mathbf{q}_{2v} + q_{2w}\mathbf{q}_{1v} + \mathbf{q}_{1v} \times \mathbf{q}_{2v} \end{bmatrix} $$ 注意乘法不满足交换律，顺序至关重要：$ \mathbf{q}_1 \otimes \mathbf{q}_2 $ 表示先应用 $ \mathbf{q}_2 $ 再应用 $ \mathbf{q}_1 $。

四元数与旋转矩阵的转换：

• 四元数转旋转矩阵：

$$ \mathbf{R}(\mathbf{q}) = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix} $$

• 旋转矩阵转四元数（需数值稳定方法，如J. B. Kuipers算法）：

给定 $ \mathbf{R} $，计算： $$ q_w = \frac{1}{2} \sqrt{1 + \mathrm{tr}(\mathbf{R})}, \quad \mathbf{q}_v = \frac{1}{4q_w} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix} $$ 若 $ q_w $ 接近零，需选择最大对角元分支避免除零。

四元数插值 (Slerp)：用于平滑姿态过渡。给定 $ \mathbf{q}_0, \mathbf{q}_1 $，插值： $$ \mathrm{Slerp}(\mathbf{q}_0, \mathbf{q}_1; t) = \mathbf{q}_0 \frac{\sin((1-t)\Omega)}{\sin \Omega} + \mathbf{q}_1 \frac{\sin(t\Omega)}{\sin \Omega} $$ 其中 $ \Omega = \arccos(\mathbf{q}_0^T \mathbf{q}_1) $，$ t \in [0,1] $。Slerp保持单位模长，是恒定角速度旋转。

代码示例：四元数运算与转换 ```python import numpy as np

def quaternion_multiply(q1, q2): """四元数乘法 q1 ⊗ q2""" w1, x1, y1, z1 = q1 w2, x2, y2, z2 = q2 return np.array([ w1*w2 - x1*x2 - y1*y2 - z1*z2, w1*x2 + x1*w2 + y1*z2 - z1*y2, w1*y2 - x1*z2 + y1*w2 + z1*x2, w1*z2 + x1*y2 - y1*x2 + z1*w2 ])

def quaternion_to_rotation_matrix(q): """单位四元数转旋转矩阵""" w, x, y, z = q return np.array([ [1 - 2*(y*y + z*z), 2*(x*y - w*z), 2*(x*z + w*y)], [2*(x*y + w*z), 1 - 2*(x*x + z*z), 2*(y*z - w*x)], [2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x*x + y*y)] ])

示例：绕Z轴旋转90度的四元数

q = np.array([np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)]) # [w, x, y, z] R = quaternion_to_rotation_matrix(q) print("旋转矩阵：\n", R) ```

1.3 欧拉角与万向锁问题

欧拉角 (Euler Angles) 使用三个角度绕特定轴序列旋转，常见序列有ZYX（ aerospace 顺序，偏航-俯仰-滚转）、XYZ等。ZYX顺序下，旋转矩阵为： $$ \mathbf{R} = \mathbf{R}_z(\psi) \mathbf{R}_y(\theta) \mathbf{R}_x(\phi) $$ 其中 $ \phi $（滚转）、$ \theta $（俯仰）、$ \psi $（偏航）。从旋转矩阵反解欧拉角（ZYX）： $$ \theta = \arcsin(-R_{31}), \quad \phi = \arctan2(R_{32}, R_{33}), \quad \psi = \arctan2(R_{21}, R_{11}) $$ 当 $ \theta = \pm 90^\circ $（俯仰角为±90度）时，$ R_{31} = \mp 1 $，此时 $ \phi $ 与 $ \psi $ 的轴重合，失去一个自由度，称为万向锁 (Gimbal Lock)。这是欧拉角的主要缺陷，导致在奇异点附近数值不稳定。

优缺点：

• **优点**：直观，3个参数，易于人类理解与设定。
• **缺点**：存在奇异点（万向锁），插值非均匀，复合旋转需转矩阵再转回。

在机器人学中，欧拉角常用于直观显示（如RViz中的姿态显示），但不推荐用于优化。

1.4 李群 SO(3) 与李代数 so(3)

旋转矩阵集合构成特殊正交群 SO(3)： $$ \mathrm{SO}(3) = \{ \mathbf{R} \in \mathbb{R}^{3 \times 3} \mid \mathbf{R}^T \mathbf{R} = \mathbf{I}, \det(\mathbf{R}) = 1 \} $$ SO(3)是一个李群 (Lie Group)：它既是流形（manifold），又是群（满足封闭性、结合律、单位元、逆元）。其维度为3。

然而，许多优化问题（如非线性最小二乘）在欧氏空间求解更简便。我们需要将SO(3)映射到其切空间——李代数 so(3)，这是一个向量空间，允许使用梯度类方法。

李代数 so(3) 定义为SO(3)在单位元处的切空间，由所有三维反对称矩阵组成： $$ \mathrm{so}(3) = \{ \boldsymbol{\Phi} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{\Phi}^T = -\boldsymbol{\Phi} \} $$ 任意 $ \boldsymbol{\Phi} \in \mathrm{so}(3) $ 可表示为： $$ \boldsymbol{\Phi} = \boldsymbol{\phi}^\wedge = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} $$ 其中 $ \boldsymbol{\phi} = [\phi_1, \phi_2, \phi_3]^T \in \mathbb{R}^3 $ 是对应李代数元素的坐标向量（称为旋转向量或axis-angle的向量形式，$ \boldsymbol{\phi} = \theta \mathbf{u} $）。

指数映射 (Exponential Map)：将李代数元素映射到李群： $$ \exp(\boldsymbol{\phi}^\wedge) = \mathbf{R} \in \mathrm{SO}(3) $$ 对于小旋转，$ \exp(\boldsymbol{\phi}^\wedge) \approx \mathbf{I} + \boldsymbol{\phi}^\wedge $。一般形式为罗德里格斯公式： $$ \mathbf{R} = \exp(\boldsymbol{\phi}^\wedge) = \mathbf{I} + \frac{\sin \theta}{\theta} \boldsymbol{\phi}^\wedge + \frac{1-\cos \theta}{\theta^2} \boldsymbol{\phi}^{\wedge 2} $$ 其中 $ \theta = \|\boldsymbol{\phi}\| $。当 $ \theta \to 0 $，使用泰勒展开避免除零。

对数映射 (Logarithmic Map)：逆运算，将旋转矩阵映射回李代数： $$ \boldsymbol{\phi} = \log(\mathbf{R})^\vee $$ 计算公式： $$ \theta = \arccos\left(\frac{\mathrm{tr}(\mathbf{R}) - 1}{2}\right), \quad \boldsymbol{\phi} = \frac{1}{2\sin\theta} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix} $$ 当 $ \theta \approx 0 $（小旋转），使用近似 $ \boldsymbol{\phi} \approx \frac{1}{2} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix} $。

为什么需要李代数？ 在优化中，我们常需对旋转进行增量更新：$ \mathbf{R} \gets \exp(\boldsymbol{\delta \phi}^\wedge) \mathbf{R} $。若直接在SO(3)上使用梯度下降，需处理流形约束（正交性）。通过李代数，我们将约束优化转化为无约束的欧氏空间优化（在 $ \boldsymbol{\phi} $ 上），迭代后通过指数映射回群上。这是VIO（Visual-Inertial Odometry）与SLAM中姿态优化的标准做法。

代码示例：李代数指数与对数映射 ```python def so3_exp(phi): """李代数 so(3) 指数映射：phi (3,) -> R (3,3)""" theta = np.linalg.norm(phi) if np.isclose(theta, 0): return np.eye(3) phi_hat = np.array([[0, -phi[2], phi[1]], [phi[2], 0, -phi[0]], [-phi[1], phi[0], 0]]) return np.eye(3) + (np.sin(theta)/theta) * phi_hat + ((1-np.cos(theta))/(theta**2)) * (phi_hat @ phi_hat)

def so3_log(R): """李代数 so(3) 对数映射：R (3,3) -> phi (3,)""" theta = np.arccos((np.trace(R) - 1) / 2) if np.isclose(theta, 0): return np.array([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]]) / 2 phi_hat = (R - R.T) / (2*np.sin(theta)) phi = np.array([phi_hat[2,1], phi_hat[0,2], phi_hat[1,0]]) return theta * phi / np.linalg.norm(phi)

示例：小旋转

phi_small = np.array([0.01, -0.02, 0.005]) R_small = so3_exp(phi_small) phi_recovered = so3_log(R_small) print("原始p