/* -*- MaTX -*- * * 【名前】 * ctr_demo() - 制御系設計ツールボックスデモンストレーション * * 【形式】 * ctr_demo() * * 【機能説明】 * デモンストレーションのメニューを表示する。 */ Func void ctr_demo() { Integer i; List mm; void ctr_demo_transfer_function(), ctr_demo_poleassign(); void ctr_demo_servo(), ctr_demo_observer(), ctr_demo_lqr(); mm = {"制御系設計ツールボックス", "伝達関数と周波数応答", "極配置問題", "1 形サーボ系の設計", "全状態オブザーバの設計", "線形二次形式最適レギュレータ", "終 了"}; i = 1; for (;;) { i = menu(mm, i); switch (i) { case 1: ctr_demo_transfer_function(); break; case 2: ctr_demo_poleassign(); break; case 3: ctr_demo_servo(); break; case 4: ctr_demo_observer(); break; case 5: ctr_demo_lqr(); break; } if (i == length(mm)-1) { break; } i++; } } Func void ctr_demo_transfer_function() { Integer win; Matrix A, B, C, D, P; Matrix num, den, kk; Rational g; Polynomial s; Array w, ga, ph, re, im, k, t, y; CoMatrix po, ze, ro; win = mgplot_cur_win(); clear; print "\n"; print "次の伝達関数をプラントについて考える。\n"; print " 2 \n"; print " 0.2 s + 0.3 s + 1 \n"; print "H(s) = -------------------------------\n"; print " 2 \n"; print " (s + 0.4 s + 1) (s + 0.5)\n"; print "\n"; print "まず,多項式変数 's' を定義し,伝達関数を入力する。\n"; print "\n"; print "\ts = Polynomial(\"s\");\n"; print "\tg = (0.2*s^2 + 0.3*s + 1)/((s^2 + 0.4*s + 1)*(s + 0.5));\n"; print "\n"; pause; s = Polynomial("s"); g = (0.2*s^2 + 0.3*s + 1)/((s^2 + 0.4*s + 1)*(s + 0.5)); print "伝達関数の分子多項式と分母多項式の係数を求める。\n"; print "\n"; print "\t{num,den} = tfn2tf(g);\n"; print "\tprint num, den;\n"; print "\n"; pause; {num,den} = tfn2tf(g); print num, "\n", den, "\n"; print "状態空間表現:\n"; print "\t. \n"; print "\tx = Ax + Bu \n"; print "\ty = Cx + Du \n"; print "\n"; print "\t{A,B,C,D} = tf2ss(num,den);\n"; print "\tprint A,B,C,D;\n"; print "\n"; pause; {A,B,C,D} = tf2ss(num,den); print A,B,C,D; print "\n"; print "極,ゼロ点, ゲイン:\n"; print "\n"; print "\t{ze,po,kk} = ss2zp(A,B,C,D);\n"; print "\tprint po,ze;\n"; print "\n"; pause; {ze,po,kk} = ss2zp(A,B,C,D); print po,"\n",ze; print "\n"; print "与えられた伝達関数\n"; print "\n"; print "\tg = ss2tfn(A, B, C, D)\n"; print "\n"; pause; print g = ss2tfn(A, B, C, D); // 根奇跡 print "\n"; print "根軌跡:\n"; print "\n"; print "\tk = [0:0.5:10];\n"; print "\tro = rlocus(num,den,k);\n"; print "\n"; print "\tmgplot(win,Re(ro),Im(ro));\n"; print "\tmgplot_title(win,\"Root-locus\");\n"; print "\tmgplot_xlabel(win,\"Real part\");\n"; print "\tmgplot_ylabel(win,\"Imag part\");\n"; print "\n"; pause; k = [0:.5:10]; ro = rlocus_tf(num,den,k); mgplot_reset(win); mgplot_title(win,"Root-locus"); mgplot_xlabel(win,"Real part"); mgplot_ylabel(win,"Imag part"); mgplot(win,Re(ro),Im(ro),{"","",""},{"1","1","1"}); // 時間応答 print "ステップ応答\n"; print "\n"; print "\tt = [0:0.3:15];\n"; print "\t{y} = step(A,B,C,D,1,t);\n"; print "\n"; print "\tmgplot(win,t,y,{\"y\"});\n"; print "\tmgplot_title(win,\"Step response\");\n"; print "\tmgplot_xlabel(win,\"time(sec)\");\n"; print "\n"; pause; t = [0:0.3:15]; {y} = step_ss(A,B,C,D,1,t); mgplot_reset(win); mgplot(win,t,y,{"y"}); mgplot_title(win,"Step response"); mgplot_xlabel(win,"time(sec)"); // 周波数応答 print "ボード線図(周波数応答)\n"; print "\n"; print "\tw = logspace(0.01, 100.0);\n"; print "\t{ga,ph} = bode_tfn(g,w);\n"; print "\tga = 20*log10(ga);\n"; print "\n"; print "\tmgplot_subplot(win,2,1,1);\n"; print "\tmgplot_semilogx(win,w,ga,{\"gain\"});\n"; print "\tmgplot_xlabel(win,\"Rad/s\");\n"; print "\n"; print "\tmgplot_subplot(win,2,1,2);\n"; print "\tmgplot_semilogx(win,w,ph,{\"phase\"});\n"; print "\tmgplot_xlabel(win,\"Rad/s\");\n"; print "\n"; pause; w = logspace(0.001, 1000.0, 1000); {ga,ph} = bode_tfn(g,w); ga = 20*log10(ga); win = mgplot_cur_win(); mgplot_reset(win); mgplot_hold(win, 1); mgplot_subplot(win,2,1,1); mgplot_semilogx(win,w,ga,{"gain"}); mgplot_xlabel(win,"Rad/s"); mgplot_subplot(win,2,1,2); mgplot_semilogx(win,w,ph,{"phase"}); mgplot_xlabel(win,"Rad/s"); mgplot_hold(win, 0); print "ナイキスト線図\n"; print "\n"; print "\t{re, im} = nyquist_tfn(g, w);\n"; print "\n"; print "\tmgplot_subplot(win, 1, 1);\n"; print "\tmgplot_cmd(win, \"set xrange [-3:1]\");\n"; print "\tmgplot_cmd(win, \"set yrange [-2:1]\");\n"; print "\tmgplot_grid(win);\n"; print "\tmgplot(win, re, im, {\"nyquist\"});\n"; print "\n"; pause; {re, im} = nyquist_tfn(g, w); mgplot_reset(win); mgplot_hold(win, 1); mgplot_subplot(win, 1, 1); mgplot_cmd(win, "set xrange [-3:1]"); mgplot_cmd(win, "set yrange [-3:1]"); mgplot_grid(win); mgplot(win, re, im, {"nyquist"}); mgplot_hold(win, 0); pause; } /* * 極配置問題 */ Func void ctr_demo_poleassign() { Complex i; Matrix A, B, C, D; Matrix V, N, T, JA, J, JJ, F; Polynomial p, pc; print "\n"; print "行列 A, B, C, Dを入力する\n"; print "\n"; print "\tA = [[ 0 1 0]\n"; print "\t [ 0 0 1]\n"; print "\t [-6, -11, -6]]\n"; print "\tB = [[0]\n"; print "\t [0]\n"; print "\t [10]]\n"; print "\tC = [1 0 0]\n"; print "\tD = [0]\n"; print "\n"; pause; A = [[ 0 1 0] [ 0 0 1] [-6, -11, -6]]; B = [[0] [0] [10]]; C = [1 0 0]; D = [0]; print "\n可制御行列 V を計算する\n"; print "\n"; print "\tV = ctrm(A, B)\n"; print "\n"; pause; print V = ctrm(A, B); print "\n"; print "可制御性を調べる\n"; print "\n"; print "\trank(V)\n"; print "\n"; pause; print rank(V); print "\n"; print "行列 A の特性多項式\n"; print "\n"; print "\tp = makepoly(A)\n"; print "\n"; pause; print p = makepoly(A); print "\n"; print "可制御正準形への変換行列 T\n"; print "\n"; print "\tN = [[p(1) p(2) 1]\n"; print "\t [p(2) 1 0]\n"; print "\t [ 1 0 0]];\n"; print "\tT = V*N\n"; print "\n"; pause; N = [[p(1) p(2) 1] [p(2) 1 0] [ 1 0 0]]; print T = V*N; print "\n"; print "望ましい極をもつ閉ループ系の特性多項式\n"; print "\n"; print "\ti = (0,1);\n"; print "\tpc = Re(makepoly([-2+3*i, -2-3*i, -10.0]))\n"; print "\n"; pause; i = (0,1); print pc = Re(makepoly([-2+3*i, -2-3*i, -10.0])); print "\n"; print "状態フィードバックゲイン行列 F \n"; print "\n"; print "\tF = [pc(0)-p(0), pc(1)-p(1), pc(2)-p(2)]*inv(T)\n"; print "\n"; pause; print F = [pc(0)-p(0), pc(1)-p(1), pc(2)-p(2)]*inv(T); print "\n"; print "閉ループ系の極を調べる\n"; print "\n"; print "\teigval(A - B*F)\n"; print "\n"; pause; print eigval(A - B*F); print "\n"; pause; } /* * サーボ系の設計 */ Func void ctr_demo_servo() { Integer win; Real Fi; Complex i; Matrix A, B, C, D, F; Matrix Aa, Ba, Ca, Da, Fa, V, P; Matrix Ac, Bc, Cc, Dc; Array T, X, Y; print "\n"; print "行列 A, B, C, D を入力する。\n"; print "\n"; print "\tA = [[ 0 1 0 0]\n"; print "\t [ 20 0 0 0]\n"; print "\t [ 0 0 0 1]\n"; print "\t [-0.5 0 0 0]];\n"; print "\tB = [[ 0 ]\n"; print "\t [ -1]\n"; print "\t [ 0 ]\n"; print "\t [0.5]];\n"; print "\tC = [0 0 1 0];\n"; print "\tD = [0];\n"; print "\n"; pause; A = [[ 0 1 0 0] [ 20 0 0 0] [ 0 0 0 1] [-0.5 0 0 0]]; B = [[0][-1][0][0.5]]; C = [0 0 1 0]; D = [0]; print "拡張系:\n"; print "\n"; print "\tAa = [[ A, Z(4,1)]\n"; print "\t [-C, Z(1)]]\n"; print "\tBa = [[B]\n"; print "\t [0]]\n"; print "\n"; pause; print Aa = [[ A Z(4,1)] [-C, Z(1)]]; print Ba = [[B][0]]; print "\n"; print "拡大系の可制御行列\n"; print "\n"; print "\tV = ctrm(Aa, Ba)\n"; print "\n"; pause; print V = ctrm(Aa, Ba); print "\n"; print "拡大系の可制御性を調べる\n"; print "\n"; print "\trank(V)\n"; print "\n"; pause; print rank(V); print "\n"; print "閉ループ系の極からなるベクトル\n"; print "\n"; print "\ti = (0,1);\n"; print "\tP = [[-1+sqrt(3)*i]\n"; print "\t [-1-sqrt(3)*i]\n"; print "\t [ -5 ]\n"; print "\t [ -5 ]\n"; print "\t [ -5 ]]\n"; print "\n"; pause; i = (0,1); print P = [[-1+sqrt(3)*i] [-1-sqrt(3)*i] [ -5 ] [ -5 ] [ -5 ]]; print "\n"; print "極配置問題を解く\n"; print "\n"; print "\tFa = pplace(Aa, Ba, P)\n"; print "\tFi = - Fa(5);\n"; print "\n"; pause; print Fa = pplace(Aa, Ba, P); print "\n"; print Fi = - Fa(5); print "\n"; pause; print "状態フィードバックゲイン行列\n"; print "\n"; print "\tF = Fa(1:4)\n"; print "\n"; pause; print F = Fa(1:4); print "\n"; print "閉ループ系:\n"; print "\n"; print "\tAc = [[ A - B*F, B*Fi]\n"; print "\t [ -C , Z(1)]];\n"; print "\tBc = [[0]\n"; print "\t [0]\n"; print "\t [0]\n"; print "\t [1]];\n"; print "\tCc = [C Z(1)];\n"; print "\tDc = [0];\n"; print "\n"; pause; print Ac = [[A - B*F, B*Fi] [ -C , Z(1)]]; print Bc = [[0][0][0][0][1]]; print Cc = [C Z(1)]; print Dc = [0]; print "\nステップ応答を計算する\n"; print "\n"; print "\tT = linspace(0.0, 5.0);\n"; print "\t{Y, X} = step_ss(Ac, Bc, Cc, Dc, 1, T);\n"; print "\n"; pause; T = linspace(0.0, 5.0); {Y, X} = step_ss(Ac, Bc, Cc, Dc, 1, T); print "ステップ応答をプロットする (y vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 2, 1, 1);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output y versus Time t\");\n"; print "\tmgplot_xlabel(win, \"Sec\");\n"; print "\tmgplot_ylabel(win, \"Output y = x3\");\n"; print "\tmgplot(win, T, Y, {\"y\"});\n"; print "\n"; pause; win = mgplot_cur_win(); mgplot_reset(win); mgplot_subplot(win, 2, 1, 1); mgplot_grid(win, 1); mgplot_title(win, "Output y versus Time t"); mgplot_xlabel(win, "Sec"); mgplot_ylabel(win, "Output y = x3"); mgplot(win, T, Y, {"y"}); print "\n"; print "ステップ応答をプロットする (x vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 2, 1, 2);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Response Curves for x1, x2, x3, x4, x5\");\n"; print "\tmgplot_xlabel(win, \"Sec\");\n"; print "\tmgplot_ylabel(win, \"x1, x2, x3, x4, x5\");\n"; print "\tmgplot_text(win, \"x1\", 1.2, 0.05);\n"; print "\tmgplot_text(win, \"x2\", 1.2, -0.32);\n"; print "\tmgplot_text(win, \"x3\", 1.2, 0.33);\n"; print "\tmgplot_text(win, \"x4\", 2.2, 0.25);\n"; print "\tmgplot_text(win, \"x5\", 1.5, 1.28);\n"; print "\tmgplot(win, T, X, {\"x1\", \"x2\", \"x3\", \"x4\", \"x5\"});\n"; print "\n"; pause; mgplot_subplot(win, 2, 1, 2); mgplot_grid(win, 1); mgplot_title(win, "Response Curves for x1, x2, x3, x4, x5"); mgplot_xlabel(win, "Sec"); mgplot_ylabel(win, "x1, x2, x3, x4, x5"); mgplot_text(win, "x1", 1.2, 0.05); mgplot_text(win, "x2", 1.2, -0.32); mgplot_text(win, "x3", 1.2, 0.33); mgplot_text(win, "x4", 2.2, 0.25); mgplot_text(win, "x5", 1.5, 1.28); mgplot(win, T, X, {"x1", "x2", "x3", "x4", "x5"}); pause; } /* * 全状態観測器の設計 */ Func void ctr_demo_observer() { Complex i; Matrix A, C, N, W, T, Fo; Polynomial p, po; print "\n"; print "行列 A, C を入力する\n"; print "\n"; print "\tA = [[ 0 1 0]\n"; print "\t [ 0 0 1]\n"; print "\t [-5, -10, -5]]\n"; print "\tC = [1 0 0]\n"; print "\n"; pause; A = [[ 0 1 0] [ 0 0 1] [-5, -10, -5]]; C = [1 0 0]; print "可観測行列:\n"; print "\n"; print "\tN = obsm(A,C)\n"; print "\n"; pause; print N = obsm(A,C); print "\n"; print "可観測性を調べる\n"; print "\n"; print "\trank(N)\n"; print "\n"; pause; print rank(N); print "\n"; print "行列 A の特性多項式\n"; print "\n"; print "\tp = makepoly(A)\n"; print "\n"; pause; print p = makepoly(A); print "\n"; print "可観測正準形への変換行列\n"; print "\n"; print "\tW = [[p(1) p(2) 1]\n"; print "\t [p(2) 1 0]\n"; print "\t [ 1 0 0]];\n"; print "\tT = W*N'\n"; print "\n"; pause; W = [[p(1) p(2) 1] [p(2) 1 0] [ 1 0 0]]; print T = W*N'; print "\n"; print "オブザーバの極をもつ特性多項式:\n"; print "\n"; print "\ti = (0,1);\n"; print "\tpo = Re(makepoly([-2+i, -2-i, -5]))\n"; print "\n"; pause; i = (0,1); print po = Re(makepoly([-2+i, -2-i, -5])); print "\n"; print "オブザーバのゲイン\n"; print "\n"; print "\tFo = T \\ [po(0)-p(0), po(1)-p(1), po(2)-p(2)]'\n"; print "\n"; pause; print Fo = T \ [po(0)-p(0), po(1)-p(1), po(2)-p(2)]'; print "\n"; pause; } /* * 最適レギュレータの設計 */ Func void ctr_demo_lqr() { Integer win; Matrix A, B, C, D, Ac, Cc; Matrix Q, R, F, P; CoMatrix pc; Array to, xo, yo, tc, xc, yc; print "\n"; print "行列 A, B を入力する\n"; print "\n"; print "\tA = [[ 0 1 0]\n"; print "\t [ 0 0 1]\n"; print "\t [-35, -27, -9]]\n"; print "\tB = [[0]\n"; print "\t [0]\n"; print "\t [1]]\n"; print "\tC = I(3);\n"; print "\tD = Z(3,1);\n"; print "\n"; pause; A = [[ 0 1 0] [ 0 0 1] [-35, -27, -9]]; B = [[0][0][1]]; C = I(3); D = Z(3,1); print "重み行列 Q と R を入力する\n"; print "\n"; print "\tQ = 100 * I(3)\n"; print "\tR = [1]\n"; print "\n"; pause; Q = 100 * I(3); R = [1]; print "最適状態フィードバックゲイン:\n"; print "\n"; print "\t{F} = lqr(A,B,Q,R);\n"; print "\n"; pause; {F} = lqr(A,B,Q,R); print F; print "\n"; print "閉ループ系の極を求めます\n"; print "\n"; print "\tpc = eigval(A - B*F)\n"; print "\n"; pause; print pc = eigval(A - B*F); print "\n"; print "開ループ系のステップ応答\n"; print "\n"; print "\tC = I(3);\n"; print "\tD = Z(3,1);\n"; print "\tto = linspace(0.0, 5.0);\n"; print "\t{yo, xo} = step_ss(A, B, C, D, 1, to);\n"; print "\n"; pause; to = linspace(0.0, 5.0); {yo, xo} = step_ss(A, B, C, D, 1, to); print "ステップ応答をプロットする (yo1 vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 3, 1, 1);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo1\");\n"; print "\tmgplot(win, t, yo(1,:), {\"yo1\"});\n"; print "\n"; pause; win = mgplot_cur_win(); mgplot_reset(win); mgplot_subplot(win, 3, 1, 1); mgplot_grid(win, 1); mgplot_title(win, "Output yo1"); mgplot(win, to, yo(1,:), {"yo1"}); print "ステップ応答をプロットする (yo2 vs t)\n"; print "\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo2\");\n"; print "\tmgplot(win, t, yo(2,:), {\"yo2\"});\n"; print "\n"; pause; mgplot_subplot(win, 3, 1, 2); mgplot_grid(win, 1); mgplot_title(win, "Output yo2"); mgplot(win, to, yo(2,:), {"yo2"}); print "ステップ応答をプロットする (yo3 vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 3, 1, 3);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo3\");\n"; print "\tmgplot(win, t, yo(3,:), {\"yo3\"});\n"; print "\n"; pause; mgplot_subplot(win, 3, 1, 3); mgplot_grid(win, 1); mgplot_title(win, "Output yo3"); mgplot(win, to, yo(3,:), {"yo3"}); print "閉ループ系のステップ応答\n"; print "\n"; print "\tAc = A - B*F;\n"; print "\tCc = C - D*F;\n"; print "\ttc = linspace(0.0, 5.0);\n"; print "\t{yc, xc} = step_ss(Ac, B, Cc, D, 1, tc);\n"; print "\n"; pause; Ac = A - B*F; Cc = C - D*F; tc = linspace(0.0, 5.0); {yc, xc} = step_ss(Ac, B, Cc, D, 1, tc); print "ステップ応答をプロットする (yc1 vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 3, 1, 1);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo1 and yc1\");\n"; print "\tmgreplot(win, t, yc(1,:), {\"yc1\"});\n"; print "\n"; pause; mgplot_subplot(win, 3, 1, 1); mgplot_grid(win, 1); mgplot_title(win, "Output yo1 and yc1"); mgreplot(win, tc, yc(1,:), {"yc1"}); print "ステップ応答をプロットする (yc2 vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 3, 1, 2);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo2 and yc2\");\n"; print "\tmgreplot(win, t, yc(2,:), {\"yc2\"});\n"; print "\n"; pause; mgplot_subplot(win, 3, 1, 2); mgplot_grid(win, 1); mgplot_title(win, "Output yo2 and yc2"); mgreplot(win, tc, yc(2,:), {"yc2"}); print "ステップ応答をプロットする (yc3 vs t)\n"; print "\n"; print "\tmgplot_subplot(win, 3, 1, 3);\n"; print "\tmgplot_grid(win, 1);\n"; print "\tmgplot_title(win, \"Output yo3 and yc3\");\n"; print "\tmgreplot(win, t, yc(3,:), {\"yc3\"});\n"; print "\n"; pause; mgplot_subplot(win, 3, 1, 3); mgplot_grid(win, 1); mgplot_title(win, "Output yo3 and yc3"); mgreplot(win, tc, yc(3,:), {"yc3"}); pause; }