Microsoft PowerPoint - ロボットの運動制御forUpload'C6H [互換モード]

ロボ トの運動制御

ロボ トの運動制御

ロボットの運動制御

ロボットの運動制御

ロボットの行動計画・軌道計画・軌道制御

産業用ロボットの制御方式 －PTP制御とCP制御ー

ロボット作業のプログラミング

ッ 作業 プ グラミング

動作コマンドと補間制御

動力学

ラグランジ 法による運動方程式の導出

動力学 －ラグランジュ法による運動方程式の導出－

位置・軌道制御 －計算トルク法とＰＤフィードバックー

力制御 －インピーダンス制御ー

行動計画・軌道計画・軌道制御

行動計画（作業計画）・動作計画

何を行うかを決めること．『ゴロを捕球して１塁へ送球する』，『コンベヤ上を流れ るボルトを掴んで部品Aを固定する』のような複雑な行動(作業)内容を決めること るボルトを掴んで部品Aを固定する』のような複雑な行動(作業)内容を決めること から，『転がるボールの1.2秒後の位置へハンド先端を移動させる』，『把持した ボルトをネジ穴へ挿入する』程度の動作を決めるレベル，更に単純な動作を決 めることまで様々な内容の決定．

軌道計画

目的の動作を実現するためにハンド先端（or 各関節角）を時間と共にどのように 目的の動作を実現するためにハンド先端（or 各関節角）を時間と共にどのように 変化させるべきかを決めること．ボール位置(あるいはネジ穴へ)にどんな速度で どんな軌跡を描いてハンド先端を移動させるか決めること．ロボットマニピュレー タが関節を

_N

個も れば

