• 検索結果がありません。

順運動学と逆運動学

ドキュメント内 錦織, 慎治 (ページ 32-37)

第 3 章 脚式ローバの形状

3.1 ローバの形状と転倒

3.2.2 順運動学と逆運動学

運動学的検討のための準備として,3 次元等方歩行型ローバの基本的な運動学である順運動学と 逆運動学について示しておく.

[A] 順運動学

関節変数ベクトルθ[  1, 2, 3]T(j:第j関節の関節角度)の値が与えられた場合に,ある座 標系における脚先の 3 次元位置ベクトル を求めることを順運動学(Forward Kinematics)問題という.これは後述する逆運動学問題を解く場合や,後述するヤコビ行列を求め るときにも必要となる.

[ , , ]T

TxT yT zT

r

まず,座標系の回転行列を以下のように定義しておく.

(z軸まわりに回転)

cos sin 0

( ) sin cos 0

0 0

z

 

  

1

 

 

  

 

 

R (3-2)

(y軸まわりに回転)

cos 0 sin

( ) 0 1 0

sin 0 cos

y

 

 

 

 

  

 

 

R (3-3)

(x軸まわりに回転)

1 0 0

( ) 0 cos sin

0 sin cos

x   

 

 

 

  

 

 

R (3-4)

第2リンク固定座標系Link2から見た脚先位置ベクトルLink2rTは次のようになる.

2 2

3 ( )3

Link Link Link

TLinkz

r p R 3rT

(3-5)

(3-6)

  

2 3

3 2, 0, 0 T, 3, 0, 0 T

Link Link

Linkl Tl

p r

これを第1リンク固定座標系Link1から見た脚先位置ベクトルLink1rT に変換すると次のようになる.

1 1

2 ( )

2

Link Link Link

T Link y z

 

 

   

r p R R 2 2rT (3-7)

 

1

2 0, 0, T

Link

Linkl

p 1 (3-8)

これを胴体中心固定座標系B1から見た脚先位置ベクトルB1rTに変換すると次のようになる.

1 1 1

1 ( )

2 2

B B Link

T Link z x z

 

  

   

       

r p R R R 1 rT (3-9)

さらに,これを天体表面固定座標系0から見た脚先位置ベクトルに変換すると次のようになる.

0 0 B1

tB

r p rt (3-10)

0pB

0, 0, h

T (3-11)

結局,θが与えられた場合,式(3-5)~(3-11)を順にたどっていくことによって0 を求めるこ とができる.

rt

ただし,式(3-9)で用いるB1pLink1およびはTable 3.4のようになる.

Table 3.4 Origin shift vector (BpLink1) and rotation angle ()

Leg Number BpLink1  

Leg 1

Rcos , 0, Rsin

T 0

Leg 2

0, Rcos , Rsin

T

2

Leg 3

Rcos , 0, Rsin

T

Leg 4

0, Rcos , Rsin

T

2



Leg 5

Rcos , 0, Rsin

T 0

Leg 6

0, Rcos , Rsin

T

2

Leg 7

Rcos , 0, Rsin

T

Leg 8

0, Rcos , Rsin

T

2



[B] 逆運動学

脚の運動を論じるには,ローバの姿勢によって関節の角度がどう変化するかを知る必要がある.

そこで,ある座標系における脚先位置ベクトル の値が与えられた場合に,それを 実現する関節変数ベクトル を求めることを考える.これを逆運動学(Inverse Kinematics)問題という.

[ , , ]T

TxT yT zT

r

1 2 3

[  , , ]T

θ

脚が胴体の基準座標系に対して水平または垂直に取り付けられていれば(0 [deg]または 90 [deg]

 ならば),天体表面と脚の姿勢との関係が幾何学的に単純になるので逆運動学問題は容 易に解くことができる.しかし,本研究で扱うローバのように,脚が胴体の基準座標系に対して傾 いた角度で取り付けられている場合,脚の姿勢が直感的には理解しにくくなる.ここでは 座標 系における脚先位置ベクトル

1

B

1 1 1 1

[ , , ]

B B B B

T T T T

x y z T

r が与えられた場合について,幾何学的・数学 的な考察を行うことによって,各脚の逆運動学問題を解いていく.

第1関節角度1の導出 Fig.3.9より

 

1 atan 2 Leg iyT, Leg i

   zT

T

(3-12)

である.ここで,Leg iyTLeg izT

 

1

Leg i B

Ty 

r R r (3-13)

から求められる.

しかし,Leg iyTLeg izT 0のときには1は一意に定まらない.このような脚先位置は“特異点

