articles:forward_kinematics [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== 順運動学 ======
===== ３自由度アームの順運動学 =====


==== 座標系の設定 ====
[{{ articles:forward_kinematics_01.png?200|図1 3関節アームの座標系設定}}]
典型的な3自由度アームとその座標系の設定を図1に示す．
この3自由度アームは[[articles:kinematics|アームの運動学の導入部]]で述べたように
可動域内の自由な位置に手先を動かすことが出来る．
座標系の設定は[[articles:kinematics#直感的に理解しやすい置き方|直感的に分かりやすい]]ように関節パラメタが$0$のときにベース座標系と同じ姿勢になっている．
回転関節の関節軸はベースからz,y,yになっている．

一般にハンドは取り換え可能であり，どのような座標系を設定するかはハンドに依存することが多い．
そこでここではハンドを除いた手首位置で考えることにする．

==== 座標変換を使わない方法 ====

次節で示すが順運動学は座標変換を用いると簡単に求めることができる．

しかし，ここでは具体的に何を計算しているかの直感を得るために，座標変換を用いずに
幾何学的な解析で順運動学を計算する．

[{{ articles:forward_kinematics_10.png?250| 図2 $\theta_1 = \pi/6, \, \theta_2 = \pi/4, ,\ \theta_3 = pi/2$のアームの姿勢}}]

図2は，$\theta_1 = \pi/6, \, \theta_2 = \pi/4, ,\ \theta_3 = pi/2$のアームの姿勢を示したものである．
以下の計算では角度が何であれ成り立つがイメージを掴むために具体的な姿勢を図示している．
また以下では$base$,$L_1$などの添え字は式が見にくくなるので,base=>0,$L_1$=>1のように変更して示す．

注意してほしいのは，下の図はベース座標系$\Sigma_0$ではなくリンク1の座標系$\Sigma_1$のzx平面での図である．

下の図から$\Sigma_1$での手首のzの値$^1z_{wrist}$，
すなわち$\Sigma_0$での手首のzの値$^0z_4$が以下のように計算出来る．
$$
^0z_{wrist} = {^1z_{wrist}} = l_b+l_1+l_2 \mathrm{cos}\, \theta_2 + l_3 \mathrm{cos}(\theta_2 + \theta_3)
$$
また，$\Sigma_1$での手首のxの値$^1x_{wrist}$は以下のようになる．
$$
{^1x_{wrist}} = l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3)
$$
この${^1x_{wrist}}$を使うと上の図から${^0x_{wrist}}$,${^0y_{wrist}}$が以下のように求められる．
$$
{^0x_{wrist}} = {^1x_{wrist}}\mathrm{cos}\,\theta_1 = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{cos}\,\theta_1
$$
$$
{^0y_{wrist}} = {^1x_{wrist}}\mathrm{sin}\,\theta_1 = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{sin}\,\theta_1
$$
繰り返しになるが結果をまとめると，
$$
{^0x_{wrist}} = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{cos}\,\theta_1
$$
$$
{^0y_{wrist}} = (l_2 \mathrm{sin}\, \theta_2 + l_3 \mathrm{sin}(\theta_2 + \theta_3))\mathrm{sin}\,\theta_1
$$
$$
^0z_{wrist} = l_b+l_1+l_2 \mathrm{cos}\, \theta_2 + l_3 \mathrm{cos}(\theta_2 + \theta_3)
$$
以上がベースから手首位置への順運動学解になる．
==== 座標変換を使う ====

ベース座標系から見たリンク1の関節座標系は，
$$
^{0}T_{J_1} =
\left( \begin{array}{cc} 
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l_b \\
0 & 0 & 0 & 1
\end{array} \right)
$$
関節における駆動変換は，
$$
^{J_1}T_{1} (\theta_1) =
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_1 & - \mathrm{sin}\, \theta_1 & 0  & 0\\
\mathrm{sin}\, \theta_1 & \mathrm{cos}\, \theta_1 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array} \right)
$$
となる．これを計算すると，
$$
^{0}T_{1} (\theta_1) = {^{0}T_{J_1}}{ ^{J_1}T_{1}} (\theta_1)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_1 & - \mathrm{sin}\, \theta_1 & 0  & 0\\
\mathrm{sin}\, \theta_1 & \mathrm{cos}\, \theta_1 & 0 & 0 \\
0 & 0 & 1 & l_b \\
0 & 0 & 0 & 1
\end{array} \right)
$$
となり，まとめても簡単な形になっている．
同様に各リンク間の座標変換行列を求めると，
$$
^{1}T_{2} (\theta_2)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_2 & 0 & \mathrm{sin}\, \theta_2 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_2 & 0 & \mathrm{cos}\, \theta_2 & l_1 \\

0 & 0 & 0 & 1
\end{array} \right)
$$
$$
^{2}T_{3} (\theta_3)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_3 & 0 & \mathrm{sin}\, \theta_3 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_3 & 0 & \mathrm{cos}\, \theta_3 & l_2 \\