θ ( ) 0≦ ≦T (T

動作終了時刻 タが関節を

_N

個もっていれば，

θ

_k

(t), 0≦t ≦T (T :

動作終了時刻)

,

k

=1,2,…

N

を決めることになる．

軌道制御

軌道制御

計画された軌道

θ

_k

(t), 0≦t ≦T, k

=1,2,…

N

を実現するためにアクチュエータ をどのように制御するか．各関節がそれぞれ1つずつのアクチュエータで制御さ れているとすれば，

_N

個の制御入力

_u

(

t

),

k

=1,2,…

N

を決めなければならない． PID制御，最適制御，ニューロ制御，ファジィ制御など･･･

産業用ロボットの制御方式

操作者

PT P

制御

_{(Point To Point Control)}

終点のみを指定して点から点へ移動する制御

操作者

T

T

(

x y

_T

_T

)

目標点座標

( , )

x y

T T

(

θ θ

)

目標点座標 目標関節角 逆運動学 T

( ,

x y

T T

)

P

S 2

θ

目標点（終点） T T 1 2

( ,

θ θ

)

目標関節角 S T 1 _{1 1} S T 2

θ θ

θ

θ θ

θ

Δ =

−

Δ =

メモリ S S S

( , )

x y

P

T 2

θ

T

θ

S 1

θ

現在位置 1 2

(

Δ Δ

θ θ

,

)

目標駆動角 S T 2 _{2 2}

θ θ

θ

Δ =

−

制御則（ 台形則） T 1

θ

現在位置 （始点） 1 1 1 2 2 2

( )

( ,

)

( )

t

t

f t

f t

(

)

θ

θ

τ

τ

=

=

Δ

Δ

制御則（ex. 台形則） 関節トルク T T

(

θ θ

)

関節角座標系 ベース 2

( )

t

f t

2

( ,

θ

2

)

τ

=

Δ

(

( ))

V t

t

モータ特性 T T 1 2

( ,

θ θ

)

(順)運動学

_逆運動学

関節角座標系 (

Joint

座標系) 1 1 2 2

( , ( ))

( , ( ))

V t

t

V t

t

τ

τ

モータ印加電圧

T

T

( , )

x y

(

)

_逆運動学

作業座標系 (Base座標系/World座標系)

PTP

制御と

CP

制御

P

_C

_C

oint

_ontinuous

_ontinuous

T

o

P

_P

_P

oint Control

_{ath Control}

_{ath Control}

S

P

P

T S

P

T

P

S

直線

補間

関節角

補間

直線

補間

円弧

補間

関節角補間の

PTP

制御

直線補間の実際

円弧

補間

S

P

T

P

S

P

T

P

1

P

_P

2 ₃ 関節角補間の

_PTP

制御 1 M

P

2 M

P

3 M

P

4 M

P

中間目標点

数

msec

数

msec

_数

msec

移動距離の短い関節角補間

PTP

制御を

繰返すことで擬似的な直線補間を実現

演習１ 関節角補間によるマニピュレータ手先軌道

1 ₄ 2 ₂

( )

( )

, 0

2

( )

t

t

t

t

t

t









 







_

_



_{ }

 



  

_{ }

θ

1

( )

t



2

( )

t



初期姿勢

初期姿勢からリンク角 θ が次式にしたがって変化する時，手先位置はどのように 移動するかその軌跡を示せ．ただしリンク

1, 2

の長さをそれぞれ

3, 2

とする． （

_t

=0.4, 0.8, 1.2, 1.6, 2.0

の時の手先位置を計算・プロットして結ぶ）

演習１ マニピュレータ手先軌道の計算 答え

3 1 1 2 _{4 4} 3 1 1 2 _{4 4}

3cos

2cos

( )

3cos

2cos(

)

, 0

2

3sin

2sin

( )

3sin

2sin(

)

t

t

x t

t

t

t

y t

   



 



 













 

_



_

_{ }







 

_

_



_



 

 

_



_

1( )t  2( )t  1( )t  3 10 10 3 10 10 3cos 2cos 3sin 2sin

(0.4)

4.029

(0.4)

2.545

x

y

     









_

_





















_

_





3 5 5 3 5 5 3cos 2cos 3sin 2sin

(0.8)

1.809

(0.8)

3.666

x

y

     









_

_





















_

_





3 9 10 10 3 9 10 10 3cos 2cos 3sin 2sin

(1.2)

0.139

(1.2)

3.045

x

y

     











_

_





















_

_





2 6 5 5 2 6 5 5 3cos 2cos 3sin 2sin

(1.6)

0.691

(1.6)

1.678

x

y

     











_

_





















_

_





3 2 2 3 2 2 3cos 2cos 3sin 2sin

(2.0)

0

(2.0)

1

x

y

     









 



_

_







 





_

_

 

ロボット言語コマンド

三菱電機

_RV-M2

_の例 コマンド (読み方)

_機能

NT

ロボットの原点出しを行う

NT

NW (NeW)

プログラム及びポジションを全て消去する

SP (Speed)

ロボットの動作速度を設定する

SP (Speed)

ボットの動作速度を設定する

TL (TooL)

X-Y平面内のツールの長さを設定する

MJ (Move Joint)

各関節を指定した角度だけ回転させる

5

自由度 垂直多関節

MJ (Move Joint)

各関節を指定した角度だけ回転させる

TI (TIme)

指定した時間だけ動作を停止する

PD

指定したポジションの座標デ タを設定する ロボットマニ ピュレータ RV-M2

PD

(Position Define) 指定したポジションの座標データを設定する

MO (MOve)

PDで指定したポジションに関節補間で移動させる グ プを開く

GO (Grip Open)

グリップを開く

GC (Grip Close)

グリップを閉じる

DW

指定した距離だけ関節補間で移動する

ED (EnD)

プログラムを終了する

作業プログラム例

１

各関節を指定した_{ハンド先端を}

_順次移動

駆動角

_{させるプログラム}だけ回転させて， #include<iostream.h> #include<stdio.h> #i l d ”RS h”

NW

メモリのクリア

SP 15

動作速度の設定 （

20

段階

15

） #include”RS_com.h”

int main(int arge, char *argv[]) { RS Com rs;

NW

（

20

段階の

15

） RS_Com rs; rs.RS_Open(); rs.Send(“NW¥n”); rs Send(“SP 15¥n”);

SP 15

MJ 33.4, 23.5,-48.6,82.1,0

rs.Send( SP 15¥n ); //////////////////////////////////////////////////////////////////// rs.Send(“MJ 33.4,23.5,-48.6,82.1,0¥n”); rs Send(“MJ 14 2 27 5 36 2 8 9 0¥n”);

MJ 14.2, 27.5, 36.2, 8.9,0

MJ 25.2,-15.8, 12.4, 3.3,0

rs.Send( MJ 14.2,27.5,36.2,8.9,0¥n ); rs.Send(“MJ 25.2,-15.8,12.4,3.3,0¥n”); //////////////////////////////////////////////////////////////////// rs.Flush();

MJ

コマンドで

5

関節をそれぞれ 14.2 25.2 33.4 ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ (); rs.RS_Close(); return(0); } 1 2 3 27.5 15.8 36.2 12.4 8.9 3.3 , , 23.5 48.6 82.1 − ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ _{⎜ ⎟ ⎜ ⎟} ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ = = = ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ −

Δ

θ

Δ

θ

Δ

θ

} 0 0 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ _{⎝ ⎠ ⎝ ⎠} ⎝ ⎠ だけ回転させる． プログラムを_{転送して実行させる}

