現在自分はパラレルリンクロボットというロボットを勉強がてら作っています．以下のような機体です．
よく見かけるロボットアーム(多関節ロボット)とはだいぶ様相が異なっています．
最下部の移動する部分が常に地面に対して水平になるような構造となっています．
ちまちま開発していたのですが，疲れてしまったので気分転換がてらブログを更新することにしました．
実機製作の話やプログラムの話は今後に回すとして，今回はモーターの角度と移動する部分の位置の関係式を導出する逆運動学の話を書くことにします．
1.イントロダクション
実際に機体を作ったとしても，プログラムが書けなければ動かすことが出来ません．その際にはこのロボットの動きを数式で表す必要があります，
ただ，機構学をまだ履修しておらず，運動学についてはさっぱりわからないので，こちらのPDFをトレースする形になりました．
The Delta Parallel Robot: Kinematics Solutionsパラレルリンクロボットの運動学について，数式や図を用いて解説しているPDFです．
Robert L. Williams II, Ph.D
http://www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf
多分僕の記事はどこか必ず間違いがあるのでこのPDFを見れば間違いないかと思います．
加えて，今回GitHubの方にリポジトリを作成しました．計算の殆どはsympyに計算は投げてまして，その過程も載せてありますので参考になるかと思います．
パラレルリンクロボットの逆運動学問題の導出過程
PLR_Controller/InversePositionKinematics.ipynb at master · TonyMooori/PLR_Controller
どういう機体であるのかという詳細な説明を書くべきですが，面倒なので省略します．上記PDFの最初で扱われているデルタロボットがそれに当たると思うのですがちゃんとした本を見つけられなかったのでなんとも……．
2.各座標の定義
実際に計算を始める前に座標系や各部の名称を決めていきます．まず各部の名称です．
また，肩から肘の部分を上腕，肘から手首の部分を下腕と名付けます．
※一般的な名前ではなく僕が勝手につけた名前です
また各腕は，反時計回りに0,1,2と番号を振ります．
座標系は次のように取ります．胴体の中央を原点としていますが，胴体は移動しないためこれがワールド座標系となります．
次に，各部分の原点からの位置ベクトルの名称を定義します．iは添字（0,1,2)です．
\vec{A_i} …… 肩の位置ベクトル
\vec{B_i} …… 肘の位置ベクトル
\vec{C_i} …… 手首の位置ベクトル
\vec{D} …… 移動体の中央部分の位置ベクトル
\vec{e_i} …… \vec{A_i} 方向の単位ベクトル．互いに120度向きが異なります．
\phi_i …… \vec{e_i} の角度． \frac{2 \pi}{3}i ．
\vec{e_z} …… z方向の単位ベクトル
各部の長さは次のように定義します．
A …… 原点から肩への距離（ \|\vec{A_i}\| )
B …… 肩から肘への距離（上腕の長さ， \| \vec{A_i} - \vec{B_i} \| )
C …… 肘から手首への距離（下腕の長さ， \| \vec{B_i} - \vec{C_i} \| )
C …… 肘から手首への距離（下腕の長さ， \| \vec{B_i} - \vec{C_i} \| )
D …… 手首から移動体の中央部分への距離（ \| \vec{C_i} - \vec{D_i} \| )
・サーボモーターを動かしたら移動体の位置がどうなるかを求めるのが順運動学
・移動体の位置を決めたときのサーボモーターの角度を求めるのが逆運動学
と考えて問題ないと思います．
ネットの記事や1で挙げたPDFなどにも書かれているように，パラレルリンクロボットでは逆運動学の方が順運動学よりも求めやすいそうです．
逆に，シリアルリンクロボット(多関節ロボット)では逆運動学のほうが順運動学よりも求めやすいとのこと．
なのでとりあえず逆運動学問題について考えていきます．
ただ，繰り返すようですが運動学を学んでいないので高校レベルのベクトルで考えていきますのでこの道のプロがやる方法とは異なるかもしれないです．
まず \vec{e_i} について考えると，肩は3つ均等に並べられていることから，
e_i = \left[\begin{matrix}\cos{\left (\phi_{i} \right )}\\\sin{\left (\phi_{i} \right )}\\0\end{matrix}\right]
となります．ただし \phi_{i} = \frac{2 \pi}{3}i ， i は0,1,2のいずれかです．さらに， \vec{A_i} は \vec{e_i} と同じ方向で長さが異なるだけなので，
\vec{A_i} = A \vec{e_i}
です．スカラーのAとベクトルのAが紛らわしいので注意してください．
次に \vec{B_i} について考えます． \vec{B_i} は \vec{A_i} から伸びており， \vec{e_i} と \vec{e_z} 成分しかありません．
したがって， \vec{B_i} は次のように表されます．
\vec{B_i} = \vec{A_i} + B( \vec{e_i}cos(\theta) - \vec{e_z}sin(\theta) )
次に， \vec{C_i} を考える前 \vec{D} を定義します．逆運動学問題では移動体の位置を決めた上で角度 \theta_i を求めるので，移動体の位置は， \vec{D} = \left[\begin{matrix}x\\y\\z\end{matrix}\right] という値が予めわかった上で考えていきます．
これを使うと \vec{C_i} は移動体がねじれずに(z軸に対して回転することなく)移動するため， \vec{C_i} = \vec{D} + D\vec{e_i} となります．
ここで，2で書いた定義， C = \| \vec{B_i} - \vec{C_i} \| を用いることで \theta_i が求まります．この式は求めたい変数 \theta_i ひとつだけなので非常に簡単というか，個人的には驚きです．
この式を両辺を二乗すると次のようになります．
C^{2} = \left(B \sin{\left (\theta_{0} \right )} + z\right)^{2} + \left(- A \sin{\left (\phi_{0} \right )} - B \sin{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + D \sin{\left (\phi_{0} \right )} + y\right)^{2} + \left(- A \cos{\left (\phi_{0} \right )} - B \cos{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + D \cos{\left (\phi_{0} \right )} + x\right)^{2} 更に展開して， C^{2} = A^{2} + 2 A B \cos{\left (\theta_{0} \right )} - 2 A D - 2 A x \cos{\left (\phi_{0} \right )} - 2 A y \sin{\left (\phi_{0} \right )} + B^{2} - 2 B D \cos{\left (\theta_{0} \right )} - 2 B x \cos{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} - 2 B y \sin{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + 2 B z \sin{\left (\theta_{0} \right )} + D^{2} + 2 D x \cos{\left (\phi_{0} \right )} + 2 D y \sin{\left (\phi_{0} \right )} + x^{2} + y^{2} + z^{2} となります．この辺はsympyという数式処理ソフトに投げてるので手計算ではないです．
この式と2,3分ほどにらめっこすると，こういう式であることに気づきます．
P + Q \sin{\left (\theta_{i} \right )} + R \cos{\left (\theta_{i} \right )} = 0 ただし，P,Q,Rは次のような値です． \begin{eqnarray*} P &=& - A^{2} + 2 A D + 2 A x \cos{\left (\phi_{i} \right )} + 2 A y \sin{\left (\phi_{i} \right )} - B^{2} + C^{2} - D^{2} \\ &-& 2 D x \cos{\left (\phi_{i} \right )} - 2 D y \sin{\left (\phi_{i} \right )} - x^{2} - y^{2} - z^{2}\\ Q &=& - 2 B z \\ R &=& - 2 A B + 2 B D + 2 B x \cos{\left (\phi_{i} \right )} + 2 B y \sin{\left (\phi_{i} \right )} \end{eqnarray*}
この式から， \theta_i を求めます． t = tan(\frac{\theta_i}{2}) を置くことで，この式を２次方程式にすることができ，そこから \theta_i を求めることが出来ます．
こちらはsympyに投げてるので途中式を省略します．結果は次のようになります． \begin{eqnarray*} \theta_i &=& - 2 \operatorname{atan}{\left (\frac{1}{P - R} \left(Q - \sqrt{- P^{2} + Q^{2} + R^{2}}\right) \right )} \\ \theta_i ' &=& - 2 \operatorname{atan}{\left (\frac{1}{P - R} \left(Q + \sqrt{- P^{2} + Q^{2} + R^{2}}\right) \right )} \end{eqnarray*}
２次方程式，と言った時点で気づいた方もいるかもしれませんが，解が2つ出てきます．これは肘が内側に来てる状態も含まれているためです．
因みに，適切な方を選ばなければこのような状態になります．
それぞれの腕に対して2パターンあるので，この条件が成立する状態がこのロボットでは合計8パターン存在します．
加えて，どちらが適切であるかどうかは角度の絶対値が小さい方，すなわち0付近である方を選択することでたいていうまくいきます（理論的な根拠があるわけではないです）．
そもそも不適切な方の角度はサーボモーターの移動範囲外である事が多いため，このような状態にはならないはずですが，間違えるとサーボモーターを傷めるので注意してください．
また根号の中が負となる場合がありますが，その場合はこのロボットの移動範囲外であるということを表していると思って問題ないと思います．
プログラムは個々に貼り付けると長くなるのでGitHubのリポジトリの方においておきます．
結果はこのようになります．
必要ならば各ベクトルの距離を計算して調べてみるとよいかと思います．
Amazonで購入した大きめのトルクの物を動かしています．
回路図はこんな感じです．サーボとマイコンを繋げただけです．
プログラムはこれもGitHubにあげてあります．
サーボモータの角度は次のように定義します．
\theta_i …… i番目の腕につけられたサーボモーターの角度．±90°とする．正なら上腕が胴体よりも下に行く．
3.逆運動学
ではまず逆運動学から考えていきます．補足ですが，・サーボモーターを動かしたら移動体の位置がどうなるかを求めるのが順運動学
・移動体の位置を決めたときのサーボモーターの角度を求めるのが逆運動学
と考えて問題ないと思います．
ネットの記事や1で挙げたPDFなどにも書かれているように，パラレルリンクロボットでは逆運動学の方が順運動学よりも求めやすいそうです．
逆に，シリアルリンクロボット(多関節ロボット)では逆運動学のほうが順運動学よりも求めやすいとのこと．
なのでとりあえず逆運動学問題について考えていきます．
ただ，繰り返すようですが運動学を学んでいないので高校レベルのベクトルで考えていきますのでこの道のプロがやる方法とは異なるかもしれないです．
まず \vec{e_i} について考えると，肩は3つ均等に並べられていることから，
e_i = \left[\begin{matrix}\cos{\left (\phi_{i} \right )}\\\sin{\left (\phi_{i} \right )}\\0\end{matrix}\right]
となります．ただし \phi_{i} = \frac{2 \pi}{3}i ， i は0,1,2のいずれかです．さらに， \vec{A_i} は \vec{e_i} と同じ方向で長さが異なるだけなので，
\vec{A_i} = A \vec{e_i}
です．スカラーのAとベクトルのAが紛らわしいので注意してください．
次に \vec{B_i} について考えます． \vec{B_i} は \vec{A_i} から伸びており， \vec{e_i} と \vec{e_z} 成分しかありません．
次に， \vec{C_i} を考える前 \vec{D} を定義します．逆運動学問題では移動体の位置を決めた上で角度 \theta_i を求めるので，移動体の位置は， \vec{D} = \left[\begin{matrix}x\\y\\z\end{matrix}\right] という値が予めわかった上で考えていきます．
これを使うと \vec{C_i} は移動体がねじれずに(z軸に対して回転することなく)移動するため， \vec{C_i} = \vec{D} + D\vec{e_i} となります．
ここで，2で書いた定義， C = \| \vec{B_i} - \vec{C_i} \| を用いることで \theta_i が求まります．この式は求めたい変数 \theta_i ひとつだけなので非常に簡単というか，個人的には驚きです．
この式を両辺を二乗すると次のようになります．
C^{2} = \left(B \sin{\left (\theta_{0} \right )} + z\right)^{2} + \left(- A \sin{\left (\phi_{0} \right )} - B \sin{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + D \sin{\left (\phi_{0} \right )} + y\right)^{2} + \left(- A \cos{\left (\phi_{0} \right )} - B \cos{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + D \cos{\left (\phi_{0} \right )} + x\right)^{2} 更に展開して， C^{2} = A^{2} + 2 A B \cos{\left (\theta_{0} \right )} - 2 A D - 2 A x \cos{\left (\phi_{0} \right )} - 2 A y \sin{\left (\phi_{0} \right )} + B^{2} - 2 B D \cos{\left (\theta_{0} \right )} - 2 B x \cos{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} - 2 B y \sin{\left (\phi_{0} \right )} \cos{\left (\theta_{0} \right )} + 2 B z \sin{\left (\theta_{0} \right )} + D^{2} + 2 D x \cos{\left (\phi_{0} \right )} + 2 D y \sin{\left (\phi_{0} \right )} + x^{2} + y^{2} + z^{2} となります．この辺はsympyという数式処理ソフトに投げてるので手計算ではないです．
この式と2,3分ほどにらめっこすると，こういう式であることに気づきます．
P + Q \sin{\left (\theta_{i} \right )} + R \cos{\left (\theta_{i} \right )} = 0 ただし，P,Q,Rは次のような値です． \begin{eqnarray*} P &=& - A^{2} + 2 A D + 2 A x \cos{\left (\phi_{i} \right )} + 2 A y \sin{\left (\phi_{i} \right )} - B^{2} + C^{2} - D^{2} \\ &-& 2 D x \cos{\left (\phi_{i} \right )} - 2 D y \sin{\left (\phi_{i} \right )} - x^{2} - y^{2} - z^{2}\\ Q &=& - 2 B z \\ R &=& - 2 A B + 2 B D + 2 B x \cos{\left (\phi_{i} \right )} + 2 B y \sin{\left (\phi_{i} \right )} \end{eqnarray*}
この式から， \theta_i を求めます． t = tan(\frac{\theta_i}{2}) を置くことで，この式を２次方程式にすることができ，そこから \theta_i を求めることが出来ます．
こちらはsympyに投げてるので途中式を省略します．結果は次のようになります． \begin{eqnarray*} \theta_i &=& - 2 \operatorname{atan}{\left (\frac{1}{P - R} \left(Q - \sqrt{- P^{2} + Q^{2} + R^{2}}\right) \right )} \\ \theta_i ' &=& - 2 \operatorname{atan}{\left (\frac{1}{P - R} \left(Q + \sqrt{- P^{2} + Q^{2} + R^{2}}\right) \right )} \end{eqnarray*}
２次方程式，と言った時点で気づいた方もいるかもしれませんが，解が2つ出てきます．これは肘が内側に来てる状態も含まれているためです．
因みに，適切な方を選ばなければこのような状態になります．
加えて，どちらが適切であるかどうかは角度の絶対値が小さい方，すなわち0付近である方を選択することでたいていうまくいきます（理論的な根拠があるわけではないです）．
そもそも不適切な方の角度はサーボモーターの移動範囲外である事が多いため，このような状態にはならないはずですが，間違えるとサーボモーターを傷めるので注意してください．
また根号の中が負となる場合がありますが，その場合はこのロボットの移動範囲外であるということを表していると思って問題ないと思います．
4.シミュレーション
シミュレーションというか，可視化というか，ちゃんと数式があっているかの確認を行います．プログラムは個々に貼り付けると長くなるのでGitHubのリポジトリの方においておきます．
PLR_Controller/IPK_Visualize.ipynb at masterTonyMooori/PLR_Controller
結果はこのようになります．
必要ならば各ベクトルの距離を計算して調べてみるとよいかと思います．
5.実機による実験
Arduino Microを用いて3つのサーボモータを制御します．Amazonで購入した大きめのトルクの物を動かしています．
回路図はこんな感じです．サーボとマイコンを繋げただけです．
PLR_Controller/Arduino/trace_circle at masterTonyMooori/PLR_Controller実際の動画です．とりあえずサンプルとして円を描くような軌跡で動かしています．
今回は以上ですが，もうしばらく続くかと思います．パラレルリンクロボットのテスト動画．円状の軌跡で動かす． pic.twitter.com/MzcZSWXja4— もりとにー (@TonyMooori) 2016年9月18日
