知能システム論1 (12) - University of Electro...

知能システム論1　(12)

2012.7.4情報システム学研究科

情報メディアシステム学専攻

知能システム学講座

末廣尚士

2012年度前学期電気通信大学大学院　情報システム学研究科情報メディアシステム学専攻　知能システム論１ 2

- 数値解法

いろいろな場合に応用できるニュートン法の応用

１変数：微分多変数：ヤコビ行列(偏微分)

解の収束性が問題良い初期近似があると良いロボットアームの場合：軌跡に沿って逐次計算それでも越えられないところがあるはずだが、、、ロボットアームの場合、特異点に近寄らなければ大

丈夫なことが多い。


- 多変数の場合（アームのヤコビアン）

制御したい(解を求めたい)パラメタと、制御できる(関節角度など)パラメタとの、微小変化量の関係を(1次偏微分係数で)表した行列を使う。

一般に、このような多変数関数の1次偏微分係数行列はヤコビ行列、ヤコビアンなどと呼ばれる。

イメージとしては、(超)曲面の接(超)平面による近似。一次微分が曲線を接線で近似していたことを思い

出して欲しい。


- ロボットアームのヤコビアンの例

関節角度を少し動かしたとき手先の位置・姿勢がどのように変化するか：いわゆる「ロボットアームのヤコビアン」

関節角度を少し動かしたときカメラ画像中の位置がどのように変化するか：「イメージヤコビアン」などと呼ばれる


- 微分について

偏微分：着目しているパラメタ以外は変化ないとしときの、そのパラメタについての微分。1変数の微分で計算できる。

良く使う微分の公式積の微分合成関数の微分


- 積の微分

積の微分

f x= f 1x f 2x

∂ f∂ x

=∂ f 1∂ x

f 2 f 1∂ f 2∂ x

のとき

とりわけ１つのパラメタが１つの部分にしか関係しない場合

f x= f 1x1 f 2x2 f i xi f nxn

∂ f∂ x i

= f 1 f 2∂ f i∂ xi

f n ∂ f j∂ x i

=0 i≠ j なぜならだから


- 合成関数の微分

合成関数の微分

f x= f ｇ x

∂ f∂ x

=∂ f∂ g

∂ g∂ x

のとき

たとえばイメージヤコビアンを考える場合、

アームから手先の位置： p=ｇ

手先位置からカメラ画像への写像： u= f p


- 求め方1(3自由度アーム)

1=6

yhandBase = xhand

L1 sin 1=l2h sin 2l3h sin 23sin 1

xhandBase = xhand

L1 cos 1=l2h sin 2l3h sin 23cos 1

zhandBase =bhl1hl2hcos 2l3hcos 23

l2h

l1h

bh

3=2

2=4

順運動学の式を素直に偏微分する


- 求め方1(xについての計算)

x=l2hsin 2l3hsin 23cos1

∂ x∂1

=−l2h sin 2l3h sin 23sin 1

∂ x∂2

=l2hcos 2l3hcos 23cos1

∂ x∂3

=l3h cos23cos1


- 求め方1(yについての計算)y=l2h sin 2l3h sin 23sin 1

∂ｙ∂θ1

=(l2h sin (θ2)+l3hsin (θ2+θ3))cos(θ1)

∂ y∂2

=l2hcos 2l3hcos 23sin 1

∂ y∂3

=l3h cos23sin 1


- 求め方1(zについての計算)z=bhl1hl2h cos2l3h cos23

∂ z∂θ1

=0

∂ z∂2

=−l2hsin 2−l3hsin 23

∂ z∂3

=l3h sin 23


- 求め方1(まとめ)

−l2h sin 2l3hsin 23sin 1 l2hcos2l3hcos23cos1 l3hcos23cos1l2hsin 2l3h sin 23cos 1 l2hcos 2l3hcos 23sin 1 l3hcos 23sin 1

0 −l2h sin 2−l3h sin 23 l3h sin 23

l2hcos 2l3hcos 23cos 1l2hcos 2l3hcos23sin 1

−l2h sin 2−l3h sin 23l3hcos23cos1l3hcos23sin 1l3hsin 23

−l2h sin 2l3h sin 23sin 1l2h sin 2l3h sin 23cos1

0


- 求め方2(座標系表現から求める)

Basex

yz

Handx

yz

L1x

yz

L2x

yz

L3x

yz

bhl1h

l2h

l3h

T 10 1=cos1 −sin 1 0 0

sin 1 cos1 0 00 0 1 bh0 0 0 1

T 21 2= cos2 0 sin 2 0

0 1 0 0−sin 2 0 cos2 l1h

0 0 0 1

T 32 3= cos3 0 sin 3 0

0 1 0 0−sin 3 0 cos3 l2h