PC

からロボットコントローラへ

_C

_{言語プログラム}

作業プログラム例２

予め位置座標を指定しておき，ハンド先端を 順次それらの点へ

直線補間

で移動させる

NW

メモリのクリア #include<iostream.h> #include<stdio.h> #include”RS_com.h”

NW

メモリのクリア

SP 15

動作速度の設定

TL 223

ツール長(223mm)設定 _

int main(int arge, char *argv[]) { RS_Com rs;

NW

SP 15

TL 223

ツ ル長(223mm)設定 rs.RS_Open(); rs.Send(“NW¥n”); rs.Send(“SP 15¥n”);

SP 15

TL 223

PD 1 33 4 23 5 -48 6 82 1 0

_{rs.Send(“TL 223¥n”);} //////////////////////////////////////////////////////////////////// rs.Send(“PD 1,33.4,23.5,-48.6,82.1,0¥n”); S d(“PD 2 14 2 27 5 36 2 8 9 0¥ ”)

PD 1,33.4, 23.5,-48.6,82.1,0

PD 2,14.2, 27.5, 36.2, 8.9,0

MC 1 2

_{rs.Send(“PD 2, 14.2,27.5,36.2,8.9,0¥n”);} rs.Send(“MC 1,2¥n”); //////////////////////////////////////////////////////////////////// rs Flush();

MC 1,2

MC

コマンドで

5

関節を順次 ⎛ ⎞ ⎛ ⎞ rs.Flush(); rs.RS_Close(); return(0); } 1 2 14.2 27.5 36 2 , 33.4 23.5 48 6 ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ _{⎜ ⎟} ⎜ ⎟ ⎜ ⎟ = − =

θ

θ

36.2 _} 8.9 0 , 48.6 82.1 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ _{⎜ ⎟} ⎜ ⎟ ⎜ ⎟ _{⎝ ⎠} ⎝ ⎠

θ

θ

の姿勢にする． プログラムをPCからロボットコントローラへ 転送して実行させる_{C言語プログラム}

補間制御コマンド

オムロン

_ORL-2

_MOVE

と

_PATH

の例

MOVE L

: ロボットの手先を現在位置から目標点P1へ直線補間 で移動させる(目標点で停止する)

MOVE C

: ロボットの手先を現在位置から中間点P1を通って，

P3

P2

P0

OV C

: ボットの手先を現在位置から中間点 を通 て， 目標点P2へ円弧補間で移動させる(目標点で停止)

PATH L

: ロボットの手先を目標点P1の近傍を通過するように 直線補間で移動させる(目標点で停止しない)

P1

P2

P0

直線補間で移動させる(目標点で停止しない)

PATH C

: ロボットの手先を中間点P1を通って，目標点P2の近傍を 通過するよう円弧補間で移動させる(目標点で停止しない) 円を描かせるには？

P2

現在位置 MOVE C, P1, P2

P1

MOVE L, P1

P1

P1

PATH L P1 PATH C P1 P2

P3

現在位置

P2

PATH L, P1 MOVE P2

P1

3

PATH C, P1, P2 MOVE P3

P2

現在位置 現在位置

P2

CP制御の軌道生成

時々刻々の

目標

ハンド先端

軌道

(

)

r

( )

t

, ( ), ( )

x

r

t

x

r

t

x

_&

_&&

時々刻々

逆運動学

を解く

時々刻々の

目標

関節角

軌道

(

)

r

( )

t

, ( ), ( )

θ

r

t

θ

r

t

θ

&

&&

Λ：

運動学関数

x

Λ

( )

θ

(

)

1 mr r r 1

( )

( )

( )

( )

( )

( )

t

N

t

N

t

t

N

t

N

t

−

=

=

θ

θ

Λ

