ロボットアームのXYZ座標の計算式

去年からやっているロボットアームの位置制御でX,Y,Z座標からモーターの角度を計算する式で一番シンプルなものができたのでメモ。



条件：水平方向に動くモータが１つ、垂直方向に動くモーターが２個あるロボットアームのとき

それぞれの部位の名称を

　・付け根を肩（垂直軸Base、水平軸Shoulder）、
　・中間を肘（Elbow）、
　・先端を手首（Wrist）

かつ腕の長さが等分であると定義する。


上記条件において、手首WristのX、Y、Z座標を与えた時それぞれの角度を求める式。（決定版）

///

/// ShoulderとElbowの長さが一緒なら二等辺三角形で計算式を解く
///
///
///
///
public void Calc(double x, double y, double z)
{
var h = Math.Sqrt(x * x + y * y);
var k = Math.Sqrt(h * h + z * z);
var pa = Math.Atan(z / h);
var pb = Math.Acos(k / 2.0);

X = x;
Y = y;
Z = z;
ThetaBase = Math.Atan(y / x);
ThetaShoulder = pa + pb;
ThetaElbow = -2.0 * pb;
}

腕の関節の長さが一緒なら二等辺三角形になる。

そうすることで何が嬉しいかというと、2等辺三角形の公式が使えるので

辺の長さを無視する事が出来る。

つまり、XYZ座標から斜辺ｋを求めれば全ての角度が計算できる事を意味している。

ロボットをこれからやろうと思った人はぜひ、参考にしてください。