非线性规划中的可行序列凸规划（Feasible Sequential Convex Programming, FSCP）进阶题：处理非凸可行域与路径约束的轨迹优化问题

题目描述

考虑一个在机器人轨迹优化中常见的、具有非凸可行域与路径约束的非线性规划问题：

minimize    f(x) = ∑_{k=1}^{N} ( || p_k - p_{k-1} ||^2 + α || v_k ||^2 )
subject to  g_j(p_k) ≤ 0,   j = 1,...,m,  k = 1,...,N   (路径约束)
            h_i(p_k) = 0,    i = 1,...,p,  k = 1,...,N   (等式约束)
            p_min ≤ p_k ≤ p_max,                         (边界约束)
            v_k = (p_k - p_{k-1}) / Δt,                   (速度定义)
            || v_k || ≤ v_max.                            (速度约束)

其中：

• 决策变量 x = [p_1, p_2, ..., p_N]，p_k ∈ ℝ³ 表示机器人在第 k 个时间步的位置。
• 目标函数 f(x) 追求轨迹平滑（相邻位置差最小化）与能量效率（速度平方和最小化），α 为权重系数。
• 路径约束 g_j(p_k) ≤ 0 描述了工作空间中的障碍物区域（通常是非凸的，例如多个圆或多边形的补集）。
• 等式约束 h_i(p_k) = 0 可能表示运动学闭合条件或特定路径点。
• 边界约束与速度约束是凸的。
  要求：设计一个可行序列凸规划（FSCP） 算法，在每次迭代中生成严格可行（满足所有约束）的迭代点，并逐步逼近原问题的最优解。

解题过程循序渐进讲解

第一步：理解FSCP的核心思想与挑战

可行序列凸规划（FSCP）是序列凸规划（SCP）的一种变体，其核心要求是：每一步迭代点都必须严格满足原始问题的所有约束。这特别适用于安全关键应用（如机器人、航空航天），因为中间迭代轨迹必须是物理可执行的。

与标准SCP（可能允许暂时违反约束，靠罚函数或过滤器处理）相比，FSCP的主要挑战在于：

1. 初始可行点获取：需要一个初始严格可行点，这本身可能是一个难题。
2. 凸近似的同时保持可行性：在非凸约束 g_j(p) ≤ 0 附近进行凸化（如线性化）时，线性近似可能会割去部分可行域，导致新子问题无解，或者其解可能违反原始的非凸约束。
3. 收敛性保证：需要在保持严格可行的同时，确保目标函数下降和收敛到局部最优。

第二步：构造凸近似并保证迭代可行性

对于非凸约束 g_j(p) ≤ 0，我们采用保守的凸近似。常见方法是在当前迭代点 p^l（上标 l 表示第 l 次迭代）进行一阶泰勒展开，并添加一个信任域（trust region） 或移动限界（move limit） 约束，以确保近似在局部足够准确，且新点仍满足原始约束。

具体地，对于每个路径约束 g_j(p)（假设连续可微）：

1. 线性化：g̃_j(p; p^l) = g_j(p^l) + ∇g_j(p^l)^T (p - p^l)。
2. 保守化处理：由于 g_j 非凸，线性化可能高估可行域（即 g̃_j ≤ 0 的区域可能包含一些实际上 g_j > 0 的点）。为确保新点 p^{l+1} 满足原始约束 g_j(p^{l+1}) ≤ 0，我们通常采取以下两种策略之一：
   • 策略A：添加安全裕度（Safety Margin）：将线性化约束收紧为 g̃_j(p; p^l) + β ≤ 0，其中 β > 0 是一个小正数，作为安全裕度。
   • 策略B：结合信任域：要求新点 p^{l+1} 位于一个以 p^l 为心、半径为 Δ^l 的球内：|| p - p^l || ≤ Δ^l，并适当控制 Δ^l 以保证在信任域内线性模型是原始约束的保守近似。这通常需要利用 g_j 的 Lipschitz 常数估计。

在本问题中，由于障碍物约束通常几何直观，我们采用策略B：信任域法，因为它能自然限制移动步长，避免“越界”。

第三步：构建第 l 次迭代的凸子问题

在第 l 次迭代，给定当前严格可行点 x^l = [p_1^l, ..., p_N^l] 和信任域半径 Δ^l，我们构造如下凸优化子问题（通常是一个二次规划QP或二阶锥规划SOCP）：

