2リンクモデルの逆運動学を求める！順運動学の式から算出する方法

ロボット工学において、順運動学や逆運動学を理解することはロボットや機械を制御する際にとても重要です。

以前の記事では、順運動学と逆運動学についての基本知識と簡単なモデルでの各運動学の算出結果を紹介しました。

ロボット工学の基礎　順運動学と逆運動学を理解するロボット工学を学ぶうえで必要な基本、『順運動学』と『逆運動学』について解説してきます。運動学の定義運動学...

前回の記事では、2リンクモデルの運動学のうち順運動学の式を求める方法を紹介しました。

２リンクモデルの運動学を求めてみよう！簡単に順運動学の式を算出する方法ロボット工学を学び、ロボットや機械を制御する際に、順運動学や逆運動学を理解することはとても重要です。以前の記事で...

今回の記事では、残りの2リンクモデルにおける逆運動学を求める方法を紹介したいと思います。

Contents

2リンクモデルと順運動学
逆運動学の算出
- θ₁について求める
- θ₂について求める
逆運動学と複数解
まとめ

2リンクモデルと順運動学

前回のおさらいです。

今回の記事では、上図のような2つの回転関節（Rotational Joint）と2つのリンク（Link）による2リンクモデルを取り扱います。

この2リンクモデルの順運動学は

の式で表されます。

2リンクモデルにおける順運動学の詳細は、こちらの記事を参考にしてください。

逆運動学の算出

では、2リンクモデルの逆運動学を求めていきます。

今回の記事では、逆運動学の式を求めるために、前回の記事で求めた順運動学の式を変換することで、ロボットの位置xとyから関節の変位θ₁とθ₂を表す数式を求めていきます。

この逆運動学の算出方法にも色々な手法がありますが、今回の記事ではθ₁を求めた後に、そのθ₁の値にあったθ₂を求める流れで逆運動学の式を算出していきたいと思います。

θ₁について求める

まず、順運動学の式を下のように移項して変換します。

$$ \begin{eqnarray} \left\{ \begin{array}{l} x &=& L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) \\ y &=& L_1 \sin \left( \theta_1 \right) + L_2 \sin \left( \theta_1 + \theta_2 \right) \end{array} \right. \\ \Rightarrow \left\{ \begin{array}{l} x – L_1 \cos \left( \theta_1 \right) &=& L_2 \cos \left( \theta_1 + \theta_2 \right) \\ y – L_1 \sin \left( \theta_1 \right) &=& L_2 \sin \left( \theta_1 + \theta_2 \right) \end{array} \right. \end{eqnarray} $$

つぎに、この変換した2つの式の両辺を2乗します。

$$ \begin{eqnarray} \left\{ \begin{array}{l} x^2 – 2 x L_1 \cos \left( \theta_1 \right) + {L_1}^2 \cos^2 \left( \theta_1 \right) &=& {L_2}^2 \cos^2 \left( \theta_1 + \theta_2 \right) \\ y – 2 y L_1 \sin \left( \theta_1 \right) + {L_1}^2 \sin^2 \left( \theta_1 \right) &=& {L_2}^2 \sin^2 \left( \theta_1 + \theta_2 \right) \end{array} \right. \end{eqnarray} $$

そして、2つの式の左辺と右辺をそれぞれ足し合わせることで、1つの式に変換します。

$$ \begin{eqnarray} x^2 + y^2 – 2 x L_1 \cos \left( \theta_1 \right) – 2 y L_1 \sin \left( \theta_1 \right) + {L_1}^2 \cos^2 \left( \theta_1 \right) + {L_1}^2 \sin^2 \left( \theta_1 \right) && \\ = {L_2}^2 \cos^2 \left( \theta_1 + \theta_2 \right) + {L_2}^2 \sin^2 \left( \theta_1 + \theta_2 \right) && \\ x^2 + y^2 – 2 L_1 \left( x \cos \left( \theta_1 \right) + y \sin \left( \theta_1 \right) \right) + {L_1}^2 \left( \cos^2 \left( \theta_1 \right) + \sin^2 \left( \theta_1 \right) \right) && \\ = {L_2}^2 \left( \cos^2 \left( \theta_1 + \theta_2 \right) + \sin^2 \left( \theta_1 + \theta_2 \right) \right) && \\ x^2 + y^2 – 2 L_1 \left( x \cos \left( \theta_1 \right) + y \sin \left( \theta_1 \right) \right) + {L_1}^2 = {L_2}^2 && \end{eqnarray} $$

その結果、θ₁のみを含む式に変形することが出来ました。

この式をθ₁について解くことで、逆運動学の式の1つを求められます。

θ₁を含む項を左辺にまとめると、

$$ \begin{eqnarray} 2 L_1 \left( x \cos \left( \theta_1 \right) + y \sin \left( \theta_1 \right) \right) &=& x^2 + y^2 + {L_1}^2 – {L_2}^2 \\ x \cos \left( \theta_1 \right) + y \sin \left( \theta_1 \right) &=& \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1} \end{eqnarray} $$

となります。

ここで、三角関数における

$$ \begin{eqnarray} a \cos x + b \sin x = R \cos \left( x – \alpha \right) \\ where \ \left\{ \begin{array}{l} R = \sqrt{a^2+b^2} \\ \alpha = \tan^{-1} \left( \frac{b}{a} \right) \end{array} \right. \end{eqnarray}$$

という関係を用いると、先ほどの式の左辺は

$$ \sqrt{x^2+y^2} \cos \left( \theta_1 – \tan^{-1} \left( \frac{y}{x} \right) \right) = \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1} $$

と変換することが出来ます。

この式をθ₁について解くと

$$ \begin{eqnarray} \cos \left( \theta_1 – \tan^{-1} \left( \frac{y}{x} \right) \right) &=& \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1 \sqrt{x^2+y^2}} \\ \theta_1 – \tan^{-1} \left( \frac{y}{x} \right) &=& \pm \cos^{-1} \left( \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1 \sqrt{x^2+y^2}} \right) \\ \theta_1 &=& \pm \cos^{-1} \left( \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1 \sqrt{x^2+y^2}} \right) + \tan^{-1} \left( \frac{y}{x} \right) \end{eqnarray} $$

となり、θ₁に関する逆運動学の式を求めることが出来ました。

θ₂について求める

同様に、残りのθ₂についての逆運動学の式を求めていきます。

まず、θ₁を求める際に変形した順運動学の式

$$ \begin{eqnarray} \left\{ \begin{array}{l} x – L_1 \cos \left( \theta_1 \right) &=& L_2 \cos \left( \theta_1 + \theta_2 \right) \\ y – L_1 \sin \left( \theta_1 \right) &=& L_2 \sin \left( \theta_1 + \theta_2 \right) \end{array} \right. \end{eqnarray} $$

について、2つ目の式を1つ目の式で割ると

$$ \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } = \frac{ L_2 \sin \left( \theta_1 + \theta_2 \right) }{ L_2 \cos \left( \theta_1 + \theta_2 \right) } $$

という関係式が導かれます。

ここで、右辺について

$$ \frac{ L_2 \sin \left( \theta_1 + \theta_2 \right) }{ L_2 \cos \left( \theta_1 + \theta_2 \right) } = \tan \left( \theta_1 + \theta_2 \right) $$

という変換を行うと、先ほどの式は

$$ \tan \left( \theta_1 + \theta_2 \right) = \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } $$

となります。

この式をθ₂について解くと

$$ \begin{eqnarray} \tan \left( \theta_1 + \theta_2 \right) &=& \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } \\ \theta_1 + \theta_2 &=& \tan^{-1} \left( \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } \right) \\ \theta_2 &=& \tan^{-1} \left( \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } \right) – \theta_1 \end{eqnarray} $$

のように、θ₂に関する逆運動学の式を求めることが出来ました。

逆運動学と複数解

取り扱っている2リンクモデルの「ロボットの位置（x、y）」から「関節の変位（θ₁、θ₂）」を求めるための逆運動学の式は

$$ \begin{eqnarray} \left\{ \begin{array}{l} \theta_1 &=& \pm \cos^{-1} \left( \frac{x^2 + y^2 + {L_1}^2 – {L_2}^2}{2 L_1 \sqrt{x^2+y^2}} \right) + \tan^{-1} \left( \frac{y}{x} \right) \\ \theta_2 &=& \tan^{-1} \left( \frac{ y – L_1 \sin \left( \theta_1 \right) }{ x – L_1 \cos \left( \theta_1 \right) } \right) – \theta_1 \end{array} \right. \end{eqnarray} $$

で表されることが分かりました。

ここで、算出した逆運動学のθ₂の式にはθ₁が含まれていることが分かります。

もちろん、θ₁のみで表されているθ₁の式と同様に、θ₂の式もθ₂のみで表すことは可能です。

しかし、今回紹介した逆運動学の式ではθ₁を含んだ形としています。

これは、逆運動学には複数の解が存在する場合があるためです。