零碎的知识点（二十一）：序列二次规划（Sequential Quadratic Programming, SQP）

0. 先造一个超简单的一维问题

我们造一个“玩具问题”：

目标：
让x xx尽量靠近 2（也就是最小化( x − 2 ) 2 (x-2)^2(x−2)2）
约束：
x xx不能太大，必须满足一个弯弯的非线性约束
g ( x ) = 1 − x 2 ≥ 0 g(x)=1-x^2\ge0g(x)=1−x2≥0

这个约束g ( x ) ≥ 0 g(x)\ge0g(x)≥0等价于：
1 − x 2 ≥ 0 ⟺ x 2 ≤ 1 ⟺ − 1 ≤ x ≤ 1 1-x^2\ge0\iff x^2\le1\iff -1\le x\le11−x2≥0⟺x2≤1⟺−1≤x≤1

所以原问题其实是：

min ⁡ x F ( x ) = ( x − 2 ) 2 s.t. 1 − x 2 ≥ 0 \begin{aligned} \min_x\quad &F(x)=(x-2)^2\ \text{s.t.}\quad &1-x^2\ge0 \end{aligned}xmin​​F(x)=(x−2)2s.t.​1−x2≥0​

• 没有约束的话，F ( x ) F(x)F(x)的最小值在x = 2 x=2x=2；
• 但x xx必须在区间[ − 1 , 1 ] [-1,1][−1,1]里，所以真正的最优解是x ⋆ = 1 x^\star=1x⋆=1（离 2 最近）。

我们现在要用“SQP 思想”一步一步往这个x ⋆ x^\starx⋆靠近。

1. 第 0 步：选一个初始点（就像选一个“初始机器人姿态”）

假设我们一开始什么都不知道，随便给一个可行点，比如：

x ( 0 ) = 0.5 x^{(0)}=0.5x(0)=0.5

检查一下约束：

g ( x ( 0 ) ) = 1 − ( 0.5 ) 2 = 1 − 0.25 = 0.75 > 0 g(x^{(0)})=1-(0.5)^2=1-0.25=0.75>0g(x(0))=1−(0.5)2=1−0.25=0.75>0

OK，说明x ( 0 ) x^{(0)}x(0)在可行域里面（就像机器人这一帧姿态安全、不穿模）。

2. 第 1 步：在当前点附近做“局部小问题”（SQP 的精髓）

现在来到SQP 的关键思想：

在x ( 0 ) x^{(0)}x(0)附近，

• 把目标函数F ( x ) F(x)F(x)用一个二次函数近似（这里它本来就是二次，所以刚好“近似=原函数”）；
• 把约束g ( x ) ≥ 0 g(x)\ge0g(x)≥0用一条直线近似。

然后解这个“目标二次 + 约束线性”的小问题（QP），作为当前迭代的更新。

2.1 目标函数：二次近似

我们的目标是：

F ( x ) = ( x − 2 ) 2 F(x)=(x-2)^2F(x)=(x−2)2

它本来就是二次函数，所以在x ( 0 ) x^{(0)}x(0)附近做“二次近似”，其实就是它自己——不用改：

F ~ ( 0 ) ( x ) = F ( x ) = ( x − 2 ) 2 \tilde{F}^{(0)}(x)=F(x)=(x-2)^2F~(0)(x)=F(x)=(x−2)2

对应论文原话：
“对匹配目标（1a）在上一次迭代的解附近做二次近似。”
在这个玩具例子里，“二次近似”刚好就是原函数本身。

2.2 约束：线性化（真正的重点！）

约束是：

g ( x ) = 1 − x 2 ≥ 0 g(x)=1-x^2\ge0g(x)=1−x2≥0

这是一条弯弯的抛物线，我们在x ( 0 ) = 0.5 x^{(0)}=0.5x(0)=0.5附近，用它的切线来逼近它：

一维函数的线性化 = 在x ( 0 ) x^{(0)}x(0)点做一阶泰勒展开：

g ( x ) ≈ g ( x ( 0 ) ) + g ′ ( x ( 0 ) ) ( x − x ( 0 ) ) g(x)\approx g(x^{(0)}) + g'(x^{(0)})(x-x^{(0)})g(x)≈g(x(0))+g′(x(0))(x−x(0))

先算这些量：

• g ( x ) = 1 − x 2 g(x)=1-x^2g(x)=1−x2

• 导数g ′ ( x ) = − 2 x g'(x)=-2xg′(x)=−2x

• 在x ( 0 ) = 0.5 x^{(0)}=0.5x(0)=0.5处：

  • g ( x ( 0 ) ) = 1 − ( 0.5 ) 2 = 0.75 g(x^{(0)})=1-(0.5)^2=0.75g(x(0))=1−(0.5)2=0.75
  • g ′ ( x ( 0 ) ) = − 2 ⋅ 0.5 = − 1 g'(x^{(0)})=-2\cdot0.5=-1g′(x(0))=−2⋅0.5=−1

所以线性化得到：

