9. 非线性误差模型如何线性化成 $\dot{x}=Ax+Bu+w$

---

9.1 定义状态和输入

设：

$$x=\left[\begin{array}{c}e\\ {\theta }_e\\ \delta \end{array}\right],u=\left[\begin{array}{c}u\end{array}\right]$$

其中：

• $e$：横向误差
• ${\theta }_e$：航向误差
• $\delta$：实际转角
• $u$：转向命令

那么根据之前的推导，系统为：

$$\dot{e}=v{\theta }_e$$

$${\dot{\theta }}_e=\frac{v}{L}\tan \delta -v{\kappa }_r$$

$$\dot{\delta }=-\frac{1}{\tau }\delta +\frac{1}{\tau }u$$

这里唯一非线性项是：

$$\tan \delta$$

---

9.2 线性化点怎么选

对参考曲率 ${\kappa }_r$，理想前轮角是：

$${\delta }_r=\arctan (L{\kappa }_r)$$

所以在这个工作点附近线性化最自然。

---

9.3 泰勒展开

设：

$$f(\delta )=\tan \delta$$

在 ${\delta }_r$ 附近一阶展开：

$$\tan \delta \approx \tan {\delta }_r+\frac{d}{d\delta }\tan \delta {|}_{{\delta }_r}(\delta -{\delta }_r)$$

而：

$$\frac{d}{d\delta }\tan \delta ={\sec }^2\delta =\frac{1}{{\cos }^2\delta }$$

所以：

$$\tan \delta \approx \tan {\delta }_r+{\sec }^2{\delta }_r(\delta -{\delta }_r)$$

展开：

$$\tan \delta \approx \tan {\delta }_r-{\delta }_r{\sec }^2{\delta }_r+{\sec }^2{\delta }_r\delta$$

---

9.4 代回 ${\dot{\theta }}_e$

原式：

$${\dot{\theta }}_e=\frac{v}{L}\tan \delta -v{\kappa }_r$$

代入线性化：

$${\dot{\theta }}_e=\frac{v}{L}{\sec }^2{\delta }_r,\delta +\left[-v{\kappa }_r+\frac{v}{L}\left(\tan {\delta }_r-{\delta }_r{\sec }^2{\delta }_r\right)\right]$$

---

9.5 得到线性仿射系统

于是：

$$\dot{x}=Ax+Bu+w$$

其中：

$$A=\left[\begin{array}{ccc}0& v& 0\\ 0& 0& \frac{v}{L}{\sec }^2{\delta }_r\\ 0& 0& -\frac{1}{\tau }\end{array}\right]$$

$$B=\left[\begin{array}{c}0\\ 0\\ \frac{1}{\tau }\end{array}\right]$$

$$w=\left[\begin{array}{c}0\\ -v{\kappa }_r+\frac{v}{L}\left(\tan {\delta }_r-{\delta }_r{\sec }^2{\delta }_r\right)\\ 0\end{array}\right]$$

这里的 $w$ 是线性化偏置项，用来表达常数项，不是噪声。

---

10. 连续时间模型如何离散化成 $x_{k+1}=A_d{x}_k+B_d{u}_k+W_d$

---

10.1 连续系统

从：

$$\dot{x}=Ax+Bu+w$$

出发。

---

10.2 为什么要离散化

MPC 是数字控制器，只在离散时刻工作：

$$t_k=kdt$$

所以需要把连续系统变成一步一步更新的离散系统。

---

10.3 最简单欧拉法（先作为参考）

用：

$$\dot{x}(t_k)\approx \frac{x_{k+1}-x_k}{dt}$$

可得：

$$x_{k+1}\approx (I+dtA)x_k+dtB{u}_k+dtw$$

但工程里常用更稳定的 Tustin / 双线性离散化。

---

10.4 Tustin 推导

从积分形式：

$$x_{k+1}-x_k={\int }_{t_k}^{t_{k+1}}(Ax+Bu+w),dt$$

用梯形法近似：

$$\int f(t)dt\approx \frac{dt}{2}(f_k+f_{k+1})$$

于是：

$$x_{k+1}-x_k\approx \frac{dt}{2}[(A{x}_k+B{u}_k+w)+(A{x}_{k+1}+B{u}_k+w)]$$

整理：

