マニピュレータの改造

5.1

はじめに

本研究で提案している打撃制御とスナップ制御に共通して言える事であるが，

振りかぶり期に，アームから正確な初期速度を対象物に与える事ができなければ，その後の指先での速度制御を正確に行う事はできない．そこで本研究室の研究テーマの

つである「皿状エンドエフェクタを持つマニピュレータによる物体の運搬」の研究で実験機として用いられている

リンクマニピュレータを改造し，実機にて検証することにした．まずは指先での制御は考えずに，棒状対象物を手先にセットし

1Link

で投げる機構において，どれぐらいの精度で対象物に目標の初速度を与えることができるかを検証するための，投擲実験を行うことにした．

2.3

でも述べているが，早い段階で実機に触れることにより，実際に実機を使ったことで見つかった問題をシミュレーションや理論構築等にフィードバックできたり，打撃制御とスナップ制御の理論が確立した後，スムーズに実機作製に移行する事ができると考えられる．

5.2

皿状エンドエフェクタを持つマニピュレータとは

現在，一般的に工場などで用いられるマニピュレータでは，作業物体を直接

把持し一つずつ運搬している．しかしもし硬さや形，大きさなどの違う作業物

体を一度に運ぶためにはプレートのようなものの上に乗せて運ばなければなら

ない．例を挙げるとファーストフード店においてマニピュレータを用いた場合

などでは，液体の入った飲料やハンバーガーなどを一度に運ぶために，その飲

料をこぼさないようにまたハンバーガーを落とさないように運ぶ．人間にとっ

てこのような物体をプレートに乗せ運搬することは，大した意識もせずに行う

ことができる．作業物体が滑落または転倒しそうになれば，人間は手首を器用

に使い，それを阻止することができる．しかしマニピュレータでこのような動

作を行うことは簡単ではない．この研究ではエンドエフェクタをプレートにし

その上に作業物体を乗せ，与えられた軌道に沿って作業物体を滑落・転倒させ

ずに運ぶことのできるマニピュレータの開発を行っている．また手首返し動作

を取り入れることにより，より速く，より正確に作業物体を運搬することがで

きるであろうと考え，鉛直面内におけるマニピュレータの軌道追従制御につい

ての研究を行っている．この研究で実験機として用いられているマニピュレー

タを

Fig.5-1

に示す．

Fig.5-1

マニピュレータ実験機

5.3

マニピュレータの運動方程式

Fig.5-2

の様に設定したリンクパラメータをもとにラグランジュ法を用いて運

動方程式を誘導した．以下に求めた運動方程式を示す．

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡ +

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

+ +

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡ +

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

⎥⎥

⎥

⎦

⎤

⎢⎢

⎢

⎣

⎡

3 2 1

3 2 3 1 2 3

2 2 2 1

2 1

32 31

22 21

12 11

3 2 1

33 32 31

23 22 21

13 12 11

3 2 1

2 2

2 0

0 0

G G G H

H H

H H M

M M

M M M

θ θ θ θ θ

θ θ θ

θ θ

θ θ τ

τ τ

(5-1)

ここで

3 2 1 3 2 3 1 3 3 2 2 2 1 2 3 2 2 2 1 3

