019_RobotArm.mw

 

ロボットアーム 

 

イントロダクション 

この例では、ロボットの代表的な経路計画問題に Maple を使います。2リンク平面ロボットの先端が、直線軌道に従うように指示されます。2リンクロボットは回転関節を持ちます。これにより、ロボットが平面状を動くように制限しています。
 

このような問題やロボットの形状は、組み立てラインの作業では一般的に使用されているものです。 

 

Image 

 

Maple のグラフィックスとアニメーションの機能は、ロボットの動きを表示するためと、動きの範囲を示すために使います。このような可視化により、ロボットが許容範囲内で動作可能かを見ることができます。 

 

解決方法 

軌道を通る(たどる)ためには、逆運動学と呼ばれる問題を解かなくてはなりません。逆運動学は、ロボットの先端が、端点で指定される直線に従うよう、角度と幾何学的状態を求めることを扱います。
 

逆運動学の方程式を解いた後、関節角度をプロットします。また、Maple のグラフィックス機能を使い、ロボットの動きをアニメーションし、動きの範囲をプロットします。
 

必要なパッケージを読み込みます。 

> with(plots):
 

逆運動学の式 

オペレータ(operator)表現を使い、逆運動学の式を定義します。 

> Theta[2] := arccos(`*`(`+`(`-`(`*`(`^`(L1, 2))), `-`(`*`(`^`(L2, 2))), `*`(`^`(x, 2)), `*`(`^`(y, 2))), `*`(`/`(`+`(`*`(2, `*`(L1, `*`(L2))))))))
 

