つくたろうのブログ

ポケモン色違い厳選や国立大学巡りが好きな大学生、つくたろうの気ままな雑記帳です。

読まれている記事

【ロボット工学】3軸リンクにおける逆運動学の式の導出【メカトロニクス】

こんにちは、つくたろうです。

今回は、2リンク・3軸マニピュレータにおける逆運動学の式の導出について紹介します。

分野としては、ロボット工学やメカトロニクスの分野になると思いますが、数式の導出についての記事なので数学カテゴリに入れちゃいます(笑)

逆運動学とは

式の導出に先立って、「そもそも逆運動学とは何か」ということについて軽く説明したいと思います。

逆運動学というのは順運動学と対になる言葉・考え方です。

長さの分かっているリンクで構成されたマニピュレータについて、順運動学が「リンクの角度を与えた時にマニピュレータの手先位置の座標を得る」というものであることに対して、逆運動学は「マニピュレータの手先位置の座標を与えたときに、各リンクの角度を求める」というものになります。

例を紹介します。


上図を、1リンク・1軸のマニピュレータの模式図だと思ってください。手描きですみません(笑)

原点に軸があって、マニピュレータの手先位置は(l_x,l_y)とします。リンクの長さl、回転角を\thetaとします。

このとき、簡単な三角関数を考えれば以下の式が成り立つことは皆さん分かると思います。
\begin{array} {}l_x = l \cos \theta \\ l_y = l \sin \theta \end{array}
ここで、l_x,l_y\thetaの式で表現するのが順運動学\thetal_x,l_yの式で表すのが逆運動学です。

この例については、上の三角関数の式から、
順運動学
\begin{array} {}l_x = l \cos \theta \\ l_y = l \sin \theta \end{array}
逆運動学
\begin{array} {} \displaystyle \theta = \arctan{ \left( \frac{l_y}{l_x} \right)} \end{array}
と、ともに簡単に求められると思いますが、軸やリンクが増えたときにも同じことができることが望ましいわけです。

3軸・2リンクのマニピュレータの逆運動学の式の導出

ということで、今から3軸・2リンクのマニピュレータの逆運動学について考えてみましょう。

ちなみに、3軸・2リンクの場合でも順運動学はそのまま簡単な三角関数として求められます。これについても導出の途中で紹介します。

問題設定

※数式が横に切れているときは、スライドすると先まで見ることができます。また、スマホを横向きにして見ると画面内に式全体が収まります。

今回は2リンク3軸マニピュレータの逆運動学の式の導出なので、以下のような模式図でマニピュレータを考えてみましょう。

2リンク3軸マニピュレータ模式図
2リンク3軸マニピュレータ模式図

3次元の座標系での模式図は上の通りで、図中の\theta_0,\theta_1,\theta_2を、x,y,zの式で表します。

リンクの長さは図に示してある通り、l_1,l_2としています。

また、図二枚目のSとは、S = \sqrt{x^{2} + z^{2}}を満たす、仮の座標軸です。計算をわかりやすくするために導入しています。

それでは、さっそく導出していきましょう!

逆運動学の式の導出

まず、先の模式図について三角関数での座標表記を考えることで、以下の式が得られます:
\begin{array}{} x = ( l_1 \cos{\theta_1} + l_2 \cos{\theta_2}) \cos{\theta_0} & \cdots (1) \\ y = l_1 \sin{\theta_1} + l_2 \sin{\theta_2} & \cdots (2)\\ z = ( l_1 \cos{\theta_1} + l_2 \cos{\theta_2}) \sin{\theta_0} & \cdots (3) \end{array}

これらの(1),(2),(3)式をまとめて式(A)とします。

この式(A)において、先ほど導入したS = \sqrt{x^{2} + z^{2}}を満たす仮の座標Sを用いると、上のx,y,zを以下のように表すことができます:
\begin{array}{} y = l_1 \sin{\theta_1} + l_2 \sin{\theta_2} & \cdots{(1) ' }\\ S = l_1 \cos{\theta_1} + l_2 \cos{\theta_2} & \cdots{(2) '} \end{array}

これらの{(1) '},{(2) '}式をまとめて式(B)とします。

①:\theta_0の導出

上記の式(A)のうち式(1),(3)において右辺同士、左辺同士でそれぞれ割ることで以下の式を得る:
\begin{array}{} \displaystyle \cfrac{z}{x} = \cfrac{\sin{\theta_0}}{\cos{\theta_0}} = \tan{\theta_0} \end{array}
したがって、\theta_0は、以下の通り。
\begin{array}{} \displaystyle \theta_0 = \arctan{\left( \cfrac{z}{x} \right)} & \cdots (a) \end{array}

メカトロニクス入門 (First Stageシリーズ)

メカトロニクス入門 (First Stageシリーズ)

  • 発売日: 2014/10/01
  • メディア: 単行本

②:\theta_1の導出

上記の式(B)を変形して、
\begin{array}{} y - l_1 \sin{\theta_1} &=& l_2 \sin{\theta_2} \\ S - l_1 \cos{\theta_1} &= & l_2 \cos{\theta_2} \end{array}

