ロボティクスーその来し方行く末

知能システム論１（１０）
ロボットの腕の軌道計算
２００７．６．５
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)
５．ロボットの腕の制御(Control)
６．軌道計算(Trajectory)
７．行列の演算と応用(Matrix)
８．応用(Application)
関節位置制御による先端部軌道制御
T1  k p1 ( r1  1 )  kv11  A111
A111  kv11  k p1 (1   r1 )  0
Pr (t )
角度検出器
r1
Pr (t )
座
標
変
換
モータ
関節サーボ系
k p1
-
T1 関節１
－
k v1
 rn
座標変換：逆運動学
d
dt
1
z0
T1  A111
T1  A11
x0
1 
 
... A1n  :   N1
n 
 
y0
動力学
逆運動学(Inverse Kinematics)
Ｐが与えられθ1,θ2,θ3を求める。
P  F (1 , 2 , 3 ) を解く。
θ3
P3
P  l1z1  l2 z2  l3 z3
1 : P  x0  (l1z1  l2 z2  l3 z3 )  x0
 l2 s2c1  l3 ( s2c3  s3c2 )c1
リンク３
リンク2
l3
l2
 (l2 s2  l3s2c3  l3s3c2 )c1 P
θ2
Z2
P  y0  (l2 s2  l3s2 c3  l3s3c2 ) s1
P  y0
1  tan (
)
P  x0
P2
X2
1
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
s1 P  y0
1 P  y0
tan 1  
 1  tan (
)
c1 P  x0
P  x0
3 : P  l1z1  l2 z2  l3 z3
(l2 s2  l3s2c3  l3s3c2 )  0 のとき
P  l1 z1  (l2 z2  l3 z3 )  (l2 z2  l3 z3 )  l22  l32  2l2l3 ( z2  z3 )
2
1
2
 3  cos (
{ P  l1 z1  l22  l32 })
2l3l2
1
c3
 2 : P  z0  (l1z1  l2 z2  l3 z3 )  z0  l1  (l2 z2  l3 ( s3 x2  c3 z2 ))  z0
 l1  {l3s3 (c2 x1  s2 z1 )  (l2  l3c3 )( s2 x1  c2 z1 )}  z0
P  z0  l1  {( l3s3c2  (l2  l3c3 ) s2 ) x1  ( l3s3s2  (l2  l3c3 )c2 ) z1}  z0
 l3s3s2  (l3c3  l2 )c2  A sin(    2 )
但し
A  (l3s3 )2  (l3c3  l2 )2 , tan   (l3c3  l2 ) (l3s3 )
 2   sin 1 (( P  z0  l1 ) A)  tan 1 (l3c3  l2 ) (l3s3 )
 180   2  180 の範囲で妥当な解を求める。
軌道の計算
x
z
時間t
y
x
t
y
z
滑らかな軌道の設計
位置と速度・加速度の連続性
t
3次スプライン補間
yj
(t , y )
y j 1
tj
折れ線軌道
y  Ay j  By j 1
A
t j 1  t
t j 1  t j
t j 1
位置の連続性
   (1)
, B  1 A 
t tj
t j 1 t j
1
A
tj
B
t j 1
t
加速度の折れ線表現
y j
(t , y)
y j 1
tj
折れ線軌道
加速度の連続性
y  Ay j  By j 1
A
t j 1  t
t j 1  t j
t j 1
   (2)
, B  1 A 
t tj
t j 1 t j
1
A
tj
B
t j 1
t
y
位置と加速度が連続な軌道
yj
y j 1
tj
t
t j 1
1
1
y  Ay j  By j 1  ( A3  A)(t j 1  t j ) 2 y j  ( B 3  B )(t j 1  t j ) 2 y j 1    (3)
6
6
t  t jの時、 A  1, B  0
y  yj
t  t j 1の時、 A  0, B  1
y  y j 1
A 
y j 1  y j
1
1
, B 
t j 1  t j
t j 1  t j
3 A2  1
3B 2  1
y 

(t j 1  t j ) y j 
(t j 1  t j ) y j 1
t j 1  t j
6
6
y  Ay j  By j 1
y j 1
y j
tj
t j 1
（３）式の導出
y  Ay j  By j 1を積分する。
A2
B2
y  
(t j 1  t j ) y j  
(t j 1  t j ) y j 1  C
2
2
1
1
y  ( A3 )(t j 1  t j ) 2 y j  ( B 3 )(t j 1  t j ) 2 y j 1  Ct  D
6
6
1 3
1 3
2
y  ( A )(t j 1  t j ) y j  ( B )(t j 1  t j ) 2 y j 1  C ((t j 1  t j ) B  t j )  D
6
6
次の条件からＣ，Ｄを決める。
t  t jの時、 A  1, B  0
t  t j 1の時、 A  0, B  1
y  yj
y  y j 1
1
1
2
y j  (t j 1  t j ) y j  Ct j  D,
y j 1  (t j 1  t j ) 2 y j 1  Ct j 1  D
6
6
1
y j 1  y j  (t j 1  t j ) 2 ( y j 1  y j )  C (t j 1  t j ) 続く
6
（３）式の導出
C
y j 1  y j
1
 (t j 1  t j )( y j 1  y j )
