Maxima で解くロボット・ダイナミクス

□ ２リンク・マニピュレータの運動方程式の導出

   重心位置

    (%i ) depends([q1, q2],[t]);
    (%i ) p1: matrix([Lg1*cos(q1)],[Lg1*sin(q1)]);
    (%i ) p2: matrix([L1*cos(q1) + Lg2*cos(q1+q2)],[L1*sin(q1)+Lg2*sin(q1+q2)]);

   重心速度

    (%i ) v1: diff(p1, t);
    (%i ) v2: diff(p2, t);

   定数の宣言

    (%i ) declare(g, constant,
                  L1, constant, L2, constant,  
                  Lg1, constant, Lg2, constant, 
                  m1, constant, m2, constant,
                  I1, constant, I2, constant);
   運動エネルギー

    (%i ) K1: m1*trigsimp(v1.v1/2) + (1/2)*Ig1*diff(q1,t)^2;
    (%i ) K2: m2*ratsimp(trigreduce(expand(v2.v2/2)),q1,q2) + (1/2)*Ig2*diff(q1+q2,t)^2;
    (%i ) K: ratsimp(K1+K2,diff(q1,t),diff(q2,t));
    (%i ) load("lrats");
    (%i ) K: ratsimp(fullratsubst([Ig2 = I2 - m2*Lg2^2, Ig1 = I1 - m1*Lg1^2], K),diff(q1,t),diff(q2,t));

   位置エネルギー

    (%i ) P: m1*g*p1[2][1] + m2*g*p2[2][1];

   ラグランジュ関数

    (%i ) L: K-P;

   ラグランジュ方程式

    (%i ) t1: ratsimp(expand(diff(diff(L, diff(q1,t)), t)) - expand(diff(L,q1)), diff(q1,t,2), diff(q2,t,2), g);
    (%i ) t2: ratsimp(expand(diff(diff(L, diff(q2,t)), t)) - expand(diff(L,q2)), diff(q1,t,2), diff(q2,t,2), g);

   慣性行列

    (%i ) M: matrix([coeff(t1, diff(q1,t,2)), coeff(t1, diff(q2,t,2))] , [coeff(t2, diff(q1,t,2)), coeff(t2, diff(q2,t,2))]);

   重力項

    (%i ) G: matrix([coeff(t1, g)], [coeff(t2, g)])*g;

   非線形項

    (%i ) h: expand(matrix([t1],[t2]) - M.matrix([diff(q1,t,2)],[diff(q2,t,2)]) - G);

   非線形項中の歪対称行列の導出

    (%i ) q: matrix([q1],[q2]);
    (%i ) s1: diff(M,t)[1] - (transpose(diff(q,t)).diff(M/2,q1))[1];
    (%i ) s2: diff(M,t)[2] - (transpose(diff(q,t)).diff(M/2,q2))[1];
    (%i ) H: matrix(s1, s2);
    (%i ) H1: expand(H-diff(M,t)/2);
    (%i ) S: expand(H1-transpose(H1));

       証明は省略するが，上記の計算で S が歪対称行列となることが証明できる．

□ 運動方程式に対するリグレッサ行列の導出

    (%i ) Reg(eq, pm) := 
          block([l:[]], for f in pm do 
            (eq: fullratsubst([f = aa], expand(eq)),
             c: coeff(ratsimp(eq, aa), aa),
             eq: eq - c*aa,
             l:endcons(c,l)),return(l))$
    (%i ) T: [I1, I2, m1*Lg1, m2*Lg2, m2];
    (%i ) Y: matrix(Reg(t1, T), Reg(t2, T));

□ リグレッサ行列からの最小動力学パラメータの導出
    参考文献：
    川崎，村田，神崎，
    「数式処理による閉リンクマニピュレータの最小動力学パラメータの解析」，
    日本ロボット学会誌，Vol. 13, No. 4, pp.558 〜 564, 1995. 

    基本関数の導出 

    (%i ) declare(a, constant, b, constant);
    (%i ) tt: expand(trigexpand(a*t1 + b*t2));
    (%i ) ff: showratvars(tt);

    (%i ) load(flatten);
    (%i ) f: flatten(map(lambda([x], if constantp(x) then [] else x), ff));

    (%i ) Indets(e, ff) := makelist(hipow(e, ff[j]), j, 1, length(ff))$
    (%i ) BF(tt, ff) := block([L:[], r],
              for i:1 thru nterms(tt) do (
                  r: Indets(part(tt, i), ff),
                  if not member(r, L) then L: cons(r, L)),
              L)$
    (%i ) Functs(l, f) := product(f[i]**l[i], i, 1, length(l))$
    (%i ) bf: map(lambda([l], Functs(l, f)), BF(tt, f));

    リグレッサと物理パラメータの更新

    (%i ) Fcoef(e, f) := map(lambda([x], if constantp(x) then x else 0), 
              makelist(ratcoef(expand(trigexpand(e)), f[i]), i, 1, length(f)))$
    (%i ) Bcoef(c, f) := block([L:[]], 
              for i:length(c) step -1 thru 1 do (
                  L: append(Fcoef(c[i,1], f), L)), L)$
    (%i ) drop(l, n) := flatten(makelist(if i = n then [] else l[i],
              i, 1, length(l)))$

    (%i ) MP(Y,T) :=  block([B:transpose(matrix(Bcoef(col(Y,1), bf))),
                  T1:T,Y1:Y,be,Be],
              for i:2 while i <= length(row(Y1,1)[1]) do (
                  bi: transpose(matrix(Bcoef(col(Y1,i), bf))),
                  B: addcol(B, bi),
                  be: echelon(B),
                  if be[i,i] = 0 then block([j,k,dt],
                      Be: block([L:row(be,1)],
                          for j:2 thru i-1 do L:addrow(L, row(be,j)), L),
                      k: invert(submatrix(Be,i)).col(Be,i),
                      dt: transpose(k)[1]*T1[i],
                      T1: drop(T1, i),
                      for j:1 thru length(dt) do T1[j]: T1[j]+dt[j],
                      Y1: submatrix(Y1,i),
		      B: submatrix(B,i),
		      i: i-1
                  )
              ),[Y1,T1])$
    (%i ) new: MP(Y,T);
    (%i ) Y: first(new);
    (%i ) T: second(new);

         この計算方法は，初期のリグレッサと動力学パラメータの値に依存することに
         注意が必要である．たとえば，次の T と Y から最小動力学パラメータを求め
         ると，異なる結果が得られる．

    (%i ) T1: [m1*Lg1, m2*Lg2, m2, I1, I2];
    (%i ) Y1: matrix(Reg(t1, T1), Reg(t2, T1));
    (%i ) new: MP(Y1,T1);
    (%i ) Y1: first(new);
    (%i ) T1: second(new);

         《注意》
         色々検証を進めた所，上記の計算手順や関数で平面４リンクマニピュレータの
	 最小動力学パラメータを導出できることが確認できましたが，垂直多関節型の
	 ３リンク以上のマニピュレータでは正しい結果が出てきません．幾つか細かな
	 点で修正が必要となります．