路徑跟蹤（Path Tracking）是指在實(shí)際行駛過程中，根據(jù)預(yù)先規(guī)劃好的路徑進(jìn)行控制，能夠沿著設(shè)定的路徑行駛。常見的路徑跟蹤算法包括基于模型的控制方法（如PID控制器）、模型預(yù)測控制（Model Predictive Control, MPC）等。

一.PID

PID被廣泛用于需要將某一個(gè)物理量“保持穩(wěn)定”的場合（比如維持平衡，穩(wěn)定溫度、轉(zhuǎn)速等），具體如平衡小車、汽車的定速巡航、3D打印機(jī)上的溫度控制器等。

1.算法原理

PID控制器由比例單元（Proportional）、積分單元（Integral）和微分單元（Derivative）組成?？梢酝高^調(diào)整3個(gè)單元的增益??，?和??來調(diào)定其特性。PID控制器主要適用于基本上線性，且動(dòng)態(tài)特性不隨時(shí)間變化的系統(tǒng)。

離散時(shí)間下的PID公式如下所示：

?

2.實(shí)例

import numpy as np
import matplotlib.pyplot as pltclass PID:def __init__(self, P=0, I=0, D=0, initValue=0):self.Kp = Pself.Ki = Iself.Kd = D'''self.curValue表示現(xiàn)在的值self.ErrorSum表示前面所有誤差之和self.PreError表示前一次的誤差self.CurError表示現(xiàn)在的誤差'''self.curValue = initValueself.ErrorSum = 0.0self.PreError = 0.0self.CurError = 0.0def PID_output(self, Target):self.PreError = self.CurErrorself.CurError = Target - self.curValuedErr = self.CurError - self.PreErrorself.ErrorSum += self.CurError# PID算法公式output = self.Kp * self.CurError + self.Ki * self.ErrorSum + self.Kd * dErrself.curValue += outputreturn 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

模型預(yù)測控制（Model Predictive Control,MPC）是1種基于模型的控制方法，通過建立車輛的動(dòng)力學(xué)模型和路面情況等環(huán)境信息來預(yù)測車輛未來的行駛軌跡，并在保證車輛穩(wěn)定性的前提下尋找最優(yōu)控制策略。相較于傳統(tǒng)的控制方法，MPC可以根據(jù)車輛實(shí)時(shí)狀態(tài)和環(huán)境信息調(diào)整控制策略，從而實(shí)現(xiàn)更加智能化和精準(zhǔn)化的路徑跟蹤。

1.算法原理

(1)問題定義

假設(shè)有1個(gè)離散時(shí)間線性系統(tǒng)，用以下狀態(tài)空間方程描述：

其中, ?是系統(tǒng)狀態(tài)，是控制輸入，A?和?B?是系統(tǒng)矩陣。目標(biāo)是通過控制，使系統(tǒng)狀態(tài)?盡可能接近期望狀態(tài)?。

(2)代價(jià)函數(shù)

代價(jià)函數(shù)及約束條件如下：

其中，分別表示系統(tǒng)狀態(tài)、系統(tǒng)輸入和終端狀態(tài)的代價(jià)矩陣。

(3)求解

將MPC問題轉(zhuǎn)換為QP問題。

其中，分別表示系統(tǒng)狀態(tài)和控制輸入的維度。

MPC的代價(jià)函數(shù)可以寫為：

和之間的關(guān)系推導(dǎo)：

矩陣形式為：

令：

則：

代入代價(jià)函數(shù)可得：

對(duì)比標(biāo)準(zhǔn)QP形式：

令：

已經(jīng)將MPC問題轉(zhuǎn)換為QP問題，后面只需要調(diào)用QP求解器進(jìn)行優(yōu)化即可。

確定矩陣后，優(yōu)化輸入為當(dāng)前t時(shí)刻的系統(tǒng)狀態(tài)，優(yōu)化輸出為控制序列，由于理論構(gòu)建的模型與系統(tǒng)真實(shí)模型存在偏差，優(yōu)化所得的未來控制量對(duì)系統(tǒng)控制的價(jià)值很低，因此MPC僅執(zhí)行輸出序列中的第1個(gè)控制輸出。

2.實(shí)例

import numpy as np
from scipy import sparse
from qpsolvers import solve_qpclass MPC:def __init__(self, Ad, Bd, Q, R, Qf, N = 10):self.Ad = Adself.Bd = Bdself.Q = Qself.R = Rself.Qf = Qfself.N = N    # 預(yù)測步數(shù)self.nx = Bd.shape[0]self.nu = Bd.shape[1]def solve(self, x0, Ad, Bd, Q, R, Qf, N = 10):self.Ad = Adself.Bd = Bdself.Q = Qself.R = Rself.Qf = Qfself.N = N    # 預(yù)測步數(shù)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.BdC[i * self.nx : (i + 1) * self.nx, j * self.nu : (j + 1) * self.nu] = C_ijelse:C_ij = np.zeros((self.nx, self.nu))C[i * self.nx : (i + 1) * self.nx, j * self.nu : (j + 1) * self.nu] = C_ijM[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:] = QfR_bar = np.kron(np.eye(self.N), R)E = M.T * Q_bar * CP = 2 * C.T * Q_bar * C + R_barq = 2 * E.T * x0# Gx <= hG_ = 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 = bA = None # 等式約束矩陣b = None # 等式約束向量# 轉(zhuǎn)換為稀疏矩陣的形式能加速計(jì)算P = sparse.csc_matrix(P)q = np.asarray(q)if G is None:passelse:G = sparse.csc_matrix(G)if A is None:passelse:A = sparse.csc_matrix(A)res = solve_qp(P, q, G, h, A, b, solver="osqp")return res

三.參考

1.https://blog.csdn.net/TeenLucifer/article/details/139887288?spm=1001.2014.3001.5501

2.https://github.com/TeenLucifer/Vehicle_PnC