KNOPPIXでロボット工学

［｜　］　 最終更新： 2004/01/13 01:18:03

KNOPPIX-Edu TG

東北学院大学工学部でＩＴ教育の試みの一つとして配布した KNOPPIX EduTG は、ＣＤ一枚から立ち上がる、OS(Linux)と様々なアプリケーションプログラムをセットにしてドイツで開発されたKNOPPIXをつくばで日本語化したものに、さらにＩＴ教育委員会の要望を組み込んで開発されたものです。
そのため、一般的なKNOPPIXには含まれていないものの工学部で使いそうなツールも搭載してあります。

このページでは、せっかく配布されているKNOPPIXをロボット工学の助けに使わない手はないということで、活用法をまとめていきます。

なお、このページの例は、

等で試すことができます。 (紙に書くとたいへんでしょう。＜敢えて印刷は避ける方向)

表計算ソフト OpenOffice Calc

表計算でシミュレーション

主要アプリケーションの一つであるOpenOfficeに含まれる表計算ソフト、Calcは簡単なロボット制御シミュレーションができます。
表計算ソフトでシミュレーションをやるときの、基本的な手順です(筆者独断)。

  1. まず、各状態変数の関係式をリストアップする。
  2. 横方向に各状態変数をおいていく。
    状態方程式のように、「状態変数の微分値＝」で式が与えられるときは数値積分が必要なので、状態変数そのものと、その微分値の枠を取る。
  3. 縦方向に時間軸をとる。左端の列は時刻。
  4. 単純な変数間の関係式をまず時刻０の行に入力する。
    システムへの入力値は適当に設定しておく。
    積分していくような変数は、初期値を書きこねおく。
  5. 入力した１行を次の行にコピーする。
    時刻＝１行上の時刻＋Δt
    変数＝一行上の変数＋変数の微分値×Δt
    の関係を入力。
  6. 以上を下方向にだぁ〜っとコピーして、入力を適宜設定して得られた結果を吟味する。
Δtは特定のセル(枠)に書いておいて、「$行$列」という固定枠表記で数式中で使います。 すると、そこを変えるだけで計算への時間ステップの影響が確認できます。 同じように制御ゲインなども固定枠に書いておきます。

移動ロボットのシミュレーション

実際に、この車輪移動ロボットのページのアニメーションなどは、OpenOfficeのCalcでまずは動作確認および軌道計算や車輪の回転などを確認して、それを３次元ＣＧソフトのデータに結合してつくっています。そのなかから、円弧走行、
円弧走行
のデータを見てみます。
円弧走行シミュレーションシート (2004/01/13, 42,948 bytes)
ロボット移動シミュレーション

変数の名前を適当につけていたため、ちょっと分かりにくいかも知れませんが、

となっています。また、$D$3にはロボットの車幅(ωの計算に必要)、$D$4には時間刻がはいっています。
このシートでは、VR,VLを適宜設定するとその際の軌道が計算されます。 X,Yをグラフにすれば、実際の軌跡がわかるわけです。 なお、グラフにするときには、縦軸と横軸が同じ縮尺になるように気をつけましょう。 違う縮尺だと、不要な勘違いを起こす危険性があります。
ＣＧ映像を作るためには、これに加えて車輪の回転角度や旋回中心の表示などいろいろ追加で計算しています。

数式処理ソフト Maxima

この講義では特に前半、やたらとcos/sinのつまった行列の演算を扱いました。 これらはある程度は手計算でできますが、コンピュータがやってくれるならまかせるにこしたことはありません(もちろん、手を動かすことは重要)。

もっとも、本来コンピュータは数値計算は得意ですが、数式をそのまま処理するということは容易ではありません。数式の解釈や展開、単純化は簡単ではないのです。 しかし、KNOPPIX EduTGに含まれているMaxima(xmaxima)というソフトウエアが数式処理をしてくれますので、この講義で出てきた演算を例につかってみます。