2 2 1 2 2 2 1 2 2 1 1 11

)) cos(

2 cos 2

cos 2 (

) cos 2

(

I I I l

l l

l l l l m

l l l l m l m M

g g

+ + + + +

+ +

+ + +

θ θ θ

θ θ

3 2

3 2 3 1 3 3 2 2 2 1 2 3 2 2 3 2 2 1 2 2 2

12 ( cos ) ( cos 2 cos cos( )

I I

l l l

l l

l l l m l

l l m

M _g _g _g _g _g

+ +

= θ θ θ θ θ

3 3 2 3 1 3 3 2 2 3 3

13 m (l l l cos ll cos( )) I

M = _g + _g θ + _g θ +θ +

21 M

M =

3 2 3 3 2 2 3 2 2 3 2 2 2

22 m l m (l l 2l l cos ) I I

M = _g + + _g + _g θ + +

3 3 3 2 2 3 3

23 m (l l l cos ) I

M = _g + _g θ +

31 M

M =

32 M

M =

3 2 3 3

33 m l I

M = _g +

)) sin(

sin sin

( ₂₁ ₂ ₂ ₃₁ ₂ ₂ ₃₁ ₃ ₂ ₃

11 =− m l l_g θ +m ll θ +m l l_g θ +θ

)) sin(

sin

( ₃ ₂ ₃ ₃ ₃ ₁ ₃ ₂ ₃

12 =− m l l_g θ +m l l_g θ +θ

21 H

H =−

) sin ( ₃ ₂ ₃ ₃

22 m l l_g θ

H =−

31 H

H =−

32 H

H =−

)) sin(

) sin(

sin (

)) sin(

sin ( sin

3 2 1 3 2 1 2 1 1 3

2 1 2 1 1 2 1 1 1 1

θ θ θ θ

θ θ

θ θ θ

+ + +

+ +

g g g

l l

l g m

l l

g m gl

m G

)) sin(

) sin(

( )

sin( ₁ ₂ ₃ ₂ ₁ ₂ ₃ ₁ ₂ ₃

2 2

2 =m gl_g θ +θ +m g l θ +θ +l_g θ +θ +θ

) sin( ₁ ₂ ₃

3 3

3 =m gl_g θ +θ +θ

θ 3

X

l

1 Link

Link3

, I

m

l 1

l

θ 2

Y

θ 1

l

2 Link

1 1

, I m

θ 3 ₃

θ

X

l

g_g₁

l

g_g₃

l 1

Link 1 Link

Link3 Link3

, I

m

₂

, I

₂

m

l 1 ₁

l

2₂

l

θ 2 ₂

θ

Y

θ 1 ₁

θ

l

g_g₂

l

2 Link 2 Link

1 1

, I m

₁

, I

₁

m

：重力加速度

：リンクｉの長さ

lgi

：関節ｉからリンクｉの質量中心までの長さ

θi

_{：関節ｉの回転角度}

θdi

：関節ｉの回転角度の目標値

θ&i

：関節ｉの角速度

θ&&i

：関節ｉの角加速度

：リンクｉの質量

：リンクｉの質量中心を通りＺ軸に平行な軸まわりの慣性モーメント

τi

：各関節にかかるトルク

Fig.5-2

マニピュレータの解析モデル

ここで求めたマニピュレータの運動方程式を用いることにより，ある軌道を目標軌道とするマニピュレータの追従シミュレーション実験を行うことができ，

この実験より各関節にかかるトルクを求めることができた．またこの実験により，実験装置を製作するときに各関節に用いるモータの選定にも役立つと考えることが出来た．しかし実際に実験装置を製作するときには，各関節に働く摩擦力を考慮する必要がある．実際にマニピュレータを動作させた場合の入力トルクに近づかせるためには，この摩擦力を補償する項を付加する必要性がある．

よって今後，運動方程式に各関節に働く摩擦力を補償する項を付加する必要があることが分かった．

5.4

制御システム

制御用のコンピュータとして

NEC

社製

PC-9821Xb(CPU: Intel Pentium

100MHz)

を用いることにした．本実験で用いる制御システムのブロック線図を

Fig.5-3

に示す．まず軌道データをコンピュータに入力し，コンピュータからの

指令電圧は

D/A

変換機を用いてアナログ量に変換する．それからパワーアンプをとおして

サーボモータを駆動させ，マニピュレータ本体を動作させる．またマニピュレータの各関節角度はモータ直結のロータリーエンコーダより検出し，エンコーダカウンタを通してデジタル信号に変換しコンピュータに情報を転送するシステムになっている．

Trajectory

Data Joint

Angle AMP

サーボランド SMCM-6AI SMCM-10AI

Manipulator

A/D Converter

MICROSCIENCE ADM1698BPC

Encoder Counter

MICROSCIENCE UDC4298CPC

Computer

NEC PC-9821Xb

D/A Converter

MICROSCIENCE QDA-2998BPC

Encoder

Acceleration Sensor

CROSSBOW CXL04LP3

DC Servo motor

山洋電気

Fig.5-3

マニピュレータの制御システム

5.5

初期設定

マニピュレータ実験機の質量，重心位置などのパラメータについて

Table.5-1

に示す．

Table.5-1 パラメータ

Link1 Link2

リンク長

[m] 0.314 0.300

重心位置

[m] 0.143 0.128

質量

[kg] 1.27 2.06

つぎにフィードバックゲインを

Table.5-2

に示す．フィードバックゲインの選定

には

Ziegler

−

Nichols

の経験的方法

[2]

を参考にし，この方法の中の限界感度法を

用いた．

Table.5-2

フィードバックゲイン

第

関節第

関節

位置ゲイン

Kp 40.80 26.21 0.32

速度ゲイン

Kd 0.919 0.574 0.007

積分ゲイン

Ki 240.0 159.5 3.8

5.6

軌道生成

ロボットマニピュレータの経路の設計は，一般に最短経路や障害物回避等の条件をもとに設計される．また，マニピュレータの軌道制御はダイナミクス

(

動力学)を考慮する必要があり，一定速度で移動するにしても経路が十分に滑らかでないと不要な振動が生じたりする．今回は変位θと時間

の

次関数によって経路の生成を行うこととした．その関数を以下の式に示す．

次関数のそれぞれの次数の係数は，位置，速度，加速度による境界条件を与えることによって決定した．

0 2 1

3 2 4 3 5 4 5

1(t) a t a t a t a t a t a

θ = + + + + + (5-2)

の関数を用いて軌道を生成することとし，

θ₁ =

(5-3)

A θ

T⁻¹ ₁ =

(5-4)

から係数行列

を決定した．以下にそれぞれの行列式について示す．

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

) (

) 0 (

) (

) 0 (

) (

) 0 (

1 1 1 1 1 1

n θ θ

θ1

(5-5)

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

0 1 2 3 4 5

a a a a a a

(5-6)

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

0 0 2 6 12 20

0 0 2 0 0 0

0 1 2 3 4 5

0 1 0 0 0 0

1 1 0 0 0 0 0

1 2 3

1 2 3 4

1 2 3 4 5

n n n

n n n n

n n n n n

t t t

t t t t

t t t t t

(5-7)

Fig.5-2

において

Link1

がθ

₁=0rad

で静止した状態から

秒間で終端位置θ

₁=

π

rad

に静止した状態に移す目標角軌道を

次の時間多項式を用いて生成した．また目標角軌道を時間

で微分し，目標角速度軌道を求めた．求めた目標軌道関数を式(5-8)，(5-9)に，軌道を

Fig.5-4

，Fig.5-5 に示す．

3 4

1(t) 0.58875t5 2.94375t 3.925t

θ = − + (5-8)

2 3

1(t) 2.94375t4 11.775t 11.775t

θ& = − + (5-9)

0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5

0 0.5 1 1.5 2

時間[s]

Link1の角度[rad]

Fig.5-4

目標角軌道

0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5

0 0.5 1 1.5 2

時間[s]

Link1の角速度[rad/s]

Fig.5-5

角速度軌道

5.7

制御方法

実験機における追従制御実験に用いる制御方法について述べる．今回はロボット制御法として代表的な

制御による追従制御実験を行うことにした．目標軌道としては

5.6

で生成したものを用いることとする．よってここではこの制御法について簡単な説明をする．

制御

目標角度

θ_d

が実際の角度

と比較され，その角度の差に位置ゲインが乗じてアクチュエータへの出力が生成される．安定度を与えるために式

(5-10)

の様に実際の速度に速度ゲインを乗じた減衰項が出力に加えられる．

θ θ

τ =K_p( _d − )−K_d ^&

(5-10)

また式(5-10)の場合，速度の最も速い期間中，特に必要の無いところに大きな減衰が存在してしまうので，これを補償するために式

(5-11)

のような目標角速度をもつ

制御が提案される．

) ( )

(θ θ θ θ

τ =K_p _d − −K_d ^&_d − ^& (5-11)

この制御系は，ロボットのモデル（動特性）を含まず，各関節に独立な

入力

出力系として扱っているので，機構が簡単であり，現在多くの産業ロボットのほとんどはこの

制御を用いている．本研究でもこの制御を用いて実験を行うことにする．Fig.5-6 にその使用する

制御のブロック線図を示す．

θｄ −

＋

θｄ ⁻

Kd Kp

＋ τ

R

θ θ

Fig.5-6PD

制御

5.8

追従実験

5.6

で生成した軌道を目標軌道とし，

制御による追従実験を実機にて行った．

なお本研究ではまず

1Link

のみで投擲実験を行うこととしているので，マニピュ

レータの

Link1

のみを使用して軌道追従実験を行った．実験結果を

Fig.5-7，

Fig.5-8

に示す．

-0.5 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5

0 0.5 1 1.5 2

時間[s]

Link1の角度[rad]

目標値実験値

Fig.5-7

目標角度と実験値の比較

-0.5 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5

0 0.5 1 1.5 2

時間[s]

Link1の角速度[rad/s]

目標値実験値

Fig.5-8

目標角速度と実験値の比較

実験の結果，この軌道ではほぼ目標軌道に追従してアームを動かすことができ

た．しかし，投擲実験ではさらに早いスピードでアームを回転させる必要があ

るので今後は実際の投打と同じ速さの軌道を生成し，それに正確に追従するこ

とが出来るかどうかを検証する必要がある．

5.9

投擲方法と投擲実験用ロボットアームの制御システム