Я пытаюсь решить задачу оптимального управления, где функция стоимости равна J = x ^ T Q x + u ^ T R u с учетом x_dot = A x + B u и границ для x и u. Я знаю, что есть некоторые решатели, такие как cvxpy, yalimp и т. Д., Которые могли бы сделать это, но я хотел бы сделать это сам, чтобы получить лучшее представление о кодировании и возможном добавлении некоторых других параметров в будущем.
Я прилагаю код, который я написал. Он запускается, но возвращает одно и то же значение для всех временных шагов. Я сложил x и u как один вектор. Я не знаю, если это правильный способ сделать это. Я думаю, что код может быть написан лучше / эффективнее. Все предложения приветствуются и очень благодарны за любую помощь заранее
Ash
import numpy as np
import sympy as sp
import scipy.optimize as opt
import matplotlib.pyplot as plt
# Optimal Control Problem
# Cost, J = x.transpose() * Q * x + u.transpose() * R * u
# x_dot = A*x + B*u
# x_min < x < x_max
# u_min < x < u_max
class mpc_opt():
def __init__(self):
self.Q = sp.diag(0.5, 1, 0) # state penalty matrix, Q
self.R = sp.eye(2) # input penalty matrix
self.A = sp.Matrix([[-0.79, -0.3, -0.1],[0.5, 0.82, 1.23], [0.52, -0.3, -0.5]]) # state matrix
self.B = sp.Matrix([[-2.04, -0.21], [-1.28, 2.75], [0.29, -1.41]]) # input matrix
self.t = np.linspace(0, 1, 30)
# reference trajectory ## static!!!
def ref_trajectory(self, i): # y = 3*sin(2*pi*omega*t)
# y = 3 * np.sin(2*np.pi*self.omega*self.t[i])
x_ref = sp.Matrix([0, 1, 0])
return x_ref
# return sp.Matrix(([[self.t[i]], [y], [0]]))
def cost_function(self, U, *args):
t = args
nx, nu = self.A.shape[-1], self.B.shape[-1]
x0 = U[0:nx]
u = U[nx:nx+nu]
u = u.reshape(len(u), -1)
x0 = x0.reshape(len(x0), -1)
x1 = self.A * x0 + self.B * u
# q = [x1[0], x1[1]]
# pos = self.end_effec_pose(q)
traj_ref = self.ref_trajectory(t)
pos_error = x1 - traj_ref
cost = pos_error.transpose() * self.Q * pos_error + u.transpose() * self.R * u
return cost
def cost_gradient(self, U, *args):
t = args
nx, nu = self.A.shape[-1], self.B.shape[-1]
x0 = U[0:nx]
u = U[nx:nx + nu]
u = u.reshape(len(u), -1)
x0 = x0.reshape(len(x0), -1)
x1 = self.A * x0 + self.B * u
traj_ref = self.ref_trajectory(t)
pos_error = x1 - traj_ref
temp1 = self.Q * pos_error
cost_gradient = temp1.col_join(self.R * u)
return cost_gradient
def optimise(self, u0, t):
umin = [-2., -3.]
umax = [2., 3.]
xmin = [-10., -9., -8.]
xmax = [10., 9., 8.]
bounds = ((xmin[0], xmax[0]), (xmin[1], xmax[1]), (xmin[2], xmax[2]), (umin[0], umax[0]), (umin[1], umax[1]))
U = opt.minimize(self.cost_function, u0, args=(t), method='SLSQP', bounds=bounds, jac=self.cost_gradient,
options={'maxiter': 200, 'disp': True})
U = U.x
return U
if __name__ == '__main__':
mpc = mpc_opt()
x0, u0, = sp.Matrix([[0.1], [0.02], [0.05]]), sp.Matrix([[0.4], [0.2]])
X, U = sp.zeros(len(x0), len(mpc.t)), sp.zeros(len(u0), len(mpc.t))
U0 = sp.Matrix([x0, u0])
nx, nu = mpc.A.shape[-1], mpc.B.shape[-1]
for i in range(len(mpc.t)):
print('i = :', i)
result = mpc.optimise(U0, i)
x0 = result[0:nx]
u = result[nx:nx + nu]
u = u.reshape(len(u), -1)
x0 = x0.reshape(len(x0), -1)
U[:, i], X[:, i] = u0, x0
# x0 = mpc.A * x0 + mpc.B * u
U0 = result
plt.plot(X[0, :], '--r')
plt.plot(X[1, :], '--b')
plt.plot(X[2, :], '*r')
plt.show()