XMaximaは起動後、メニュー→数学演習→xmaximaで起動します。 下半分には簡単な使い方がでます。青い表示の数式をクリックするとその演算が実行されます。 解説文は英語ですが、微積の使い方などはすぐにわかるとおもいます。 ここではそれらは飛ばして、実際に行列演算をやってみます。 参考：Maximaのマニュアル＠狸穴

２次元回転の合成(プチテストより)

プチテストで出題した問題、

Ｑ：
θ1およびθ2で回転させる行列
\vect{R}_1=\Mtt{\cth_1}{-\sth_1}{\sth_1}{\cth_1},~~~\vect{R}_1=\Mtt{\cth_2}{-\sth_2}{\sth_2}{\cth_2}
に対して、
\vect{R}_1\vect{R}_2
を計算し、結果を論じよ。
Ａ：
計算については
&&\Mtt{\cth_1}{-\sth_1}{\sth_1}{\cth_1}\Mtt{\cth_2}{-\sth_2}{\sth_2}{\cth_2}\nonumber\\&=& \Mtt{\cth_1\cth_2-\sth_1\sth_2}{-\cth_1\sth_2-\sth_1\cth_2}{\sth_1\cth_2+\cth_1\sth_2}{\cth_1\cth_2-\sth_1\sth_2}\nonumber\\&=& \Mtt{\cos(\theta_1+\theta_2)}{-\sin(\theta_1+\theta_2)}{\sin(\theta_1+\theta_2)}{\cos(\theta_1+\theta_2)}

の結果を論じることは無理としても、計算だけやらせてみます。

まず、行列を定義します。Maximaでの行列は 

A:matrix([a,b],[c,d]);        行列Aを定義
B:matrix([e,f],[g,h]);        行列Bを定義
A+B;                          A+Bを演算
A.B;                          行列積 AB
A*B;                           注意 これは各要素ごとに乗じる:使えない
という感じで演算します。 注意点は行列積でうっかり＊をつかうと違う結果になることです。「．」をつかいます。

さて、上の例題の通りの定義は

A:matrix([cos(th1),-sin(th1)],[sin(th1),cos(th1)]);
B:matrix([cos(th2),-sin(th2)],[sin(th2),cos(th2)]);
A.B;
です。 実際にXmaximaの上の枠で入力してEnterすると妥当な結果が得られます(最後の「;」に注意。忘れるとちゃんと動かない)。
さて、この結果、積和公式でまとめて欲しいものです。 それを行うには 
TRIGREDUCE(%);
と入力します。「%」は直前の結果を使うという意味です。 手を抜かないでやるならば、「TRIGREDUCE(A.B);」とします(TRIGREDUCEは積和公式などで単純化を試みる)。 ここまでの実行例を下に示します。
回転行列の積

オイラー角の行列

次は、オイラー角の回転行列です。座標変換のページから抜粋。

上記オイラー角での回転を数式で考えてみましょう。

まず、Ｚ軸回りの回転(0→1)の角度をφ(phi, ファイ)として、
^0\vect{R}_1=\left(\begin{array}{rrr}C\phi&-S\phi&0 \\ S\phi&C\phi&0 \\ 0&0&~~1\end{array}\right)
となります。ただし、
C\phi=\cos\phi, ~~~~S\phi=\sin\phi
と置き換えています(この先面倒なので)。
つぎにＸ軸回り(1→2)の角度をθとして、
^1\vect{R}_2=\left(\begin{array}{rrr}1&0&0 \\ 0& ~~C\theta&-S\theta \\ 0&S\theta&C\theta \end{array}\right)
を得ます(同じ置き換えパターン)。最後のＺ軸回り(2→3)の角度をψ(psi, プサイ)として、
^2\vect{R}_3=\left(\begin{array}{rrr}C\psi&-S\psi&0 \\ S\psi&C\psi&0 \\ 0&0&~~1\end{array}\right)
となります。

