Question

我正在尝试解决一个最优控制问题，其中成本函数为J = x ^ T Q x + u ^ T R u，这取决于x_dot = A x + B u以及x和u的边界。我知道有一些求解器（例如cvxpy，yalimp等）可以做到这一点，但我想自己做一下，以便对编码有更好的了解，并可能在将来添加一些其他参数。我正在附上我编写的代码。它运行，但在所有时间步长中返回相同的值。我将x和u堆叠为一个向量。我不知道这是否是正确的方法。我认为代码可以用更好/有效的方式编写。我们欢迎所有建议，并感谢您预先提供的帮助

灰

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

我注意到的一些要点：

1）为什么要使用sympy矩阵？在您看来，numpy.matrix或numpy.array就足够了。当您要使用符号表达式（例如符号微分）时，通常使用sympy。 注意：对矩阵乘法使用@运算符，例如在cost_gradient中，您将使用temp1 = self.Q @ pos_error

2）您可以使用staticmethod decorator将类方法表示为静态方法

3）为什么使用优化方法SLSQP？它用于约束优化（不适用于您的问题）。如果我们将the notes用于scipy.optimize.minimize，我们可以看到有很多算法用于无约束优化（有界），收敛速度可能更快。在大多数情况下，您只需不指定任何内容就可以自动确定算法。

4）将绘图命令放在main方法中，而不是放在外面

否则，看起来不错。我还没有测试功能。

Answer 2

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

即使这是我的课程网站，该应用程序的功劳也归功于Junho Park。 linear MPC that uses MATLAB and Python Gekko还有另一本教程。在开发应用程序时，此源代码可以为您提供帮助。

使用scipy最优化控制

2 个答案: