3. 优化控制数学基础

在机器人优化控制与模型预测控制（MPC）中，几乎所有控制器设计最终都会落到一个优化问题上：给定系统模型、约束条件和性能指标，求解一组最优控制输入。理解这些优化问题的数学结构，是判断问题是否可解、解是否唯一、求解是否高效的前提。本章围绕凸集、凸函数、无约束优化、约束优化、拉格朗日乘子法、KKT条件与对偶问题展开，建立从“问题描述”到“最优性判据”的完整链条。

一、凸集与凸函数：优化问题的“好结构”

凸集是指集合中任意两点连线仍完全属于该集合。形式化地，若集合 C 满足对任意 x, y ∈ C 和 θ ∈ [0,1]，都有 θx + (1-θ)y ∈ C，则称 C 为凸集。机器人控制中常见的凸集包括：线性等式约束构成的超平面、线性不等式约束构成的半空间、多面体、椭球、二阶锥等。例如关节速度限制 v_min ≤ v ≤ v_max 构成一个超矩形，是典型凸集。

凸函数则要求函数图像上任意两点连线位于函数图像上方。若 f(θx + (1-θ)y) ≤ θf(x) + (1-θ)f(y)，则 f 为凸函数。凸函数的重要性质是：局部最优即全局最优。对于机器人轨迹优化，若目标函数是二次型（如能量、跟踪误差平方和），约束是线性的，则整个问题为凸二次规划（QP），可以稳定、快速地求得全局最优解。

二、无约束优化：梯度为零与二阶条件

无约束优化问题形式为 min f(x)，其中 x ∈ R^n。若 f 可微，最优解 x* 的必要条件是梯度为零：∇f(x*) = 0。若 f 二阶可微，还需满足海森矩阵半正定：∇²f(x*) ≥ 0。对于凸函数，梯度为零即为充要条件。

在机器人控制中，无约束优化常用于无约束轨迹跟踪、参数辨识和神经网络训练。典型迭代方法包括梯度下降、牛顿法和拟牛顿法。梯度下降更新为 x_{k+1} = x_k - α∇f(x_k)，其中 α 为步长。牛顿法利用二阶信息，更新为 x_{k+1} = x_k - [∇²f(x_k)]^{-1}∇f(x_k)，收敛更快但计算海森矩阵代价高。

# 梯度下降伪代码
x = x0
for k in range(max_iter):
    g = grad_f(x)
    if norm(g) < tol:
        break
    x = x - alpha * g

三、约束优化与拉格朗日乘子法

机器人控制中更常见的是约束优化：min f(x)，满足 g_i(x) ≤ 0 和 h_j(x) = 0。等式约束下，拉格朗日乘子法将约束优化转化为无约束问题。构造拉格朗日函数：

L(x, λ) = f(x) + Σ λ_j h_j(x)

其中 λ_j 为拉格朗日乘子。最优解满足 ∇_x L = 0 且 h_j(x) = 0。从几何上看，最优点的目标函数梯度与约束梯度共线，乘子表示约束对目标函数的“敏感度”。在机器人中，等式约束常出现在闭链机构、接触约束和固定端点轨迹中。

四、KKT条件：约束优化的统一判据

对于同时含等式和不等式约束的问题，KKT条件是最优解的一阶必要条件。设问题为：

min f(x)，s.t. g_i(x) ≤ 0, h_j(x) = 0

KKT条件包括：

• 平稳性：∇f(x*) + Σ μ_i ∇g_i(x*) + Σ λ_j ∇h_j(x*) = 0
• 原始可行性：g_i(x*) ≤ 0, h_j(x*) = 0
• 对偶可行性：μ_i ≥ 0
• 互补松弛：μ_i g_i(x*) = 0

互补松弛条件说明：若某个不等式约束未起作用（g_i(x*) < 0），则对应乘子 μ_i = 0；若乘子大于零，则该约束必须取等号，即处于“激活”状态。在MPC中，关节限位、速度上限、避障距离等约束是否激活，直接决定控制器的行为模式。

五、对偶问题：从另一侧看优化

对偶问题通过拉格朗日函数构造：d(λ, μ) = min_x L(x, λ, μ)，对偶问题为 max_{λ, μ≥0} d(λ, μ)。弱对偶定理指出对偶问题的最优值不超过原始问题最优值；在凸问题且满足Slater条件时，强对偶成立，两者相等。对偶变量具有明确物理意义：在MPC中，对偶变量可解释为约束的“影子价格”，即约束边界放松一个单位时目标函数的改善量。

对偶问题在机器人优化中常用于分布式求解、灵敏度分析和控制器调参。例如，当多个机器人共享约束时，对偶分解可将大问题拆成多个子问题并行求解。

六、案例与场景：MPC中的约束优化

考虑一个移动机器人轨迹跟踪MPC问题：状态为位置和速度，控制为加速度，目标是最小化跟踪误差和控制量，约束包括速度上限、加速度上限和障碍物距离。该问题可写成QP形式：

min  Σ (x_k - x_ref)^T Q (x_k - x_ref) + u_k^T R u_k
s.t. x_{k+1} = A x_k + B u_k
     v_min ≤ v_k ≤ v_max
     a_min ≤ u_k ≤ a_max
     dist(x_k, obstacle) ≥ d_safe

若障碍物约束为线性或可凸化，则整体为凸QP，可用内点法或有效集法求解。若障碍物约束非凸，则问题可能陷入局部最优，需要序列凸化或引入整数变量。实际工程中，常通过保守膨胀障碍物、线性化距离约束等方式保持凸性。

七、常见误区与复盘