![]() |
![]() |
![]() |
左上: ![]() |
右上:![]() |
|
---|---|
左下:![]() |
|
次に外乱による追従誤差をPI制御によっておさえましょう.
![]() |
![]() |
![]() |
左上: ![]() |
右上:![]() |
|
---|---|
左下:![]() |
|
// "manip2track.mm" // Simulation of the tow link manipulator // グローバル変数 Real J1,J2,m1,m2,l1,l2,D1,D2,g; Matrix H; //軌道の係数行列 Func void main() { Real t0,t1; Real h,dtsav; Matrix x0,TC,XC,UC; Matrix qr0,qrd0,qrdd0,qr1,qrd1,qrdd1; List q0L,q1L; void diff_eqs(),link_eqs(); Matrix calH(); t0 = 0.0; t1 = 20.0; h = 1.0e-3; dtsav = 1.0e-2; // 物理パラメータ m1 = m2 = 1.0; l1 = l2 = 1.0; J1 = m1*l1^2/3.0+(m1+4*m2)*l1^2; J2 = m2*l2^2/3.0+m2*l2^2; D1 = 5.0; D2 = 5.0; g = 9.805; qr0 = [PI,0]'; qrd0 = Z(2,1); qrdd0 = Z(2,1); qr1 = [0, PI]'; qrd1 = [0, 0]'; qrdd1 = [0, 0]'; q0L = {qr0,qrd0,qrdd0}; q1L = {qr1,qrd1,qrdd1}; H = calH(t0,t1,q0L,q1L); x0 = [[qr0][qrd0]]; print "Now simulating\n"; {TC,XC,UC} = Ode(t0,t1,x0,diff_eqs,link_eqs,h,dtsav); mgplot(1,TC,XC(1:2,*)); // 横軸 t, 縦軸 tau1(t),tau2(t) mgplot(2,TC,UC(1:2,*)); // 横軸 t, 縦軸追従誤差 q(t)-qr(t) mgplot(3,TC,UC(3:4,*)); pause; // ファイルに保存 print TC >> "TC.mat"; print XC >> "XC.mat"; print UC >> "UC.mat"; } |
// diff_eqs() は微分方程式を記述する関数 Func void diff_eqs(DX,t,X,UY) Real t; Matrix X,DX,UY; { Matrix xp,up,dxp; Matrix q,qd; Matrix d; Matrix J,C,D,G; Matrix calJ(),calC(),calD(),calG(); q = X(1:2,1); qd = X(3:4,1); up = UY(1:2,1); J = calJ(q); C = calC(q,qd); D = [[D1,0] [0,D2]]; G = calG(q); // d = Z(2,1); // 外乱がゼロの場合 d = [0.1*sin(t),0.1*sin(5*t)]'; // 外乱がある場合 dxp=[[qd][-J~*(C+D*qd+G)]]+[[Z(2,2)][J~]]*(up+d); DX = [dxp]; } // link_eqs() は複数の微分方程式の関係を記述する関数 Func void link_eqs(UY,t,X) Real t; Matrix UY,X; { Matrix xp,up; Matrix q,qd; Matrix qref,qrd,qrdd; Matrix J,C,D,G; Matrix Kp,Kd; Matrix calqr(),calqrd(),calqrdd(); Matrix calJ(),calC(),calD(),calG(); q = X(1:2,1); qd = X(3:4,1); J = calJ(q); C = calC(q,qd); D = [[D1,0] [0,D2]]; G = calG(q); qref = calqr(t); qrd = calqrd(t); qrdd = calqrdd(t); // フィードフォワード制御 // up = J*qrdd+C+D*qd+G; // PD制御 Kp = diag(4.0,4.0); Kd = diag(4.0,4.0); up = J*qrdd+C+D*qd+G-Kp*(q-qref)-Kd*(qd-qrd); UY = [[up][q-qref]]; } |
// J(q)の計算を行う関数 Func Matrix calJ(q) Matrix q; { Real th1,th2; th1 = q(1,1); th2 = q(2,1); return [[J1+J2+2*2*m2*l1*l2*cos(th2), J2+2*m2*l1*l2*cos(th2)] [J2+2*m2*l1*l2*cos(th2),J2]]; } // C(q,qd)の計算を行う関数 Func Matrix calC(q,qd) Matrix q,qd; { Real th1,th2,th1d,th2d; th1 = q(1,1); th2 = q(2,1); th1d = qd(1,1); th2d = qd(2,1); return [[-2*m2*l1*l2*(2*th1d*th2d+th2d^2)*sin(th2)] [2*m2*l1*l2*th1d^2*sin(th2)]]; } // Gの計算を行う関数 Func Matrix calG(q) Matrix q; { Real th1,th2; th1 = q(1,1); th2 = q(2,1); return -[[(m1+2*m2)*l1*g*sin(th1)+m2*l2*g*sin(th1+th2)] [m2*l2*g*sin(th1+th2)]]; } //5次軌道の係数Hを求める関数 Func Matrix calH(t0,t1,q0L,q1L) Real t0,t1; List q0L,q1L; { Matrix T; Matrix q0,qd0,qdd0; Matrix q1,qd1,qdd1; Matrix calTT0(),calTT1(),calTT2(); {q0,qd0,qdd0} = q0L; {q1,qd1,qdd1} = q1L; T = [calTT0(t0),calTT1(t0),calTT2(t0), calTT0(t1),calTT1(t1),calTT2(t1)]; return [q0,qd0,qdd0,q1,qd1,qdd1]*T~; } |
Func Matrix calTT0(t) Real t; { return [1,t,t^2,t^3,t^4,t^5]'; } Func Matrix calTT1(t) Real t; { return [0,1,2*t,3*t^2,4*t^3,5*t^4]'; } Func Matrix calTT2(t) Real t; { return [0,0,2,6*t,12*t^2,20*t^3]'; } Func Matrix calqr(t) Real t; { return H * calTT0(t); } Func Matrix calqrd(t) Real t; { return H * calTT1(t); } Func Matrix calqrdd(t) Real t; { return H * calTT2(t); } |