お習字ロボットアームの逆キネ

スマホの画面に指で書いた字をロボットアームが筆ペンで書く、お習字ロボットアーム。その制御のキモである逆キネについて解説。といっても、サーボのトルクが十分大きく、十分ゆっくり筆を動かすという前提で速度・加速度については考慮しません。（ベクトル解析？なにそれ美味しいの？）また後述するように、計算が簡単になるように考えてアームの寸法を設計してあります。

(1)ヨー軸

真上から見た水平面上の回転について。
肩を原点として、筆先の座標を(x,y)とすると、
原点からの筆先までの距離r および x軸とアームのなす角θは、
$x = r\cos\theta$
$y = r\sin\theta$
より
$r = \sqrt{x^2+y^2}$
$\theta = \tan^{-1}\frac{y}{x}$
※ ゼロ除算と象限には注意

(1)ピッチ軸

真横から見た垂直面上の回転について。
肩-肘のリンクと肘-手首のリンクの長さを等しく設計してあるので、
肩・肘・手首を頂点とする三角形は二等辺三角形になる。
筆先を紙に下ろしたとき、二等辺三角形の底辺を常に水平に保つとする。
この三角形の角を $\varphi_{1},\varphi_{2},\varphi_{3}$ とする。
また、リンクの長さをL、手首から先の長さをdとし、
肩から筆先までの水平距離をr、肩から手首までの水平距離をr'とすると、
$\varphi_{2} = \pi - 2\varphi_{1}$
$\varphi_{3} = \varphi_{1}$
また、
$r' = 2L\cos\varphi_{1}$
$r = r' + d$
$r = \sqrt{x^2+y^2}$
より
$\varphi_{1} = \cos^{-1}\frac{sqrt{x^2+y^2}-d}{2L}$