R-2．ロボット制御プログラミング

1. 目的

３自由度のロボットを用いて、ロボットの動作の設計、ロボット言語を用いたプログラ

ミングを行い、２自由度のリンクの関節角と手先座標との関係を理解し、指示した座標へ

手先を動かすようにロボットを制御することを体験し、簡単な図形や文字を対象に描画の

最適化について検討する。

2. 事前レポート

4．「ロボットの手先位置の計算」を参考に、手先位置(x,y)が与えられたときにロボ

ットの関節角θ１、θ２を求める式を導きレポートとして実験初回の開始時に提出して

ください。求めるにはアームの 2 本のリンクとアーム基部と手先を結ぶ直線で作られる

3 角形に着目し余弦定理を用いればηとξが求められる。

3. 概要

実験に用いるロボットを

図１に示す。３個のサーボ

モータをアクチュエータ

（駆動装置）として、基部

での旋回、腕の途中での屈

曲、そして手先のペンを上

下する動きを可能にしてい

る。

パソコンで各モータの

動きを制御し手先に付け

たペンや鉛筆などでいろいろな図形や文字を書くプログラムを描かせてみる。

4. ロボットの手先位置の計算

このロボットの機構を真上から見て単純

化したものを図２に示す。基部 O と途中の

肘部 E のモータの回転で手先 H が水平面

内で移動できる機構になっている。

機構をｘ軸に沿ってまっすぐ伸ばした状

態でθ１、θ２の角度を０°（この実験では

角度は deg で扱う)とする。図２の状態では 図２ ロボットの平面座標

図１ ロボット概形

θ２は負の値になっている。この時の手先位置をθ１、θ２から求める式は次のようになる。

)sin(sin)cos(cos

21211

21211

θθθθθθ

++=++=

llyllx

（１）

θ1、θ2 に分けた式にすると

)sincoscos(sinsin)sinsincos(coscos

2121211

2121211

θθθθθθθθθθ

++=−+=

llyllx

（２）

となる。モータの回転角度が分かっている場合は手先位置を式（１）か（２）を用いれば求め

られることになる。

次に、手先の位置(x,y)が決まっている場

合について、θ１、θ２を求めることを考え

てみる。 図３にθ１、θ２を求める際の考

え方を示す。まず最初に手先位置(x,y)の原

点からの距離 r と x 軸からの角度ρを求め

る。次に、ロボットのリンクの長さｌ１、

ｌ２と原点と手先の距離 r から余弦定理を

用いて原点と手先を結ぶ線とリンク OE が

なす角(∠EOH)η、肘の部分の角度(∠OEH)

ξを求める。

ξθηρθ−°=

+=1802

1 （３）

求まったη、ρとξから式（３）のようにθ１、θ２が求まる。

ロボットの関節の値（プログラム上での角度を表す数値［便宜上この実験では robix と

いう単位で呼ぶこととする］）と実際の角度の関係はロボットを組み立てた上で入力した数

値[robix]と分度器で測った関節の角度[°]から変換式を求めること。個々のモータにより

換算の係数が異なるので肩の部分、肘の部分ともそれぞれ測定して換算式を導く必要があ

る。このような座標変換はロボットマニピュレータの制御を行う場合に必ず必要となるも

のなのでロボット工学関連の書籍ではほとんどの本で説明がされているので、ロボットの

手先位置の制御に関して図書館などで調べてどのような方法でロボットの手先位置計算を

行っているかを調べてみること。

5. アクチュエータの動作確認

ここからは実際のロボットの製作、制御の方法を説明する。

最初にサーボモータの制御の仕方を確認する。

・ロボットのケースから制御用基板を取り出してＡＣアダプターの電源を接続する（図４参照）。

この時、赤と黒の線を間違えないようにねじの色に合わせてしっかり止める。ＵＳＢコネクタ

図３ ロボットの平面座標（その２）

ーに近い方が赤（＋）、遠い方が黒（－）であ

る。コードを止めるときにはねじはゆるめる

だけで外さないようにする。

・パソコンのＵＳＢコネクターに接続ケーブ

ルをさして、反対側を基板のコネクターにつ

なぐ。

・ケースからサーボモータを３個出して図４

のモータ出力と示されているブロックのピン

に外側から順にコネクターを接続する。ＵＳ

Ｂコネクターを上側にした場合モータのコネ

クターの黄色い線（延長ケーブルだとオレン

ジの線）を図４のように基板を見た場合に上

になるように、黒い線（延長ケーブルだと茶

色の線）を下にする。逆に差すとサーボモー

タを壊す可能性があるので注意すること。

・パソコンの画面上で Usbor Nexus というア

イコンをダブルクリックして実行する。（基板

を認識するためのソフトである。）これは一度開

いたらそのままにしておく。

・次に左の Usbor Nexway というアイコンをダ

ブルクリックして実行する。このソフトが実際

にモータを制御するプログラムになる。

・ 図 ５ の よ う な ウ ィ ン ド ウ が 開 く の で

Localhost の左の＋マークをクリックすると接

続されている基板のナンバーが表示される。さ

らに、そのナンバーの左の＋をクリックすると

Pod の一覧が表示される（図６参照）のでそれ

らの中から Pod1 をクリックする。

・基板のピンに接続されたモータを制御する画

面（図７）が表示される。この後、ロボットを

駆動するプログラム作成はこの画面を用いて作

っていくことになる。

・図６のウィンドウで上の列の control をク

リックして表示される一覧から Open Teach

図４ モータ制御基板

電源入力 USB コネクタ

モータ出力

図５ モータ制御プログラム画面１

図６ モータ制御プログラム画面２

Mode をクリックすると図８のようなウィンドウが

開き、各モータをパソコンのキーボードで操作する

ことができる。キーボードの左側から縦一列がモー

タ１の操作用のボタンになり、”１”は右回転”Ｑ”

は左回転、”Ａ”、”Ｚ”はそれぞれ右回転、左回転

の微小な動きをさせるボタンになる。モータ２、モ

ータ３についてもキーボードの左側２番目の列、３

番目の列が操作ボタンになるのでモータが反応す

るかどうかを確認する。この時モータの回転軸（白

い歯車の部分）に十字の形をしたプラスチック部

品をはめておくと回転の様子が分かりやすくな

る。また、制御画面の青い窓の部分の下側に各モータの状態が数値

で表示されているのでモータの動きに伴ってこれらの値が変化し

ていることを確認する。ここまでがアクチュエータの動作確認の手

順である。

6. モータ制御プログラム

モータのチェックができたら図７の青い窓の部分で動作のプログラ

ムを作成してみる。

move 1 to 1000

と一行だけ書いて左上の一番左の▽マークをクリックしてみる。そうするとモータ１が動く。

move *1 (to,by) *2;

がモータを動かす命令となる。

*1 の部分でどのモータを動かすかを指定する。複数のモータに同じ動きをさせる場合は”1,2”

のように列挙することができる。モータの指定の後は”to”か”by”で動きの種類を指定する。”

to”はモータの絶対位置を指定する場合に、”by”は現在の角度から相対的にどれだけ動かすか

を指示する場合に用いる。*2 で位置の値を記入する。モータの取り得る位置の値は -1400～

1400[robix]までの範囲なのでこの範囲を超えるような値になるような命令は出さないように

注意すること。動きを何行か並べると 1 行文の動作が終わると次の動作をすることになる。ま

た、1 行でも”;”（セミコロン）で区切って複数の動作を並べることもできる。

次に、複数のモータを同時に違う動きをさせる場合の記述方法を説明する。

move 1 to 1000, 2 to -1000;

と書くとモータ 1 は 1000 の位置へ、モータ 2 はｰ 1000 の位置へ同時に動き出す。注意が必要

なのは

move 1 to 1000; move 2 to -1000;

図７ モータ制御プログラム画面３

図８ モータマニ

ュアル操作画面

と書くと、最初にモータ１だけが動きその動作が終了してからモータ２が動き出すこと

である。プログラムの記述によって動きが変わるのでどのような記述でどのような動きを

するかロボットを組み立てる前にモータ単体の状態でいろいろ試してみる。

7. モータ制御プログラム（マクロ）

この実験に用いているプログラミング言語は繰り返しや、条件判断による分岐ができ

ないシステムになっている。ただ、ひとまとまりの処理の部分をマクロと呼ばれるＣ言語

の関数の様に扱う方法があり、マクロを用いると繰り返しの動作が可能になる。また、決

まったパターンの動作が処理の中で何度も行われる場合などにもマクロを用いると便利で

ある。

マクロの例として

macro penup; move 3 to 0; end; # pen up

のように書かれる。

文法の決まりとして、

macro *1;

・

・

end;

のように書く。最初に macro *1;（*1 はマクロの名前）と書いて、マクロを書き始める

ことを宣言し、名前を定義する。その後は、処理の手順を記述し、マクロの処理が終わっ

たら最後に end;を付ける。処理の途中でマクロの名前が出てくると、そのマクロの部分に

処理が移動する。

マクロの呼び出し方は、マクロの名称をそのまま 1 行の処理として記述する。例えば

先に定義した penup という名前のマクロを呼び出す場合は

・

・

penup;

・

・

のように書く。この時にマクロの処理を何回か繰り返したいときは

penup 5;

のように書くと、その回数だけ（この場合は 5 回）繰り返してくれる。この数値として

０を書くと無限に繰り返すことになる。

例えば

macro rev;

move 1 to 1000, 2 to -1000;

move 1 to -1000, 2 to 1000;

end;

rev 10;

を実行するとモータ 1 とモータ 2 が互いに逆方向に 10 回首振りをする。マクロとそれ

を使った処理の記述の順番は特に決められていないので（C 言語と似ている）自由に書い

て構わない。ただ、

後から見直したり他の人が参考にするために見たときに分かりやすいようにごちゃごち

ゃにはしないように注意する必要がある。

8. ロボットの組立

モータの動かし方が分かったら、AC アダプター

のコンセントを抜き、モータが動かないようにして

から組立に取りかかる。構造部品を取り出してロボ

ットを組み立ててみる。図９の組み立て図を見なが

らロボットを組み立てる。組み立てでねじを締める

ときアルミ部品などがゆがんでしまうほど強く締め

すぎないように注意すること。また、組み立てる前

に各モータを真ん中の０ [robix]の位置にするか±

1400[robix]のどちらかに振り切った状態にしてか

ら組み立てるとロボットの動きが分かりやすくなる。このとき、中間の肘の部分は原点、肘、

ペンが一直線になるような状態にできるようにしながら、できるだけ深く曲げられるように組

図９ ロボット組立図

ワッシャー

リンク

サーボモータ

リンク

リンクホーン

ねじ

図１０ 関節部組立図

み立てると手先が移動できる範囲を広く取ることができる。図１０に関節部の部品の組み立て

方を示す。肘の部分とペンを上下に動かす関節部分は図１０のように組み立てる。

基部の黒い支柱の上のオレンジのパーツは横方向に押すと外れるので、いったん外してモー

タにねじ止めしたうえで支柱にはめ直す。ねじ止めの際に先にリンク用の部材をモータに取り

付けておくことを忘れないようにする。また、このリンクには横に斜めの支えを止めるアルミ

の円柱を通す穴が開いている部材を使うこと。

モータの軸の取り付けなど細かいところの組立て方はセットの中の説明書に書かれているの

でそれも参考にして組み立てること。図９のように組み立てると基部のモータと中間のモータ

が水平の動きを、手先に近いモータが上下の動きをし、ペンの上げ下げを行うようになってい

る。組立が終わったら、基部のモータを 1 番、中間を 2 番、手先側のモータを 3 番として制御

基板に接続する。（注意：基板とモータを接続し、電源を入れるとモータが突然動くので電源を

入れるときはロボットから離れていること。）

モータの配線までが終わったら、パソコン上のソ

フトを実行して、キーボードからの操作でロボット

を動かして手先の可動範囲を調べる。手先の可動範

囲が狭い場合はモータを一旦外して可動範囲が広く

なるように組み立て直すこと。

組み立てが完成したら、次に座標変換に必要な関

節角 [robix]と実際の関節の角度 [deg]との換算のた

めのパラメータを求めるための計測をする。具体的

には根元の関節はベースに対して図１１のようにま

っすぐ正面に突き出した状態の関節角 [robix]

と -90°回転させた斜線のところまで回転させた場合の関節角

[robix]を調べて実際の関節角 [deg]とコンピュータ上での関節

角[robix]との関係を求める。

次に同様にアームの中間の肘の関節の部分でも同じように実際

の関節角 [deg]とコンピュータ上での関節角 [robix]との関係を

求める(図１２参照)。このとき肘から先の関節の方向は肘の関

節軸となるモータの軸とペン先を結んだ直線となり、アームの

アルミフレームの向きとはずれるので注意すること。

ｃ

例としてロボットに星を書かせるプログラムを示す。こ

れは、ロボットのマニュアルに載っていたものであるが、

図１１ 関節角パラメータの測定(肩)

図１２ 関節角パラメータ

の測定(肘)

実際に作ったロボットに適用しても多分星を描けないので参考にするだけにすること。

maxspd all 12 # low speed,

accdec all 20 # moderate acceleration

macro penup; move 3 to 0; end; # pen up

macro pendown; move 3 to -375; end; # pen down

macro start; move 1 to -979, 2 to 1400 end

macro star;

start;

move 1 to -499, 2 to 1261; move 1 to -154 , 2 to 1240

move 1 to -226, 2 to 937;

move 1 to 173, 2 to 286, 3 by -50 # increase pen pressure slightly

move 1 to -298, 2 to 739; move 1 to -106 , 2 to 202

move 1 to -532, 2 to 796

move 1 to -859, 2 to 883, 3 by 50 # ease off pen pressure

move 1 to -745, 2 to 1111

end

penup; start # don't make a line moving to start

pendown; star 0 # pen down then cycle continuously through the figure

最初の２行で速度と加速度の最大値を抑える設定をしている。文字を書かせるプログラムを作

る場合にも同じ行を最初に書くこと。このプログラムでは penup, pendown, start, star の４種

類のマクロを作って、実際に実行させる文は最後の２行だけとなっている。star 0 として星を

書かせる部分を何度も繰り返すことで描画を完成させるようになっている。

このプログラムを参考に各々別の文字を書かせるプログラムを作成すること。方眼紙に書か

せたい文字を書いて各線の始点と終点の座標を求め、それをロボットの関節の値に変換する作

業を行いプログラムを作成する。

実際に動かしてみて気付いた点をいくつか上げておくのでプログラム作成の参考にして下さ

い。

・あまり長い直線を両端を指定しただけで描かせようとすると、ぐにゃぐにゃの曲線になる場

合がほとんどなので、途中の経由点を設けてそれらの経由点をたどることで求める直線になる

ようにする。

・軌跡がある点で折れたような線を引く場合、その場所で一旦止まるようにポーズの命

令を入れたほうが良い。

10. 描画の最適化

一筆書きの図形や文字などを描画

する場合はペンを上げることなく描

ききるのが最短の軌跡での描画とな

るが、何本かの線を描かなければな

らない場合は描画する線上の動きだ

けでなく、上げたペンの移動を含め

て効率よく図形を描くことを検討し

てみる。

図１３の「王」の字を例にして考えてみる。正規の筆順では１→２→７→８→３→４→

５→６の順であるが、ロボットに書かせる場合にはペンの運びが左→右、上→下の方向で

なくても構わないため１→２→４→３→５→６→８→７のような動きの方がペンを挙げた

場合の移動距離が短くて済む。各自、５～６画程度の文字を選んで Excel などを利用して

最短経路を求めてみること。また、求めた経路での文字の描画を行うロボット用プログラ

ムを作成すること。このような問題を作業の最適化とよび、手順が多くて複雑な場合には

コンピュータで解析し手作業手順を作成することになる。

10. まとめ

レポートは詳細な内容については実験の際の指示に従ってください。

実験第６週の最終日は R1 のテーマと合同で成果発表会を行います。当日までにパワーポイ

ントでの発表の準備をしておいてください。

図１３ 文字の例