next up previous
Next: 終りに Up: 2軸スカラ型マニピュレータのシミュレーション Previous: 逆動力学によるフィードフォワード

逆運動学によるPI制御

次に外乱を加えてみましょう.

\begin{displaymath}
{\bf d}(t) :=
\left[
\begin{array}
{l}
0.1\sin(t) \\ 0.1\sin(5t) \\ \end{array}\right]\end{displaymath}

追従誤差${\bf q}(t)-{\bf q_r}(t)$が生じるのがわかります.
\includegraphics [width=7cm,clip]{manip2track_plot_q_d0.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_u_d0.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_e_d0.ps}

左上:${\bf q}(t)$


右上:${\bf u}(t)$  
左下:${\bf q}(t)-{\bf q_r}(t)$  
   

次に外乱による追従誤差${\bf q}(t)-{\bf q_r}(t)$をPI制御によっておさえましょう.

\begin{displaymath}
{\bf u}(t) := 
J({\bf q})\ddot{\bf q_r} + C({\bf q},\dot{\bf...
 ...right)
- K_d \left( \dot {\bf q}(t) - \dot {\bf q_r}(t) \right)\end{displaymath}

\includegraphics [width=7cm,clip]{manip2track_plot_q_d.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_u_d.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_e_d.ps}

左上:${\bf q}(t)$


右上:${\bf u}(t)$  
左下:${\bf q}(t)-{\bf q_r}(t)$  
   




//  "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);
}



``manip2track.mm'': フィードフォワード制御/PD制御



Kenichiro Nonaka
5/15/1998