第2章：数学基础

章节概要

本章介绍机器人控制中的核心数学工具。我们将深入探讨李群与李代数如何优雅地描述刚体运动，理解凸优化与非凸优化在轨迹规划中的应用，以及变分原理如何为最优控制提供理论基础。通过SE(3)上的轨迹优化案例，您将看到这些数学工具如何在实际问题中发挥作用。

学习目标：

• 掌握李群SE(3)、SO(3)及其李代数在机器人学中的应用
• 理解凸优化与非凸优化的本质区别及求解策略
• 掌握变分原理与庞特里亚金极大值原理
• 能够构建并求解SE(3)上的轨迹优化问题

2.1 李群与李代数在机器人学中的应用

2.1.1 为什么需要李群

在机器人学中，刚体的位姿（位置和姿态）是最基本的状态描述。传统的欧拉角表示存在奇异性问题（万向节锁），而四元数虽然避免了奇异性，但需要额外的归一化约束。李群理论提供了一个统一、优雅的框架来处理刚体运动，其优势包括：

1. 无奇异性：李群表示在整个状态空间内都是良定的，不存在欧拉角的万向节锁问题
2. 紧凑性：旋转矩阵自然满足正交性约束，无需额外的归一化步骤
3. 几何直观：李群保持了运动的几何结构，物理意义明确
4. 计算高效：指数映射和对数映射提供了高效的插值和优化工具
5. 理论完备：李群的微分结构（李代数）为速度、加速度和控制提供了自然框架

历史背景：李群理论由挪威数学家Sophus Lie在19世纪末创立，最初用于研究微分方程的对称性。20世纪中叶，随着机器人学的发展，研究者发现李群是描述刚体运动的完美工具。Murray、Li和Sastry的专著《A Mathematical Introduction to Robotic Manipulation》(1994)系统地将李群理论引入机器人学，成为该领域的经典。

2.1.2 特殊正交群 SO(3)

特殊正交群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}$$ 关键性质：

• 叉积表示：$[\omega]_\times v = \omega \times v$，对任意$v \in \mathbb{R}^3$
• 李括号：$[[\omega_1]_\times, [\omega_2]_\times] = [[\omega_1]_\times, [\omega_2]_\times]_\times = [\omega_1 \times \omega_2]_\times$
• 内积保持：$\langle[\omega]_\times v, w\rangle = \langle v, [\omega]_\times^T w\rangle = -\langle v, [\omega]_\times w\rangle$

指数映射将李代数元素映射到李群（将角速度映射为旋转）： $$\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$$ 特殊情况：

• 当$\theta \to 0$时：$\exp([\omega]_\times) \approx I + [\omega]_\times$（小角度近似）
• 当$\theta = \pi$时：需要特殊处理以避免数值不稳定

2.1.3 特殊欧几里得群 SE(3)

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_1 T_2 = \begin{bmatrix} R_1R_2 & R_1p_2 + p_1 \\ 0 & 1 \end{bmatrix}$
• 逆元（逆变换）：$T^{-1} = \begin{bmatrix} R^T & -R^Tp \\ 0 & 1 \end{bmatrix}$
• 单位元：$I_4 = \begin{bmatrix} I_3 & 0 \\ 0 & 1 \end{bmatrix}$

物理意义：变换矩阵$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)，表示刚体的瞬时运动：

• $v \in \mathbb{R}^3$：线速度分量
• $\omega \in \mathbb{R}^3$：角速度分量

指数映射（从速度到位移）： $$\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$$ 特殊情况：

• 纯平移（$\omega = 0$）：$\exp(\xi^\wedge) = \begin{bmatrix} I & v \\ 0 & 1 \end{bmatrix}$
• 纯旋转（$v = 0$）：退化为SO(3)的指数映射
• 螺旋运动：一般情况，绕螺旋轴的组合运动

2.1.4 伴随表示与雅可比