角度(φ, θ, ψ)で表される回転を総合して得られる回転行列は、
&&^0\vect{R}_3=^0\vect{R}_1~~^1\vect{R}_2~~^2\vect{R}_3 \nonumber\\ &=&\left(\begin{array}{rrr}C\phi&-S\phi&0 \\ S\phi&C\phi&0 \\ 0&0&~~1\end{array}\right) \left(\begin{array}{rrr}1&0&0 \\ 0& ~~C\theta&-S\theta \\ 0&S\theta&C\theta \end{array}\right) \left(\begin{array}{rrr}C\psi&-S\psi&0 \\ S\psi&C\psi&0 \\ 0&0&~~1\end{array}\right) \nonumber\\ &=& \left( \begin{array}{ccc} C\phi & -S\phi C\theta & S\phi S\theta \\ S\phi & C\phi C\theta & -C\phi S\theta \\ 0 & S\theta & C\theta \end{array} \right) \left(\begin{array}{rrr}C\psi&-S\psi&0 \\ S\psi&C\psi&0 \\ 0&0&~~1\end{array}\right )\nonumber\\ &=&\left( \begin{array}{ccc} C\phi C\psi-S\phi C\theta S\psi & -C\phi S\psi-S\phi C\theta C\psi & S\phi S\theta \\ S\phi C\psi+C\phi C\theta S\psi & -S\phi S\psi+C\phi C\theta C\psi& -C\phi S\theta \\ S\theta S\psi & S\theta C\psi & C\theta \end{array}\right)
となります。

これをXMaximaにかけてみます。 
R01:matrix([AC,-AS,0],[AS,AC,0],[0,0,1]);
R12:matrix([1,0,0],[0,BC,-BS],[0,BS,BC]);
R23:matrix([CC, -CS,0],[CS,CC,0],[0,0,1]);
R01.R12.R23;
オイラー角(行列計算)
ただし、簡単にするために、φ、θ、ψをそれぞれ、A,B,Cとおき、しかも、cos(θ)をBCとおいています。 これは小細工で、maximaがこちらの意図とは関係なく、アルファベット順に変数を並べてしまうため、角度順に並ぶように敢えてこういう表記をしました。
結果は、右図のようになりました。良く見比べると、式が一致していることが確認できます。 (xmaximaがあっていたのではなく、このページがあっていた、といったほうがいいかも。 もちろん、xmaximaの使い方が正しかったとも言えます。)

ロール・ピッチ・ヨーの行列(レポート第１回)

次はほとんど同じですが、レポートに出したロールピッチヨーの方です。

Ｑ：
Ｚ-Ｙ-Ｘの順番で回転させるロールピッチヨー角回転変換の変換行列
\vect{R}=\Mss{\Cph}{-\Sph}{~~~~0}{\Sph}{\Cph}{0}{0}{0}{1} \Mss{\Cth}{~~0}{~~\Sth}{0}{1}{0}{-\Sth}{0}{\Cth} \Mss{~1}{0}{0}{0}{~~\Cps}{-\Sps}{0}{\Sps}{\Cps}
を求めよ。

Ａ：
\vect{R}&=&\Mss{\Cph}{-\Sph}{~~~~0}{\Sph}{\Cph}{0}{0}{0}{1} \Mss{\Cth}{~~0}{~~\Sth}{0}{1}{0}{-\Sth}{0}{\Cth} \Mss{~1}{0}{0}{0}{~~\Cps}{-\Sps}{0}{\Sps}{\Cps} \nonumber\\ &=&\Mss{\Cph\Cth}{-\Sph}{\Cph\Sth}{\Sph\Cth}{\Cph}{\Sph\Sth}{-\Sth}{0}{\Cth} \Mss{~1}{0}{0}{0}{~~\Cps}{-\Sps}{0}{\Sps}{\Cps} \nonumber\\ &=&\Mss{\Cph\Cth}{-\Sph\Cps+\Cph\Sth\Sps}{\Sph\Sps+\Cph\Sth\Cps}{\Sph\Cth}{\Cph\Cps+\Sph\Sth\Sps}{-\Cph\Sps+\Sph\Sth\Cps}{-\Sth}{\Cth\Sps}{\Cth\Cps}