x

θ

&

θ

&

J

_&

( )

J θ&

( )

( )

Λ：

運動学関数 1 1 2 1 2 1 2 1 1 2 1 2 ( ) ( ) cos cos ( , ) sin sin x l l y l l θ θ θ θ θ θ θ θ + + + ⎡ ⎤_{= =}⎡ ⎤ ⎢ ⎥ ⎢ ₊ ⎥ ⎣ ⎦ Λ ⎣ ⎦

( )

= θ

Λ

x

(

)

1 mr r r 1 mr r r r

( )

( )

( )

( )

( )

( )

( )

t

N

t

N

t

t

N

t

N

t

t

− −

=

=

=

=

−

θ

θ

J x

θ

θ

J

x

Jθ

&

&&

&&

_&&

&

&

r

( )

t

=

( )

t

r

( )

t

x

_&

J θ

_ヤコビ

_行列

_の定義 両辺を微分

(

)

mr

( )

r

( )

r

( )

r

( )

r

( )

t

=

r

( )

t

+

r

( )

t

x

Jθ

&

&

Jθ

&&

&&

CP動作はPTP動作より

遅い

CP動作はPTP動作より

遅い

1 r

( )

t

=

− r

( )

t

θ

&&

J x

_&&

=

J 0

&

（除 特異点）

ヤコビ行列を用いた逆運動学の解法

ヤコビ行列を用いた逆運動学の解法

逆運動学を解くのは容易ではない．リンク間にオフセットを付けると 解が求まらないことも多い 解が求まらないことも多い．

ニュートン・ラフソン法

に基づく，逆ヤコビ行列を用いた

逆運動学

解法

逆運動学の解法

ニュートン・ラフソン法の繰り返し計算

( )(

)

1

1

r

_i+

=

r

_i

+

−

r

_i

r

_i

−

r

,

i

=

1,2,

θ

θ

KJ

θ

x

x

_L

グゲイ

ニュ トン・ラフソン法の繰り返し計算

K: チューニングゲイン

収束値

θ

_r∞

を逆運動学の解

θ

_r

として採用

繰り返し不要 ← 現在位置と目標位置が極めて近い

( )(

)

1

1

r

_i+

= +

i

−

r

_i

r

_i

−

i

θ

θ

J

θ

x

x

ロボットの動力学

ロボットの

運動エネルギー

T

と

位置エネルギー

U

2 2 1 1 1 _T 1 _T

m v

+

I

ω

m

v v

+

ω Iω

n T

mgh

⇒

∑

m

g r

ラグランジュ

方程式

2 2 2 2 2 2 T T

m v

+

I

ω

⇒

m

v v

+

ω Iω

0, 1 i Ci i

mgh

m

=

⇒

∑

g r

(

T U

)

(

T U

)

d

⎛

_⎜

∂

−

⎞

_⎟

∂

−

₌

_τ

による導出

ロボ トの動力学 ロボ トの動特性を定める

運動方程式

(

)

(

)

i i i

q

q

d

dt

⎜

_⎝

∂

_&

⎟

_⎠

−

∂

=

τ

ロボットの動力学：ロボットの動特性を定める

運動方程式

( )

( , )

( )

H q q + h q q + G q = τ

_&&

_&

ロボット制御

ボッ 制御

1.

軌跡制御

₍

軌道制御

_{): ロボット(の手先)を望ましい}

軌跡に

追従させる

ような制御

2.

力制御

_{: ロボットの手先が作業対象物に対して望ましい}

力

を加える

ような制御

Lagrange

方程式の考え方

⎛

⎞

x

mx

&&

= −

f

mg

(

)

(

)

d

T U

T U

dt

x

x

f

∂ −

∂ −

⎛

_{⎞ −}

⎜

_∂

⎟

_∂

⎝

&

⎠

=

から

を導く

2 2

1

1

2

2

T

=

mv

=

mx

_&

f

v

∂ −

(

T U

)

_{= −}

_mg

∂

2

2

0,

T

T

mx

∂

∂

=

=

∂

∂

_&

&

m

g

g

x

∂

(

T U

)

_mx

∂ −

=

_&

U

=

mgx

x

x

∂

∂

_∂

_x

_&

mx

(

)

d

_⎛

∂ −

T U

_{⎞ =}

_mx

⎜

⎟

&&

U

=

mgx

,

