You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
273 lines
7.5 KiB
Python
273 lines
7.5 KiB
Python
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()
|