検算例：
３行：
(-\Sth)^2+(\Cth\Sps)^2+(\Cth\Cps)^2=(\Sth)^2+(\Cth)^2((\Sps)^2+(\Cps)^2)=1
１行：
&&(\Cph\Cth)^2+(-\Sph\Cps+\Cph\Sth\Sps)^2+(\Sph\Sps+\Cph\Sth\Cps)^2\nonumber\\&=&\Cph^2\Cth^2+\Sph^2\Cps^2+\Cph^2\Sth^2\Sps^2-2\Cph\Sph\Sth\Cps\Sps\nonumber\\ &&+\Sph^2\Sps^2+\Cph^2\Sth^2\Cps^2+2\Cph\Sph\Sth\Cps\Sps \nonumber\\ &=&\Cph^2\Cth^2+\Sph^2(\Cps^2+\Sps^2)+\Cph^2\Sth^2(\Sps^2+\Cps^2)\nonumber\\ &=&\Cph^2(\Cth^2+\Sth^2)+\Sph^2 = 1
２行・３行：
&&(\Sph\Cth)(-\Sth)+(\Cph\Cps+\Sph\Sth\Sps)(\Cth\Sps)+(-\Cph\Sps+\Sph\Sth\Cps)(\Cth\Cps) \nonumber\\ &=& -\Sph\Cth\Sth+\Cph\Cth\Cps\Sps+\Sph\Cth\Sth\Sps^2 - \Cph\Cth\Cps\Sps + \Sph\Cth\Sth\Cps^2 \nonumber\\ &=& -\Sph\Cth\Sth+ \Sph\Cth\Sth(\Sps^2+\Cps^2) = 0

まずは、同じように 
R01:matrix([AC,-AS,0],[AS,AC,0],[0,0,1]);
R12:matrix([BC,0,BS],[0,1,0],[-BS,0,BC]);
R23:matrix([1,0,0],[0,CC,-CS],[0,CS,CC]);
R01.R12.R23;
をしてみます。
ロールピッチヨー(行列計算)
答えはちゃんとあいます。

次に、せっかくなので検算も行ってみます。
検算するには、最終的に１になるか０になるかをチェックしたいので、ACなどの表記ではだめです。 面倒ですが、ちゃんとcos/sinで打ち込みます。 

R01:matrix([cos(th1),-sin(th1),0],[sin(th1),cos(th1),0],[0,0,1]);
R12:matrix([cos(th2),0,sin(th2)],[0,1,0],[-sin(th2),0,cos(th2)]);
R23:matrix([1,0,0],[0,cos(th3),-sin(th3)],[0,sin(th3),cos(th3)]);
R03:R01.R12.R23;
ROW(R03,1).ROW(R03,1);
TRIGSIMP(%);
TRIGSIMP(ROW(R03,1).ROW(R03,2));
結果：
ロールピッチヨー(検算)
ここでは、行列R03をR01,R12,R23の積としてまず定義しています。 結果は前と同じ意味ですが、大きくなり過ぎたので列ごとに分解されて表示されたようです。
次に、ROWで行列の行を抜いてきます。ROW(R03,1)でR03の１行目がとれます。 これを二つとって内積をとると、回転行列なので１になるはずです。 ただ内積をとるとごちゃっと出てきますが、簡略化の処理、TRIGSIMPを使用すると１になります。 異なる行(ここでは１と２)を取ってきて内積をとった場合は０になります。 ROWのかわりに列をとるCOLを使っても、同列なら１、異列なら０になります。
数式処理ソフトは使い方さえ間違わなければ強力です。 ですが、うちまちがったり、ちょっと勘違いしただけで当然結果はかわりますので、過信は禁物です。 必ず妥当性をチェックしながら進みましょう。 また、これに頼ると中間結果に注意を払わなくなってしまいます。やっぱりレポートは手で計算して、最後にこれでチェックするくらいがいいでしょう。

熊谷正朗 [→連絡]
東北学院大学 工学部 機械知能工学科 RDE
［｜　］