next up previous
Next: おわりに Up: 無題 Previous: の為の設計

  
2軸ロボットAdaptiveプログラム例

以下に2軸スカラロボットのAdaptiveの例のプログラムを載せます。アルゴ リズムはSlotine & Li(1986)によります。注意してほしいのですが，この プログラムはこのままでは走りません。計算のチェックも全くしていない ので参考にする場合は計算が正しいかどうか自分で計算してから使うよう にして下さい。例えば，33, 34行目のlambdaを始めとする諸定数は もちろん，35行目の回帰行列Yを求める関数も，目標値 qD, dqD, ddqDもありませんので， 自分で作る必要があります。ここでは Adaptiveのパラメータ更新則の積分はdiff_eqs()の中で行なっており， diff_eqs()の返すdxは実プラントの状態と推定パラメータとで 構成されています。
Func Matrix diff_eqs(t, x, u)
    Real t;
    Matrix x, u;
{
    Matrix M, C, tau;
    Matrix dx, q, dq;
    Real th1, th2, dth1, dth2;
    Matrix makeYcMatrix();

    M   = Z(2,2);    C   = Z(2,2);
    tau = Z(2,1);
    q   = Z(2,1);    dq  = Z(2,1);    
    qr  = Z(2,1);    dqr = Z(2,1);

    th1   = x(1,1);    th2   = x(2,1);
    dth1  = x(3,1);    dth2  = x(4,1);
    tau = u;
    q  = [[th1][th2]];
    dq = [[dth1][dth2]];

    M(1,1) = a1 + a2 + 2 * a3 * cos(th2);
    M(1,2) = a2 + a3 *  cos(th2);
    M(2,1) = a2 + a3 *  cos(th2);
    M(2,2) = a2;

    C(1,1) = -a3 * sin(th2) * dth2 + a4;  
    C(1,2) = -a3 * sin(th2) * (dth1 + dth2);
    C(2,1) =  a3 * sin(th2) * dth1;
    C(2,2) =  a5;

    ddq = D \ (tau - C * dq);
    
    ddth1 = ddq(1,1);
    ddth2 = ddq(2,1);

    dx = Z(9,1);

    dx(1,1) = dth1;
    dx(2,1) = dth2;
    dx(3,1) = ddth1;
    dx(4,1) = ddth2;  // プラント

    dqr = dqD  - lambda * (q  - qD); 
    ddqr= ddqD - lambda * (dq - dqD);

    Yc = makeYcMatrix(dqr, ddqr, q, dq);

    s = dq - dqr;

    da_hat = - gamma * Yc' * s;

    dx(5:9,1) = da_hat;
    return dx;
}

Func Matrix link_eqs(t, x)
    Real t;
    Matrix x;
{
    Matrix a_hat;
    Matrix u, q, dq, qr, dqr;
    Real th1, th2, dth1, dth2;

    q   = Z(2,1);    dq  = Z(2,1);    
    qr  = Z(2,1);    dqr = Z(2,1);

    th1   = x(1,1);    th2   = x(2,1);
    dth1  = x(3,1);    dth2  = x(4,1);

    q  = [[th1][th2]];
    dq = [[dth1][dth2]];

    a_hat = x(5:9,1);
    u = Z(2,1);

    dqr = dqD  - lambda * (q  - qD);
    ddqr= ddqD - lambda * (dq - dqD);

    Yc = makeYcMatrix(dqr, ddqr, q, dq);

    s = dq - dqr;
    
    u = Yc * a_hat - Kd * s;
    return u;
}
ここで賢明な人は，どうしてパラメータの更新則がdiff_eqs()の中 に書いてあるのだろう？これでは実際のインプリメントの時に実現できな いのではないか？という疑問を持たれるでしょう。この疑問は正しいとい えます。実際に計算機で制御プログラムを作る時にはコントローラは離散 系ですから，このような積分は出来ないと言っても良いでしょう。

ある意味では，制御則は全てlink_eqs()に書くべきだとも言えます。 実際のインプリメントの時には，サンプリング時間が短いということで， 大抵，パラメータ更新則の積分などをオイラー積分ですませることが多い ようです(もちろんルンゲクッタ法関数rngkut4()を入れるようにすれ ばなお良いのですが)。ただし前述したように，その場合結局は diff_eqs()に書いた事と本質的な違いはなくなってしまいます。

したがって，シミュレーションの場合は積分を必要とするものは diff_eqs()の中に書く方が簡単になると思われます。このような理由で， この例プログラムはパラメータ更新則をdiff_eqs()の中に書いてい ます。

Masanobu KOGA 平成11年9月20日