$$\left(I-\frac{dt}{2}A\right)x_{k+1}=\left(I+\frac{dt}{2}A\right)x_k+dtB{u}_k+dtw$$

左乘逆矩阵：

$$x_{k+1}={\left(I-\frac{dt}{2}A\right)}^{-1}\left(I+\frac{dt}{2}A\right)x_k+{\left(I-\frac{dt}{2}A\right)}^{-1}Bdt{u}_k+{\left(I-\frac{dt}{2}A\right)}^{-1}wdt$$

定义：

$$A_d={\left(I-\frac{dt}{2}A\right)}^{-1}\left(I+\frac{dt}{2}A\right)$$

$$B_d={\left(I-\frac{dt}{2}A\right)}^{-1}Bdt$$

$$W_d={\left(I-\frac{dt}{2}A\right)}^{-1}wdt$$

所以：

$$x_{k+1}=A_d{x}_k+B_d{u}_k+W_d$$

---

11. 单步模型为什么还不够：MPC 为什么必须做未来时域展开

单步模型只告诉你：

如果现在给一个 $u_k$，下一步状态会怎样。

但 MPC 要做的是：

同时优化未来 $N$ 步的输入，让未来整个误差轨迹最好。

所以必须引入：

$$U_{ex}=\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]$$

并预测：

$$X_{ex}=\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_N\end{array}\right]$$

---

12. 从单步离散模型到批量预测方程

---

12.1 单步模型

对每一步：

$$x_{k+1}=A_k{x}_k+B_k{u}_k+W_k$$

注意这里是时变系统，所以每一步矩阵可能不同。

---

12.2 展开前几步

第一步：

$$x_1=A_0{x}_0+B_0{u}_0+W_0$$

第二步：

$$x_2=A_1{x}_1+B_1{u}_1+W_1$$

代入 $x_1$：

$$x_2=A_1{A}_0{x}_0+A_1{B}_0{u}_0+B_1{u}_1+A_1{W}_0+W_1$$

第三步：

$$x_3=A_2{x}_2+B_2{u}_2+W_2$$

代入 $x_2$：

$$x_3=A_2{A}_1{A}_0{x}_0+A_2{A}_1{B}_0{u}_0+A_2{B}_1{u}_1+B_2{u}_2+A_2{A}_1{W}_0+A_2{W}_1+W_2$$

---

12.3 堆叠形式

把所有状态堆起来：

$$X_{ex}=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ ⋮\\ x_N\end{array}\right]$$

则可以写成：

$$X_{ex}=A_{ex}{x}_0+B_{ex}{U}_{ex}+W_{ex}$$

---

12.4 $A_{ex}$

$$A_{ex}=\left[\begin{array}{c}{A}_0\\ A_1{A}_0\\ A_2{A}_1{A}_0\\ ⋮\end{array}\right]$$

---

12.5 $B_{ex}$

$$B_{ex}=\left[\begin{array}{cccc}{B}_0& 0& 0& \cdots \\ A_1{B}_0& B_1& 0& \cdots \\ A_2{A}_1{B}_0& A_2{B}_1& B_2& \cdots \\ ⋮& ⋮& ⋮& \ddots \end{array}\right]$$

这是一个块下三角矩阵。

---

12.6 $W_{ex}$

$$W_{ex}=\left[\begin{array}{c}{W}_0\\ A_1{W}_0+W_1\\ A_2{A}_1{W}_0+A_2{W}_1+W_2\\ ⋮\end{array}\right]$$

---

12.7 输出展开

定义：

$$Y_{ex}=C_{ex}{X}_{ex}$$

$$y_k=\left[\begin{array}{c}{e}_k\\ {\theta }_{e,k}\end{array}\right]$$
这里的$Y_{ex}$用于后续做约束，求解最小值。

其中：
$$C_k=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$$
$$C_{ex}=\mathrm{diag}(C_0,C_1,\dots ,C_{N-1})$$

---

13. 代价函数是怎么设计出来的，为什么是二次型

代价函数不是“物理定律推导出来的”，而是根据控制目标设计出来的。

---

13.1 目标 1：跟踪误差小

希望：

• 横向误差小
• 航向误差小

设每步输出为：

$$y_k=C_k{x}_k$$

单步惩罚：

$$y_k^T{Q}_k{y}_k$$

堆叠后：

$$J_{\text{state}}=Y_{ex}^T{Q}_{ex}{Y}_{ex}$$