伴随表示(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$$ 其中：

• $\xi^s$：空间速度（相对于固定的空间坐标系）
• $\xi^b$：体速度（相对于运动的物体坐标系）

伴随表示的性质：

1. 群同态：$\text{Ad}_{T_1 T_2} = \text{Ad}_{T_1} \text{Ad}_{T_2}$
2. 逆映射：$\text{Ad}_{T^{-1}} = \text{Ad}_T^{-1}$
3. 单位元：$\text{Ad}_I = I_6$
4. 行列式：$\det(\text{Ad}_T) = 1$（保体积变换）

小伴随表示(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$$

2.1.5 应用：机械臂运动学

对于n自由度串联机械臂，末端执行器的位姿可以用指数积(Product of Exponentials, PoE)公式表示： $$T_{0n}(\theta) = \prod_{i=1}^n \exp(\xi_i^\wedge \theta_i) T_{0n}(0)$$ 其中：

• $\xi_i \in \mathbb{R}^6$：第i个关节在零位形时的扭转坐标
• $\theta_i$：第i个关节角度
• $T_{0n}(0)$：零位形时的末端位姿

关节扭转的计算：

• 旋转关节：$\xi_i = \begin{bmatrix} -\omega_i \times q_i \\ \omega_i \end{bmatrix}$，其中$\omega_i$是旋转轴，$q_i$是轴上任一点
• 移动关节：$\xi_i = \begin{bmatrix} v_i \\ 0 \end{bmatrix}$，其中$v_i$是移动方向

空间雅可比矩阵： 速度运动学关系$\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$$ 雅可比的应用：

1. 速度映射：$V_{ee} = J(\theta)\dot{\theta}$
2. 力/力矩传递：$\tau = J^T(\theta)F_{ee}$（虚功原理）
3. 奇异性分析：$\det(J^TJ) = 0$处为奇异位形
4. 可操作性椭球：$J(\theta)J^T(\theta)$的特征值描述末端执行器的运动能力

计算优化技巧：

• 利用递推关系避免重复计算：$T_{0(i+1)} = T_{0i} \exp(\xi_{i+1}^\wedge \theta_{i+1})$
• 使用空间向量代数减少矩阵运算
• 缓存常用的变换矩阵和雅可比列

2.2 凸优化与非凸优化

2.2.1 凸优化基础

凸优化是现代机器人控制的基石，其核心优势在于任何局部最优解都是全局最优解。这一性质保证了算法的收敛性和解的唯一性（对于严格凸问题）。

标准形式： $$\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\}$

保凸运算：

1. 交集：凸集的交集仍是凸集
2. 仿射变换：$f(\mathcal{C}) = \{Ax + b | x \in \mathcal{C}\}$是凸集
3. 透视变换：$P(\mathcal{C}) = \{x/t | (x,t) \in \mathcal{C}, t > 0\}$
4. 线性分式变换：结合仿射和透视

凸函数的判定：

1. 一阶条件（充要）：$f$可微时，$f(y) \geq f(x) + \nabla f(x)^T(y-x)$
2. 二阶条件（充要）：$f$二次可微时，$\nabla^2 f(x) \succeq 0$（Hessian半正定）
3. 限制条件：$f$凸当且仅当对任意直线，其限制是凸的

常见凸函数：

• 仿射：$a^Tx + b$
• 二次型：$\frac{1}{2}x^TQx + c^Tx$，$Q \succeq 0$
• 范数：$||x||_p$，$p \geq 1$
• 负熵：$x\log x$
• 对数和指数：$e^{ax}$，$-\log x$

保凸函数运算：

• 非负加权和：$\sum_i \alpha_i f_i$，$\alpha_i \geq 0$
• 复合：$f(Ax + b)$当$f$凸
• 逐点最大：$\max_i f_i(x)$
• 部分最小化：$g(x) = \inf_y f(x,y)$

2.2.2 二次规划(QP)

二次规划是机器人控制中最常见的凸优化问题，广泛应用于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^*$使得：

1. 原始可行性： - $Ax^* \leq b$ - $A_{eq}x^* = b_{eq}$ - $l \leq x^* \leq u$

2. 对偶可行性：$\lambda^* \geq 0$

3. 互补松弛性： - $\lambda_i^*(a_i^Tx^* - b_i) = 0$，$\forall i$ - 活跃约束：$\lambda_i^* > 0 \Rightarrow a_i^Tx^* = b_i$ - 非活跃约束：$a_i^Tx^* < b_i \Rightarrow \lambda_i^* = 0$

4. 稳定性（梯度为零）： $$Qx^* + c + A^T\lambda^* + A_{eq}^T\nu^* = 0$$ 求解算法：

5. 活跃集方法(Active Set)： - 适合小规模问题（n < 1000） - 热启动效率高 - 精确识别活跃约束 - 复杂度：最坏$O(2^m)$次迭代

6. 内点法(Interior Point)： - 适合大规模问题 - 多项式时间复杂度 - 不精确识别活跃约束 - 典型迭代次数：10-50

7. 梯度投影法： - 适合简单约束（如边界约束） - 易于并行化 - 收敛速度依赖于条件数

8. ADMM(交替方向乘子法)： - 适合分布式计算 - 处理大规模稀疏问题 - 收敛速度中等

在机器人控制中的应用：

1. 逆运动学： $$\min_{\dot{q}} ||J\dot{q} - v_{des}||^2 + \alpha||\dot{q}||^2$$

2. 力分配： $$\min_{f} ||f||^2 \quad \text{s.t.} \quad Gf = \tau_{des}, \quad f \in \mathcal{F}$$

3. 轨迹跟踪： $$\min_{u} \sum_{k} ||x_k - x_{ref}||_Q^2 + ||u_k||_R^2$$

2.2.3 半定规划(SDP)

半定规划扩展了线性规划到矩阵变量： $$\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在接触力优化和稳定性分析中有重要应用。

2.2.4 非凸优化与局部方法

非凸优化问题普遍存在于机器人学中，如逆运动学、碰撞避免等。

序列二次规划(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$，解收敛到原问题的解。

2.2.5 凸松弛技术

对于非凸问题，凸松弛提供了可行解的下界：

凸包络：函数$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问题求解。

2.2.6 在机器人学中的应用

轨迹优化中的障碍避免： 非凸约束$||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$是足够大的常数。

2.3 变分原理与最优控制

2.3.1 变分法基础

变分法研究泛函的极值问题。对于泛函： $$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$。

2.3.2 Hamilton原理

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$$

2.3.3 Pontryagin极大值原理

对于控制系统： $$\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^*$满足：

1. 状态方程：$\dot{x}^* = \frac{\partial H}{\partial \lambda}$
2. 协态方程：$\dot{\lambda}^* = -\frac{\partial H}{\partial x}$
3. 极值条件：$H(x^*, u^*, \lambda^*, t) = \max_u H(x^*, u, \lambda^*, t)$
4. 横截条件：$\lambda(t_f) = \frac{\partial \phi}{\partial x}(x(t_f))$

2.3.4 动态规划与HJB方程

定义值函数： $$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\}$$

2.3.5 线性二次调节器(LQR)

对于线性系统： $$\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$$

2.3.6 迭代LQR(iLQR)

对于非线性系统，iLQR通过局部线性化求解：

1. 前向模拟：给定控制序列$u_k$，得到轨迹$x_k$
2. 反向传播：计算局部LQR增益
3. 前向更新：$u_{k+1} = u_k + \alpha \delta u$

反向递推： $$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)$$