0 0 0 1


- 求め方2(座標系表現から求める)

Basex

yz

Handx

yz

L1x

yz

L2x

yz

L3x

yz

bhl1h

l2h

l3h

T hand3 =1 0 0 0

0 1 0 00 0 1 l3h0 0 0 1

手先の座標系は、

T hand0 1 ,2 ,3= Ahand

0 d hand0

0 0 0 1 = T 10 1 T 2

1 2 T 32 3 T hand

3


- 求め方2( について)

∂ phand0

∂1=

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3

phand0 1 ,2 ,3= T 1

0 1 T 21 2 T 3

2 3 phand3

T 10 1=cos1 −sin 1 0 0

sin 1 cos1 0 00 0 1 bh0 0 0 1

∂ T 10 1∂1

=−sin 1 −cos1 0 0cos1 −sin 1 0 00 0 0 00 0 0 0

1

：位置の関係だけにしておく

：積の公式

：　　について偏微分1


- 求め方2(積の公式を右から計算)∂ phand0

∂1=

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3

T 32 3= cos3 0 sin 3 0

0 1 0 0−sin 3 0 cos3 l2h

0 0 0 1 phand

3 = 00l3h1 T 32 3 phand

3 = l3hsin 30

l3hcos3l2h1



∂1=

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3

T 32 3 phand

3 = l3hsin 30

l3hcos3l2h1 T 2

1 2= cos2 0 sin 2 00 1 0 0

−sin 2 0 cos2 l1h0 0 0 1

T 21 2 T 3

2 3 phand3 = l3hcos2sin 3sin 2cos3l2h sin 2

0l3h −sin 2sin 3cos2cos3l2h cos2l1h

1 = l2hsin 2l3hsin 23

0l1hl2hcos2l3hcos23

1



∂1=

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3

∂ T 10 1∂1

=−sin 1 −cos1 0 0cos1 −sin 1 0 00 0 0 00 0 0 0

T 21 2 T 3

2 3 phand3 = l2hsin 2l3hsin 23

0l1hl2hcos2l3hcos23

1 ∂ T 10 1∂1

T 21 2 T 3

2 3 phand3 =−l2hsin 2l3hsin 23sin 1

l2hsin 2l3hsin 23cos100


- 求め方1と求め方2との比較

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3 =−l2h sin 2l3h sin 23sin 1

l2h sin 2l3h sin 23cos 100

−l2h sin 2l3hsin 23sin 1 l2h cos2l3h cos23cos1 l3hcos23cos1l2hsin 2l3h sin 23cos 1 l2hcos 2l3hcos 23sin 1 l3hcos 23sin 1

0 −l2h sin 2−l3h sin 23 l3h sin 23


- 求め方2の意味

T 10 1=cos1 −sin 1 0 0

sin 1 cos1 0 00 0 1 bh0 0 0 1

∂ T 10 1∂1

=−sin 1 −cos1 0 0cos1 −sin 1 0 00 0 0 00 0 0 0

=cos1 −sin 1 0 0sin 1 cos1 0 00 0 1 bh0 0 0 1

0 −1 0 01 0 0 00 0 0 00 0 0 0= T 1

0 10 −1 0 01 0 0 00 0 0 00 0 0 0


- 求め方2の意味

0 −1 0 01 0 0 00 0 0 00 0 0 0 はz軸回りの回転の微分を表現する行列

0 −1 0 01 0 0 00 0 0 00 0 0 0

xyz1=− yx00

ベクトルに掛けると

0010×xyz1=− yx00 これはz軸ベクトルとの外積と同じ

注：（4次元ではなく）3次元ベクトルとして考える．


- 求め方2の意味∂ phand0

∂1=

∂ T 10 1∂1

T 21 2 T 3

2 3 phand3

= T 10 10 −1 0 0

1 0 0 00 0 0 00 0 0 0 T 21 2 T 3

2 3 phand3

= T 10 10 −1 0 0

1 0 0 00 0 0 00 0 0 0 phand1

= T 10 10010× phand

1 = r10 × l 1−hand

0

何故こうなるか良く考えること


- 関節パラメタの微分の行列表現

0 −1 0 01 0 0 00 0 0 00 0 0 0

回転並進

x軸

y軸

z軸

0 0 1 00 0 0 0

−1 0 0 00 0 0 0

0 0 0 00 0 −1 00 1 0 00 0 0 0 0 0 0 1

0 0 0 00 0 0 00 0 0 00 0 0 00 0 0 10 0 0 00 0 0 00 0 0 00 0 0 00 0 0 10 0 0 0



∂ phand0

∂2= T 10 1

∂ T 21 2∂2

T 32 3 phand

3

phand0 1 ,2 ,3 = T 1

0 1 T 21 2 T 3

2 3 phand3