其中：

$$Q_{ex}=\mathrm{diag}(Q_0,Q_1,\dots ,Q_{N-1})$$

其中 $Q_k⪰0$ 是权重矩阵。

---

13.2 目标 2：输入不要过大，尽量接近轨迹角度

不希望控制器猛打方向，即输入不要太大。但更合理的是让输入靠近参考前馈输入：

$$u_{ref,k}=\arctan (L{\kappa }_k)$$

于是：

$$J_{\text{input}}=(U_{ex}-U_{ref,ex}{)}^T{R}_{1ex}(U_{ex}-U_{ref,ex})$$

---

13.3 目标 3：输入变化要平滑

为了避免转向抖动，希望：

$$u_k-u_{k-1}$$

尽量小，惩罚：

$$(u_k-u_{k-1}{)}^2$$

堆叠后可写成：

$$J_{\text{smooth}}=U_{ex}^T{R}_{2ex}{U}_{ex}$$

其中 $R_{2ex}$ 编码了输入差分的二次项。

---

13.4 最终总代价函数

$$J=Y_{ex}^T{Q}_{ex}{Y}_{ex}+(U_{ex}-U_{ref,ex}{)}^T{R}_{1ex}(U_{ex}-U_{ref,ex})+U_{ex}^T{R}_{2ex}{U}_{ex}$$

再由于：

$$Y_{ex}=C_{ex}{X}_{ex}$$

也可写成：

$$J=X_{ex}^T{C}_{ex}^T{Q}_{ex}{C}_{ex}{X}_{ex}+(U_{ex}-U_{ref,ex}{)}^T{R}_{1ex}(U_{ex}-U_{ref,ex})+U_{ex}^T{R}_{2ex}{U}_{ex}$$

---

14. 把预测方程代入代价函数：如何变成只关于输入的优化问题

现在关键来了。

已经有：

$$X_{ex}=A_{ex}{x}_0+B_{ex}{U}_{ex}+W_{ex}$$

$$Y_{ex}=C_{ex}{X}_{ex}$$

所以：

$$Y_{ex}=C_{ex}(A_{ex}{x}_0+B_{ex}{U}_{ex}+W_{ex})$$

定义简写：

$$A:=A_{ex},B:=B_{ex},C:=C_{ex},Q:=Q_{ex},R_1:=R_{1ex},R_2:=R_{2ex},U:=U_{ex},U_r:=U_{ref,ex},W:=W_{ex}$$

则：

$$X=A{x}_0+BU+W$$

$$Y=C(A{x}_0+BU+W)$$

设：

$$d:=C(A{x}_0+W)$$

则：

$$Y=d+CBU$$

于是状态项：

$$J_{\text{state}}=(d+CBU{)}^{T}Q(d+CBU)$$

展开：

$$J_{\text{state}}=U^T{B}^T{C}^{T}QCBU+2d^{T}QCBU+d^{T}Qd$$

输入项：

$$(U-U_r{)}^T{R}_1(U-U_r)=U^T{R}_{1}U-2U^T{R}_1{U}_r+U_r^T{R}_1{U}_r$$

平滑项：

$$U^T{R}_{2}U$$

合并后：

$$J=U^T(B^T{C}^{T}QCB+R_1+R_2)U+2(d^{T}QCB-U_r^T{R}_1)U+\text{const}$$

其中常数项与 $U$ 无关，不影响优化，可以忽略。

---

15. 输入约束和输入变化率约束如何写成 QP 形式

前面我们已经把代价函数整理成了只关于未来输入序列 $U$ 的二次型。
但只有目标函数还不够，因为真实车辆永远存在物理限制：

• 转角不能无限大
• 转角变化不能无限快

因此，MPC 不是一个“只看目标函数”的无约束优化，而是一个带线性约束的二次优化问题。
也正因为如此，它最后自然落到 QP 这个数学形式上。

---

15.1 优化变量到底是什么

在这一层里，真正被优化的不是状态 $x$，而是未来一段时间内的控制输入序列。

把未来 $N$ 步输入堆叠起来：

$$U=\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]\in {\mathbb{R}}^{N_u}$$

这里：

• 若每一步只有一个转向输入，则 $N_u=N$
• 更一般地，若每一步输入维度为 ${\mathrm{DIM}}_U$，则 $N_u=N\cdot {\mathrm{DIM}}_U$

