MPC轨迹跟踪|python

TeenLucifer

于 2024-06-22 19:59:59 发布

阅读量1k

点赞数 23

文章标签： python 概率论开发语言算法自动驾驶

本文链接：https://blog.csdn.net/TeenLucifer/article/details/139887344

版权

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}$

$\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