2.4 案例研究：SE(3)上的轨迹优化

2.4.1 问题描述

考虑一个四旋翼无人机在三维空间中的轨迹优化问题。无人机的状态包括位置$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$是惯性矩阵。

2.4.2 SE(3)上的误差度量

定义位姿误差： $$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)$$

2.4.3 优化问题表述

最小化代价函数： $$J = \int_0^T \left(||e_p||^2_Q + ||e_R||^2_W + ||u||^2_R\right) dt$$ 约束条件：

• 动力学约束（上述方程）
• 推力限制：$0 \leq f \leq f_{max}$
• 力矩限制：$||\tau|| \leq \tau_{max}$
• 障碍避免：$||p - p_{obs}|| \geq r_{safe}$

2.4.4 基于微分平坦的方法

四旋翼系统是微分平坦的，平坦输出为$\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||}$$ 这将问题简化为轨迹空间的优化。

2.4.5 直接配点法求解

将时间离散化为$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}$$

2.4.6 数值实现要点

李群上的优化：使用局部参数化避免过度参数化： $$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*生成初始轨迹。

2.4.7 结果分析

典型的SE(3)轨迹优化可以在100-200次迭代内收敛，计算时间约10-50ms（取决于时间节点数）。关键性能指标：

• 位置精度：厘米级
• 姿态精度：1-2度
• 实时性：10-20Hz重规划频率

轨迹示意图（ASCII）：

    z↑    ╱────╲ 目标
     │   ╱      ╲
     │  │   ↻    │ （旋转避障）
     │  ╲   ⬡   ╱ 障碍物
     │   ╲____╱
     │     │
     └─────┴────→ x
         起点

本章小结

本章建立了机器人控制的数学基础：

核心概念：

1. 李群与李代数：提供了描述刚体运动的统一框架，避免了欧拉角的奇异性和四元数的过参数化问题
2. 凸优化：保证全局最优性，是实时控制的基础工具
3. 非凸优化：处理实际约束，需要良好的初始化和局部搜索策略
4. 变分原理：连接了物理直觉和数学优化，为轨迹优化提供理论基础
5. 最优控制：Pontryagin原理和动态规划提供了两种互补的视角

关键公式汇总：

• Rodrigues公式：$\exp([\omega]_\times) = I + \frac{\sin\theta}{\theta}[\omega]_\times + \frac{1-\cos\theta}{\theta^2}[\omega]_\times^2$
• Euler-Lagrange方程：$\frac{d}{dt}(\frac{\partial L}{\partial \dot{x}}) - \frac{\partial L}{\partial x} = 0$
• HJB方程：$-\frac{\partial V}{\partial t} = \min_u \{L + \frac{\partial V}{\partial x}^T f\}$
• Riccati方程：$-\dot{P} = PA + A^TP + Q - PBR^{-1}B^TP$

常见陷阱与错误

1. 李群运算陷阱

错误：直接对旋转矩阵做线性插值

R_interp = (1-t)*R_0 + t*R_1  # 错误！

正确：使用李群上的插值

R_interp = R_0 * exp(t * log(R_0^T * R_1))

2. 优化问题建模

陷阱：忽略问题的凸性结构

• 许多看似非凸的问题可以通过变量替换转为凸问题
• 例：$||Ax||^2 \leq b^T x$看似非凸，但通过Schur补可以转为SDP

建议：先尝试凸松弛，再考虑非凸方法

3. 数值积分误差

问题：在SO(3)上使用欧拉积分会破坏正交性

R_{k+1} = R_k + dt * R_k * [w]_x  # 错误！

解决：使用指数映射保持流形结构

R_{k+1} = R_k * exp([dt * w]_x)  # 正确

4. 优化求解器选择

常见错误：

• 对小规模QP使用内点法（应该用active-set方法）
• 对大规模稀疏问题使用密集求解器
• 忽略问题的结构（如块对角、带状等）

5. 初始化策略

陷阱：随机初始化非凸问题

• 轨迹优化需要动力学可行的初始猜测
• 使用简单控制器（如PD）生成初始轨迹

6. 约束违反处理

问题：硬约束导致无可行解 解决：

• 使用软约束with惩罚项
• 逐步收紧约束（homotopy方法）
• 检查约束的相容性

7. 计算效率

瓶颈识别：

• 雅可比/Hessian计算占主要时间→使用自动微分
• 矩阵运算密集→利用稀疏性和结构
• 迭代次数过多→改进初始化或使用warm-start

练习题

基础题

题目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)轨迹优化中处理四旋翼的微分平坦性质来减少计算复杂度？ 提示：考虑将优化变量从完整状态空间投影到平坦输出空间。