順運動学と逆運動学

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

[A] 順運動学

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

[ , , ]^T

T  xT 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から見た脚先位置ベクトル^Link2r_Tは次のようになる．

2 2

3 ( )3

Link Link Link

T  Link  z 

r p R ³r_T



（3-5）

（3-6）

  

2 3

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

Link Link

Link  l T  l

p r

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

1 1

2 ( )

2

Link Link Link

T Link y z

 

 

   

r p R R ₂ ²r_T （3-7）

 

1

2 0, 0, ^T

Link

Link  l

p ₁ （3-8）

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

1 1 1

1 ( )

2 2

B B Link

T Link z x z

 

  

   

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

r p R R R ₁ r_T （3-9）

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

0 0 B1

t  B

r p r_t （3-10）

⁰pB



^{0, 0,} h



^T （3-11）

結局，θが与えられた場合，式（3-5）～（3-11）を順にたどっていくことによって⁰ を求めるこ とができる．

rt

ただし，式（3-9）で用いる^B1p_Link1およびはTable 3.4のようになる．

Table 3.4 Origin shift vector (^Bp_Link1) and rotation angle ()

Leg Number ^Bp_Link1  

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

T  xT 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関節角度₁の導出 Fig.3.9より

 

1 atan 2 ^{Leg i}y_T, ^{Leg i}

   z_T

T

（3-12）

である．ここで，^{Leg i}y_Tと^{Leg i}z_Tは

 

¹

Leg i B

T  y 

r R r （3-13）

から求められる．

しかし，^{Leg i}y_T ^{Leg i}z_T 0のときには₁は一意に定まらない．このような脚先位置は“特異点

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

1 B

zT

1 1

B B tan

T T

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

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

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

第3関節角度₃の導出

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



^{B1xT, B1yT, B1zT}



^を用いて



^{B1 T B1 J2}

 

^{2 B1 T B1 J2}

 

^{2 B1 T B1 J}



s x  x  y  y  z  z ₂ ²



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₃ とsin₃   1² から₃は次式で与えられる．



²



3 atan 2 1 ,

     （3-17）

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

※ 脚姿勢と関節角度₁，₃との関係

  1

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



^{ 1, 3}



^



^{1, atan 2}



^{ 1 2, }

 

^,



^{ 1 , atan 2}



^{ 1 2, }

 

^{for   10}



^{ 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}



^{ 12, }

 

^{（複号同順）}

for   ₁0



^{ 1, 3}



^



^{1, atan 2}



^{ 1 2, }

 

^,



^{ 1 , atan 2}



^{ 12, }

 

^{（複号同順）}

for 0 ₁

つまり，特異点以外の脚先座標が指定されたときの逆運動学問題の解は数値的には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 ^B1x_J2, ^B1y_J2, ^B1z_J2^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関節角度₂の導出

, 1 , 1 , 1

-

-Leg i Link Leg i Link Leg i Link

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



^{1 1}



2 atan 2 ^Link y_T, ^Link x_{T 1}

    （3-18）

と表せる．ただし，



3 3 2 3 3



atan 2 sin , cos

       （3-19）

である．また，^Link1y_Tと^Link1z_Tは式（3-9）から

  

1 1

1 1

2 2

Link B B

T z x z T Link

 

 ^{ } ^{  } ^

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

r R R R r ¹p



（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)