这一步一定要想清楚，因为 QP 求解器根本不直接“优化轨迹”或“优化误差”，它优化的是：

未来一串控制量 $U$

而未来状态 $X_{ex}$、未来输出 $Y_{ex}$ 都只是这个输入序列通过预测模型推出来的结果。

---

15.2 输入幅值约束为什么是 box 约束

如果转向命令存在物理上限 ${\delta }_{\max }$，那么每一步都必须满足：

$$-{\delta }_{\max }\le u_k\le {\delta }_{\max }$$

把全部时刻堆叠起来后，就得到最简单的一类约束：

$$lb\le U\le ub$$

其中：

$$lb=-{\delta }_{\max }1,ub={\delta }_{\max }1$$

这类约束叫 box 约束，本质上就是：

每个优化变量单独都有一个上下界

从几何上看，它在输入空间里对应一个超矩形区域。
QP 求解器只能在这个可行区域内寻找最优 $U$。

---

15.3 输入变化率约束从哪里来

真实车辆除了“能打多大方向”之外，还有“能多快打方向”的限制。

若连续时间下存在转角速度上限：

$$|\dot{u}(t)|\le {\dot{\delta }}_{\max }(t)$$

那么离散化之后，相邻两步输入之间就不能差太多。
在第 $k$ 步，可写成：

$$-{\dot{\delta }}_{\max ,k}\Delta t\le u_k-u_{k-1}\le {\dot{\delta }}_{\max ,k}\Delta t$$

这条式子的物理意义非常直接：

• 左边限制“不要往负方向变得太快”
• 右边限制“不要往正方向变得太快”

如果 ${\dot{\delta }}_{\max ,k}$ 随速度、曲率或工况变化，那么它就不再是一个常数，而是一组逐步变化的上限，这也是工程实现里常见的做法。

---

15.4 为什么差分约束可以写成矩阵形式

QP 喜欢的约束形式是仿射不等式：

$$lbA\le A_{\text{ineq}}U\le ubA$$

所以关键问题变成：

如何把“相邻输入之差”写成矩阵乘法

定义差分矩阵：

$$D=\left[\begin{array}{cccc}1& 0& 0& \cdots \\ -1& 1& 0& \cdots \\ 0& -1& 1& \cdots \\ ⋮& ⋮& ⋮& \ddots \end{array}\right]$$

则：

$$DU=\left[\begin{array}{c}{u}_0\\ u_1-u_0\\ u_2-u_1\\ ⋮\end{array}\right]$$

这条式子就是整个输入变化率约束写成 QP 形式的核心。

它告诉我们：

• 第一行取出的是 $u_0$
• 第二行取出的是 $u_1-u_0$
• 第三行取出的是 $u_2-u_1$
• 以此类推

因此，原本逐项写的差分约束，现在可以统一改写为：

$$lbA\le DU\le ubA$$

这一步的本质是“结构重写”：

没有改变任何物理含义，只是把一串标量不等式改写成了一个矩阵不等式

---

15.5 为什么第一项要单独处理

对 $k\ge 1$ 的项，差分总是：

$$u_k-u_{k-1}$$

但对第一步来说，并不存在预测时域内部的 $u_{-1}$。
第一个控制量真正要比较的对象，不是“预测时域中的前一个量”，而是：

上一个控制周期已经实际发出去的控制命令

记它为 $u_{\mathrm{prev}}$，控制周期为 $T_c$，则第一项约束应写成：

$$u_{\mathrm{prev}}-{\dot{\delta }}_{\max ,0}{T}_c\le u_0\le u_{\mathrm{prev}}+{\dot{\delta }}_{\max ,0}{T}_c$$

这非常重要，因为它把当前优化问题和上一拍真实执行过的控制量接起来了。
没有这一步，优化器就可能在当前时刻给出一个相对上一拍跳变过大的新命令。

所以完整的差分约束实际是：

$$DU=\left[\begin{array}{c}{u}_0\\ u_1-u_0\\ u_2-u_1\\ ⋮\end{array}\right]$$

并配套边界：

$$lbA=\left[\begin{array}{c}{u}_{\mathrm{prev}}-{\dot{\delta }}_{\max ,0}{T}_c\\ -{\dot{\delta }}_{\max ,1}\Delta t\\ -{\dot{\delta }}_{\max ,2}\Delta t\\ ⋮\end{array}\right]$$

