add constraints of mpc.py
This commit is contained in:
Shunichi09
parent
36801f9fb6
commit
bc9a2ebe9f
Binary file not shown.
|
|
@ -1,6 +1,7 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
import copy
|
||||
|
||||
from mpc_func import MpcController
|
||||
from control import matlab
|
||||
|
|
@ -39,9 +40,6 @@ class FirstOrderSystem():
|
|||
time history of system states
|
||||
"""
|
||||
|
||||
if init_states is not None:
|
||||
self.states = init_states
|
||||
|
||||
self.A = A
|
||||
self.B = B
|
||||
self.C = C
|
||||
|
|
@ -51,9 +49,12 @@ class FirstOrderSystem():
|
|||
|
||||
self.xs = np.zeros(self.A.shape[0])
|
||||
|
||||
if init_states is not None:
|
||||
self.xs = copy.deepcopy(init_states)
|
||||
|
||||
self.history_xs = [init_states]
|
||||
|
||||
def update_state(self, us, dt=0.01):
|
||||
def update_state(self, u, dt=0.01):
|
||||
"""calculating input
|
||||
Parameters
|
||||
------------
|
||||
|
|
@ -62,34 +63,39 @@ class FirstOrderSystem():
|
|||
dt : float in seconds, optional
|
||||
sampling time of simulation, default is 0.01 [s]
|
||||
"""
|
||||
temp = self.xs.reshape(-1, 1)
|
||||
temp_x = self.xs.reshape(-1, 1)
|
||||
temp_u = u.reshape(-1, 1)
|
||||
|
||||
# solve Runge-Kutta
|
||||
k0 = dt * (np.dot(self.A, temp) + np.dot(self.B, us))
|
||||
k1 = dt * (np.dot(self.A, temp + k0/2.) + np.dot(self.B, us))
|
||||
k2 = dt * (np.dot(self.A, temp + k1/2.) + np.dot(self.B, us))
|
||||
k3 = dt * (np.dot(self.A, temp + k2) + np.dot(self.B, us))
|
||||
k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u))
|
||||
k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u))
|
||||
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.state += k0
|
||||
self.xs += k0.flatten()
|
||||
|
||||
# print("xs = {0}".format(self.xs))
|
||||
# a = input()
|
||||
# save
|
||||
self.history_xs.append(self.xs)
|
||||
save_states = copy.deepcopy(self.xs)
|
||||
self.history_xs.append(save_states)
|
||||
# print(self.history_xs)
|
||||
|
||||
def main():
|
||||
dt = 0.01
|
||||
simulation_time = 100 # in seconds
|
||||
simulation_time = 300 # in seconds
|
||||
iteration_num = int(simulation_time / dt)
|
||||
|
||||
# you must be care about this matrix
|
||||
# these A and B are for continuos system if you want to use discret system matrix please skip this step
|
||||
tau = 0.53
|
||||
A = np.array([[1./tau, 0., 0., 0.],
|
||||
[0., 1./tau, 0., 0.],
|
||||
tau = 0.63
|
||||
A = np.array([[-1./tau, 0., 0., 0.],
|
||||
[0., -1./tau, 0., 0.],
|
||||
[1., 0., 0., 0.],
|
||||
[1., 0., 0., 0.]])
|
||||
[0., 1., 0., 0.]])
|
||||
B = np.array([[1./tau, 0.],
|
||||
[0., 1./tau],
|
||||
[0., 0.],
|
||||
|
|
@ -99,7 +105,8 @@ def main():
|
|||
D = np.zeros((4, 2))
|
||||
|
||||
# make simulator with coninuous matrix
|
||||
plant = FirstOrderSystem(A, B, C)
|
||||
init_xs = np.array([0., 0., -3000., 50.])
|
||||
plant = FirstOrderSystem(A, B, C, init_states=init_xs)
|
||||
|
||||
# create system
|
||||
sysc = matlab.ss(A, B, C, D)
|
||||
|
|
@ -111,20 +118,31 @@ def main():
|
|||
|
||||
# evaluation function weight
|
||||
Q = np.diag([1., 1., 1., 1.])
|
||||
R = np.diag([1., 1.])
|
||||
R = np.diag([100., 100.])
|
||||
pre_step = 3
|
||||
|
||||
# make controller with discreted matrix
|
||||
controller = MpcController(Ad, Bd, Q, R, pre_step)
|
||||
controller.initialize_controller()
|
||||
|
||||
xs = np.array([0., 0., 0., 0.])
|
||||
|
||||
for i in range(iteration_num):
|
||||
controller.calc_input(xs)
|
||||
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)
|
||||
|
||||
states = plant.xs
|
||||
opt_u = controller.calc_input(states, reference)
|
||||
plant.update_state(opt_u)
|
||||
|
||||
# states = plant.states
|
||||
# controller.calc_input
|
||||
history_states = np.array(plant.history_xs)
|
||||
|
||||
print(history_states[:, 2])
|
||||
|
||||
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])
|
||||
plt.show()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ class MpcController():
|
|||
|
||||
"""
|
||||
|
||||
def __init__(self, A, B, Q, R, pre_step, input_upper=None, input_lower=None):
|
||||
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):
|
||||
"""
|
||||
"""
|
||||
self.A = np.array(A)
|
||||
|
|
@ -27,8 +27,25 @@ class MpcController():
|
|||
self.state_size = self.A.shape[0]
|
||||
self.input_size = self.B.shape[1]
|
||||
|
||||
self.history_us = []
|
||||
self.history_us = [np.zeros(self.input_size)]
|
||||
|
||||
# initial state
|
||||
if initial_input is not None:
|
||||
self.history_us = [initial_input]
|
||||
|
||||
# constraints
|
||||
if dt_input_lower in not None:
|
||||
self.dt_input_lower = dt_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
|
||||
|
||||
def initialize_controller(self):
|
||||
"""
|
||||
make matrix to calculate optimal controller
|
||||
|
|
@ -66,6 +83,7 @@ class MpcController():
|
|||
|
||||
print("theta_mat = \n{0}".format(self.theta_mat))
|
||||
|
||||
# evaluation function weight
|
||||
diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)])
|
||||
diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)])
|
||||
|
||||
|
|
@ -75,6 +93,16 @@ class MpcController():
|
|||
print("Qs = {0}".format(self.Qs))
|
||||
print("Rs = {0}".format(self.Rs))
|
||||
|
||||
# constraints
|
||||
# dt U
|
||||
F = np.array([[], [], []])
|
||||
|
||||
# u
|
||||
|
||||
|
||||
# state
|
||||
|
||||
|
||||
def calc_input(self, states, references):
|
||||
"""
|
||||
Parameters
|
||||
|
|
@ -89,10 +117,10 @@ class MpcController():
|
|||
|
||||
|
||||
"""
|
||||
temp_1 = np.dot(self.phi_mat, states)
|
||||
temp_2 = np.dot(self.gamma_mat, self.history_us[-1])
|
||||
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))
|
||||
|
||||
error = references - temp_1 - temp_2
|
||||
error = references.reshape(-1, 1) - temp_1 - temp_2
|
||||
|
||||
G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) )
|
||||
|
||||
|
|
@ -101,35 +129,22 @@ class MpcController():
|
|||
def optimized_func(dt_us):
|
||||
"""
|
||||
"""
|
||||
return np.dot(dt_us.T, np.dot(H, dt_us)) - np.dot(G.T, dt_us)
|
||||
return np.dot(dt_us.flatten(), np.dot(H, dt_us)) - np.dot(G.T, dt_us)
|
||||
|
||||
def constraint_func():
|
||||
"""
|
||||
"""
|
||||
return
|
||||
return None
|
||||
|
||||
init_dt_us = np.zeros(self.pre_step)
|
||||
init_dt_us = np.zeros((self.input_size * self.pre_step, 1))
|
||||
|
||||
opt_result = minimize(optimized_func, init_dt_us)
|
||||
|
||||
opt_dt_us = opt_result
|
||||
opt_dt_us = opt_result.x
|
||||
|
||||
opt_us = opt_dt_us[0] + self.history_us[-1]
|
||||
opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
|
||||
|
||||
# save
|
||||
self.history_us.append(opt_us)
|
||||
return opt_us
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
self.history_us.append(opt_u)
|
||||
|
||||
return opt_u
|
||||
Loading…
Reference in New Issue