0

U

U

mg

∂

₌

∂

₌

∂

∂

dt

⎜

_⎝

∂

x

_&

⎟

_⎠

=

mx

,

g

x

x

∂

_∂&

⎛

⎞

O

(

)

(

)

d

T U

T U

dt

x

x

mx mg

f

∂ −

∂ −

⎛

_{⎞ −}

₌

⎜

_∂

⎟

_∂

⎝

&

⎠

&&

+

=

運動エネルギーと位置エネルギー

h

運動エネルギーと位置エネルギー ⇒ 運動方程式

2

1

_T

1

⎧

mgh

(

)

x T v ⎛ ⎞ ⎜ ⎟ 2 2

1

1

2

2

1

1

T T

m

m v

I

ω

⎧

_⇐

⎪⎪

⎨

⎪

_{ω Iω}

_⇐

v v

並 進 運 動 エ ネ ル ギ ー 回 転 運 動 エ ネ ル ギ

(

)

T x y z y z v v v v v ⎜ ⎟ = _{⎜ ⎟} ⎜ ⎟ ⎝ ⎠

v

=

2

2

2

T

_I

ω

⎪

_⇐

⎪⎩

回 転 運 動 エ ネ ル ギ ー

ω Iω

2 Inertia V ρr dV =

∫

v

=

ロボット全体の運動エネルギー

I

: 慣性モーメント

ロボット全体の運動エネルギ

(

)

1 ( ) ( ) ( ) ( )

1

2

n i i T i i T i T T i L L A i A

T

m

=

=

∑

q J

_&

J

q

_&

+

q J

_&

I J

q

_&

1 2 1 2 ( ) ( ) ( ) 1

,

_{C i L}i i _{i A}i i L

θ

θ

θ

θ

=

⎛

⎞

=

_⎜

_⎟

→

⎝

⎛

⎞

=

=

_⎜

_⎟

⎝

⎠

⎠

J

=

J

q

q

v

J

q

&

ω

&

&

&

&

&

&

並進速度ベクトル 2 回転速度ベクトル 2

1

2

:

T

⎝

⎠

⎝

⎠

=

q

&

H

q

&

H

慣 性 行 列

並進速度ベクトル 回転速度ベクトル

ロボット全体の位置エネルギー

_0, 1 n T i Ci i

U

m

=

=

∑

g r

Lagrange

方程式による運動方程式の導出

(

)

(

)

_,

_{1, 2,}

_,

i i i

T

U

T

U

d

i

n

dt

q

q

τ

⎛

∂

−

⎞

∂

−

−

=

=

⎜

_∂

⎟

_∂

⎝

&

i

⎠

i

L

dt

_⎝

∂

q

_⎠

∂

q

,

:

i

L

d

L

L

d

t q

q

τ

=

T

−

U

⎛

∂

∂

⎞

−

=

⎜

_∂

_∂

⎟

⎝

&

ラ グ ラ ン ジ ュ 関 数

⎠

0 , 1

1

,

2

i n T T i C i

T

U

m

=

=

q H q

_&

_&

=

∑

g r

i i

dt q

∂

∂

q

⎝

⎠

1

2

_i= 1 1 1

,

1, 2,

,

n n n ij j ijk k j i i j j k

H q

+

h q q

+

G

=

τ

i

=

n

∑

&&

∑ ∑

& &

L

(

7.16

)

1 1 1 j= j= k =

(

_mx

₊

₍

_dx

2

₎

₊

_mg

₌

_f

)

&&

&

に 対 応

慣性

力

遠心力・コリオリ力

重

力

外

力

(

(

)

g

f

)

( ) 1

1

,

2

n ij jk T j ijk i j Li j k i

H

H

h

G

m

q

q

₌

∂

∂

=

−

=

∂

∂

∑

g J

ただし

( )

( , )

( )

H q q + h q q + G q = τ

_&&

_&

ベクトル・行列で １つにまとめると

ロボットの位置・軌道制御

ロボット

₍

手先位置

_)がある

軌跡

p

_d

(t)

_{= ( x}

_d

(t)

_{, y}

_d

(t)

_{, z}

_d

(t)

₎

T

に

追従

するような制御系の構成法

追従

するような制御系の構成法

手先方向も追従させる場合 ⇒ 軌跡として

q (t)

= ( x (t) y (t) z (t) θ (t) θ (t) θ (t))T を考える

•

基準座標系

上で表現された手先位置

₍

p

_{=(x, y, z)}

T

_ג

R

3

_{) や}