$$ubA=\left[\begin{array}{c}{u}_{\mathrm{prev}}+{\dot{\delta }}_{\max ,0}{T}_c\\ {\dot{\delta }}_{\max ,1}\Delta t\\ {\dot{\delta }}_{\max ,2}\Delta t\\ ⋮\end{array}\right]$$

这样写以后，第 0 行和后续各行就被统一装进了同一个矩阵不等式里。

---

15.6 最终约束的统一写法

因此，这个 MPC 对输入的约束可以概括成两组：

幅值约束

$$lb\le U\le ub$$

变化率约束

$$lbA\le DU\le ubA$$

如果愿意，也可以进一步把二者堆成一个更大的线性约束系统：

$$\left[\begin{array}{c}lb\\ lbA\end{array}\right]\le \left[\begin{array}{c}I\\ D\end{array}\right]U\le \left[\begin{array}{c}ub\\ ubA\end{array}\right]$$

这说明一个很本质的事实：

只要模型是线性的、约束是对输入的线性不等式、代价函数是二次型，问题就天然是 QP

---

16. QP 求解器到底在解什么数学问题

从原理上说，MPC 在这一层解的是：

一个关于未来输入序列 $U$ 的凸二次优化问题

它之所以是“二次”的，来自二次代价函数；
它之所以是“带约束”的，来自输入幅值和输入变化率限制。

---

16.1 为什么 MPC 最后会落到 QP

回顾前面的链条：

• 预测模型是线性的：
  $$X_{ex}=A_{ex}{x}_0+B_{ex}{U}_{ex}+W_{ex}$$
• 输出也是线性的：
  $$Y_{ex}=C_{ex}{X}_{ex}$$
• 代价函数是二次型：
  $$J=Y_{ex}^T{Q}_{ex}{Y}_{ex}+(U_{ex}-U_{ref,ex}{)}^T{R}_{1ex}(U_{ex}-U_{ref,ex})+U_{ex}^T{R}_{2ex}{U}_{ex}$$
• 约束是线性不等式：
  $$lb\le U\le ub,lbA\le DU\le ubA$$

这四点一组合，就已经把问题的类型完全定死了：

线性预测模型 + 二次代价函数 + 线性约束
就是标准的 Quadratic Programming

所以 QP 不是“额外选的求解工具”，而是这套建模自然导出的数学结果。

---

16.2 把输出写成关于 $U$ 的仿射函数

从预测方程出发：

$$X=A{x}_0+BU+W$$

$$Y=CX=C(A{x}_0+BU+W)$$

把与 $U$ 无关的部分记为：

$$d:=C(A{x}_0+W)$$

把输入到输出的映射记为：

$$M:=CB$$

于是：

$$Y=d+MU$$

这一步非常关键。它告诉我们：

• $d$ 描述“在当前状态 $x_0$、当前参考轨迹和偏置项 $W$ 已知时，不考虑优化变量也会自然产生的输出偏移”
• $MU$ 描述“未来输入序列 $U$ 会怎样改变整个预测输出”

因此，从优化角度看，问题已经被压缩成：

用输入序列 $U$ 去修正一个已有偏移 $d$

---

16.3 从总代价函数推到二次型

把：

$$Y=d+MU$$

代回总代价函数：

$$J=Y^{T}QY+(U-U_r{)}^T{R}_1(U-U_r)+U^T{R}_{2}U$$

得到：

$$J=(d+MU{)}^{T}Q(d+MU)+(U-U_r{)}^T{R}_1(U-U_r)+U^T{R}_{2}U$$

下面分三部分展开。

第一部分：状态跟踪项

$$(d+MU{)}^{T}Q(d+MU)=U^T{M}^{T}QMU+2d^{T}QMU+d^{T}Qd$$

这里：

• $U^T{M}^{T}QMU$ 是二次项，表示输入变化对跟踪误差带来的二阶影响
• $2d^{T}QMU$ 是一次项，表示当前偏差 $d$ 会把最优解往某个方向“推过去”
• $d^{T}Qd$ 是常数项，不依赖于 $U$

第二部分：输入偏离前馈项

$$(U-U_r{)}^T{R}_1(U-U_r)=U^T{R}_{1}U-2U_r^T{R}_{1}U+U_r^T{R}_1{U}_r$$

