2016年2月15日月曜日

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


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



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

それぞれの部位の名称を

 ・付け根を肩(垂直軸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座標から斜辺kを求めれば全ての角度が計算できる事を意味している。


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

コメントを投稿

Androider