From 54320b3ab499cf070aeed48cc8376acee8783e5a Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 26 Dec 2018 22:08:15 +0900 Subject: [PATCH] add mpc --- mpc/main_example.py | 73 ++++++++++++------ mpc/mpc_func.py | 184 ++++++++++++++++++++++++++++++++++++-------- 2 files changed, 202 insertions(+), 55 deletions(-) diff --git a/mpc/main_example.py b/mpc/main_example.py index fcf66f7..f594ba7 100644 --- a/mpc/main_example.py +++ b/mpc/main_example.py @@ -72,10 +72,10 @@ class FirstOrderSystem(): k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) - # self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() + self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() # for oylar - self.xs += k0.flatten() + # self.xs += k0.flatten() # print("xs = {0}".format(self.xs)) # a = input() @@ -85,8 +85,8 @@ class FirstOrderSystem(): # print(self.history_xs) def main(): - dt = 0.01 - simulation_time = 300 # in seconds + dt = 0.05 + simulation_time = 50 # in seconds iteration_num = int(simulation_time / dt) # you must be care about this matrix @@ -105,7 +105,7 @@ def main(): D = np.zeros((4, 2)) # make simulator with coninuous matrix - init_xs = np.array([0., 0., -3000., 50.]) + init_xs = np.array([0., 0., 0., 0.]) plant = FirstOrderSystem(A, B, C, init_states=init_xs) # create system @@ -118,38 +118,63 @@ def main(): # evaluation function weight Q = np.diag([1., 1., 1., 1.]) - R = np.diag([100., 100.]) - pre_step = 3 + R = np.diag([0.1, 0.1]) + pre_step = 5 # make controller with discreted matrix - controller = MpcController(Ad, Bd, Q, R, pre_step) + controller = MpcController(Ad, Bd, Q, R, pre_step, + dt_input_upper=np.array([0.5 * dt, 0.5 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), + input_upper=np.array([2. ,5.]), input_lower=np.array([-2., -5.])) + controller.initialize_controller() for i in range(iteration_num): print("simulation time = {0}".format(i)) - reference = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]) - controller.calc_input(plant.xs, reference) - + reference = np.array([[0., 0., -10., -5.] for _ in range(pre_step)]).flatten() states = plant.xs opt_u = controller.calc_input(states, reference) plant.update_state(opt_u) history_states = np.array(plant.history_xs) - print(history_states[:, 2]) + time_history_fig = plt.figure() + x_fig = time_history_fig.add_subplot(411) + y_fig = time_history_fig.add_subplot(412) + v_x_fig = time_history_fig.add_subplot(413) + v_y_fig = time_history_fig.add_subplot(414) - plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0]) - plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1]) - plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2], linestyle="dashed") - plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3]) + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0]) + v_x_fig.set_xlabel("time [s]") + v_x_fig.set_ylabel("v_x") + + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1]) + v_y_fig.set_xlabel("time [s]") + v_y_fig.set_ylabel("v_y") + + x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2]) + x_fig.set_xlabel("time [s]") + x_fig.set_ylabel("x") + + y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3]) + y_fig.set_xlabel("time [s]") + y_fig.set_ylabel("y") + time_history_fig.tight_layout() + plt.show() + + history_us = np.array(controller.history_us) + input_history_fig = plt.figure() + u_1_fig = input_history_fig.add_subplot(211) + u_2_fig = input_history_fig.add_subplot(212) + + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 0]) + u_1_fig.set_xlabel("time [s]") + u_1_fig.set_ylabel("u_x") + + u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 1]) + u_2_fig.set_xlabel("time [s]") + u_2_fig.set_ylabel("u_y") + input_history_fig.tight_layout() plt.show() if __name__ == "__main__": - main() - - - - - - - + main() \ No newline at end of file diff --git a/mpc/mpc_func.py b/mpc/mpc_func.py index 3b4ddc8..ae45087 100644 --- a/mpc/mpc_func.py +++ b/mpc/mpc_func.py @@ -9,11 +9,46 @@ class MpcController(): """ Attributes ------------ - + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight + R : numpy.ndarray + evaluation function weight + pre_step : int + prediction step + dt_input_upper : numpy.ndarray + constraints of input dt + dt_input_lower : numpy.ndarray + constraints of input dt + input_upper : numpy.ndarray + constraints of input + input_lower : numpy.ndarray + constraints of input + history_ """ - def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): """ + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight + R : numpy.ndarray + evaluation function weight + pre_step : int + prediction step + dt_input_upper : numpy.ndarray + constraints of input dt + dt_input_lower : numpy.ndarray + constraints of input dt + input_upper : numpy.ndarray + constraints of input + input_lower : numpy.ndarray + constraints of input """ self.A = np.array(A) self.B = np.array(B) @@ -34,22 +69,19 @@ class MpcController(): self.history_us = [initial_input] # constraints - if dt_input_lower in not None: - self.dt_input_lower = dt_input_lower + self.dt_input_lower = dt_input_lower + self.dt_input_upper = dt_input_upper + self.input_upper = input_upper + self.input_lower = input_lower - if dt_input_upper in not None: - self.dt_input_upper = dt_input_upper - - if input_upper in not None: - self.input_upper = input_upper - - if input_lower in not None: - self.input_lower = input_lower + self.W = None + self.omega = None + self.F = None + self.f = None def initialize_controller(self): """ - make matrix to calculate optimal controller - + make matrix to calculate optimal control input """ A_factorials = [self.A] @@ -90,21 +122,69 @@ class MpcController(): self.Qs = np.diag(diag_Qs.flatten()) self.Rs = np.diag(diag_Rs.flatten()) - print("Qs = {0}".format(self.Qs)) - print("Rs = {0}".format(self.Rs)) + print("Qs = \n{0}".format(self.Qs)) + print("Rs = \n{0}".format(self.Rs)) # constraints - # dt U - F = np.array([[], [], []]) + # about dt U + if self.input_lower is not None: + # initialize + self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size)) + for i in range(self.input_size): + self.F[i * 2: (i + 1) * 2, i] = np.array([1., -1.]) + temp_F = copy.deepcopy(self.F) - # u + print("F = \n{0}".format(self.F)) + for i in range(self.pre_step - 1): + temp_F = copy.deepcopy(temp_F) - # state + for j in range(self.input_size): + temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) + + self.F = np.vstack((self.F, temp_F)) + self.F1 = self.F[:, :self.input_size] + + temp_f = [] + + for i in range(self.input_size): + temp_f.append(-1 * self.input_upper[i]) + temp_f.append(self.input_lower[i]) + + self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten() + + print("F = \n{0}".format(self.F)) + print("F1 = \n{0}".format(self.F1)) + print("f = \n{0}".format(self.f)) + + # about dt_u + if self.dt_input_lower is not None: + self.W = np.zeros((2, self.pre_step * self.input_size)) + self.W[:, 0] = np.array([1., -1.]) + + for i in range(self.pre_step * self.input_size - 1): + temp_W = np.zeros((2, self.pre_step * self.input_size)) + temp_W[:, i+1] = np.array([1., -1.]) + self.W = np.vstack((self.W, temp_W)) + + temp_omega = [] + + for i in range(self.input_size): + temp_omega.append(self.dt_input_upper[i]) + temp_omega.append(-1. * self.dt_input_lower[i]) + + self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten() + + print("W = \n{0}".format(self.W)) + print("omega = \n{0}".format(self.omega)) + + # about state + print("check the matrix!! if you think rite, plese push enter") + input() def calc_input(self, states, references): - """ + """calculate optimal input Parameters ----------- states : numpy.array @@ -114,8 +194,8 @@ class MpcController(): References ------------ - - + opt_input : numpy.ndarray + optimal input, size is (1, input_length) """ temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1)) temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1)) @@ -126,25 +206,67 @@ class MpcController(): H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs + # constraints + A = [] + b = [] + + if self.W is not None: + A.append(self.W) + b.append(self.omega.reshape(-1, 1)) + + if self.F is not None: + b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1) + A.append(self.F) + b.append(b_F) + + A = np.array(A).reshape(-1, self.input_size * self.pre_step) + b = np.array(b).reshape(-1, 1) + def optimized_func(dt_us): """ """ - return np.dot(dt_us.flatten(), np.dot(H, dt_us)) - np.dot(G.T, dt_us) + temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)]) - def constraint_func(): + return (np.dot(temp_dt_us, np.dot(H, temp_dt_us.reshape(-1, 1))) - np.dot(G.T, temp_dt_us.reshape(-1, 1)))[0] + + def constraint_func(dt_us): + """ we consider the constraints in Ax <= b + however the scipy constraints is -(Ax - b) >= 0 + so we should recalculate the A and B """ - """ - return None + temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)]) + + constraint = [] + for i in range(self.pre_step * self.input_size): + sums = -1. * (np.dot(A[i], temp_dt_us) - b[i])[0] + constraint.append(sums) - init_dt_us = np.zeros((self.input_size * self.pre_step, 1)) + return np.array(sums) - opt_result = minimize(optimized_func, init_dt_us) + init_dt_us = np.zeros(self.input_size * self.pre_step) + # constraint + if self.W is not None or self.F is not None : + print("consider constraint!") + cons = ({'type' : 'ineq', 'fun' : constraint_func}) + opt_result = minimize(optimized_func, init_dt_us, constraints=cons) + opt_dt_us = opt_result.x - + print("opt_u = {0}".format(np.round(opt_dt_us, 5))) opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] # save self.history_us.append(opt_u) + - return opt_u \ No newline at end of file + return opt_u + + + + +""" +constraint = [] +for i in range(self.pre_step * self.input_size): + sums = -1. * (np.dot(A[i], init_dt_us) - b[i])[0] + constraint.append(sums) +""" \ No newline at end of file