next up previous
Next: 2軸スカラ型マニピュレータのシミュレーション Up: 線形システムのシミュレーション Previous: バネマスダンパ系のフィードバック制御:PD制御

バネマスダンパ系のフィードバック制御:PID制御

それでは,PID制御によってx(t)をに収束させましょう.

\begin{displaymath}
u(t) = - K_p x(t) - K_d \dot x(t) -K_i \int^{t}_0 x(t) \mbox{dt}\end{displaymath}

問題は,$\int^{t}_0 x(t) \mbox{dt}$の積分をMaTXでどのようにして 計算するかです.

まず,

\begin{displaymath}
q(t) := \int^{t}_0 x(t) \mbox{dt}
,\hspace*{1zw} q(0)=0\end{displaymath}

と定義することにします.すると,  
 \begin{displaymath}
\dot q(t) = x(t)\end{displaymath} (9)
となるので,(8)(9)の 二つの微分方程式をMaTXのプログラムに記述して,${\bf x}(t)$q(t)を計算し,入力uを次のように構成すれば良いことになります.

\begin{displaymath}
u(t) = 
\left[
\begin{array}
{rrr}
- K_p & - K_d & -K_i \\ \...
 ...egin{array}
{r}
x(t) \\ \dot x(t) \\ q(t) \\ \end{array}\right]\end{displaymath}

Ki = 1.0とした場合のグラフを次に示します.PD制御の場合の グラフと比較してください. 左がx(t)で右がu(t)です.

\includegraphics [width=7cm,clip]{mck_plot_pidx.ps}
\includegraphics [width=7cm,clip]{mck_plot_pidu.ps}




/*
   "mckpid.mm"
   Simulation of the following system:
   M x^(2) + C x^(1) + K x = u + d
   q^(1) = x
   u = -Kp x - Kd x^(1) -Ki q , d = 0.5
   Kp = 4.0; Kd = 4.0; Ki = 1.0;
*/

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)=[1.0,0.0,0.0]' にする
	X0 = [1.0,0.0,0.0]';
//  x0 = [[xp(0)][q(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, 縦軸 x(t)
	mgplot(1,TC,XC(1,*));
//  XC(1,*)はXCの1行目を表す
//  数値解を表示する.横軸 t, 縦軸 u(t)
	mgplot(2,TC,UC);

//  ファイルに保存
	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 K,C,M;
	Matrix xp,up,dxp;
	Matrix q,dq;
	Matrix A,B;
	Matrix d;

//  物理パラメータの設定
	K = 1.0; C = 0.4; M = 1.0;

//  A,Bの設定
	A = [[0,1][-K/M,-C/M]];
	B = [0,1/M]';

// X=[[xp(t)][q(t)]]を表している.
	xp = X(1:2,1); 
	q = X(3:3,1);

//  link_eqs()で計算したUYを代入する.
	up = UY;

//  外乱dの設定
	d = [0.5];

//  微分方程式
	dxp = A * xp + B * (up + d);
    dq = xp(1:1,1);

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

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

//  X=[[xp(t)][q(t)]]を表している.
	xp = X(1:2,1); 
	q = X(3:3,1);
//  分解する必要はありませんが...

//  Kp,Kd,Kiを設定
    Kp = 4.0; Kd = 4.0; Ki = 1.0;

//  最後にUYにupの時刻tの値を渡す.
	up = [-Kp,-Kd,-Ki] * [[xp][q]];

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



``mckpid.mm'': バネマスダンパ系のシミュレーションプログラム(PID制御)


Kenichiro Nonaka
5/15/1998