minimize    f̃(x; x^l) = ∑_{k=1}^{N} ( || p_k - p_{k-1} ||^2 + α || v_k ||^2 )   （注意：目标函数本身是凸的，无需近似）
subject to  g̃_j(p_k; p_k^l) ≤ 0,   ∀j,k          （线性化路径约束）
            h_i(p_k^l) + ∇h_i(p_k^l)^T (p_k - p_k^l) = 0,   ∀i,k   （线性化等式约束）
            p_min ≤ p_k ≤ p_max,                 ∀k
            || v_k || ≤ v_max,                    ∀k
            || p_k - p_k^l || ≤ Δ^l,              ∀k   （信任域约束）

其中 v_k = (p_k - p_{k-1}) / Δt 是线性的，故速度约束 || v_k || ≤ v_max 是二阶锥约束（凸）。

注意：

• 等式约束 h_i 被线性化，由于是等式，线性化可能导致子问题不可行。若发生，可采用松弛变量或仅线性化到一阶，并在后续迭代中调整。
• 信任域约束 || p_k - p_k^l || ≤ Δ^l 是球约束，也可用盒约束（每个分量变化限幅）简化，但球约束在几何上更自然。

第四步：求解凸子问题并保证新迭代点严格可行

求解上述凸子问题（可用内点法、积极集QP求解器），得到候选点 x*。
关键检查：x* 是否满足原始问题的所有约束（即非凸的 g_j(p_k*) ≤ 0 和 h_i(p_k*) = 0）？

• 如果 x* 严格可行，则接受它为下一步迭代点：x^{l+1} = x*。
• 如果 x* 违反某些原始约束（由于线性化误差），则我们需要缩小信任域半径 Δ^l（例如乘以因子 0.5），重新求解子问题，直到找到一个严格可行的 x*。

这个过程保证了每次迭代点 x^{l+1} 都是原始问题的可行点，符合FSCP的要求。

第五步：更新信任域半径与收敛判断

在接受新点 x^{l+1} 后，我们需要更新信任域半径 Δ^{l+1} 以供下一次迭代使用。常用更新规则基于实际目标下降与预测下降之比（类似信赖域方法）：
令 ρ = (实际下降) / (预测下降) = [f(x^l) - f(x^{l+1})] / [f̃(x^l) - f̃(x*)]。

• 如果 ρ 接近1（模型预测准确），则可适当增大 Δ^{l+1}（例如 Δ^{l+1} = 1.5 Δ^l）。
• 如果 ρ 很小或为负（模型预测差），则减小 Δ^{l+1}（例如 Δ^{l+1} = 0.5 Δ^l）。
• 如果 ρ 处于中间范围，则保持 Δ^{l+1} = Δ^l。

收敛判断：当以下条件之一满足时停止：

1. 迭代点变化很小：|| x^{l+1} - x^l || < ε_x。
2. 目标函数下降很小：| f(x^{l+1}) - f(x^l) | < ε_f。
3. 信任域半径变得非常小：Δ^{l+1} < ε_Δ（表示可能已达到当前精度下的局部最优）。
4. 达到最大迭代次数。

第六步：算法完整步骤总结

1. 初始化：找到一个初始严格可行点 x^0（可通过解一个可行性问题，或从简单轨迹如直线插值开始，并用投影或修复方法使其满足约束）。初始化信任域半径 Δ^0（例如基于问题尺度）。
2. 循环迭代（对于 l = 0, 1, 2, ...）：
   a. 构建凸子问题：在当前点 x^l 线性化非凸约束，添加信任域约束。
   b. 求解子问题：得到候选点 x*。
   c. 可行性验证：检查 x* 是否满足原始问题所有约束。若不满足，缩小 Δ^l，返回步骤 b。
   d. 接受迭代点：设 x^{l+1} = x*。
   e. 更新信任域半径：基于下降比 ρ 更新 Δ^{l+1}。
   f. 收敛判断：若满足收敛条件，则停止并输出 x^{l+1} 作为近似最优解。
3. 输出：返回最终迭代点作为局部最优轨迹。

第七步：算法特点与应用注意

• 优势：FSCP生成的中间轨迹始终可行，适合在线或安全关键优化。
• 局限性：
  • 需要初始可行点，这有时难以获得。
  • 对于高度非凸的可行域，信任域可能需要很小，导致收敛慢。
  • 线性化可能导致子问题可行域为空，此时需要更复杂的处理（如约束松弛或可行性恢复阶段）。
• 改进方向：可结合逐步凸近似（SCA） 更精确地凸化非凸约束（如用凸上界替代线性化），或采用自适应裕度调整策略，平衡保守性与进度。

通过以上步骤，FSCP能在保持严格可行的前提下，逐步优化轨迹，适用于机器人避障、无人机路径规划等实际问题。