FC2ブログ

爪車

平面2リンクロボットの逆運動学 ヤコビアンを用いた数値解法

平面2リンクロボットの逆運動学問題をヤコビアンを用いた数値解法で求めてみました。
ヤコビアンを用いた数値解法は汎用性があり、平面2リンク以外のロボットでもヤコビアンを計算できれば、容易に逆運動学問題が解けるようになります。

詳しい理論はロボット工学などの文献をご参照下さい。
参考文献
吉川恒夫 : ロボット制御基礎論, コロナ社. (1988).
内山勝, 中村仁彦 : 岩波講座 ロボット学2 ロボットモーション, 岩波書店 (2004)
梶田秀司編者 : ヒューマノイドロボット, オーム社 (2005)
出村公成 : 簡単!実践!ロボットシミュレーション, 森北出版株式会社 (2007)

勝手に制御分析!あのロボットはどう動く?
第3回「操縦者とロボットをつなぐ運動学と,静力学の導入 -スパロボの華麗な操縦と動作を分析-」(前編)


勝手に制御分析!あのロボットはどう動く?
第4回「操縦者とロボットをつなぐ運動学と、静力学の導入
-スパロボの華麗な操縦と動作を分析-(後編)九州大学 田原健二



ヤコビアンを用いた逆運動学の数値解法はいくつか方法があるようです。

今回の方法は、ロボットの手先と目標との位置誤差を、ヤコビアンの逆行列を用いて小さくするな間接角度を求め、収束計算する方法です。

ここでは簡単に手順の説明をします。

リンク1,2の長さ: l1, l2
間接角度: q(1), q(2)

ロボットの順運動学とヤコビアンを求められるように準備しておきます。
原点(0,0)からリンク1の先端位置(x,y) = (p1(1),p1(2))
p1(1) = l1*cos(q(1))
p1(2) = l1*sin(q(1))
リンク2の先端位置(x,y) = (p2(1),p2(2))。これがロボットの手先となります。
p2(1) = l1*cos(q(1))+l2*cos(q(1)+q(2))
p2(2) = l1*sin(q(1))+l2*sin(q(1)+q(2))

ヤコビアンJは
J(1,1) = -l1*sin(q(1))-l2*sin(q(1)+q(2)), J(1,2) = -l2*sin(q(1)+q(2))
J(2,1) = l1*cos(q(1))+l2*cos(q(1)+q(2)), J(2,2) = l2*cos(q(1)+q(2))

Step1. 順運動学で、手先位置p2を求める。
Step2. 手先位置p2と目標位置rdとの位置誤差dpを計算する。
Step3. 位置誤差の大きさ|dp|が許容誤差以下であれば計算終了。そうでなければStep4へ。
Step4. 位置誤差を小さくするための間接角度の修正量dqを計算する。
Step5. 間接角度を更新q = q + dqし、Step1に戻る。

フリーの数値計算ソフトのScilabで計算しました。

// IK.sce


clear;
clf;

l1 = 1.0; // リンク長
l2 = 1.0;

x0 = 1.4;// 目標軌道初期位置
y0 = 1.4;

q = [30*%pi/180;30*%pi/180]; // 初期間接角度

dT = 0.08; // 時間刻み幅

for t = 0:dT:1
rd = [x0;y0-(3-2*t)*(t^2)*y0]; // 目標軌道

for n= 0:1:100

// Step1 位置の計算
p1 = [l1*cos(q(1));l1*sin(q(1))];
p2 = [l1*cos(q(1))+l2*cos(q(1)+q(2));l1*sin(q(1))+l2*sin(q(1)+q(2))];

// Step2 位置誤差の計算
dp = rd - p2;

// Step3 誤差の評価
pe = dp'*dp; // dp(1)^2+dp(2)^2
if pe < 0.1^6
break; // 許容誤差であれば計算終了
end

// Step4 間接角度修正量の計算
// ヤコビアンの計算
J = [-l1*sin(q(1))-l2*sin(q(1)+q(2)), -l2*sin(q(1)+q(2));
l1*cos(q(1))+l2*cos(q(1)+q(2)), l2*cos(q(1)+q(2))];

Kp = 0.2; // 数値計算を安定させるための係数
dq = Kp*inv(J)*dp; // 間接角度修正量の計算

// Step5 間接角度の更新
q = q + dq;

end

// ロボットの描写
plot(rd(1),rd(2), "+r"); // 目標位置
plot(0,0, ".b");
plot(p1(1),p1(2), ".b");
plot2d([0,p1(1)],[0,p1(2)], 1,"011", "", [-2,-2, 2,2]);
plot2d([p1(1),p2(1)],[p1(2),p2(2)], 1,"011", "", [-2,-2, 2,2]);
end



実行結果です。赤い点が目標軌道。
ik_20100220.png

« ヒト型ロボット 49 プログラム再構築中|Top|ヒト型ロボット 48 下半身完成 »

コメント

コメントの投稿

管理者にだけ表示を許可する

トラックバック

http://tsumeguruma.blog46.fc2.com/tb.php/115-38250bec

Top

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

06 | 2019/07 | 08
- 1 2 3 4 5 6
7 8 9 10 11 12 13
14 15 16 17 18 19 20
21 22 23 24 25 26 27
28 29 30 31 - - -