局部线性化与 SQP 收敛性

一、非线性 MPC 优化问题

1.1 问题 formulation

非线性 MPC 优化问题：

$$\begin{array}{rl}\underset{U_k}{\min }& J_N(x(k),U_k)=\sum _{i=0}^{N-1}\ell (x_{i|k},u_{i|k})+V(x_{N|k})\\ \text{s.t.}& x_{i+1|k}=f(x_{i|k},u_{i|k}),i=0,\dots ,N-1\\ & x_{0|k}=x(k)\\ & x_{i|k}\in \mathcal{X},i=0,\dots ,N\\ & u_{i|k}\in \mathcal{U},i=0,\dots ,N-1\\ & x_{N|k}\in {\mathcal{X}}_f\end{array}$$

其中：

• $f:{\mathbb{R}}^n\times {\mathbb{R}}^m\to {\mathbb{R}}^n$ 是非线性动态
• $\ell :{\mathbb{R}}^n\times {\mathbb{R}}^m\to \mathbb{R}$ 是阶段代价
• $V:{\mathbb{R}}^n\to \mathbb{R}$ 是终端代价

1.2 优化问题特点

特点	说明
非凸性	非线性动态导致非凸优化
多局部最优	可能存在多个局部极小值
计算复杂	需迭代求解，无解析解
初值敏感	收敛性依赖初始猜测

---

二、序列二次规划 (SQP) 方法

2.1 基本思想

SQP 核心思想：在每一步迭代中，将非线性问题近似为二次规划 (QP) 子问题，通过求解一系列 QP 问题逼近原问题的最优解。

非线性问题 → 线性化/二次近似 → QP 子问题 → 求解 → 更新 → 迭代
                                          ↓
                                    收敛判断

2.2 拉格朗日函数

增广拉格朗日函数：

$$\begin{array}{rl}\mathcal{L}(X,U,\Lambda )=& \sum _{i=0}^{N-1}\ell (x_i,u_i)+V(x_N)\\ & +\sum _{i=0}^{N-1}{\lambda }_{i+1}^{\top }(f(x_i,u_i)-x_{i+1})\\ & +\sum _{i=0}^N{\mu }_i^{\top }g(x_i)+\sum _{i=0}^{N-1}{\nu }_i^{\top }h(u_i)\end{array}$$

其中：

• $\Lambda =\{{\lambda }_1,\dots ,{\lambda }_N\}$ 是动态约束的拉格朗日乘子
• ${\mu }_i$ 是状态约束的乘子
• ${\nu }_i$ 是输入约束的乘子
• $g(x_i)\le 0$ 是状态不等式约束
• $h(u_i)\le 0$ 是输入不等式约束

2.3 KKT 条件

最优性条件 (KKT)：

$${\nabla }_{X,U}\mathcal{L}(X^{\ast },U^{\ast },{\Lambda }^{\ast })=0$$

展开为：

$$\begin{array}{rl}\frac{\partial \mathcal{L}}{\partial x_i}& =\frac{\partial \ell }{\partial x_i}+{\left(\frac{\partial f}{\partial x_i}\right)}^{\top }{\lambda }_{i+1}-{\lambda }_i+{\left(\frac{\partial g}{\partial x_i}\right)}^{\top }{\mu }_i=0\\ \frac{\partial \mathcal{L}}{\partial u_i}& =\frac{\partial \ell }{\partial u_i}+{\left(\frac{\partial f}{\partial u_i}\right)}^{\top }{\lambda }_{i+1}+{\left(\frac{\partial h}{\partial u_i}\right)}^{\top }{\nu }_i=0\end{array}$$

---

三、SQP 算法

3.1 算法框架

算法（序列二次规划）：

输入：初始猜测 (X⁽⁰⁾, U⁽⁰⁾), 容差 ε
输出：最优解 (X*, U*)

1. For k = 0, 1, 2, ...:
   
   a. 线性化动态：
      f(x, u) ≈ f(x⁽ᵏ⁾, u⁽ᵏ⁾) + A⁽ᵏ⁾(x - x⁽ᵏ⁾) + B⁽ᵏ⁾(u - u⁽ᵏ⁾)
      其中 A⁽ᵏ⁾ = ∂f/∂x|(x⁽ᵏ⁾,u⁽ᵏ⁾), B⁽ᵏ⁾ = ∂f/∂u|(x⁽ᵏ⁾,u⁽ᵏ⁾)
   
   b. 二次近似代价：
      J ≈ ½ ΔUᵀ H⁽ᵏ⁾ ΔU + f⁽ᵏ⁾ᵀ ΔU + const
   
   c. 构造 QP 子问题：
      min ½ ΔUᵀ H⁽ᵏ⁾ ΔU + f⁽ᵏ⁾ᵀ ΔU
      s.t. 线性化约束
   
   d. 求解 QP 得 ΔU⁽ᵏ⁾
   
   e. 线搜索/信赖域：
      选择步长 α⁽ᵏ⁾ ∈ (0, 1]
      U⁽ᵏ⁺¹⁾ = U⁽ᵏ⁾ + α⁽ᵏ⁾ ΔU⁽ᵏ⁾
   
   f. 收敛判断：
      If ‖ΔU⁽ᵏ⁾‖ < ε:
          Return (X⁽ᵏ⁾, U⁽ᵏ⁾)

3.2 线性化动态

一阶泰勒展开：

