机器人学的数学基石：李代数、概率图与矩阵论核心

当我们观察一辆自动驾驶汽车在城市道路中行驶时，它需要完成三个核心任务：通过激光雷达与摄像头估计自身在全局坐标系中的六自由度位姿，融合多传感器数据识别周围行人、车辆的位置与运动状态，最终规划出无碰撞的行驶路径并控制车辆精准执行。这三个任务看似分属感知、规划、控制三个独立模块，但其底层都依赖同一套数学语言——李群与李代数（Lie Group & Lie Algebra） 用于描述刚体位姿的流形结构，概率图模型（Probabilistic Graphical Model） 用于建模不确定性与多变量依赖关系，矩阵论（Matrix Theory） 工具则为大规模优化问题提供高效求解方案。本章将系统梳理机器人自主导航所需的这三类核心数学工具，既包含严格的数学推导，也配套可直接运行的工程实现代码，为后续SLAM、运动规划、鲁棒控制等章节的内容打下坚实基础。

---

1.1 李群与李代数：刚体位姿的流形表示与运算

刚体（Rigid Body）的位姿由旋转与平移两部分组成，其数学表示天然满足群（Group）与流形（Manifold）的结构：旋转矩阵满足乘法封闭、结合律、单位元与逆元存在，因此构成特殊正交群（Special Orthogonal Group, SO(3)）；旋转与平移组合的变换矩阵满足同样的群性质，构成特殊欧氏群（Special Euclidean Group, SE(3)）。李群与李代数的核心价值在于：它将流形上的约束优化问题转化为无约束的向量空间优化问题，避免了直接操作变换矩阵时破坏约束（如旋转矩阵不再正交、行列式不为1）的问题，是机器人位姿估计、运动规划、控制律设计的底层数学基础。

1.1.1 SO(3)与李代数so(3)：旋转的约束表示与无约束优化

SO(3)的定义为所有3×3实正交矩阵且行列式为1的集合，即： $$ \mathrm{SO}(3) = \left\{ R \in \mathbb{R}^{3 \times 3} \mid R^T R = I, \det(R) = 1 \right\} \tag{1.1} $$ 若直接将9维的旋转矩阵作为优化变量，需要额外添加6个约束条件保证结果属于SO(3)，大幅提升优化难度。而李代数$\mathfrak{so}(3)$是SO(3)在单位元处的切空间，每个元素对应一个3维向量$\boldsymbol{\omega} \in \mathbb{R}^3$，通过指数映射（Exponential Map） 与SO(3)的元素一一对应，且$\boldsymbol{\omega}$是无约束的向量，可直接在向量空间中进行优化。

罗德里格斯公式与指数映射

从几何直观来看，任意旋转都可以表示为绕某一单位轴$\hat{\boldsymbol{k}} \in \mathbb{R}^3$旋转角度$\theta$得到，对应的旋转矩阵满足罗德里格斯公式（Rodrigues' Formula）： $$ R = \exp(\boldsymbol{\omega}^\wedge) = I + \sin\theta \cdot \boldsymbol{\omega}^\wedge + (1 - \cos\theta) \cdot (\boldsymbol{\omega}^\wedge)^2 \tag{1.2} $$ 其中$\boldsymbol{\omega} = \theta \hat{\boldsymbol{k}}$是李代数$\mathfrak{so}(3)$的向量表示，$\boldsymbol{\omega}^\wedge$是$\boldsymbol{\omega}$对应的反对称矩阵（Skew-Symmetric Matrix），满足$\boldsymbol{a}^T \boldsymbol{\omega}^\wedge \boldsymbol{b} = \boldsymbol{\omega}^T (\boldsymbol{a} \times \boldsymbol{b})$，即反对称矩阵和向量的乘法等价于叉乘。反对称矩阵与向量的转换被称为Hat与Vee运算，是李代数代码实现的基础： $$ \boldsymbol{\omega}^\wedge = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}, \quad \text{Vee}(\boldsymbol{\omega}^\wedge) = \boldsymbol{\omega} \tag{1.3} $$

指数映射的推导可以通过泰勒展开验证：将$\exp(\boldsymbol{\omega}^\wedge)$按泰勒级数展开，利用$(\boldsymbol{\omega}^\wedge)^3 = -\|\boldsymbol{\omega}\|^2 \boldsymbol{\omega}^\wedge$的性质，可将无穷级数合并为正弦与余弦项，最终得到式(1.2)。该映射是双射，存在对应的对数映射（Logarithmic Map），可从旋转矩阵$R$反推得到对应的李代数向量$\boldsymbol{\omega} = \log(R)$，其闭式解为： $$ \theta = \arccos\left( \frac{\mathrm{trace}(R) - 1}{2} \right), \quad \boldsymbol{\omega} = \frac{\theta}{2\sin\theta} \begin{bmatrix} R_{32} - R_{23} \\ R_{13} - R_{31} \\ R_{21} - R_{12} \end{bmatrix} \tag{1.4} $$ 当$\theta$接近0时，需用泰勒近似$\boldsymbol{\omega} \approx \frac{1}{2} \begin{bmatrix} R_{32}-R_{23} & R_{13}-R_{31} & R_{21}-R_{12} \end{bmatrix}^T$避免除以0的数值问题。