这里：

• $U^T{R}_{1}U$ 惩罚输入本身过大
• $-2U_r^T{R}_{1}U$ 体现“最优输入倾向于贴近参考前馈输入 $U_r$”
• $U_r^T{R}_1{U}_r$ 仍然是常数项

第三部分：输入平滑项

$$U^T{R}_{2}U$$

它本身已经是标准二次型，无需再展开。

---

16.4 合并得到标准二次目标函数

把三部分合并：

$$J=U^T(M^{T}QM+R_1+R_2)U+2(d^{T}QM-U_r^T{R}_1)U+\text{const}$$

记：

$$G:=M^{T}QM+R_1+R_2$$

$$c:=M^{T}Qd-R_1{U}_r$$

则：

$$J(U)=U^{T}GU+2c^{T}U+\text{const}$$

由于常数项不影响最优解，可以省略，于是优化问题只剩下：

$$\underset{U}{\min }{U}^{T}GU+2c^{T}U$$

这已经是一个非常标准的二次优化问题了。

---

16.5 为什么教材里常写成 $\frac{1}{2}{U}^{T}HU+g^{T}U$

数学上，二次规划最常见的标准形式是：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U$$

它和上面的写法完全等价，只是记号不同。

若从：

$$J(U)=U^{T}GU+2c^{T}U$$

变成标准形式，只需令：

$$H=2G,g=2c$$

于是：

$$\frac{1}{2}{U}^{T}HU+g^{T}U=\frac{1}{2}{U}^T(2G)U+(2c{)}^{T}U=U^{T}GU+2c^{T}U$$

所以这里的 $\frac{1}{2}$ 没有任何新的物理意义，它只是一个为了让导数更简洁的记号约定：

$$\nabla \left(\frac{1}{2}{U}^{T}HU\right)=HU(\text{当 }H\text{ 对称时})$$

也就是说：

标准 QP 形式中的 $\frac{1}{2}$ 是“记号规范”，不是“新的控制假设”

---

16.6 Hessian 为什么一定是对称的

由定义：

$$G=M^{T}QM+R_1+R_2$$

若：

$$Q⪰0,R_1⪰0,R_2⪰0$$

则：

$$M^{T}QM⪰0$$

因此：

$$G⪰0$$

从而标准形式中的：

$$H=2G$$

也是对称半正定的。

这意味着：

• 目标函数是凸的
• 只要约束集合非空，优化问题就是凸 QP
• 若再进一步有 $R_1+R_2\succ 0$，通常还能保证解更稳定、甚至唯一

这就是为什么在线性 MPC 里我们特别喜欢二次型代价函数：

它既能表达跟踪误差、输入大小、输入平滑性，又能保持问题的凸性

---

16.7 一次项 $g$ 的物理意义是什么

很多人第一次学 QP 时，只记住了 Hessian，却不太明白一次项为什么存在。

从上面的推导可知：

$$c=M^{T}Qd-R_1{U}_r$$

所以一次项的来源有两部分：

来源 1：当前偏差 $d$

若当前状态已经偏离参考，那么即便未来输入全为零，输出也会带着偏移量：

$$d=C(A{x}_0+W)$$

这一项会把优化器“推向能消除当前误差的方向”。

来源 2：前馈输入 $U_r$

若参考轨迹本身是弯的，那么理想情况下系统本来就不该围绕零输入工作，而应围绕某个参考转向输入工作：

$$U_r$$

这一项会把优化器“推向靠近参考前馈输入的方向”。

所以可以把一次项理解成：

当前误差和参考曲率共同告诉优化器：最优解应当朝哪个方向偏移

而 Hessian 则告诉优化器：

往那个方向偏移时，会付出多大的二次代价

---

16.8 如果没有约束，最优解长什么样

为了建立直觉，先暂时去掉所有约束，只看：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U$$

若 $H$ 可逆，则一阶最优条件为：

$$\nabla J=HU+g=0$$

因此：

$$U^{\star }=-H^{-1}g$$

这条式子很有启发性，因为它说明：

• 若没有物理限制，最优解只是一个线性代数问题
• 真正让问题变成“需要 QP 求解器”的，不是二次代价本身，而是线性约束的加入

一旦加入：

$$lb\le U\le ub,lbA\le DU\le ubA$$