$$f(x,u)\approx f(\bar{x},\bar{u})+A(\bar{x},\bar{u})(x-\bar{x})+B(\bar{x},\bar{u})(u-\bar{u})$$

其中：

$$A(\bar{x},\bar{u})=\frac{\partial f}{\partial x}{|}_{(\bar{x},\bar{u})}\in {\mathbb{R}}^{n\times n}$$ $$B(\bar{x},\bar{u})=\frac{\partial f}{\partial u}{|}_{(\bar{x},\bar{u})}\in {\mathbb{R}}^{n\times m}$$

3.3 二次代价近似

高斯 - 牛顿近似：

$$J(X,U)\approx J(\bar{X},\bar{U})+\nabla J^{\top }\left[\begin{array}{c}\Delta X\\ \Delta U\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}\Delta X\\ \Delta U\end{array}\right]}^{\top }H\left[\begin{array}{c}\Delta X\\ \Delta U\end{array}\right]$$

其中 Hessian 矩阵 $H$ 可近似为：

$$H\approx \nabla J\nabla J^{\top }\text{（高斯 - 牛顿）}$$

或精确计算：

$$H={\nabla }^{2}J$$

---

四、收敛性分析

4.1 局部收敛性

定理（SQP 局部收敛性）：若：

1. 二阶充分条件：Hessian 矩阵正定
2. LICQ：线性无关约束规格成立
3. 初值足够接近：$\Vert U^{(0)}-U^{\ast }\Vert <\delta$

则 SQP 算法局部二次收敛：

$$\Vert U^{(k+1)}-U^{\ast }\Vert \le C\Vert U^{(k)}-U^{\ast }{\Vert }^2$$

其中 $C>0$ 是常数。

4.2 收敛速率

条件	收敛速率	说明
初值接近最优	二次收敛	$\Vert e_{k+1}\Vert \le C\Vert e_k{\Vert }^2$
初值较远	线性收敛	$\Vert e_{k+1}\Vert \le \rho \Vert e_k\Vert$
初值很远	可能发散	需全局化策略

4.3 全局化策略

方法 1：线搜索

选择步长 ${\alpha }^{(k)}$ 使得 merit function 下降：

$$\varphi (x,u)=J(x,u)+\rho \sum _i\max (0,g_i(x))$$

方法 2：信赖域

限制步长大小：

$$\Vert \Delta U\Vert \le {\Delta }_{\max }$$

方法 3：过滤方法 (Filter)

同时考虑目标函数和约束违反度，避免惩罚参数选择。

---

五、实时迭代 (RTI) 方案

5.1 基本思想

实时迭代：在每个采样时刻只执行一次 SQP 迭代，而非等待完全收敛。

传统 SQP: [优化 → 收敛 → 实施] (耗时较长)
RTI:      [优化一步 → 实施 → 优化一步 → 实施] (持续进行)

5.2 算法流程

实时迭代 SQP：

For each sampling time k:
    1. 测量状态 x(k)
    2. 使用上一时刻解作为初值：U⁽⁰⁾ = U*(k-1) (shifted)
    3. 执行一次 SQP 迭代得 U⁽¹⁾
    4. 实施控制 u(k) = U⁽¹⁾[0]
    5. 准备下一时刻初值

5.3 稳定性保证

命题：若初值足够接近最优解且采样时间 $T_s$ 足够短，则 RTI 方案产生的”次优”控制律仍能保证闭环稳定性。

---

六、数值示例

6.1 非线性系统

倒立摆：

$$\begin{array}{rl}\dot{\theta }& =\omega \\ \dot{\omega }& =\frac{g}{l}\sin (\theta )-\frac{b}{m{l}^2}\omega +\frac{1}{m{l}^2}u\end{array}$$

6.2 离散化

欧拉离散化（$T_s=0.05s$）：

$$\begin{array}{rl}\theta (k+1)& =\theta (k)+T_s\omega (k)\\ \omega (k+1)& =\omega (k)+T_s\left(\frac{g}{l}\sin (\theta (k))-\frac{b}{m{l}^2}\omega (k)+\frac{1}{m{l}^2}u(k)\right)\end{array}$$

6.3 SQP 求解结果

迭代次数	代价 $J$	$\Vert \Delta U\Vert$
0	2.50	-
1	1.85	0.42
2	1.62	0.15
3	1.58	0.03
4	1.57	0.005
5	1.57	0.0008

观察：

• 二次收敛：$\Vert \Delta U\Vert$ 快速减小
• 4-5 次迭代后收敛

---

七、总结

概念	公式/结论
SQP	序列二次规划，迭代求解 QP
线性化	$f(x,u)\approx f+A\Delta x+B\Delta u$
KKT 条件	${\nabla }_{X,U}\mathcal{L}=0$
收敛速率	局部二次收敛 $\Vert e_{k+1}\Vert \le C\Vert e_k{\Vert }^2$
RTI	每时刻一次迭代，实时应用

核心结论

SQP 方法求解 NMPC：

1. 线性化动态 + 二次近似代价 → QP 子问题
2. 局部二次收敛：初值接近最优时收敛快
3. 全局化策略：线搜索/信赖域保证全局收敛
4. 实时迭代：每时刻一次迭代，适合高速应用

相关内容

离散化误差见 01-连续系统离散化误差分析 稳定性分析见 03-基于收缩映射的局部稳定性条件

---

更新记录

日期	内容
2026-04-10	初始版本