工程代码实现

以下Python代码实现了$\mathfrak{so}(3)$的Hat/Vee运算、指数/对数映射，并验证了$\exp(\log(R)) \approx R$的数值稳定性： ```python import numpy as np

def hat(v: np.ndarray) -> np.ndarray: """向量转反对称矩阵 (Hat运算)""" v = v.flatten() return np.array([ [0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0] ])

def vee(omega_hat: np.ndarray) -> np.ndarray: """反对称矩阵转向量 (Vee运算)""" return np.array([omega_hat[2,1], omega_hat[0,2], omega_hat[1,0]])

def so3_exp(omega: np.ndarray) -> np.ndarray: """so3指数映射: 李代数向量 -> SO(3)旋转矩阵""" theta = np.linalg.norm(omega) if theta < 1e-8: return np.eye(3) + hat(omega) omega_hat = hat(omega) return np.eye(3) + np.sin(theta)/theta * omega_hat + (1 - np.cos(theta))/(theta**2) * (omega_hat @ omega_hat)

def so3_log(R: np.ndarray) -> np.ndarray: """so3对数映射: SO(3)旋转矩阵 -> 李代数向量""" trace = np.trace(R) theta = np.arccos(np.clip((trace - 1)/2, -1.0, 1.0)) if theta < 1e-8: return vee(R - np.eye(3)) omega_hat = (theta / (2 * np.sin(theta))) * (R - R.T) return vee(omega_hat)

测试用例: 绕z轴旋转90度，验证exp(log(R)) ≈ R

R_test = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) omega = so3_log(R_test) R_recon = so3_exp(omega) print("重构误差:", np.linalg.norm(R_recon - R_test)) # 输出应小于1e-15 ``` 💡 工程提示：在实际的SLAM与导航系统中，所有位姿的增量更新、雅可比计算、非线性优化均基于李代数实现，避免直接操作变换矩阵导致的约束破坏，这也是g2o、Ceres、GTSAM等主流优化库的位姿参数化标准方案。

1.1.2 SE(3)与李代数se(3)：六自由度位姿的完整表示

机器人的六自由度位姿由3维旋转与3维平移组成，对应特殊欧氏群SE(3)，其元素为4×4齐次变换矩阵： $$ \mathrm{SE}(3) = \left\{ T = \begin{bmatrix} R & \boldsymbol{t} \\ \boldsymbol{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid R \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^3 \right\} \tag{1.5} $$ 对应的李代数$\mathfrak{se}(3)$是6维向量$\boldsymbol{\xi} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\omega} \end{bmatrix}$，其中$\boldsymbol{\rho} \in \mathbb{R}^3$是平移部分的线性速度，$\boldsymbol{\omega} \in \mathbb{R}^3$是旋转部分的角速度，其对应的4×4反对称矩阵为： $$ \boldsymbol{\xi}^\wedge = \begin{bmatrix} \boldsymbol{\omega}^\wedge & \boldsymbol{\rho} \\ \boldsymbol{0}^T & 0 \end{bmatrix} \tag{1.6} $$

指数映射与位姿增量更新

$\mathfrak{se}(3)$的指数映射可得到SE(3)的变换矩阵，其闭式解为： $$ \exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix} \exp(\boldsymbol{\omega}^\wedge) & (\boldsymbol{I} - \exp(\boldsymbol{\omega}^\wedge)) \boldsymbol{\omega}^\wedge \boldsymbol{\rho} + \frac{\boldsymbol{\omega} \boldsymbol{\omega}^T}{\|\boldsymbol{\omega}\|^2} \boldsymbol{\rho} \theta \\ \boldsymbol{0}^T & 1 \end{bmatrix} \tag{1.7} $$ 其中$\theta = \|\boldsymbol{\omega}\|$，当$\theta$接近0时需用泰勒展开近似避免数值奇异性。该映射的核心工程价值在于位姿的增量更新：若当前机器人位姿为$T$，位姿增量为$\Delta \boldsymbol{\xi}$，则更新后的位姿为： $$ T' = T \exp(\Delta \boldsymbol{\xi}^\wedge) \tag{1.8} $$ 该式是机器人位姿优化、运动控制、传感器融合中最常用的运算，例如激光里程计的帧间匹配、视觉里程计的位姿迭代优化，均通过不断求解增量$\Delta \boldsymbol{\xi}$并更新位姿实现收敛。

