5.1 MPC & OSQP

0. 前言

​ 本文是一篇面向初学者的 MPC（模型预测控制）入门教程，旨在帮助你建立从零到一的知识体系：理解什么是 MPC、它解决什么问题、OSQP 在其中扮演什么角色，以及如何用 OSQP 的 Python 接口搭建一个可运行的线性 MPC 控制器，并在 MuJoCo 中验证控制效果。

读完本文后，你将能够：

• 理解 MPC 的核心思想及其数学表述
• 掌握将线性 MPC 转化为二次规划（QP）问题的完整流程
• 知道 OSQP 是什么、为什么选择它
• 理解 OSQP 的 ADMM 算法是如何求解 QP 问题的
• 对一个完整的四旋翼 MPC 控制实例有整体把握
• 知道如何调节关键参数来影响控制性能

---

目录

2. 什么是模型预测控制(MPC)
3. MPC问题的基本形式
4. 标准QP问题与OSQP简介
5. OSQP的基本原理与相关概念
6. 将MPC问题的OSQP部署
7. 参数调优指南
8. 附录 A：关键函数详解与速查

---

2. 什么是模型预测控制(MPC)

2.1 核心思想

核心思想：在每个控制周期，利用系统的数学模型预测未来一段时间内的系统行为，然后求解一个优化问题来找到最优的控制序列，但只执行第一个控制量，下一周期重复整个过程。

这个思想可以用四个关键词概括：

关键词	含义
模型（Model）	使用数学方程描述系统的动态行为（如何从当前状态演变到未来状态）
预测（Predict）	在有限时域 $T_f$ 内，基于模型推算系统的未来状态轨迹
优化（Optimize）	在满足约束的前提下，求解使某个代价函数最小的控制序列
滚动时域（Receding Horizon）	只执行第一个控制量，下一周期用新的测量状态重新优化

滚动时域示意图（= 表示已执行, - 表示预测, x 表示优化后的第一步）：

时间轴 →  t=0        t=Ts       t=2Ts      t=3Ts      ...
         |---N步预测------>|
周期0:   x=========---------...----
                          (仅执行第一个控制量)
周期1:              x=========---------...----
                                     (仅执行第一个控制量)
周期2:                         x=========---------...----
         <--滑动窗口始终向前移动, 每次重新优化-->

2.2 为什么需要预测？

​ 考虑一个日常类比：你开车时，眼睛看的不只是车头前方 1 米，而是几十米甚至更远。你根据前方的路况（弯道、障碍物）提前调整方向盘和油门，而不是等车轮压到弯道才开始打方向。这就是"预测 + 提前动作"的逻辑，MPC 正是将这种直觉数学化。

​ 再类比一个徒步者的故事：他有一张局部地形图（这对应 MPC 的系统模型），能看到方圆一定范围内的地形（对应 MPC 的预测时域），但看不到远处的全貌。他的目标是走最短路径到达一个遥远的目的地。因为他只能看到有限范围内的地形，所以需要根据当前视野规划一段路线，走一段后，随着视野向前推移，他会根据新看到的信息重新规划。这与 MPC 的滚动时域优化思想完全一致，即由于预测视野有限，未来的"障碍"（如约束）可能在当前时域内不可见，因此需要不断重新优化。

2.3 预测时域与控制时域

MPC 中有两个关键的时间概念：

概念	符号	含义
预测时域（Prediction Horizon）	$T_p$	控制器向前预测系统行为的时间长度
控制时域（Control Horizon）	$T_c$	控制器规划自由控制动作的时间长度

通常 $T_c\le T_p$，即规划的控制动作数量不超过预测步数。在控制时域之后，控制量保持为常数，而状态预测仍然持续到预测时域的末端。在本教程的工程中，控制时域与预测时域相等：

$$T_p=T_c=N\cdot \mathrm{\Delta }t=50\times 0.01\text{s}=0.5\text{s}$$

其中 $N$ 是预测步数，$\mathrm{\Delta }t$ 是每步的时间步长。通常来说，时间步长 $\mathrm{\Delta }t$ 越短（离散化精度越高）、预测时间 $T_p$ 越长（视野越远），往往控制效果越好，但两者都会增加 QP 问题的维度，导致计算量大幅增加，可能无法实现实时计算。在有限的计算资源下，需要在控制效果和计算量之间达到尽可能的平衡。

2.4 MPC 算法的基本步骤

将上述思想综合起来，MPC 控制回路在每个采样时刻执行以下步骤：

1. 获取状态：测量或估计系统的当前状态 $\hat{x}(t)$
2. 求解 QP：以 $\hat{x}(t)$ 为初始条件，求解一个在预测时域 $T_p$ 上的开环最优控制问题，得到最优控制序列 ${\overline{u}}^{\ast }(\tau )$
3. 执行第一步：将最优控制序列的第一个值 $u(t)={\overline{u}}^{\ast }(t)$ 施加到系统上
4. 滚动向前：等待到下一个采样时刻 $t+\delta$，用新的测量状态回到步骤 1

这种"求解→执行第一步→重新求解"的循环被称为**滚动时域（Receding Horizon）**策略，是 MPC 区别于其他控制方法的最核心特征。

2.5 线性 MPC vs. 非线性 MPC

线性 MPC（Linear MPC）：使用线性模型 $x_{k+1}=A{x}_k+B{u}_k$ 来描述系统。优点是优化问题为凸二次规划（QP），求解快速且保证全局最优。缺点是无法准确描述大多数真实系统的非线性特性。

非线性 MPC（NMPC）：使用非线性模型 $x_{k+1}=f(x_k,u_k)$ 来描述系统。优点是可以精确建模复杂的物理系统（如四旋翼的空气动力学、机械臂的关节动力学）。缺点是优化问题为非凸非线性规划（NLP），求解更复杂且不一定能保证全局最优。

那么到底该选哪个？ 一个实用的判断标准：

• 如果系统在工作点附近近似线性（如悬停状态下的无人机），线性 MPC 通常足够。
• 如果系统大范围运动、动力学强烈耦合、或包含姿态变化，NMPC 是更好的选择。
• 如果实时性要求极高（kHz 级别）且算力有限，线性 MPC 更安全。对于 100Hz 级别的控制频率，两者都可以胜任。

3. MPC问题的基本形式

线性 MPC 的核心是将控制问题表述为一个标准的二次规划（QP）问题，然后交给 QP 求解器（如 OSQP）求解。本章将从零开始，逐步推导这个转化过程。

3.1 状态空间模型

一个线性时不变离散时间系统可以写为：

$$x_{k+1}=A{x}_k+B{u}_k$$

其中：

符号	含义
$x_k\in {\mathbb{R}}^n$	第 $k$ 步的系统状态向量
$u_k\in {\mathbb{R}}^m$	第 $k$ 步的控制输入向量
$A\in {\mathbb{R}}^{n\times n}$	系统状态转移矩阵
$B\in {\mathbb{R}}^{n\times m}$	控制输入矩阵

连续到离散的转换：实际的物理系统通常是连续时间的，形式为 $\dot{x}=A_{c}x+B_{c}u$。在 MPC 中，我们需要将其转换为离散时间模型。常用的方法是**零阶保持（ZOH）**离散化：假设控制输入 $u$ 在一个采样周期 $T_s$ 内保持恒定，则：

