站在天堂看地狱，人生就像情景剧；站在地狱看天堂，为谁辛苦为谁忙。 —武林外传 白展堂

🏰代码及环境配置：请参考 环境配置和代码运行!

---

在运动规划算法中, QP优化是非常常见的优化问题形式, 本节我们将进行介绍.

5.4.1 QP优化定义

二次规划（Quadratic Programming）优化，是指优化问题的目标函数为二次函数, 且约束条件为线性的问题。定义如下:

$$\begin{array}{ll}\mathrm{minimize}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \text{ subject to }& l\le Ax\le u\end{array}$$

• 决策变量是$x\in {\mathbf{R}}^n$
• 目标函数: 二次函数, 矩阵 $P\in {\mathbf{R}}^{n\times n}$并是一个对称矩阵, 向量$q\in {\mathbf{R}}^n$
• 线性约束: 矩阵 $A\in {\mathbf{R}}^{m\times n}$, 上下界向量$l,u$确定.

5.4.2 QP优化的特点

首先我们介绍一下半正定矩阵定义:

$A$ 是一个 $n\times n$ 的实对称矩阵，如果对所有的非零实向量 $x\in {\mathbb{R}}^n$，都有 $x^{T}Ax\ge 0$，则称 $A$ 为半正定矩阵。特别地，如果 $x^{T}Ax>0$ 对所有非零 $x$ 都成立，则称 $A$ 为正定矩阵。

在QP优化问题中, 当$P$矩阵是半正定矩阵时, 该问题是一个凸二次规划问题. 对于凸优化问题, 极值点就是全局最优解, 具有求解快速的优点.

5.4.3 求解器

在工程中, 我们经常会调用现有的求解器来完成优化问题. 这样可以专注于构造合适的优化问题来解决运动规划问题, 而不是求解优化问题本身. 这很适合初学者快速上手相关运动规划算法.

有这样几种常见的QP优化求解器:

• OSQP
• qpOASES
• OOQP

其中OSQP（Operator Splitting Quadratic Program）是一个在运动规划算法中被广泛运用的求解器。它基于交替乘子法（Alternating Direction Method of Multipliers, ADMM）求解QP问题, 我们将基于OSQP求解一个简单的例子.

5.4.4 例子

我们定义了这样一个QP问题, 目标函数是二次函数, 约束条件是3个线性约束:

$$\begin{array}{rl}minimizef(x)=2x_1^2& +x_2^2+x_1{x}_2+x_1+x_2\\ \text{ subject to }1\le x_1& +x_2\le 1\\ 0\le & x_1\le 0.7\\ 0\le & x_2\le 0.7\end{array}$$

设$x=\left[\begin{array}{l}{x}_1\\ x_2\end{array}\right]$, 将问题整理成矩阵的形式:

$$\begin{array}{ll}\mathrm{minimize}& f(x)=\frac{1}{2}{x}^T\left[\begin{array}{ll}4& 1\\ 1& 2\end{array}\right]x+{\left[\begin{array}{l}1\\ 1\end{array}\right]}^{T}x\\ \text{ subject to }& \left[\begin{array}{l}1\\ 0\\ 0\end{array}\right]\le \left[\begin{array}{ll}1& 1\\ 1& 0\\ 0& 1\end{array}\right]x\le \left[\begin{array}{c}1\\ 0.7\\ 0.7\end{array}\right]\end{array}$$

将对应的矩阵整理到代码中, 就可以调用OSQP求解这个QP问题了

import osqp
import numpy as np
from scipy import sparse

# Define problem data
P = sparse.csc_matrix([[4, 1], [1, 2]])
q = np.array([1, 1])
A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]])
l = np.array([1, 0, 0])
u = np.array([1, 0.7, 0.7])

# Create an OSQP object
prob = osqp.OSQP()

# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)

# Solve problem
res = prob.solve()
print(f"optimal x:{res.x}")

python tests/optimization/osqp_test.py

求解结果如下:

status:               solved
number of iterations: 50
optimal objective:    1.8800
run time:             3.42e-05s
optimal rho estimate: 1.36e+00

optimal x:[0.30000019 0.69999981]

---

🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn