【OSQP】MPC代码学习与部署

OSQP官网

prerequisite

1. sparse.csc_matrix 稀疏矩阵

稀疏矩阵有多种存储格式，包括CSR（Compressed Sparse Row）、CSC（Compressed Sparse Column）、COO（Coordinate format）等。CSC格式是一种以列为主的存储格式，对于进行列向量操作或者矩阵与向量乘法等涉及列操作的计算可以达到较高的效率。

• np.array: 会直接在内存中创建一个连续的数据块来存储数组中的所有元素。这意味着如果创建了一个非常大的np.array，即使其中大部分元素都是0，仍然会消耗相应的内存资源。因此，np.array最适合表示元素稠密的数组。

• sparse.csc_matrix: 是SciPy的一种稀疏矩阵存储格式，csc代表列优先（Compressed Sparse Column）。它只存储非零元素和它们的位置信息，对于那些零元素，不会浪费任何存储空间。这使得sparse.csc_matrix在处理大型稀疏矩阵时，尤其是矩阵中大部分元素为零的情况下，具有更好的内存效率。另外，由于其列优先的特性，它在列切片操作（如获取或设置某几列的值）时性能较好。
  – data、row和col都是数组，分别代表非零元素的值、非零元素的行索引和非零元素的列索引
  – shape：矩阵的形状

from scipy.sparse import csc_matrix
data = np.array([1, 2, 3, 4, 5, 6])
row = np.array([0, 2, 2, 0, 1, 2])
col = np.array([0, 0, 1, 2, 2, 2])
X = csc_matrix((data, (row, col)), shape=(3, 3)) #元组形式

👆上述代码产生的矩阵，当然也可以直接将完整的矩阵（二维NumPy数组或者嵌套的Python列表）传递给函数，也是有效的，比如：

Bd = sparse.csc_matrix([
  [0.,      -0.0726,  0.,     0.0726],
  [-0.0726,  0.,      0.0726, 0.    ],
  [-0.0152,  0.0152, -0.0152, 0.0152],
  [-0.,     -0.0006, -0.,     0.0006],
  [0.0006,   0.,     -0.0006, 0.0000],
  [0.0106,   0.0106,  0.0106, 0.0106],
  [0,       -1.4512,  0.,     1.4512],
  [-1.4512,  0.,      1.4512, 0.    ],
  [-0.3049,  0.3049, -0.3049, 0.3049],
  [-0.,     -0.0236,  0.,     0.0236],
  [0.0236,   0.,     -0.0236, 0.    ],
  [0.2107,   0.2107,  0.2107, 0.2107]])

2. sparse.block_diag 生成块矩阵

块对角矩阵是一个特殊的方阵，其主对角线上的元素是矩阵（这些矩阵可以是任何大小和形状），而非对角线上的元素都是零。这些主对角线上的矩阵被称为这个块对角矩阵的"块"。

matrices = [matrix1, matrix2, ..., matrixn]
result = scipy.sparse.block_diag(matrices)

Model

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

x0 = np.zeros(12) # 初值
xr = np.array([0.,0.,1.,0.,0.,0.,0.,0.,0.,0.,0.,0.]) # 期望参考值
N = 10 # Prediction horizon

Constraint 定义可行域

控制约束

• u的最大值最小值约束

u0 = 10.5916
umin = np.array([9.6, 9.6, 9.6, 9.6]) - u0
umax = np.array([13., 13., 13., 13.]) - u0

状态约束

• x的最大值最小值约束
  -np.inf和np.inf代表设置为负无穷到正无穷，表示没有具体的约束。

xmin = np.array([-np.pi/6,-np.pi/6,-np.inf,-np.inf,-np.inf,-1.,
                 -np.inf,-np.inf,-np.inf,-np.inf,-np.inf,-np.inf])
xmax = np.array([ np.pi/6, np.pi/6, np.inf, np.inf, np.inf, np.inf,
                  np.inf, np.inf, np.inf, np.inf, np.inf, np.inf])

Objective function

weight matrix

• 权重矩阵也需要是稀疏的

Q = sparse.diags([0., 0., 10., 10., 10., 10., 0., 0., 0., 5., 5., 5.])# 状态偏差权重矩阵
QN = Q #最后一个step的状态权重矩阵
R = 0.1*sparse.eye(4)#控制输入权重矩阵，值越大表示对这个输入的调整越想要避免

OSQP求解器

参数

• P是QP问题中的二次项的系数矩阵，由状态权重矩阵Q、QN和控制权重矩阵R通过Kronecker积和块对角堆叠得到

• q是QP问题中的线性项的系数矩阵

• Ax和Bu是线性动力学约束的系数矩阵（stack form）

• Aeq，leq，ueq定义了等式约束，主要来自于系统的动力学模型。leq和ueq指定了等式约束的上下界。

• Aineq，lineq，uineq定义了不等式约束，主要是状态和控制输入的界。lineq和uineq指定了不等式约束的上下界。

• A，l，u定义了用于OSQP求解器的约束矩阵和约束界

# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
                       sparse.kron(sparse.eye(N), R)], format='csc')
# - linear objective q = np.hstack([np.kron(np.ones(N), -Q@xr), -QN@xr, np.zeros(N*nu)])
# - linear dynamics 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]) leq = np.hstack([-x0, np.zeros(N*nx)]) ueq = leq
# - input and state constraints Aineq = sparse.eye((N+1)*nx + N*nu) lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)]) uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
# - OSQP constraints A = sparse.vstack([Aeq, Aineq], format='csc') l = np.hstack([leq, lineq]) u = np.hstack([ueq, uineq]) ```

### 实例

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

# Setup workspace
prob.setup(P, q, A, l, u)

闭环仿真

在每一步仿真中，都会求解一个优化问题，然后将优化得到的控制输入应用到系统模型中，以获取下一个时刻的状态，然后根据新的状态更新优化问题，并在下一步仿真中重新求解。

nsim = 15 # 进行15次仿真
for i in range(nsim):
    # Solve
    res = prob.solve()

    # Check solver status 如果没有成功求解抛出异常
    if res.info.status != 'solved':
        raise ValueError('OSQP did not solve the problem!')

    # Apply first control input to the plant 
    ctrl = res.x[-N*nu:-(N-1)*nu] #MPC思想 取优化得到的第一个值应用到系统模型中
    x0 = Ad@x0 + Bd@ctrl #前向forwardstate

    # Update initial state 基于新的状态x0更新约束条件
    l[:nx] = -x0
    u[:nx] = -x0
    prob.update(l=l, u=u)

总结

一个osqp实例需要参数P，q，A，l，u
分别对应目标函数中的二次型矩阵，线性矩阵，关于决策变量的线性约束A，l，u