0 & 0 & 0 & 1
\end{array} \right)
$$
となる．最後にリンク3から見た手首座標系への変換は，
$$
^{3}T_{wrist}
=
\left( \begin{array}{cc} 
1 & 0 & 0  & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l_3 \\
0 & 0 & 0 & 1
\end{array} \right)
$$
となる．
これらを使ってベースから見た手首座標系は，
$$
^0T_{wrist}(\theta_1,\, \theta_2, \, \theta_3)
= {^0T_1}(\theta_1){^1T_2}(\theta_2){^2T_3}(\theta_3){^3T_{wrist}}
$$
となる．

座標変換行列を用いた場合,順運動学計算はこれで終わりである．
\\
\\
==== 座標変換を使わない方法との比較 ====

念のため式を展開して座標変換を使わない方法との一致を確認する．
手首位置だけを問題にしていたので手首位置だけの式に書き換える．
$$
^0\boldsymbol{p}_{wrist}(\theta_1,\, \theta_2, \, \theta_3)
= {^0T_1}(\theta_1){^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}}
$$
これを右から順に計算していく．ここで
$$
^3\boldsymbol{p}_{wrist} = \left( \begin{array}{c} 0 \\ 0 \\ l_3 \\ 1 \end{array} \right)
$$
である．よって，
$$
^2T_3(\theta_3){^3\boldsymbol{p}_{wrist}} 
=  \left( \begin{array}{c} 
l_3\mathrm{sin}\,\theta_3 \\ 
0 \\ l_3\mathrm{cos}\,\theta_3 + l_2 \\ 
1 \end{array} \right)
$$
さらに
((
ここの簡略化は三角関数の加法定理．分かりますよね．
))，
$$
{^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}} 
=  \left( \begin{array}{c} 
l_3(\mathrm{cos}\,\theta_2 \, \mathrm{sin}\,\theta_3 + \mathrm{sin}\,\theta_2 \, \mathrm{cos}\,\theta_3)+l_2\mathrm{sin}\,\theta_2 \\ 
0 \\ 
l_3(-\mathrm{sin}\,\theta_2 \, \mathrm{sin}\,\theta_3 + \mathrm{cos}\,\theta_2 \, \mathrm{cos}\,\theta_3)+l_2\mathrm{cos}\,\theta_2 + l_1 \\ 
1 \end{array} \right)
=
\left( \begin{array}{c} 
l_3\mathrm{sin}(\theta_2 + \theta_3) + l_2\mathrm{sin}\,\theta_2 \\ 
0 \\ 
l_3\mathrm{cos}(\theta_2 + \theta_3) + l_2\mathrm{cos}\,\theta_2 + l_1 \\ 
1 \end{array} \right)
$$
またさらに
$$
^0\boldsymbol{p}_{wrist}={^0T_1}(\theta_1){^1T_2}(\theta_2){^2T_3}(\theta_3){^3\boldsymbol{p}_{wrist}} 
= 
\left( \begin{array}{c} 
(l_3\mathrm{sin}(\theta_2 + \theta_3) + l_2\mathrm{sin}\,\theta_2)\mathrm{cos}\,\theta_1 \\ 
(l_3\mathrm{sin}(\theta_2 + \theta_3) + l_2\mathrm{sin}\,\theta_2)\mathrm{sin}\,\theta_1 \\ 
l_3\mathrm{cos}(\theta_2 + \theta_3) + l_2\mathrm{cos}\,\theta_2 + l_1 + l_b\\ 
1 \end{array} \right)
$$
となり，同じ結果が得られることが確認できる．

同じ結果が得られるなら座標変換を使わない方が直感的で簡単にも見える．
しかし，座標変換行列の場合は式として展開する必要はなく，簡単な行列演算の繰り返しで解を得ることができる．
また，座標変換を使わないやり方では，手先にハンドを付けた場合にどのように修正すればよいのかとか
手先の姿勢をどのように計算するかなど簡単な拡張に対しても対応が簡単ではない．
実際には座標変換を用いた方が簡単なだけでなく以下のようにさまざまな利点がある．

  * 位置だけでなく姿勢も計算できる
  * 関節が増えた場合や斜めの回転軸などの複雑な構造にも簡単に対応できる
  * ハンドなどの手先の変更にも容易に対応できる
  * ヤコビ行列の計算も機械的にできるだけでなくその意味付けも分かりやすい．


===== ６自由度アームの順運動学 =====

座標系を用いた本格的な順運動学計算の例としてハンドのついた6自由度アームについて扱う．

ここでは「ハンド」としているが，ハンドに限らずアームの手首（アームの最終リンク）に固定されたものであれば
どのような「ツール(tool, end-effector)」であっても良い．
==== ハンド座標系の設定 ====

