From c7ffc043d0b3881561af143d7338051ff2e0688c Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sat, 29 Dec 2018 15:49:04 +0900 Subject: [PATCH] add ACC.py of mpc --- mpc/README.md | 110 ------------------ mpc/basic/README.md | 144 ++++++++++++++++++++++++ mpc/{ => basic}/animation.py | 2 +- mpc/{ => basic}/main_ACC.py | 1 - mpc/{ => basic}/main_example.py | 8 +- mpc/{ => basic}/mpc_func_with_cvxopt.py | 0 mpc/{ => basic}/mpc_func_with_scipy.py | 0 mpc/{ => basic}/test_compare_methods.py | 0 8 files changed, 149 insertions(+), 116 deletions(-) delete mode 100644 mpc/README.md create mode 100644 mpc/basic/README.md rename mpc/{ => basic}/animation.py (99%) rename mpc/{ => basic}/main_ACC.py (99%) rename mpc/{ => basic}/main_example.py (97%) rename mpc/{ => basic}/mpc_func_with_cvxopt.py (100%) rename mpc/{ => basic}/mpc_func_with_scipy.py (100%) rename mpc/{ => basic}/test_compare_methods.py (100%) diff --git a/mpc/README.md b/mpc/README.md deleted file mode 100644 index 67d1112..0000000 --- a/mpc/README.md +++ /dev/null @@ -1,110 +0,0 @@ -# Model Predictive Control Tool -This program is about template, generic function of linear model predictive control - -# Documentation of the MPC function -Linear model predicitive control should have state equation. -So if you want to use this function, you should model the plant as state equation. -Therefore, the parameters of this class are as following - -**class MpcController()** - -Attributes : - -- A : numpy.ndarray - - system matrix -- B : numpy.ndarray - - input matrix -- Q : numpy.ndarray - - evaluation function weight for states -- Qs : numpy.ndarray - - concatenated evaluation function weight for states -- R : numpy.ndarray - - evaluation function weight for inputs -- Rs : numpy.ndarray - - concatenated evaluation function weight for inputs -- pre_step : int - - prediction step -- state_size : int - - state size of the plant -- input_size : int - - input size of the plant -- dt_input_upper : numpy.ndarray, shape(input_size, ), optional - - constraints of input dt, default is None -- dt_input_lower : numpy.ndarray, shape(input_size, ), optional - - constraints of input dt, default is None -- input_upper : numpy.ndarray, shape(input_size, ), optional - - constraints of input, default is None -- input_lower : numpy.ndarray, shape(input_size, ), optional - - constraints of input, default is None - -Methods: - -- initialize_controller() initialize the controller -- calc_input(states, references) calculating optimal input - -More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py** - -We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py -Both functions have same variable and member function. However the solver is different. -Plese choose the right method for your environment. - -- example of import - -```py -from mpc_func_with_scipy import MpcController as MpcController_scipy -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -``` - -# Examples -## Problem Formulation - -** updating soon !! - -- first order system - - -- ACC (Adaptive cruise control) - - - -## Expected Results - -- first order system - - -- ACC (Adaptive cruise control) - - -# Usage - -- for example(first order system) - -``` -$ python main_example.py -``` - -- for example(ACC (Adaptive cruise control)) - -``` -$ python main_ACC.py -``` - -- for comparing two methods of optimization solvers - -``` -$ python test_compare_methods.py -``` - -# Requirement - -- python3.5 or more -- numpy -- matplotlib -- cvxopt -- scipy1.2.0 or more -- python-control - -# Reference -I`m sorry that main references are written in Japanese - -- モデル予測制御―制約のもとでの最適制御 著：Jan M. Maciejowski　訳：足立修一　東京電機大学出版局 \ No newline at end of file diff --git a/mpc/basic/README.md b/mpc/basic/README.md new file mode 100644 index 0000000..d101024 --- /dev/null +++ b/mpc/basic/README.md @@ -0,0 +1,144 @@ +# Model Predictive Control Basic Tool +This program is about template, generic function of linear model predictive control + +# Documentation of the MPC function +Linear model predicitive control should have state equation. +So if you want to use this function, you should model the plant as state equation. +Therefore, the parameters of this class are as following + +**class MpcController()** + +Attributes : + +- A : numpy.ndarray + - system matrix +- B : numpy.ndarray + - input matrix +- Q : numpy.ndarray + - evaluation function weight for states +- Qs : numpy.ndarray + - concatenated evaluation function weight for states +- R : numpy.ndarray + - evaluation function weight for inputs +- Rs : numpy.ndarray + - concatenated evaluation function weight for inputs +- pre_step : int + - prediction step +- state_size : int + - state size of the plant +- input_size : int + - input size of the plant +- dt_input_upper : numpy.ndarray, shape(input_size, ), optional + - constraints of input dt, default is None +- dt_input_lower : numpy.ndarray, shape(input_size, ), optional + - constraints of input dt, default is None +- input_upper : numpy.ndarray, shape(input_size, ), optional + - constraints of input, default is None +- input_lower : numpy.ndarray, shape(input_size, ), optional + - constraints of input, default is None + +Methods: + +- initialize_controller() initialize the controller +- calc_input(states, references) calculating optimal input + +More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py** + +We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py +Both functions have same variable and member function. However the solver is different. +Plese choose the right method for your environment. + +- example of import + +```py +from mpc_func_with_scipy import MpcController as MpcController_scipy +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +``` + +# Examples +## Problem Formulation + +- **first order system** + + + +- **ACC (Adaptive cruise control)** + +The two wheeled model are expressed the following equation. + + + +However, if we assume the velocity are constant, we can approximate the equation as following, + + + +then we can apply this model to linear mpc, we should give the model reference V although. + +- **evaluation function** + +the both examples have same evaluation function form as following equation. + + + +- is predicit state by using predict input + +- is reference state + +- is predict amount of change of input + +- are evaluation function weights + +## Expected Results + +- first order system + +- time history + + + +- input + + + +- ACC (Adaptive cruise control) + + + + +- animation + + + +# Usage + +- for example(first order system) + +``` +$ python main_example.py +``` + +- for example(ACC (Adaptive cruise control)) + +``` +$ python main_ACC.py +``` + +- for comparing two methods of optimization solvers + +``` +$ python test_compare_methods.py +``` + +# Requirement + +- python3.5 or more +- numpy +- matplotlib +- cvxopt +- scipy1.2.0 or more +- python-control + +# Reference +I`m sorry that main references are written in Japanese + +- モデル予測制御―制約のもとでの最適制御 著：Jan M. Maciejowski　訳：足立修一　東京電機大学出版局 \ No newline at end of file diff --git a/mpc/animation.py b/mpc/basic/animation.py similarity index 99% rename from mpc/animation.py rename to mpc/basic/animation.py index 2d7cd99..6ece541 100755 --- a/mpc/animation.py +++ b/mpc/basic/animation.py @@ -127,7 +127,7 @@ class AnimDrawer(): self._set_axis() self._set_img() - self.skip_num = 3 + self.skip_num = 1 frame_num = int((len(self.history_xs[0])-1) / self.skip_num) animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num) diff --git a/mpc/main_ACC.py b/mpc/basic/main_ACC.py similarity index 99% rename from mpc/main_ACC.py rename to mpc/basic/main_ACC.py index ffab36f..a868559 100644 --- a/mpc/main_ACC.py +++ b/mpc/basic/main_ACC.py @@ -218,7 +218,6 @@ def main(): theta_fig.legend() time_history_fig.tight_layout() - time_history_fig.legend() traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") diff --git a/mpc/main_example.py b/mpc/basic/main_example.py similarity index 97% rename from mpc/main_example.py rename to mpc/basic/main_example.py index 538d912..082693a 100644 --- a/mpc/main_example.py +++ b/mpc/basic/main_example.py @@ -85,7 +85,7 @@ class FirstOrderSystem(): def main(): dt = 0.05 - simulation_time = 100 # in seconds + simulation_time = 30 # in seconds iteration_num = int(simulation_time / dt) # you must be care about this matrix @@ -116,9 +116,9 @@ def main(): Bd = sysd.B # evaluation function weight - Q = np.diag([1., 1., 10., 10.]) - R = np.diag([0.01, 0.01]) - pre_step = 5 + Q = np.diag([1., 1., 1., 1.]) + R = np.diag([1., 1.]) + pre_step = 10 # make controller with discreted matrix # please check the solver, if you want to use the scipy, set the MpcController_scipy diff --git a/mpc/mpc_func_with_cvxopt.py b/mpc/basic/mpc_func_with_cvxopt.py similarity index 100% rename from mpc/mpc_func_with_cvxopt.py rename to mpc/basic/mpc_func_with_cvxopt.py diff --git a/mpc/mpc_func_with_scipy.py b/mpc/basic/mpc_func_with_scipy.py similarity index 100% rename from mpc/mpc_func_with_scipy.py rename to mpc/basic/mpc_func_with_scipy.py diff --git a/mpc/test_compare_methods.py b/mpc/basic/test_compare_methods.py similarity index 100% rename from mpc/test_compare_methods.py rename to mpc/basic/test_compare_methods.py