ネット上で見当たらなかったのでメモも兼ねて

平面パラレルリンクロボットというのは上のようなロボットで，根本についた2つのモータで手先を動かすやつ
このロボットで手先位置P(Tx,Ty)が与えられたときに各モータの角度θ1,θ2を求める逆運動学の式を導出します.(順運動学はよくわかりませんでした,このロボットに関しては逆運動のほうが簡単に求められるっぽいです).M1とM2は各モータの出力軸の位置.

θ1もθ2もアプローチは同じなのでθ1だけ求めます



手先であるPから垂直に補助線を引くと直角三角形ができます.このときdxとdyは与えられている手先位置から



また角度θaは

斜辺の長さcは三平方の定理を用いて

ここで余弦定理から

の関係が成り立つのでθbは


で求められて,ここからモータ1の出力すべき角度θ1は

となって手先位置からモータの角度が求められましたﾊﾟﾁﾊﾟﾁ
実装するときはdxが負になったり,サーボ角度との整合とかあると思うのでそのへんは各自頑張ってください.

手先の操作範囲はM1,M2から半径R=L1+L2の円を描いてその範囲内で,実際には特異点はよろしくないので操作範囲には余裕をもたせたほうがいいですたぶん.