$$A=e^{A_c{T}_s},B={\int }_0^{T_s}{e}^{A_c\tau }{B}_{c}d\tau$$

在配套工程中，我们使用 SciPy 的 scipy.signal.cont2discrete 函数，通过 method='zoh' 参数完成这一转换。无需手动推导矩阵指数。

3.2 代价函数的设计

MPC 的代价函数通常由两部分组成：过程代价和终端代价。

$$\underset{x_0,\dots ,x_N,u_0,\dots ,u_{N-1}}{min}\sum _{k=0}^{N-1}\underset{\text{过程代价}}{\underbrace{(x_k-x_{\text{ref}}{)}^{T}Q(x_k-x_{\text{ref}})+u_k^{T}R{u}_k}}+\underset{\text{终端代价}}{\underbrace{(x_N-x_{\text{ref}}{)}^T{Q}_N(x_N-x_{\text{ref}})}}$$

其中各符号的含义：

符号	含义
$N$	预测时域的离散步数
$x_{\text{ref}}$	目标/参考状态
$Q$	过程状态权重矩阵（半正定，对角矩阵）
$R$	控制输入权重矩阵（正定，对角矩阵）
$Q_N$	终端状态权重矩阵，通常 $Q_N=Q$ 或更大

权重矩阵的作用：权重决定了不同目标之间的相对重要性。例如：

• 位置权重 大 → 控制器优先保证位置精度，可以接受较大的姿态偏差
• 控制权重 大 → 控制器更"保守"，优先减小控制量幅值
• 角速度权重 小 → 允许更快的姿态机动

在本教程的配套工程中：

• $Q$ 是 $12\times 12$ 的对角矩阵，包含姿态、角速度、位置、速度的权重
• $R$ 是 $4\times 4$ 的对角矩阵，四个电机推力的权重
• $Q_N=Q$，即终端权重等于过程权重

关于控制量的说明：配套工程中使用叠加控制量 $\mathrm{\Delta }{u}_k$（即偏离悬停推力的增量），代价函数中的 $u_k$ 就是 $\mathrm{\Delta }{u}_k$，目标值为零。实际执行时再叠加上悬停推力 $u_{\text{hover}}$：$u_{\text{actual}}=u_{\text{hover}}+\mathrm{\Delta }u$。这种设计让优化问题的参考点为"零控制增量"，避免了代价函数中需要设置非零的控制参考值。

3.3 约束处理

线性 MPC 可以自然地处理两种约束：

控制输入约束：

$$u_{min}\le u_k\le u_{max}$$

在配套工程中，以叠加控制量的形式：

$$-(u_{\text{hover}}-u_{min})\le \mathrm{\Delta }{u}_k\le (u_{max}-u_{\text{hover}})$$

状态约束（可选）：

$$x_{min}\le x_k\le x_{max}$$

在配套工程中，状态约束设置为非常大的范围（$\pm {10}^6$​），实际上相当于无约束。这是教学代码中的简化处理：在实际应用中，你可能需要为姿态角或速度设置合理的上下界。

3.4 ZOH零阶保持离散化

实际的物理系统是连续时间的，而 MPC 需要在离散时间点上进行优化。因此需要将连续状态空间模型转换为离散模型。最常用的方法是零阶保持（Zero-Order Hold, ZOH）。

ZOH 的基本假设：控制输入 $u(t)$ 在一个采样周期 $T_s$ 内保持恒定（"零阶保持"），即：

$$u(t)=u[k],t\in [k{T}_s,(k+1)T_s)$$

这一假设与实际数字控制系统的工作方式完全吻合：控制器在每个周期计算出一个控制量，通过 DAC（数模转换器）保持到下一个周期。

数学推导

给定连续时间线性系统：

$$\dot{x}(t)=A_{c}x(t)+B_{c}u(t)$$

在 ZOH 假设下，该系统在采样时刻 $t=k{T}_s$ 的精确解为：

$$x((k+1)T_s)=e^{A_c{T}_s}\cdot x(k{T}_s)+\left({\int }_0^{T_s}{e}^{A_c\tau }{B}_{c}d\tau \right)\cdot u(k{T}_s)$$

因此离散时间系统矩阵为：

$$A_d=e^{A_c{T}_s},B_d={\int }_0^{T_s}{e}^{A_c\tau }{B}_{c}d\tau$$

代码实现

在实际工程中，我们不需要手动计算矩阵指数和积分。SciPy 提供了 scipy.signal.cont2discrete 函数：

from scipy import signal

# Ac: 连续时间状态转移矩阵 (n×n)
# Bc: 连续时间控制输入矩阵 (n×m)
# Ts: 采样周期 (s)

sys_d = signal.cont2discrete(
    (Ac, Bc, np.zeros((1, n)), np.zeros((1, m))),
    Ts,
    method='zoh'
)
Ad = sys_d[0]  # 离散状态转移矩阵
Bd = sys_d[1]  # 离散控制输入矩阵

函数签名 (Ac, Bc, C, D) 中 C 和 D 是输出矩阵：因为在 MPC 中我们只需要状态方程，所以传入零矩阵即可。

ZOH 为什么是 MPC 中首选的离散化方法？

1. 与控制硬件一致：实际控制器通过 PWM/DAC 输出控制量，在采样周期内自然保持恒定
2. 数值精确：ZOH 给出了连续系统在采样时刻的精确解，无截断误差（区别于欧拉法、双线性变换等近似方法）
3. 适配线性系统：对于线性时不变系统，ZOH 是最自然的离散化

采样时间 $T_s$ 的选择

• 太短：QP 问题中动力学约束增多，计算负担加重，且 $A_d\approx I$（近似单位矩阵），离散模型退化
• 太长：离散化误差增大，线性化假设可能不再成立（姿态角在一个大步长内变化可能过大）

在本教程中，$T_s=0.01\text{s}$（100Hz），这是四旋翼控制的常用频率：足够快以捕捉姿态动态，同时给 OSQP 留出足够的求解时间（约 0.2~0.5ms）。

4. 标准QP问题与OSQP简介

4.1 标准QP问题

二次规划（Quadratic Programming, QP） 是一类特殊的数学优化问题：目标函数为二次型，约束条件为线性。具体而言，标准的凸二次规划问题可以写为：

$$\begin{array}{ll}\text{最小化}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \text{约束条件}& l\le Ax\le u\end{array}$$

其中各符号含义：

符号	维度	含义
$x$	${\mathbb{R}}^n$	优化变量（决策变量）
$P$	${\mathbb{R}}^{n\times n}$	二次项系数矩阵，必须是对称半正定矩阵
$q$	${\mathbb{R}}^n$	线性项系数向量
$A$	${\mathbb{R}}^{m\times n}$	约束矩阵
$l,u$	${\mathbb{R}}^m$	约束下界和上界

关于约束形式的说明

OSQP 采用统一的双边不等式形式 $l\le Ax\le u$，这一设计非常灵活：

• 不等式约束 $a^{T}x\le b$：设置对应行 $l_i=-\mathrm{\infty },u_i=b$
• 不等式约束 $a^{T}x\ge b$：设置对应行 $l_i=b,u_i=+\mathrm{\infty }$
• 等式约束 $a^{T}x=b$：设置对应行 $l_i=u_i=b$
• 无约束变量：对应行不加入 $A$ 矩阵