両辺ともに2乗して足し合わせると、({l_2 \sin{\theta_2}})^{2} + ({l_2 \cos{\theta_2}})^{2} = {l_2}^{2}より以下の式を得る:
\begin{array}{} (y - l_1 \sin{\theta_1})^{2} + (S - l_1 \cos{\theta_1})^{2} &=& {l_2}^{2} \end{array}
これを展開して、
\begin{array}{} y^{2} - 2 y {l_1 \sin{\theta_1}} + {l_1 \sin{\theta_1}}^{2} + S^{2} - 2 S {l_1 \cos{\theta_1}} + {l_1 \cos{\theta_1}}^{2} &=& {l_2}^{2} \end{array} ({l_1 \sin{\theta_1}})^{2} + ({l_1 \cos{\theta_1}})^{2} = {l_1}^{2}を用いて整理すると、
\begin{array}{} y^{2} + S^{2} - 2 l_1 (y {\sin{\theta_1}} + S {\cos{\theta_1}}) &=& {l_2}^{2} - {l_1}^{2} \end{array}
よって、
\begin{array}{} \displaystyle \frac{y^{2} + S^{2} - ({l_2}^{2} - {l_1}^{2})}{2 l_1} &=& y {\sin{\theta_1}} + S {\cos{\theta_1}} \\ &=& \displaystyle \sqrt{y^{2} + S^{2}} \sin{(\theta_1 + \alpha)} & (\alpha = \arctan{\left( \cfrac{S}{y} \right)}、三角関数の合成) \end{array}
したがって、
\begin{array}{} \displaystyle \theta_1 + \alpha &=& \arcsin{ \left( \cfrac{y^{2} + S^{2} - ({l_2}^{2} - {l_1}^{2})}{2 l_1 \sqrt{y^{2} + S^{2}}}\right)} \end{array}
ここで、S^{2} = {x^{2} + z^{2}}および\alpha = \arctan{\left( \cfrac{S}{y} \right)} = \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)}を代入すると、
\begin{array}{} \displaystyle \theta_1 + \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)} &=& \arcsin{ \left( \cfrac{x^{2} + y^{2} + z^{2} - ({l_2}^{2} - {l_1}^{2})}{2 l_1 \sqrt{x^{2} + y^{2} + z^{2}}}\right)} \end{array}
したがって、\theta_1は以下の通りとなる:
\begin{array}{} \displaystyle \theta_1 &=& \arcsin{ \left( \cfrac{x^{2} + y^{2} + z^{2} - ({l_2}^{2} - {l_1}^{2})}{2 l_1 \sqrt{x^{2} + y^{2} + z^{2}}}\right)} - \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)} & \cdots(b) \end{array}

③:\theta_2の導出

\theta_1と同様に式(B)を、今度は以下のように変形します:
\begin{array}{} y - l_2 \sin{\theta_2} &=& l_1 \sin{\theta_1} \\ S - l_2 \cos{\theta_2} &= & l_1 \cos{\theta_1} \end{array}

この変形は「\theta_1の時と、l\thetaの添え字が入れ替わるだけであることが分かると思います。

したがって、あとは\theta_1を求めるときと同じプロセスを行ってください。

答えもl\thetaの添え字が入れ替わるだけなので、\theta_2は以下のようになります:
\begin{array}{} \displaystyle \theta_2 &=& \arcsin{ \left( \cfrac{x^{2} + y^{2} + z^{2} - ({l_1}^{2} - {l_2}^{2})}{2 l_2 \sqrt{x^{2} + y^{2} + z^{2}}}\right)} - \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)} & \cdots(c) \end{array}

導出結果

上記の式(a)(b)(c)で求まったとおり、3軸・2リンクのマニピュレータの逆運動学の式({\theta_0},{\theta_1},{\theta_2}x,y,zの式で表したもの)は、以下の通りになります:
\begin{array}{} \displaystyle \theta_0 &= \arctan{\left( \cfrac{z}{x} \right)} \\ \displaystyle \theta_1 &= \arcsin{ \left( \cfrac{x^{2} + y^{2} + z^{2} - ({l_2}^{2} - {l_1}^{2})}{2 l_1 \sqrt{x^{2} + y^{2} + z^{2}}}\right)} - \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)} \\ \displaystyle \theta_2 &= \arcsin{ \left( \cfrac{x^{2} + y^{2} + z^{2} - ({l_1}^{2} - {l_2}^{2})}{2 l_2 \sqrt{x^{2} + y^{2} + z^{2}}}\right)} - \arctan{\left( \cfrac{\sqrt{x^{2} + z^{2}}}{y}\right)} \end{array}

えらく複雑な式ですが、Unityなどでマニピュレータを作る際にはこの逆運動学の式をプログラミングで実装することになるんですね。大変ですが頑張りましょう。

おわりに

今回は、「3軸・2リンクのマニピュレータの逆運動学の式の導出」でした。皆さんの参考になれば幸いです。

久しぶりにTeX記法で数式を書いたのですが、ほとんど書き方を忘れてしまっていました(笑)

[改訂第8版]LaTeX2ε美文書作成入門

[改訂第8版]LaTeX2ε美文書作成入門

この記事を作成する際は間違い等無いように細心の注意を払っておりますが、もし間違い等見つけられた方がいらっしゃいましたら遠慮なくコメントしてください。

また、もしこの式の導出で分からない点などありましたら、こちらもコメントくだされば対応いたします!

これからもよろしくお願いします。

ここまで読んでくれて、ありがとうございました!

2021/04/18 つくたろう

メカトロニクス入門 (第2版)

メカトロニクス入門 (第2版)


大学生ランキング