2自由度マニピュレータ
こんにちは。
いつも銭湯に入ったりお絵描きばかりしているハリボーです。
毎日のんびり生きていますが、たまには頑張って研究らしきものをすることがあります。
実は大学院で機械工学を本格的に取り組んでいた過去もあります。
というわけで今回は、機械工学の中でもポピュラーなロボットアームの制御について語ります。
あまり堅苦しくないよう、ロボットはLEGOマインドストームEV3を使ってみました。
制御の世界では頻繁に用いられる2自由度マニピュレータ（ロボットアーム）ですが、現実世界でも産業用ロボットをはじめとして多くの機構に用いられます。
LEGOで作ったアームに制御ソフトウェアのMATLABを用いて動かしてみようと思います。
この記事で紹介するもの・学べるもの
- 2自由度マニピュレータとは何か
- 順運動学と逆運動学
- MATLABを用いたEV3の制御プログラム
アームのモデル化
今回用いるロボットアームはこんな感じ。
回転関節が2個、2自由度を有するマニピュレータです。
自由度については詳しくはこちらの本で紹介しています。
メカトロニクス技術はロボットやIoTが増えるこれからの時代において非常に求められています。
まずは2自由度マニピュレータを簡単なモデルに置き換えます。
2自由度マニピュレータのスケルトン図はこちら。
手先位置\((x,y)\)はアームの長さ\(l_1,l_2\)およびそれぞれの関節角度\(θ_1,θ_2\)を用いることで
\(x=l1\cos(θ_1)+l2\cos(θ_1+θ_2)\)
\(y=l1\sin(θ_1)+l2\sin(θ_1+θ_2)\)
と表すことができます。
今回のアームの長さはそれぞれ\(l_1=65,l_2=120\rm[mm]\)です。
順運動学の実験検証
まずは関節角度を指定してアームを動かしてみましょう。
いわゆる順運動学です。
EV3とMATLABの環境構築はこちらの記事で解説しています。
MATLAB用のmファイルはこちら。
%forward_kinematics.m
l1=65;
l2=120;
t1=30; %target theta_1
t2=30; %target theta_2
x=l1*cos(deg2rad(t1))+l2*cos(deg2rad(t1+t2))
y=l1*sin(deg2rad(t1))+l2*sin(deg2rad(t1+t2))
resetRotation(mymotor1)
resetRotation(mymotor2)
motor_move(mymotor1,10,t1)
motor_move(mymotor2,10,t2)
function motor_move(motorname,power,theta)
rotation=readRotation(motorname);
error=theta-rotation;
while(abs(error)>2)
motorname.Speed=error/power+10;
start(motorname);
rotation=readRotation(motorname);
error=(theta-rotation);
end
stop(motorname);
end実験では\(θ_1=30,θ_2=30\rm[deg]\)としました。
MATLABでの返り値は\(x=116.2917,y=136.4230\rm[mm]\)と、だいたいそれっぽいところに動いたことを検証できました。
EV3のモータパワーによって少しずれることがよくあります。
今回はP制御で動かしていますが、正確さを求めるならPIDなどを用いるのも良いかもしれません。
以上のように順運動学用いてマニピュレータを簡単に動作させてみました。
ここまでだとMATLABを使う必要もなく、ただのEV3ソフトウェアのブロックでも再現可能です。
それでは続いて、目標手先位置が指定された場合の関節角度を逆算して動作させる逆運動学を用いたいと思います。
制御らしくなってきました。
逆運動学でマニピュレータを動作させる
MATLAB では逆運動学ソルバーという便利機能がありますが、理論が飛んでしまうので今回は使用しません。
要は逆運動学の式を\(θ\)について解けば良いわけです。
導出する方法はいつくかありますが、今回は文献[1]に基づいて記載していきます。
目標手先位置\((x,y)\)が与えられた時の関節角度\(θ_1,θ_2\)は
\(θ_1=\rm atan2\) \((y,x)+\) \(\rm atan2\) \((κ,x^2+y^2+l^2_1−l^2_2)\)
\(θ_2=\rm atan2\) \((κ,x^2+y^2−l^2_1−l^2_2)\)
で表されます。ただし、
\(\kappa=\sqrt{(x^2+y^2+l_1^2-l_2^2)-2{(x^2+y^2)^2+l_1^4-l_2^4}}\)
です。
これをMATLABで表現すると次のようになります。
%reverse_kinematics.m
l1=65;
l2=120;
x=-30; %target x
y=150; %target y
k=sqrt((x^2+y^2+l1^2-l2^2)^2-2*((x^2+y^2)^2+l1^4-l2^4))
k=abs(k)
t1=atan2(y,x)-atan2(k,(x^2+y^2+l1^2-l2^2))
t2=atan2(k,(x^2+y^2-l1^2-l2^2))
resetRotation(mymotor1)
resetRotation(mymotor2)
motor_move(mymotor1,10,rad2deg(t1))
motor_move(mymotor2,10,rad2deg(t2))
function motor_move(motorname,power,theta)
rotation=readRotation(motorname);
error=theta-rotation;
while(abs(error)>2)
motorname.Speed=error/power+10;
start(motorname);
rotation=readRotation(motorname);
error=(theta-rotation);
end
stop(motorname);
end目標手先位置\((−30,150)\)に対し、返り値は\(θ_1=0.7143,θ_2=1.3684\rm[rad]\)
それっぽいところに手先を動かすことができました。
2自由度マニピュレータまでは理論を追っていくことができますが、これ以上自由度が増えると数式で追うのは無理が出てきます。
ここから先はソルバーなどのツールを活用するべきですね。
EV3とMATLABに慣れればルービックキューブだって回せます。
参考文献
[1]吉川, ‘‘ロボット制御基礎論,” コロナ社, 1988.