最优解就不再是简单的闭式解，而必须在可行域边界和目标函数之间做折中。

---

16.9 最终的完整 QP 数学形式

因此，基于前面的预测模型、代价函数和输入约束，最终得到的 QP 可以完整写成：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U$$

其中：

$$H=2(B_{ex}^T{C}_{ex}^T{Q}_{ex}{C}_{ex}{B}_{ex}+R_{1ex}+R_{2ex})$$

$$g=2(B_{ex}^T{C}_{ex}^T{Q}_{ex}{C}_{ex}(A_{ex}{x}_0+W_{ex})-R_{1ex}{U}_{ref,ex})$$

满足：

$$lb\le U\le ub$$

$$lbA\le DU\le ubA$$

其中：

$$lb=-{\delta }_{\max }1,ub={\delta }_{\max }1$$

并且：

$$lbA=\left[\begin{array}{c}{u}_{\mathrm{prev}}-{\dot{\delta }}_{\max ,0}{T}_c\\ -{\dot{\delta }}_{\max ,1}\Delta t\\ ⋮\end{array}\right],ubA=\left[\begin{array}{c}{u}_{\mathrm{prev}}+{\dot{\delta }}_{\max ,0}{T}_c\\ {\dot{\delta }}_{\max ,1}\Delta t\\ ⋮\end{array}\right]$$

这就是求解器真正面对的数学问题。

如果某个实现把目标函数写成：

$$U^{T}HU+2f^{T}U$$

那只是符号约定不同，和上面的标准形式完全等价。
本质上，求解器解的始终是同一个问题：

在物理约束允许的范围内，寻找一串未来控制输入，使未来预测误差最小、输入不过大、输入变化又足够平滑

---

16.10 这个 QP 一般是怎么求解的

到这里为止，我们已经把 MPC 写成了一个标准凸二次规划：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U$$

满足：

$$lb\le U\le ub,lbA\le DU\le ubA$$

---

先看没有约束时会怎样

如果暂时把所有约束都拿掉，那么问题退化为：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U$$

对 $U$ 求导并令梯度为零：

$$\nabla J=HU+g=0$$

因此，若 $H$ 可逆，则：

$$U^{\star }=-H^{-1}g$$

这说明一件很重要的事：

• 二次目标函数本身并不难
• 真正让问题变成 QP 的，是约束

一旦加入转角幅值约束和转角变化率约束，最优解就不能再直接写成一个闭式公式，而必须在“目标最优”和“约束可行”之间寻找平衡。

---

统一写成单边不等式

为了更方便讨论求解原理，可以把双边约束统一改写成单边不等式：

$$GU\le h$$

例如可以写成：

$$G=\left[\begin{array}{c}I\\ -I\\ D\\ -D\end{array}\right],h=\left[\begin{array}{c}ub\\ -lb\\ ubA\\ -lbA\end{array}\right]$$

于是整个问题变成：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U,GU\le h$$

这就是教科书里最常见的凸 QP 形式。

---

最优解满足什么条件：KKT 条件

对这个带不等式约束的凸 QP，最优解满足 KKT 条件。

引入拉格朗日乘子：

$$\lambda \ge 0$$

构造拉格朗日函数：

$$\mathcal{L}(U,\lambda )=\frac{1}{2}{U}^{T}HU+g^{T}U+{\lambda }^T(GU-h)$$

最优解满足：

$$HU+g+G^T\lambda =0$$

$$GU-h\le 0$$

$$\lambda \ge 0$$

$${\lambda }_i(GU-h{)}_i=0,\forall i$$

这四条分别表示：

• 驻点条件：目标函数梯度与约束反作用力平衡
• 原始可行性：解必须满足原约束
• 对偶可行性：拉格朗日乘子非负
• 互补松弛：只有真正“卡住”的约束才会对应非零乘子

所以从本质上说，所有 QP 求解器都在做同一件事：

找到一组满足 KKT 条件的 $(U,\lambda )$

---

常见求解方法 1：Active-Set

Active-Set 方法的核心想法是：

先猜哪些约束在最优点会正好卡住

把这些“活跃约束”记为：

$$G_{A}U=h_A$$

那么原问题暂时变成一个等式约束二次规划：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U,G_{A}U=h_A$$

它对应的 KKT 线性系统为：

