变更

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()