python - 使用 scipy optimize 进行最优控制

Question

我正在尝试解决一个最优控制问题，其中成本函数为 J = x^T Q x + u^T R u ，且服从 x_dot = A x + B u 以及 x 和 u 的界限。我知道有一些解算器，如 cvxpy、yalimp 等可以做到这一点，但我想自己做，以获得关于编码和将来可能添加的一些其他参数的更好想法。 我附上我写的代码。它运行但为所有时间步返回相同的值。我将 x 和 u 堆叠为单个向量。我不知道这是否是正确的方法。我认为代码可以用更好/更有效的方式编写。欢迎所有建议，并非常感谢您提前提供的任何帮助

Ember

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

Answer 1

有一个类似的 MPC 应用程序，它使用发布在 process dynamics and control page for Model Predictive Control 上的 Scipy.optimize.minimize (选择显示 Python MPC)。它使用也可以用状态空间形式表示的一阶线性系统。这是动画的屏幕截图:

尽管这是我的类(class)网站，但此应用程序的功劳归 Junho Park 。还有另一个教程linear MPC that uses MATLAB and Python Gekko 。此源代码可能会在您开发应用程序时为您提供帮助。