t j 1  t j 6
1
D  y j  (t j 1  t j ) 2 y j  Ct j
6
y j 1  y j 1
1
2
 y j  (t j 1  t j ) y j  (
 (t j 1  t j )( y j 1  y j ) )t j
6
t j 1  t j 6
1 3
1 3
2


y  ( A )(t j 1  t j ) y j  ( B )(t j 1  t j ) 2 y j 1
6
6
y j 1  y j 1
(
 (t j 1  t j )( y j 1  y j ))(( t j 1  t j ) B  t j )
t j 1  t j 6
y j 1  y j 1
1
2
 y j  (t j 1  t j ) y j  (
 (t j 1  t j )( y j 1  y j ) )t j
6
t j 1  t j 6
続く
1
1
y  ( A3 )(t j 1  t j ) 2 y j  ( B 3 )(t j 1  t j ) 2 y j 1
6
6
y j 1  y j
1
1
 ( y j 1  y j ) B 
t j  (t j 1  t j ) 2 ( y j 1  y j ) B  (t j 1  t j )( y j 1  y j )t j
t j 1  t j
6
6
y j 1  y j
1
1
2


 y j  (t j 1  t j ) y j 
t j  (t j 1  t j )( y j 1  y j )t j
6
t j 1  t j
6
1
1
 ( A3 )(t j 1  t j ) 2 y j  ( B 3 )(t j 1  t j ) 2 y j 1
6
6
1
1
 y j 1 B  y j (1  B)  (t j 1  t j ) 2 y j 1 B  (t j 1  t j ) 2 y j (1  B )
6
6
1
1
 ( A3 )(t j 1  t j ) 2 y j  ( B 3 )(t j 1  t j ) 2 y j 1
6
6
1
1
 y j 1 B  y j A  (t j 1  t j ) 2 y j 1 B  (t j 1  t j ) 2 y j A
6
6
1 3
1 3
2
 Ay j  By j 1  ( A  A)(t j 1  t j ) y j  ( B  B)(t j 1  t j ) 2 y j 1
6
6
速度の連続性
速度の式を用いて連続条件を与える。
y j 1  y j
3 A2  1
3B 2  1
y 

(t j 1  t j ) y j 
(t j 1  t j ) y j 1
t j 1  t j
6
6


区間 t j 1 t j の t  t jの時の速度、
y j  y j 1
t j  t j 1

区間 x j
y j 1  y j

1
2
(t j  t j 1 ) y j 1  (t j  t j 1 ) y j
6
6

x j 1 の t  t jの時の速度、
2
1
 (t j 1  t j ) y j  (t j 1  t j ) y j 1
t j 1  t j 6
6
2つの区間の境界点で速度が等しい
y j 1  y j
t j 1  t j

y j  y j 1
t j  t j 1

t j  t j 1
6
y j 1 
t j 1  t j 1
3
y j 
y j ( j  1 ~ n )に関する n  2個の1次方程式
2個のパラメータを境界条件に使える。
通常y1 , y nの境界条件を与える。
t j 1  t j
6
y j 1
練習問題
右図の3点が与えられた時、
始点終点の速度０の条件で
3次スプライン補間を求めよ。
1
2
1
y1  y2  y3
6
3
6
2
1
0  1  y1  y2
6
6
1
2
0  y2  y3
6
6
y
t1  0, t2  1, t3  2
y1  0, y2  1, y3  1
1
1 
t
1
2
練習問題
y
右図の3点が与えられた時、
始点終点の速度０の条件で
3次スプライン補間を求めよ。
x1  0, x2  1, x3  2
y1  0, y2  1, y3  1
1
1
2
1
9
y1  y2  y3
y1  ,
6
3
6
2
2
1
y2  3,
0  1  y1  y2
6
6
3
y3 
1
2
2
0  y2  y3
6
6
1
9 1
y  B  ( A3  A)  ( B 3  B )( 3)
6
2 6
1 
x
1
2
1
1
3
y  1  ( A3  A)( 3)  ( B 3  B )
6
6
2
A  1, B  0
A  0.8, B  0.2
0
0.08
1
1.096
A  0.6, B  0.4
A  0.4, B  0.6
0.27
0.54
1.108
1.072
A  0.2, B  0.8
A  0, B  1
0.80
1
1.024
1

Download Report