From aa323cdf1daa0de4c82f392a504d1ee4fd87fa74 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 19 May 2019 23:30:02 +0900 Subject: [PATCH 1/3] add newton method --- README.md | 1 + nmpc/newton/README.md | 40 ++++++++++++------------------------- nmpc/newton/main_example.py | 34 +++++++------------------------ 3 files changed, 21 insertions(+), 54 deletions(-) diff --git a/README.md b/README.md index 083791a..7650568 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ Now you can see the examples of control theories as following - **Iterative Linear Quadratic Regulator (iLQR)** - **Nonlinear Model Predictive Control (NMPC) with CGMRES** +- **Nonlinear Model Predictive Control (NMPC) with Newton method** - **Linear Model Predictive Control (MPC)**(as generic function such as matlab tool box) - **Extended Linear Model Predictive Control for vehicle model** diff --git a/nmpc/newton/README.md b/nmpc/newton/README.md index a46901c..dc5ffca 100644 --- a/nmpc/newton/README.md +++ b/nmpc/newton/README.md @@ -1,7 +1,8 @@ -# CGMRES method of Nonlinear Model Predictive Control -This program is about Continuous gmres method for NMPC -Although usually we have to calculate the partial differential of optimal matrix, it could be really complicated. -By using CGMRES, we can pass the calculating step and get the optimal input quickly. +# Newton method of Nonlinear Model Predictive Control +This program is about NMPC with newton method. +Usually we have to calculate the partial differential of optimal matrix. +In this program, in stead of using any paticular methods to calculate the partial differential of optimal matrix, I used numerical differentiation. +Therefore, I believe that it easy to understand and extend your model. # Problem Formulation @@ -13,39 +14,26 @@ By using CGMRES, we can pass the calculating step and get the optimal input quic - evaluation function +To consider the constraints of input u, I introduced dummy input. + - **two wheeled model** -- model - - - -- evaluation function - - - - -if you want to see more detail about this methods, you should go https://qiita.com/MENDY/items/4108190a579395053924. -However, it is written in Japanese +coming soon ! # Expected Results - example -![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/261584/3347fb3c-3fce-63fe-36d5-8a7bb053531a.png) +![Figure_1.png]() + +you can confirm that the my method could consider the constraints of input. - two wheeled model -- trajectory - -![image.png](https://qiita-image-store.s3.amazonaws.com/0/261584/8e39150d-24ed-af13-51f0-0ca97cb5f5ec.png) - -- time history - -![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/261584/e67794f3-e8ef-5162-ea84-eb6adefd4241.png) -![Figure_2.png](https://qiita-image-store.s3.amazonaws.com/0/261584/d74fa06d-2eae-5aea-4a33-6c63e284341e.png) +coming soon ! # Usage @@ -57,9 +45,7 @@ $ python main_example.py - for two wheeled -``` -$ python main_two_wheeled.py -``` +coming soon ! # Requirement diff --git a/nmpc/newton/main_example.py b/nmpc/newton/main_example.py index 06d8c17..5d1115b 100644 --- a/nmpc/newton/main_example.py +++ b/nmpc/newton/main_example.py @@ -407,19 +407,13 @@ class NMPCControllerWithNewton(): """ Attributes ------------ - zeta : float - gain of optimal answer stability - tf : float - predict time - alpha : float - gain of predict time N : int predicte step, discritize value threshold : float - cgmres's threshold value - input_num : int + newton's threshold value + NUM_INPUT : int system input length, this should include dummy u and constraint variables - max_iteration : int + MAX_ITERATION : int decide by the solved matrix size simulator : NMPCSimulatorSystem class us : list of float @@ -444,11 +438,10 @@ class NMPCControllerWithNewton(): None """ # parameters - self.tf = 1. # 最終時間 - self.N = 10 # 分割数 - self.threshold = 0.0001 # break値 + self.N = 10 # time step + self.threshold = 0.0001 # break - self.NUM_INPUT = 3 # dummy, 制約条件に対するrawにも合わせた入力の数 + self.NUM_INPUT = 3 # u with dummy, and 制約条件に対するrawにも合わせた入力の数 self.Jacobian_size = self.NUM_INPUT * self.N # newton parameters @@ -492,11 +485,6 @@ class NMPCControllerWithNewton(): # Newton method for i in range(self.MAX_ITERATION): - # check - # print("all_us = {}".format(all_us)) - # print("newton iteration in {}".format(i)) - # input() - # calc all state x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) @@ -504,8 +492,6 @@ class NMPCControllerWithNewton(): F_hat = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt) # judge - # print("F_hat = {}".format(F_hat)) - # print(np.linalg.norm(F_hat)) if np.linalg.norm(F_hat) < self.threshold: # print("break!!") break @@ -531,9 +517,6 @@ class NMPCControllerWithNewton(): F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt) - # print("check val of F = {0}".format(np.linalg.norm(F))) - # input() - # for save self.history_f.append(np.linalg.norm(F)) self.history_u.append(self.us[0]) @@ -641,7 +624,4 @@ def main(): if __name__ == "__main__": - main() - - - + main() \ No newline at end of file From e7fac02beafd0b82b34ca5ae176fb450c5789711 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 19 May 2019 23:36:32 +0900 Subject: [PATCH 2/3] add fig --- nmpc/newton/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nmpc/newton/README.md b/nmpc/newton/README.md index dc5ffca..84dea3a 100644 --- a/nmpc/newton/README.md +++ b/nmpc/newton/README.md @@ -27,7 +27,7 @@ coming soon ! - example -![Figure_1.png]() +![Figure_1.png](https://github.com/Shunichi09/linear_nonlinear_control/blob/demo_gif/nmpc/newton/Figure_1.png width = 70%) you can confirm that the my method could consider the constraints of input. From 8e9fd7c09138b4bbfba15e15cdb329aa4ac7da30 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 19 May 2019 23:38:48 +0900 Subject: [PATCH 3/3] Update README.md --- nmpc/newton/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nmpc/newton/README.md b/nmpc/newton/README.md index 84dea3a..94f24c9 100644 --- a/nmpc/newton/README.md +++ b/nmpc/newton/README.md @@ -27,7 +27,7 @@ coming soon ! - example -![Figure_1.png](https://github.com/Shunichi09/linear_nonlinear_control/blob/demo_gif/nmpc/newton/Figure_1.png width = 70%) + you can confirm that the my method could consider the constraints of input. @@ -62,4 +62,4 @@ I`m sorry that main references are written in Japanese - 非線形最適制御入門（コロナ社） -- 実時間最適化による制御の実応用（コロナ社） \ No newline at end of file +- 実時間最適化による制御の実応用（コロナ社）