[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method #553
Conversation
|
https://github.com/euslisp/jskeus/blob/master/irteus/test/mathtest.l
みたいな形でサンプルプログラムができるとよいですね．
…--
◉ Kei Okada
2019年7月20日(土) 14:12 Kohei Kimura <[email protected]>:
euslispでriccati方程式を解くためのsolverを追加しました．
既存のriccati-equationは繰り返し計算により定常解を得るもの（イテレーション数などの設定が必要）で，うまく解けないことがありましたが
繰り返し計算をしない方法として新しく「有本-Potterの方法（固有値分解法）」というアルゴリズムを採用して，riccati方程式の解が求められるようになっています．
また，LQRのフィードバックゲインも同時に出力されるようにしています．
以下，倒立振子のLQR制御を例に，
riccati-equation（繰り返し計算）
riccati-equation-arimoto-potter-method（有本-Potterの方法）
matlab（octave）
を比較した結果です．
- riccati-equation（繰り返し計算）
irteusgl$ ;; (send (instance riccati-equation :init AA bb cc QQ RR) :solve)
irteusgl$ (setq *riccati-result* (send (instance riccati-equation :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 2 4 '((1 0 0 0) (0 1 0 0))) 4 7) :solve))
(#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0)) #2f((-27.0313 0.0 0.0 0.0)))
irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation
175719.519 0.000 0.000 0.000
0.000 4.000 0.000 0.000
0.000 0.000 175719.519 0.000
0.000 0.000 0.000 4.000
#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0))
irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K
-27.031 0.000 0.000 0.000
#2f((-27.0313 0.0 0.0 0.0))
- riccati-equation-arimoto-potter-method（有本-Potterの方法）
irteusgl$ ;; (send (instance riccati-equation-arimoto-potter-method :init AA BB QQ RR) :solve)
irteusgl$ (setq *riccati-result* (send (instance riccati-equation-arimoto-potter-method :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 4 4 '((4 0 0 0) (0 4 0 0) (0 0 4 0) (0 0 0 4))) (make-matrix 1 1 '((7)))) :solve))
(#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846)) #2f((-191.33 -0.755929 -59.3904 -1.3284)))
irteusgl$ (format-array (car *riccati-result*)) ;; solution P of riccati equation
35705.647 314.265 12343.274 441.199
314.265 7.029 111.060 4.176
12343.274 111.060 4331.802 156.495
441.199 4.176 156.495 5.828
#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846))
irteusgl$ (format-array (cadr *riccati-result*)) ;; LQR state feedback gain K
-191.330 -0.756 -59.390 -1.328
#2f((-191.33 -0.755929 -59.3904 -1.3284))
- matlab（octave）
>> riccati_solve_test
A =
0.00000 0.00000 1.00000 0.00000
0.00000 0.00000 0.00000 1.00000
34.67763 0.00000 0.00000 0.00000
-679.76829 0.00000 0.00000 0.00000
B =
0.00000
0.00000
-1.27842
32.73032
Q =
Diagonal Matrix
4 0 0 0
0 4 0 0
0 0 4 0
0 0 0 4
R = 7
P =
35705.77395 314.26480 12343.29561 441.19909
314.26480 7.02922 111.06011 4.17624
12343.29561 111.06011 4331.80142 156.49470
441.19909 4.17624 156.49470 5.82845
K =
-191.32998 -0.75593 -59.39046 -1.32840
新しく実装したriccati-equation-arimoto-potter-method（有本-Potterの方法）のほうは，ほぼmatlab（octave）と同じ結果が得られました．
現状は，「有本-Potterの方法」のポイントとなるHamilton行列（A,B,Q,Rから作成）の固有ベクトルの計算に関して，
euslispの固有値分解の関数(eigen-decompose)が虚数までカバーできていないため
Hamilton行列の固有ベクトルが虚数（固有値も虚数）になる場合には例外処理しています．
（今のところ，倒立振子モデルの場合はHamilton行列の固有値，固有ベクトルは実数となっています）
------------------------------
You can view, comment on, or merge this pull request online at:
#553
Commit Summary
- [irteus/irtdyna.l] Add riccati equation solver based on Arimoto
Potter method
File Changes
- *M* irteus/irtdyna.l
<https://github.com/euslisp/jskeus/pull/553/files#diff-0> (63)
Patch Links:
- https://github.com/euslisp/jskeus/pull/553.patch
- https://github.com/euslisp/jskeus/pull/553.diff
—
You are receiving this because you are subscribed to this thread.
Reply to this email directly, view it on GitHub
<#553?email_source=notifications&email_token=AADYNXBMHUOMCK2NIP6W7CDQAKNFDA5CNFSM4IFNS6AKYY3PNVWWK3TUL52HS4DFUVEXG43VMWVGG33NNVSW45C7NFSM4HANLBXQ>,
or mute the thread
<https://github.com/notifications/unsubscribe-auth/AADYNXG5ZLQLQNXZ6AZ3VTTQAKNFDANCNFSM4IFNS6AA>
.
|
… add sample test program
の問題を#554 の変更で対応させました． それをテストする（matlabの結果と比較する）ためのriccati-equation-arimoto-potter-methodのサンプルプログラムも追加しました． |
…tion-arimoto-potter-method
|
euslisp/EusLisp#393 と同様にこちらも |
|
Thank you for contributing jskeus documentation PDF version of Japanese jmanual: jmanual.pdf |
|
@k-kimura これは今のriccati方程式 を使っているプログラム（予見制御とかがそう？）には影響を与えないんだよね？ となっているべきだよね．ただ，肝心の となっているので，こうしたからって何かいいことがあるとはならなそう，となると， となっていて，同じクラスの中でアルゴリズムを切り替えるような書き方にすると， で，もう一歩考えると，もし完全上位互換のものが作れているのであればそもそも切り替えても良い． この辺は，riccatti 方程式を使っているユーザがどれぐらいいるかというのが問題で，ユーザがいないなら，そのまま色々切り替えていけば良いし，既に既存のユーザがいるなら，後方互換も意識して考えていく必要がある． パット見，riccatti 方程式はpreview control に使われていて，このレイヤで歩行制御しているのはサンプルプログラムぐらいしか無いのかなぁ，とは思っているんだけどどうだろうか？ 案外実は色々なところで活用されているのかな． |
|
プログラムを作って，サンプルプログラムを入れて，ドキュメントを追加して， 確かに
（プログラムコード等々を書いて追加するだけでもすでに苦労はしていますが） |
これは普段から自分が使っているコードのタイポとかドキュメント直してPR送っていれば，誰がどの部分を使っているか可視化されるのでいいんだけど，そうでないとなかなかわからないですよね． |
|
ありがとうございます． まだ理解が足りていないかもしれませんが， |
はい．そういう意味でした． |
euslispでriccati方程式を解くためのsolverを追加しました．
既存のriccati-equationは繰り返し計算により定常解を得るもの（イテレーション数などの設定が必要）で，うまく解けないことがありましたが
繰り返し計算をしない方法として新しく「有本-Potterの方法（固有値分解法）」というアルゴリズムを採用して，riccati方程式の解が求められるようになっています．
また，LQRのフィードバックゲインも同時に出力されるようにしています．
以下，倒立振子のLQR制御を例に，
riccati-equation（繰り返し計算）
riccati-equation-arimoto-potter-method（有本-Potterの方法）
matlab（octave）
を比較した結果です．
新しく実装したriccati-equation-arimoto-potter-method（有本-Potterの方法）のほうは，ほぼmatlab（octave）と同じ結果が得られました．
現状は，「有本-Potterの方法」のポイントとなるHamilton行列（A,B,Q,Rから作成）の固有ベクトルの計算に関して，
euslispの固有値分解の関数(eigen-decompose)が虚数までカバーできていないため
Hamilton行列の固有ベクトルが虚数（固有値も虚数）になる場合には例外処理しています．
（今のところ，倒立振子モデルの場合はHamilton行列の固有値，固有ベクトルは実数となっています）