在 MPC 问题中：

• 动力学方程是等式约束（$l_i=u_i$，收紧两端）
• 电机推力范围是不等式约束（$u_{min}\le T_i\le u_{max}$）
• 状态边界也是不等式约束

凸性的重要性

$P$ 矩阵必须是对称半正定的，这保证了 QP 问题是凸优化问题，具有以下优良性质：

1. 全局最优性：任何局部最优解就是全局最优解（不存在"卡在局部最优"的风险）
2. 对偶性保证：强对偶定理成立，对偶变量（乘子）有明确的物理/几何含义
3. 求解效率：凸 QP 可在多项式时间内求解，迭代算法保证收敛

在 MPC 中，由于 $Q$、$Q_N$ 为半正定对角矩阵，$R$ 为正定对角矩阵，构造的 $P$ 矩阵自然满足半正定性。

一个简单的 QP 示例

考虑如下问题：在约束 $x_1+x_2=1$ 且 $0\le x_1,x_2\le 0.7$ 的条件下，最小化 $2x_1^2+x_1{x}_2+x_2^2+x_1+x_2$。

$$\begin{array}{ll}\text{最小化}& 2x_1^2+x_1{x}_2+x_2^2+x_1+x_2\\ \text{约束条件}& x_1+x_2=1\\ & 0\le x_1\le 0.7\\ & 0\le x_2\le 0.7\end{array}$$

转化为 OSQP 标准形式：

import osqp
import numpy as np
from scipy import sparse

# P = [[4, 1],    (因为 ½(4x₁² + 2x₁x₂ + 2x₂²))
#      [1, 2]]
P = sparse.csc_matrix([[4, 1], [1, 2]])
q = np.array([1, 1])

# 约束: x₁+x₂=1, x₁∈[0, 0.7], x₂∈[0, 0.7]
A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]])
l = np.array([1, 0, 0])
u = np.array([1, 0.7, 0.7])

prob = osqp.OSQP()
prob.setup(P, q, A, l, u, verbose=False)
res = prob.solve()
print(f"x = [{res.x[0]:.4f}, {res.x[1]:.4f}], obj = {res.info.obj_val:.4f}")
# 输出: x = [0.3000, 0.7000], obj = 1.8800

最优解 $x=[0.3,0.7{]}^T$：$x_2$ 达到上界 0.7，$x_1=0.3$ 以满足等式约束。这个简单例子展示了 OSQP 如何使用双边约束 $l\le Ax\le u$ 同时表达等式和不等式约束。

从上面的示例可以归纳出 OSQP 的基本调用流程：

1. 创建求解器 — prob = osqp.OSQP()
2. 初始化 — prob.setup(P, q, A, l, u) 传入问题数据，完成矩阵分解和工作空间分配（仅执行一次）
3. 求解 — res = prob.solve() 执行 ADMM 迭代，返回结果对象
4. （可选）在线更新 — 在 MPC 中，每个控制周期通过 prob.update(q=q_new, l=l_new, u=u_new) 更新时变参数，然后再次 solve()。由于 $P$ 和 $A$ 结构不变，矩阵分解可重用，配合热启动大幅减少迭代次数

4.2 OSQP是什么

OSQP（Operator Splitting Quadratic Program solver）是一个专门用于求解**凸二次规划（Convex QP）**问题的数值优化求解器。它使用 C 语言编写核心算法，同时提供 Python、MATLAB、Julia、Rust 等多种高级语言的接口。

OSQP 求解如下标准形式的 QP 问题：

$$\begin{array}{ll}\text{最小化}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \text{约束条件}& l\le Ax\le u\end{array}$$

其中 $x\in {\mathbf{R}}^n$ 是优化变量，$P\in {\mathbf{S}}_{+}^n$ 是半正定矩阵。约束采用统一的双边不等式形式，即等式约束只需设置 $l_i=u_i$ 即可。

4.3 为什么选择OSQP

在众多 QP 求解器中，OSQP 有几个突出的优势：

1. 一阶算法，无除法运算

OSQP 基于 ADMM（交替方向乘子法）的一阶方法，只在初始化阶段需要一次矩阵分解。所有后续迭代中完全没有除法运算，这使得算法非常鲁棒，不会因为矩阵条件数差而导致数值崩溃。

2. 对稀疏问题的效率高

OSQP 为稀疏矩阵进行了专门的优化。在 MPC 场景中，$P$ 和 $A$ 矩阵通常是高度稀疏的（因为每个时间步只与相邻步有关），OSQP 可以充分利用这种结构。

3. 热启动友好

MPC 中每个控制周期都需要求解一个新的 QP 问题，但这些问题非常相似（只改变了初始状态和参考轨迹）。OSQP 支持热启动，能够直接使用上一个周期的解来初始化当前求解，大幅减少迭代次数。

4. 无外部依赖

OSQP 的 C 核心不依赖任何外部库，可以轻松嵌入嵌入式系统。同时，通过 Python 接口，它又可以无缝集成到工作流中。

5. 可检测不可行问题

OSQP 能够检测问题是原始不可行还是对偶不可行，并提供不可行性证书。这在调试 MPC 控制器时非常有用。

6. 多语言接口

提供 C、C++、Python、Julia、MATLAB、Rust 等多种语言接口，便于在不同环境中使用。

5. OSQP的基本原理与相关概念

5.1 ADMM 算法概述

OSQP 采用 ADMM（交替方向乘子法），这是一种将复杂优化问题分解为多个较简单子问题的一阶算法。对于 QP 问题，ADMM 将原问题分解为两个交替求解的子问题。

ADMM 首先将原问题等价地重写为：

$$\begin{array}{ll}\text{最小化}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \text{约束条件}& Ax=z,l\le z\le u\end{array}$$

其中 $z$ 是引入的辅助变量（松弛变量），约束 $Ax=z$ 和 $l\le z\le u$ 共同替代了原来的 $l\le Ax\le u$。

然后，ADMM 在每个迭代 $k$ 中依次执行以下步骤：

$$\begin{array}{rl}(x^{k+1},{\nu }^{k+1})& \leftarrow \text{求解线性方程组}\\ {\tilde{z}}^{k+1}& \leftarrow z^k+{\rho }^{-1}({\nu }^{k+1}-y^k)\\ z^{k+1}& \leftarrow \mathrm{\Pi }({\tilde{z}}^{k+1}+{\rho }^{-1}{y}^k)\\ y^{k+1}& \leftarrow y^k+\rho ({\tilde{z}}^{k+1}-z^{k+1})\end{array}$$

其中：

符号	含义
$x$	原始优化变量
$z$	松弛变量（满足约束 $l\le z\le u$）
$y$	对偶变量（乘子）
$\nu$	内部对偶变量
$\rho$	步长参数（算法自适应调节）
$\mathrm{\Pi }(\cdot )$	到超矩形 $[l,u]$ 上的投影（即截断/钳位操作）

直观理解：每个 ADMM 迭代分为三个核心操作：

1. 求解线性方程组：在给定当前松弛变量和对偶变量的条件下，计算最优的 $x$ 和 $\nu$
2. 投影到约束框：将当前解投影到可行区间 $[l,u]$ 上（这步极其简单，只是逐元素的 clamp 操作）
3. 更新对偶变量：根据约束违反程度（残差）调整乘子

线性方程组求解是算法的核心（也是最耗时的部分）。OSQP 支持两种方法：

• 直接法：求解具有拟定矩阵的 KKT 系统

$$\left[\begin{array}{cc}P+\sigma I& A^T\\ A& -{\rho }^{-1}I\end{array}\right]\left[\begin{array}{c}{x}^{k+1}\\ {\nu }^{k+1}\end{array}\right]=\left[\begin{array}{c}\sigma x^k-q\\ z^k-{\rho }^{-1}{y}^k\end{array}\right]$$

• 间接法：求解正定约化系统

$$(P+\sigma I+\rho A^{T}A)x^{k+1}=\sigma x^k-q+A^T(\rho z^k-y^k)$$

直接法在初始化时进行一次矩阵分解（如 Cholesky 或 LDL 分解），后续每步只需回代求解，适合小到中等规模问题。间接法使用共轭梯度等迭代法，适合大规模稀疏问题。

5.2 收敛性与残差

OSQP 通过两个残差来判断收敛：

原始残差（衡量等式约束 $Ax=z$ 的满足程度）：

$$r_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{m}}^k=A{x}^k-z^k$$

对偶残差（衡量 KKT 最优性条件的满足程度）：

$$r_{\mathrm{d}\mathrm{u}\mathrm{a}\mathrm{l}}^k=P{x}^k+q+A^T{y}^k$$

当两个残差的无穷范数都小于设定的容差时，算法停止：

$$\parallel r_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{m}}^k{\parallel }_{\mathrm{\infty }}\le {ϵ}_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{m}},\parallel r_{\mathrm{d}\mathrm{u}\mathrm{a}\mathrm{l}}^k{\parallel }_{\mathrm{\infty }}\le {ϵ}_{\mathrm{d}\mathrm{u}\mathrm{a}\mathrm{l}}$$

容差的设置结合了绝对容差（eps_abs）和相对容差（eps_rel）：

$${ϵ}_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{m}}={ϵ}_{\mathrm{a}\mathrm{b}\mathrm{s}}+{ϵ}_{\mathrm{r}\mathrm{e}\mathrm{l}}max\{\parallel A{x}^k{\parallel }_{\mathrm{\infty }},\parallel z^k{\parallel }_{\mathrm{\infty }}\}$$$${ϵ}_{\mathrm{d}\mathrm{u}\mathrm{a}\mathrm{l}}={ϵ}_{\mathrm{a}\mathrm{b}\mathrm{s}}+{ϵ}_{\mathrm{r}\mathrm{e}\mathrm{l}}max\{\parallel P{x}^k{\parallel }_{\mathrm{\infty }},\parallel A^T{y}^k{\parallel }_{\mathrm{\infty }},\parallel q{\parallel }_{\mathrm{\infty }}\}$$

默认值通常为 ${ϵ}_{\mathrm{a}\mathrm{b}\mathrm{s}}={10}^{-4},{ϵ}_{\mathrm{r}\mathrm{e}\mathrm{l}}={10}^{-4}$。对于实时 MPC，可以适当放宽容差（如 eps_abs = 1e-3），以减少迭代次数。

5.3 自适应步长 $\rho$ 调节

$\rho$（步长参数）是 ADMM 算法性能的关键。如果 $\rho$ 太小，收敛缓慢，如果 $\rho$ 太大，算法可能振荡。OSQP 通过自动平衡原始残差和对偶残差来调节 $\rho$：

$${\rho }^{k+1}\leftarrow {\rho }^k\sqrt{\frac{\parallel r_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{m}}{\parallel }_{\mathrm{\infty }}}{\parallel r_{\mathrm{d}\mathrm{u}\mathrm{a}\mathrm{l}}{\parallel }_{\mathrm{\infty }}}}$$

直观理解：如果原始残差远大于对偶残差（等式约束满足得不好），就增大 $\rho$ 以给约束满足更高的"权重"，反之则减小 $\rho$。$\rho$ 只在变化足够大时才更新（由 adaptive_rho_tolerance 控制）。

这个自适应机制是 OSQP 的一大特色，用户无需手动调节 $\rho$，算法会自动找到合适的步长。

5.4 不可行问题检测

当 QP 问题无解时，例如约束过紧导致没有可行点，OSQP 能够检测并报告：

原始不可行：约束条件 $l\le Ax\le u$ 无法同时满足。OSQP 会提供一个证书向量 $v$，使得 $A^{T}v=0$ 且 $u^T{v}_{+}+l^T{v}_{-}<0$。

对偶不可行：目标函数在可行域上无下界。OSQP 会提供一个证书向量 $s$，使得 $Ps=0$ 且 $q^{T}s<0$。

在调试 MPC 控制器时，如果 OSQP 报告 status: primal infeasible，通常是以下原因之一：

• 控制约束 $[u_{min},u_{max}]$ 太紧，无法将状态驱动到目标
• 初始状态偏离参考状态太远，在预测时域内无法到达
• 状态约束过于严格

解决方法通常是放宽约束，或增大预测时域。

5.5 精炼（Polishing）

精炼是 OSQP 的一个可选后处理步骤（设置 polish=True 启用）。它尝试识别最优解处的有效约束集（即哪些不等式约束的边界被达到了），然后精确地求解一个更小的线性方程组。

• 如果有效约束集猜测正确 → 返回高精度解
• 如果猜测错误 → 返回 ADMM 解

精炼会增加一些额外的计算时间，但在需要高精度解的场合很有价值。对于实时 MPC，"够用就好"的 ADMM 解通常已经足够。

5.6 热启动（Warm Start）

热启动是 OSQP 在 MPC 场景中性能提升的关键。对于 MPC，相邻控制周期的 QP 问题非常相似，即只有初始状态和参考轨迹发生了小幅度变化。因此，上一个周期的解（以及相关的内部状态）可以作为当前周期的良好初始猜测。

在工程代码中，热启动是隐式实现的：调用 prob.solve() 时会保留求解器的内部状态，下一次调用自动基于上次的状态继续。通过 prob.warm_start(x) 可以显式提供初始猜测。

说明：在代码中，我们没有显式调用 warm_start，但 OSQP 默认会保留上一次求解的内部状态（对偶变量、$\rho$ 等），从而实现隐式热启动。这是因为同一个 OSQP 对象在 controlUpdate() 中反复调用 solve()。需要注意的是，当通过 update() 修改了 $q$、$l$、$u$ 向量后，热启动仍然有效，因为 $P$ 和 $A$ 矩阵未变，矩阵分解可以重用。

6. 将MPC问题的OSQP部署

6.1 将MPC问题转换为标准QP形式

线性 MPC 的核心是将多步优化问题转化为 OSQP 可求解的一次 QP 问题。基本思路是将所有预测步的状态和控制量堆叠为单一优化变量：

$$z=[x_0,x_1,\dots ,x_N,u_0,u_1,\dots ,u_{N-1}{]}^T\in {\mathbb{R}}^{(N+1)n+Nm}$$

然后分别推导代价函数的二次型矩阵 $P$、线性项 $q$，以及动力学约束矩阵 $A$ 和上下界 $l,u$。以下四个小节逐一展开推导。关于所用到的稀疏矩阵构建函数的说明，参见附录 A。

