李群 SE(3) 与具身变换

本章建立物理运动流形 $\mathcal{M}_{\mathrm{phy}}$ 的核心数学工具箱：$\mathrm{SE}(3)$ 的李群几何。从刚体运动的群结构出发，依次给出李代数、指数/对数映射、测地距离、黎曼运动规划，最后建立 $\mathrm{SE}(3)$ 几何与CBF执行安全的精确联系。

4.1 刚体运动的群结构：李群、李代数与 BCH 公式

4.1.1 为什么刚体运动是群

两个刚体变换的顺序执行（先做A再做B）仍然是一个刚体变换；存在恒等变换（不动）；每个变换都有逆（反向变换）；结合律成立。这正是群（group）的公理。$\mathrm{SE}(3)$ 不仅是群，还是光滑流形，且群乘法和取逆都是光滑映射——这是李群（Lie group）的定义。

李群结构的意义在于：它允许我们在群元素（位姿）上做微积分，而微积分的”线性化”发生在李代数 $\mathfrak{se}(3)$（切空间，线性空间）上，而非在非线性的 $\mathrm{SE}(3)$ 本身上。大量的实际计算（速度合成、误差传播、优化）在李代数中进行，然后通过指数映射”推回”到 $\mathrm{SE}(3)$。

4.1.2 李代数 se(3)：切空间的代数结构

$\mathrm{SE}(3)$ 在单位元处的切空间 $\mathfrak{se}(3)$ 是一个6维向量空间，元素形如 $4\times 4$ 矩阵：

$\xi^\wedge = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix} \in \mathfrak{se}(3)$

其中 $\omega \in \mathbb{R}^3$ 是角速度向量，$v \in \mathbb{R}^3$ 是线速度向量，$[\omega]_\times$ 是 $\omega$ 的反对称矩阵表示。李括号 $[\xi_1^\wedge, \xi_2^\wedge] = \xi_1^\wedge \xi_2^\wedge - \xi_2^\wedge \xi_1^\wedge$ 捕获了两个速度向量”不对易性”——这是姿态变换中旋转轴顺序依赖性的数学表达。

4.1.3 BCH 公式与近似合成

Baker-Campbell-Hausdorff（BCH）公式给出 $\mathrm{SE}(3)$ 中两个元素乘积的李代数表达：

$\ln(\exp(\xi_1^\wedge)\exp(\xi_2^\wedge)) = \xi_1 + \xi_2 + \frac{1}{2}[\xi_1, \xi_2] + \frac{1}{12}([\xi_1,[\xi_1,\xi_2]] + [\xi_2,[\xi_2,\xi_1]]) + \cdots$

在小扰动假设下（$\|\xi_1\|, \|\xi_2\|$ 较小），高阶项可以截断，给出一阶近似 $\ln(\exp(\xi_1)\exp(\xi_2)) \approx \xi_1 + \xi_2$——这是李群上线性化的理论基础，在扩展卡尔曼滤波（EKF）中被广泛使用。

写作占位 — $\mathrm{SE}(3)$ 与 $\mathrm{SO}(3) \times \mathbb{R}^3$ 的关系澄清（半直积结构）

4.2 指数映射与对数映射：se(3) ↔ SE(3) 的桥梁

4.2.1 指数映射：从速度到位姿

指数映射 $\exp: \mathfrak{se}(3) \to \mathrm{SE}(3)$ 将李代数中的元素（“速度”）映射到李群中的元素（“位姿变化”），定义为矩阵指数：

$\exp(\xi^\wedge) = I + \xi^\wedge + \frac{(\xi^\wedge)^2}{2!} + \frac{(\xi^\wedge)^3}{3!} + \cdots$

对 $\mathrm{SE}(3)$，这个级数有闭合形式（通过Rodrigues公式的推广），不需要计算无穷级数。指数映射的物理意义：沿速度向量 $\xi$ 以单位时间运动的位姿变化。

在实际计算中，指数映射用于：给定当前位姿 $T \in \mathrm{SE}(3)$ 和控制输入 $\xi \in \mathfrak{se}(3)$，下一时刻位姿为 $T' = T \cdot \exp(\xi^\wedge \cdot \Delta t)$——这是 $\mathrm{SE}(3)$ 上运动方程的积分形式，替代欧氏坐标中的简单加法。

