From bab97dd35907caf2a16c030116b3d34d90273ed0 Mon Sep 17 00:00:00 2001 From: liu <387636706@qq.com> Date: Mon, 8 May 2023 10:26:32 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=98=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MPC.py | 272 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 272 insertions(+) create mode 100644 MPC.py diff --git a/MPC.py b/MPC.py new file mode 100644 index 0000000..e203bd3 --- /dev/null +++ b/MPC.py @@ -0,0 +1,272 @@ +import time + +import matplotlib.pyplot as plt +import numpy as np +from math import * +from cvxopt import matrix, solvers + + +class MPC: + def __init__(self): + self.Np = 60 # 预测步长 + self.Nc = 30 # 控制步长 + + self.dt = 0.1 # 时间间隔 + self.Length = 1.0 # 车辆轴距 + + max_steer = 30 * pi / 180 # 最大方向盘打角 + max_steer_v = 15 * pi / 180 # 最大方向盘打角速度 + max_v = 8.7 # 最大车速 + max_a = 1.0 # 最大加速度 + + # 目标函数相关矩阵 + self.Q =150* np.identity(3 * self.Np) # 位姿权重 100为权重 + print('S',self.Q) + self.R = 100 * np.identity(2 * self.Nc) # 控制权重 + print('r',self.R) + + self.kesi = np.zeros((5, 1)) + print('kesi',self.kesi) + + self.A = np.identity(5) + print('A',self.A) + + self.B = np.block([ + [np.zeros((3, 2))], + [np.identity(2)] + ]) + print('B',self.B) + + self.C = np.block([ + [np.identity(3), np.zeros((3, 2))] + ]) + print(self.C) + self.PHI = np.zeros((3 * self.Np, 5)) + self.THETA = np.zeros((3 * self.Np, 2 * self.Nc)) + + self.CA = (self.Np + 1) * [self.C] + # print(self.CA) + self.H = np.zeros((2 * self.Nc, 2 * self.Nc)) + + self.f = np.zeros((2 * self.Nc, 1)) + + # 不等式约束相关矩阵 + A_t = np.zeros((self.Nc, self.Nc)) + for p in range(self.Nc): + # print('p',p) + for q in range(p + 1): + # print('q', q) + + A_t[p][q] = 1 + + + A_I = np.kron(A_t, np.identity(2)) + # print(A_I) + # 控制量约束 + umin = np.array([[-max_v], [-max_steer]]) + print(umin) + umax = np.array([[max_v], [max_steer]]) + self.Umin = np.kron(np.ones((self.Nc, 1)), umin) + self.Umax = np.kron(np.ones((self.Nc, 1)), umax) + + # 控制增量约束 + delta_umin = np.array([[-max_a * self.dt], [-max_steer_v * self.dt]]) + delta_umax = np.array([[max_a * self.dt], [max_steer_v * self.dt]]) + delta_Umin = np.kron(np.ones((self.Nc, 1)), delta_umin) + delta_Umax = np.kron(np.ones((self.Nc, 1)), delta_umax) + + self.A_cons = np.zeros((2 * 2 * self.Nc, 2 * self.Nc)) + self.A_cons[0:2 * self.Nc, 0:2 * self.Nc] = A_I + self.A_cons[2 * self.Nc:4 * self.Nc, 0:2 * self.Nc] = np.identity(2 * self.Nc) + + self.lb_cons = np.zeros((2 * 2 * self.Nc, 1)) + self.lb_cons[2 * self.Nc:4 * self.Nc, 0:1] = delta_Umin + + self.ub_cons = np.zeros((2 * 2 * self.Nc, 1)) + self.ub_cons[2 * self.Nc:4 * self.Nc, 0:1] = delta_Umax + + def mpcControl(self, x, y, yaw, v, angle, tar_x, tar_y, tar_yaw, tar_v, tar_angle): # mpc优化控制 + T = self.dt + L = self.Length + + # 更新误差 + self.kesi[0][0] = x - tar_x + print('1,0',self.kesi[0][0]) + self.kesi[1][0] = y - tar_y + self.kesi[2][0] = self.normalizeTheta(yaw - tar_yaw) + self.kesi[3][0] = v - tar_v + self.kesi[4][0] = angle - tar_angle + + # 更新A矩阵 + self.A[0][2] = -tar_v * sin(tar_yaw) * T + self.A[0][3] = cos(tar_yaw) * T + self.A[1][2] = tar_v * cos(tar_yaw) * T + self.A[1][3] = sin(tar_yaw) * T + self.A[2][3] = tan(tar_angle) * T / L + self.A[2][4] = tar_v * T / (L * (cos(tar_angle) ** 2)) + + # 更新B矩阵 + self.B[0][0] = cos(tar_yaw) * T + self.B[1][0] = sin(tar_yaw) * T + self.B[2][0] = tan(tar_angle) * T / L + self.B[2][1] = tar_v * T / (L * (cos(tar_angle) ** 2)) + + # 更新CA + for i in range(1, self.Np + 1): + self.CA[i] = np.dot(self.CA[i - 1], self.A) + + # 更新PHI和THETA + for j in range(self.Np): + self.PHI[3 * j:3 * (j + 1), 0:5] = self.CA[j + 1] + for k in range(min(self.Nc, j + 1)): + self.THETA[3 * j:3 * (j + 1), 2 * k: 2 * (k + 1) + ] = np.dot(self.CA[j - k], self.B) + + # 更新H + self.H = np.dot(np.dot(self.THETA.transpose(), self.Q), + self.THETA) + self.R + + # 更新f + self.f = 2 * np.dot(np.dot(self.THETA.transpose(), self.Q), + np.dot(self.PHI, self.kesi)) + + # 更新约束 + Ut = np.kron(np.ones((self.Nc, 1)), np.array([[v], [angle]])) + self.lb_cons[0:2 * self.Nc, 0:1] = self.Umin - Ut + self.ub_cons[0:2 * self.Nc, 0:1] = self.Umax - Ut + + # 求解QP + P = matrix(self.H) + q = matrix(self.f) + G = matrix(np.block([ + [self.A_cons], + [-self.A_cons] + ])) + h = matrix(np.block([ + [self.ub_cons], + [-self.lb_cons] + ])) + + solvers.options['show_progress'] = False + sol = solvers.qp(P, q, G, h) + X = sol['x'] + + # 输出结果 + v += X[0] + angle += X[1] + + return v, angle + + def normalizeTheta(self, angle): # 角度归一化 + while (angle >= pi): + angle -= 2 * pi + + while (angle < -pi): + angle += 2 * pi + + return angle + + def findIdx(self, x, y, cx, cy): # 寻找欧式距离最近的点 + min_dis = float('inf') + idx = 0 + + for i in range(len(cx)): + dx = x - cx[i] + dy = y - cy[i] + dis = dx ** 2 + dy ** 2 + if (dis < min_dis): + min_dis = dis + idx = i + + return idx + + def update(self, x, y, yaw, v, angle): # 模拟车辆位置 + x += v * cos(yaw) * self.dt + y += v * sin(yaw) * self.dt + yaw += v / self.Length * tan(angle) * self.dt + + return x, y, yaw + + +if __name__ == '__main__': + cx = np.linspace(0, 200, 2000) + cy = np.zeros(len(cx)) + dx = np.zeros(len(cx)) + ddx = np.zeros(len(cy)) + cyaw = np.zeros(len(cx)) + ck = np.zeros(len(cx)) + + for i in range(len(cx)): + cy[i] = cos(cx[i] / 10) * cx[i] / 10 + + # 计算一阶导数 + for i in range(len(cx) - 1): + dx[i] = (cy[i + 1] - cy[i]) / (cx[i + 1] - cx[i]) + dx[len(cx) - 1] = dx[len(cx) - 2] + + # 计算二阶导数 + for i in range(len(cx) - 2): + ddx[i] = (cy[i + 2] - 2 * cy[i + 1] + cy[i]) / (0.5 * (cx[i + 2] - cx[i])) ** 2 + ddx[len(cx) - 2] = ddx[len(cx) - 3] + ddx[len(cx) - 1] = ddx[len(cx) - 2] + + # 计算偏航角 + for i in range(len(cx)): + cyaw[i] = atan(dx[i]) + + # 计算曲率 + for i in range(len(cx)): + ck[i] = ddx[i] / (1 + dx[i] ** 2) ** 1.5 + + # 初始状态 + x = 0.0 + y = 5.0 + yaw = 0.0 + v = 0.0 + angle = 0.0 + t = 0 + + # 历史状态 + xs = [x] + ys = [y] + vs = [v] + angles = [angle] + ts = [t] + + # 实例化 + mpc = MPC() + + while (1): + idx = mpc.findIdx(x, y, cx, cy) + if (idx == len(cx) - 1): + break + + tar_v = 80 / 3.6 + tar_angle = atan(mpc.Length * ck[idx]) + + (v, angle) = mpc.mpcControl(x, y, yaw, v, angle, + cx[idx], cy[idx], cyaw[idx], tar_v, tar_angle) + + (x, y, yaw) = mpc.update(x, y, yaw, v, angle) + + # 保存状态 + xs.append(x) + ys.append(y) + vs.append(v) + angles.append(angle) + t = t + 0.1 + ts.append(t) + + # 显示 + plt.plot(cx, cy) + plt.scatter(xs, ys, c='r', marker='*') + plt.pause(0.01) # 暂停0.01秒 + plt.clf() + + + # plt.close() + # plt.subplot(2, 1, 1) + # plt.plot(ts, vs) + # plt.subplot(2, 1, 2) + # plt.plot(ts, angles) + # plt.show()