Typesetting:-mprintslash([Theta[2] := arccos(`+`(`/`(`*`(`/`(1, 2), `*`(`+`(`-`(`*`(`^`(L1, 2))), `-`(`*`(`^`(L2, 2))), `*`(`^`(x, 2)), `*`(`^`(y, 2))))), `*`(L1, `*`(L2)))))], [arccos(`+`(`/`(`*`(`/`... (3.1)
 

> Theta[1] := `+`(arctan(y, x), `-`(arctan(`/`(`*`(L2, `*`(sin(Theta[2]))), `*`(`+`(L1, `*`(L2, `*`(cos(Theta[2])))))))))
 

Typesetting:-mprintslash([Theta[1] := `+`(arctan(y, x), `-`(arctan(`+`(`/`(`*`(`/`(1, 2), `*`(L2, `*`(`^`(`+`(4, `-`(`/`(`*`(`^`(`+`(`-`(`*`(`^`(L1, 2))), `-`(`*`(`^`(L2, 2))), `*`(`^`(x, 2)), `*`(`^`... (3.2)
 

 

> Theta[1] := unapply(Theta[1], x, y); 1Theta[2] := unapply(Theta[2], x, y); 1
 

 

Typesetting:-mprintslash([Theta[1] := proc (x, y) options operator, arrow; `+`(arctan(y, x), `-`(arctan(`+`(`/`(`*`(`/`(1, 2), `*`(L2, `*`(`^`(`+`(4, `-`(`/`(`*`(`^`(`+`(`-`(`*`(`^`(L1, 2))), `-`(`*`(...
Typesetting:-mprintslash([Theta[2] := proc (x, y) options operator, arrow; arccos(`+`(`/`(`*`(`/`(1, 2), `*`(`+`(`-`(`*`(`^`(L1, 2))), `-`(`*`(`^`(L2, 2))), `*`(`^`(x, 2)), `*`(`^`(y, 2))))), `*`(L1, ... (3.3)
 

 

経路設計 

指定された軌道に沿って動く、ロボットの関節角度を表す式を求めます。特に直線は、始点 (x1, y1) と (x2, y2) をつないだ、パラメトリックな形で定義します。Theta__1Theta__2 を軌道上のいくつかの点で評価します。パラメータ t は、点と点の間をコントロールします。( t は 0.05 刻みに増えます) 


はじめに、変数を初期化します。
 

> increment := 0.05-1;
divisions := round();
q1 := Array(1 .. 2*divisions);
q2 := Array(1 .. 2*divisions);
 

 

 

 

increment := 0.5e-1
divisions := 21
q1 := Array(%id = 4473922834)
q2 := Array(%id = 4473971554) (4.1)
 

 

ループの中で、関節角度を計算します。 

 

> i := 1;
for t from 0 by increment to 1 do
 x := `+`(x1, `*`(t, `*`(`+`(x2, `-`(x1))))); 1
 y := `+`(y1, `*`(t, `*`(`+`(y2, `-`(y1))))); 1
 q1[i] := t;
 q2[i] := t;
 q1[i+1] := evalf(convert( , degrees));
 q2[i+1] := evalf(convert( , degrees));
 i := i+2 ;
end do;
 

 

> degrees := 1:
 

 

上の式は今、一般的な形です。例えば、q1[2]: 

 

> q1[2];
 

`+`(`*`(57.29577950, `*`(arctan(y1, x1))), `-`(`*`(57.29577950, `*`(arctan(`+`(`/`(`*`(.5000000000, `*`(L2, `*`(`^`(`+`(4., `-`(`/`(`*`(1., `*`(`^`(`+`(`-`(`*`(1., `*`(`^`(L1, 2)))), `-`(`*`(1., `*`(`... (4.2)
 

 

これから、ロボットリンクの長さと軌道の始点と終点の位置を定義します。 これらの値を変えれば、ロボットの別の応答を見ることができます。 

 

 

軌道1 

リンクの長さを定義します。 

> L1 := 2:
L2 := 2:
 

 

直線軌道の始点を定義します。 

> x1 := 2:
y1 := 3:
 

 

直線軌道の終点を定義します。 

> x2 := -3:
y2 := 0.5:
 

 

指定した直線軌道に沿って動く、ロボットの角度をプロットします。 

 

* 出力される式が長い為、ここでは出力を抑えています。出力される式は、実際のワークシートをご覧ください。 

> data1 := evalf(convert(q1, list)):
 

> data2 := evalf(convert(q2, list)):
 

 

> plot1 := pointplot(data1, color=red):
plot2 := pointplot(data2, color=blue):
display(plot1, plot2, title="Robot arm trajectories");
 

Plot_2d
 

 

ロボットの動きをアニメーションにするために、関節位置を計算します。 

> pl := Array(1 .. divisions);
i := 1;
for t from 0 by increment to 1 do
 x := `+`(x1, `*`(t, `*`(`+`(x2, `-`(x1))))); 1
 y := `+`(y1, `*`(t, `*`(`+`(y2, `-`(y1)))));
 px1 := [0, 0,
 pl[i] := pointplot(px1, style = LINE, scaling = CONSTRAINED, color = blue);
 i := i+1;
end do;
 

 

ロボットアームの可動範囲のプロットと動きのアニメーションです。 

> display(convert(pl, list), title="Robot swept area");
 

Plot_2d
 

> display(convert(pl, list), insequence=true, title="Robot arm motion");
 

Plot_2d
 

 

軌道2 

新しい端点を定義して、別の直線軌道を見ます。 

> x1 := 2:
y1 := 3:
x2 := -2:
y2 := 3:
 

 

新しい直線軌道に沿って動く、新しいロボットの角度をプロットします。 

> plot1 := pointplot(data1, color=red):
plot2 := pointplot(data2, color=blue):
display([plot1, plot2], title="Robot angle trajectories");
 

Plot_2d
 

 

ロボットの動きをアニメーションするための、新しい関節位置を計算します。 

> pl := Array(1 .. divisions);
i := 1;
for t from 0 by increment to 1 do
 x := `+`(x1, `*`(t, `*`(`+`(x2, `-`(x1))))); 1
 y := `+`(y1, `*`(t, `*`(`+`(y2, `-`(y1))))); 1
 px1 := [0, 0, ;
 pl[i] := pointplot(px1, style = LINE, scaling = CONSTRAINED, color = blue);
 i := i+1;
end do;
 

 

新しい軌道のロボットの動きをプロットし、アニメーションします。 

> display(convert(pl, list), title="Robot swept area");
 

Plot_2d
 

> display(convert(pl,list), insequence=true, title="Robot arm motion");
 

Plot_2d
 

>
 

Copyright (c) 2005-2015, Cybernet Systems, Co., Ltd. All rights reserved.