⇒ 軌跡として

q

_d

(t)

= ( x_d(t), y_d(t), z_d(t), θ_x(t), θ_y(t), θ_z(t)) を考える

関節座標系

(関節角ベクトルθ

R

N

)をもとに構成する方法

p

姿勢をもとに構成する方法

•

関節座標系

(関節角ベクトルθ

ג

R

N

)をもとに構成する方法

逆運動

学

目標

手先

軌跡

目標

関節角

軌跡

d

( ) ( ( ), ( ), ( ))

t

x t y t z t

d d d

→

( ) ( ( ),

d

t

θ

1d

t

θ

2d

( ),

t

θ

3d

( )

t

θ

Nd

( ))

t

p

=

θ

=

_L

逆運動

学

目標

手先

軌跡

目標

関節角

軌跡

動力学モデル

運動学モデル

( )

( , )

( )

H θ θ + h θ θ + G θ = τ

&&

&

動力学モデル

運動学モデル

ロボットの軌跡制御

( )

( , )

( )

H q q + h q q + G q = τ

_&&

_&

_非線形

_システム

Mq + Dq + Kq = τ

_&&

_&

線形システム

_{(機械振動系)→制御しやすい}

線形システムへ変換

トルク計算

法

線形システムへ変換

線形システムを得るための予備的な非線形フィードバック

ˆ

ˆ

ˆ

ˆ

ˆ

ˆ ˆ

ˆ

_{( )}

_{( , )}

_{( ) (}

ˆ

₎

H

H h h G G

⇓

τ = H q u + h q q + G q

_&

H,h,G

は

H,h,G

の推定値

,

,

ˆ

ˆ

ˆ

H

H h h G G

Hq h G Hu h G Hu h G

⇓

≈

≈

≈

+ + =

+ + ≈

+ +

&&

　　

( )( - )

∴ H q q u = 0

Iq + Oq + Oq = u

q = u

q = u

&&

&

&&

&&

&&

は

特殊な形をした線形システム

⇒

Iq + Oq + Oq = u

q u

は

特殊な形をした線形システム

線形システムに対する制御理

　　　

論を使える

PDフィードバック制御

&&

「

q u

&&

=

線

ドバ

d d d

( )

t

⇒

( )

t

⇒

( )

t

=

( )

t

⇒

( )

t

=

( )

t

q

q

u

q

_&&

q

_&&

q

_&&

所望の

「

」に対して線形フィードバック制御を行う

desired

_{理想的な加速度を生じるよう} な力を制御入力として加える モデルや初期条件 が正確なら

実際には デ

は誤差を含むの

d

( )

t

≡

( )

t

q

q

を実現できる

確なら

実際にはモデルは誤差を含むので

V P d d

⇓

　

u = q

&&

+

K q

(

&

_d

− +

q

)

K q

(

−

q

)

u = K e K e&

−

_P

−

_V

d V

(

d

)

V

(

d

)

q q

=

+

K q

− +

q

K q

−

q

e

e

&& &&

&

&

_PD制御

V

P

( )

t

∴

+

+

=

e

0

K K

e K e K e 0

&&

&

が に収束するように _{V P}を決めれば

s

2

I

+

s

K

V

−

K

P

=

0

d

( )

,

( )

( )

t

t

→

t

e

0

K K

q

q

が に収束するように を決めれば

が実現する

V P

0

s

I

+

s

K

K

の特性根の実数部が負

ロボットの力制御

ロボットの力制御

ロボットの作業： 手先軌跡だけでなく，手先が対象物に

対して加える力の制御が必要

Ex. バリ取り，研磨，組み立て

[1]

インピーダンス制御

外力に対して望ましい

インピーダンス

特性をもつように

制御入力を決める

[2]

ハイブリッド制御

位置制御と力制御を同時に行うのに，

誤差選択行列

を

用いて必要情報を抽出する

インピ－ダンス制御

システムの入出力特性が望ましいイン_{ピーダンスをもつように入力を決める制御} R L C v t0( ) ( ) i v t 2次遅れ系のステップ応答 2 2

₂

2

( )

n

s

s

G s

=

₊

_ςω

ω

₊

_ω

( ) i t C ( ) q t 2

₂

_{n n}2

s

+

ςω

s

+

ω

1 y t( ) 1 ( ) ₁ ( ) ( ) ( ) ( ) ( ) ( ), ( ) ( ) ( ) ( ) ( ) t t i o t C di t t i t dt C dt di t d Ri t L i t dt v t v q t i t dt q t i t q t = + = = = =

+

_∫

_∫

∫

& &&

_{( )}

_t

_L

−1

_{[ ( ) ( )]}

_G

0.7 ζ ≈ の理想的応答 ( ) 1 u t = ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) , ( ) ,