(singular point)”と呼ばれている.理論上,1は任意の値をとりうるので,特異点に脚先がある場 合は,直前と直後にとるべき姿勢に滑らかにつながるような1を任意に指定できる.例えば,脚先 が特異点に接触して静止する場合には1 0 とすれば第2・第3リンクがつくる平面が移動対象面 に対して垂直になるような姿勢となる.なお,特異点は脚先の高さ( )により変化する.つま り,

1 B

zT

1 1

B B tan

T T

zx  を満たす直線上にあるすべての点がLeg 1の特異点である.特異点については

3.2.4.5節において詳しく述べる.

なお,式(3-12)によって計算される1を1と表すと, 1 も解になる.脚先を連続的に動か す場合には直前と直後の関節角度を監視しながら,それらと連続的な値をとるような1を選択する ことで関節角度の急激な変化を防ぐ.

第3関節角度3の導出

第2関節から脚先位置までの距離をsとおくと,

B1xT, B1yT, B1zT

を用いて

B1 T B1 J2

 

2 B1 T B1 J2

 

2 B1 T B1 J

sxxyyzz 2 2

2

(3-14)

となる.ただし,第2関節位置

B1xJ2, B1yJ2, B1zJ は脚によってTable 3.4のようになる.

一方,第2関節・第3関節・脚先の3点から成る三角形 (Fig.3.10を参照)について余弦 定理を適用すると

J J T2 3

2 3

 

3

22 23 2

2 3

cos J J T cos

2

  s

    

  (3-15)

が成り立ち,これを変形すると

2 2 2

2 3

3

2 3

cos 2

 s

  (3-16)

となる.したがって,上で得られるcos3 とsin3   12 から3は次式で与えられる.

2

3 atan 2 1 ,

     (3-17)

つまり,3としてとりうる値は2通り考えられ,幾何学的にはFig.3.11のような関係になる.これ は前述の1のときの議論と合わせて,適宜選択する必要がある.

※ 脚姿勢と関節角度1,3との関係

  1

   の範囲で考えると,同じ脚先座標となる1と3の組み合わせとして次の 4 通りが存 在する.

 1, 3

1, atan 2

 1 2,

 

,

 1 , atan 2

 1 2,

 

for   10

 1, 3

1, atan 2

 1 2,

 

,

 1 , atan 2

 1 2,

 

for 0 1

このうち,幾何学的に等価な脚姿勢をとる1と3の組み合わせが2通り存在する.

 1, 3

1, atan 2

 1 2,

 

,

 1 , atan 2

12,

 

(複号同順)

for   10

 1, 3

1, atan 2

 1 2,

 

,

 1 , atan 2

12,

 

(複号同順)

for 0 1

つまり,特異点以外の脚先座標が指定されたときの逆運動学問題の解は数値的には4通りあるが,

幾何学的には2通りのみ(幾何学的に等価なものが2つずつある)ということである.

Fig.3.9 Angle of Joint 1. Fig.3.10 Angle of Joint 3.

Leg i

y

Leg i

z

3( 0)

  s

2

3

J2

J3

T

1

Joint 1 Joint 3

Joint 2

1 Link y

1 Link x

Joint 2

Joint 3

3 (< 0)

3 (> 0) Joint 3

Fig.3.11 Two angles of Joint 3 and the postures of rover’s leg for the same landing point.

Table 3.4 Positions of Joint 2 in B1

Leg Number B1xJ2, B1yJ2, B1zJ2T

Leg 1 

R1

cos , 0, 

R1

sin T Leg 2 0,

R1

cos , 

R1

sin T Leg 3 

R1

cos , 0, 

R1

sin T Leg 4 0, 

R1

cos , 

R1

sin T Leg 5 

R1

cos , 0,

R1

sin T Leg 6 0,

R1

cos ,

R1

sin T Leg 7  

R1

cos , 0,

R1

sin T Leg 8 0, 

R1

cos ,

R1

sin T

第2関節角度2の導出

, 1 , 1 , 1

-

-Leg i Link Leg i Link Leg i Link

x y z系で見ると,Fig.3.12のような位置関係が成り立っている.この図 から,

1 1

2 atan 2 Link yT, Link xT 1

    (3-18)

と表せる.ただし,

3 3 2 3 3

atan 2 sin , cos

       (3-19)

である.また,Link1yTLink1zTは式(3-9)から

  

1 1

1 1

2 2

Link B B

T z x z T Link

 

        

r R R R r 1p

(3-20)

により求められる.

3( 0)

 

2

3

T

1 Link z

1 Link y

2( 0)

  R

1

Fig.3.12 Angle of Joint 2.

ドキュメント内 錦織, 慎治 (ページ 32-37)