本章介绍机器人控制中的核心数学工具。我们将深入探讨李群与李代数如何优雅地描述刚体运动，理解凸优化与非凸优化在轨迹规划中的应用，以及变分原理如何为最优控制提供理论基础。通过SE(3)上的轨迹优化案例，您将看到这些数学工具如何在实际问题中发挥作用。
学习目标：
在机器人学中，刚体的位姿（位置和姿态）是最基本的状态描述。传统的欧拉角表示存在奇异性问题（万向节锁），而四元数虽然避免了奇异性，但需要额外的归一化约束。李群理论提供了一个统一、优雅的框架来处理刚体运动，其优势包括：
历史背景：李群理论由挪威数学家Sophus Lie在19世纪末创立，最初用于研究微分方程的对称性。20世纪中叶，随着机器人学的发展，研究者发现李群是描述刚体运动的完美工具。Murray、Li和Sastry的专著《A Mathematical Introduction to Robotic Manipulation》(1994)系统地将李群理论引入机器人学，成为该领域的经典。
特殊正交群SO(3)描述了三维空间中的旋转，是一个3维紧致李群：
\[SO(3) = \{R \in \mathbb{R}^{3\times3} | R^TR = I, \det(R) = 1\}\]几何解释：SO(3)是9维欧氏空间$\mathbb{R}^{3\times3}$中的一个3维流形。正交性约束$R^TR = I$提供6个独立约束（对称矩阵），行列式约束提供1个约束，但由于正交性已经保证$\det(R) = \pm 1$，所以实际维度为$9 - 6 = 3$。
拓扑性质：SO(3)拓扑上同构于三维实射影空间$\mathbb{RP}^3$，它是通过将三维球面$S^3$的对径点等同得到的。这意味着SO(3)是连通但非单连通的（基本群$\pi_1(SO(3)) = \mathbb{Z}_2$）。
SO(3)的李代数so(3)是反对称矩阵的集合：
\[\mathfrak{so}(3) = \{[\omega]_\times \in \mathbb{R}^{3\times3} | [\omega]_\times = -[\omega]_\times^T\}\]其中$[\omega]_\times$是向量$\omega \in \mathbb{R}^3$的反对称矩阵（也称为帽子算子）：
\[[\omega]_\times = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}\]关键性质：
指数映射将李代数元素映射到李群（将角速度映射为旋转）：
\[\exp([\omega]_\times) = I + \frac{\sin\theta}{\theta}[\omega]_\times + \frac{1-\cos\theta}{\theta^2}[\omega]_\times^2\]| 其中$\theta = | \omega | $，这就是著名的Rodrigues公式（也称为轴角表示）。 |
推导要点：利用$[\omega]\times^3 = -\theta^2[\omega]\times$和泰勒展开： \(\exp([\omega]_\times) = \sum_{k=0}^{\infty} \frac{1}{k!}[\omega]_\times^k\)
特殊情况：
SE(3)描述了刚体的完整变换（旋转+平移），是6维李群：
\[SE(3) = \left\{T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \bigg| R \in SO(3), p \in \mathbb{R}^3 \right\}\]群运算：
物理意义：变换矩阵$T$将点从物体坐标系映射到世界坐标系： \(p_{world} = Rp_{body} + p\)
SE(3)的李代数se(3)为：
\[\mathfrak{se}(3) = \left\{\xi^\wedge = \begin{bmatrix} [\omega]_\times & v \\ 0 & 0 \end{bmatrix} \bigg| \omega, v \in \mathbb{R}^3 \right\}\]其中$\xi = [v^T, \omega^T]^T \in \mathbb{R}^6$称为扭转(twist)，表示刚体的瞬时运动：
指数映射（从速度到位移）： \(\exp(\xi^\wedge) = \begin{bmatrix} \exp([\omega]_\times) & J(\omega)v \\ 0 & 1 \end{bmatrix}\)
其中$J(\omega)$是左雅可比矩阵： \(J(\omega) = I + \frac{1-\cos\theta}{\theta^2}[\omega]_\times + \frac{\theta - \sin\theta}{\theta^3}[\omega]_\times^2\)
特殊情况：
伴随表示(Adjoint representation)描述了李群元素对李代数元素的线性作用，是坐标变换的核心工具：
\[\text{Ad}_T = \begin{bmatrix} R & [p]_\times R \\ 0 & R \end{bmatrix} \in \mathbb{R}^{6\times6}\]几何意义：伴随变换描述了扭转在不同参考系间的变换关系。对于变换$T \in SE(3)$和扭转$\xi \in se(3)$： \(T \exp(\xi^\wedge) T^{-1} = \exp((\text{Ad}_T \xi)^\wedge)\)
这在计算空间速度和体速度之间的转换时至关重要：
\[\xi^s = \text{Ad}_T \xi^b\]其中：
伴随表示的性质：
小伴随表示(adjoint of Lie algebra)： \(\text{ad}_\xi = \begin{bmatrix} [\omega]_\times & [v]_\times \\ 0 & [\omega]_\times \end{bmatrix}\)
满足李括号关系：$[\xi_1, \xi_2] = \text{ad}_{\xi_1} \xi_2$
微分关系： \(\frac{d}{dt}\text{Ad}_T = \text{Ad}_T \text{ad}_{\xi^b} = \text{ad}_{\xi^s} \text{Ad}_T\)
对于n自由度串联机械臂，末端执行器的位姿可以用指数积(Product of Exponentials, PoE)公式表示：
\[T_{0n}(\theta) = \prod_{i=1}^n \exp(\xi_i^\wedge \theta_i) T_{0n}(0)\]其中：
关节扭转的计算：
空间雅可比矩阵： 速度运动学关系$\dot{x} = J_s(\theta)\dot{\theta}$，其中空间雅可比为：
\[J_s = [\xi_1', \xi_2', ..., \xi_n']\]其中第i列为： \(\xi_i' = \begin{cases} \xi_1 & i = 1 \\ \text{Ad}_{T_{01}...T_{0(i-1)}}\xi_i & i > 1 \end{cases}\)
体雅可比矩阵： 相对于末端坐标系的雅可比： \(J_b = \text{Ad}_{T_{0n}^{-1}} J_s\)
雅可比的应用：
计算优化技巧：
凸优化是现代机器人控制的基石，其核心优势在于任何局部最优解都是全局最优解。这一性质保证了算法的收敛性和解的唯一性（对于严格凸问题）。
标准形式： \(\begin{align} \min_{x} \quad & f(x) \\ \text{s.t.} \quad & g_i(x) \leq 0, \quad i = 1,...,m \\ & h_j(x) = 0, \quad j = 1,...,p \end{align}\)
其中$f(x)$是凸函数，$g_i(x)$是凸函数，$h_j(x)$是仿射函数（$h_j(x) = a_j^Tx + b_j$）。
凸集的定义与性质：
集合$\mathcal{C}$是凸的，当且仅当对于任意$x, y \in \mathcal{C}$和$\theta \in [0,1]$，有： \(\theta x + (1-\theta)y \in \mathcal{C}\)
常见的凸集：
| 超平面：${x | a^Tx = b}$ |
| 半空间：${x | a^Tx \leq b}$ |
| 球：${x | x - x_c | \leq r}$ |
| 椭球：${x | (x-x_c)^TP^{-1}(x-x_c) \leq 1}$，$P \succ 0$ |
| 多面体：${x | Ax \preceq b}$（有限个半空间的交集） |
| 锥：${x | x = \theta y, y \in \mathcal{C}, \theta \geq 0}$ |
保凸运算：
| 仿射变换：$f(\mathcal{C}) = {Ax + b | x \in \mathcal{C}}$是凸集 |
| 透视变换：$P(\mathcal{C}) = {x/t | (x,t) \in \mathcal{C}, t > 0}$ |
凸函数的判定：
常见凸函数：
| 范数：$ | x | _p$，$p \geq 1$ |
保凸函数运算：
二次规划是机器人控制中最常见的凸优化问题，广泛应用于MPC、WBC和逆运动学：
\[\begin{align} \min_{x} \quad & \frac{1}{2}x^TQx + c^Tx \\ \text{s.t.} \quad & Ax \leq b \\ & A_{eq}x = b_{eq} \\ & l \leq x \leq u \end{align}\]其中$Q \succeq 0$（半正定）保证问题的凸性，$Q \succ 0$（正定）保证唯一解。
KKT最优性条件： 点$x^$是最优解的充要条件是存在对偶变量$\lambda^, \nu^*$使得：
对偶可行性：$\lambda^* \geq 0$
求解算法：
在机器人控制中的应用：
逆运动学： \(\min_{\dot{q}} ||J\dot{q} - v_{des}||^2 + \alpha||\dot{q}||^2\)
力分配： \(\min_{f} ||f||^2 \quad \text{s.t.} \quad Gf = \tau_{des}, \quad f \in \mathcal{F}\)
轨迹跟踪： \(\min_{u} \sum_{k} ||x_k - x_{ref}||_Q^2 + ||u_k||_R^2\)
半定规划扩展了线性规划到矩阵变量：
\[\begin{align} \min_{X} \quad & \text{tr}(CX) \\ \text{s.t.} \quad & \text{tr}(A_iX) = b_i, \quad i = 1,...,m \\ & X \succeq 0 \end{align}\]SDP在接触力优化和稳定性分析中有重要应用。
非凸优化问题普遍存在于机器人学中，如逆运动学、碰撞避免等。
序列二次规划(SQP)：将非线性问题在当前点线性化，求解QP子问题：
\[\begin{align} \min_{\delta x} \quad & \frac{1}{2}\delta x^T \nabla^2 L(x_k, \lambda_k) \delta x + \nabla f(x_k)^T \delta x \\ \text{s.t.} \quad & g(x_k) + \nabla g(x_k)^T \delta x \leq 0 \\ & h(x_k) + \nabla h(x_k)^T \delta x = 0 \end{align}\]内点法：通过障碍函数将约束融入目标函数：
\[\phi_\mu(x) = f(x) - \mu \sum_{i=1}^m \log(-g_i(x))\]随着$\mu \to 0$，解收敛到原问题的解。
对于非凸问题，凸松弛提供了可行解的下界：
凸包络：函数$f$的凸包络是最大的凸函数$g$满足$g(x) \leq f(x)$。
SOS(Sum-of-Squares)松弛：将非负多项式约束松弛为SOS约束：
\[p(x) \geq 0 \quad \Rightarrow \quad p(x) = \sum_i q_i^2(x)\]这可以转换为SDP问题求解。
轨迹优化中的障碍避免： 非凸约束$||x - x_{obs}|| \geq r$可以通过引入二元变量转换为混合整数规划：
\[\begin{align} x - x_{obs} &\geq r - M(1-b) \\ x_{obs} - x &\geq r - Mb \\ b &\in \{0, 1\} \end{align}\]其中$M$是足够大的常数。
变分法研究泛函的极值问题。对于泛函：
\[J[x] = \int_{t_0}^{t_f} L(x(t), \dot{x}(t), t) dt\]其极值条件由Euler-Lagrange方程给出：
\[\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{x}}\right) - \frac{\partial L}{\partial x} = 0\]第一变分：泛函$J$在$x$处沿方向$\delta x$的第一变分为：
\[\delta J = \lim_{\epsilon \to 0} \frac{J[x + \epsilon \delta x] - J[x]}{\epsilon}\]极值的必要条件是$\delta J = 0$。
Hamilton原理指出，系统在两个时刻之间的真实轨迹使作用量取驻值：
\[\delta S = \delta \int_{t_0}^{t_f} L(q, \dot{q}, t) dt = 0\]其中$L = T - V$是Lagrangian，$T$是动能，$V$是势能。
对于机器人系统：
\[L = \frac{1}{2}\dot{q}^T M(q) \dot{q} - V(q)\]导出运动方程：
\[M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = \tau\]对于控制系统：
\[\dot{x} = f(x, u, t)\]优化目标：
\[J = \phi(x(t_f)) + \int_{t_0}^{t_f} L(x, u, t) dt\]定义Hamiltonian：
\[H(x, u, \lambda, t) = L(x, u, t) + \lambda^T f(x, u, t)\]极大值原理指出，最优控制$u^*$满足：
定义值函数：
\[V(x, t) = \min_{u[t,t_f]} \left\{\phi(x(t_f)) + \int_t^{t_f} L(x, u, s) ds\right\}\]Hamilton-Jacobi-Bellman方程：
\[-\frac{\partial V}{\partial t} = \min_u \left\{L(x, u, t) + \frac{\partial V}{\partial x}^T f(x, u, t)\right\}\]最优控制律：
\[u^*(x, t) = \arg\min_u \left\{L(x, u, t) + \frac{\partial V}{\partial x}^T f(x, u, t)\right\}\]对于线性系统：
\[\dot{x} = Ax + Bu\]二次代价函数：
\[J = \frac{1}{2}x(t_f)^T Q_f x(t_f) + \frac{1}{2}\int_{t_0}^{t_f} (x^T Q x + u^T R u) dt\]Riccati方程：
\[-\dot{P} = PA + A^TP + Q - PBR^{-1}B^TP\]最优控制：
\[u^* = -R^{-1}B^T P x = -K x\]对于非线性系统，iLQR通过局部线性化求解：
反向递推：
\(V_x = Q_x + A^T V'_x\) \(V_{xx} = Q_{xx} + A^T V'_{xx} A + V'_x f_{xx}\)
控制增益：
\[K = -(Q_{uu} + B^T V'_{xx} B)^{-1}(Q_u + B^T V'_x)\]考虑一个四旋翼无人机在三维空间中的轨迹优化问题。无人机的状态包括位置$p \in \mathbb{R}^3$和姿态$R \in SO(3)$，构成SE(3)的元素。
动力学模型：
\[\begin{align} \dot{p} &= v \\ \dot{v} &= \frac{1}{m}(R e_3 f - mg e_3) \\ \dot{R} &= R [\omega]_\times \\ J\dot{\omega} &= \tau - \omega \times J\omega \end{align}\]其中$f$是总推力，$\tau$是力矩，$m$是质量，$J$是惯性矩阵。
定义位姿误差：
\(e_p = p - p_d\) \(e_R = \frac{1}{2}\text{vex}(R_d^T R - R^T R_d)\)
其中$\text{vex}$是反对称矩阵到向量的映射。
对数映射提供了另一种误差表示：
\[\xi_e = \log(T_d^{-1} T)\]最小化代价函数：
\[J = \int_0^T \left(||e_p||^2_Q + ||e_R||^2_W + ||u||^2_R\right) dt\]约束条件：
| 力矩限制：$ | \tau | \leq \tau_{max}$ |
| 障碍避免：$ | p - p_{obs} | \geq r_{safe}$ |
四旋翼系统是微分平坦的，平坦输出为$\sigma = [p^T, \psi]^T$，其中$\psi$是偏航角。
从平坦输出可以恢复所有状态和控制：
\[f = m||p^{(2)} + g e_3||\] \[R e_3 = \frac{p^{(2)} + g e_3}{||p^{(2)} + g e_3||}\]这将问题简化为轨迹空间的优化。
将时间离散化为$N$个节点，使用梯形积分：
\[x_{k+1} = x_k + \frac{\Delta t}{2}(f(x_k, u_k) + f(x_{k+1}, u_{k+1}))\]对于SO(3)上的积分，使用指数映射：
\[R_{k+1} = R_k \exp([\Delta t \cdot \omega_k]_\times)\]优化变量：$z = [x_0, u_0, …, x_N, u_N]$
构建NLP问题：
\[\begin{align} \min_z \quad & \sum_{k=0}^{N-1} L(x_k, u_k) \\ \text{s.t.} \quad & c(x_k, u_k, x_{k+1}, u_{k+1}) = 0 \\ & g(x_k, u_k) \leq 0 \end{align}\]李群上的优化：使用局部参数化避免过度参数化：
\[R(\delta) = R_0 \exp([\delta]_\times)\]优化增量$\delta$而非$R$本身。
约束雅可比计算：
\[\frac{\partial}{\partial R} f(R) = \frac{\partial}{\partial \delta} f(R_0 \exp([\delta]_\times))\bigg|_{\delta=0}\]初始猜测生成：使用多项式轨迹或RRT*生成初始轨迹。
典型的SE(3)轨迹优化可以在100-200次迭代内收敛，计算时间约10-50ms（取决于时间节点数）。关键性能指标：
轨迹示意图（ASCII）：
z↑ ╱────╲ 目标
│ ╱ ╲
│ │ ↻ │ （旋转避障）
│ ╲ ⬡ ╱ 障碍物
│ ╲____╱
│ │
└─────┴────→ x
起点
本章建立了机器人控制的数学基础：
核心概念：
关键公式汇总：
错误：直接对旋转矩阵做线性插值
R_interp = (1-t)*R_0 + t*R_1 # 错误！
正确：使用李群上的插值
R_interp = R_0 * exp(t * log(R_0^T * R_1))
陷阱：忽略问题的凸性结构
| 例：$ | Ax | ^2 \leq b^T x$看似非凸，但通过Schur补可以转为SDP |
建议：先尝试凸松弛，再考虑非凸方法
问题：在SO(3)上使用欧拉积分会破坏正交性
R_{k+1} = R_k + dt * R_k * [w]_x # 错误！
解决：使用指数映射保持流形结构
R_{k+1} = R_k * exp([dt * w]_x) # 正确
常见错误：
陷阱：随机初始化非凸问题
问题：硬约束导致无可行解 解决：
瓶颈识别：
题目2.1：证明SO(3)是一个李群 提示：需要证明群的四个性质（封闭性、结合律、单位元、逆元）以及流形的光滑性。
题目2.2：给定$\omega = [1, 0, 0]^T$，计算$\exp([\omega]_\times)$ 提示：使用Rodrigues公式，注意$\theta = ||\omega|| = 1$。
题目2.3：判断以下优化问题是否为凸问题，并说明理由 \(\min_{x,y} \quad x^2 + y^2 \quad \text{s.t.} \quad xy \geq 1\) 提示：检查目标函数和约束集的凸性。
题目2.4：推导简单钟摆的Euler-Lagrange方程 提示：$L = T - V = \frac{1}{2}ml^2\dot{\theta}^2 + mgl\cos\theta$
题目2.5：设计一个QP问题来计算点到凸多面体的最短距离 提示：凸多面体可表示为${x | Ax \leq b}$，使用投影的思想。
题目2.6：证明SE(3)的伴随表示满足$\text{Ad}{T_1T_2} = \text{Ad}{T_1}\text{Ad}_{T_2}$ 提示：使用伴随表示的定义和块矩阵乘法。
题目2.7（开放题）：比较直接配点法和直接射击法在轨迹优化中的优劣 提示：考虑问题规模、稀疏性、初始猜测敏感性等因素。
题目2.8：如何在SE(3)轨迹优化中处理四旋翼的微分平坦性质来减少计算复杂度？ 提示：考虑将优化变量从完整状态空间投影到平坦输出空间。