4.2.2 对数映射：从位姿差到速度

对数映射 $\ln: \mathrm{SE}(3) \to \mathfrak{se}(3)$ 是指数映射的逆（在单位元邻域内），将两个位姿之间的差异映射到李代数中的”速度向量”。给定两个位姿 $T_1, T_2 \in \mathrm{SE}(3)$，它们之间的”差”定义为 $\ln(T_1^{-1} T_2) \in \mathfrak{se}(3)$，其范数 $\|\ln(T_1^{-1} T_2)\|$ 正是 $T_1$ 和 $T_2$ 之间的测地距离。

对数映射在EICPS中的核心应用是误差计算：给定参考位姿 $T_{\mathrm{ref}}$ 和当前位姿 $T_{\mathrm{cur}}$，位姿误差 $e = \ln(T_{\mathrm{ref}}^{-1} T_{\mathrm{cur}}) \in \mathfrak{se}(3)$ 是李代数中的6维向量，可以直接用于比例-微分控制律的计算，而不引入欧氏近似误差。

4.2.3 奇异点与避奇处理

指数映射在 $\mathrm{SO}(3)$ 分量上存在奇异点：当旋转角恰好为 $\pm\pi$ 时（对极点），对数映射不唯一。在带电作业场景中，姿态接近180度翻转的情形虽然少见，但需要在实现中明确处理（通过四元数表示的双重覆盖，或通过避免接近奇异点的路径约束）。

写作占位 — 指数/对数映射闭合形式的推导（Rodrigues旋转公式推广）

4.3 SE(3) 上的测地距离与平行移动

4.3.1 测地线与最短路径

$\mathrm{SE}(3)$ 上两点 $T_1, T_2$ 之间的测地距离 $d_{\mathrm{SE}(3)}(T_1, T_2) = \|\ln(T_1^{-1} T_2)\|$ 是具身空间中”位姿距离”的正确度量。在带 左不变黎曼度量的 $\mathrm{SE}(3)$ 上，测地线（最短路径）由指数映射的单参数子群给出：从 $T_1$ 出发沿速度 $\xi = \ln(T_1^{-1} T_2)$ 以均匀速度运动的曲线。

与欧氏插值（对位置和旋转分量分别做线性插值）相比，沿 $\mathrm{SE}(3)$ 测地线的运动具有以下保证：路径是最短的；旋转速度恒定（没有加速减速效应）；不经过奇异点（在足够小的邻域内）。

4.3.2 平行移动与协变导数

在弯曲流形上，“方向不变地沿曲线移动切向量”需要用平行移动（parallel transport）来精确定义，而非欧氏空间中简单的平行移动（保持分量不变）。$\mathrm{SE}(3)$ 上的平行移动由Levi-Civita联络定义，是协变导数（描述切向量沿曲线的变化率）的核心。

在EICPS中，平行移动在CBF约束的导数计算中出现：当安全集 $\mathcal{S}$ 在 $\mathrm{SE}(3)$ 上定义，CBF条件 $\dot{h}(T, \xi) + \alpha(h(T)) \geq 0$ 中的 $\dot{h}$ 需要通过协变导数正确计算，而非欧氏导数。

4.3.3 Fréchet 均值与多位姿融合

$\mathrm{SE}(3)$ 上的Fréchet均值将欧氏空间中的”算术平均”推广到流形上：给定 $n$ 个位姿 $\{T_1, \ldots, T_n\}$，其Fréchet均值是最小化测地距离平方和的位姿 $\bar{T} = \arg\min_{T} \sum_i d_{\mathrm{SE}(3)}(T, T_i)^2$。在EKF的多传感器融合中，融合多个位姿估计需要在 $\mathrm{SE}(3)$ 上定义的均值，而非欧氏分量的平均。

写作占位 — 测地距离与欧氏距离的数值比较图（展示两者在高曲率区域的差异）

4.4 黎曼运动规划：具身变换的最优路径

4.4.1 在 SE(3) 上的最优控制问题

