站在天堂看地狱，人生就像情景剧；站在地狱看天堂，为谁辛苦为谁忙。 —武林外传 白展堂
🏰代码及环境配置：请参考 环境配置和代码运行!
在运动规划算法中, QP优化是非常常见的优化问题形式, 本节我们将进行介绍.
5.4.1 QP优化定义
二次规划（Quadratic Programming）优化，是指优化问题的目标函数为二次函数, 且约束条件为线性的问题。定义如下:
minimize 1 2 x T P x + q T x subject to l ≤ A x ≤ u \begin{array}{ll}\operatorname{minimize} & \frac{1}{2} x^T P x+q^T x \\\text { subject to } & l \leq A x \leq u\end{array} minimize subject to 21xTPx+qTxl≤Ax≤u
- 决策变量是 x ∈ R n x \in \mathbf{R}^n x∈Rn
- 目标函数: 二次函数, 矩阵 P ∈ R n × n P \in \mathbf{R}^{n \times n} P∈Rn×n并是一个对称矩阵, 向量 q ∈ R n q \in \mathbf{R}^n q∈Rn
- 线性约束: 矩阵 A ∈ R m × n A \in \mathbf{R}^{m \times n} A∈Rm×n, 上下界向量 l , u l,u l,u确定.
5.4.2 QP优化的特点
首先我们介绍一下半正定矩阵定义:
A A A 是一个 n × n n \times n n×n 的实对称矩阵，如果对所有的非零实向量 x ∈ R n x \in \mathbb{R}^n x∈Rn，都有 x T A x ≥ 0 x^T A x \geq 0 xTAx≥0，则称 A A A 为半正定矩阵。特别地，如果 x T A x > 0 x^T A x > 0 xTAx>0 对所有非零 x x x 都成立，则称 A A A 为正定矩阵。
在QP优化问题中, 当 P P 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个线性约束:
m i n i m i z e f ( x ) = 2 x 1 2 + x 2 2 + x 1 x 2 + x 1 + x 2 subject to 1 ≤ x 1 + x 2 ≤ 1 0 ≤ x 1 ≤ 0.7 0 ≤ x 2 ≤ 0.7 \begin{align*} minimize \ f(x)=2x_1^2 &+x_2^2+x_1 x_2 +x_1+x_2 \\ \text { subject to } \ 1\leq x_1&+x_2\leq 1 \\ 0\leq &x_1\leq 0.7 \\ 0\leq &x_2\leq 0.7 \end{align*} minimize f(x)=2x12 subject to 1≤x10≤0≤+x22+x1x2+x1+x2+x2≤1x1≤0.7x2≤0.7
设 x = [ x 1 x 2 ] x=\left[\begin{array}{l}x_1 \\x_2\end{array}\right] x=[x1x2], 将问题整理成矩阵的形式:
minimize f ( x ) = 1 2 x T [ 4 1 1 2 ] x + [ 1 1 ] T x subject to [ 1 0 0 ] ≤ [ 1 1 1 0 0 1 ] x ≤ [ 1 0.7 0.7 ] \begin{array}{ll}\operatorname{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] \leq\left[\begin{array}{ll}1 & 1 \\1 & 0 \\0 & 1\end{array}\right] x \leq\left[\begin{array}{c}1 \\0.7 \\0.7\end{array}\right]}\end{array} minimize subject to f(x)=21xT[4112]x+[11]Tx 100 ≤ 110101 x≤ 10.70.7
将对应的矩阵整理到代码中, 就可以调用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+00optimal x:[0.30000019 0.69999981]
🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn
