4. 二次规划（QP）基础：QP 问题标准形式，求解器介绍（OSQP, qpOASES）
好，咱们进入正题。轨迹平滑这件事，说白了就是找一个既满足约束、又足够顺滑的路径。那怎么找？二次规划（QP）就是最常用的数学工具之一。我个人习惯把 QP 看作一个「带镣铐的优化问题」——目标函数是二次的，约束是线性的，求解起来高效又稳定。
4.1 QP 问题的标准形式
先看标准形式，别被公式吓到。其实就三部分：
minimize 1/2 * x^T * P * x + q^T * x
subject to l <= A * x <= u这里：
- x：你要优化的变量，比如轨迹上的每个点
- P：一个对称正定矩阵，控制「平滑度」的权重
- q：线性项，通常跟参考路径有关
- A：约束矩阵，比如道路边界、速度限制
- l, u：下界和上界，说白了就是「不能越界」
重点：为什么是 1/2 * x^T * P * x？因为求导后系数刚好消掉，数学上更干净。嗯，这里要注意，P 必须是对称正定的，否则问题可能无解。
我在项目中遇到过一件事：有一次我直接用单位矩阵当 P，结果轨迹虽然平滑，但偏离参考线太远。后来我调整了 P 的对角线元素，让某些维度更「硬」一些，效果就好多了。你想想看，这就像调弹簧的刚度——太软了乱晃，太硬了又颠簸。
4.2 求解器介绍
标准形式搞清楚了，接下来就是谁来算的问题。市面上 QP 求解器不少，但我最常用的就两个：OSQP 和 qpOASES。为啥？一个快，一个稳。
4.2.1 OSQP（Operator Splitting QP）
OSQP 是我个人比较偏爱的求解器。它基于交替方向乘子法（ADMM），说白了就是「分而治之」——把大问题拆成几个小问题，迭代求解。
- 优点：收敛快，支持稀疏矩阵，适合大规模问题
- 缺点：对病态问题敏感，需要调参
- 适用场景：实时路径规划，比如自动驾驶中的局部轨迹优化
我的经验：OSQP 的默认参数通常够用，但如果你发现收敛慢，可以试试调小 rho 值。我曾经在一个高速场景下，因为没调参，求解器卡了 200 多步才收敛——差点把车开到路肩上。
4.2.2 qpOASES
qpOASES 走的是另一条路——基于积极集法（Active Set）。它特别适合小规模、但需要高精度的场景。
- 优点：精度高，对约束变化响应快
- 缺点：不适合大规模问题，内存占用高
- 适用场景：模型预测控制（MPC）中的小规模 QP
避坑指南：我曾经在 qpOASES 里遇到一个坑——如果约束条件变化太剧烈，求解器会频繁重启，导致计算时间暴增。后来我加了一个「约束缓冲带」，让变化平缓一些，问题就解决了。
4.3 两种求解器的对比
| 特性 | OSQP | qpOASES |
|---|---|---|
| 算法原理 | ADMM（交替方向乘子法） | 积极集法（Active Set） |
| 适用规模 | 大规模（万级变量） | 小规模（千级变量） |
| 收敛速度 | 快（线性收敛） | 中等（二次收敛） |
| 精度 | 中等 | 高 |
| 内存占用 | 低 | 高 |
| 实时性 | 优秀 | 良好 |
怎么选？我的建议是：如果你做的是实时轨迹规划，变量多、要求快，选 OSQP；如果你做的是离线优化，或者对精度要求极高，选 qpOASES。当然，两个都试试也没坏处——我经常在同一个项目里来回切换，看哪个更顺眼。
4.4 核心逻辑流程图
下面这张图展示了 QP 求解的核心流程，从问题建模到最终输出轨迹：
4.5 实际使用示例
光说不练假把式。下面我用 OSQP 写一个简单的轨迹平滑示例。假设我们有 10 个路径点，需要让它们更平滑，同时不能偏离道路边界太远。
import osqp
import numpy as np
from scipy import sparse
# 假设有 10 个路径点，每个点有 x, y 坐标
n = 10
x_ref = np.linspace(0, 10, n) # 参考路径
y_ref = np.sin(x_ref) # 带噪声的参考
# 构建 QP 问题
# 目标：最小化加速度（二阶差分）
P = sparse.eye(2*n) * 2.0 # 平滑权重
q = np.zeros(2*n)
# 约束：道路边界（上下各 0.5 米）
A = sparse.eye(2*n)
l = np.concatenate([x_ref - 0.5, y_ref - 0.5])
u = np.concatenate([x_ref + 0.5, y_ref + 0.5])
# 创建求解器
prob = osqp.OSQP()
prob.setup(P, q, A, l, u, verbose=False)
# 求解
res = prob.solve()
x_opt = res.x[:n]
y_opt = res.x[n:]
print("平滑完成！")
print(f"优化前最大偏差：{np.max(np.abs(y_ref - np.sin(x_ref))):.3f}")
print(f"优化后最大偏差：{np.max(np.abs(y_opt - np.sin(x_ref))):.3f}")小技巧：如果你发现求解结果不理想，可以试试调整 P 矩阵的权重。我一般从 1.0 开始试，然后根据轨迹的平滑度逐步增加。记住，权重越大，轨迹越平滑，但偏离参考路径也越远——这是个 trade-off。
4.6 常见问题与避坑
- 问题：求解器报错「问题不可行」
原因：约束太紧，没有可行解
解决：放宽约束边界，或者检查是否有矛盾约束 - 问题：求解速度慢
原因：矩阵规模太大，或者参数没调好
解决：试试稀疏矩阵，或者调小迭代次数上限 - 问题：轨迹出现抖动
原因：权重设置不合理，或者约束变化太剧烈
解决：增加平滑项的权重，或者加一个低通滤波器
嗯，以上就是 QP 的基础内容。说白了，QP 就是个「带约束的最小二乘」——你只要把问题写成标准形式，剩下的交给求解器就行。我个人觉得，理解 QP 的关键不在于数学推导，而在于知道怎么把实际问题「翻译」成标准形式。这个能力，多做几个项目自然就有了。
可可精品资料库