黎曼运动规划的核心问题：在 $\mathrm{SE}(3)$ 上，从初始位姿 $T_0$ 到目标位姿 $T_f$，在满足约束（安全间距、关节限位、动力学可行性）的前提下，沿最优测地路径运动。这不是欧氏空间中的直线插值问题，而是带约束的黎曼流形上的变分问题。

在安全约束（CBF）的框架下，最优控制问题变为：最小化路径长度（关于 $\mathrm{SE}(3)$ 测地度量），同时保持轨迹在安全集的前向不变集合内。这可以用QP（二次规划）框架求解，其中CBF约束作为线性化的安全约束加入QP。

4.4.2 测地线与障碍物的交互处理

当测地线（最短路径）穿越障碍物（带电体附近的禁区）时，需要将最优路径偏转到安全集边界附近。这在黎曼框架下有两种处理方式：（1）修改黎曼度量（在障碍物附近增加度量，使得”通过障碍物”的路径长度趋于无穷）；（2）在QP框架中将CBF约束直接投影到控制输入空间。

EICPS选择第二种方式，因为它与实时控制框架更自然地结合，且CBF约束的激活/不激活状态可以作为形式证据链的一部分被记录和审计。

4.4.3 多目标位姿序列的规划

带电作业任务通常由一系列目标位姿构成（接近杆塔→定位绝缘子→执行操作→撤离）。全局最优路径是这些中间位姿序列的联合优化问题，在 $\mathrm{SE}(3)$ 上对应多段测地线的拼接，需要在每段接合点处处理速度连续性（切向量的匹配条件）。HTN规划器（第8章）将这个序列分解问题与 $\mathrm{SE}(3)$ 运动规划对接。

写作占位 — 带障碍约束的SE(3)运动规划算法伪代码

4.5 SE(3) 度量与执行安全的几何联系

4.5.1 安全集在 SE(3) 上的几何定义

带电作业的安全约束可以在 $\mathrm{SE}(3)$ 上被几何地表述：安全集 $\mathcal{S} = \{T \in \mathrm{SE}(3) : h(T) \geq 0\}$ 其中 $h(T)$ 是机械臂末端执行器与最近带电体之间的距离（依电压等级设定阈值）。$h$ 是 $\mathrm{SE}(3)$ 上的光滑函数，其梯度（在 $\mathrm{SE}(3)$ 度量下的梯度，即 $\mathfrak{se}(3)$ 中的元素）给出使机械臂”最快远离危险”的速度方向。

4.5.2 CBF 在 SE(3) 框架下的正确形式

经典CBF理论在欧氏状态空间 $\mathbb{R}^n$ 中建立。在 $\mathrm{SE}(3)$ 框架下，需要用李导数（Lie derivative）替代普通方向导数：

$\mathcal{L}_f h(T) = \langle \mathrm{grad}_{\mathrm{SE}(3)} h(T), f(T, u) \rangle_{\mathfrak{se}(3)}$

其中 $f(T, u) \in \mathfrak{se}(3)$ 是控制输入在李代数中的表示，$\mathrm{grad}_{\mathrm{SE}(3)} h$ 是 $h$ 关于 $\mathrm{SE}(3)$ 度量的黎曼梯度。CBF条件变为 $\mathcal{L}_f h(T) + \alpha(h(T)) \geq 0$，在 $\mathrm{SE}(3)$ 的内蕴度量下计算，确保安全判定的几何正确性。

4.5.3 度量选择对安全保证的影响

$\mathrm{SE}(3)$ 上存在多种左不变黎曼度量，对应不同的”位置-姿态”权衡。不同的度量给出不同的测地距离，从而给出不同的”最近障碍物方向”估计。在工程实现中，需要根据具体任务（带电作业的安全距离定义是三维欧氏距离，而非 $\mathrm{SE}(3)$ 上的”自然”度量）选择合适的度量，并在CBF设计中明确这一选择。这是 $\mathrm{SE}(3)$ 几何与工程安全约束之间的精确接口设计问题，在第8章（CBF设计）中详细处理。

本章完成了 $\mathcal{M}_{\mathrm{phy}}$ 的几何工具箱。第5章转向 $\mathcal{M}_{\mathrm{sem}}$，建立语义覆盖率 $\mathrm{SEC}$ 的形式化理论与规范闭合性证明。

参考文献

写作占位 — 本章参考文献列表（随正文写作逐步完善）