1

1

o i dt t t t t t t q t i t dt q t i t q t

C

C

Rq

Lq

q

v

v

=

q

= = =

+

∫

+

=

&

&&

ラプラス変換 1

( )

[ ( ) ( )]

y t

= L

G s u s

望ましい応答を実現する理想

ζ

があ 2 0 1 1 ( ) ( ) _C ( ) _i( ), ( ) _C ( ) Rsq s + Ls q s + q s = v s v s = q s ラプラス変換 _的な

ω

n

,

ζ

がある 伝達関数 望ましい応答を実現する 1 2 ₁ ( ) _{( )}( ) ₂ 1 1 LC LC o R C i v s s _{v s} LCs LRs

s

s

G

_{+ +} + +

=

=

=

伝達関数 望ましい応答を実現する

微分方程式

の係数 C 望ましい応答を実現する インピーダンス(

L

,

R

,

C

の組 合せ)がある インピーダンス

R

2

(

L

1

)

2

C

Z

=

+

ω

−

_ω

ロボットのインピーダンス制御

ロボットに作用した

外力

と

，

それにより生じる

変位量

との関係 が，

望ま

しい

バネ

_{-マス-ダンパ}

系と等しくなる

よう，

ロボット駆動力を与える

制御

例

m

x&&

f

f

(7 33)

m

0

f

f

e

x

d

x

システムの 入出力特性 が望ましいインピーダンスをもつように入力を決める制御

例

m

x

= −

f

f

_e

(7.33)

0

f

f

e

d

x

望ましいインピーダンス特性

（

入出力

特性

）

望ましいインピ ダンス特性

（

入出力

特性

）

(

)

(

)

(

)

d d d d d d e

m x x

_{&& &&}

−

+

d x x

_{& &}

−

+

k x x

−

=

f

C

&

0

&&

0

(

)

d

d

d

d

e

m x d x k x x

_&&

+

_&

+

−

=

f

Constant

:

0,

0

d d d

x

⇒

x

=

x

=

(7.34)

(

)

f

インピーダンス制御

( )

ˆ

ˆ

ˆ

ˆ

m

（

：

_{mの推定値）}

( )

1

d d

(

d

)

d

e

d d

m

m

m

f

f

d x

k x x

m

m

m

= +

−

_&

−

−

(7.35)

インピーダンス制御

_-

多自由度ロボットへの拡張

-ˆ

m

(7.35)を

m

=

として

_{(7.33)に代入}

( )

1

m

f

m

d

m

k

(

)

f

&&

( )

1

d

&

d

(

d

)

d

e

d d

e

m

m

m

mx

f

d x

k x x

f

m

m

m

= +

−

−

−

−

(

)

d

e

d d d

mm x mf

d

_&&

=

e

−

md x mk x x

d

_&

−

d

(

−

d

)

mm x mf

=

−

md x mk x x

−

−

(

)

d d d d

e

m x d x k x x

_&&

+

_&

+

−

=

f

∴

望ましい入出力特性が実現

多自由度ロボットシステムに拡張

τ

=

J F

T ヤ ビ 行列

運動方程式

_{H p h}

_p

_&&

+

_p

+

_G

_p

= −

_{F F}

_e

(7.39)

ヤコビアン行列

所望インピーダンス

制御則

(

)

d

+

d

+

d

−

d

=

e

M p D p K p p

_&&

_&

F

1 1 p p

ˆ

ˆ

(

_d−

)

e

_d− d

=

+

−

F

I H M

F

_{H M D p&}

(7.44)

制御則

_{p p} 1 p p p

(

)

ˆ

ˆ

ˆ

(

)

d d d d d d

e

−

=

+

−

−

+

+

F

I H M

F

H M D p

H M K p p

h

G

(7.45)