为什么使用稀疏矩阵？ 预测步数 $N=50$ 时，优化变量 $z$ 的维度为 $12\times 51+4\times 50=812$。$P$ 矩阵（$812\times 812$）和 $A$ 矩阵（$1424\times 812$）中绝大多数元素为零。稀疏矩阵可以显著加速 OSQP 求解：这也是 OSQP 相比传统 QP 求解器的一大优势。

6.1.1 代价函数展开与 $P$ 矩阵推导

回顾 MPC 的代价函数（为简洁，假设参考状态 $x_r=0$，即目标为原点，非零参考状态仅影响线性项 $q$，不影响 $P$）：

$$J=\sum _{k=0}^{N-1}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)+x_N^T{Q}_N{x}_N$$

将决策变量 $z=[x_0,x_1,\dots ,x_N,u_0,\dots ,u_{N-1}{]}^T$ 代入：

$$\begin{array}{rl}J& =x_0^{T}Q{x}_0+x_1^{T}Q{x}_1+\cdots +x_{N-1}^{T}Q{x}_{N-1}\\ & +x_N^T{Q}_N{x}_N\\ & +u_0^{T}R{u}_0+u_1^{T}R{u}_1+\cdots +u_{N-1}^{T}R{u}_{N-1}\end{array}$$

每一项都是二次型 $v^{T}Mv$，对应 $P$ 矩阵中的一块。因为不同时间步的状态和控制之间没有交叉项（$x_i^T(\cdot )x_j$ 当 $i\ne j$），所以 $P$ 是分块对角矩阵：

$$P=\left[\begin{array}{cccccccc}Q& & & & & & & \\ & \ddots & & & & & & \\ & & Q& & & & & \\ & & & Q_N& & & & \\ & & & & R& & & \\ & & & & & \ddots & & \\ & & & & & & R\end{array}\right]$$

在代码中，这一结构用 sparse.block_diag 和 sparse.kron 一行完成：

P = sparse.block_diag([
    sparse.kron(sparse.eye(N), Q),       # N 个 Q 块 (每个 nx×nx)
    QN,                                   # 1 个 QN 块 (nx×nx)
    sparse.kron(sparse.eye(N), R)         # N 个 R 块 (每个 nu×nu)
], format='csc')

推导思路拆解：

代码片段	数学含义	结构
sparse.kron(sparse.eye(N), Q)	$I_N\otimes Q$	$\left[\begin{array}{ccc}Q& & \\ & \ddots & \\ & & Q\end{array}\right]$ 共 $N$ 块
QN	终端代价矩阵	单独的 $Q_N$ 块，维度 $n_x\times n_x$
sparse.kron(sparse.eye(N), R)	$I_N\otimes R$	$\left[\begin{array}{ccc}R& & \\ & \ddots & \\ & & R\end{array}\right]$ 共 $N$ 块

sparse.kron 和 sparse.block_diag 的详细用法参见附录 A。

以 $N=2$ 为例，$P$ 的具体结构为（假设 $n_x=12,n_u=4$）：

$$P_{N=2}=\left[\begin{array}{ccccc}{Q}_{12\times 12}& 0& 0& 0& 0\\ 0& Q_{12\times 12}& 0& 0& 0\\ 0& 0& Q_{N,12\times 12}& 0& 0\\ 0& 0& 0& R_{4\times 4}& 0\\ 0& 0& 0& 0& R_{4\times 4}\end{array}\right]$$

6.1.2 线性项 $q$ 向量的推导

当参考状态 $x_r\ne 0$ 时，代价函数中 $(x_k-x_r{)}^{T}Q(x_k-x_r)$ 展开：

$$(x_k-x_r{)}^{T}Q(x_k-x_r)=\underset{\text{二次项，进入 }P}{\underbrace{x_k^{T}Q{x}_k}}-\underset{\text{线性项，进入 }q}{\underbrace{2x_r^{T}Q{x}_k}}+\underset{\text{常数项，忽略}}{\underbrace{x_r^{T}Q{x}_r}}$$

因此，每个过程状态 $x_k$ 对 $q$ 的贡献为 $-2Q{x}_r$，终端状态 $x_N$ 贡献 $-2Q_N{x}_r$。控制输入 $u_k$ 没有线性项（因为我们的参考控制为零增量）。

代码中：

q = np.hstack([
    np.kron(np.ones(N), -Q @ xr),   # N 个过程状态的线性项
    -QN @ xr,                        # 终端状态的线性项
    np.zeros(N * nu)                 # 控制输入（全零）
])

$q$ 向量的结构：

$$q=\left[\begin{array}{c}-Q{x}_r\\ ⋮\\ -Q{x}_r\\ -Q_N{x}_r\\ 0\\ ⋮\\ 0\end{array}\right]\begin{array}{l}\leftarrow \text{N 次重复（过程状态）}\\ \leftarrow \text{1 次（终端状态）}\\ \leftarrow \text{N 次（控制输入，全零）}\end{array}$$

关于系数 2 的说明：严格推导的线性项应为 $-2Q{x}_r$，但工程代码中写的是 -Q @ xr（缺少系数 2）。这等效于将代价函数整体缩放 $1/2$：由于常数因子不影响最优解的位置，控制效果不变。但在对目标函数值 obj_val 做分析时需要注意此缩放。（np.kron 的用法详见附录 A。）

6.1.3 动力学约束与 $A$ 矩阵推导

动力学约束是 MPC 问题中最核心的约束。对于离散时间系统 $x_{k+1}=A_d{x}_k+B_d{u}_k$，我们需要为每个预测步 $k=0,\dots ,N-1$ 写一个等式：

$$\{\begin{array}{l}{x}_1-A_d{x}_0-B_d{u}_0=0\\ x_2-A_d{x}_1-B_d{u}_1=0\\ ⋮\\ x_N-A_d{x}_{N-1}-B_d{u}_{N-1}=0\end{array}$$

再加上初始条件 $x_0=x_0^{\text{meas}}$（等价于 $-x_0=-x_0^{\text{meas}}$），共 $N+1$ 个等式约束。

将所有约束写为矩阵形式 $A_{\text{eq}}\cdot z=l_{\text{eq}}$：

$$\underset{A_{\text{eq}}}{\underbrace{\left[\begin{array}{cccccccccc}-I& 0& 0& \cdots & 0& |& 0& 0& \cdots & 0\\ A_d& -I& 0& \cdots & 0& |& B_d& 0& \cdots & 0\\ 0& A_d& -I& \cdots & 0& |& 0& B_d& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& |& ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & A_d& -I& |& 0& 0& \cdots & B_d\end{array}\right]}}\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_N\\ u_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]=\left[\begin{array}{c}-x_0^{\text{meas}}\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]$$

$A_{\text{eq}}$ 由两部分水平拼接而成：$A_{\text{eq}}=[A_x{B}_u]$。

$A_x$ 矩阵的构建（状态部分）：

$$A_x=\underset{\text{第一部分}}{\underbrace{\left[\begin{array}{cccc}-I& & & \\ & -I& & \\ & & \ddots & \\ & & & -I\end{array}\right]}}+\underset{\text{第二部分}}{\underbrace{\left[\begin{array}{ccccc}0& 0& 0& \cdots & 0\\ A_d& 0& 0& \cdots & 0\\ 0& A_d& 0& \cdots & 0\\ ⋮& ⋮& \ddots & \ddots & ⋮\\ 0& 0& \cdots & A_d& 0\end{array}\right]}}$$