g ~ ( 0 ) ( x ) = g ( x ( 0 ) ) + g ′ ( x ( 0 ) ) ( x − x ( 0 ) ) = 0.75 + ( − 1 ) ( x − 0.5 ) \tilde{g}^{(0)}(x) = g(x^{(0)})+g'(x^{(0)})(x-x^{(0)}) = 0.75 + (-1)(x-0.5)g~​(0)(x)=g(x(0))+g′(x(0))(x−x(0))=0.75+(−1)(x−0.5)

展开一下：

g ~ ( 0 ) ( x ) = 0.75 − x + 0.5 = 1.25 − x \tilde{g}^{(0)}(x) = 0.75 - x + 0.5 = 1.25 - xg~​(0)(x)=0.75−x+0.5=1.25−x

于是原来的非线性约束：

g ( x ) ≥ 0 g(x)\ge0g(x)≥0

在这一轮迭代里，被近似成线性的：

g ~ ( 0 ) ( x ) = 1.25 − x ≥ 0 ⇒ x ≤ 1.25 \tilde{g}^{(0)}(x)=1.25-x\ge0\quad\Rightarrow\quad x\le1.25g~​(0)(x)=1.25−x≥0⇒x≤1.25

这就是论文那句：
“对非穿透约束（1b）进行线性化处理。”
——原本弯的“不能穿透曲线”，在这一轮里变成一条直线约束。

注意：

• 真约束是− 1 ≤ x ≤ 1 -1\le x\le1−1≤x≤1；
• 线性近似约束是x ≤ 1.25 x\le1.25x≤1.25；
• 在x ( 0 ) = 0.5 x^{(0)}=0.5x(0)=0.5附近，这个线性近似是“差不太多”的（至少方向对）。

真正的 SQP 还会加“信任域/步长控制”确保你不要一步走太远，我们待会简单提一下。

3. 第 2 步：解这一轮的“小问题”（一个 QP）

现在，本轮的“局部近似问题”变成：

min ⁡ x F ~ ( 0 ) ( x ) = ( x − 2 ) 2 s.t. g ~ ( 0 ) ( x ) = 1.25 − x ≥ 0 ⇒ x ≤ 1.25 \begin{aligned} \min_x\quad &\tilde{F}^{(0)}(x)=(x-2)^2\ \text{s.t.}\quad &\tilde{g}^{(0)}(x)=1.25-x\ge0\quad\Rightarrow\quad x\le1.25 \end{aligned}xmin​​F~(0)(x)=(x−2)2s.t.​g~​(0)(x)=1.25−x≥0⇒x≤1.25​

这是一个非常简单的二次规划问题：

• 目标( x − 2 ) 2 (x-2)^2(x−2)2是碗形向上的曲线，最小值在x = 2 x=2x=2；
• 但加了约束x ≤ 1.25 x\le1.25x≤1.25，所以“能选的最小点”就是
  x ( 1 ) = 1.25 x^{(1)}=1.25x(1)=1.25

直观：

• 碗的最低点在 2；
• 但你被限制只能站在“2 左边不超过 1.25”的地方；
• 那你能到的最低的点自然是“最靠近 2 的 1.25”。

于是：

x ( 1 ) = 1.25 x^{(1)}=1.25x(1)=1.25

这就是 SQP 第 1 轮迭代算出来的“新姿态”（对应机器人那边的一帧q t ( 1 ) q_t^{(1)}qt(1)​）。

4. 第 3 步：更新 & 进入下一轮迭代

在真正的 SQP 里，这时候还会做两件事：

1. 检查原始约束是不是被破坏太多？
   比如我们看一下真约束g ( x ) = 1 − x 2 ≥ 0 g(x)=1-x^2\ge0g(x)=1−x2≥0在x ( 1 ) = 1.25 x^{(1)}=1.25x(1)=1.25处：

g ( 1.25 ) = 1 − ( 1.25 ) 2 = 1 − 1.5625 = − 0.5625 < 0 g(1.25)=1-(1.25)^2=1-1.5625=-0.5625<0g(1.25)=1−(1.25)2=1−1.5625=−0.5625<0

说明：

• 在线性近似约束里x = 1.25 x=1.25x=1.25还算“合法”；
• 但对原始非线性约束来说，1.25 1.251.25已经跑到可行域外了。

所以真正的 SQP 会用“步长缩放 / 信任域”，不会一下子从 0.5 跳到 1.25，
而是走一小步，比如从 0.5 走到 0.8、0.9 之类的，以保持可行性或至少不离太远。

2. 把x ( 1 ) x^{(1)}x(1)当成下一轮的“当前点”x ( 1 ) x^{(1)}x(1)，再线性化/二次近似一次，重复刚才的步骤。

如果继续迭代几次，你会发现解会慢慢靠近真正的可行最优点x ⋆ = 1 x^\star=1x⋆=1。

你此时需要记住的核心点只有：

• 每一轮：

  • 目标 → 在当前点附近用二次函数近似；
  • 非线性约束 → 在当前点附近用直线近似；

• 得到一个“二次目标 + 线性约束”的小问题 → 很好解；

• 解完 → 更新当前点 → 再近似 → 再解。

这就是“序列二次规划”名字的由来：

不断解一串（二次规划）问题，来逼近原来的非线性规划。