next up previous
Next: 目標軌道の生成 Up: 2軸スカラ型マニピュレータのシミュレーション Previous: 微分方程式と状態空間表現

振り子のシミュレーション

それでは早速シミュレーションを行ってみましょう. まず最初は簡単なものとして, 入力${\bf u}(t)$および外乱${\bf d}(t)$をゼロとして, ぶら下がった状態から45度傾けて減衰振動させてみましょう.
\includegraphics [width=7cm,clip]{manip2_plot_nofbdec.ps}
data-1$\theta_1(t)$data-2$\theta_2(t)$です.




//   "manip2.mm"
//   Simulation of the tow link manipulator
Func void main()
{
	Real t0,t1;
	Real h,dtsav;
	Matrix x0,TC,XC,UC;
	void diff_eqs(),link_eqs();

	t0 = 0.0;  //シミュレーションの開始時刻を t=0 にする
	t1 = 20.0;  //シミュレーションの終了時刻を t=20 にする
//  初期状態を x(0)=[PI*0.75,0.0,0.0,0.0]' にする
	x0 = [PI*0.75,0.0,0.0,0.0]';
	h = 1.0e-3; // シミュレーションの刻を1msecにする
	dtsav = 1.0e-2;// データは10msecおきに保存する

	print "Now simulating\n";

// Ode()によってシミュレーションを行う
	{TC,XC,UC} = Ode(t0,t1,x0,diff_eqs,link_eqs,h,dtsav);
// diff_eqs() は微分方程式を記述する関数
// link_eqs() は複数の微分方程式の関係を記述する関数
// TC: 時間の時系列
// XC: 状態x(t)の時系列
// UC: 入出力ベクトルの時系列(diff_eqs,link_eqsのUY)

//  数値解を表示する.横軸 t, 縦軸 th1(t),th2(t)
	mgplot(1,TC,XC(1:2,*));
//  XC(1:2,*)はXCの1,2行目を表す
//  数値解を表示する.横軸 t, 縦軸 tau1(t),tau2(t)
//	mgplot(2,TC,UC);
	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;
{
	Real th1,th2,th1d,th2d,J1,J2,m1,m2,l1,l2,D1,D2,g;
	Matrix xp,up,dxp;
	Matrix q,qd;
	Matrix d;
	Matrix J,C,D,G;
// X = [[q][qd]];
	q = X(1:2,1);
	qd = X(3:4,1);
//  link_eqs()で計算したUYを代入する.
	up = UY;



 

// q = [th1,th2]';
	th1 = q(1,1);
	th2 = q(2,1);
// qd = [th1d,th2d]';
	th1d = qd(1,1);
	th2d = qd(2,1);

//  物理パラメータ
	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;

//  J(q)の計算
	J = [[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)の計算
	C = [[-2*m2*l1*l2*(2*th1d*th2d+th2d^2)*sin(th2)]
		 [2*m2*l1*l2*th1d^2*sin(th2)]];
//  Dの計算
	D = [[D1,0]
		 [0,D2]];
//  Gの計算
	G=-[[(m1+2*m2)*l1*g*sin(th1)+m2*l2*g*sin(th1+th2)]
		  [m2*l2*g*sin(th1+th2)]];

//  外乱dの設定
	d = Z(2,1);

//  微分方程式
	dxp=[[qd][-J~*(C+D*qd+G)]]+[[Z(2,2)][J~]]*(up+d);

//  最後に速度をDXに渡す
	DX = [dxp];
}

// link_eqs() は複数の微分方程式の関係を記述する関数
Func void link_eqs(UY,t,X)
Real t;
Matrix UY,X;
{
	Real Kp,Kd;
	Matrix xp,up;
	Matrix q,qd;

// X = [[q][qd]];
	q = X(1:2,1);
	qd = X(3:4,1);

//  最後にUYにupの時刻tの値を渡す.
	up = Z(2,1);

//  UYにupを代入し,diff_eqsに渡す
	UY = [up];
}



``manip2.mm'': 2リンクマニピュレータのシミュレーションプログラム(自由振動)

後の都合により,このプログラムを関数とグローバル変数を用いて 書き換えておきましょう.




//   "manip2new.mm"
//   Simulation of the tow link manipulator
Real J1,J2,m1,m2,l1,l2,D1,D2,g;//  グローバル変数
Func void main()
{
	Real t0,t1;
	Real h,dtsav;
	Matrix x0,TC,XC,UC;
	void diff_eqs(),link_eqs();

	t0 = 0.0;  //シミュレーションの開始時刻を t=0 にする
	t1 = 20.0;  //シミュレーションの終了時刻を t=20 にする
//  初期状態を x(0)=[PI*0.75,0.0,0.0,0.0]' にする
	x0 = [PI*0.75,0.0,0.0,0.0]';
	h = 1.0e-3; // シミュレーションの刻を1msecにする
	dtsav = 1.0e-2;// データは10msecおきに保存する
//  物理パラメータ
	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;

	print "Now simulating\n";
// Ode()によってシミュレーションを行う
	{TC,XC,UC} = Ode(t0,t1,x0,diff_eqs,link_eqs,h,dtsav);
// diff_eqs() は微分方程式を記述する関数
// link_eqs() は複数の微分方程式の関係を記述する関数
// TC: 時間の時系列
// XC: 状態x(t)の時系列
// UC: 入出力ベクトルの時系列(diff_eqs,link_eqsのUY)

//  数値解を表示する.横軸 t, 縦軸 th1(t),th2(t)
	mgplot(1,TC,XC(1:2,*));
//  XC(1:2,*)はXCの1,2行目を表す
//  数値解を表示する.横軸 t, 縦軸 tau1(t),tau2(t)
//	mgplot(2,TC,UC);
	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();

// X = [[q][qd]];
	q = X(1:2,1);
	qd = X(3:4,1);

	up = UY;
	J = calJ(q); //  J(q)の計算
	C = calC(q,qd); //  C(q,qd)の計算



 

	D = [[D1,0][0,D2]]; //  Dの計算
	G = calG(q); //  Gの計算
	d = Z(2,1); //  外乱dの設定
//  微分方程式
	dxp=[[qd][-J~*(C+D*qd+G)]]+[[Z(2,2)][J~]]*(up+d);
//  最後に速度をDXに渡す
	DX = [dxp];
}

// link_eqs() は複数の微分方程式の関係を記述する関数
Func void link_eqs(UY,t,X)
Real t;
Matrix UY,X;
{
	Real Kp,Kd;
	Matrix xp,up;
	Matrix q,qd;
// X = [[q][qd]];
	q = X(1:2,1);
	qd = X(3:4,1);
//  最後にUYにupの時刻tの値を渡す.
	up = Z(2,1);
//  UYにupを代入し,diff_eqsに渡す
	UY = [up];
}
//  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)]];
}



``manip2new.mm'': グローバル変数と関数による改良版


Kenichiro Nonaka
5/15/1998