MPC轨迹跟踪|python
MPC轨迹跟踪 | python实现
基本概念
MPC问题推导及转换为QP问题的过程可以看这篇博客，这里直接给出结果。
\begin{aligned} &J = \frac{1}{2}U(t)^T P U(t) + q^T U(t)\\ &\begin{aligned} s.t. \quad G U(t) \leq h \end{aligned}\\ &U(t) = [u_0^T, u_1^T, ..., u_{N-1}^T]^T_{(N \times n_u, 1)}\\ &P = 2C^T\bar{Q}C + \bar{R}\\ &q = 2C^T\bar{Q}^TMx_0 \end{aligned}
M = \begin{bmatrix} I\\ A\\ A^2\\ \vdots\\ A^N \end{bmatrix}_{(N \times n_x, nx)}, C = \begin{bmatrix} 0 & 0 & 0 & \cdots & 0 \\ B & 0 & 0 & \cdots & 0 \\ AB & B & Q & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^{N-2}B & A^{N-3} & \cdots & B \\ \end{bmatrix}_{((N+1 \times n_x), (N \times n_u))}
\bar{Q} = \begin{bmatrix} Q & 0 & 0 & \cdots & 0 \\ 0 & Q & 0 & \cdots & 0 \\ 0 & 0 & Q & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & Q_f \\ \end{bmatrix}_{((N+1) \times n_x, (N+1) \times n_x)}, \bar{R} = \begin{bmatrix} R & 0 & 0 & \cdots & 0 \\ 0 & R & 0 & \cdots & 0 \\ 0 & 0 & R & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & R \\ \end{bmatrix}_{(N \times n_u, N \times n_u)}
式中，A, B表示系统矩阵；Q, R, Q_f分别表示系统状态、系统输入和终端状态的代价矩阵，自行设置；n_x, n_u分别表示系统状态和控制输入的维度。
MPC问题的输入为当前系统状态x_0，输出为最优的控制序列U(t)，执行时仅适用输出序列U(t)中的第一个控制输出 u_0。
求解步骤
- 建立MPC问题，包括目标函数和约束
- 将MPC问题转换为QP问题
- 调用QP求解器进行优化求解得到控制序列
- 取控制序列中的第一个控制输入执行
python实现轨迹跟踪
实现效果
图1 MPC轨迹跟踪效果示意
这里用MPC控制以后轴中心为车辆原点的模型进行轨迹跟踪，用这个模型是因为方便线性化进行控制，该模型的线性化推导过程可以参考这篇博客。
MPC求解
MPC轨迹跟踪演示的完整代码在github仓库，这里仅给出MPC求解部分，代码里用了qpsolvers进行求解，后台调用的是osqp。
import math
import osqp
import numpy as np
from scipy import sparse
from qpsolvers import solve_qp
class MPC:
def __init__(self, Ad, Bd, Q, R, Qf, N = 10):
self.Ad = Ad
self.Bd = Bd
self.Q = Q
self.R = R
self.Qf = Qf
self.N = N # 预测步数
self.nx = Bd.shape[0]
self.nu = Bd.shape[1]
def solve(self, x0, Ad, Bd, Q, R, Qf, N = 10):
self.Ad = Ad
self.Bd = Bd
self.Q = Q
self.R = R
self.Qf = Qf
self.N = N # 预测步数
self.nx = Bd.shape[0]
self.nu = Bd.shape[1]
A_powers = []
for i in range(self.N + 1):
A_powers.append(np.linalg.matrix_power(Ad, i))
C = np.zeros(((self.N + 1) * self.nx, self.N * self.nu))
M = np.zeros(((self.N + 1) * self.nx, self.nx))
for i in range(self.N + 1):
for j in range(self.N):
if i - j - 1 >= 0:
C_ij = A_powers[i - j - 1] * self.Bd
C[i * self.nx : (i + 1) * self.nx, j * self.nu : (j + 1) * self.nu] = C_ij
else:
C_ij = np.zeros((self.nx, self.nu))
C[i * self.nx : (i + 1) * self.nx, j * self.nu : (j + 1) * self.nu] = C_ij
M[i * self.nx : (i + 1) * self.nx, :] = A_powers[i]
Q_bar = np.kron(np.eye(self.N + 1), Q)
Q_bar[self.N * self.nx : (1 + self.N) * self.nx, self.N * self.nx : (1 + self.N) * self.nx:] = Qf
R_bar = np.kron(np.eye(self.N), R)
E = M.T * Q_bar * C
P = 2 * C.T * Q_bar * C + R_bar
q = 2 * E.T * x0
# Gx <= h
G_ = np.eye(self.N * self.nu)
G = np.block([ # 不等式约束矩阵
[G_, np.zeros_like(G_)],
[np.zeros_like(G_), -G_]
])
h = np.vstack(np.ones((2 * self.N * self.nu, 1)) * 999) # 不等式约束向量
# Ax = b
A = None # 等式约束矩阵
b = None # 等式约束向量
# 转换为稀疏矩阵的形式能加速计算
P = sparse.csc_matrix(P)
q = np.asarray(q)
if G is None:
pass
else:
G = sparse.csc_matrix(G)
if A is None:
pass
else:
A = sparse.csc_matrix(A)
res = solve_qp(P, q, G, h, A, b, solver="osqp")
return res
python