在代码中，这两个矩阵通过 sparse.kron 一步生成：

# 第一部分: kron(I_{N+1}, -I_{nx})
Ax = sparse.kron(sparse.eye(N+1), -sparse.eye(nx)) \
   # 第二部分: kron(subdiag(I_{N+1}), Ad)
   + sparse.kron(sparse.eye(N+1, k=-1), Ad)

• sparse.eye(N+1) — $(N+1)\times (N+1)$ 单位矩阵，Kronecker 积后在对角线位置产生 $-I_{nx}$ 块
• sparse.eye(N+1, k=-1) — $(N+1)\times (N+1)$ 次对角线矩阵（主对角线下方第一条对角线为 1），Kronecker 积后在次对角线位置产生 $A_d$ 块

$$\text{sparse.eye}(4,k=-1)=\left[\begin{array}{cccc}0& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$

sparse.eye(N, k) 的详细用法参见附录 A。

$B_u$ 矩阵的构建（控制部分）：

$$B_u=\left[\begin{array}{cccc}0& 0& \cdots & 0\\ B_d& 0& \cdots & 0\\ 0& B_d& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & B_d\end{array}\right]$$

第一行全零表示 $x_0$ 不依赖任何控制输入（初始状态已固定）。后续每行 $k$ 的 $B_d$ 位于第 $k-1$ 列，表示 $x_k$ 受 $u_{k-1}$ 影响。

代码实现：

Bu = sparse.kron(
    sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]),
    Bd
)

• sparse.csc_matrix((1, N)) — $1\times N$ 的零矩阵（第一行）
• sparse.eye(N) — $N\times N$ 单位矩阵（后续 $N$ 行）
• sparse.vstack([...]) — 垂直拼接，形成 $(N+1)\times N$ 的模板矩阵
• sparse.kron(模板, Bd) — 将模板中的每个 1 替换为 $B_d$ 块，每个 0 替换为零矩阵块

$A_{\text{eq}}$ 的水平拼接：

Aeq = sparse.hstack([Ax, Bu])  # [Ax | Bu]

sparse.vstack / sparse.hstack 的详细用法参见附录 A。

最终 $A$ 矩阵：合并等式约束与不等式约束：

Aineq = sparse.eye((N+1)*nx + N*nu)  # 不等式约束: 单位矩阵
A = sparse.vstack([Aeq, Aineq], format='csc')

• $A_{\text{eq}}$（$612\times 812$）：动力学等式约束
• $A_{\text{ineq}}$（$812\times 812$）：单位矩阵，配合 $l_{\text{ineq}}/u_{\text{ineq}}$ 实现逐变量的上下界约束
• A（$1424\times 812$）：垂直拼接后的完整约束矩阵

6.1.4 约束上下界 $l$、$u$ 向量的推导

等式约束部分（$l_{\text{eq}}=u_{\text{eq}}$）：

第一行固定初始状态 $x_0=x_0^{\text{meas}}$，后续行强制动力学等式 $x_{k+1}-A_d{x}_k-B_d{u}_k=0$：

$$l_{\text{eq}}=u_{\text{eq}}=\left[\begin{array}{c}-x_0^{\text{meas}}\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]\begin{array}{l}\leftarrow \text{初始条件（每个控制周期更新）}\\ \leftarrow \text{动力学递推 }N\text{ 个零}\end{array}$$

leq = np.hstack([-x0, np.zeros(N * nx)])
ueq = leq  # l == u 形成等式约束

不等式约束部分（$l_{\text{ineq}}\ne u_{\text{ineq}}$）：

控制输入限制（以叠加控制量 $\mathrm{\Delta }u$ 的形式）：

$$-(u_{\text{hover}}-u_{min})\le \mathrm{\Delta }{u}_k\le u_{max}-u_{\text{hover}}$$

状态限制（教学代码中设为极宽范围，实际无约束效果）：

$$x_{min}\le x_k\le x_{max}$$

lineq = np.hstack([
    np.kron(np.ones(N+1), xMin),   # (N+1)*nx 个状态下界
    np.kron(np.ones(N), uMin)      # N*nu 个控制下界
])
uineq = np.hstack([
    np.kron(np.ones(N+1), xMax),   # (N+1)*nx 个状态上界
    np.kron(np.ones(N), uMax)      # N*nu 个控制上界
])

合并：

l = np.hstack([leq, lineq])   # 先等式约束，后不等式约束
u = np.hstack([ueq, uineq])

在线更新的关键：在每个控制周期，只需修改 $l$ 和 $u$ 的前 $n_x$ 个元素（初始状态约束），其余部分不变：

self.l[:self.nx] = -predState   # 更新当前状态
self.u[:self.nx] = -predState
self.prob.update(q=self.q, l=self.l, u=self.u)

6.1.5 稀疏矩阵构建小结

以上所有矩阵构建的核心技巧是：利用 MPC 问题内在的重复结构，用克罗内克积（sparse.kron / np.kron）和块对角操作（sparse.block_diag）替代手动循环填充。涉及的 10 个关键函数（sparse.kron、sparse.block_diag、sparse.eye(k=-1)、sparse.vstack/hstack、sparse.csc_matrix、sparse.diags、np.kron、np.hstack）的完整说明和速查表见附录 A。

6.2 OSQP部署

前面几节完成了 MPC → QP 的数学转化。在工程实现中，完整的控制循环仅需三步：

1. 状态预测 — 利用线性模型递推补偿电机延迟，得到预测状态作为 MPC 的初始条件
2. 更新并求解 — 更新 $l,u$ 的前 $n_x$ 个元素为预测状态（负值），按需更新 $q$ 以反映新的参考状态，调用 prob.update(q, l, u) + prob.solve()
3. 提取并执行 — 取 res.x 中第一个控制量 $\mathrm{\Delta }{u}_0$，叠加悬停推力 $u_{\text{hover}}$ 后输出给电机

为什么使用 update() 而非重新 setup()？ 矩阵 $P$ 和 $A$ 的结构和数值在整个仿真中不变，只有向量 $q,l,u$ 随时间变化。update() 只修改这些向量，保留首次 setup() 完成的矩阵分解，配合隐式热启动将每次求解的迭代次数从 ~200 降至 ~20~50 次。

两个工程的 QP 规模对比

参数	姿态控制 (5.2)	位置控制 (5.4)
状态维度 $n_x$	6	12
控制维度 $n_u$	4	4
预测步数 $N$	50	50
优化变量维度	$6\times 51+4\times 50=506$	$12\times 51+4\times 50=812$
$P$ 矩阵大小	$506\times 506$	$812\times 812$
$A$ 矩阵大小	$908\times 506$	$1424\times 812$
等式约束行数	$6\times 51=306$	$12\times 51=612$
不等式约束行数	$506\times 1=506$	$812\times 1=812$
典型求解时间	0.2~0.3 ms	0.3~0.5 ms

可以看出，12 维状态下的 QP 规模几乎是 6 维的两倍，但在现代 PC 上仍可轻松在 1ms 内完成求解，完全满足 100Hz 控制频率（10ms 周期）的实时性要求。

7. 参数调优指南

本工程的代价函数平衡了姿态稳定性、位置跟踪精度和控制能耗：

状态权重（$Q$ 矩阵的对角元素）：

状态分量	权重	物理含义
$\varphi ,\theta$	5.0	滚转/俯仰姿态角（鼓励水平飞行）
$\psi$	10.0	偏航角跟踪（权重最高）
${\omega }_x,{\omega }_y$	0.1	滚转/俯仰角速度（允许机动）
${\omega }_z$	0.1	偏航角速度
$p_x,p_y$	10.0	水平位置跟踪精度
$p_z$	20.0	高度跟踪精度（安全优先，权重最高）
$v_x,v_y$	1.0	水平速度抑制
$v_z$	2.0	垂向速度抑制

控制权重（$R$ 矩阵）：四个电机推力增量各有权重 30.0，防止控制量过大或振荡。

求解器配置

本工程的关键参数：

参数	设置值	说明
预测步数 $N$	50	50 个预测步
控制周期 $T_s$	0.01 s	每 10ms 更新一次
预测时域 $T_p$	0.50 s	预测 500ms 的未来
OSQP eps_abs	1e-4	绝对收敛容差（默认值）
OSQP eps_rel	1e-4	相对收敛容差（默认值）
OSQP verbose	False	关闭求解器详细输出

状态预测器

因为 MPC 对控制环路中的延迟较为敏感，若 MPC 预测的状态和实际的状态存在偏差，则会导致控制稳定性的下降甚至发散。对于四旋翼控制系统，主要的延迟源自于电机延迟（约 30ms）和求解延迟。

这里引入状态预测器对此进行补偿：

1. 利用同样的线性化离散模型 $x_{k+1}=A_d{x}_k+B_d{u}_k$
2. 基于最近几个控制周期的电机推力历史
3. 进行开环递推预测（PREDICT_STEP = 3 步，即提前约 30ms）
4. 将预测状态作为 MPC 求解的初始状态

这个设计弥补了电机延迟带来的影响，使控制器的表现更接近"零延迟"的理想情况。与 ACADOS NMPC 教程中的 RK4 预测器不同，这里使用线性模型递推，更简单且计算量更小。

预测时域参数（核心）

MPC_CONTROL_DT = 0.01   # 控制更新周期 (s)
MPC_PREDICT_N = 50      # 预测步数

预测时域长度 $T_p=N\times dt$：

• 太短（如 0.2s）：控制器"目光短浅"，容易产生激进的控制动作和超调
• 太长（如 1.0s）：计算量增大，且远未来的线性模型预测不可靠（模型误差累积）
• 推荐范围：系统主要时间常数的 3-5 倍。四旋翼位置环的响应时间约 0.1-0.3s，所以 $T_p=0.5\text{s}$ 是合理选择

步数 $N$：

• 步数越多 → 时间分辨率越高，但 QP 维度线性增加
• 步数越少 → 求解快，但离散化误差大
• 推荐：$N$ 的选择应使每步时间 $dt=T_p/N$ 接近控制周期。本项目中控制周期是 10ms，$dt=0.5/50=0.01\text{s}=10\text{ms}$，刚好匹配

权重矩阵调优

MPC_Q_POS_XY = 10.0      # ← 增大 → 更激进的水平位置跟踪
MPC_Q_POS_Z  = 20.0      # ← 高度权重更高（安全优先）
MPC_Q_RP = 5.0           # ← 增大 → 更强调水平姿态保持
MPC_Q_Y  = 10.0          # ← 偏航角权重
MPC_R_THRUST = 30.0      # ← 增大 → 控制增量更小（控制更保守）

状态权重 vs. 控制权重的权衡：

• 状态权重 $\uparrow$ → 更积极跟踪目标，可能产生较大的控制动作
• 控制权重 $\uparrow$ → 控制增量更小（更保守），但跟踪精度下降

一个系统的调参方法：

1. 首先将位置速度权重设置为 0，仅调节姿态权重和角速度权重，使四旋翼能够保持水平
2. 在姿态能够保持水平并能在扰动下快速恢复的情况下，依次调节速度权重和位置权重
3. 如果不希望姿态波动太大，适当增大姿态权重
4. 如果控制量变化过于剧烈（高频振荡），增大 R_THRUST
5. 反复迭代，直到位置精度和姿态稳定性之间取得满意平衡

注意：权重是相对值。将所有权重同时乘以一个常数不会改变最优解（只是缩放代价函数）。

电机参数

MOTOR_TIME_CONSTANT = 0.031   # 电机一阶响应时间常数 (s)
MOTOR_MAX_THRUST = 1.0       # 单电机最大推力 (N)
MOTOR_HOVER_THRUST = mass * g / 4  # 悬停推力

• 电机时间常数：反映电机的响应速度。如果实际电机比仿真中更慢，需要增大此值以在控制器中补偿。同时考虑增大 PREDICT_STEP 来匹配更大延迟
• 推力上下限：确保约束不会太紧（导致不可行）或太松（控制器可能输出物理上无法实现的推力）
• 悬停推力：线性化模型的工作点。如果四旋翼质量变化（如挂载额外载荷），此值需相应调整

OSQP 求解器参数

# OSQP 默认设置通常已足够，以下是可能需要调整的：
eps_abs = 1e-4    # 绝对收敛容差
eps_rel = 1e-4    # 相对收敛容差
max_iter = 4000   # 最大迭代次数
polish = False    # 是否启用精炼

• 对于实时控制，如果求解速度不够，可以适当增大容差（如 eps_abs = 1e-3）来减少迭代次数
• 如果 QP 求解失败（status != 'solved'），首先检查约束是否合理，而不是调整求解器参数

常见参数问题与解决

症状	可能原因	解决方法
位置缓慢漂移，无法到达目标	位置权重太小	增大 Q_POS_XY / Q_POS_Z
姿态剧烈振荡	姿态权重太小，或控制权重太小	增大 Q_RP，或增大 R_THRUST
稳态误差	模型与实际系统不匹配	检查质量、转动惯量等模型参数是否准确
OSQP 报 infeasible	约束太紧或初始状态太远	放宽控制约束，或增大预测时域
求解时间过长	QP 维度太大或容差太严	减少预测步数 $N$，或增大 eps_abs/eps_rel
控制输出始终为悬停值	目标状态设置问题	检查 GOAL_STATE 是否合理设置

本章参考资料与引申阅读已汇总至 参考资料。

附录 A：关键函数详解与速查

本附录对 MPC 矩阵构建中使用的 10 个关键 Python 函数进行集中说明，包括 SciPy 稀疏矩阵函数和 NumPy 数组操作函数。

A.1 速查总表