まずハンド座標系をどのように設定するかの例を説明する．
これについては明確な決まりがあるわけではない．
多くの場合，作業を行う際に作業対象に設定された座標系との関係で直感的に分かりやすく設定することになる．

[{{ hand_01.png?300| 図3 ハンド座標系}}]
平行2指ハンドで，対象の把持点（box.gripなど）に
対して，アプローチし，その把持点で掴むことを想定した場合，
図3のように把持点座標系とハンド座標系とが一致したときに把持すると考えると分かりやすい．
((これが分かりやすいかどうかは個人の趣味の問題もある))
((ハンドの進行方向をz軸正方向にとる例も多いがその場合把持点座標のz軸が下向きになる))．

[{{ hand_02.png?150| 図4 手首とハンドの関係}}]
このようにハンド座標系を設定してハンドを手首に搭載することを考えると，
手首とハンドの座標系は図4のようになり，その変換行列は，

$$
^{wrist}T_{hand} 
=
\left( \begin{array}{cc}
1 & 0 & 0 & 0 \\
0 & -1 & 0 & 0 \\
0 & 0 & -1 & l_{hand} \\
0 & 0 & 0 & 1
\end{array} \right)
$$
となる．
==== 6自由度アームの順運動学 ====

[{{ forward_kinematics_05.png?200| 図5 6自由度アーム}}]

上記のハンド座標系の設定を踏まえて図5のハンド付き6自由度アームの順運動学を求める．
このアームの回転軸はベースからz,y,y,z,y,zとなっており座標変換行列を列挙すると以下のようになる．

$$
^{0}T_{1} (\theta_1)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_1 & - \mathrm{sin}\, \theta_1 & 0  & 0\\
\mathrm{sin}\, \theta_1 & \mathrm{cos}\, \theta_1 & 0 & 0 \\
0 & 0 & 1 & l_b \\
0 & 0 & 0 & 1
\end{array} \right)
\;
$$
$$
^{1}T_{2} (\theta_2)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_2 & 0 & \mathrm{sin}\, \theta_2 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_2 & 0 & \mathrm{cos}\, \theta_2 & l_1 \\

0 & 0 & 0 & 1
\end{array} \right)
\;
$$
$$
^{2}T_{3} (\theta_3)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_3 & 0 & \mathrm{sin}\, \theta_3 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_3 & 0 & \mathrm{cos}\, \theta_3 & l_2 \\

0 & 0 & 0 & 1
\end{array} \right)
$$
$$
^{3}T_{4} (\theta_4)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_4 & - \mathrm{sin}\, \theta_4 & 0  & 0\\
\mathrm{sin}\, \theta_4 & \mathrm{cos}\, \theta_4 & 0 & 0 \\
0 & 0 & 1 & l_3 \\
0 & 0 & 0 & 1
\end{array} \right)
\;
$$
$$
^{4}T_{5} (\theta_5)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_5 & 0 & \mathrm{sin}\, \theta_5 & 0\\
0 & 1 & 0 & 0 \\
-\mathrm{sin}\, \theta_5 & 0 & \mathrm{cos}\, \theta_5 & l_4 \\

0 & 0 & 0 & 1
\end{array} \right)
\;
$$
$$
^{5}T_{6} (\theta_6)
=
\left( \begin{array}{cc} 
\mathrm{cos}\, \theta_6 & - \mathrm{sin}\, \theta_6 & 0  & 0\\
\mathrm{sin}\, \theta_6 & \mathrm{cos}\, \theta_6 & 0 & 0 \\
0 & 0 & 1 & l_5 \\
0 & 0 & 0 & 1
\end{array} \right)
$$
$$
^{6}T_{wrist}
=
\left( \begin{array}{cc} 
1 & 0 & 0  & 0 \\
0 & 1 & 0 & 0 \\
0 & 0 & 1 & l_6 \\
0 & 0 & 0 & 1
\end{array} \right)
$$
$$
^{wrist}T_{hand} 
=
\left( \begin{array}{cc}
1 & 0 & 0 & 0 \\
0 & -1 & 0 & 0 \\
0 & 0 & -1 & l_h \\
0 & 0 & 0 & 1
\end{array} \right)
$$
よって，ベースからハンドまでの順運動学は，
$$
^0T_{hand}(\theta_1,\,\theta_2,\,\theta_3,\,\theta_4,\,\theta_5,\,\theta_6)
={^0T_1(\theta_1)}{^1T_2(\theta_2)}{^2T_3(\theta_3)}{^3T_4(\theta_4)}{^4T_5(\theta_5)}{^5T_6(\theta_6)}{^6T_{wrist}}{^{wrist}T_{hand}}
$$
となる．

以上のように，シリアルリンクアームでは各関節の設置座標系と駆動軸，最終リンクから手首座標系，手首座標系からハンド座標系が与えられれば機械的に順運動学を計算することが出来る．