2

= T 10 1 T 2

1 20100× phand2 = r2

0 × l 2−hand0



∂ phand0

∂3= T 10 1 T 2

1 2∂ T 32 3∂3

phand3

phand0 1 ,2 ,3 = T 1

0 1 T 21 2 T 3

2 3 phand3

3

= T 10 1 T 2

1 2 T 32 30100× phand

3 = r30 × l 3−hand

0


- 求め方2(まとめ)

∂ phand0

∂= r10 × l 1−hand

0 r 20 × l 2−hand

0 r30 × l 3−hand

0

phand0 1 ,2 ,3 = T 1

0 1 T 21 2 T 3

2 3 phand3


- ヤコビアンの直感的意味

∂ phand0

∂=r1×l1 r2×l 2 r3×l 3

r1

r2

r3

l 1

l 2

l 3l

?

回転軸　　は紙面に垂直

r

r

r×l


- 座標回転に関するヤコビアンr 座標系の位置の変化は回

転軸からの距離と関係があるが、姿勢の変化は回転軸からの距離とは無関係

座標系の回転を回転軸ベクトル　　（回転角度に比例した大きさの回転軸ベクトル）で表すと、

=r すなわち

∂∂

=r

注意：一般に、回転はベクトルのように足せない（加法の交換法則が成り立たない）。しかし、一次の微小変化（テーラー展開の一次の項）を考えたときはベクトルとして足すことが出来る


- 並進関節に関するヤコビアン

r

並進関節の場合は関節での並進移動がそのまま手先位置の移動量となる。よって、

∂ｐ∂ q

=r

q

また姿勢は変化しないので

∂∂ q

=0


- アーム(n自由度)のヤコビアン（まとめ）

J=∂ p∂∂∂

=r1×l1 r2×l 2 ⋯ r n×l nr1 r2 ⋯ rn

回転関節n自由度アームのヤコビアン　　はまとめると以下のようになる。J

これは微小量、

(Δ pΔϕ)=J Δθ

または時間微分、

v=d pd td d t =J d

d t=J

などの関係として表現され、利用される。


- アームのヤコビアン（並進・回転まとめ）

J=∂ p∂q =... ∂ pi∂ q...

...∂i

∂ q...

関節パラメタを　　、手先位置姿勢を　　とまとめて表現すると、q

これは微小量、 p=J q または時間微分、 p= v=J d qd t =J qなどの関係として表現され、利用される。

pq=q1,q2, ... , qn

Tは回転関節の場合は回転角、並進の場合は並進移動量qi

p= px , p y , p z ,x ,y ,zT

p

は手先の位置および回転ベクトルp ,

∂ pi∂qi =r i×l i∂i

∂ qi =0∂ pi∂qi =r i

∂i

∂ qi =r i

回転関節並進関節


- ヤコビアンの機械的な計算方法

手先から順番に根元の方へ座標変換しながら下っていく。

rnn × phand

n

rnn

phandn

rn−1n−1 × phand

n−1

rn−1n−1

T nn−1 n phand

n = phandn−1

T nn−1 n rn

n = rnn−1

T nn−1 n rn

n × phandn

ベクトルの変換

位置の変換


- larm_w_hand.py(1)

def calc_jacob(self) : if self.hand : p_hand = self.wrist.rel_trans * self.hand.rel_trans.vec else : p_hand = self.wrist.rel_trans.vec

from numpy import *from numpy.linalg import *

vpythonでも使っている numpyモジュールに線形代数のルーチンがある。数値解を求めるときそれを使うのでヤコビアンのデータ型をそれに合わせておく。


- larm_w_hand.py(2) for i in range(self.dof-1,-1,-1) : if self.links[i].type == 0 : # translation self.links[i].jt=VECTOR(vec=self.links[i].j_axis) self.links[i].jr=VECTOR(0,0,0) else : # rotation self.links[i].jr=VECTOR(vec=self.links[i].j_axis) self.links[i].jt=self.links[i].jr*p_hand for j in range(self.dof-1,i-1,-1): self.links[j].jt=self.links[i].rel_trans.mat * self.links[j].jt self.links[j].jr=self.links[i].rel_trans.mat * self.links[j].jr self.links[j].jt=self.links[i].base.rel_trans.mat * self.links[j].jt self.links[j].jr=self.links[i].base.rel_trans.mat * self.links[j].jr p_hand = self.links[i].rel_trans * p_hand p_hand = self.links[i].base.rel_trans * p_hand for i in range(self.dof) : for j in range(3) : self.j_mat[j,i]=self.links[i].jt[j] self.j_mat[j+3,i]=self.links[i].jr[j]

知能システム論1 (12) - University of Electro...

Documents

Transcript of 知能システム論1 (12) - University of Electro...