函数	所属库	作用	MPC 典型用法
sparse.kron(A, B)	scipy.sparse	稀疏克罗内克积 $A\otimes B$	kron(eye(N), Q) 沿对角线复制 $N$ 个 $Q$
sparse.block_diag(blocks)	scipy.sparse	构建分块对角矩阵	将 $Q$/$Q_N$/$R$ 块堆叠为完整 $P$ 矩阵
sparse.eye(N)	scipy.sparse	$N\times N$ 稀疏单位矩阵	克罗内克积的模板（产生对角重复结构）
sparse.eye(N, k)	scipy.sparse	偏移对角单位矩阵	k=-1 产生次对角线，放置 $A_d$ 块
sparse.vstack([A, B])	scipy.sparse	垂直拼接稀疏矩阵	将 $A_{\text{eq}}$ 和 $A_{\text{ineq}}$ 上下堆叠
sparse.hstack([A, B])	scipy.sparse	水平拼接稀疏矩阵	将 $A_x$ 和 $B_u$ 左右拼为 $[A_x{B}_u]$
sparse.csc_matrix((r, c))	scipy.sparse	创建全零稀疏矩阵	$B_u$ 模板中表示"无控制影响"的零块
sparse.diags([...])	scipy.sparse	由对角线值创建稀疏对角矩阵	构建权重对角矩阵 $Q$、$Q_N$、$R$
np.kron(a, b)	numpy	克罗内克积（稠密）	kron(ones(N), v) 将向量 $v$ 重复 $N$ 份
np.hstack([...])	numpy	水平拼接数组	拼接 $q$、$l$、$u$ 向量的各段

A.2 sparse.kron — 稀疏克罗内克积

from scipy import sparse
sparse.kron(A, B)

原理：将矩阵 $B\in {\mathbb{R}}^{p\times q}$ 的每个元素 $b_{ij}$ 替换为 $b_{ij}\cdot A$。如果 $A$ 是 $m\times n$，则结果是 $mp\times nq$。

在 MPC 中的两个关键用法：

(1) 沿对角线复制权重矩阵：sparse.kron(sparse.eye(N), Q) 将 $Q$ 放到 $N$ 个对角块位置，对应 $N$ 个预测步的状态代价。

$$I_N\otimes Q=\left[\begin{array}{cccc}Q& 0& \cdots & 0\\ 0& Q& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & Q\end{array}\right]$$

(2) 在次对角线放置状态转移矩阵：sparse.kron(sparse.eye(N+1, k=-1), Ad) 将 $A_d$ 放在次对角线位置，对应动力学递推 $x_{k+1}\leftarrow A_d{x}_k$。

A.3 sparse.block_diag — 分块对角矩阵

P = sparse.block_diag([块1, 块2, 块3], format='csc')

原理：将输入的多个矩阵沿主对角线依次排列，其余位置填零。在 MPC 中用于堆叠状态代价块和控制代价块。

$P$ 矩阵的结构（$N=2$ 为例）：

$$P_{N=2}=\left[\begin{array}{ccccc}Q& 0& 0& 0& 0\\ 0& Q& 0& 0& 0\\ 0& 0& Q_N& 0& 0\\ 0& 0& 0& R& 0\\ 0& 0& 0& 0& R\end{array}\right]$$

format='csc' 至关重要：OSQP 要求所有输入矩阵必须是 CSC（Compressed Sparse Column）格式。CSC 按列存储非零元素，适合数值优化中的矩阵分解运算。

A.4 sparse.eye(N, k) — 偏移对角单位矩阵

sparse.eye(N, k=0)   # N×N 单位矩阵（默认主对角线）
sparse.eye(N, k=-1)  # 次对角线（主对角线下方）
sparse.eye(N, k=1)   # 超对角线（主对角线上方）

k=-1 在 MPC 中的关键作用：

$$\text{sparse.eye}(4,k=-1)=\left[\begin{array}{cccc}0& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$

将此矩阵与 $A_d$ 做克罗内克积：sparse.kron(sparse.eye(N+1, k=-1), Ad) 产生：

$$\left[\begin{array}{ccccc}0& 0& 0& \cdots & 0\\ A_d& 0& 0& \cdots & 0\\ 0& A_d& 0& \cdots & 0\\ ⋮& ⋮& \ddots & \ddots & ⋮\\ 0& 0& \cdots & A_d& 0\end{array}\right]$$

这恰好对应动力学约束 $x_1=A_d{x}_0,x_2=A_d{x}_1,\dots$ 中，$x_k$ 受 $x_{k-1}$ 影响的关系。

A.5 sparse.vstack / sparse.hstack — 矩阵拼接

# 垂直拼接: 将 A 和 B 上下堆叠
sparse.vstack([A, B])

# 水平拼接: 将 A 和 B 左右堆放
sparse.hstack([A, B])

在 MPC 中的用法：

• sparse.vstack([Aeq, Aineq]) — 等式约束在上，不等式约束在下，合成完整约束矩阵 $A$
• sparse.hstack([Ax, Bu]) — 状态部分在左，控制部分在右，合成 $A_{\text{eq}}=[A_x{B}_u]$

A.6 np.kron — NumPy 克罗内克积

import numpy as np
np.kron(np.ones(N), v)

原理：与 sparse.kron 相同，但操作在稠密的 NumPy 数组上。np.kron(np.ones(N), v) 是 $[1,1,\dots ,1{]}^T\otimes v=[v,v,\dots ,v{]}^T$，即将向量 $v$ 重复 $N$ 次拼接。

在 MPC 中的用法：构建 $q$ 向量的线性项和 $l/u$ 向量的约束边界。

# 将 -Q@xref 重复 N 次，每个预测步的状态都有相同的参考追踪代价
q_x_part = np.kron(np.ones(N), -Q @ xr)

# 将 uMin 重复 N 次，每个控制步有相同的上下界
l_u_part = np.kron(np.ones(N), uMin)

A.7 稀疏 vs 稠密：内存对比

以 $N=50,n_x=12,n_u=4$ 为例，优化变量 $z$ 共 812 维：

矩阵	维度	稠密存储	稀疏存储	节省
$P$	$812\times 812$	~5.3 MB	~8 KB	99.8%
$A$	$1424\times 812$	~9.2 MB	~20 KB	99.8%

稀疏矩阵仅存储非零元素，而 MPC 的 $P$ 和 $A$ 矩阵中绝大多数元素为零：这正是 OSQP 能高效求解大规模 MPC 问题的关键所在。

A.8 代码片段速查

from scipy import sparse
import numpy as np

# === P 矩阵 (二次项) ===
Q  = sparse.diags([5.0, 5.0, 10.0, 0.1, 0.1, 0.1, 10.0, 10.0, 20.0, 1.0, 1.0, 2.0])
QN = Q
R  = sparse.diags([30.0, 30.0, 30.0, 30.0])
P  = sparse.block_diag([
    sparse.kron(sparse.eye(N), Q),
    QN,
    sparse.kron(sparse.eye(N), R)
], format='csc')

# === A 矩阵 (约束) ===
Ax = sparse.kron(sparse.eye(N+1), -sparse.eye(nx)) \
   + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(
    sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd
)
Aeq = sparse.hstack([Ax, Bu])
Aineq = sparse.eye((N+1)*nx + N*nu)
A = sparse.vstack([Aeq, Aineq], format='csc')

# === q, l, u 向量 ===
q = np.hstack([
    np.kron(np.ones(N), -Q @ xr),
    -QN @ xr,
    np.zeros(N * nu)
])
l = np.hstack([
    -x0, np.zeros(N * nx),                            # 等式约束下界
    np.kron(np.ones(N+1), xMin), np.kron(np.ones(N), uMin)  # 不等式约束下界
])
u = np.hstack([
    -x0, np.zeros(N * nx),                            # 等式约束上界
    np.kron(np.ones(N+1), xMax), np.kron(np.ones(N), uMax)  # 不等式约束上界
])