代码实现与验证

扩展1.1.1的代码，实现$\mathfrak{se}(3)$的指数与对数映射： ```python def se3_exp(xi: np.ndarray) -> np.ndarray: """se3指数映射: 6维李代数向量 -> SE(3)变换矩阵""" rho = xi[:3].flatten() omega = xi[3:].flatten() theta = np.linalg.norm(omega) R = so3_exp(omega) if theta < 1e-8: V = np.eye(3) + 0.5 * hat(omega) else: omega_hat = hat(omega) V = (np.eye(3) + (1 - np.cos(theta))/(theta**2) * omega_hat + (theta - np.sin(theta))/(theta**3) * (omega_hat @ omega_hat)) t = V @ rho T = np.eye(4) T[:3, :3] = R T[:3, 3] = t return T

def se3_log(T: np.ndarray) -> np.ndarray: """se3对数映射: SE(3)变换矩阵 -> 6维李代数向量""" R = T[:3, :3] t = T[:3, 3] omega = so3_log(R) theta = np.linalg.norm(omega) if theta < 1e-8: V_inv = np.eye(3) - 0.5 * hat(omega) else: omega_hat = hat(omega) V_inv = (np.eye(3) - 0.5 * hat(omega) + (1/theta**2 - (1 + np.cos(theta))/(2*theta*np.sin(theta))) * (omega_hat @ omega_hat)) rho = V_inv @ t return np.hstack([rho, omega])

测试用例: 绕z轴转90度，平移[1,0,0]

T_test = np.eye(4) T_test[:3, :3] = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) T_test[:3, 3] = [1, 0, 0] xi = se3_log(T_test) T_recon = se3_exp(xi) print("SE(3)重构误差:", np.linalg.norm(T_recon - T_test)) # 输出应小于1e-14 ```

---

1.2 概率图模型：不确定性与多变量依赖的建模框架

机器人系统天然充满不确定性：激光雷达的测距误差可达厘米级，IMU的零偏会随时间漂移，轮子打滑会导致运动模型失真，动态行人的运动轨迹无法精确预测。概率图模型（Probabilistic Graphical Model, PGM）是建模不确定性、表示多变量依赖关系的高效工具，它通过图结构将复杂的

多变量联合概率分布转化为局部条件概率的乘积，大幅降低概率推理的计算复杂度。根据图结构的边是否有向，概率图模型主要分为贝叶斯网络（Bayesian Network, 有向图）与马尔可夫随机场（Markov Random Field, 无向图）两大类，因子图（Factor Graph）作为无向图的特殊形式，因其对因子分解的显式表示，成为机器人优化问题的主流建模工具。

1.2.1 贝叶斯网络：有向依赖的概率表示与推理

贝叶斯网络是有向无环图（DAG），节点表示随机变量，有向边表示变量间的条件依赖关系，全联合概率分布可分解为各节点条件概率的乘积： $$ p(x_1, x_2, \dots, x_n) = \prod_{i=1}^n p(x_i \mid \text{Pa}(x_i)) \tag{1.9} $$ 其中$\text{Pa}(x_i)$是$x_i$的父节点集合。贝叶斯网络的核心价值是通过有向边显式表示因果依赖关系，适合建模时序系统的状态转移与观测过程。

以移动机器人的里程计估计为例：假设机器人$t-1$时刻的位姿为$x_{t-1}$，运动模型给出$t$时刻的位姿先验$x_t \sim \mathcal{N}(f(x_{t-1}, u_t), \Sigma_u)$，其中$u_t$是轮速计/IMU的输入，$\Sigma_u$是运动噪声协方差；激光雷达在$x_t$处观测到环境特征$z_t \sim \mathcal{N}(h(x_t, m), \Sigma_z)$，其中$m$是全局地图，$\Sigma_z$是观测噪声协方差。对应的贝叶斯网络结构为$x_{t-1} \rightarrow x_t \rightarrow z_t$，以及$x_t \rightarrow z_t$与地图$m$的依赖，其Mermaid结构示意图如下： ```mermaid graph TD A[x_{t-1} <br/> 前一时刻位姿] --> B[x_t <br/> 当前时刻位姿] B --> C[z_t <br/> 激光观测] D[m <br/> 全局地图] --> C E[u_t <br/> 运动输入] --> B style A fill:#f9f,stroke:#333,stroke-width:2px style B fill

多变量联合概率分布转化为局部条件概率的乘积，大幅降低概率推理的计算复