【路径跟踪】PIDMPC

路径跟踪（Path Tracking）是指在实际行驶过程中，根据预先规划好的路径进行控制，能够沿着设定的路径行驶。常见的路径跟踪算法包括基于模型的控制方法（如PID控制器）、模型预测控制（Model Predictive Control, MPC）等。

一.PID

PID被广泛用于需要将某一个物理量“保持稳定”的场合（比如维持平衡，稳定温度、转速等），具体如平衡小车、汽车的定速巡航、3D打印机上的温度控制器等。

1.算法原理

PID控制器由比例单元（Proportional）、积分单元（Integral）和微分单元（Derivative）组成。可以透过调整3个单元的增益 $K_{p}$ ， $K_{i}$ 和 $K_{d}$ 来调定其特性。PID控制器主要适用于基本上线性，且动态特性不随时间变化的系统。

离散时间下的PID公式如下所示：

2.实例

import numpy as np
import matplotlib.pyplot as plt


class PID:
    def __init__(self, P=0, I=0, D=0, initValue=0):
        self.Kp = P
        self.Ki = I
        self.Kd = D
        '''
        self.curValue表示现在的值
        self.ErrorSum表示前面所有误差之和
        self.PreError表示前一次的误差
        self.CurError表示现在的误差
        '''
        self.curValue = initValue
        self.ErrorSum = 0.0
        self.PreError = 0.0
        self.CurError = 0.0

    def PID_output(self, Target):
        self.PreError = self.CurError
        self.CurError = Target - self.curValue
        dErr = self.CurError - self.PreError
        self.ErrorSum += self.CurError
        # PID算法公式
        output = self.Kp * self.CurError + self.Ki * self.ErrorSum + self.Kd * dErr
        self.curValue += output
        return self.curValue
def test_PID(P=0, I=0, D=0, initValue=0, len=1, target=0):
    pid = PID(P, I, D, initValue)
    pid_list = []
    time_list = []
    pid_list.append(pid.curValue)
    time_list.append(0)

    for i in range(1, len + 1):
        output = pid.PID_output(target)
        pid_list.append(output)
        time_list.append(i)

    time_list = np.array(time_list)
    pid_list = np.array(pid_list)

    plt.figure()
    plt.style.use('seaborn')
    plt.plot(time_list, pid_list)
    plt.axhline(target, c='green')
    plt.xlim((0, len))
    plt.ylim((min(pid_list) - 1, max(pid_list) + 1))
    plt.xlabel('time(s)')
    plt.ylabel('value')
    plt.title('PID control')
    plt.grid(True)
    plt.show()

if __name__ == '__main__':
    test_PID(P=0.1, I=0.1, D=0.1, initValue=0, len=200, target=100)

二.MPC

模型预测控制（Model Predictive Control,MPC）是1种基于模型的控制方法，通过建立车辆的动力学模型和路面情况等环境信息来预测车辆未来的行驶轨迹，并在保证车辆稳定性的前提下寻找最优控制策略。相较于传统的控制方法，MPC可以根据车辆实时状态和环境信息调整控制策略，从而实现更加智能化和精准化的路径跟踪。

1.算法原理

(1)问题定义

假设有1个离散时间线性系统，用以下状态空间方程描述：

其中, $x(k)$ 是系统状态， $u(k)$ 是控制输入，A 和 B 是系统矩阵。目标是通过控制 $u(k)$ ，使系统状态 $x(k)$ 尽可能接近期望状态 $x(ref)$ 。

(2)代价函数

代价函数及约束条件如下：

其中， $Q,R,Q_{f}$ 分别表示系统状态、系统输入和终端状态的代价矩阵。

(3)求解

将MPC问题转换为QP问题。

其中， $n_{x},n_{u}$ 分别表示系统状态和控制输入的维度。

MPC的代价函数可以写为：

$X(t)$ 和 $U(t)$ 之间的关系推导：

矩阵形式为：

令：

则：

代入代价函数可得：

对比标准QP形式：

令：

已经将MPC问题转换为QP问题，后面只需要调用QP求解器进行优化即可。

确定矩阵后，优化输入为当前t时刻的系统状态 $x(t)$ ，优化输出为控制序列 $u(t)$ ，由于理论构建的模型与系统真实模型存在偏差，优化所得的未来控制量对系统控制的价值很低，因此MPC仅执行输出序列 $u(t)$ 中的第1个控制输出。

2.实例

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