$$\left[\begin{array}{cc}H& G_A^T\\ G_A& 0\end{array}\right]\left[\begin{array}{c}U\\ {\lambda }_A\end{array}\right]=\left[\begin{array}{c}-g\\ h_A\end{array}\right]$$

解出之后，再检查：

• 有没有别的约束被违反
• 当前 active set 里的某些乘子是否变成负值

如果有，就调整活跃约束集合，再重复。

这个方法很适合 MPC，因为相邻时刻的问题往往很像，活跃约束集合也常常变化不大，所以 warm start 很有效。

---

常见求解方法 2：Interior-Point

Interior-Point 的思路不是去猜哪条约束活跃，而是始终待在可行域内部，然后逐渐逼近边界上的最优点。

做法通常是先引入松弛变量：

$$GU+s=h,s>0$$

再构造带对数障碍项的目标函数：

$$\underset{U}{\min }\frac{1}{2}{U}^{T}HU+g^{T}U-\mu \sum _i\log s_i$$

其中 $\mu >0$ 是 barrier 参数。

当 $\mu$ 较大时，解会远离约束边界；当 $\mu \to 0$ 时，解会逐渐逼近原始 QP 的最优点。

这类方法的特点是：

• 数值上通常很稳健
• 对中大型问题很强
• 每一步往往需要解较大的牛顿线性系统

本质上，它是在“可行域内部”沿着一条中心路径逐步逼近最优解。

---

常见求解方法 3：Operator Splitting / ADMM

还有一类方法不直接一次性求解完整 KKT 系统，而是把问题拆成几个更容易的子问题交替求解。

典型思想是：

• 一步解一个带正则项的二次问题
• 一步把结果投影回约束集合
• 一步更新乘子或对偶变量

这类方法的代表就是 ADMM 风格的 QP 求解器。

它的优点通常是：

• 算法结构简单
• 很适合稀疏大规模问题
• 很适合嵌入式和实时优化
• warm start 也比较自然

它的缺点是：

• 相比高精度 interior-point，往往需要更多迭代
• 但对 MPC 这类“每拍都要很快给出一个够好的答案”的问题，往往已经非常合适

---

对 MPC 来说一般怎么选

从控制应用的角度看，常见选择大致是：

• 若问题规模中小、相邻时刻变化不大、希望充分利用 warm start，常用 Active-Set
• 若问题规模更大、追求稳健和高精度，常用 Interior-Point
• 若问题稀疏、强调实时性和工程实现便利，常用 ADMM / OSQP 这一类方法

---

17. 整套链条的统一大图景

总链条：

理想阿克曼几何
→ \tan\delta = L/R
→ 世界坐标自行车模型
→ Frenet坐标定义：e, θe
→ 精确非线性误差模型
→ 小误差近似
→ 执行器动态：τ
→ 连续时间线性仿射模型
→ Tustin离散化
→ 单步离散模型
→ N步批量预测方程
→ 代价函数设计（误差、输入、平滑）
→ 代入预测方程
→ QP标准形式
→ 输入与输入差分约束
→ QP求解器
→ 得到最优未来控制序列
→ 执行第一步输入并滚动优化

---

18. 常见误区与统一澄清

---

18.1 “代价函数是推导出来的吗？”

不是。
更准确地说：

• 动力学模型是推导出来的
• 代价函数是根据控制目标设计出来的

但一旦设计好，它就可以严格地代入模型并整理为 QP。

---

18.2 “$\tau$ 是阿克曼几何里推出来的吗？”

不是。
$\tau$ 是执行器动态参数，用来描述“转角不会瞬间到位”。

---

18.3 “为什么还要前馈输入 $U_{ref}$？”

因为沿参考曲率走本来就需要一个理想转角：

$$u_{ref}=\arctan (L\kappa )$$

MPC 只优化相对于这个前馈值的偏差，能让控制更自然，也减小误差负担。

---

18.4 “为什么平滑项会变成 $U^T{R}_{2}U$ 而不是显式写 $(u_i-u_{i-1}{)}^2$”

因为所有差分平方项展开后，本质上都能写成一个整体二次型：

$$U^T{R}_{2}U$$

这样求解器更容易处理。

---

18.5 “为什么最后只执行第一个输入？”

因为 MPC 是滚动优化。未来总会变化，所以：

• 先优化整个未来
• 但只执行眼前最优那一步
• 下一时刻重新测量、重新预测、重新优化